#ifndef __PARTICLE_FILTER_HAND_PREDICTION_HPP
#define __PARTICLE_FILTER_HAND_PREDICTION_HPP

#include <ctime>
#include <Eigen/Dense>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/normal_distribution.hpp>

#include "particle_filter_hand.hpp"
#include "particle_filter_base.hpp"

#define SMOOTH_FACTOR (1.)

class HandPredictionModel: public PredictionModel<HAND_STATE_SIZE>
{
private:
  double initQSigma;
  double initWSigma;
  double updateQSigma;
  double updateWSigma;

  void _updateParticle(Particle<HAND_STATE_SIZE> &p,
		       double deltaT, double qSigma, double wSigma,
		       boost::random::mt19937 &rng) const
  {
    // Quaternion kinematics from
    // Sola, Joan. "Quaternion kinematics for the error-state KF." Laboratoire d’Analyse et d’Architecture des Systemes-Centre national de la recherche scientifique (LAAS-CNRS), Toulouse, France, Tech. Rep (2012).
    boost::random::normal_distribution<double> Nq(0., qSigma);
    boost::random::normal_distribution<double> Nw(0., wSigma);
    boost::random::uniform_01<double> U;

    // Current state quaternions (orientation and angular velocity)
    Eigen::Quaternion<double> qt, wt;
    qt.w() = p.state[0];
    qt.x() = p.state[1];
    qt.y() = p.state[2];
    qt.z() = p.state[3];
    wt.w() = 0.;
    wt.x() = p.state[4];
    wt.y() = p.state[5];
    wt.z() = p.state[6];

    // Perturbation quaternions
    Eigen::Quaternion<double> deltaqt, deltawt;
    //deltaqt.w() =1. +Nq(rng);
    //deltaqt.x() = Nq(rng);
    //deltaqt.y() = Nq(rng);
    //deltaqt.z() = Nq(rng);
    Eigen::AngleAxisd deltaaxt;
    deltaaxt.angle() = Nq(rng);
    deltaaxt.axis().x() = -1.+2.*U(rng);
    deltaaxt.axis().y() = -1.+2.*U(rng);
    deltaaxt.axis().z() = -1.+2.*U(rng);
    deltaqt = deltaaxt;
    deltawt.w() = 0;
    deltawt.x() = Nw(rng);
    deltawt.y() = Nw(rng);
    deltawt.z() = Nw(rng);

    // Update quaternion rotation
    Eigen::Quaternion<double> qtw,qt1;
    // - quaternion update (for non zero velocity)
    if (deltaT)
    {
      qtw.w() = cos(wt.norm()*deltaT/2);
      qtw.x() = wt.x()/wt.norm()*sin(wt.norm()*deltaT/2);
      qtw.y() = wt.y()/wt.norm()*sin(wt.norm()*deltaT/2);
      qtw.z() = wt.z()/wt.norm()*sin(wt.norm()*deltaT/2);
      // - multiply by perturbation and update
      qt1 = deltaqt * qtw * qt;
    }
    else
    {
      // Null velocity, apply the random perturbation only
      qt1 = deltaqt * qt;
    }
	// Normalize
	qt1.normalize();

    // Update velocity
    Eigen::Quaternion<double> deltaq, wt1;
    Eigen::AngleAxisd axang;
    if (deltaT)
    {
      deltaq = qt1 * qt.inverse();
      axang = deltaq;
      wt1.w() = 0.;
      wt1.x() = axang.axis().x()*axang.angle()/deltaT + deltawt.x();
      wt1.y() = axang.axis().y()*axang.angle()/deltaT + deltawt.y();
      wt1.z() = axang.axis().z()*axang.angle()/deltaT + deltawt.z();
    }
    else
    {
      wt1.w() = 0.;
      wt1.x() = wt.x()+deltawt.x();
      wt1.y() = wt.y()+deltawt.y();
      wt1.z() = wt.z()+deltawt.z();
    }

    // Done, update particle status
    Eigen::Quaternion<double> iq;
    iq = qt.slerp(SMOOTH_FACTOR, qt1);
    p.state[0] = iq.w();
    p.state[1] = iq.x();
    p.state[2] = iq.y();
    p.state[3] = iq.z();

    p.state[4] = SMOOTH_FACTOR*wt1.x() + (1.-SMOOTH_FACTOR)*wt.x();
    p.state[5] = SMOOTH_FACTOR*wt1.y() + (1.-SMOOTH_FACTOR)*wt.y();
    p.state[6] = SMOOTH_FACTOR*wt1.z() + (1.-SMOOTH_FACTOR)*wt.z();
  }

  void _updateParticles(std::vector<Particle<HAND_STATE_SIZE> > &particles,
			double deltaT, double qSigma, double wSigma) const
  {
    boost::random::mt19937 rng(time(NULL));
    for (std::vector<Particle<HAND_STATE_SIZE> >::iterator it=particles.begin();
	 it!=particles.end(); ++it)
    {
      Particle<HAND_STATE_SIZE> &p = *it;
      _updateParticle(p, deltaT, qSigma, wSigma, rng);
    }
  }

public:
  HandPredictionModel(double initQSigma, double initWSigma,
		      double updateQSigma, double updateWSigma):
    initQSigma(initQSigma),
    initWSigma(initWSigma),
    updateQSigma(updateQSigma),
    updateWSigma(updateWSigma)
  {}

  void init(const double *initState,
	    std::vector<Particle<HAND_STATE_SIZE> > &particles) const
  {
    // Set all particle to the initial state
    for (std::vector<Particle<HAND_STATE_SIZE> >::iterator it=particles.begin();
	 it!=particles.end(); ++it)
    {
      Particle<HAND_STATE_SIZE> &p = *it;
      std::copy(initState, initState+HAND_STATE_SIZE, p.state);
      p.likelihood = 0.;
    }

    _updateParticles(particles, 0., initQSigma, initWSigma);
  }

  void update(std::vector<Particle<HAND_STATE_SIZE> > &particles, double deltaT) const
  {
    _updateParticles(particles, deltaT, updateQSigma, updateWSigma);
  }
};

#endif // __PARTICLE_FILTER_HAND_PREDICTION_HPP


#include <kv/eig.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <kv/dd.hpp>
#include <kv/rdd.hpp>

namespace ub = boost::numeric::ublas;

typedef kv::interval< kv::dd > itvdd;
typedef kv::interval<double> itv;

int main()
{
	ub::matrix<double> a;
	ub::matrix< kv::complex<double> > v, d;

	std::cout.precision(17);

	a.resize(2,2);

	a(0,0) = 1.;
	a(0,1) = 2.;
	a(1,0) = 3.;
	a(1,1) = 4.;

	// approximation
	eig(a, v, d);
	std::cout << v << "\n";
	std::cout << d << "\n";

	// verified
	ub::vector< kv::complex<itv> > l;
	veig(a, l);
	std::cout << l << "\n";

	// verified (interval matrix)
	ub::matrix<itv> ia;
	ia = a;
	veig(ia, l);
	std::cout << l << "\n";

	// verified (interval dd matrix)
	ub::matrix<itvdd> iadd;
	ub::vector< kv::complex<itvdd> > ldd;
	std::cout.precision(34);
	iadd = a;
	veig(iadd, ldd);
	std::cout << ldd << "\n";
}


//#define ARMA_NO_DEBUG

#include <iostream>
#include <armadillo>
#include <float.h>
#include <ctime>

using namespace std;
using namespace arma;

typedef double (*callback_type)(int, double[]);
typedef bool (*is_terminate_type)(long, int, double); // runid, iter, value -> isTerminate

static uvec inverse(const uvec& indices) {
    uvec inverse = uvec(indices.size());
    for (int i = 0; i < indices.size(); i++)
        inverse(indices(i)) = i;
    return inverse;
}

static vec sequence(double start, double end, double step) {
    int size = (int) ((end - start) / step + 1);
    vec d(size);
    double value = start;
    for (int r = 0; r < size; r++) {
        d(r) = value;
        value += step;
    }
    return d;
}

// wrapper around the fittness function, scales according to boundaries

class Fittness {

public:

    Fittness(callback_type pfunc, const vec &lower_limit,
            const vec &upper_limit) {
        func = pfunc;
        lower = lower_limit;
        upper = upper_limit;
        evaluationCounter = 0;
        if (lower.size() > 0) { // bounds defined
            scale = 0.5 * (upper - lower);
            typx = 0.5 * (upper + lower);
        }
    }

    void closestFeasible(vec &X) const { // in place
        if (lower.size() > 0)
            X.for_each([](double &val) {
                val = max(min(val, 1.0), -1.0);
            });
    }

    vec getClosestFeasible(const vec &col) const {
        if (lower.size() > 0) {
            vec X(col);
            X.for_each([](double &val) {
                val = max(min(val, 1.0), -1.0);
            });
            return X;
        }
        return col;
    }

    double eval(const vec &X) {
        int n = X.size();
        double parg[n];
        for (int i = 0; i < n; i++)
            parg[i] = X(i);
        double res = func(n, parg);
        evaluationCounter++;
        return res;
    }

    double value(const vec &X) {
        double value;
        if (lower.size() > 0) {
            return eval(decode(getClosestFeasible(X)));
        } else
            return eval(X);
    }

    vec encode(const vec &X) const {
        if (lower.size() > 0)
            return (X - typx) / scale;
        else
            return X;
    }

    vec decode(const vec &X) const {
        if (lower.size() > 0)
            return (X % scale) + typx;
        else
            return X;
    }

    int getEvaluations() {
        return evaluationCounter;
    }

private:
   callback_type func;
   vec lower;
   vec upper;
   long evaluationCounter;
   vec scale;
   vec typx;
};

class AcmaesOptimizer {

public:

