// Copyright (c) Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of differential evolution using on the DE/best/1 strategy.
// Uses two deviations from the standard DE algorithm:
// a) temporal locality introduced in
// https://www.researchgate.net/publication/309179699_Differential_evolution_for_protein_folding_optimization_based_on_a_three-dimensional_AB_off-lattice_model
// b) reinitialization of individuals based on their age.
// requires https://github.com/imneme/pcg-cpp
//
// Supports parallel fitness function evaluation.
//
// You may keep parameters F and CR at their defaults since this implementation works well with the given settings for most problems,
// since the algorithm oscillates between different F and CR settings.
//
// For expensive objective functions (e.g. machine learning parameter optimization) use the workers
// parameter to parallelize objective function evaluation. The workers parameter is limited by the
// population size.
//
// The ints parameter is a boolean array indicating which parameters are discrete integer values. This
// parameter was introduced after observing non optimal DE-results for the ESP2 benchmark problem:
// https://github.com/AlgTUDelft/ExpensiveOptimBenchmark/blob/master/expensiveoptimbenchmark/problems/DockerCFDBenchmark.py
// If defined it causes a "special treatment" for discrete variables: They are rounded to the next integer value and
// there is an additional mutation to avoid getting stuck at local minima.

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <stdint.h>
#include <ctime>
#include <random>
#include <queue>
#include <tuple>
#include "pcg_random.hpp"
#include "evaluator.h"

using namespace std;

namespace differential_evolution {

class DeOptimizer {

public:

    DeOptimizer(long runid_, Fitness *fitfun_, int dim_, int seed_,
            int popsize_, int maxEvaluations_, double keep_,
            double stopfitness_, double F_, double CR_, bool *isInt_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Population size
        popsize = popsize_ > 0 ? popsize_ : 15 * dim;
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_ > 0 ? maxEvaluations_ : 50000;
        // keep best young after each iteration.
        keep = keep_ > 0 ? keep_ : 30;
        // Limit for fitness value.
        stopfitness = stopfitness_;
        F = F0 = F_ > 0 ? F_ : 0.5;
        CR = CR0 = CR_ > 0 ? CR_ : 0.9;
        // Number of iterations already performed.
        iterations = 0;
        bestY = DBL_MAX;
        // stop criteria
        stop = 0;
        pos = 0;
        //std::random_device rd;
        rs = new pcg64(seed_);
        // Indicating which parameters are discrete integer values. If defined these parameters will be
        // rounded to the next integer and some additional mutation of discrete parameters are performed.
        isInt = isInt_;
        init();
    }

    ~DeOptimizer() {
        delete rs;
    }

    double rnd01() {
        return distr_01(*rs);
    }

    int rndInt(int max) {
        return (int) (max * distr_01(*rs));
    }

    vec nextX(int p, const vec &xp, const vec &xb) {
        if (p == 0) {
            iterations++;
            CR = iterations % 2 == 0 ? 0.5 * CR0 : CR0;
            F = iterations % 2 == 0 ? 0.5 * F0 : F0;
        }
        int r1, r2;
        do {
            r1 = rndInt(popsize);
        } while (r1 == p || r1 == bestI);
        do {
            r2 = rndInt(popsize);
        } while (r2 == p || r2 == bestI || r2 == r1);
        vec x1 = popX.col(r1);
        vec x2 = popX.col(r2);
        vec x = xb + (x1 - x2) * F;
        int r = rndInt(dim);
        for (int j = 0; j < dim; j++)
            if (j != r && rnd01() > CR)
                x[j] = xp[j];
        vec nextx = fitfun->getClosestFeasible(x);
        modify(nextx);
        return nextx;
    }

    vec next_improve(const vec &xb, const vec &x, const vec &xi) {
        vec nextx = fitfun->getClosestFeasible(xb + ((x - xi) * 0.5));
        modify(nextx);
        return nextx;
    }

    void modify(vec &x) {
        if (isInt == NULL)
            return;
        double n_ints = 0;
        for (int i = 0; i < dim; i++)
            if (isInt[i]) n_ints++;
        double min_mutate = 0.5;
        double max_mutate = std::max(1.0, n_ints/20.0);
        double to_mutate = min_mutate + rnd01()*(max_mutate - min_mutate);
        for (int i = 0; i < dim; i++) {
            if (isInt[i]) {
                if (rnd01() < to_mutate/n_ints)
                    x[i] = fitfun->sample_i(i, *rs); // resample
                x[i] = std::round(x[i]);
            }
        }
    }

    vec ask(int &p) {
        // ask for one new argument vector.
        if (improvesX.empty()) {
            p = pos;
            vec x = nextX(p, popX.col(p), popX.col(bestI));
            pos = (pos + 1) % popsize;
            return x;
        } else {
            p = improvesP.front();
            vec x = improvesX.front();
            improvesP.pop();
            improvesX.pop();
            return x;
        }
    }

    int tell(double y, const vec &x, int p) {
        //tell function value for a argument list retrieved by ask_one().
        if (isfinite(y) && y < popY[p]) {
            if (iterations > 1) {
                // temporal locality
                improvesP.push(p);
                improvesX.push(next_improve(popX.col(bestI), x, popX0.col(p)));
            }
            popX0.col(p) = popX.col(p);
            popX.col(p) = x;
            popY[p] = y;
            popIter[p] = iterations;
            if (y < popY[bestI]) {
                bestI = p;
                if (y < bestY) {
                    bestY = y;
                    bestX = x;
                    if (isfinite(stopfitness) && bestY < stopfitness)
                        stop = 1;
                }
            }
        } else {
            // reinitialize individual
            if (keep * rnd01() < iterations - popIter[p]) {
                popX.col(p) = fitfun->sample(*rs);
                popY[p] = DBL_MAX;
            }
        }
        return stop;
    }

    void doOptimize() {

        // -------------------- Generation Loop --------------------------------
        for (iterations = 1; fitfun->evaluations() < maxEvaluations
        		&& !fitfun->terminate(); iterations++) {

            CR = iterations % 2 == 0 ? 0.5 * CR0 : CR0;
            F = iterations % 2 == 0 ? 0.5 * F0 : F0;

            for (int p = 0; p < popsize; p++) {
                vec xp = popX.col(p);
                vec xb = popX.col(bestI);
                int r1, r2;
                do {
                    r1 = rndInt(popsize);
                } while (r1 == p || r1 == bestI);
                do {
                    r2 = rndInt(popsize);
                } while (r2 == p || r2 == bestI || r2 == r1);
                vec x1 = popX.col(r1);
                vec x2 = popX.col(r2);
                int r = rndInt(dim);
                vec x = vec(xp);
                for (int j = 0; j < dim; j++) {
                    if (j == r || rnd01() < CR) {
                        x[j] = xb[j] + F * (x1[j] - x2[j]);
                        if (!fitfun->feasible(j, x[j]))
                            x[j] = fitfun->sample_i(j, *rs);
                    }
                }
                modify(x);
                double y = fitfun->eval(x)(0);
                if (isfinite(y) && y < popY[p]) {
                    // temporal locality
                    vec x2 = next_improve(xb, x, xp);
                    double y2 = fitfun->eval(x2)(0);
                    if (isfinite(y2) && y2 < y) {
                        y = y2;
                        x = x2;
                    }
                    popX.col(p) = x;
                    popY(p) = y;
                    popIter[p] = iterations;
                    if (y < popY[bestI]) {
                        bestI = p;
                        if (y < bestY) {
                            bestY = y;
                            bestX = x;
                            if (isfinite(stopfitness) && bestY < stopfitness) {
                                stop = 1;
                                return;
                            }
                        }
                    }
                } else {
                    // reinitialize individual
                    if (keep * rnd01() < iterations - popIter[p]) {
                        popX.col(p) = fitfun->sample(*rs);
                        popY[p] = DBL_MAX;
                    }
                }
            }
        }
    }

    void do_optimize_delayed_update(int workers) {
    	 iterations = 0;
    	 fitfun->resetEvaluations();
         workers = std::min(workers, popsize); // workers <= popsize
    	 evaluator eval(fitfun, 1, workers);
         int evals_size = popsize*10;
    	 vec evals_x[evals_size];
   	     int evals_p[evals_size];
         int cp = 0;

	     // fill eval queue with initial population
    	 for (int i = 0; i < workers; i++) {
    		 int p;
    		 vec x = ask(p);
    		 eval.evaluate(x, cp);
    		 evals_x[cp] = x;
    		 evals_p[cp] = p;
             cp = (cp + 1) % evals_size;
    	 }
    	 while (fitfun->evaluations() < maxEvaluations && !fitfun->terminate()) {
    		 vec_id* vid = eval.result();
    		 vec y = vec(vid->_v);
    		 int id = vid->_id;
    		 delete vid;
    		 vec x = evals_x[id];
             int p = evals_p[id];
    		 tell(y(0), x, p); // tell evaluated x
    		 if (fitfun->evaluations() >= maxEvaluations)
    			 break;
    		 x = ask(p);
    		 eval.evaluate(x, cp);
    		 evals_x[cp] = x;
    		 evals_p[cp] = p;
             cp = (cp + 1) % evals_size;
    	 }
	}

