utils.h

#ifndef SOT_UTILS_H
#define SOT_UTILS_H

#include <thread>         // std::this_thread::sleep_for
#include <chrono>         // std::chrono::seconds
#include "common.h"

namespace sot {
    

    template <class MatType = mat, class VecType = vec>
    inline VecType squaredPointSetDistance(const VecType& x, const MatType& Y) {
        return arma::abs(arma::repmat(arma::sum(x % x,0), Y.n_cols,1) + arma::sum(Y % Y,0).t() - 2*Y.t()*x);
    };
    

    template <class MatType = mat>
    inline MatType squaredPairwiseDistance(const MatType& X, const MatType& Y) {
        MatType dists = - 2*(X.t()*Y);
        dists.each_row() += arma::sum(Y % Y, 0);
        dists.each_col() += arma::sum(X % X, 0).t();
        dists = arma::abs(dists);
        return dists;
    };
    

    inline vec toUnitBox(const vec& x, const vec& xLow, const vec& xUp) {
        return (x - xLow)/(xUp - xLow);
    };
    

    inline mat toUnitBox(const mat& X, const vec& xLow, const vec& xUp) {
        return (X - arma::repmat(xLow, 1, X.n_cols))/arma::repmat(xUp - xLow, 1, X.n_cols);
    };
    

    inline vec fromUnitBox(const vec& x, const vec& xLow, const vec& xUp) {
        return xLow + (xUp - xLow) % x;
    };
    

    inline mat fromUnitBox(const mat& X, const vec& xLow, const vec& xUp) {
        return arma::repmat(xLow, 1, X.n_cols) + arma::repmat(xUp - xLow, 1, X.n_cols) % X;
    };
    

    inline vec unitRescale(const vec& x) {
        double xMin = arma::min(x);
        double xMax = arma::max(x);
        if( xMin == xMax ) {
            return arma::ones(x.n_elem);
        }
        return (x - xMin)/(xMax - xMin);
    };
    

    class Result {
    protected:
        int mNumEvals = 0; 
        int mDim; 
        int mMaxEvals; 
        vec mfX; 
        mat mX; 
        double mfBest; 
        vec mxBest; 
    public:

        Result(int maxEvals, int dim) {
            mMaxEvals = maxEvals;
            mNumEvals = 0;
            mDim = dim;
            mfX = std::numeric_limits<double>::max() * arma::ones<vec>(mMaxEvals);
            mfBest = std::numeric_limits<double>::max();
            mX = std::numeric_limits<double>::max() * arma::ones<mat>(dim, maxEvals);
            mxBest = std::numeric_limits<double>::max() * arma::ones<mat>(dim);
        }

        int dim() const {
            return mDim;
        }


        int numEvals() const {
            return mNumEvals;
        }


        vec fX() const {
            if(mNumEvals == 0) {
                throw std::logic_error("No evaluations have been added!");
            }
            return mfX.rows(0, mNumEvals-1);
        }


        mat X() const {
            if(mNumEvals == 0) {
                throw std::logic_error("No evaluations have been added!");
            }
            return mX.cols(0, mNumEvals-1);
        }


        vec xBest() const {
            if(mNumEvals == 0) {
                throw std::logic_error("No evaluations have been added!");
            }
            return mxBest;
        }


        double fBest() const {
            if(mNumEvals == 0) {
                throw std::logic_error("No evaluations have been added!");
            }
            return mfBest;
        }


        void addEval(const vec &x, double funVal) {
            if(mNumEvals >= mMaxEvals) {
                throw std::logic_error("Capacity exceeded");
            }

            mX.col(mNumEvals) = x;
            mfX(mNumEvals) = funVal;
            if (funVal < mfBest) {
                mfBest = funVal;
                mxBest = x;
            }
            mNumEvals++;
        }

        void addEvals(const mat &X, const vec &funVals) {
            for(int i=0; i < X.n_cols; i++) {
                vec x = X.col(i);
                addEval(x, funVals(i));
            }
        }
        
        void reset() {
            mNumEvals = 0;
            mX = std::numeric_limits<double>::max() * arma::ones<mat>(mDim, mMaxEvals);
            mxBest = std::numeric_limits<double>::max() * arma::ones<mat>(mDim);
            mfX = std::numeric_limits<double>::max() * arma::ones<vec>(mMaxEvals);
            mfBest = std::numeric_limits<double>::max();
        }
    };
    

    inline uvec paretoFront(const vec &x, const vec &y) {
        if(x.n_rows != y.n_rows) { 
            throw std::logic_error("paretoFront: x and y need to have the same length"); 
        }
        uvec isort = sort_index(x);
        vec x2 = x(isort);
        vec y2 = y(isort);
        uvec indvec = arma::ones<uvec>(x.n_rows);
        indvec(0) = isort(0);
        int indcur = 1;
        double ycur = y2(0);
        
        for(int i=1; i < x.n_rows; i++) {
            if (y2(i) <= ycur) {
                indvec(indcur) = isort(i);
                ycur = y2(i);
                indcur++;
            }
        }
        indvec = indvec.head(indcur);
        return indvec;
    };
    

    inline vec cumMin(const vec& x) {
        vec out(x.n_elem);
        out(0) = x(0);
        for(int i=1; i < x.n_elem; i++) {
            out(i) = std::min(x(i), out(i-1));
        }
        return out;
    };
    

    class StopWatch {
    private:
        std::chrono::time_point<std::chrono::system_clock> mStartTime; 
        std::chrono::time_point<std::chrono::system_clock> mEndTime; 
        bool mStarted; 
    public:

        StopWatch() {
            mStarted = false;
        }

        void start() {
            if(mStarted) { throw std::logic_error("StopWatch: The StopWatch is already running, so can't start!"); }
            mStartTime = std::chrono::system_clock::now();
            mStarted = true;
        }

        double stop() {
            if(not mStarted) { throw std::logic_error("StopWatch: The StopWatch is not running, so can't stop!"); }
            mEndTime = std::chrono::system_clock::now();
            mStarted = false;
            std::chrono::duration<double> elapsedSeconds = mEndTime - mStartTime;
            return elapsedSeconds.count();
        }
    };
    
    

    inline double randi(int i) { 
        std::uniform_int_distribution<int> randi(0, i);
        return randi(rng::mt);
    }
    

    inline double randn() {
        std::normal_distribution<double> randn(0.0, 1.0);
        return randn(rng::mt);
    }
    

    inline double rand() {
        std::uniform_real_distribution<double> rand(0, 1);
        return rand(rng::mt);
    }


    inline void setSeedRandom() {
        // Set the armadillo seed randomly
        arma::arma_rng::set_seed_random();
        
        // Set the SOT seed using chrono
        typedef std::chrono::high_resolution_clock myClock;
        myClock::time_point beginning = myClock::now();
        myClock::duration d = myClock::now() - beginning;
        unsigned newSeed = d.count();
                
        rng::mt.seed(newSeed);
    }
    

    inline void setSeed(unsigned seed) {
        arma::arma_rng::set_seed(seed);
        rng::mt.seed(seed);
    }
}

#endif