    AcmaesOptimizer(long runid_, Fittness* fitfun_, int popsize_, int mu_, const vec &guess_, const vec &inputSigma_,
            int maxIterations_, int maxEvaluations_, double accuracy_, double stopfitness_, is_terminate_type isTerminate_) {
// runid used in isTerminate callback to identify a specific run at different iteration
        runid = runid_;
// fitness function to minimize
        fitfun = fitfun_;
// initial guess for the arguments of the fitness function
        guess = guess_;
// accuracy = 1.0 is default, > 1.0 reduces accuracy
        accuracy = accuracy_;
// callback to check if to terminate
        isTerminate = isTerminate_;
// Number of objective variables/problem dimension
        dim = guess_.size();
//     Population size, offspring number. The primary strategy parameter to play
//     with, which can be increased from its default value. Increasing the
//     population size improves global search properties in exchange to speed.
//     Speed decreases, as a rule, at most linearly with increasing population
//     size. It is advisable to begin with the default small population size.
        if (popsize_ > 0)
            popsize = popsize_;
        else
            popsize = 4 + int(3. * log(dim));
//     Individual sigma values - initial search volume. inputSigma determines
//     the initial coordinate wise standard deviations for the search. Setting
//     SIGMA one third of the initial search region is appropriate.
        if (inputSigma_.size() == 1)
            inputSigma = vec(dim).fill(inputSigma_(0));
        else
            inputSigma = inputSigma_;
// Overall standard deviation - search volume.
        sigma = max(inputSigma);
// termination criteria
// maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_;
// maximal number of iterations allowed.
        maxIterations = maxIterations_;
// Limit for fitness value.
        stopfitness = stopfitness_;
// Stop if x-changes larger stopTolUpX.
        stopTolUpX = 1e3 * sigma;
// Stop if x-change smaller stopTolX.
        stopTolX = 1e-11 * sigma * accuracy;
// Stop if fun-changes smaller stopTolFun.
        stopTolFun = 1e-12 * accuracy;
// Stop if back fun-changes smaller stopTolHistFun.
        stopTolHistFun = 1e-13 * accuracy;
// selection strategy parameters
// Number of parents/points for recombination.
        mu = mu_ > 0 ? mu_ : popsize / 2;
// Array for weighted recombination.
        weights = (log(sequence(1, mu, 1)) * -1.) + log(mu + 0.5);
        double sumw = sum(weights);
        double sumwq = sum(weights % weights);
        weights *= 1. / sumw;
// Variance-effectiveness of sum w_i x_i.
        mueff = sumw * sumw / sumwq;

// dynamic strategy parameters and constants
// Cumulation constant.
        cc = (4. + mueff / dim) / (dim + 4. + 2. * mueff / dim);
// Cumulation constant for step-size.
        cs = (mueff + 2.) / (dim + mueff + 3.);
// Damping for step-size.
           damps = (1. + 2. * ::max(0., ::sqrt((mueff - 1.)
                    / (dim + 1.)) - 1.))
                    * max(0.3, 1.
                            - // modification for short runs
                            dim / (1e-6 + min(maxIterations, maxEvaluations
                                    / popsize))) + cs; // minor increment
// Learning rate for rank-one update.
        ccov1 = 2. / ((dim + 1.3) * (dim + 1.3) + mueff);
// Learning rate for rank-mu update'
        ccovmu = min(1. - ccov1,
                2. * (mueff - 2. + 1. / mueff)
                        / ((dim + 2.) * (dim + 2.) + mueff));
// Expectation of ||N(0,I)|| == norm(randn(N,1)).
        chiN = sqrt(dim) * (1. - 1. / (4. * dim) + 1 / (21. * dim * dim));
        ccov1Sep = min(1., ccov1 * (dim + 1.5) / 3.);
        ccovmuSep = min(1. - ccov1, ccovmu * (dim + 1.5) / 3.);

// CMA internal values - updated each generation
// Objective variables.
        xmean = fitfun->encode(guess);
// Evolution path.
        pc = zeros<vec>(dim);
// Evolution path for sigma.
        ps = zeros<vec>(dim);
// Norm of ps, stored for efficiency.
        normps = norm(ps);
// Coordinate system.
        B = mat(dim,dim).eye();
// Diagonal of sqrt(D), stored for efficiency.
        diagD = inputSigma / sigma;
        diagC = square(diagD);
// B*D, stored for efficiency.
        BD = B % repmat(diagD.t(), dim, 1);
// Covariance matrix.
        C = B * (mat(dim,dim).eye() * B.t());
// Number of iterations already performed.
        iterations = 0;
// Size of history queue of best values.
        historySize = 10 + int(3. * 10. * dim / popsize);
// stop criteria
        stop = 0;
// best value so far
        bestValue = fitfun->value(xmean);
// best parameters so far
        bestX = guess;
// History queue of best values.
        fitnessHistory = vec(historySize).fill(DBL_MAX);
        fitnessHistory(0) = bestValue;
    }

    // param zmean weighted row matrix of the gaussian random numbers generating the current offspring
    // param xold xmean matrix of the previous generation
    // return hsig flag indicating a small correction

    bool updateEvolutionPaths(const vec& zmean, const vec& xold) {
        ps = ps * (1. - cs) + ((B * zmean) * sqrt(cs * (2. - cs) * mueff));
        normps = norm(ps);
        bool hsig = normps / sqrt(1. - pow(1. - cs, 2. * iterations)) / chiN < 1.4 + 2. / (dim + 1.);
        pc *= (1. - cc);
        if (hsig)
            pc += (xmean - xold) * (sqrt(cc * (2. - cc) * mueff) / sigma);
        return hsig;
    }

    // param hsig flag indicating a small correction
    // param bestArx fitness-sorted matrix of the argument vectors producing the current offspring
    // param arz unsorted matrix containing the gaussian random values of the current offspring
    // param arindex indices indicating the fitness-order of the current offspring
    // param xold xmean matrix of the previous generation

    double updateCovariance(bool hsig, const mat& bestArx, const mat& arz,
            const uvec& arindex, const mat& xold) {
        double negccov = 0;
        if (ccov1 + ccovmu > 0) {
            mat arpos = (bestArx - repmat(xold, 1, mu)) * (1. / sigma); // mu difference vectors
            mat roneu = pc * pc.t() * ccov1;
            // minor correction if hsig==false
            double oldFac = hsig ? 0 : ccov1 * cc * (2. - cc);
            oldFac += 1. - ccov1 - ccovmu;
            // Adapt covariance matrix C active CMA
            negccov = (1. - ccovmu) * 0.25 * mueff
                    / (pow(dim + 2., 1.5) + 2. * mueff);
            double negminresidualvariance = 0.66;
            // keep at least 0.66 in all directions, small popsize are most critical
            double negalphaold = 0.5; // where to make up for the variance loss,
            // prepare vectors, compute negative updating matrix Cneg
            uvec arReverseIndex = reverse(arindex);
            mat arzneg = arz.cols(arReverseIndex.head(mu));
            vec arnorms = sqrt(sum(square(arzneg.t()), 1)); // rowsum
            uvec idxnorms = sort_index(arnorms);
            vec arnormsSorted = arnorms(idxnorms);
            uvec idxReverse = reverse(idxnorms);
            vec arnormsReverse = arnorms(idxReverse);
            arnorms = arnormsReverse / arnormsSorted;
            vec arnormsInv = arnorms(inverse(idxnorms));
            mat sqarnw = square(arnormsInv).t() * weights;
            double negcovMax = (1. - negminresidualvariance) / sqarnw(0);
            if (negccov > negcovMax)
                negccov = negcovMax;
            arzneg = arzneg % repmat(arnormsInv.t(), dim, 1);
            mat artmp = BD * arzneg;
            mat Cneg = artmp * diagmat(weights) * artmp.t();
            oldFac += negalphaold * negccov;
            C = (C * oldFac) + roneu
                    + ( arpos * (ccovmu + (1. - negalphaold) * negccov) *
                               (repmat(weights, 1, dim) % arpos.t()))
                    - (Cneg * negccov);
        }
        return negccov;
    }

    // Update B and diagD from C
    // param negccov Negative covariance factor.

    void updateBD(double negccov) {

        if (ccov1 + ccovmu + negccov > 0
                && (std::fmod(iterations, 1. / (ccov1 + ccovmu + negccov) / dim / 10.)) < 1.) {
            // to achieve O(N^2) enforce symmetry to prevent complex numbers
            C = trimatu(C) + trimatu(C, 1).t();
            // diagD defines the scaling
            vec eigval;
            mat eigvec;
            eig_sym(eigval, eigvec, C);

            diagD = reverse(eigval); // descending order of eigenvalues
            for (int i = 0; i < dim; i++)
                B.col(i) = eigvec.col(dim - 1 - i);

            if (diagD.min() <= 0) {
                for (int i = 0; i < dim; i++)
                    if (diagD(i, 0) < 0)
                        diagD(i, 0) = 0.;
                double tfac = diagD.max() / 1e14;
                C += mat(dim,dim).eye() * tfac;
                diagD += ones(dim, 1) * tfac;
            }
            if (diagD.max() > 1e14 * diagD.min()) {
                double tfac = diagD.max() / 1e14 - diagD.min();
                C += mat(dim,dim).eye() * tfac;
                diagD += ones(dim, 1) * tfac;
            }
            diagC = C.diag();
            diagD = sqrt(diagD); // D contains standard deviations now
            BD = B % repmat(diagD.t(), dim, 1);
        }
    }