    void init() {
        popX = mat(dim, popsize);
        popX0 = mat(dim, popsize);
        popY = vec(popsize);
        for (int p = 0; p < popsize; p++) {
            popX0.col(p) = popX.col(p) = fitfun->sample(*rs);
            popY[p] = DBL_MAX; // compute fitness
        }
        bestI = 0;
        bestX = popX.col(bestI);
        popIter = zeros(popsize);
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestY;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

    Fitness* getFitfun() {
        return fitfun;
    }

    int getDim() {
        return dim;
    }

private:
    long runid;
    Fitness *fitfun;
    int popsize; // population size
    int dim;
    int maxEvaluations;
    double keep;
    double stopfitness;
    int iterations;
    double bestY;
    vec bestX;
    int bestI;
    int stop;
    double F0;
    double CR0;
    double F;
    double CR;
    pcg64 *rs;
    mat popX;
    mat popX0;
    vec popY;
    vec popIter;
    queue<vec> improvesX;
    queue<int> improvesP;
    int pos;
    bool *isInt;
};

}

using namespace differential_evolution;

extern "C" {
void optimizeDE_C(long runid, callback_type func, int dim, int seed,
        double *lower, double *upper, bool *ints,
        int maxEvals, double keep,
        double stopfitness, int popsize, double F, double CR, int workers, double* res) {
    vec lower_limit(dim), upper_limit(dim);
    bool isInt[dim];
    bool useIsInt = false;
    for (int i = 0; i < dim; i++) {
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
        isInt[i] = ints[i];
        useIsInt |= ints[i];
        if (isInt[i]) {
            // adjust bounds because ints are rounded
            lower_limit[i] -= .499999999;
            upper_limit[i] += .499999999;
        }
    }
    Fitness fitfun(func, dim, 1, lower_limit, upper_limit);
    DeOptimizer opt(runid, &fitfun, dim, seed, popsize, maxEvals, keep,
            stopfitness, F, CR, useIsInt ? isInt : NULL);
    try {
        if (workers <= 1)
            opt.doOptimize();
        else
            opt.do_optimize_delayed_update(workers);
        vec bestX = opt.getBestX();
        double bestY = opt.getBestValue();
        for (int i = 0; i < dim; i++)
            res[i] = bestX[i];
        res[dim] = bestY;
        res[dim + 1] = fitfun.evaluations();
        res[dim + 2] = opt.getIterations();
        res[dim + 3] = opt.getStop();
    } catch (std::exception &e) {
        cout << e.what() << endl;
    }
}
}


/*
 * StokesM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <iomanip>
#include <iostream>

#define DIRECTLAYER 2
#define PI314 (static_cast<double>(3.1415926535897932384626433))

namespace Laplace1D3D {

using EVec3 = Eigen::Vector3d;

inline double gKernel(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    return rnorm < 1e-14 ? 0 : 1 / rnorm;
}

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */

template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

double directSum(const EVec3 &target, const EVec3 &source, const int directTerm = 500000) {
    // use asymptotic
    const double L3 = 1.0;
    double potentialDirect = 0;
    for (int t = DIRECTLAYER + 1; t < directTerm; t++) {
        potentialDirect +=
            gKernel(target, source + EVec3(t * L3, 0, 0)) + gKernel(target, source - EVec3(t * L3, 0, 0));
    }

    return potentialDirect;
}

int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);

    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {-(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {-(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};

    const double scaleLEquiv = 1.05;
    const double scaleLCheck = 2.95;
    const double pCenterLEquiv[3] = {-(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2};
    const double pCenterLCheck[3] = {-(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2};

    auto pointMEquiv = surface(pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(pCheck, (double *)&(pCenterCheck[0]), scaleCheck,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    auto pointLEquiv = surface(pEquiv, (double *)&(pCenterLCheck[0]), scaleLCheck,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointLCheck = surface(pCheck, (double *)&(pCenterLEquiv[0]), scaleLEquiv,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd M2L(equivN, equivN); // Laplace, 1->1

    Eigen::MatrixXd A(1 * checkN, 1 * equivN);
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1], pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l], pointLEquiv[3 * l + 1], pointLEquiv[3 * l + 2]);
            A(k, l) = gKernel(Cpoint, Lpoint);
        }
    }
    Eigen::MatrixXd ApinvU(A.cols(), A.rows());
    Eigen::MatrixXd ApinvVT(A.cols(), A.rows());
    pinv(A, ApinvU, ApinvVT);

#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1], pointMEquiv[3 * i + 2]);
        //		std::cout << "debug:" << Mpoint << std::endl;

        // assemble linear system
        Eigen::VectorXd f(checkN);
        for (int k = 0; k < checkN; k++) {
            Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1], pointLCheck[3 * k + 2]);
            //			std::cout<<"debug:"<<k<<std::endl;
            // sum the images
            f(k) = directSum(Cpoint, Mpoint); // gKernelFF(Cpoint, Mpoint);
        }
        //		std::cout << "debug:" << f << std::endl;

        M2L.col(i) = (ApinvU.transpose() * (ApinvVT.transpose() * f));
    }

    // dump M2L
    for (int i = 0; i < equivN; i++) {
        for (int j = 0; j < equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> chargePoint(4);
    std::vector<double> chargeValue(4);
    chargePoint[0] = Eigen::Vector3d(0.125, 0.5, 0.5);
    chargeValue[0] = 1;
    chargePoint[1] = Eigen::Vector3d(0.375, 0.5, 0.5);
    chargeValue[1] = -1;
    chargePoint[2] = Eigen::Vector3d(0.625, 0.5, 0.5);
    chargeValue[2] = 1;
    chargePoint[3] = Eigen::Vector3d(0.875, 0.5, 0.5);
    chargeValue[3] = -1;

    // solve M
    A.resize(checkN, equivN);
    ApinvU.resize(A.cols(), A.rows());
    ApinvVT.resize(A.cols(), A.rows());
    Eigen::VectorXd f(checkN);
    for (int k = 0; k < checkN; k++) {
        double temp = 0;
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        for (size_t p = 0; p < chargePoint.size(); p++) {
            temp = temp + gKernel(Cpoint, chargePoint[p]) * (chargeValue[p]);
        }
        f(k) = temp;
        for (int l = 0; l < equivN; l++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1], pointMEquiv[3 * l + 2]);
            A(k, l) = gKernel(Mpoint, Cpoint);
        }
    }
    pinv(A, ApinvU, ApinvVT);
    Eigen::VectorXd Msource = (ApinvU.transpose() * (ApinvVT.transpose() * f));

    // impose zero sum
    double fx = 0;
    for (int i = 0; i < equivN; i++) {
        fx += Msource[i];
    }
    fx /= equivN;
    double test = 0;
    for (int i = 0; i < equivN; i++) {
        Msource[i] -= fx;
        test += Msource[i];
    }

    std::cout << "Msource net: " << test << std::endl;
    std::cout << "Msource: " << Msource << std::endl;

    Eigen::VectorXd M2Lsource = M2L * (Msource);

    Eigen::Vector3d samplePoint(0.125, 0.5, 0.5);
    double Usample = 0;
    double UsampleSP = 0;

    for (int k = -DIRECTLAYER; k < 1 + DIRECTLAYER; k++) {
        for (size_t p = 0; p < chargePoint.size(); p++) {
            Usample += gKernel(samplePoint, chargePoint[p] + EVec3(k, 0, 0)) * chargeValue[p];
        }
    }

    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1], pointLEquiv[3 * p + 2]);
        UsampleSP += gKernel(samplePoint, Lpoint) * M2Lsource[p];
    }

    std::cout << "samplePoint:" << samplePoint << std::endl;
    std::cout << "Usample NF:" << Usample << std::endl;
    std::cout << "Usample FF:" << UsampleSP << std::endl;
    std::cout << "Usample FF+NF total:" << UsampleSP + Usample << std::endl;
    std::cout << "error:" << UsampleSP + Usample + 8 * log(2) << std::endl;

    samplePoint = EVec3(0.625, 0.5, 0.5);
    Usample = 0;
    UsampleSP = 0;

    for (int k = -DIRECTLAYER; k < 1 + DIRECTLAYER; k++) {
        for (size_t p = 0; p < chargePoint.size(); p++) {
            Usample += gKernel(samplePoint, chargePoint[p] + EVec3(k, 0, 0)) * chargeValue[p];
        }
    }

    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1], pointLEquiv[3 * p + 2]);
        UsampleSP += gKernel(samplePoint, Lpoint) * M2Lsource[p];
    }

    std::cout << "samplePoint:" << samplePoint << std::endl;
    std::cout << "Usample NF:" << Usample << std::endl;
    std::cout << "Usample FF:" << UsampleSP << std::endl;
    std::cout << "Usample FF+NF total:" << UsampleSP + Usample << std::endl;
    std::cout << "error:" << UsampleSP + Usample + 8 * log(2) << std::endl;

    return 0;
}

} // namespace Laplace1D3D

#undef DIRECTLAYER
#undef PI314


/////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2021 Andreas Milton Maniotis.
//
// Email: andreas.maniotis@gmail.com
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
/////////////////////////////////////////////////////////////////////////////////////////////

#include "aml/functional.hpp"

#include <type_traits>

#include <iostream>
#include <boost/core/demangle.hpp>

namespace test::functional
{
    template< typename T1
            , typename T2
            , typename T3
            , typename T4
            , typename T5
            , typename T6
            , typename...
            >
    struct F
    {
        using type = F<T1, T2, T3, T4, T5, T6>;
    };

    template< int n >
    struct t;

    using f  =  F< t<1>, t<2>, t<3>, t<4>, t<5>, t<6> >;

    template<template<typename...> class F>
    struct bound_function_
    {
        template< typename... X >
        using apply_to = F< X... >;
    };

    //    template< auto n >
    //  struct number { static constexpr auto eval() { return n; } };

    void test_curry()
    {
        using aml::_;

        using c0  =  aml::curry<F, _<2> >::apply_to<t<1>, t<2> >::apply_to<t<3>, t<4>, t<5>, t<6> >;

        static_assert( std::is_same< c0, f >::value );

        using c1  =  aml::curry< F, _<1>, _<3>, _<2> >::
            apply_to<  t<1>  >::
            template apply_to<  t<2>, t<3>, t<4>  >::
            template apply_to<  t<5>, t<6>  >::
            template apply_to<>;

        static_assert( std::is_same< c1, f >::value );

        using c2  =  aml::curry<F, _<3>, _<1> >::
                     apply_to< t<1>, t<2>, t<3> >::
                     apply_to< t<4> >::
                     apply_to< t<5>, t<6> >;

        static_assert( std::is_same< c2, f >::value );

        using c3  =  aml::curry<F, aml::infinity>::apply_to< t<1>, t<2>, t<3>, t<4>, t<5>, t<6> >::apply_to<>;

        static_assert( std::is_same< c3, f >::value );

        using cb1  =  aml::curry_and_bind<F, t<1>, t<2> >::apply_to<t<3>, t<4>, t<5>, t<6> >;

        using cb2  =  aml::curry_and_bind<F>::apply_to< t<1>, t<2>, t<3>, t<4>, t<5>, t<6> >;

        using cb3  =  aml::curry_and_bind<F, t<1>, t<2>, t<3>, t<4>, t<5>, t<6> >::apply_to<>;

        static_assert( std::is_same< cb1, f >::value );
        static_assert( std::is_same< cb2, f >::value );
        static_assert( std::is_same< cb3, f >::value );
    }