    void doOptimize() {

        // -------------------- Generation Loop --------------------------------

        for (iterations = 1; iterations <= maxIterations &&
                    fitfun->getEvaluations() < maxEvaluations; iterations++) {
            // Generate and evaluate popsize offspring
            mat arz = mat(dim, popsize).randn();
            mat arx = mat(dim, popsize);
            vec fitness = vec(popsize).fill(DBL_MAX);
            // generate random offspring
            fitfun->closestFeasible(xmean);
            for (int k = 0; k < popsize; k++) {
                vec delta = (BD * arz.col(k)) * sigma;
                arx.col(k) = fitfun->getClosestFeasible(xmean + delta);
                fitness[k] = fitfun->value(arx.col(k)); // compute fitness
                if (!isfinite(fitness[k])) {
                    stop = -1;
                    break;
                }
            }
            if (stop != 0)
                break;
            // Sort by fitness and compute weighted mean into xmean
            uvec arindex = sort_index(fitness);

            // Calculate new xmean, this is selection and recombination
            vec xold = xmean; // for speed up of Eq. (2) and (3)
            uvec bestIndex = arindex.head(mu);
            mat bestArx = arx.cols(bestIndex);
            xmean = bestArx * weights;
            mat bestArz = arz.cols(bestIndex);
            mat zmean = bestArz * weights;

            bool hsig = updateEvolutionPaths(zmean, xold);
            double negccov = updateCovariance(hsig, bestArx, arz, arindex, xold);
            updateBD(negccov);
            // Adapt step size sigma - Eq. (5)
            sigma *= exp(min(1.0, (normps / chiN - 1.) * cs / damps));
            double bestFitness = fitness(arindex(0));
            double worstFitness = fitness(arindex(arindex.size()-1));
            if (bestValue > bestFitness) {
                bestValue = bestFitness;
                bestX = fitfun->decode(bestArx.col(0));
            }

            // handle termination criteria
            if (isfinite(stopfitness) && bestFitness < stopfitness) {
                stop = 1;
                break;
            }
            vec sqrtDiagC = sqrt(diagC);
            vec pcCol = pc;

            for (int i = 0; i < dim; i++) {
                if (sigma * (max(abs(pcCol[i]), sqrtDiagC[i])) > stopTolX)
                    break;
                if (i >= dim - 1)
                    stop = 2;
            }
            for (int i = 0; i < dim; i++)
                if (sigma * sqrtDiagC[i] > stopTolUpX)
                    stop = 3;
            if (stop > 0)
                break;
            double historyBest = min(fitnessHistory);
            double historyWorst = max(fitnessHistory);
            if (iterations > 2
                    && max(historyWorst, worstFitness)
                            - min(historyBest, bestFitness) < stopTolFun) {
                stop = 4;
                break;
            }
            if (iterations > fitnessHistory.size()
                    && historyWorst - historyBest < stopTolHistFun) {
                stop = 5;
                break;
            }
            // condition number of the covariance matrix exceeds 1e14
            if ( diagD.max() / diagD.min() > 1e7 * 1.0 / sqrt(accuracy)) {
                stop = 6;
                break;
            }
            if (isTerminate != NULL && isTerminate(runid, iterations, bestValue)) {
                stop = 7;
                break;
            }
            // Adjust step size in case of equal function values (flat fitness)
            if (bestValue == fitness[arindex[(int) (0.1 + popsize / 4.)]]) {
                sigma *= exp(0.2 + cs / damps);
            }
            if (iterations > 2
                    && max(historyWorst, bestFitness)
                            - ::min(historyBest, bestFitness) == 0) {
                sigma *= ::exp(0.2 + cs / damps);
            }
            // store best in history
            for (int i = 1; i < fitnessHistory.size(); i++)
                fitnessHistory[i] = fitnessHistory[i - 1];
            fitnessHistory[0] = bestFitness;
        }
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestValue;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

private:
      long runid;
      Fittness* fitfun;
      vec guess;
      double accuracy;
      is_terminate_type isTerminate;
      int popsize; // population size
      vec inputSigma;
      int dim;
      int maxIterations;
      int maxEvaluations;
      double stopfitness;
      double stopTolUpX;
      double stopTolX;
      double stopTolFun;
      double stopTolHistFun;
      int mu; //
      vec weights;
      double mueff; //
      double sigma;
      double cc;
      double cs;
      double damps;
      double ccov1;
      double ccovmu;
      double chiN;
      double ccov1Sep;
      double ccovmuSep;
      vec xmean;
      vec pc;
      vec ps;
      double normps;
      mat B;
      mat BD;
      mat diagD;
      mat C;
      vec diagC;
      int iterations;
      vec fitnessHistory;
      int historySize;
      double bestValue;
      vec bestX;
      int stop;
};

// see https://cvstuff.wordpress.com/2014/11/27/wraping-c-code-with-python-ctypes-memory-and-pointers/

extern "C" {
    void seed(int s) {
        arma_rng::set_seed(s);
    }
}

extern "C" {
    void seedRandom() {
        arma_rng::set_seed_random();
    }
}

extern "C" {
    void free_mem(double* a) {
        delete[] a;
    }
}

extern "C" {
    double* optimizeACMA_C(long runid, callback_type func, int dim, double *init,
            double *lower, double *upper, double *sigma, int maxIter, int maxEvals,
            double stopfitness, int mu, int popsize, double accuracy,
            bool useTerminate, is_terminate_type isTerminate) {
        int n = dim;
        double *res = new double[n + 4];
        vec guess(n), lower_limit(n), upper_limit(n), inputSigma(n);
        bool useLimit = false;
        for (int i = 0; i < n; i++) {
            guess[i] = init[i];
            inputSigma[i] = sigma[i];
            lower_limit[i] = lower[i];
            upper_limit[i] = upper[i];
            useLimit |= (lower[i] != 0);
            useLimit |= (upper[i] != 0);
        }
        if (useLimit == false) {
            lower_limit.resize(0);
            upper_limit.resize(0);
        }
        Fittness fitfun(func, lower_limit, upper_limit);
        AcmaesOptimizer opt(
            runid,
            &fitfun,
            popsize,
            mu,
            guess,
            inputSigma,
            maxIter,
            maxEvals,
            accuracy,
            stopfitness,
            useTerminate ? isTerminate : NULL);
        try {
            opt.doOptimize();
            vec bestX = opt.getBestX();
            double bestY = opt.getBestValue();
            for (int i = 0; i < n; i++)
                res[i] = bestX[i];
            res[n] = bestY;
            res[n+1] = fitfun.getEvaluations();
            res[n+2] = opt.getIterations();
            res[n+3] = opt.getStop();
            return res;
        } catch (std::exception& e) {
            cout << e.what() << endl;
            return res;
        }
    }
}


#include <boost/math/differentiation/autodiff.hpp>
#include <iostream>

using namespace std;
using namespace boost::math::differentiation;

constexpr double PI = M_PI;

template<typename X>
auto sqr(const X& x) { return x * x; }

#include <random>

double clamp(double x, double min_x, double max_x) {
    if (x > max_x) return max_x;
    if (x < min_x) return min_x;
    return x;
}

// TODO adam, vector as input, benchmarks

template<typename Func>
void optimize(const char* name, double range, bool newton, const Func& func) {
    cout << name << endl;
    std::mt19937 rng(0);

    double best = 1e100;
    std::uniform_real_distribution<double> unif(-range, range);
    double best_wx = unif(rng), best_wy = unif(rng);
    double dev = range / 3;
    size_t evals = 100 * 1000 * 1000;
    const double alpha = newton ? 1 : 1e-2;
    size_t misses = 0;

    for (size_t j = 0; evals > 0; j++) {
        double wx = clamp(std::normal_distribution<>(best_wx, dev)(rng), -range, range);
        double wy = clamp(std::normal_distribution<>(best_wy, dev)(rng), -range, range);
        bool improved = false;
        for (size_t i = 0; i < 10000; i++) {
            double result, dx, dy, d2x = 1, d2y = 1;
            if (newton) {
                const auto in = make_ftuple<double, 2, 2>(wx, wy);
                const auto& f = func(std::get<0>(in), std::get<1>(in));
                result = f.derivative(0, 0);
                dx = f.derivative(1, 0);
                dy = f.derivative(0, 1);
                d2x = f.derivative(2, 0);
                d2y = f.derivative(0, 2);
            } else {
                const auto in = make_ftuple<double, 1, 1>(wx, wy);
                const auto& f = func(std::get<0>(in), std::get<1>(in));
                result = f.derivative(0, 0);
                dx = f.derivative(1, 0);
                dy = f.derivative(0, 1);
            }
            if (result < best) {
                best = result;
                best_wx = wx;
                best_wy = wy;
                improved = true;
            }
            if (isnan(dx) || isnan(dy) || isnan(d2x) || isnan(d2y)) {
                cout << "nan gradients at " << wx << " " << wy << endl;
                return;
            }
            wx -= alpha * dx / d2x;
            wy -= alpha * dy / d2y;
            if (abs(dx) < 1e-15 && abs(dy) < 1e-15) break;
            if (--evals == 0) break;
            if (wx < -range || wx > range) break;
            if (wy < -range || wy > range) break;
        }
        if (improved) {
            misses = 0;
            dev *= 0.95;
            cout << setprecision(15) << "x=" << best_wx << " y=" << best_wy << " f=" << best << " dev=" << dev << endl;
        } else {
            misses += 1;
            if (misses == 1000)
                break;
        }
    }
    cout << endl;
}

#define Optimize(A, B, C) \
    optimize(A, B, false, [](const auto& x, const auto& y){C;}); \
    optimize(A, B, true, [](const auto& x, const auto& y){C;})

int main() {
    Optimize("sphere", 10, return sqr(x) + sqr(y));
    Optimize("booth", 10, return sqr(x + 2*y - 7) + sqr(2*x + y - 5));
    Optimize("rastrigin", 5.12, return 20 + (x*x - 10*cos(2*PI*x)) + (y*y - 10*cos(2*PI*y)));
    Optimize("rosenbrock", 10, return 100*sqr(y - x*x) + sqr(1 - x));
    Optimize("easom", 100, return -cos(x)*cos(y)*exp(-sqr(x - PI) - sqr(y - PI)));
    Optimize("himmelblau", 5, return sqr(x*x + y - 11) + sqr(x + y*y - 7));
    Optimize("styblinski–tang", 5, return x*x*x*x - 16*x*x + 5*x + y*y*y*y - 16*y*y + 5*y);
    Optimize("schaffer n.2", 100, return 0.5 + (sqr(sin(x*x - y*y)) - 0.5) / sqr(1 + 0.001*(x*x + y*y)));
    Optimize("hölder table", 10, return -abs(sin(x)*cos(y)*exp(abs(1 - sqrt(x*x + y*y)/PI))));
}


#include <boost/lexical_cast.hpp>
#include "Poisson.hh"
#include "TMath.h"
#include <Eigen/Core>
#include <cmath>

using namespace Eigen;

Poisson::Poisson(bool ln_approx) {
  transformation_("poisson")
    .output("poisson")
    .types(&Poisson::checkTypes)
    .func(ln_approx ? &Poisson::calcPoissonApprox : &Poisson::calcPoisson)
    ;
  m_transform = t_["poisson"];
}

void Poisson::add(SingleOutput &theory, SingleOutput &data) {
  t_["poisson"].input(theory);
  t_["poisson"].input(data);
}

void Poisson::checkTypes(TypesFunctionArgs fargs) {
  auto& args=fargs.args;
  auto& rets=fargs.rets;
  if (args.size()%2 != 0) {
    throw args.undefined();
  }
  for (size_t i = 0; i < args.size(); i+=2) {
    if (args[i+0].shape.size() != 1) {
      throw rets.error(rets[0], "non-vector theory");
    }

    if (args[i+1].shape != args[i+0].shape) {
      throw rets.error(rets[0], "data and theory have different shape");
    }
  }
  rets[0] = DataType().points().shape(1);
}

double addOneInLnGamma(double in)
{
  return TMath::LnGamma(in + 1);
}

double lnFactorialApprox(double x)
{
  if (!(x == 0.0 || x == 1.0))
  {
    return x * std::log(x);
  }
  else
    return 0;
}

void Poisson::calcPoissonApprox(FunctionArgs fargs) {
  /***************************************************************************
   *       Formula: log of Poisson
   *
   *        -2 * ln(Poisson) =
   *            -2 * sum(data_i * log(theory_j) -  theory_j  - ln data_i! )
   *
   ****************************************************************************/
  auto& args=fargs.args;

  double res(0.0);
  for (size_t i = 0; i < args.size(); i+=2) {
    auto& theory=args[i+0].arr;
    auto& data=args[i+1].arr;
    res += (data*theory.log() - theory - data.unaryExpr(&lnFactorialApprox)).sum();
  }
  fargs.rets[0].arr(0) = -2*res;
}

void Poisson::calcPoisson(FunctionArgs fargs) {
  /***************************************************************************
   *       Formula: log of Poisson
   *
   *        -2 * ln(Poisson) =
   *               -2 * sum(data_i * log(theory_j) -  theory_j  - ln data_i! )
   *
   ****************************************************************************/
  auto& args=fargs.args;

  double res(0.0);
  for (size_t i = 0; i < args.size(); i+=2) {
    auto& theory=args[i+0].arr;
    auto& data=args[i+1].arr;
    res += (data*theory.log() - theory - data.unaryExpr(&addOneInLnGamma) ).sum();
  }
  fargs.rets[0].arr(0) = -2*res;
}


// Copyright (c) 2015
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#define BOOST_RATIO_EXTENSIONS
#include <boost/ratio.hpp>
using namespace boost;

//////////////////////////////////////////

void case1()
{
    typedef ratio<1, 2> half;

    assert(half::num == 1);
    assert(half::den == 2);

    auto v = half::value();
    cout << v << endl;
    assert(v * 2 == 1);

    half frac;
    assert(frac().numerator() == frac.num);

    typedef ratio<2, 4> two_fourth;
    cout << two_fourth()() << endl;
    assert(half::value() == two_fourth::value());

    typedef ratio<12> dozen;
    assert(2* dozen()() == 24);
}

//////////////////////////////////////////
typedef ratio<1, 2> half;
typedef ratio<1, 4> quater;

typedef ratio<12, 1> dozen;
typedef ratio<kilo::num*10, 1> cn_wan;

void case2()
{
    assert(kilo::num < kibi::num);
    cout << kilo::num << endl;
    cout << kibi::num << endl;

    assert((quater())()*2 == half()());
    assert((mega())() == cn_wan()()*100);
}

//////////////////////////////////////////
boost::intmax_t operator"" _kb(unsigned long long n)
{
    return n * boost::kibi::num;
}

boost::intmax_t operator"" _gb(unsigned long long n)
{
    return n * boost::gibi::num;
}

void case3()
{
    auto x = 2_gb;
    auto y = 10_kb;

    assert(x = 2 * 100 * y);
}

//////////////////////////////////////////
template<typename R>
using string_out = ratio_string<R, char>;

void case4()
{
    cout << string_out<kilo>::prefix() << endl;
    cout << string_out<kilo>::symbol() << endl;

    cout << string_out<nano>::prefix() << endl;
    cout << string_out<nano>::symbol() << endl;

    cout << string_out<ratio<22, 7>>::prefix() << endl;
}

//////////////////////////////////////////

int main()
{
    case1();
    case2();
    case3();
    case4();
}


//
//  Copyright (C) Toon Knapen 2003
//
// 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)
//

#ifndef BOOST_NUMERIC_BINDINGS_BLAS_BLAS3_OVERLOADS_HPP
#define BOOST_NUMERIC_BINDINGS_BLAS_BLAS3_OVERLOADS_HPP

#include <boost/numeric/bindings/blas/blas.h>
#include <boost/numeric/bindings/traits/type_traits.hpp>

namespace boost { namespace numeric { namespace bindings { namespace blas { namespace detail {

  using namespace boost::numeric::bindings::traits ;

  inline
  void gemm( char TRANSA, char TRANSB, const integer_t& m, const integer_t& n, const integer_t& k, const float    & alpha, const float    * a_ptr, const integer_t& lda, const float    * b_ptr, const integer_t& ldb, const float    & beta, float    * c_ptr, const integer_t& ldc ) { BLAS_SGEMM( &TRANSA, &TRANSB, &m, &n, &k,          ( &alpha ),          ( a_ptr ), &lda,          ( b_ptr ), &ldb,          ( &beta ),          ( c_ptr ), &ldc ) ; }
  inline
  void gemm( char TRANSA, char TRANSB, const integer_t& m, const integer_t& n, const integer_t& k, const double   & alpha, const double   * a_ptr, const integer_t& lda, const double   * b_ptr, const integer_t& ldb, const double   & beta, double   * c_ptr, const integer_t& ldc ) { BLAS_DGEMM( &TRANSA, &TRANSB, &m, &n, &k,          ( &alpha ),          ( a_ptr ), &lda,          ( b_ptr ), &ldb,          ( &beta ),          ( c_ptr ), &ldc ) ; }
  inline
  void gemm( char TRANSA, char TRANSB, const integer_t& m, const integer_t& n, const integer_t& k, const complex_f& alpha, const complex_f* a_ptr, const integer_t& lda, const complex_f* b_ptr, const integer_t& ldb, const complex_f& beta, complex_f* c_ptr, const integer_t& ldc ) { BLAS_CGEMM( &TRANSA, &TRANSB, &m, &n, &k, complex_ptr( &alpha ), complex_ptr( a_ptr ), &lda, complex_ptr( b_ptr ), &ldb, complex_ptr( &beta ), complex_ptr( c_ptr ), &ldc ) ; }
  inline
  void gemm( char TRANSA, char TRANSB, const integer_t& m, const integer_t& n, const integer_t& k, const complex_d& alpha, const complex_d* a_ptr, const integer_t& lda, const complex_d* b_ptr, const integer_t& ldb, const complex_d& beta, complex_d* c_ptr, const integer_t& ldc ) { BLAS_ZGEMM( &TRANSA, &TRANSB, &m, &n, &k, complex_ptr( &alpha ), complex_ptr( a_ptr ), &lda, complex_ptr( b_ptr ), &ldb, complex_ptr( &beta ), complex_ptr( c_ptr ), &ldc ) ; }

  //
  // SYRK
  //
  inline
  void syrk( char uplo, char trans, const integer_t& n, const integer_t& k, const float& alpha,
             const float* a_ptr, const integer_t lda, const float& beta, float* c_ptr,
             const integer_t& ldc)
  {
     BLAS_SSYRK( &uplo, &trans, &n, &k, &alpha, a_ptr, &lda, &beta, c_ptr, &ldc);
  }

  inline
  void syrk( char uplo, char trans, const integer_t& n, const integer_t& k, const double& alpha,
             const double* a_ptr, const integer_t lda, const double& beta, double* c_ptr,
             const integer_t& ldc)
  {
     BLAS_DSYRK( &uplo, &trans, &n, &k, &alpha, a_ptr, &lda, &beta, c_ptr, &ldc);
  }

  inline
  void syrk( char uplo, char trans, const integer_t& n, const integer_t& k, const complex_f& alpha,
             const complex_f* a_ptr, const integer_t lda, const complex_f& beta, complex_f* c_ptr,
             const integer_t& ldc)
  {
     BLAS_CSYRK( &uplo, &trans, &n, &k, complex_ptr( &alpha ), complex_ptr( a_ptr ),
                 &lda, complex_ptr( &beta ), complex_ptr( c_ptr ), &ldc);
  }

  inline
  void syrk( char uplo, char trans, const integer_t& n, const integer_t& k, const complex_d& alpha,
             const complex_d* a_ptr, const integer_t lda, const complex_d& beta, complex_d* c_ptr,
             const integer_t& ldc)
  {
     BLAS_ZSYRK( &uplo, &trans, &n, &k, complex_ptr( &alpha ), complex_ptr( a_ptr ),
                 &lda, complex_ptr( &beta ), complex_ptr( c_ptr ), &ldc);
  }

  //
  // HERK
  //
  inline
  void herk( char uplo, char trans, const integer_t& n, const integer_t& k, const float& alpha,
             const float* a_ptr, const integer_t lda, const float& beta, float* c_ptr,
             const integer_t& ldc)
  {
     BLAS_SSYRK( &uplo, &trans, &n, &k, &alpha, a_ptr, &lda, &beta, c_ptr, &ldc);
  }

  inline
  void herk( char uplo, char trans, const integer_t& n, const integer_t& k, const double& alpha,
             const double* a_ptr, const integer_t lda, const double& beta, double* c_ptr,
             const integer_t& ldc)
  {
     BLAS_DSYRK( &uplo, &trans, &n, &k, &alpha, a_ptr, &lda, &beta, c_ptr, &ldc);
  }

  inline
  void herk( char uplo, char trans, const integer_t& n, const integer_t& k, const float& alpha,
             const complex_f* a_ptr, const integer_t lda, const float& beta, complex_f* c_ptr,
             const integer_t& ldc)
  {
     BLAS_CHERK( &uplo, &trans, &n, &k, &alpha, complex_ptr( a_ptr ),
                 &lda, &beta, complex_ptr( c_ptr ), &ldc);
  }

  inline
  void herk( char uplo, char trans, const integer_t& n, const integer_t& k, const double& alpha,
             const complex_d* a_ptr, const integer_t lda, const double& beta, complex_d* c_ptr,
             const integer_t& ldc)
  {
     BLAS_ZHERK( &uplo, &trans, &n, &k, &alpha, complex_ptr( a_ptr ),
                 &lda, &beta, complex_ptr( c_ptr ), &ldc);
  }