    template<typename...> class K_ {};
    template<typename >   class H_ {};
    template<typename...> class G_ {};

    void test_composition()
    {
        using aml::_;

        using t1  =  K_<H_<G_<int, void, char> > >;
        using c1  =  aml::composition<K_, H_, G_>::apply_to<int, void, char>;
        using c2  =  aml::composition< aml::composition<K_, H_>::apply_to, G_>::apply_to<int, void, char>;
        using c3  =  aml::composition< K_,
                                       aml::composition<H_, G_>::template apply_to >::
                     apply_to<int, void, char>;

        static_assert( std::is_same< t1, c1 >::value );
        static_assert( std::is_same< t1, c2 >::value );
        static_assert( std::is_same< t1, c3 >::value );

        static_assert( std::is_same< aml::composition<>::apply_to<int*>,
                       int* >::value );

        static_assert( std::is_same< aml::identity<int*>, int* >::value );

        struct T0
        {
            struct type_
            {
                struct type;
            };

            using type = type_;
        };

        using T0_ = aml::composition<>::power<T0, _<0> >;

        static_assert( std::is_same< T0_, T0 >::value );
        static_assert( std::is_same<T0, aml::power<T0, _<0> >>::value );

        using T0_t  =  aml::composition<>::power< T0, _<1> >;

        static_assert( std::is_same< T0_t, typename T0::type >::value );
        static_assert( std::is_same< T0_t, aml::power<T0, _<1> > >::value );

        using T0_tt  =  aml::composition<>::power<T0, _<2> >;

        static_assert( std::is_same< T0_tt, T0::type::type >::value );
        static_assert( std::is_same< T0_tt, aml::power<T0, _<2> > >::value );

        struct n0 { static constexpr auto eval() {  return 0; } };
        struct n1 { static constexpr auto eval() {  return 1; } };
        struct n2 { static constexpr auto eval() {  return 2; } };
    }

    void test_limit()
    {
        struct T0
        {
            struct type_
            {
                struct type;
            };

            using type = type_;
        };

        using limit_t   =  aml::composition<>::limit<T0>;
        using limit_t_  =  aml::limit<T0>;

        static_assert( std::is_same< limit_t, T0::type::type >::value );
        static_assert( std::is_same< limit_t, limit_t_ >::value );

        struct has_type_forever
        {
            struct type_
            {
                using type = type_;
            };

            using type = type_;
        };

        // has_type_forever::type::type::type .... == has_type_foever::type;

        using limit_1  =  aml::composition<>::limit< has_type_forever >;
        using limit_2  =  aml::limit< has_type_forever >;

        static_assert( std::is_same<limit_1, limit_2>::value );
        static_assert( std::is_same<limit_1, has_type_forever::type >::value );

        using power_1  =  aml::composition<>::power< has_type_forever, aml::infinity >;
        using power_2  =  aml::power< has_type_forever, aml::infinity >;

        static_assert( std::is_same<limit_1, power_1>::value );
        static_assert( std::is_same<limit_2, power_2>::value );
    }

    template<typename... >
    struct X;

    template< typename A, typename B >
    struct X< A, B >;

    template< typename A, typename B, typename C, typename... D >
    struct X< A, B, C, D... >
    {
        using type  =  X<A, B, D...>;
    };

    template<  template< typename... > class F  >
    struct function_
    {
        template< typename... X >
        using apply_to = F< X... >;
    };

    void test_monoid()
    {
        using aml::_;

        using t0  =  aml::monoid<X>::power<_<0> >::apply_to< int** >;
        using t1  =  aml::monoid<X>::power< _<1> >::apply_to<int*, int**, int***, int****>;
        using t2  =  aml::monoid<X>::power< _<2> >::apply_to<int*, int**, int***, int****>;

        static_assert( std::is_same< t0, int** >::value );
        static_assert( std::is_same< t1, X< int*, int**, int***, int**** > >::value );
        static_assert( std::is_same< t2, X< X< int*, int**, int***, int**** > > >::value );
    }
}

#include <iostream>
#include <string>

int main()
{
    void (*test_set[])() =
    {
        test::functional::test_curry,
        test::functional::test_composition,
        test::functional::test_limit,
        test::functional::test_monoid,
    };

    for ( auto test : test_set )
        test();

    std::cout << __FILE__ << ": " << sizeof(test_set)/sizeof(test_set[0])  << " tests passed." << std::endl;

}


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

/// \file exponential_sum.hpp

#ifndef MXPFIT_EXPONENTIAL_SUM_HPP
#define MXPFIT_EXPONENTIAL_SUM_HPP

#include <algorithm>
#include <iosfwd>
#include <random>

#include <Eigen/Core>

namespace mxpfit
{
// --- Forward declarations
template <typename ExpScalarT, typename WScalarT = ExpScalarT>
class ExponentialSum;

template <typename ExponentsArrayT, typename WeightsArrayT>
class ExponentialSumWrapper;

namespace detail
{

template <typename T>
struct ExponentialSumTraits;

//
// Generate random real number between [0,1]
//
template <typename T>
struct random_real_scalar
{
    random_real_scalar() : distr_(T(), T(1))
    {
        std::random_device rd;
        std::seed_seq seeds({rd(), rd(), rd(), rd(), rd(), rd(), rd(), rd()});
        engine_.seed(seeds);
    }

    T operator()()
    {
        return distr_(engine_);
    }

    std::mt19937 engine_;
    std::uniform_real_distribution<T> distr_;
};

template <typename T>
struct random_exponent_impl
{
    T operator()()
    {
        return m_rng(); // distributed on [0, 1]
    }

private:
    random_real_scalar<T> m_rng;
};

template <typename T>
struct random_exponent_impl<std::complex<T>>
{
    std::complex<T> operator()()
    {
        static const T pi = EIGEN_PI;
        // zi distributed on unit disk
        const auto zi = std::polar(m_rng(), pi * (m_rng() - T(0.5)));
        return -Eigen::numext::log(zi);
    }

private:
    random_real_scalar<T> m_rng;
};

template <typename T>
struct random_weight_impl
{
    T operator()()
    {
        return m_rng(); // distributed on [0, 1]
    }

private:
    random_real_scalar<T> m_rng;
};

template <typename T>
struct random_weight_impl<std::complex<T>>
{
    std::complex<T> operator()()
    {
        // distributed on [-1, 1] + i[-1,1]
        return std::complex<T>(T(2) * m_rng() - 1, T(2) * m_rng() - T(1));
    }

private:
    random_real_scalar<T> m_rng;
};

} // namespace detail

///
/// ### ExponentialSumBase
///
/// Base class for expressions of exponential sum function
///
template <typename Derived>
class ExponentialSumBase
{
public:
    using Index  = Eigen::Index;
    using Traits = detail::ExponentialSumTraits<Derived>;

    using ExponentsArray = typename Traits::ExponentsArray;
    using WeightsArray   = typename Traits::WeightsArray;

    using ExponentScalar = typename ExponentsArray::Scalar;
    using WeightScalar   = typename WeightsArray::Scalar;

    using ExponentsArrayNested =
        typename Eigen::internal::ref_selector<ExponentsArray>::type;
    using WeightsArrayNested =
        typename Eigen::internal::ref_selector<WeightsArray>::type;

    using PlainExponentsArray =
        Eigen::Array<typename ExponentsArray::Scalar, Eigen::Dynamic, 1>;
    using PlainWeightsArray =
        Eigen::Array<typename WeightsArray::Scalar, Eigen::Dynamic, 1>;

    /// \return reference to the derived object
    Derived& derived()
    {
        return *static_cast<Derived*>(this);
    }
    /// \return const reference to the derived object
    const Derived& derived() const
    {
        return *static_cast<const Derived*>(this);
    }

    /// \return the number of exponential terms
    Index size() const
    {
        assert(exponents().size() == weights().size());
        return exponents().size();
    }

    /// \return value of the multi-exponential function at given argument x
    template <typename ArgT>
    auto operator()(const ArgT& x) const
        -> decltype(ExponentScalar() * WeightScalar())
    {
        // return derived().evalAt(x);
        return ((-x * exponents()).exp() * weights()).sum();
    }

    /// \return const reference to the array of exponents
    ExponentsArrayNested exponents() const
    {
        return derived().exponents();
    }

    /// \return reference to the array of weights
    WeightsArrayNested weights() const
    {
        return derived().weights();
    }

    /// \return value or reference of `i`-th exponents
    typename ExponentsArray::CoeffReturnType exponent(Index i) const
    {
        assert(Index() <= i && i < size());
        return exponents().coeff(i);
    }