  //
  // trsm
  //
  inline
  void trsm( char side, char uplo, char transa, char diag, integer_t m, integer_t n,
             float const& alpha, float const* a_ptr, integer_t lda,
             float* b_ptr, integer_t ldb )
  {
     BLAS_STRSM( &side, &uplo, &transa, &diag, &m, &n, &alpha, a_ptr, &lda, b_ptr, &ldb ) ;
  }

  inline
  void trsm( char side, char uplo, char transa, char diag, integer_t m, integer_t n,
             double const& alpha, double const* a_ptr, integer_t lda,
             double* b_ptr, integer_t ldb )
  {
     BLAS_DTRSM( &side, &uplo, &transa, &diag, &m, &n, &alpha, a_ptr, &lda, b_ptr, &ldb ) ;
  }

  inline
  void trsm( char side, char uplo, char transa, char diag, integer_t m, integer_t n,
             complex_f const& alpha, complex_f const* a_ptr, integer_t lda,
             complex_f* b_ptr, integer_t ldb )
  {
     BLAS_CTRSM( &side, &uplo, &transa, &diag, &m, &n, complex_ptr( &alpha ), complex_ptr( a_ptr ), &lda, complex_ptr( b_ptr ), &ldb ) ;
  }

  inline
  void trsm( char side, char uplo, char transa, char diag, integer_t m, integer_t n,
             complex_d const& alpha, complex_d const* a_ptr, integer_t lda,
             complex_d* b_ptr, integer_t ldb )
  {
     BLAS_ZTRSM( &side, &uplo, &transa, &diag, &m, &n, complex_ptr( &alpha ), complex_ptr( a_ptr ), &lda, complex_ptr( b_ptr ), &ldb ) ;
  }

}}}}}

#endif // BOOST_NUMERIC_BINDINGS_BLAS_BLAS3_OVERLOADS_HPP


#include <algorithm>
#include <iostream>
#include <vector>

#include <boost/range/counting_range.hpp>

struct Entity {
	std::int64_t health;
	std::int64_t damage;
	std::int64_t armor;
};

struct Item {
	std::int64_t price;
	std::int64_t dmg_bonus;
	std::int64_t ac_bonus;
};

template<typename T>
auto factorial(T n) -> T {
    if(n > 1) {
        return n * factorial(n - 1);
    }
    return 1;
}

template<typename N, typename K>
auto binomial_coefficient(N n, K k) {
	return factorial(n) / (factorial(k) * factorial(n-k));
}

// NOTE: this function is adapted from https://stackoverflow.com/a/9430993/699211
// It works by creating a "selection array", where we place k selectors, then
// we create all permutations of these selectors, and add the corresponding set
// member if it is selected in in the current permutation of the selectors.
auto get_rings() {

	auto rings = std::vector<Item>{{}, {}, {25,1,0}, {50,2,0}, {100,3,0}, {20,0,1}, {40,0,2}, {80,0,3}};

	// a combination of n rings with k elements in each combination
	const auto n = rings.size();
	const auto k = 2;

	// the binomial coefficient gives us the number of all combinations of size k from n elements
	auto ring_combos = std::vector<std::vector<Item>>(binomial_coefficient(n, k));

	enum class Bool { False, True };

	auto selector = std::vector<Bool>(n);

	std::fill(selector.begin(), selector.begin() + k, Bool::True);

	do {

		for(const auto i : boost::counting_range({}, n)) {

			if(selector[i] == Bool::True) {

				// the vectors containing the ring combinations must not exceed k elements
				const auto fillable = std::find_if(ring_combos.begin(), ring_combos.end(), [k] (const auto combo) {
					return combo.size() < k;
				});

				// ring_combos is sized to contain all the combinations of length k,
				// so we are guaranteed that fillable never points to end()
				fillable->push_back(rings[i]);
			}
		}

	} while(std::prev_permutation(selector.begin(), selector.end()));

	return ring_combos;
}

int main() {

	const auto weapons = std::vector<Item>{{8,4,0}, {10,5,0}, {25,6,0}, {40,7,0}, {74,8,0}};
	const auto armors = std::vector<Item>{{}, {13,0,1}, {31,0,2}, {53,0,3}, {75,0,4}, {102,0,5}};
	const auto rings = get_rings();

	auto min_gold = std::numeric_limits<decltype(Item::price)>::max();

	const auto boss = Entity{103, 9, 2};
	Entity hero; // no need to initialize, the innermost loop provides the values

	// cartesian product of all the equipment vectors
	for(const auto& weapon : weapons) {

		for(const auto& armor : armors) {

			for(const auto& ring : rings) {

				const auto& ring1 = ring.front();
				const auto& ring2 = ring.back();

				hero = {
					100,
					(weapon.dmg_bonus + ring1.dmg_bonus + ring2.dmg_bonus),
					(armor.ac_bonus + ring1.ac_bonus + ring2.ac_bonus)
				};

				const auto hero_dmg = (hero.damage - boss.armor);
				const auto boss_dmg = (boss.damage - hero.armor);

				const auto hero_real_dmg = (hero_dmg > 0) ? hero_dmg : 1;
				const auto boss_real_dmg = (boss_dmg > 0) ? boss_dmg : 1;

				const auto hero_health = hero.health;
				const auto boss_health = boss.health;

				// fight sequence is always the same, so we can just calculate how many rounds they both last
				const auto hero_rounds = ((hero_health / boss_real_dmg) + ((hero_health % boss_real_dmg) > 0));
				const auto boss_rounds = ((boss_health / hero_real_dmg) + ((boss_health % hero_real_dmg) > 0));

				// since the hero always attacks first in any given round,
				// equal rounds means the hero always wins
				if(hero_rounds >= boss_rounds) {
					min_gold = std::min(min_gold, (weapon.price + armor.price + ring1.price + ring2.price));
				}
			}
		}
	}

	std::cout << min_gold << std::endl;

	return 0;
}


/**
 *
 * Core Libraries.
 * Sophus Interpolations & other missing bits.
 *
 * Copyright (c) Robert Lukierski 2016. All rights reserved.
 * Author: Robert Lukierski.
 *
 */

#ifndef VISIONCORE_SOPHUS_MISSINGBITS_HPP
#define VISIONCORE_SOPHUS_MISSINGBITS_HPP

#include <VisionCore/Platform.hpp>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <sophus/so2.hpp>
#include <sophus/so3.hpp>
#include <sophus/se2.hpp>
#include <sophus/se3.hpp>
#include <sophus/rxso3.hpp>
#include <sophus/sim3.hpp>

/**
 * NOTE: Don't use <sophus/interpolate.hpp> for SE groups. It's wrong, see page 41:
 * https://www.cvl.isy.liu.se/education/graduate/geometry-for-computer-vision-2014/geometry2014/lecture7.pdf
 */

namespace Sophus
{

// SO2
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SO2<T> interpolateLinear(const Sophus::SO2<T>& t0, const Sophus::SO2<T>& t1, T ratio)
{
    return Sophus::SO2<T>(t0 * Sophus::SO2<T>::exp(ratio * ( t0.inverse() * t1 ).log() ));
}

// SE2
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SE2<T> interpolateLinear(const Sophus::SE2<T>& t0, const Sophus::SE2<T>& t1, T ratio)
{
    return Sophus::SE2<T>(t0.so2() * Sophus::SO2<T>::exp(ratio * ( t0.so2().inverse() * t1.so2() ).log() ),
                          t0.translation() + ratio * (t1.translation() - t0.translation()));
}

// SO3
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SO3<T> interpolateLinear(const Sophus::SO3<T>& t0, const Sophus::SO3<T>& t1, T ratio)
{
    return Sophus::SO3<T>(t0 * Sophus::SO3<T>::exp(ratio * ( t0.inverse() * t1 ).log() ));
}

// SE3
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SE3<T> interpolateLinear(const Sophus::SE3<T>& t0, const Sophus::SE3<T>& t1, T ratio)
{
    return Sophus::SE3<T>(t0.so3() * Sophus::SO3<T>::exp(ratio * ( t0.so3().inverse() * t1.so3() ).log() ),
                          t0.translation() + ratio * (t1.translation() - t0.translation()));
}

template<typename T>
struct B4SplineGenerator
{