    /// \return value or reference of `i`-th weight
    typename WeightsArray::CoeffReturnType weight(Index i) const
    {
        assert(Index() <= i && i < size());
        return weights().coeff(i);
    }
};

/// Output stream operator for `ExponentialSumBase`
template <typename Ch, typename Tr, typename Derived>
std::basic_ostream<Ch, Tr>&
operator<<(std::basic_ostream<Ch, Tr>& os,
           const ExponentialSumBase<Derived>& expsum)
{
    auto n = expsum.size();
    os << n << '\n';
    for (decltype(n) i = 0; i < n; ++i)
    {
        os << expsum.exponent(i) << '\t' << expsum.weight(i) << '\n';
    }

    return os;
}

///
/// Remove terms in exponential sum satisfying the specific criteria
///
/// \param[in] esum original exponential sum,
///   \f$f(t)=\sum_{j=1}^{n}c_{j}e^{-a_{j}t}.\f$
/// \param[in] pred unary predicate which returns `true` if the term should
///   be removed. The signature of the predicate function should be equivalent
///   to the following:
///   ``` c++
///     bool pred(const Scalar& aj, const Scalar& cj);
///   ```
///   where `aj` and `cj` are j-th exponent, \f$a_{j},\$ and coefficient
///   \f$c_{j},\f$ respectively, and `Scalar` is the scalar type of original
///   exponential sum, `esum`. The signature does not need to have `const &`,
///   but the function must not modify the objects passed to it.
///
/// \return an object of ExponentialSum with same scalar types for `esum`.
///
template <typename Derived, typename Predicate>
ExponentialSum<typename ExponentialSumBase<Derived>::ExponentScalar,
               typename ExponentialSumBase<Derived>::WeightScalar>
removeIf(const ExponentialSumBase<Derived>& esum, Predicate pred)
{
    using Index      = Eigen::Index;
    using IndexArray = Eigen::Array<Index, Eigen::Dynamic, 1>;
    using ResultType =
        ExponentialSum<typename ExponentialSumBase<Derived>::ExponentScalar,
                       typename ExponentialSumBase<Derived>::WeightScalar>;
    using Eigen::numext::abs;
    using Eigen::numext::real;

    if (esum.size() == Index())
    {
        return ResultType();
    }

    IndexArray index(IndexArray::LinSpaced(esum.size(), 0, esum.size() - 1));

    auto* last =
        std::remove_if(index.data(), index.data() + index.size(), [&](Index x) {
            return pred(esum.exponent(x), esum.weight(x));
        });

    Index n = static_cast<Index>(last - index.data());
    ResultType ret(n);
    for (Index i = 0; i < n; ++i)
    {
        ret.exponent(i) = esum.exponent(index(i));
        ret.weight(i)   = esum.weight(index(i));
    }

    return ret;
}

//==============================================================================
// ExponentialSum class
//==============================================================================
namespace detail
{

template <typename ExpScalarT, typename WScalarT>
struct ExponentialSumTraits<ExponentialSum<ExpScalarT, WScalarT>>
{
    using ExponentsArray = Eigen::Array<ExpScalarT, Eigen::Dynamic, 1>;
    using WeightsArray   = Eigen::Array<WScalarT, Eigen::Dynamic, 1>;
};

} // namespace detail

///
/// ### ExponentialSum
///
/// Representation of an exponential sum function with its storage.
///
/// \tparam T  the scalar type for parameters
/// \tparam Size_ size of internal arrays to store exponents and weights. Set
///     Eigen::Dynamic for changing the size dynamically. Default is
///     Eigen::Dynamic.
/// \tparam MaxSize_ maximum size of internal arrays. Default is `Size_`.
///
template <typename ExpScalarT, typename WScalarT>
class ExponentialSum
    : public ExponentialSumBase<ExponentialSum<ExpScalarT, WScalarT>>
{
    using Base = ExponentialSumBase<ExponentialSum<ExpScalarT, WScalarT>>;

public:
    using Index = Eigen::Index;

    using ExponentsArray = typename Base::ExponentsArray;
    using WeightsArray   = typename Base::WeightsArray;
    using ExponentScalar = typename ExponentsArray::Scalar;
    using WeightScalar   = typename WeightsArray::Scalar;

protected:
    using IndexArray = Eigen::Array<Index, Eigen::Dynamic, 1>;

    ExponentsArray m_exponents;
    WeightsArray m_weights;

public:
    /// Default constructor
    ExponentialSum() = default;

    /// Create an exponential sum function with number of terms
    explicit ExponentialSum(Index n) : m_exponents(n), m_weights(n)
    {
    }

    /// Create an exponential sum function from other expression
    template <typename Derived>
    explicit ExponentialSum(const ExponentialSumBase<Derived>& other)
        : m_exponents(other.exponents()), m_weights(other.weights())
    {
    }

    /// Copy constructor
    ExponentialSum(const ExponentialSum&) = default;

    /// Move constructor
    ExponentialSum(ExponentialSum&&) = default;

    /// Destuctor
    ~ExponentialSum() = default;

    /// Copy assignment operator
    ExponentialSum& operator=(const ExponentialSum&) = default;

    /// Move assignment operator
    ExponentialSum& operator=(ExponentialSum&&) = default;

    /// Create an exponential sum function from expressions of arrays
    template <typename Derived1, typename Derived2>
    explicit ExponentialSum(const Eigen::DenseBase<Derived1>& exponents_,
                            const Eigen::DenseBase<Derived2>& weights_)
        : m_exponents(exponents_), m_weights(weights_)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived1);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
        eigen_assert(exponents_.size() == weights_.size() &&
                     "Size of exponents and weights array must be the same");
    }

    /// \return const reference to the array of exponents
    const ExponentsArray& exponents() const
    {
        return m_exponents;
    }

    /// \return  reference to the array of exponents
    ExponentsArray& exponents()
    {
        return m_exponents;
    }

    /// \return const reference to the array of exponents
    const WeightsArray& weights() const
    {
        return m_weights;
    }

    /// \return  reference to the array of exponents
    WeightsArray& weights()
    {
        return m_weights;
    }

    /// Set the number of exponential terms
    void resize(Index n)
    {
        m_exponents.resize(n);
        m_weights.resize(n);
    }

    void swap(ExponentialSum& other)
    {
        m_exponents.swap(other.m_exponents);
        m_weights.swap(other.m_weights);
    }

    using Base::size;
    using Base::exponent;
    using Base::weight;
    using Base::operator();

    ExponentScalar& exponent(Index i)
    {
        return m_exponents(i);
    }

    WeightScalar& weight(Index i)
    {
        return m_weights(i);
    }

    /// sort exponents \f$ \xi_i \f$ and weights \f$ w_i \f$ by the ratio
    /// \f$|w_i| / |\mathrm{Re}(\xi)|\f$
    void sortByDominanceRatio()
    {
        using Eigen::numext::abs;
        using Eigen::numext::real;
        using std::swap;

        if (size() <= Index(1))
        {
            return;
        }

        IndexArray order(IndexArray::LinSpaced(size(), 0, size() - 1));

        std::sort(order.data(), order.data() + order.size(),
                  [this](Index x, Index y) {
                      return abs(m_weights(x)) / abs(real(m_exponents(x))) >
                             abs(m_weights(y)) / abs(real(m_exponents(y)));
                  });

        ExponentsArray e_tmp(m_exponents);
        WeightsArray w_tmp(m_weights);
        for (Index i = 0; i < size(); ++i)
        {
            m_exponents(i) = e_tmp(order(i));
            m_weights(i)   = w_tmp(order(i));
        }
    }

    ///
    /// Set exponents and weights to a random number.
    ///
    /// Exponents \f$a_{i}\f$ are distributed on the right half-plane, i.e.,
    /// \f$Re(a_{i})>0,\f$ while weights are located at the region \f$w_{i}\in
    /// [0,1] + i[-1,1].\f$
    ///
    void setRandom()
    {
        static detail::random_exponent_impl<ExponentScalar> rnd_c;
        static detail::random_weight_impl<WeightScalar> rnd_w;

        for (Index i = 0; i < size(); ++i)
        {
            m_exponents(i) = rnd_c();
            m_weights(i)   = rnd_w();
        }
    }

    ///
    /// Merge the terms with same exponents on an exponential sum.
    ///
    /// \param[in] pred binary predicate which returns `true` if two
    ///   exponents can be regarded as the same. The signature of the predicate
    ///   function should be equivalent to the following:
    ///
    ///   ``` c++
    ///     bool pred(const Scalar& AI, const Scalar& aj);
    ///   ```
    ///
    ///   where `ai` and `aj` are the i-th and j-th exponentand `Scalar` is the
    ///   scalar type of original exponential sum, `esum`. The signature does
    ///   not need to have `const &`, but the function must not modify the
    ///   objects passed to it.
    ///
    void
    uniqueExponents(typename Eigen::NumTraits<ExponentScalar>::Real tolerance)
    {
        if (size() <= Index(1))
        {
            return;
        }
        IndexArray order(IndexArray::LinSpaced(size(), 0, size() - 1));
        std::sort(order.data(), order.data() + order.size(),
                  [this](Index x, Index y) {
                      return std::make_tuple(std::abs(m_exponents(x)),
                                             std::arg(m_exponents(x))) >
                             std::make_tuple(std::abs(m_exponents(y)),
                                             std::arg(m_exponents(y)));
                  });

        Index first = 0;
        Index ret   = first;
        Index last  = size();
        while (++first != last)
        {
            const auto lhs = m_exponents(order(ret));
            const auto rhs = m_exponents(order(first));
            // if (pred(m_exponents(order(ret)), m_exponents(order(first))))
            if (std::abs(rhs - lhs) <= tolerance * std::abs(rhs) ||
                std::abs(rhs - lhs) <= tolerance * std::abs(lhs))
            {
                m_weights(order(first)) += m_weights(order(ret));
            }
            else if (++ret != first)
            {
                m_exponents(order(ret)) = m_exponents(order(first));
                m_weights(order(ret))   = m_weights(order(first));
            }
        }
        ++ret;

        m_exponents.conservativeResize(ret);
        m_weights.conservativeResize(ret);

        return;
    }
};

//==============================================================================
// ExponentialSumWrapper class
//==============================================================================
namespace detail
{

template <typename ExpArrayT, typename WArrayT>
struct ExponentialSumTraits<ExponentialSumWrapper<ExpArrayT, WArrayT>>
{
    using ExponentsArray = ExpArrayT;
    using WeightsArray   = WArrayT;
};

} // namespace: detail

///
/// ### ExponentialSumWrapper
///
/// Expression of an exponential sum function formed by wrapping existing array
/// expressions.
///
template <typename ExpArrayT, typename WArrayT>
class ExponentialSumWrapper
    : public ExponentialSumBase<ExponentialSumWrapper<ExpArrayT, WArrayT>>
{
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(ExpArrayT);
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(WArrayT);
    using Base = ExponentialSumBase<ExponentialSumWrapper<ExpArrayT, WArrayT>>;

public:
    using Index = typename Base::Index;

    using ExponentsArray = typename Base::ExponentsArray;
    using WeightsArray   = typename Base::WeightsArray;
    using ExponentsArrayNested =
        typename Eigen::internal::ref_selector<ExponentsArray>::type;
    using WeightsArrayNested =
        typename Eigen::internal::ref_selector<WeightsArray>::type;

protected:
    ExponentsArrayNested m_exponents;
    WeightsArrayNested m_weights;

public:
    /// Create an exponential sum function from expressions of arrays
    explicit ExponentialSumWrapper(const ExponentsArray& exponents_,
                                   const WeightsArray& weights_)
        : m_exponents(exponents_), m_weights(weights_)
    {
        eigen_assert(exponents_.size() == weights_.size() &&
                     "Size of exponents and weights array must be the same");
    }

    /// \return const reference to the array of exponents
    const ExponentsArray& exponents() const
    {
        return m_exponents;
    }

    /// \return const reference to the array of exponents
    const WeightsArray& weights() const
    {
        return m_weights;
    }

    using Base::size;
    using Base::exponent;
    using Base::weight;
    using Base::operator();
};

///
/// Create an instance of `ExponentialSumWrapper` from given arrays.
///
template <typename ExpArrayT, typename WArrayT>
ExponentialSumWrapper<ExpArrayT, WArrayT>
makeExponentialSum(const Eigen::ArrayBase<ExpArrayT>& exponents,
                   const Eigen::ArrayBase<WArrayT>& weights)
{
    return ExponentialSumWrapper<ExpArrayT, WArrayT>(exponents.derived(),
                                                     weights.derived());
}

} // namespace: mxpfit

#endif /* MXPFIT_EXPONENTIAL_SUM_HPP */


/*
Copyright 2016 Andrich van Wyk

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#include <Eigen/Core>
#include "domain.h"
#include "gtest/gtest.h"
#include "particle.h"
#include "random.h"

using Eigen::ArrayXd;

TEST(ParticleTest, should_have_values) {
  Random rng(1L);
  auto domain = Domain(-5.0, 5.0, 30);

  auto position = rng.random_vector(domain);
  auto velocity = ArrayXd::Zero(domain.size);
  auto fitness = std::numeric_limits<double>::max();

  auto p = Particle(position, velocity, fitness);

  ASSERT_TRUE(position.isApprox(p.position));
  ASSERT_TRUE(velocity.isApprox(p.velocity));
  ASSERT_TRUE(position.isApprox(p.pbest_position));
  EXPECT_EQ(fitness, p.fitness);
  EXPECT_EQ(fitness, p.pbest_fitness);
}


#pragma once

#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>

namespace common_robotics_utilities
{
namespace ros_conversions
{
Eigen::Vector3d GeometryPointToEigenVector3d(
    const geometry_msgs::Point& point);

geometry_msgs::Point EigenVector3dToGeometryPoint(
    const Eigen::Vector3d& point);

Eigen::Vector4d GeometryPointToEigenVector4d(
    const geometry_msgs::Point& point);

geometry_msgs::Point EigenVector4dToGeometryPoint(
    const Eigen::Vector4d& point);

geometry_msgs::PointStamped EigenVector3dToGeometryPointStamped(
    const Eigen::Vector3d& point, const std::string& frame_id);

Eigen::Vector3d GeometryVector3ToEigenVector3d(
    const geometry_msgs::Vector3& vector);

geometry_msgs::Vector3 EigenVector3dToGeometryVector3(
    const Eigen::Vector3d& vector);

Eigen::Vector4d GeometryVector3ToEigenVector4d(
    const geometry_msgs::Vector3& vector);

geometry_msgs::Vector3 EigenVector4dToGeometryVector3(
    const Eigen::Vector4d& vector);

Eigen::Quaterniond GeometryQuaternionToEigenQuaterniond(
    const geometry_msgs::Quaternion& quat);

geometry_msgs::Quaternion EigenQuaterniondToGeometryQuaternion(
    const Eigen::Quaterniond& quat);

Eigen::Isometry3d GeometryPoseToEigenIsometry3d(
    const geometry_msgs::Pose& pose);

geometry_msgs::Pose EigenIsometry3dToGeometryPose(
    const Eigen::Isometry3d& transform);

geometry_msgs::PoseStamped EigenIsometry3dToGeometryPoseStamped(
    const Eigen::Isometry3d& transform, const std::string& frame_id);

Eigen::Isometry3d GeometryTransformToEigenIsometry3d(
    const geometry_msgs::Transform& transform);

geometry_msgs::Transform EigenIsometry3dToGeometryTransform(
    const Eigen::Isometry3d& transform);

geometry_msgs::TransformStamped EigenIsometry3dToGeometryTransformStamped(
    const Eigen::Isometry3d& transform, const std::string& frame_id,
    const std::string& child_frame_id);

Eigen::Matrix3Xd VectorGeometryPointToEigenMatrix3Xd(
    const std::vector<geometry_msgs::Point>& vector_geom);

std::vector<geometry_msgs::Point> EigenMatrix3XdToVectorGeometryPoint(
    const Eigen::Matrix3Xd& eigen_matrix);

std::vector<geometry_msgs::Point>
VectorEigenVector3dToVectorGeometryPoint(
    const common_robotics_utilities::math::VectorVector3d& vector_eigen);

common_robotics_utilities::math::VectorVector3d
VectorGeometryPointToVectorEigenVector3d(
    const std::vector<geometry_msgs::Point>& vector_geom);

common_robotics_utilities::math::VectorVector3d
VectorGeometryVector3ToEigenVector3d(
    const std::vector<geometry_msgs::Vector3>& vector_geom);

common_robotics_utilities::math::VectorIsometry3d
VectorGeometryPoseToVectorIsometry3d(
    const std::vector<geometry_msgs::Pose>& vector_geom);

common_robotics_utilities::math::VectorIsometry3d
VectorGeometryPoseToVectorIsometry3d(
    const std::vector<geometry_msgs::Transform>& vector_geom);

std::vector<geometry_msgs::Pose> VectorIsometry3dToVectorGeometryPose(
    const common_robotics_utilities::math::VectorIsometry3d& vector_eigen);

std::vector<geometry_msgs::Transform> VectorIsometry3dToVectorGeometryTransform(
    const common_robotics_utilities::math::VectorIsometry3d& vector_eigen);
}  // namespace ros_conversions
}  // namespace common_robotics_utilities


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"
#include "graph_filtering.hh"
#include "graph_properties.hh"
#include "graph_selectors.hh"

#include <boost/python.hpp>

#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct do_all_pairs_search
{
    template <class Graph, class VertexIndexMap, class DistMap, class WeightMap>
    void operator()(const Graph& g, VertexIndexMap vertex_index,
                    DistMap dist_map, WeightMap weight, bool dense) const
    {
        typedef typename property_traits<DistMap>::value_type::value_type
            dist_t;

        parallel_vertex_loop
            (g,
             [&](auto& v)
             {
                 dist_map[v].clear();
                 dist_map[v].resize(num_vertices(g), 0);
             });

        if (dense)
        {
            floyd_warshall_all_pairs_shortest_paths
                (g, dist_map,
                 weight_map(ConvertedPropertyMap<WeightMap,dist_t>(weight)).
                 vertex_index_map(vertex_index));
        }
        else
        {
            johnson_all_pairs_shortest_paths
                (g, dist_map,
                 weight_map(ConvertedPropertyMap<WeightMap,dist_t>(weight)).
                 vertex_index_map(vertex_index));
        }
    }
};

struct do_all_pairs_search_unweighted
{
    template <class DistMap, class PredMap>
    class bfs_visitor: public boost::bfs_visitor<null_visitor>
    {
    public:
        bfs_visitor(DistMap& dist_map, PredMap& pred, size_t source)
        : _dist_map(dist_map), _pred(pred), _source(source) {}

        template <class Graph>
        void initialize_vertex(typename graph_traits<Graph>::vertex_descriptor v,
                               Graph&)
        {
            typedef typename DistMap::value_type dist_t;
            dist_t inf = std::is_floating_point<dist_t>::value ?
                numeric_limits<dist_t>::infinity() :
                numeric_limits<dist_t>::max();
            _dist_map[v] = (v == _source) ? 0 : inf;
            _pred[v] = v;
        }

        template <class Graph>
        void tree_edge(const typename graph_traits<Graph>::edge_descriptor& e,
                       Graph& g)
        {
            _pred[target(e,g)] = source(e,g);
        }

        template <class Graph>
        void discover_vertex(typename graph_traits<Graph>::vertex_descriptor v,
                             Graph&)
        {
            if (size_t(_pred[v]) == v)
                return;
            _dist_map[v] = _dist_map[_pred[v]] + 1;
        }

    private:
        DistMap& _dist_map;
        PredMap& _pred;
        size_t _source;
    };

    template <class Graph, class DistMap>
    void operator()(const Graph& g, DistMap dist_map) const
    {
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
        typedef typename property_traits<DistMap>::value_type dist_t;

        vector<vertex_t> pred_map(num_vertices(g));
        #pragma omp parallel if (num_vertices(g) > OPENMP_MIN_THRESH) \
            firstprivate(pred_map)
        parallel_vertex_loop_no_spawn
                (g,
                 [&](auto v)
                 {
                     dist_map[v].resize(num_vertices(g), 0);
                     bfs_visitor<dist_t,vector<size_t>>
                         vis(dist_map[v], pred_map, v);
                     breadth_first_search(g, v, visitor(vis));
                 });
    }
};

void get_all_dists(GraphInterface& gi, boost::any dist_map, boost::any weight,
                   bool dense)
{
    if (weight.empty())
    {
        run_action<>()
            (gi, std::bind(do_all_pairs_search_unweighted(),
                           std::placeholders::_1, std::placeholders::_2),
             vertex_scalar_vector_properties())
            (dist_map);
    }
    else
    {
        run_action<>()
            (gi, std::bind(do_all_pairs_search(), std::placeholders::_1,
                           gi.get_vertex_index(), std::placeholders::_2,
                           std::placeholders::_3, dense),
             vertex_scalar_vector_properties(),
             edge_scalar_properties())
            (dist_map, weight);
    }
}

void export_all_dists()
{
    python::def("get_all_dists", &get_all_dists);
};


/*!
  @file   PCA.cpp
  @author David Hirvonen
  @brief  Internal PCA class implementation.

  \copyright Copyright 2014-2016 Elucideye, Inc. All rights reserved.
  \license{This project is released under the 3 Clause BSD License.}

*/