    EIGEN_DEVICE_FUNC static inline void get(const T& u, T& b1, T& b2, T& b3)
    {
        const T u2 = u*u;
        const T u3 = u*u*u;

        b1 = (u3 - T(3.0) * u2 + T(3.0) * u + T(5.0)) / T(6.0);
        b2 = (T(-2.0) * u3 + T(3.0) * u2 + T(3.0) * u + T(1.0)) / T(6.0);
        b3 = u3 / T(6.0);
    }

#if 0
    EIGEN_DEVICE_FUNC static inline void get(const T& u, T& b1, T& b2, T& b3)
    {
        Eigen::Matrix<T,4,1> v(T(1.0),u,u*u,u*u*u);
        Eigen::Matrix<T,4,4> m;
        m << T(6.0) , T(0.0) , T(0.0) , T(0.0),
             T(5.0) , T(3.0) , T(-3.0), T(1.0),
             T(1.0) , T(3.0) , T(3.0), T(-2.0),
             T(0.0) , T(0.0) , T(0.0) , T(1.0);

        const Eigen::Matrix<T,4,1> uvec = (m * v) / T(6.0);
        b1 = uvec(1);
        b2 = uvec(2);
        b3 = uvec(3);
    }
#endif
};

// B4-Spline SO2
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SO2<T> interpolateB4Spline(const Sophus::SO2<T>& tm1,
                                                                   const Sophus::SO2<T>& t0,
                                                                   const Sophus::SO2<T>& t1,
                                                                   const Sophus::SO2<T>& t2,
                                                                   const T& u)
{
    T b1, b2, b3;

    B4SplineGenerator<T>::get(u,b1,b2,b3);

    return tm1 * Sophus::SO2<T>::exp(b1 * ( tm1.inverse() * t0 ).log() +
                                          b2 * ( t0.inverse()  * t1 ).log() +
                                          b3 * ( t1.inverse()  * t2 ).log() );
}

// B4-Spline SE2
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SE2<T> interpolateB4Spline(const Sophus::SE2<T>& tm1,
                                                                   const Sophus::SE2<T>& t0,
                                                                   const Sophus::SE2<T>& t1,
                                                                   const Sophus::SE2<T>& t2,
                                                                   const T& u)
{
    T b1, b2, b3;

    B4SplineGenerator<T>::get(u,b1,b2,b3);

    const Sophus::SO2<T> rot_final = tm1.so2() * Sophus::SO2<T>::exp(b1 * ( tm1.so2().inverse() * t0.so2() ).log() +
                                                                               b2 * ( t0.so2().inverse()  * t1.so2() ).log() +
                                                                               b3 * ( t1.so2().inverse()  * t2.so2() ).log() );

    const typename Sophus::SE2<T>::Point tr_final = tm1.translation() + (b1 * (t0.translation() - tm1.translation())) +
                                                                             (b2 * (t1.translation() - t0.translation())) +
                                                                             (b3 * (t2.translation() - t1.translation()));

    return Sophus::SE2<T>(rot_final, tr_final);
}

// B4-Spline SO3
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SO3<T> interpolateB4Spline(const Sophus::SO3<T>& tm1,
                                                                   const Sophus::SO3<T>& t0,
                                                                   const Sophus::SO3<T>& t1,
                                                                   const Sophus::SO3<T>& t2,
                                                                   const T& u)
{
    T b1, b2, b3;

    B4SplineGenerator<T>::get(u,b1,b2,b3);

    return tm1 * Sophus::SO3<T>::exp(b1 * ( tm1.inverse() * t0 ).log() +
                                          b2 * ( t0.inverse()  * t1 ).log() +
                                          b3 * ( t1.inverse()  * t2 ).log() );
}

// B4-Spline SE3
template<typename T>
EIGEN_DEVICE_FUNC static inline Sophus::SE3<T> interpolateB4Spline(const Sophus::SE3<T>& tm1,
                                                                   const Sophus::SE3<T>& t0,
                                                                   const Sophus::SE3<T>& t1,
                                                                   const Sophus::SE3<T>& t2,
                                                                   const T& u)
{
    T b1, b2, b3;

    B4SplineGenerator<T>::get(u,b1,b2,b3);

    const Sophus::SO3<T> rot_final = tm1.so3() * Sophus::SO3<T>::exp(b1 * ( tm1.so3().inverse() * t0.so3() ).log() +
                                                                               b2 * ( t0.so3().inverse()  * t1.so3() ).log() +
                                                                               b3 * ( t1.so3().inverse()  * t2.so3() ).log() );

    const typename Sophus::SE3<T>::Point tr_final = tm1.translation() + (b1 * (t0.translation() - tm1.translation())) +
                                                                             (b2 * (t1.translation() - t0.translation())) +
                                                                             (b3 * (t2.translation() - t1.translation()));

    return Sophus::SE3<T>(rot_final, tr_final);
}

// let's put ostreams here
template<typename Derived>
inline std::ostream& operator<<(std::ostream& os, const SO2Base<Derived>& p)
{
    os << "(" << p.log() << ")";
    return os;
}

template<typename Derived>
inline std::ostream& operator<<(std::ostream& os, const SO3Base<Derived>& p)
{
    os << "(" << p.unit_quaternion().x() << "," << p.unit_quaternion().y() << ","
       << p.unit_quaternion().z() << "|" << p.unit_quaternion().w() << ")";
    return os;
}

template<typename Derived>
inline std::ostream& operator<<(std::ostream& os, const SE2Base<Derived>& p)
{
    os << "[t = " << p.translation()(0) << "," << p.translation()(1) << " | r = " << p.so2() << ")";
    return os;
}

template<typename Derived>
inline std::ostream& operator<<(std::ostream& os, const SE3Base<Derived>& p)
{
    os << "[t = " << p.translation()(0) << "," << p.translation()(1) << "," << p.translation()(2) << " | r = " << p.so3() << ")";
    return os;
}

#ifdef VISIONCORE_ENABLE_CEREAL

/**
 * SO2
 */
template<typename Archive, typename Derived>
void load(Archive & archive, SO2Base<Derived>& m, std::uint32_t const version)
{
    typename SO2Base<Derived>::Point cplx;
    archive(cplx);
    m.setComplex(cplx);
}

template<typename Archive, typename Derived>
void save(Archive & archive, SO2Base<Derived> const & m, std::uint32_t const version)
{
    archive(m.unit_complex());
}

/**
 * SO3
 */
template<typename Archive, typename Derived>
void load(Archive & archive, SO3Base<Derived>& m, std::uint32_t const version)
{
    Eigen::Quaternion<typename SO3Base<Derived>::Scalar> quaternion;
    archive(cereal::make_nvp("Quaternion", quaternion));
    m.setQuaternion(quaternion);
}

template<typename Archive, typename Derived>
void save(Archive & archive, SO3Base<Derived> const & m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Quaternion", m.unit_quaternion()));
}

/**
 * SE2
 */
template<typename Archive, typename Derived>
void load(Archive & archive, SE2Base<Derived>& m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.so2()));
}

template<typename Archive, typename Derived>
void save(Archive & archive, SE2Base<Derived> const & m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.so2()));
}

/**
 * SE3
 */
template<typename Archive, typename Derived>
void load(Archive & archive, SE3Base<Derived>& m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.so3()));
}

template<typename Archive, typename Derived>
void save(Archive & archive, SE3Base<Derived> const & m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.so3()));
}

/**
 * RxSO3
 */
template<typename Archive, typename Derived>
void load(Archive & archive, RxSO3Base<Derived>& m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Quaternion", m.quaternion()));
}

template<typename Archive, typename Derived>
void save(Archive & archive, RxSO3Base<Derived> const & m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Quaternion", m.quaternion()));
}

/**
 * Sim3
 */
template<typename Archive, typename Derived>
void load(Archive & archive, Sim3Base<Derived>& m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.rxso3()));
}

template<typename Archive, typename Derived>
void save(Archive & archive, Sim3Base<Derived> const & m, std::uint32_t const version)
{
    archive(cereal::make_nvp("Translation", m.translation()));
    archive(cereal::make_nvp("Rotation", m.rxso3()));
}

#endif // VISIONCORE_ENABLE_CEREAL

/**
 * Stuff that Hauke removed.
 *
 * \param alpha1 rotation around x-axis
 * \param alpha2 rotation around y-axis
 * \param alpha3 rotation around z-axis
 *
 * Since rotations in 3D do not commute, the order of the individual rotations
 * matter. Here, the following convention is used. We calculate a SO3 member
 * corresponding to the rotation matrix \f$ R \f$ such
 * that \f$ R=\exp\left(\begin{array}{c}\alpha_1\\ 0\\ 0\end{array}\right)
 *    \cdot   \exp\left(\begin{array}{c}0\\ \alpha_2\\ 0\end{array}\right)
 *    \cdot   \exp\left(\begin{array}{c}0\\ 0\\ \alpha_3\end{array}\right)\f$.
 */
template<typename T>
EIGEN_DEVICE_FUNC static inline SO3<T> fromEulerAngles(const T alpha1, const T alpha2, const T alpha3)
{
    typedef typename SO3<T>::Tangent Tangent;
    const static T zero = static_cast<T>(0);

    return SO3<T>((SO3<T>::exp(Tangent(alpha1, zero, zero)) *
                   SO3<T>::exp(Tangent(zero, alpha2, zero)) *
                   SO3<T>::exp(Tangent(zero, zero, alpha3)))
                 );
}

}

#endif // VISIONCORE_SOPHUS_MISSINGBITS_HPP