#include "drishti/ml/PCA.h"
#include "drishti/core/make_unique.h"

#include <Eigen/Dense>

DRISHTI_ML_NAMESPACE_BEGIN

// ########## Scaling params ############

StandardizedPCA::Standardizer::Standardizer() = default;
StandardizedPCA::Standardizer::~Standardizer() = default;
StandardizedPCA::Standardizer::Standardizer(int size, int type)
{
    create(size, type);
}

void StandardizedPCA::Standardizer::create(int size, int type)
{
    mu.create(1, size, type);
    sigma.create(1, size, type);
}

void StandardizedPCA::Standardizer::compute(const cv::Mat& src)
{
    create(src.cols, CV_32FC1);
    float *pMu = mu.ptr<float>(), *pSigma = sigma.ptr<float>();
    for (int i = 0; i < src.cols; i++, pMu++, pSigma++)
    {
        cv::Scalar mu, sigma;
        cv::Mat x = src.col(i);
        cv::meanStdDev(x, mu, sigma);
        pMu[0] = mu[0];
        pSigma[0] = sigma[0];
    }
}

cv::Mat StandardizedPCA::Standardizer::standardize(const cv::Mat& data) const
{
    cv::Mat tmp_sigma = cv::repeat(sigma, data.rows / sigma.rows, data.cols / sigma.cols);
    cv::Mat tmp_mean = cv::repeat(mu, data.rows / mu.rows, data.cols / mu.cols), tmp_data;
    int ctype = mu.type();
    if (data.type() != ctype || tmp_mean.data == mu.data)
    {
        data.convertTo(tmp_data, ctype);
        cv::subtract(tmp_data, tmp_mean, tmp_data);
        cv::divide(tmp_data, tmp_sigma, tmp_data);
    }
    else
    {
        cv::subtract(data, tmp_mean, tmp_mean);
        cv::divide(tmp_mean, tmp_sigma, tmp_data);
    }
    return tmp_data;
}

cv::Mat StandardizedPCA::Standardizer::unstandardize(const cv::Mat& data) const
{
    cv::Mat tmp_sigma = cv::repeat(sigma, data.rows / sigma.rows, data.cols / sigma.cols);
    cv::Mat tmp_mean = cv::repeat(mu, data.rows / mu.rows, data.cols / mu.cols), tmp_data;
    int ctype = mu.type();
    if (data.type() != ctype || tmp_mean.data == mu.data)
    {
        data.convertTo(tmp_data, ctype);
        cv::multiply(tmp_data, tmp_sigma, tmp_data);
        cv::add(tmp_data, tmp_mean, tmp_data);
    }
    else
    {
        cv::multiply(data, tmp_sigma, tmp_data);
        cv::add(tmp_data, tmp_mean, tmp_data);
    }
    return tmp_data;
}

// ########### ScalePCA #############

StandardizedPCA::StandardizedPCA() = default;
StandardizedPCA::~StandardizedPCA() = default;

size_t StandardizedPCA::getNumComponents() const
{
    return m_transform.mu.cols;
}

void StandardizedPCA::compute(const cv::Mat& data, cv::Mat& projection, float retainedVariance)
{
    cv::Mat mu;
    m_transform.compute(data);
    cv::Mat data_ = m_transform.standardize(data);
    m_pca = drishti::core::make_unique<cv::PCA>(data_, mu, cv::PCA::DATA_AS_ROW, retainedVariance);
    m_pca->project(data_, projection);

    init();
}

void StandardizedPCA::compute(const cv::Mat& data, cv::Mat& projection, int maxComponents)
{
    cv::Mat mu;
    m_transform.compute(data);
    cv::Mat data_ = m_transform.standardize(data);
    m_pca = drishti::core::make_unique<cv::PCA>(data_, mu, cv::PCA::DATA_AS_ROW, maxComponents);
    m_pca->project(data_, projection);

    init();
}

void StandardizedPCA::init()
{
    // Cache transposed vectors for faster multiplication:
    if (!m_pca->eigenvectors.empty())
    {
        m_eT = m_pca->eigenvectors.t();
    }
}

cv::Mat StandardizedPCA::project(const cv::Mat& samples, int n) const
{
    cv::Mat samples_ = m_transform.standardize(samples), projection;
    if (n > 0)
    {
        // Construct partial eigenvectors
        cv::Mat eigenvectors = m_pca->eigenvectors({ 0, n }, cv::Range::all());
        cv::Mat eigenvalues = m_pca->eigenvalues({ 0, n }, cv::Range::all());
        cv::Mat mean = m_pca->mean;

        cv::Mat data = samples_;
        CV_Assert(!mean.empty() && !eigenvectors.empty() && ((mean.rows == 1 && mean.cols == data.cols) || (mean.cols == 1 && mean.rows == data.rows)));
        cv::Mat tmp_data, tmp_mean = cv::repeat(mean, data.rows / mean.rows, data.cols / mean.cols);
        int ctype = mean.type();
        if (data.type() != ctype || tmp_mean.data == mean.data)
        {
            data.convertTo(tmp_data, ctype);
            cv::subtract(tmp_data, tmp_mean, tmp_data);
        }
        else
        {
            cv::subtract(data, tmp_mean, tmp_mean);
            tmp_data = tmp_mean;
        }
        if (mean.rows == 1)
        {
            cv::gemm(tmp_data, eigenvectors, 1, {}, 0, projection, cv::GEMM_2_T);
        }
        else
        {
            cv::gemm(eigenvectors, tmp_data, 1, {}, 0, projection, 0);
        }
    }
    else
    {
        m_pca->project(samples_, projection);
    }
    return projection;
}

void StandardizedPCA::gemm_transpose(const cv::Mat& A, const cv::Mat& Bt, cv::Mat& result)
{
    assert(A.type() == CV_32F);
    assert(A.isContinuous());
    assert(A.cols == Bt.cols);
    assert(Bt.type() == CV_32F);

    using DynamicStride = Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>;
    using MatrixRowMajor = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    using MatrixColMajor = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
    using MapMatrixRowMajor = Eigen::Map<MatrixRowMajor, Eigen::Unaligned>;
    using MapMatrixColMajor = Eigen::Map<MatrixColMajor, Eigen::Unaligned, DynamicStride>;

    result.create(A.rows, Bt.rows, CV_32F);
    MapMatrixRowMajor C1(result.ptr<float>(), result.rows, result.cols);
    MapMatrixRowMajor A1(const_cast<float*>(A.ptr<float>()), A.rows, A.cols);
    MapMatrixColMajor B1t(const_cast<float*>(Bt.ptr<float>()), /*n*/ Bt.cols, Bt.rows, DynamicStride(Bt.step1(), 1));
    C1 = A1 * B1t;
}

cv::Mat StandardizedPCA::backProject(const cv::Mat& projection) const
{
    cv::Mat result;
    int n = projection.cols;
    if (n != m_pca->eigenvectors.cols)
    {
        // Construct partial eigenvectors
        cv::Mat eigenvectors = m_pca->eigenvectors({ 0, n }, cv::Range::all());
        cv::Mat eigenvalues = m_pca->eigenvalues({ 0, n }, cv::Range::all());
        cv::Mat mean = m_pca->mean;

        cv::Mat data = projection;
        CV_Assert(!mean.empty() && !eigenvectors.empty() && ((mean.rows == 1 && eigenvectors.rows == data.cols) || (mean.cols == 1 && eigenvectors.rows == data.rows)));

        cv::Mat tmp_data, tmp_mean;
        if (data.type() != tmp_data.type())
        {
            data.convertTo(tmp_data, mean.type()); // 12%
        }
        else
        {
            tmp_data = data;
        }
        if (mean.rows == 1)
        {
            tmp_mean = cv::repeat(mean, data.rows, 1);

            // Eigen multiplication ( no copy )
            const cv::Mat& A = tmp_data;
            const cv::Mat& Bt = m_eT;
            gemm_transpose(A, Bt({ 0, Bt.rows }, { 0, n }), result);
        }
        else
        {
            tmp_mean = cv::repeat(mean, 1, data.cols);
            cv::gemm(eigenvectors, tmp_data, 1, tmp_mean, 1, result, cv::GEMM_1_T);
        }
    }
    else
    {
        result = m_pca->backProject(projection);
    }

    result = m_transform.unstandardize(result);

    return result;
}

DRISHTI_ML_NAMESPACE_END


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file k_cut.hpp
 * @brief
 * @author Piotr Smulewicz, Piotr Godlewski
 * @version 1.0
 * @date 2013-09-25
 */
#ifndef PAAL_K_CUT_HPP
#define PAAL_K_CUT_HPP

#include "paal/utils/functors.hpp"
#include "paal/utils/type_functions.hpp"
#include "paal/utils/irange.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/graph/subgraph.hpp>
#include <boost/range/as_array.hpp>

#include <queue>