/*
 * Copyright 2018 James Dyer
 *
 * 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.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include <sstream>
#include <NTL/RR.h>
#include <jsoncpp/json/json.h>
#include "GACDEncrypter.h"

GACDEncrypter::GACDEncrypter(int lambda) {
	k = NTL::RandomPrime_ZZ(lambda, 20);
	NTL::RR kreal = to_RR(k);
	minr = RoundToZZ(sqrt(sqrt(power(kreal,3)))); // minr = k^{3/4}
	maxr = k-minr;
}

NTL::ZZ GACDEncrypter::encrypt(NTL::ZZ& plaintext){
	NTL::ZZ r = rng.nextBigInteger(minr, maxr);
	return plaintext*k+r;
}

std::string GACDEncrypter::writeSecretsToJSON(){
	Json::Value root;
	std::stringstream keyStr;
	keyStr << k;
	root["key"] = keyStr.str();
	Json::FastWriter writer;
	return writer.write(root);
}

int GACDEncrypter::getMinimumKeyLength(int bits){
	return ceil(8.0*bits/3);
}


// This file is part of PoseEstimation.
// Copyright (c) 2021, Eijiro Shibusawa <phd_kimberlite@yahoo.co.jp>
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#ifndef SEVEN_POINT_HPP_
#define SEVEN_POINT_HPP_

#include <Eigen/Dense>

#include <vector>

class SevenPointTest;

namespace SevenPoint
{

template <typename FloatType>
class SevenPoint
{
	friend ::SevenPointTest;
private:
	typedef Eigen::Matrix<std::complex<FloatType>, 3, 1> CMatrix_3x1;
	typedef Eigen::Matrix<FloatType, 3, 3> Matrix_3x3;
	typedef Eigen::Matrix<FloatType, 7, 9> Matrix_7x9;
	typedef Eigen::Matrix<FloatType, 9, 9> Matrix_9x9;
	typedef Eigen::Matrix<FloatType, 9, Eigen::Dynamic> Matrix_9xD;

public:
	SevenPoint() :
		m_F1(m_F)
		, m_F2(m_F1 + 9)
	{
	}

	virtual ~SevenPoint()
	{
	}

	bool getMatrix(const FloatType *pts1, const FloatType *pts2, int &nSolutions, std::vector<FloatType> &Fs)
	{
		getConstraints(pts1, pts2);
		bool ret = getRoot();
		if (!ret)
		{
			return false;
		}

		nSolutions = m_mFvec.cols();
		Fs.resize(9 * nSolutions);
		for (int i = 0; i < nSolutions; i++)
		{
			for (int j = 0; j < 9; j++)
			{
				Fs[9 * i + j] = m_mFvec(j, i);
			}
		}

		return true;
	}

private:
	// 1st order polynomial multiplication
	// (ax + b) * (cx + d) = (acx^2 + (ad + bc)x + bd)
	// p1 = [x 1]
	// p2 = [x 1]
	// p3 = [x^2 x 1]
	static inline void polynomial_multiplication1(const FloatType *p1, const FloatType *p2, FloatType *p3)
	{
		p3[0] = (p1[0])*(p2[0]);					// x^2
		p3[1] = (p1[0])*(p2[1]) + (p1[1])*(p2[0]);	// x
		p3[2] = (p1[1])*(p2[1]);					// 1
	}

	// 2nd order polynomial multiplication
	// (ax + b) * (cx^2 + dx + e) = (acx^3 + (ad + bc)x^2 + ... + be)
	// p1 = [x 1]
	// p3 = [x^2 x 1]
	// p3 = [x^3 x^2 x 1]
	static inline void polynomial_multiplication2(const FloatType *p1, const FloatType *p2, FloatType *p3)
	{
		p3[0] = (p1[0])*(p2[0]);					// x^3
		p3[1] = (p1[0])*(p2[1]) + (p1[1])*(p2[0]);	// x^2
		p3[2] = (p1[0])*(p2[2]) + (p1[1])*(p2[1]);	// x
		p3[3] = (p1[1])*(p2[2]);					// 1
	}

	static inline void getLinearConstraints(const FloatType *pts1, const FloatType *pts2, FloatType *F1, FloatType *F2)
	{
		Matrix_7x9 mC;
		for (unsigned int i = 0; i < 7; i++){
			const FloatType *p1 = &(pts1[2*i]), *p2 = &(pts2[2*i]);
			mC(i, 0) = (p2[0])*(p1[0]);
			mC(i, 1) = (p2[0])*(p1[1]);
			mC(i, 2) = (p2[0]);
			mC(i, 3) = (p2[1])*(p1[0]);
			mC(i, 4) = (p2[1])*(p1[1]);
			mC(i, 5) = (p2[1]);
			mC(i, 6) = (p1[0]);
			mC(i, 7) = (p1[1]);
			mC(i, 8) = 1;
		}

		Eigen::JacobiSVD<Matrix_7x9> svd(mC, Eigen::ComputeFullV);
		Matrix_9x9 mV = svd.matrixV();
		for (int i = 0; i < 9; i++)
		{
			F1[i] = mV(i, 7);
			F2[i] = mV(i, 8);
		}
	}
	static inline void getDeterminantConstraints(const FloatType *F1, const FloatType *F2, FloatType *C)
	{
		C[0] = C[1] = C[2] = C[3] = 0;
		FloatType p00[] = {F1[0], F2[0]}; // (0, 0)
		FloatType p01[] = {F1[1], F2[1]}; // (0, 1)
		FloatType p02[] = {F1[2], F2[2]}; // (0, 2)
		FloatType p10[] = {F1[3], F2[3]}; // (1, 0)
		FloatType p11[] = {F1[4], F2[4]}; // (1, 1)
		FloatType p12[] = {F1[5], F2[5]}; // (1, 2)
		FloatType p20[] = {F1[6], F2[6]}; // (2, 0)
		FloatType p21[] = {F1[7], F2[7]}; // (2, 1)
		FloatType p22[] = {F1[8], F2[8]}; // (2, 2)

		FloatType c1[3], c2[3], c3[4];
		polynomial_multiplication1(p11, p22, c1);
		polynomial_multiplication1(p12, p21, c2);
		c1[0] -= c2[0]; c1[1] -= c2[1]; c1[2] -= c2[2];
		polynomial_multiplication2(p00, c1, c3);
		C[0] += c3[0]; C[1] += c3[1]; C[2] += c3[2]; C[3] += c3[3];

		polynomial_multiplication1(p01, p22, c1);
		polynomial_multiplication1(p02, p21, c2);
		c1[0] -= c2[0]; c1[1] -= c2[1]; c1[2] -= c2[2];
		polynomial_multiplication2(p10, c1, c3);
		C[0] -= c3[0]; C[1] -= c3[1]; C[2] -= c3[2]; C[3] -= c3[3];

		polynomial_multiplication1(p01, p12, c1);
		polynomial_multiplication1(p02, p11, c2);
		c1[0] -= c2[0]; c1[1] -= c2[1]; c1[2] -= c2[2];
		polynomial_multiplication2(p20, c1, c3);
		C[0] += c3[0]; C[1] += c3[1]; C[2] += c3[2]; C[3] += c3[3];
	}

protected:
	static const size_t m_minSet; // minimum data number for parameter esimation
	static const bool m_acceptArbitraryNSet; // if true, this model accepts arbitrary data number for parameter esimation

private:
	void getConstraints(const FloatType *pts1, const FloatType *pts2)
	{
		getLinearConstraints(pts1, pts2, m_F1, m_F2);
		getDeterminantConstraints(m_F1, m_F2, m_C);
	}

	bool getRoot()
	{
		// construct the companion matrix
		Matrix_3x3 mC;
		mC.setZero();
		mC(1, 0) = mC(2, 1) = 1;
		mC(0, 2) = -m_C[3] / m_C[0];
		mC(1, 2) = -m_C[2] / m_C[0];
		mC(2, 2) = -m_C[1] / m_C[0];
		Eigen::EigenSolver<Matrix_3x3> eig(mC, false);
		CMatrix_3x1 mSols = eig.eigenvalues();

		std::vector<int> idx(0);
		for(int j = 0; j < 3; j++)
		{
			if(mSols[j].imag() == 0)
			{
				idx.push_back(j);
			}
		}

		if (idx.empty())
		{
			return false; // 3rd oder polynomial has at least one real root!
		}

		m_mFvec = Matrix_9xD(9, idx.size());
		for (size_t i = 0; i < idx.size(); i++)
		{
			for (int j = 0; j < 9; j++)
			{
				m_mFvec(j, i) = (mSols[i].real()) * (m_F1[j]) + (m_F2[j]);
			}
		}
		return true;
	}

	FloatType m_F[9*2]; // orthogonal basis of null space of linear constarints
	FloatType m_C[4]; 	// deteminant constraints
	FloatType *m_F1, *m_F2; // pointer to F
	Matrix_9xD m_mFvec; // Essential matrix solutions
};
template <typename FloatType>
const size_t SevenPoint<FloatType>::m_minSet = 7;
template <typename FloatType>
const bool SevenPoint<FloatType>::m_acceptArbitraryNSet = false;

}

#endif // SEVEN_POINT_HPP_


//
// harmonic.cpp
//
// Copyright (c) 1996-1998 Takashi Kanai
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

#include "StdAfx.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

#include <math.h>
#include "smd.h"
#include "harmonic.h"
#include "ppdedge.h"
#include "hgppd.h"
// #include "linbcg.h"

#define KAPPA 1.0

#include <vector>
#include <Eigen/Sparse>
#include <Eigen/OrderingMethods>

/*******************************************************************************
  STEP 2: harmonic maps (Related functions are in "harmonic.c".)
*******************************************************************************/

void harmonic_initialize_vector_2( HGfc *hgfc, Splp *lp,
                                   Eigen::VectorXd& bx, Eigen::VectorXd& by )
{
  int    i;
  HGvt   *vt;
  Vec2d  *uvbprm;
  Sple   *le, *tle, *sle;
  Splv   *lv, *tlv, *slv, *elv;
  double sum, esum, val;
  Vec2d  sub;

  /* initialize vector */
  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt )
    {
      bx[vt->sid] = 0.0;
      by[vt->sid] = 0.0;
    //   emat->bx[vt->sid] = emat->xx[vt->sid] = 0.0;
    // emat->by[vt->sid] = emat->yy[vt->sid] = 0.0;
    }

  uvbprm = calc_uvbprm( hgfc->hgcn );

  /* calculate boundary values */

  lv = lp->splv;
  le = lp->sple;

  i = 0;
  while ( i < hgfc->hgcn ) {

    slv = lv;
    sle = le;
    sum = 0.0;

    while ( 1 ) {
      sum += le->ed->length;
      lv = lv->nxt;
      le = le->nxt;
      if ( lv == (Splv *) NULL ) {
        elv = lp->splv;
        break;
      }
      if ( lv->vt->sp_type == SP_VERTEX_HVERTEX ) {
        elv = lv;
        break;
      }
    }

    esum = 0.0;
    tlv = slv;
    tle = sle;
    sub.x = uvbprm[i+1].x - uvbprm[i].x;
    sub.y = uvbprm[i+1].y - uvbprm[i].y;
    while ( 1 ) {

      val = esum / sum;
      bx[ tlv->hgvt->sid ] = uvbprm[i].x + sub.x * val;
      by[ tlv->hgvt->sid ] = uvbprm[i].y + sub.y * val;
      // emat->bx[ tlv->hgvt->sid ] = uvbprm[i].x + sub.x * val;
      // emat->by[ tlv->hgvt->sid ] = uvbprm[i].y + sub.y * val;

      esum += tle->ed->length;

      tlv = tlv->nxt;
      tle = tle->nxt;

      if ( (tlv == elv) || (tlv == (Splv *) NULL) ) break;
    }

    ++i;
  }

  free(uvbprm);
}

// harmonic mapping functions with Eigen