namespace paal {
namespace greedy {

/**
 * @brief this is solve k_cut problem
 * and return cut_cost
 * example:
 *  \snippet k_cut_example.cpp K Cut Example
 *
 * example file is k_cut_example.cpp
 * @param graph
 * @param number_of_parts
 * @param result pairs of vertex_descriptor and number form (1,2,
* ... ,k) id of part
 * @param index_map
 * @param weight_map
 * @tparam InGraph
 * @tparam OutputIterator
 * @tparam VertexIndexMap
 * @tparam EdgeWeightMap
 */
template<typename InGraph, class OutputIterator, typename VertexIndexMap, typename EdgeWeightMap>
auto k_cut(const InGraph& graph, unsigned int number_of_parts,OutputIterator result,
            VertexIndexMap index_map, EdgeWeightMap weight_map) ->
            typename boost::property_traits<EdgeWeightMap>::value_type{
    using cost_t = typename boost::property_traits<EdgeWeightMap>::value_type;
    using Vertex = typename boost::graph_traits<InGraph>::vertex_descriptor;

    using Graph = boost::adjacency_list<
        boost::vecS, boost::vecS, boost::undirectedS, boost::no_property,
        boost::property<boost::edge_weight_t, cost_t,
                        boost::property<boost::edge_index_t, int>>>;

    assert(num_vertices(graph) >= number_of_parts);

    std::vector<int> vertex_to_part(num_vertices(graph));
    using VertexIndexToVertex = typename std::vector<Vertex>;
    using VertexIndexToVertexIndex = std::vector<int>;
    VertexIndexToVertex vertex_in_subgraph_to_vertex(num_vertices(graph));
    VertexIndexToVertexIndex vertex_to_vertex_in_subgraph(num_vertices(graph));
    int vertex_in_part;
    int parts = 1;
    // cuts contain pair(x,y)
    // x is the cost of the cut
    // y and y+1 are index parts of graph after make a cut
    std::priority_queue<
            std::pair<cost_t,int>,
            std::vector<std::pair<cost_t,int> >
            ,utils::greater> cuts;

    int id_part = 0;

    //get part id and compute minimum cost of cut of that part and add it to queue
    auto make_cut = [&](int id) {
        vertex_in_part=0;
        for (auto v: boost::as_array(vertices(graph))) {
            if (vertex_to_part[get(index_map, v)] == id) {
                vertex_in_subgraph_to_vertex[vertex_in_part] = v;
                vertex_to_vertex_in_subgraph[get(index_map, v)] = vertex_in_part;
                ++vertex_in_part;
            }
        }
        Graph part(vertex_in_part);
        for (auto edge : boost::as_array(edges(graph))) {
            auto sour = get(index_map, source(edge,graph));
            auto targ = get(index_map, target(edge,graph));
            if (vertex_to_part[sour] == id &&
                    vertex_to_part[targ] == id &&
                    sour != targ) {
                add_edge(vertex_to_vertex_in_subgraph[sour],
                         vertex_to_vertex_in_subgraph[targ],
                         get(weight_map, edge),
                         part);
            }
        }
        if (vertex_in_part < 2) {
            ++id_part;
            *result = std::make_pair(vertex_in_subgraph_to_vertex[0], id_part);
            ++result;
            return;
        }
        auto parities = boost::make_one_bit_color_map(num_vertices(part),
                                            get(boost::vertex_index, part));
        auto cut_cost = boost::stoer_wagner_min_cut(part,
                                          get(boost::edge_weight, part),
                                          boost::parity_map(parities));

        for (auto i : irange(num_vertices(part))) {
            vertex_to_part[get(index_map, vertex_in_subgraph_to_vertex[i])] =
                    parts + get(parities, i); //return value convertable to 0/1
        }
        cuts.push(std::make_pair(cut_cost, parts));
        parts += 2;
    };

    make_cut(0);
    cost_t k_cut_cost = cost_t();
    while (--number_of_parts) {
        auto cut = cuts.top();
        cuts.pop();
        k_cut_cost += cut.first;
        make_cut(cut.second);
        make_cut(cut.second + 1);
    }

    while (!cuts.empty()) {
        auto cut = cuts.top();
        cuts.pop();
        ++id_part;
        for (auto v: boost::as_array(vertices(graph))) {
            if (vertex_to_part[get(index_map, v)] == cut.second ||
                    vertex_to_part[get(index_map, v)] == cut.second + 1) {
                *result = std::make_pair(v, id_part);
                ++result;
            }
        }
    }
    return k_cut_cost;
}

/**
 * @brief this is solve k_cut problem
 * and return cut_cost
 * example:
 *  \snippet k_cut_example.cpp K Cut Example
 *
 * example file is k_cut_example.cpp
 * @param graph
 * @param number_of_parts
 * @param result pairs of vertex_descriptor and number form (1,2, ... ,k) id of part
 * @param params
 * @tparam InGraph
 * @tparam OutputIterator
 * @tparam T
 * @tparam P
 * @tparam R
 */
template<typename InGraph
        ,class OutputIterator
        ,typename T
        ,typename P
        ,typename R>
auto k_cut(const InGraph& graph, unsigned int number_of_parts,
    OutputIterator result, const boost::bgl_named_params<P, T, R>& params) ->
        typename boost::property_traits<
            puretype(boost::choose_const_pmap(get_param(params, boost::edge_weight), graph, boost::edge_weight))
            >::value_type {
    return k_cut(graph, number_of_parts, result,
        boost::choose_const_pmap(get_param(params, boost::vertex_index), graph,boost::vertex_index),
        boost::choose_const_pmap(get_param(params, boost::edge_weight), graph,boost::edge_weight)
    );
}

/**
 * @brief this is solve k_cut problem
 * and return cut_cost
 * example:
 *  \snippet k_cut_example.cpp K Cut Example
 *
 * example file is k_cut_example.cpp
 * @param graph
 * @param number_of_parts
 * @param result pairs of vertex_descriptor and number form (1,2, ... ,k) id of part
 * @tparam InGraph
 * @tparam OutputIterator
 */
template<typename InGraph, class OutputIterator>
auto k_cut(const InGraph& graph, unsigned int number_of_parts, OutputIterator result) ->
        typename boost::property_traits<puretype(get(boost::edge_weight,graph))>::value_type{
    return k_cut(graph, number_of_parts, result, boost::no_named_parameters());
}

} //!greedy
} //!paal

#endif // PAAL_K_CUT_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_MAX_POS
#define MTL_MATRIX_MAX_POS

#include <cmath>
#include <utility>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/pos_type.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/operation/look_at_each_nonzero.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#if 0
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#endif

namespace mtl {

    namespace vec {

	template <typename Vector>
	struct max_pos_functor
	{
	    typedef typename Collection<Vector>::value_type       value_type;
	    typedef typename mtl::traits::pos_type<Vector>::type  pos_type;
	    typedef std::pair<value_type, pos_type>               result_type;

	    // initialize with max value and max position
	    max_pos_functor()
	    {
		value.first= math::identity(math::max<value_type>(), value_type()); // minimal value for comparison
		value.second= math::identity(math::min<pos_type>(), pos_type());    // maximal position to check if changed
	    }

	    void operator()(const value_type& x, const pos_type& p)
	    {
		if (x > value.first)
		    value= std::make_pair(x, p);
	    }

	    bool unchanged() const { return value.second == math::identity(math::min<pos_type>(), pos_type()); }

	    result_type  value;
	};
	///Returns position of maximal entry of %vector v
	template <typename Vector>
	typename max_pos_functor<Vector>::pos_type
	inline max_pos(const Vector& v)
	{
	    vampir_trace<2013> tracer;
	    max_pos_functor<Vector> f;
	    look_at_each_nonzero_pos(v, f);

	    MTL_DEBUG_THROW_IF(f.unchanged(), runtime_error("max_pos cannot be applied on empty container"));
	    return f.value.second;
	}

    } // namespace vector

    namespace mat {
	///Returns pair (row, col) from maximal entry of %matrix A
	using mtl::vec::max_pos;
    }

#if 0
namespace matrix {
///Returns pair (row, col) from maximal entry of %matrix A
    template <typename Matrix>
    std::pair<typename Collection<Matrix>::size_type, typename Collection<Matrix>::size_type>
    inline max_pos(const Matrix& A)
    {
	namespace traits = mtl::traits;
	typedef typename Collection<Matrix>::value_type   value_type;
	typedef typename Collection<Matrix>::size_type    size_type;

	value_type max(A[0][0]);
	size_type r= 0, c= 0;

	typename traits::row<Matrix>::type             row(A);
	typename traits::col<Matrix>::type             col(A);
	typename traits::const_value<Matrix>::type     value(A);
	typedef typename traits::range_generator<tag::major, Matrix>::type  cursor_type;

	for (cursor_type cursor = begin<tag::major>(A), cend = end<tag::major>(A); cursor != cend; ++cursor) {
	    typedef typename traits::range_generator<tag::nz, cursor_type>::type icursor_type;
	    for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
		if (value(*icursor) > max) {
		    max= value(*icursor);
		    r= row(*icursor);
		    c= col(*icursor);
		}
	}

	return std::make_pair(r, c);
}

} // namespace matrix

namespace vector {
///Returns position from maximal entry of %vector v
    template <typename Vector>
    typename Collection<Vector>::size_type
    inline max_pos(const Vector& v)
    {
	typedef typename Collection<Vector>::size_type    size_type;
	typedef typename Collection<Vector>::value_type   value_type;

	size_type i= 0;

	size_type max_col= size(v);
	value_type max(v[0]);

	for(size_type j= 1;i < max_col; j++)
	    if(v[j] > max) {
		max = v[j];
		i= j;
	    }
	return i;
    }

} // namespace vector

#endif

} // namespace mtl

#endif // MTL_MATRIX_MAX_POS


#include <armadillo>
#include <complex>

#include "Quaca.h"
#include "catch.hpp"

TEST_CASE("PolarizabilityNoBath constructors work as expected",
          "[PolarizabilityNoBath]") {

  SECTION("Direct constructor") {
    Polarizability pol(1.3, 6E-9, nullptr);
    REQUIRE(pol.get_omega_a() == 1.3);
    REQUIRE(pol.get_alpha_zero() == 6E-9);
    // Since no internal bath is implemented the get_mu should return 0
    // for any omega
    std::complex<double> test = pol.get_mu(3.0);
    REQUIRE(test.real() == 0.);
  }

  SECTION("json file constructor") {
    Polarizability pol("../data/test_files/PolarizabilityNoBath.json");
    REQUIRE(pol.get_omega_a() == 1.3);
    REQUIRE(pol.get_alpha_zero() == 6E-9);
    // Since no internal bath is implemented the get_mu should return 0
    // for any omega
    std::complex<double> test = pol.get_mu(3.0);
    REQUIRE(test.real() == 0.);
  }
}