void hgfcharmonic_Eigen( HGfc *hgfc, Splp *lp )
{
  int vn = initialize_hgfc( hgfc );

  // alternates harmonic_calc_kappa function
  double* kappa = (double*) malloc( hgfc->hgen * sizeof(double) );
  for ( HGed* ed = hgfc->shged; ed != (HGed*) NULL; ed = ed->nxt )
    {
      kappa[ed->sid] = 1.0;
    }

  // vt->vval initialization
  for ( HGvt* vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt )
    {
      vt->vval = 0.0;
    }

  // setup val and vval
  for ( HGvt* vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {
    for ( HGvted* ve = vt->shgve; ve != (HGvted *) NULL; ve = ve->nxt )
      {
        HGed* ed = ve->ed;
        // sv
        int id = ed->sv->sid;
        if (vt->sid != id)
          {
            HGvtvt* vv = find_sort_hgvtvt( vt, id );
            vv->val -= kappa[ed->sid];
          }
        else
          {
            vt->vval += kappa[ed->sid];
          }
        // ev
        id = ed->ev->sid;
        if (vt->sid != id)
          {
            HGvtvt* vv = find_sort_hgvtvt( vt, id );
            vv->val -= kappa[ed->sid];
          }
        else
          {
            vt->vval += kappa[ed->sid];
          }
      }
  }

  free(kappa);

  // boundary points are fixed
  for ( HGvt* vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt )
    {
      if ( vt->vt->sp_type != SP_VERTEX_NORMAL )
        {
          vt->vval = 1.0;
          free_hgppdvertexvertex( vt );
        }
    }

  // setup matrix A
  // build up sparse matrix A

  int n_vt = hgfc->hgvn;
  Eigen::SparseMatrix<double> spmat( n_vt, n_vt );

  for ( HGvt* vt = hgfc->shgvt; vt != (HGvt*) NULL; vt = vt->nxt )
    {
      int i = vt->sid;
      spmat.insert( i, i ) = vt->vval;
      if ( vt->vt->sp_type == SP_VERTEX_NORMAL )
        {
          for ( HGvtvt* vv = vt->shgvv; vv != (HGvtvt*) NULL; vv = vv->nxt )
            {
              int j = vv->id;
              spmat.insert( i, j ) = vv->val;
            }
        }
    }
  spmat.makeCompressed();

  // setup vector b
  Eigen::VectorXd bx(n_vt), by(n_vt);
  harmonic_initialize_vector_2( hgfc, lp, bx, by );

  // solve x and y
  Eigen::VectorXd xx(n_vt), xy(n_vt);
  Eigen::BiCGSTAB<Eigen::SparseMatrix<double> > solver(spmat);
  xx = solver.solve(bx);
  xy = solver.solve(by);

  // store x to uvw
  for ( HGvt* vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt )
    {
      vt->uvw.x = xx[vt->sid];
      vt->uvw.y = xy[vt->sid];
    }

  exit_hgfc( hgfc );
}

void hppdharmonic( HPpd *hppd )
{
  HFace *hf;

  /* calc edge length */
  edge_length( hppd->ppd1 );
  edge_length( hppd->ppd2 );
  for ( hf = hppd->shfc; hf != (HFace *) NULL; hf = hf->nxt )
    {
      hgfcharmonic_Eigen( hf->hgfc1, hf->hloop->lp1 );
      hgfcharmonic_Eigen( hf->hgfc2, hf->hloop->lp2 );
      // hgfcharmonic( hf->hgfc1, hf->hloop->lp1 );
      // hgfcharmonic( hf->hgfc2, hf->hloop->lp2 );
    }
}

#if 0
/* harmonic mapping functions */
void hgfcharmonic( HGfc *hgfc, Splp *lp )
{
  int    iter;
  int    vn;
  double rsq;
  Semat  *emat;

  vn = initialize_hgfc( hgfc );
  emat = create_harmonic_emat( hgfc );
  harmonic_initialize_vector( hgfc, lp, emat );

  linbcg( emat, emat->bx, emat->xx, 1, SMDZEROEPS, 1000, &iter, &rsq );
  linbcg( emat, emat->by, emat->yy, 1, SMDZEROEPS, 1000, &iter, &rsq );
  solvec_hgfc( hgfc, emat );

  free_emat( emat );
  exit_hgfc( hgfc );
}
#endif

int initialize_hgfc( HGfc *hgfc )
{
  int  i;
  int  cnt = 0;
  HGvt *hv;
  HGed *he;

  /* id */

  for (i = 0, hv = hgfc->shgvt; hv != (HGvt *) NULL; hv = hv->nxt, ++i) {
    hv->sid  = i;
    if ( hv->vt->sp_type == SP_VERTEX_NORMAL ) {
      ++cnt;
    }
  }
  /* , and create hgvt->hged links */
  for (i = 0, he = hgfc->shged; he != (HGed *) NULL; he = he->nxt, ++i) {
    he->sid  = i;
    create_hgvted( he->sv, he );
    create_hgvted( he->ev, he );
  }
  return cnt;
}

#if 0
Semat *create_harmonic_emat( HGfc *hgfc )
{
  int    i, vn;
  int    id;
  double *kappa;
  Semat  *emat;
  HGvt   *vt;
  HGed   *ed;
  HGvted *ve;
  HGvtvt *vv;

  emat = init_emat( hgfc->hgvn );

  kappa = harmonic_calc_kappa( hgfc );

  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {
    vt->vval = 0.0;
  }

  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {
    for ( ve = vt->shgve; ve != (HGvted *) NULL; ve = ve->nxt ) {

      ed = ve->ed;
      /* sv */
      id = ed->sv->sid;
      if (vt->sid != id) {
        vv = find_sort_hgvtvt( vt, id );
        vv->val -= kappa[ed->sid];
      } else {
        vt->vval += kappa[ed->sid];
      }
      /* ev */
      id = ed->ev->sid;
      if (vt->sid != id) {
        vv = find_sort_hgvtvt( vt, id );
        vv->val -= kappa[ed->sid];
      } else {
        vt->vval += kappa[ed->sid];
      }
    }
  }

  /* Boundary Points are fixed. */
  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {

    if ( vt->vt->sp_type != SP_VERTEX_NORMAL ) {
      vt->vval = 1.0;
      free_hgppdvertexvertex( vt );
    }

  }

  vn = 0;
  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {
    vn += vt->hgvvn;
  }

  emat->num = vn + hgfc->hgvn + 1;
  emat->sa  = (double *) malloc(emat->num * sizeof(double));
  for (i = 0; i < emat->num; ++i) emat->sa[i] = 0.0;
  emat->ija = (int *) malloc(emat->num * sizeof(int));

  dsprsin( hgfc, emat );

  free(kappa);

  FreeHGppdVertexVertex( hgfc );

  return emat;
}

double *harmonic_calc_kappa( HGfc *hgfc )
{
  double *kappa;
  HGed   *ed;

  /* initialize length, area, kappa */
  kappa   = (double *) malloc( hgfc->hgen * sizeof(double));

  for ( ed = hgfc->shged; ed != (HGed *) NULL; ed = ed->nxt ) {
    kappa[ed->sid] = 1.0;
  }
  return kappa;
}

void harmonic_initialize_vector( HGfc *hgfc, Splp *lp, Semat *emat )
{
  int    i;
  HGvt   *vt;
  Vec2d  *uvbprm;
  Sple   *le, *tle, *sle;
  Splv   *lv, *tlv, *slv, *elv;
  double sum, esum, val;
  Vec2d  sub;

  /* initialize vector */
  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {

    emat->bx[vt->sid] = emat->xx[vt->sid] = 0.0;
    emat->by[vt->sid] = emat->yy[vt->sid] = 0.0;

  }

  uvbprm = calc_uvbprm( hgfc->hgcn );

  /* calculate boundary values */

  lv = lp->splv;
  le = lp->sple;

  i = 0;
  while ( i < hgfc->hgcn ) {

    slv = lv;
    sle = le;
    sum = 0.0;

    while ( 1 ) {
      sum += le->ed->length;
      lv = lv->nxt;
      le = le->nxt;
      if ( lv == (Splv *) NULL ) {
        elv = lp->splv;
        break;
      }
      if ( lv->vt->sp_type == SP_VERTEX_HVERTEX ) {
        elv = lv;
        break;
      }
    }

    esum = 0.0;
    tlv = slv;
    tle = sle;
    sub.x = uvbprm[i+1].x - uvbprm[i].x;
    sub.y = uvbprm[i+1].y - uvbprm[i].y;
    while ( 1 ) {

      val = esum / sum;
      emat->bx[ tlv->hgvt->sid ] = uvbprm[i].x + sub.x * val;
      emat->by[ tlv->hgvt->sid ] = uvbprm[i].y + sub.y * val;

      esum += tle->ed->length;

      tlv = tlv->nxt;
      tle = tle->nxt;

      if ( (tlv == elv) || (tlv == (Splv *) NULL) ) break;
    }

    ++i;
  }

  free(uvbprm);
}
#endif

Vec2d *calc_uvbprm( int cn )
{
  int i;
  double angle;
  Vec2d *uvbprm;

  uvbprm = (Vec2d *) malloc( (cn+1) * sizeof(Vec2d) );

  for ( i = 0; i <= cn; ++i ) {
    angle = 2 * (double) i * SMDPI / (double) cn;
    uvbprm[i].x = cos( angle );
    uvbprm[i].y = sin( angle );
/*     display("uvbprm %g %g\n", uvbprm[i].x, uvbprm[i].y); */
  }
  return uvbprm;
}

#if 0
void solvec_hgfc( HGfc *hgfc, Semat *emat )
{
  HGvt *vt;

  for ( vt = hgfc->shgvt; vt != (HGvt *) NULL; vt = vt->nxt ) {
    vt->uvw.x = emat->xx[vt->sid];
    vt->uvw.y = emat->yy[vt->sid];
  }
}
#endif

void exit_hgfc( HGfc *hgfc )
{
  HGvt *hv;
  HGed *he;

  for ( hv = hgfc->shgvt; hv != (HGvt *) NULL; hv = hv->nxt ) {
    hv->sid  = SMDNULL;
    hv->vval = 0.0;
    free_hgppdvertexedge( hv );
  }

  /* create hgfc surface */

  for ( he = hgfc->shged; he != (HGed *) NULL; he = he->nxt ) {
    he->sid  = SMDNULL;
  }

}

#if 0
/* Semat functions */
Semat *init_emat( int num )
{
  Semat *emat;

  emat = (Semat *) malloc(sizeof(Semat));
  emat->rnum = num;
  emat->cnum = num;
  /* constant vectors initialize */
  emat->bx = (double *) malloc(num * sizeof(double));
  emat->by = (double *) malloc(num * sizeof(double));
  /* solution vectors initialize */
  emat->xx = (double *) malloc(num * sizeof(double));
  emat->yy = (double *) malloc(num * sizeof(double));

  return emat;
}

void free_emat(Semat *emat)
{
  free(emat->sa);
  free(emat->ija);
  free(emat->bx);
  free(emat->by);
  free(emat->xx);
  free(emat->yy);
  free(emat);
}
#endif

/*
void printemat(Semat *emat)
{
  int i;

  for (i = 0; i < emat->cnum; ++i) {
    display("%d (b) %f %f\n", i, emat->bx[i], emat->by[i]);
    display("%d (x) %f %f\n", i, emat->xx[i], emat->yy[i]);
  }
  for (i = 0; i < emat->num; ++i)
    display("(%d) (sa) %f (ija) %d\n", i, emat->sa[i], emat->ija[i]);
}
*/