TEST_CASE("PolarizabilityNoBath integrand works as expected",
          "[PolarizabilityNoBath]") {
  // define greens tensor
  double v = 0.1;
  double beta = 10;
  double relerr_k = 1E-9;
  auto greens = std::make_shared<GreensTensorVacuum>(v, beta, relerr_k);

  // define polarizability
  double omega_a = 3.0;
  double alpha_zero = 2.4;
  Polarizability pol(omega_a, alpha_zero, greens);

  // frequency to evaluate
  double omega = 3.0;

  // calculate as a reference the normal way
  cx_mat::fixed<3, 3> alpha;
  pol.calculate_tensor(omega, alpha, IM);

  // calculation with calculate_tensor give the same result as integrand
  // Matrix to store the result
  cx_mat::fixed<3, 3> alpha_int(fill::zeros);

  // loop over all indices
  for (size_t i = 0; i < 3; i++) {
    for (size_t j = 0; j < 3; j++) {
      alpha_int(i, j) = pol.integrand_omega(omega, {i, j}, IM);
    }
  }

  // Ensure non-trivial results
  REQUIRE(!alpha.is_zero());
  REQUIRE(!alpha_int.is_zero());

  if (!approx_equal(alpha, alpha_int, "reldiff", 1e-6)) {
    std::cout << alpha << alpha_int << alpha - alpha_int << std::endl;
  }
  REQUIRE(approx_equal(alpha, alpha_int, "reldiff", 1e-6));
};


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2017 Sebastian Koch <s.koch@tu-berlin.de> and Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <Eigen/Sparse>

#include "../../python_shared.h"

#include <igl/copyleft/comiso/nrosy.h>
#include <igl/copyleft/comiso/miq.h>

void python_export_igl_comiso(py::module &me) {

  py::module m = me.def_submodule(
    "comiso", "Wrappers for libigl functions that use comiso");

  #include "../../py_igl/copyleft/comiso/py_nrosy.cpp"
  #include "../../py_igl/copyleft/comiso/py_miq.cpp"

}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NETKET_VARIATIONALMONTECARLODM_HPP
#define NETKET_VARIATIONALMONTECARLODM_HPP

#include <complex>
#include <string>
#include <unordered_map>
#include <vector>

#include <Eigen/Dense>
#include <nonstd/optional.hpp>

#include "Machine/machine.hpp"
#include "Operator/abstract_operator.hpp"
#include "Optimizer/optimizer.hpp"
#include "Optimizer/stochastic_reconfiguration.hpp"
#include "Output/json_output_writer.hpp"
#include "Sampler/abstract_sampler.hpp"
#include "Sampler/vmc_sampling.hpp"
#include "Stats/stats.hpp"
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"
#include "common_types.hpp"

namespace netket {

// Variational Monte Carlo schemes to learn the ground state
// Available methods:
// 1) Stochastic reconfiguration optimizer
//   both direct and sparse version
// 2) Gradient Descent optimizer

/** TTComment: This class provides a central point to all of the
 *  calculations.
 */
class VariationalMonteCarloDM {
  /// TTComment: @ref reference to hamiltonian operator
  const AbstractOperator &ham_;
  /// TTComment: @ref reference to sampler
  AbstractSampler &sampler_;
  /// TTComment: @ref reference to machine
  AbstractMachine &psi_;

  int totalnodes_;
  int mynode_;

  /// TTComment: @ref reference to optimizer
  AbstractOptimizer &opt_;
  /** TTComment: @param dosr_ chooses between stochastic reconfiguration
   *  and gradient descent
   */
  SR sr_;
  bool dosr_;

  /** TTComment: @param obs_ is a vector holding any number of operators
   *  for which we want to know the expectation value
   */
  std::vector<const AbstractOperator *> obs_;
  std::vector<std::string> obsnames_;
  std::vector<Complex> obs_values_;

  using StatsMap = std::unordered_map<std::string, vmc::Stats>;
  StatsMap observable_stats_;

  /// TTComment: @param vmc_data_ - stores the vmc samples
  vmc::Result vmc_data_;
  Eigen::VectorXcd locvals_;
  /** TTComment: @param grad_ - stores the gradients of variational
   *  parameters.
   */
  Eigen::VectorXcd grad_;

  int nsamples_;
  int nsamples_node_;
  int ninitsamples_;
  int ndiscard_;

  int npar_;

  /** TTComment: @param target_ holds the information about the
   *  cost function.
   */
  std::string target_;
  const VectorXd gamma_;

 public:
  VariationalMonteCarloDM(const AbstractOperator &hamiltonian,
                        AbstractSampler &sampler, AbstractOptimizer &optimizer,
                        const VectorXd &gamma,
                        int nsamples, int discarded_samples = -1,
                        int discarded_samples_on_init = 0,
                        const std::string &target = "energy",
                        const std::string &method = "Sr",
                        double diag_shift = 0.01, bool use_iterative = false,
                        bool use_cholesky = true)
      : ham_(hamiltonian),
        sampler_(sampler),
        psi_(sampler.GetMachine()),
        opt_(optimizer),
        target_(target),
        gamma_(gamma){
    Init(nsamples, discarded_samples, discarded_samples_on_init, method,
         diag_shift, use_iterative, use_cholesky);
  }

  void Init(int nsamples, int discarded_samples, int discarded_samples_on_init,
            const std::string &method, double diag_shift, bool use_iterative,
            bool use_cholesky) {
    npar_ = psi_.Npar();
    opt_.Init(npar_, psi_.IsHolomorphic());
    grad_.resize(npar_);

    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    nsamples_ = nsamples;
    nsamples_node_ = int(std::ceil(double(nsamples_) / double(totalnodes_)));
    ninitsamples_ = discarded_samples_on_init;

    if (discarded_samples == -1) {
      ndiscard_ = 0.1 * nsamples_node_;
    } else {
      ndiscard_ = discarded_samples;
    }

    if (method == "Gd") {
      dosr_ = false;
      InfoMessage() << "Using a gradient-descent based method" << std::endl;
    } else {
      setSrParameters(diag_shift, use_iterative, use_cholesky);
    }

    if (target_ != "energy" && target_ != "variance" && target_ != "cost") {
      InvalidInputError(
          "Target minimization should be either energy or variance or cost\n");
    }

    InfoMessage() << "Variational Monte Carlo running on " << totalnodes_
                  << " processes" << std::endl;

    MPI_Barrier(MPI_COMM_WORLD);
  }

  void AddObservable(AbstractOperator &ob, const std::string &obname) {
    obs_.push_back(&ob);
    obsnames_.push_back(obname);
  }

  void InitSweeps() {
    sampler_.Reset();
    for (int i = 0; i < ninitsamples_; i++) {
      sampler_.Sweep();
    }
  }

  void Reset() {
    opt_.Reset();
    InitSweeps();
  }

  /**
   * Computes the expectation values of observables from the currently stored
   * samples.
   */
  void ComputeObservables() {
    for (std::size_t i = 0; i < obs_.size(); ++i) {
      auto ex = vmc::Expectation(vmc_data_, psi_, *obs_[i]);
      observable_stats_[obsnames_[i]] = ex;
    }
  }

  void ComputeObservablesDM()
  {
    for (std::size_t i = 0; i < obs_.size(); ++i)
    {
      auto ex = vmc::ExpectationDM(vmc_data_, psi_, *obs_[i]);
      obs_values_[i] = ex;
    }
  }

  /**
   * Advances the simulation by performing `steps` VMC iterations.
   */
  void Advance(Index steps = 1) {
    assert(steps > 0);
    for (Index i = 0; i < steps; ++i) {
      vmc_data_ = vmc::ComputeSamples(sampler_, nsamples_node_, ndiscard_);

      const auto energy = vmc::Expectation(vmc_data_, psi_, ham_, locvals_);
      const auto variance =
          vmc::Variance(vmc_data_, psi_, ham_, energy.mean, locvals_);

      observable_stats_["Energy"] = energy;
      observable_stats_["EnergyVariance"] = variance;

      if (target_ == "energy") {
        grad_ = vmc::Gradient(vmc_data_, psi_, ham_, locvals_);
      } else if (target_ == "variance") {
        grad_ = vmc::GradientOfVariance(vmc_data_, psi_, ham_);
      } else if (target_ == "cost") {
        grad_ = vmc::GradientDM(vmc_data_, psi_, ham_, gamma_);
      } else {
        throw std::runtime_error("This should not happen.");
      }

      UpdateParameters();
    }
  }

  void Run(const std::string &output_prefix,
           nonstd::optional<Index> n_iter = nonstd::nullopt,
           Index step_size = 1, Index save_params_every = 50) {
    assert(n_iter > 0);
    assert(step_size > 0);
    assert(save_params_every > 0);

    nonstd::optional<JsonOutputWriter> writer;
    if (mynode_ == 0) {
      writer.emplace(output_prefix + ".log", output_prefix + ".wf",
                     save_params_every);
    }
    opt_.Reset();

    for (Index step = 0; !n_iter.has_value() || step < *n_iter;
         step += step_size) {
      Advance(step_size);
      ComputeObservablesDM();

      // writer.has_value() iff the MPI rank is 0, so the output is only
      // written once
      if (writer.has_value()) {
        auto obs_data = json(observable_stats_);
        obs_data["Acceptance"] = sampler_.Acceptance();

        writer->WriteLog(step, obs_data);
        writer->WriteState(step, psi_);
      }
      MPI_Barrier(MPI_COMM_WORLD);
    }
  }

  void UpdateParameters() {
    auto pars = psi_.GetParameters();

    Eigen::VectorXcd deltap(npar_);

    if (dosr_) {
      sr_.ComputeUpdate(vmc_data_.LogDerivs()->transpose(), grad_, deltap);
    } else {
      deltap = grad_;
    }
    opt_.Update(deltap, pars);

    SendToAll(pars);

    psi_.SetParameters(pars);

    MPI_Barrier(MPI_COMM_WORLD);
  }

  void setSrParameters(double diag_shift = 0.01, bool use_iterative = false,
                       bool use_cholesky = true) {
    dosr_ = true;
    sr_.setParameters(diag_shift, use_iterative, use_cholesky,
                      psi_.IsHolomorphic());
  }

  AbstractMachine &GetMachine() { return psi_; }

  const StatsMap &GetObservableStats() const noexcept {
    return observable_stats_;
  }

  const vmc::Result &GetVmcData() const noexcept { return vmc_data_; }
};

}  // namespace netket

#endif