// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library 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
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

//Based on the svd of the KDL-0.2 library by Erwin Aertbelien
#ifndef SVD_EIGEN_HH_HPP
#define SVD_EIGEN_HH_HPP

#include <Eigen/Array>
#include <algorithm>

namespace KDL
{
    template<typename Scalar> inline Scalar PYTHAG(Scalar a,Scalar b) {
        double at,bt,ct;
        at = fabs(a);
        bt = fabs(b);
        if (at > bt ) {
            ct=bt/at;
            return Scalar(at*sqrt(1.0+ct*ct));
        } else {
            if (bt==0)
                return Scalar(0.0);
            else {
                ct=at/bt;
                return Scalar(bt*sqrt(1.0+ct*ct));
            }
        }
    }

    template<typename Scalar> inline Scalar SIGN(Scalar a,Scalar b) {
        return ((b) >= Scalar(0.0) ? fabs(a) : -fabs(a));
    }

    /**
     * svd calculation of boost ublas matrices
     *
     * @param A matrix<double>(mxn)
     * @param U matrix<double>(mxn)
     * @param S vector<double> n
     * @param V matrix<double>(nxn)
     * @param tmp vector<double> n
     * @param maxiter defaults to 150
     *
     * @return -2 if maxiter exceeded, 0 otherwise
     */
	template<typename MatrixA, typename MatrixUV, typename VectorS>
	int svd_eigen_HH(
		const Eigen::MatrixBase<MatrixA>&		A,
		Eigen::MatrixBase<MatrixUV>&			U,
		Eigen::MatrixBase<VectorS>&				S,
		Eigen::MatrixBase<MatrixUV>&			V,
		Eigen::MatrixBase<VectorS>&				tmp,
		int maxiter=150)
	{
        //get the rows/columns of the matrix
        const int rows = A.rows();
        const int cols = A.cols();

        U = A;

        int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
        int ppi(0);
        bool flag;
        e_scalar maxarg1,maxarg2,anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);

        g=scale=anorm=e_scalar(0.0);

        /* Householder reduction to bidiagonal form. */
        for (i=0;i<cols;i++) {
            ppi=i+1;
            tmp(i)=scale*g;
            g=s=scale=e_scalar(0.0);
            if (i<rows) {
                // compute the sum of the i-th column, starting from the i-th row
                for (k=i;k<rows;k++) scale += fabs(U(k,i));
                if (scale!=0) {
                    // multiply the i-th column by 1.0/scale, start from the i-th element
                    // sum of squares of column i, start from the i-th element
                    for (k=i;k<rows;k++) {
                        U(k,i) /= scale;
                        s += U(k,i)*U(k,i);
                    }
                    f=U(i,i);  // f is the diag elem
                    g = -SIGN(e_scalar(sqrt(s)),f);
                    h=f*g-s;
                    U(i,i)=f-g;
                    for (j=ppi;j<cols;j++) {
                        // dot product of columns i and j, starting from the i-th row
                        for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
                        f=s/h;
                        // copy the scaled i-th column into the j-th column
                        for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
                    }
                    for (k=i;k<rows;k++) U(k,i) *= scale;
                }
            }
            // save singular value
            S(i)=scale*g;
            g=s=scale=e_scalar(0.0);
            if ((i <rows) && (i+1 != cols)) {
                // sum of row i, start from columns i+1
                for (k=ppi;k<cols;k++) scale += fabs(U(i,k));
                if (scale!=0) {
                    for (k=ppi;k<cols;k++) {
                        U(i,k) /= scale;
                        s += U(i,k)*U(i,k);
                    }
                    f=U(i,ppi);
                    g = -SIGN(e_scalar(sqrt(s)),f);
                    h=f*g-s;
                    U(i,ppi)=f-g;
                    for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
                    for (j=ppi;j<rows;j++) {
                        for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
                        for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k);
                    }
                    for (k=ppi;k<cols;k++) U(i,k) *= scale;
                }
            }
            maxarg1=anorm;
            maxarg2=(fabs(S(i))+fabs(tmp(i)));
            anorm = maxarg1 > maxarg2 ?	maxarg1 : maxarg2;
        }
        /* Accumulation of right-hand transformations. */
        for (i=cols-1;i>=0;i--) {
            if (i<cols-1) {
                if (g) {
                    for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
                    for (j=ppi;j<cols;j++) {
                        for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
                        for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i);
                    }
                }
                for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0;
            }
            V(i,i)=1.0;
            g=tmp(i);
            ppi=i;
        }
        /* Accumulation of left-hand transformations. */
        for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
            ppi=i+1;
            g=S(i);
            for (j=ppi;j<cols;j++) U(i,j)=0.0;
            if (g) {
                g=e_scalar(1.0)/g;
                for (j=ppi;j<cols;j++) {
                    for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
                    f=(s/U(i,i))*g;
                    for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
                }
                for (j=i;j<rows;j++) U(j,i) *= g;
            } else {
                for (j=i;j<rows;j++) U(j,i)=0.0;
            }
            ++U(i,i);
        }

        /* Diagonalization of the bidiagonal form. */
        for (k=cols-1;k>=0;k--) { /* Loop over singular values. */
            for (its=1;its<=maxiter;its++) {  /* Loop over allowed iterations. */
                flag=true;
                for (ppi=k;ppi>=0;ppi--) {  /* Test for splitting. */
                    nm=ppi-1;             /* Note that tmp(1) is always zero. */
                    if ((fabs(tmp(ppi))+anorm) == anorm) {
                        flag=false;
                        break;
                    }
                    if ((fabs(S(nm)+anorm) == anorm)) break;
                }
                if (flag) {
                    c=e_scalar(0.0);           /* Cancellation of tmp(l), if l>1: */
                    s=e_scalar(1.);
                    for (i=ppi;i<=k;i++) {
                        f=s*tmp(i);
                        tmp(i)=c*tmp(i);
                        if ((fabs(f)+anorm) == anorm) break;
                        g=S(i);
                        h=PYTHAG(f,g);
                        S(i)=h;
                        h=e_scalar(1.0)/h;
                        c=g*h;
                        s=(-f*h);
                        for (j=0;j<rows;j++) {
                            y=U(j,nm);
                            z=U(j,i);
                            U(j,nm)=y*c+z*s;
                            U(j,i)=z*c-y*s;
                        }
                    }
                }
                z=S(k);

                if (ppi == k) {       /* Convergence. */
                    if (z < e_scalar(0.0)) {   /* Singular value is made nonnegative. */
                        S(k) = -z;
                        for (j=0;j<cols;j++) V(j,k)=-V(j,k);
                    }
                    break;
                }

                x=S(ppi);            /* Shift from bottom 2-by-2 minor: */
                nm=k-1;
                y=S(nm);
                g=tmp(nm);
                h=tmp(k);
                f=((y-z)*(y+z)+(g-h)*(g+h))/(e_scalar(2.0)*h*y);

                g=PYTHAG(f,e_scalar(1.0));
                f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;

                /* Next QR transformation: */
                c=s=1.0;
                for (j=ppi;j<=nm;j++) {
                    i=j+1;
                    g=tmp(i);
                    y=S(i);
                    h=s*g;
                    g=c*g;
                    z=PYTHAG(f,h);
                    tmp(j)=z;
                    c=f/z;
                    s=h/z;
                    f=x*c+g*s;
                    g=g*c-x*s;
                    h=y*s;
                    y=y*c;
                    for (jj=0;jj<cols;jj++) {
                        x=V(jj,j);
                        z=V(jj,i);
                        V(jj,j)=x*c+z*s;
                        V(jj,i)=z*c-x*s;
                    }
                    z=PYTHAG(f,h);
                    S(j)=z;
                    if (z) {
                        z=e_scalar(1.0)/z;
                        c=f*z;
                        s=h*z;
                    }
                    f=(c*g)+(s*y);
                    x=(c*y)-(s*g);
                    for (jj=0;jj<rows;jj++) {
                        y=U(jj,j);
                        z=U(jj,i);
                        U(jj,j)=y*c+z*s;
                        U(jj,i)=z*c-y*s;
                    }
                }
                tmp(ppi)=0.0;
                tmp(k)=f;
                S(k)=x;
            }
        }

        //Sort eigen values:
        for (i=0; i<cols; i++){

            double S_max = S(i);
            int i_max = i;
            for (j=i+1; j<cols; j++){
                double Sj = S(j);
                if (Sj > S_max){
                    S_max = Sj;
                    i_max = j;
                }
            }
            if (i_max != i){
                /* swap eigenvalues */
                e_scalar tmp = S(i);
                S(i)=S(i_max);
                S(i_max)=tmp;

                /* swap eigenvectors */
                U.col(i).swap(U.col(i_max));
                V.col(i).swap(V.col(i_max));
            }
        }

        if (its == maxiter)
            return (-2);
        else
            return (0);
    }

}
#endif


#include <boost/random/fisher_f_distribution.hpp>


#pragma once

#include "spectral/hermiten_impl.hpp"
#include "spectral_function_base.hpp"
#include "spectral_weight_function.hpp"

#include <boost/math/special_functions/gamma.hpp>
#include <stdexcept>

namespace boltzmann {

namespace local_ {
struct hermite_id_t
{
 private:
  constexpr const static double FUZZY = 1e6;

 public:
  typedef hermite_id_t id_t;

  /// Default constructor
  hermite_id_t()
      : hermite_id_t(-1, std::nan("nan"))
  { /* empty */
  }

  hermite_id_t(int k_, double fw_)
      : k(k_)
      , fw(fw_)
      , idw(FUZZY * fw_)
  { /* empty */
  }

  hermite_id_t(const id_t &id)
      : k(id.k)
      , fw(id.fw)
      , idw(id.idw)
  {
  }

  /// degree
  int k;
  /// weight exponent
  double fw;
  /// weight id
  long int idw;

  bool operator<(const id_t &other) const
  {
    return std::tie(k, idw) < std::tie(other.k, other.idw);
  }

  // ----------------------------------------------------------------------
  inline bool operator==(const id_t &other) const
  {
    return std::tie(k, idw) == std::tie(other.k, other.idw);
  }

  // ----------------------------------------------------------------------
  friend std::ostream &operator<<(std::ostream &stream, const id_t &x)
  {
    stream << x.to_string();
    return stream;
  }

  // ----------------------------------------------------------------------
  std::string to_string() const
  {
    return "H_" + boost::lexical_cast<std::string>(k) + ", fw_" +
           boost::lexical_cast<std::string>(fw);
  }

  // ----------------------------------------------------------------------
  inline std::tuple<int, long int> key() const { return std::make_tuple(k, idw); }
};
}  // end namespace local_
}  // end namespace boltzmann

namespace std {
// hash functions for id's
template <>
class hash<boltzmann::local_::hermite_id_t>
{
 public:
  size_t operator()(const boltzmann::local_::hermite_id_t &id) const
  {
    std::size_t current = std::hash<double>()(id.fw);
    boost::hash_combine(current, std::hash<int>()(id.k));
    return current;
  }
};
}  // end namespace std

namespace boltzmann {

/**
 * @brief Normalized physicists' Hermite polynomial
 *
 */
class HermiteH : public weighted<HermiteH, true>, public local_::index_policy<local_::hermite_id_t>
{
 public:
  typedef double numeric_t;

 public:
  /**
   * Hermite basis function with exp weight
   *
   * @param k
   * @param w
   *
   * @return
   */
  explicit HermiteH(int k, double w = 0.5);
  explicit HermiteH(const id_t &id)
      : id_(id)
  {
  }
  HermiteH(){};

  /// evaluate polynomial part
  numeric_t evaluate(double x) const;

  /// evaluate weight
  numeric_t weight(double x) const;

  /// return weight
  numeric_t w() const { return id_.fw; }

  const id_t &get_id() const { return id_; }
  unsigned int get_degree() const { return id_.k; }

 private:
  id_t id_;
};

// ----------------------------------------------------------------------
inline HermiteH::HermiteH(int k, double w)
    : id_(k, w)
{ /* empty */
}

// ----------------------------------------------------------------------
inline typename HermiteH::numeric_t
HermiteH::evaluate(double x) const
{
  return boost::math::hermiten(id_.k, x);
}

// ----------------------------------------------------------------------
inline typename HermiteH::numeric_t
HermiteH::weight(double x) const
{
  return std::exp(-x * x * id_.fw);
}

}  // end namespace boltzmann


// 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 active CMA-ES

// Supports parallel fitness function evaluation.
//
// For expensive objective functions (e.g. machine learning parameter optimization) use the workers
// parameter to parallelize objective function evaluation. The workers parameter should be limited
// the population size because otherwize poulation update is delayed.

// Derived from http://cma.gforge.inria.fr/cmaes.m which follows
// https://www.researchgate.net/publication/227050324_The_CMA_Evolution_Strategy_A_Comparing_Review
// Requires Eigen version >= 3.3.90 because new slicing capabilities are used, see
// https://eigen.tuxfamily.org/dox-devel/group__TutorialSlicingIndexing.html
// requires https://github.com/imneme/pcg-cpp

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

using namespace std;

namespace acmaes {

static ivec inverse(const ivec &indices) {
    ivec inverse = ivec(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;
}

class AcmaesOptimizer {

public:

    AcmaesOptimizer(long runid_, Fitness *fitfun_, int popsize_, int mu_,
            const vec &guess_, const vec &inputSigma_, int maxEvaluations_,
            double accuracy_, double stopfitness_,
            int update_gap_, long seed) {
        // runid used for debugging / logging
        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_;
        // 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::Constant(dim, inputSigma_[0]);
        else
            inputSigma = inputSigma_;
        // overall standard deviation - search volume.
        sigma = inputSigma.maxCoeff();
        // termination criteria
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_;
        // 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).array()) * -1.) + log(mu + 0.5);
        double sumw = weights.sum();
        double sumwq = weights.squaredNorm();
        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. * std::max(0., sqrt((mueff - 1.) / (dim + 1.)) - 1.))
                        * max(0.3,
                                1. - // modification for short runs
                                dim / (1e-6 + (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.);
        // lazy covariance update gap
        lazy_update_gap =
                update_gap_ >= 0 ?
                        update_gap_ :
                        1.0 / (ccov1 + ccovmu + 1e-23) / dim / 10.0;
        // CMA internal values - updated each generation
        // objective variables.
        xmean = fitfun->encode(guess);
        // evolution path.
        pc = zeros(dim);
        // evolution path for sigma.
        ps = zeros(dim);
        // norm of ps, stored for efficiency.
        normps = ps.norm();
        // coordinate system.
        B = Eigen::MatrixXd::Identity(dim, dim);
        // diagonal of sqrt(D), stored for efficiency.
        diagD = inputSigma / sigma;
        diagC = diagD.cwiseProduct(diagD);
        // B*D, stored for efficiency.
        BD = B.cwiseProduct(diagD.transpose().replicate(dim, 1));
        // covariance matrix.
        C = B * (Eigen::MatrixXd::Identity(dim, dim) * B.transpose());
        // number of iterations.
        iterations = 1;
        // size of history queue of best values.
        historySize = 10 + int(3. * 10. * dim / popsize);
        // stop criteria
        stop = 0;
        // best value so far
        bestValue = DBL_MAX;
        // best parameters so far
        bestX = guess;
        // history queue of best values.
        fitnessHistory = vec::Constant(historySize, DBL_MAX);
        fitnessHistory(0) = bestValue;
        rs = new pcg64(seed);
    }

    ~AcmaesOptimizer() {
        delete rs;
    }

    // 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 = ps.norm();
        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 ivec &arindex, const mat &xold) {
        double negccov = 0;
        if (ccov1 + ccovmu > 0) {
            mat arpos = (bestArx - xold.replicate(1, mu)) * (1. / sigma); // mu difference vectors
            mat roneu = pc * pc.transpose() * 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
            ivec arReverseIndex = arindex.reverse();
            mat arzneg = arz(Eigen::all, arReverseIndex.head(mu));
            vec arnorms = arzneg.colwise().norm();
            ivec idxnorms = sort_index(arnorms);
            vec arnormsSorted = arnorms(idxnorms);
            ivec idxReverse = idxnorms.reverse();
            vec arnormsReverse = arnorms(idxReverse);
            arnorms = arnormsReverse.cwiseQuotient(arnormsSorted);
            vec arnormsInv = arnorms(inverse(idxnorms));
            mat sqarnw = arnormsInv.cwiseProduct(arnormsInv).transpose()
                            * weights;
            double negcovMax = (1. - negminresidualvariance) / sqarnw(0);
            if (negccov > negcovMax)
                negccov = negcovMax;
            arzneg = arzneg.cwiseProduct(
                    arnormsInv.transpose().replicate(dim, 1));
            mat artmp = BD * arzneg;
            mat Cneg = artmp * weights.asDiagonal() * artmp.transpose();
            oldFac += negalphaold * negccov;
            C = (C * oldFac) + roneu
                    + (arpos * (ccovmu + (1. - negalphaold) * negccov)
                            * weights.replicate(1, dim).cwiseProduct(
                                    arpos.transpose())) - (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
            mat triC = C.triangularView<Eigen::Upper>();
            mat triC1 = C.triangularView<Eigen::StrictlyUpper>();
            C = triC + triC1.transpose();
            Eigen::SelfAdjointEigenSolver<mat> sades;
            sades.compute(C);
            // diagD defines the scaling
            diagD = sades.eigenvalues();
            B = sades.eigenvectors();
            if (diagD.minCoeff() <= 0) {
                for (int i = 0; i < dim; i++)
                    if (diagD(i, 0) < 0)
                        diagD(i, 0) = 0.;
                double tfac = diagD.maxCoeff() / 1e14;
                C += Eigen::MatrixXd::Identity(dim, dim) * tfac;
                diagD += vec::Constant(dim, 1.0) * tfac;
            }
            if (diagD.maxCoeff() > 1e14 * diagD.minCoeff()) {
                double tfac = diagD.maxCoeff() / 1e14 - diagD.minCoeff();
                C += Eigen::MatrixXd::Identity(dim, dim) * tfac;
                diagD += vec::Constant(dim, 1.0) * tfac;
            }
            diagC = C.diagonal();
            diagD = diagD.cwiseSqrt(); // D contains standard deviations now
            BD = B.cwiseProduct(diagD.transpose().replicate(dim, 1));
        }
    }

    void newArgs() {
        // generate popsize offspring.
        xmean = fitfun->getClosestFeasible(xmean);
        arz = normal(dim, popsize, *rs);
        arx = mat(dim, popsize);
        for (int k = 0; k < popsize; k++) {
            vec delta = (BD * arz.col(k)) * sigma;
            arx.col(k) = fitfun->getClosestFeasible(xmean + delta);
        }
        fitness = vec(popsize);
    }

    vec ask() {
        // ask for one new argument vector.
        vec arz1 = normalVec(dim, *rs);
        vec delta = (BD * arz1) * sigma;
        vec arx1 = fitfun->getClosestFeasible(xmean + delta);
        return fitfun->decode(arx1);
    }

    int tell(double y, const vec &x) {
        //tell function value for a argument list retrieved by ask_one().
        if (told == 0) {
            fitness = vec(popsize);
            arx = mat(dim, popsize);
            arz = mat(dim, popsize);
        }
        fitness[told] = isfinite(y) ? y : DBL_MAX;
        arx.col(told) = fitfun->encode(x);
        told++;

        if (told >= popsize) {
            xmean = fitfun->getClosestFeasible(xmean);
            try {
                arz = (BD.inverse()
                        * ((arx - xmean.replicate(1, popsize)) / sigma));
            } catch (std::exception &e) {
                arz = normal(dim, popsize, *rs);
            }
            updateCMA();
            told = 0;
            iterations += 1;
        }
        return stop;
    }

    void updateCMA() {
        // sort by fitness and compute weighted mean into xmean
        ivec arindex = sort_index(fitness);
        // calculate new xmean, this is selection and recombination
        vec xold = xmean; // for speed up of Eq. (2) and (3)
        ivec bestIndex = arindex.head(mu);
        mat bestArx = arx(Eigen::all, bestIndex);
        xmean = bestArx * weights;
        mat bestArz = arz(Eigen::all, bestIndex);
        mat zmean = bestArz * weights;
        bool hsig = updateEvolutionPaths(zmean, xold);
        // adapt step size sigma
        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));
            if (isfinite(stopfitness) && bestFitness < stopfitness) {
                stop = 1;
                return;
            }
        }
        if (iterations >= last_update + lazy_update_gap) {
            last_update = iterations;
            double negccov = updateCovariance(hsig, bestArx, arz, arindex,
                    xold);
            updateBD(negccov);
            // handle termination criteria
            vec sqrtDiagC = diagC.cwiseSqrt();
            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;
            }
            if (stop > 0)
                return;
            for (int i = 0; i < dim; i++)
                if (sigma * sqrtDiagC[i] > stopTolUpX)
                    stop = 3;
            if (stop > 0)
                return;
        }
        double historyBest = fitnessHistory.minCoeff();
        double historyWorst = fitnessHistory.maxCoeff();
        if (iterations > 2
                && max(historyWorst, worstFitness)
        - min(historyBest, bestFitness) < stopTolFun) {
            stop = 4;
            return;
        }
        if (iterations > fitnessHistory.size()
                && historyWorst - historyBest < stopTolHistFun) {
            stop = 5;
            return;
        }
        // condition number of the covariance matrix exceeds 1e14
        if (diagD.maxCoeff() / diagD.minCoeff() > 1e7 * 1.0 / sqrt(accuracy)) {
            stop = 6;
            return;
        }
        // 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)
        - std::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;
    }

    void doOptimize() {

        // -------------------- Generation Loop --------------------------------
        for (iterations = 1; fitfun->evaluations() < maxEvaluations && !fitfun->terminate();
                iterations++) {
            // generate and evaluate popsize offspring
            newArgs();
            for (int k = 0; k < popsize; k++) {
                fitness[k] = fitfun->eval(fitfun->decode(arx.col(k)))(0);
                if (!isfinite(fitness[k]))
                    fitness[k] = DBL_MAX;
            }
            updateCMA();
            if (stop != 0)
                return;
        }
    }

    void do_optimize_delayed_update(int workers) {
        iterations = 0;
        fitfun->resetEvaluations();
        evaluator eval(fitfun, 1, workers);
        vec evals_x[workers];
        // fill eval queue with initial population
        for (int i = 0; i < workers; i++) {
            vec x = ask();
            eval.evaluate(x, i);
            evals_x[i] = x;
        }
        while (fitfun->evaluations() < maxEvaluations) {
            vec_id* vid = eval.result();
            vec y = vec(vid->_v);
            int p = vid->_id;
            delete vid;
            vec x = evals_x[p];
            tell(y(0), x); // tell evaluated x
            if (fitfun->evaluations() >= maxEvaluations)
                break;
            x = ask();
            eval.evaluate(x, p);
            evals_x[p] = x;
        }
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestValue;
    }

    double getIterations() {
        return iterations;
    }

    int getStop() {
        return stop;
    }

    Fitness* getFitfun() {
        return fitfun;
    }

    int getDim() {
        return dim;
    }

private:
    long runid;
    Fitness *fitfun;
    vec guess;
    double accuracy;
    int popsize; // population size
    vec inputSigma;
    int dim;
    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;
    double lazy_update_gap = 0;
    vec xmean;
    vec pc;
    vec ps;
    double normps;
    mat B;
    mat BD;
    mat diagD;
    mat C;
    vec diagC;
    mat arz;
    mat arx;
    vec fitness;
    int iterations = 0;
    int last_update = 0;
    vec fitnessHistory;
    int historySize;
    double bestValue;
    vec bestX;
    int stop;
    int told = 0;
    pcg64 *rs;
};
}

using namespace acmaes;

extern "C" {
void optimizeACMA_C(long runid, callback_type func, int dim,
        double *init, double *lower, double *upper, double *sigma,
        int maxEvals, double stopfitness, int mu, int popsize, double accuracy,
        long seed, bool normalize, int update_gap, int workers, double* res) {
    int n = dim;
    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);
    }
    Fitness fitfun(func, n, 1, lower_limit, upper_limit);
    fitfun.setNormalize(normalize);
    AcmaesOptimizer opt(runid, &fitfun, popsize, mu, guess, inputSigma,
            maxEvals, accuracy, stopfitness, update_gap, seed);
    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 < n; i++)
            res[i] = bestX[i];
        res[n] = bestY;
        res[n + 1] = fitfun.evaluations();
        res[n + 2] = opt.getIterations();
        res[n + 3] = opt.getStop();
    } catch (std::exception &e) {
        cout << e.what() << endl;
    }
}
}


// 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_OPERATIONS_INCLUDE
#define MTL_OPERATIONS_INCLUDE

#include <boost/numeric/mtl/operation/adjoint.hpp>
#include <boost/numeric/mtl/operation/clone.hpp>
#include <boost/numeric/mtl/operation/cholesky.hpp>
#include <boost/numeric/mtl/operation/column_in_matrix.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/mtl/operation/copysign.hpp>
#include <boost/numeric/mtl/operation/crop.hpp>
#include <boost/numeric/mtl/operation/cross.hpp>
#include <boost/numeric/mtl/operation/cuppen.hpp>
#include <boost/numeric/mtl/operation/diagonal.hpp>
#include <boost/numeric/mtl/operation/dot.hpp>
#include <boost/numeric/mtl/operation/eigenvalue.hpp>
#include <boost/numeric/mtl/operation/eigenvalue_symmetric.hpp>
#include <boost/numeric/mtl/operation/entry1D.hpp>
#include <boost/numeric/mtl/operation/entry_similar.hpp>
#include <boost/numeric/mtl/operation/evaluate_lazy.hpp>
#include <boost/numeric/mtl/operation/extended_complex.hpp>
#include <boost/numeric/mtl/operation/fill.hpp>
#include <boost/numeric/mtl/operation/fuse.hpp>
#include <boost/numeric/mtl/operation/givens.hpp>
#include <boost/numeric/mtl/operation/hermitian.hpp>
#include <boost/numeric/mtl/operation/hessenberg.hpp>
#include <boost/numeric/mtl/operation/householder.hpp>
#include <boost/numeric/mtl/operation/imag.hpp>
#include <boost/numeric/mtl/operation/inv.hpp>
#include <boost/numeric/mtl/operation/invert_diagonal.hpp>
#include <boost/numeric/mtl/operation/is_negative.hpp>
#include <boost/numeric/mtl/operation/lazy.hpp>
#include <boost/numeric/mtl/operation/left_scale_inplace.hpp>
#include <boost/numeric/mtl/operation/lower_trisolve.hpp>
#include <boost/numeric/mtl/operation/lu.hpp>
#include <boost/numeric/mtl/operation/make_sparse.hpp>
#include <boost/numeric/mtl/operation/make_tag_vector.hpp>
#include <boost/numeric/mtl/operation/merge_complex_vector.hpp>
#include <boost/numeric/mtl/operation/minimal_increase.hpp>
#include <boost/numeric/mtl/operation/misc.hpp>
#include <boost/numeric/mtl/operation/mult.hpp>
#include <boost/numeric/mtl/operation/norms.hpp>
#include <boost/numeric/mtl/operation/ones.hpp>
#include <boost/numeric/mtl/operation/operators.hpp>
#include <boost/numeric/mtl/operation/orth.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/product.hpp>
#include <boost/numeric/mtl/operation/qr.hpp>
#include <boost/numeric/mtl/operation/random.hpp>
#include <boost/numeric/mtl/operation/rank_one_update.hpp>
#include <boost/numeric/mtl/operation/rank_two_update.hpp>
#include <boost/numeric/mtl/operation/real.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/operation/right_scale_inplace.hpp>
#include <boost/numeric/mtl/operation/scale.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/mtl/operation/secular.hpp>
#include <boost/numeric/mtl/operation/signum.hpp>
#include <boost/numeric/mtl/operation/split_complex_vector.hpp>
#include <boost/numeric/mtl/operation/sub_matrix.hpp>
#include <boost/numeric/mtl/operation/sum.hpp>
#include <boost/numeric/mtl/operation/min.hpp>
#include <boost/numeric/mtl/operation/min_pos.hpp>
#include <boost/numeric/mtl/operation/max.hpp>
#include <boost/numeric/mtl/operation/max_pos.hpp>
#include <boost/numeric/mtl/operation/max_abs_pos.hpp>
#include <boost/numeric/mtl/operation/num_cols.hpp>
#include <boost/numeric/mtl/operation/num_rows.hpp>
#include <boost/numeric/mtl/operation/row_in_matrix.hpp>
#include <boost/numeric/mtl/operation/size.hpp>
#include <boost/numeric/mtl/operation/size1D.hpp>
#include <boost/numeric/mtl/operation/static_num_cols.hpp>
#include <boost/numeric/mtl/operation/static_num_rows.hpp>
#include <boost/numeric/mtl/operation/static_size.hpp>
#include <boost/numeric/mtl/operation/svd.hpp>
#include <boost/numeric/mtl/operation/swap_row.hpp>
#include <boost/numeric/mtl/operation/trace.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/operation/unary_dot.hpp>
#include <boost/numeric/mtl/operation/unroll.hpp>
#include <boost/numeric/mtl/operation/upper_trisolve.hpp>

#include <boost/numeric/mtl/matrix/bands.hpp>
#include <boost/numeric/mtl/matrix/identity.hpp>
#include <boost/numeric/mtl/matrix/lower.hpp>
#include <boost/numeric/mtl/matrix/permutation.hpp>
#include <boost/numeric/mtl/matrix/reorder.hpp>
#include <boost/numeric/mtl/matrix/reorder_ref.hpp>
#include <boost/numeric/mtl/matrix/reorder_matrix_rows.hpp>
#include <boost/numeric/mtl/matrix/strict_upper.hpp>
#include <boost/numeric/mtl/matrix/strict_lower.hpp>
#include <boost/numeric/mtl/matrix/upper.hpp>

#include <boost/numeric/mtl/io/path.hpp>

#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/string_to_enum.hpp>
#include <boost/numeric/mtl/utility/make_copy_or_reference.hpp>

#include <boost/numeric/mtl/interface/umfpack_solve.hpp>

#endif // MTL_OPERATIONS_INCLUDE


// #include <opencv2/core/core.hpp> // needed for verbosity >= 3, DISVISUAL
// #include <opencv2/highgui/highgui.hpp> // needed for verbosity >= 3, DISVISUAL
// #include <opencv2/imgproc/imgproc.hpp> // needed for verbosity >= 3, DISVISUAL

#include <iostream>
#include <string>
#include <vector>
#include <valarray>

#include <thread>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Dense>

#include <stdio.h>

#include "patch.h"
#include "patchgrid.h"

using std::cout;
using std::endl;
using std::vector;

namespace OFC
{

  PatGridClass::PatGridClass(
    const camparam* cpt_in,
    const camparam* cpo_in,
    const optparam* op_in)
  :
    cpt(cpt_in),
    cpo(cpo_in),
    op(op_in)
  {

  // Generate grid on current scale
  steps = op->steps;
  nopw = ceil( (float)cpt->width /  (float)steps );
  noph = ceil( (float)cpt->height / (float)steps );
  const int offsetw = floor((cpt->width - (nopw-1)*steps)/2);
  const int offseth = floor((cpt->height - (noph-1)*steps)/2);

  nopatches = nopw*noph;
  pt_ref.resize(nopatches);
  p_init.resize(nopatches);
  pat.reserve(nopatches);

  im_ao_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
  im_ao_dx_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
  im_ao_dy_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);

  im_bo_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
  im_bo_dx_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
  im_bo_dy_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);

  int patchid=0;
  for (int x = 0; x < nopw; ++x)
  {
    for (int y = 0; y < noph; ++y)
    {
      int i = x*noph + y;

      pt_ref[i][0] = x * steps + offsetw;
      pt_ref[i][1] = y * steps + offseth;
      p_init[i].setZero();

      pat.push_back(new OFC::PatClass(cpt, cpo, op, patchid));
      patchid++;
    }
  }
}

PatGridClass::~PatGridClass()
{
  delete im_ao_eg;
  delete im_ao_dx_eg;
  delete im_ao_dy_eg;

  delete im_bo_eg;
  delete im_bo_dx_eg;
  delete im_bo_dy_eg;

  for (int i=0; i< nopatches; ++i)
    delete pat[i];
}

void PatGridClass::SetComplGrid(PatGridClass *cg_in)
{
  cg = cg_in;
}

void PatGridClass::InitializeGrid(const float * im_ao_in, const float * im_ao_dx_in, const float * im_ao_dy_in)
{
  im_ao = im_ao_in;
  im_ao_dx = im_ao_dx_in;
  im_ao_dy = im_ao_dy_in;

  new (im_ao_eg) Eigen::Map<const Eigen::MatrixXf>(im_ao,cpt->height,cpt->width); // new placement operator
  new (im_ao_dx_eg) Eigen::Map<const Eigen::MatrixXf>(im_ao_dx,cpt->height,cpt->width);
  new (im_ao_dy_eg) Eigen::Map<const Eigen::MatrixXf>(im_ao_dy,cpt->height,cpt->width);

  #pragma omp parallel for schedule(static)
  for (int i = 0; i < nopatches; ++i)
  {
    pat[i]->InitializePatch(im_ao_eg, im_ao_dx_eg, im_ao_dy_eg, pt_ref[i]);
    p_init[i].setZero();
  }

}

void PatGridClass::SetTargetImage(const float * im_bo_in, const float * im_bo_dx_in, const float * im_bo_dy_in)
{
  im_bo = im_bo_in;
  im_bo_dx = im_bo_dx_in;
  im_bo_dy = im_bo_dy_in;

  new (im_bo_eg) Eigen::Map<const Eigen::MatrixXf>(im_bo,cpt->height,cpt->width); // new placement operator
  new (im_bo_dx_eg) Eigen::Map<const Eigen::MatrixXf>(im_bo_dx,cpt->height,cpt->width); // new placement operator
  new (im_bo_dy_eg) Eigen::Map<const Eigen::MatrixXf>(im_bo_dy,cpt->height,cpt->width); // new placement operator

  #pragma omp parallel for schedule(static)
  for (int i = 0; i < nopatches; ++i)
    pat[i]->SetTargetImage(im_bo_eg, im_bo_dx_eg, im_bo_dy_eg);

}

void PatGridClass::Optimize()
{
    #pragma omp parallel for schedule(dynamic,10)
    for (int i = 0; i < nopatches; ++i)
    {
      pat[i]->OptimizeIter(p_init[i], true); // optimize until convergence
    }
}

// void PatGridClass::OptimizeAndVisualize(const float sc_fct_tmp) // needed for verbosity >= 3, DISVISUAL
// {
//   bool allconverged=0;
//   int cnt = 0;
//   while (!allconverged)
//   {
//     cnt++;
//
//     allconverged=1;
//
//     for (int i = 0; i < nopatches; ++i)
//     {
//       if (pat[i]->isConverged()==0)
//       {
//         pat[i]->OptimizeIter(p_init[i], false); // optimize, only one iterations
//         allconverged=0;
//       }
//     }
//
//
//     // Display original image
//     const cv::Mat src(cpt->height+2*cpt->imgpadding, cpt->width+2*cpt->imgpadding, CV_32FC1, (void*) im_ao);
//     cv::Mat img_ao_mat = src(cv::Rect(cpt->imgpadding,cpt->imgpadding,cpt->width,cpt->height));
//     cv::Mat outimg;
//     img_ao_mat.convertTo(outimg, CV_8UC1);
//     cv::cvtColor(outimg, outimg, CV_GRAY2RGB);
//     cv::resize(outimg, outimg, cv::Size(), sc_fct_tmp, sc_fct_tmp, cv::INTER_NEAREST);
//
//     for (int i = 0; i < nopatches; ++i)
//     {
//       // Show displacement vector
//       const Eigen::Vector2f pt_ret = pat[i]->GetPointPos();
//
//       Eigen::Vector2f pta, ptb;
//
//       cv::line(outimg, cv::Point( (pt_ref[i][0]+.5)*sc_fct_tmp, (pt_ref[i][1]+.5)*sc_fct_tmp ), cv::Point( (pt_ret[0]+.5)*sc_fct_tmp, (pt_ret[1]+.5)*sc_fct_tmp ), cv::Scalar(255*pat[i]->isConverged() ,255*(!pat[i]->isConverged()),0),  2);
//
//       cv::line(outimg, cv::Point( (cpt->cx+.5)*sc_fct_tmp, (cpt->cy+.5)*sc_fct_tmp ), cv::Point( (cpt->cx+.5)*sc_fct_tmp, (cpt->cy+.5)*sc_fct_tmp ), cv::Scalar(0,0, 255),  2);
//
//     }
//
//     char str[200];
//     sprintf(str,"Iter: %i",cnt);
//     cv::putText(outimg, str, cv::Point2f(20,20), cv::FONT_HERSHEY_PLAIN, 1,  cv::Scalar(0,0,255,255), 2);
//
//     cv::namedWindow( "Img_iter", cv::WINDOW_AUTOSIZE );
//     cv::imshow( "Img_iter", outimg);
//
//     cv::waitKey(500);
//   }
// }

void PatGridClass::InitializeFromCoarserOF(const float * flow_prev)
{
  #pragma omp parallel for schedule(dynamic,10)
  for (int ip = 0; ip < nopatches; ++ip)
  {
    int x = floor(pt_ref[ip][0] / 2); // better, but slower: use bil. interpolation here
    int y = floor(pt_ref[ip][1] / 2);
    int i = y*(cpt->width/2) + x;

    #if (SELECTMODE==1)
    p_init[ip](0) = flow_prev[2*i  ]*2;
    p_init[ip](1) = flow_prev[2*i+1]*2;
    #else
    p_init[ip](0) = flow_prev[  i  ]*2;
    #endif
  }
}

void PatGridClass::AggregateFlowDense(float *flowout) const
{
  float* we = new float[cpt->width * cpt->height];

  memset(flowout, 0, sizeof(float) * (op->nop * cpt->width * cpt->height) );
  memset(we,      0, sizeof(float) * (          cpt->width * cpt->height) );

  #ifdef USE_PARALLEL_ON_FLOWAGGR // Using this enables OpenMP on flow aggregation. This can lead to race conditions. Experimentally we found that the result degrades only marginally. However, for our experiments we did not enable this.
    #pragma omp parallel for schedule(static)
  #endif
  for (int ip = 0; ip < nopatches; ++ip)
  {

    if (pat[ip]->IsValid())
    {
      #if (SELECTMODE==1)
      const Eigen::Vector2f*            fl = pat[ip]->GetParam(); // flow displacement of this patch
      Eigen::Vector2f flnew;
      #else
      const Eigen::Matrix<float, 1, 1>* fl = pat[ip]->GetParam(); // horz. displacement of this patch
      Eigen::Matrix<float, 1, 1> flnew;
      #endif

      const float * pweight = pat[ip]->GetpWeightPtr(); // use image error as weight

      int lb = -op->p_samp_s/2;
      int ub = op->p_samp_s/2-1;

      for (int y = lb; y <= ub; ++y)
      {
        for (int x = lb; x <= ub; ++x, ++pweight)
        {
          int yt = (y + pt_ref[ip][1]);
          int xt = (x + pt_ref[ip][0]);

          if (xt >= 0 && yt >= 0 && xt < cpt->width && yt < cpt->height)
          {

            int i = yt*cpt->width + xt;

            #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // single channel/gradient image
            float absw = 1.0f /  (float)(std::max(op->minerrval  ,*pweight));
            #else  // RGB image
            float absw = (float)(std::max(op->minerrval  ,*pweight)); ++pweight;
                  absw+= (float)(std::max(op->minerrval  ,*pweight)); ++pweight;
                  absw+= (float)(std::max(op->minerrval  ,*pweight));
            absw = 1.0f / absw;
            #endif

            flnew = (*fl) * absw;
            we[i] += absw;

            #if (SELECTMODE==1)
            flowout[2*i]   += flnew[0];
            flowout[2*i+1] += flnew[1];
            #else
            flowout[i] += flnew[0];
            #endif
          }
        }
      }
    }
  }

  // if complementary (forward-backward merging) is given, integrate negative backward flow as well
  if (cg)
  {
      Eigen::Vector4f wbil; // bilinear weight vector
      Eigen::Vector4i pos;

      #ifdef USE_PARALLEL_ON_FLOWAGGR
        #pragma omp parallel for schedule(static)
      #endif
      for (int ip = 0; ip < cg->nopatches; ++ip)
      {
        if (cg->pat[ip]->IsValid())
        {
          #if (SELECTMODE==1)
          const Eigen::Vector2f*            fl = (cg->pat[ip]->GetParam()); // flow displacement of this patch
          Eigen::Vector2f flnew;
          #else
          const Eigen::Matrix<float, 1, 1>* fl = (cg->pat[ip]->GetParam()); // horz. displacement of this patch
          Eigen::Matrix<float, 1, 1> flnew;
          #endif

          const Eigen::Vector2f rppos = cg->pat[ip]->GetPointPos(); // get patch position after optimization
          const float * pweight = cg->pat[ip]->GetpWeightPtr(); // use image error as weight

          Eigen::Vector2f resid;

          // compute bilinear weight vector
          pos[0] = ceil(rppos[0] +.00001); // make sure they are rounded up to natural number
          pos[1] = ceil(rppos[1] +.00001); // make sure they are rounded up to natural number
          pos[2] = floor(rppos[0]);
          pos[3] = floor(rppos[1]);

          resid[0] = rppos[0] - pos[2];
          resid[1] = rppos[1] - pos[3];
          wbil[0] = resid[0]*resid[1];
          wbil[1] = (1-resid[0])*resid[1];
          wbil[2] = resid[0]*(1-resid[1]);
          wbil[3] = (1-resid[0])*(1-resid[1]);

          int lb = -op->p_samp_s/2;
          int ub = op->p_samp_s/2-1;

          for (int y = lb; y <= ub; ++y)
          {
            for (int x = lb; x <= ub; ++x, ++pweight)
            {

              int yt = y + pos[1];
              int xt = x + pos[0];
              if (xt >= 1 && yt >= 1 && xt < (cpt->width-1) && yt < (cpt->height-1))
              {

                #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // single channel/gradient image
                float absw = 1.0f /  (float)(std::max(op->minerrval  ,*pweight));
                #else  // RGB
                float absw = (float)(std::max(op->minerrval  ,*pweight)); ++pweight;
                      absw+= (float)(std::max(op->minerrval  ,*pweight)); ++pweight;
                      absw+= (float)(std::max(op->minerrval  ,*pweight));
                absw = 1.0f / absw;
                #endif

                flnew = (*fl) * absw;

                int idxcc =  xt    +  yt   *cpt->width;
                int idxfc = (xt-1) +  yt   *cpt->width;
                int idxcf =  xt    + (yt-1)*cpt->width;
                int idxff = (xt-1) + (yt-1)*cpt->width;

                we[idxcc] += wbil[0] * absw;
                we[idxfc] += wbil[1] * absw;
                we[idxcf] += wbil[2] * absw;
                we[idxff] += wbil[3] * absw;

                #if (SELECTMODE==1)
                flowout[2*idxcc  ] -= wbil[0] * flnew[0];   // use reversed flow
                flowout[2*idxcc+1] -= wbil[0] * flnew[1];

                flowout[2*idxfc  ] -= wbil[1] * flnew[0];
                flowout[2*idxfc+1] -= wbil[1] * flnew[1];

                flowout[2*idxcf  ] -= wbil[2] * flnew[0];
                flowout[2*idxcf+1] -= wbil[2] * flnew[1];

                flowout[2*idxff  ] -= wbil[3] * flnew[0];
                flowout[2*idxff+1] -= wbil[3] * flnew[1];
                #else
                flowout[idxcc] -= wbil[0] * flnew[0]; // simple averaging of inverse horizontal displacement
                flowout[idxfc] -= wbil[1] * flnew[0];
                flowout[idxcf] -= wbil[2] * flnew[0];
                flowout[idxff] -= wbil[3] * flnew[0];
                #endif
              }
            }
          }
        }
      }
  }

  #pragma omp parallel for schedule(static, 100)
  // normalize each pixel by dividing displacement by aggregated weights from all patches
  for (int yi = 0; yi < cpt->height; ++yi)
  {
    for (int xi = 0; xi < cpt->width; ++xi)
    {
      int i    = yi*cpt->width + xi;
      if (we[i]>0)
      {
        #if (SELECTMODE==1)
        flowout[2*i  ] /= we[i];
        flowout[2*i+1] /= we[i];
        #else
        flowout[i] /= we[i];
        #endif
      }
    }
  }

  delete[] we;
}

}


#include <CExprI.h>
#include <boost/math/special_functions/erf.hpp>
#include <CMathGen.h>
#include <CInvNorm.h>
#include <COSNaN.h>

#include <cmath>
#include <ccomplex>
#include <cstdlib>
#include <cstring>

// NOTE: types are only needed if normal conversion rules don't handle the type correctly

#ifdef GNUPLOT_EXPR
namespace {

double invnorm(double x) {
  //return sqrt(2)/erf(2*x - 1);
  double y = CInvNorm::calc(x);
  if (COSNaN::is_pos_inf(y) || COSNaN::is_neg_inf(y))
    COSNaN::set_nan(y);
  return y;
}

double norm(double x)
{
  x = 0.5*sqrt(2)*x;
  x = 0.5*erfc(-x);

  return x;
}

double inverf(double x) {
  try {
    return boost::math::erf_inv(x);
  } catch (...) {
    return CMathGen::getNaN();
  }
}

// TODO
std::complex<double> cerf(const std::complex<double> &c) {
  double r = ::erf(c.real());
  return std::complex<double>(r, 0);
}

// TODO
std::complex<double> cerfc(const std::complex<double> &c) {
  double r = ::erfc(c.real());
  return std::complex<double>(r, 0);
}

double RadToDeg(double x) {
  return 180.0*x/M_PI;
}

double DegToRad(double x) {
  return M_PI*x/180.0;
}

}

class CExprSubStr {
 public:
  std::string operator()(const std::string &str, int i1, int i2) {
    if (i1 < 1              ) i1 = 1;
    if (i1 > int(str.size())) i1 = str.size();

    if (i2 < 0              ) i2 = str.size();
    if (i2 > int(str.size())) i2 = str.size();

    std::string str1;

    for (int j = i1 - 1; j <= i2 - 1; ++j)
      str1 += str[j];

    return str1;
  }
};

class CExprStrStrT {
 public:
  // find position of str2 in str1
  int operator()(const std::string &str1, const std::string &str2) {
    auto p = str1.find(str2);

    if (p == std::string::npos)
      return 0;
    else
      return p + 1;
  }
};

class CExprWords {
 public:
  int operator()(const std::string &str) {
    std::vector<std::string> words;

    (void) CStrUtil::addWords(str, words);

    return words.size();
  }
};

class CExprWord {
 public:
  std::string operator()(const std::string &str, int i) {
    std::vector<std::string> words;

    (void) CStrUtil::addWords(str, words);

    if (i > 0 && i <= int(words.size()))
      return words[i - 1];
    else
      return "";
  }
};

class CExprStrLen {
 public:
  int operator()(const std::string &str) {
    return str.size();
  }
};

class CExprSystem {
 public:
  std::string operator()(const std::string &str) {
    FILE *fp = popen(str.c_str(), "r");
    if (! fp) return "";

    std::string res;

    int c;

    while ((c = fgetc(fp)) != EOF)
      res += char(c);

    pclose(fp);

    return res;
  }
};
#endif

struct CExprBuiltinFunction {
  const char        *name;
  const char        *args;
  CExprFunctionProc  proc;
};

static CExprValuePtr
CExprFunctionSqrt(CExpr *expr, const CExprValueArray &values)
{
  assert(values.size() == 1);

  double real;

  if (! values[0]->getRealValue(real))
    return CExprValuePtr();

  if (real >= 0.0) {
    double real1 = ::sqrt(real);

    return expr->createRealValue(real1);
  }
  else {
    double real1 = ::sqrt(-real);

    return expr->createComplexValue(std::complex<double>(0, real1));
  }
}

static CExprValuePtr
CExprFunctionExp(CExpr *expr, const CExprValueArray &values)
{
  assert(values.size() == 1);

  double               r;
  std::complex<double> c;

  if      (values[0]->isComplexValue()) {
    if (values[0]->getComplexValue(c)) {
      double r1 = exp(c.real())*cos(c.imag());
      double c1 = exp(c.real())*sin(c.imag());

      return expr->createComplexValue(std::complex<double>(r1, c1));
    }
    else
      return CExprValuePtr();
  }
  else if (values[0]->getRealValue(r)) {
    double r1 = exp(r);

    return expr->createRealValue(r1);
  }
  else
    return CExprValuePtr();
}

#define CEXPR_REAL_TO_REAL_FUNC(NAME, F) \
static CExprValuePtr \
CExprFunction##NAME(CExpr *expr, const CExprValueArray &values) { \
  assert(values.size() == 1); \
  double r = 0.0; \
  if (values[0]->getRealValue(r)) { \
  } \
  else \
    return CExprValuePtr(); \
  double r1 = F(r); \
  return expr->createRealValue(r1); \
}

#define CEXPR_REALC_TO_REAL_FUNC(NAME, F) \
static CExprValuePtr \
CExprFunction##NAME(CExpr *expr, const CExprValueArray &values) { \
  assert(values.size() == 1); \
  double r = 0.0; \
  if (values[0]->isComplexValue()) { \
    std::complex<double> c; \
    if (values[0]->getComplexValue(c)) { \
      r = c.real(); \
    } \
    else \
      return CExprValuePtr(); \
  } \
  else if (values[0]->getRealValue(r)) { \
  } \
  else \
    return CExprValuePtr(); \
  double r1 = F(r); \
  return expr->createRealValue(r1); \
}

#define CEXPR_REALC_TO_REALC_FUNC(NAME, F) \
static CExprValuePtr \
CExprFunction##NAME(CExpr *expr, const CExprValueArray &values) { \
  assert(values.size() == 1); \
  double r; \
  if (values[0]->isComplexValue()) { \
    std::complex<double> c; \
    if (values[0]->getComplexValue(c)) { \
      errno = 0; \
      std::complex<double> c1 = F(c); \
      if (errno != 0) return CExprValuePtr(); \
      return expr->createComplexValue(c1); \
    } \
      return CExprValuePtr(); \
  } \
  else if (values[0]->getRealValue(r)) { \
    double r1 = F(r); \
    return expr->createRealValue(r1); \
  } \
  else \
    return CExprValuePtr(); \
}

#define CEXPR_ANGLE_TO_REAL_FUNC(NAME, F) \
static CExprValuePtr \
CExprFunction##NAME(CExpr *expr, const CExprValueArray &values) { \
  assert(values.size() == 1); \
  double real; \
  if (values[0]->getRealValue(real)) { \
    if (expr->getDegrees()) \
      real = DegToRad(real); \
    double real1 = F(real); \
    return expr->createRealValue(real1); \
  } \
  return CExprValuePtr(); \
}

#define CEXPR_REALC_TO_ANGLE_FUNC(NAME, F) \
static CExprValuePtr \
CExprFunction##NAME(CExpr *expr, const CExprValueArray &values) { \
  assert(values.size() == 1); \
  double r; \
  if (values[0]->isComplexValue()) { \
    std::complex<double> c; \
    if (values[0]->getComplexValue(c)) { \
      errno = 0; \
      std::complex<double> c1 = F(c); \
      if (errno != 0) return CExprValuePtr(); \
      if (expr->getDegrees()) \
        c1 = std::complex<double>(RadToDeg(c1.real()), RadToDeg(c1.imag())); \
      return expr->createComplexValue(c1); \
    } \
    else \
      return CExprValuePtr(); \
  } \
  else if (values[0]->getRealValue(r)) { \
    errno = 0; \
    double r1 = F(r); \
    if (errno == 0) { \
      if (expr->getDegrees()) \
        r1 = RadToDeg(r1); \
      return expr->createRealValue(r1); \
    } \
    else if (errno == EDOM) { \
      std::complex<double> c(r,0); \
      errno = 0; \
      std::complex<double> c1 = F(c); \
      if (errno != 0) return CExprValuePtr(); \
      if (expr->getDegrees()) \
        c1 = std::complex<double>(RadToDeg(c1.real()), RadToDeg(c1.imag())); \
      return expr->createComplexValue(c1); \
    } \
    else { \
      return CExprValuePtr(); \
    } \
  } \
  else \
    return CExprValuePtr(); \
}

#define CEXPR_REAL2_TO_ANGLE_FUNC(NAME, F) \
static CExprValuePtr \
CExprFunction##NAME(CExpr *expr, const CExprValueArray &values) { \
  assert(values.size() == 2); \
  double real1, real2; \
  if (values[0]->getRealValue(real1) && values[1]->getRealValue(real2)) { \
    double real = F(real1, real2); \
    if (expr->getDegrees()) \
      real = RadToDeg(real); \
    return expr->createRealValue(real); \
  } \
  return CExprValuePtr(); \
}

#define CEXPR_COMPLEX_TO_COMPLEX_FUNC(NAME, F) \
static CExprValuePtr \
CExprFunction##NAME(CExpr *expr, const CExprValueArray &values) { \
  assert(values.size() == 1); \
  std::complex<double> c; \
  if (! values[0]->getComplexValue(c)) { \
    double r; \
    if (! values[0]->getRealValue(r)) \
      return CExprValuePtr(); \
    c = std::complex<double>(r, 0); \
  } \
  std::complex<double> c1 = F(c); \
  return expr->createComplexValue(c1); \
}

class CExprFunctionAbs : public CExprFunctionObj {
 public:
  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) {
    assert(values.size() == 1);
    if      (values[0]->isRealValue()) {
      double real;
      if (values[0]->getRealValue(real))
        return expr->createRealValue(std::abs(real));
    }
    else if (values[0]->isIntegerValue()) {
      long integer;
      if (values[0]->getIntegerValue(integer))
        return expr->createIntegerValue(std::abs(integer));
    }
    else if (values[0]->isComplexValue()) {
      std::complex<double> c;
      if (values[0]->getComplexValue(c))
        return expr->createRealValue(std::abs(c));
    }
    return CExprValuePtr();
  }
};

class CExprFunctionCArg : public CExprFunctionObj {
 public:
  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) {
    assert(values.size() == 1);
    if (values[0]->isComplexValue()) {
      std::complex<double> c;
      if (values[0]->getComplexValue(c)) {
        double r = std::arg(c);
        if (expr->getDegrees())
          r = DegToRad(r);
        return expr->createRealValue(r);
      }
    }
    return CExprValuePtr();
  }
};

class CExprFunctionImag : public CExprFunctionObj {
 public:
  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) {
    assert(values.size() == 1);
    if (values[0]->isComplexValue()) {
      std::complex<double> c;
      if (values[0]->getComplexValue(c))
        return expr->createRealValue(c.imag());
    }
    return CExprValuePtr();
  }
};

class CExprFunctionSign : public CExprFunctionObj {
 public:
  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) {
    assert(values.size() == 1);
    if      (values[0]->isRealValue()) {
      double real;
      if (values[0]->getRealValue(real))
        return expr->createIntegerValue(real >= 0 ? (real == 0 ? 0 : 1) : -1);
    }
    else if (values[0]->isIntegerValue()) {
      long integer;
      if (values[0]->getIntegerValue(integer))
        return expr->createIntegerValue(integer >= 0 ? (integer == 0 ? 0 : 1) : -1);
    }
    else if (values[0]->isComplexValue()) {
      std::complex<double> c;
      if (values[0]->getComplexValue(c))
        return expr->createIntegerValue(c.real() >= 0 ? (c.real() == 0 ? 0 : 1) : -1);
    }
    return CExprValuePtr();
  }
};

class CExprFunctionExpr : public CExprFunctionObj {
 public:
  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) {
    assert(values.size() == 1);

    std::string exprStr;

    if (! values[0]->getStringValue(exprStr))
      return CExprValuePtr();

    expr->saveCompileState();

    CExprValuePtr value;

    if (! expr->evaluateExpression(exprStr, value))
      value = CExprValuePtr();

    expr->restoreCompileState();

    return value;
  }
};

#ifdef GNUPLOT_EXPR
class CExprFunctionRand : public CExprFunctionObj {
 public:
  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) {
    assert(values.size() == 1);
    if (values[0]->isIntegerValue()) {
      long integer = 0;
      (void) values[0]->getIntegerValue(integer);
      if     (integer < 0)
        srand(0);
      else if (integer > 0)
        srand(integer);
      double r = (1.0*rand())/RAND_MAX;
      return expr->createRealValue(r);
    }
    return CExprValuePtr();
  }
};
#endif

#define CEXPR_REALC_TO_REAL_FOBJ(NAME, F) \
class CExprFunction##NAME : public CExprFunctionObj { \
 public: \
  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) { \
    assert(values.size() == 1); \
    double r = 0.0; \
    if      (values[0]->isRealValue()) { \
      if (! values[0]->getRealValue(r)) return CExprValuePtr(); \
    } \
    else if (values[0]->isIntegerValue()) { \
      long i = 0; \
      if (! values[0]->getIntegerValue(i)) return CExprValuePtr(); \
      r = i; \
    } \
    else if (values[0]->isComplexValue()) { \
      std::complex<double> c; \
      if (! values[0]->getComplexValue(c)) return CExprValuePtr(); \
      r = c.real(); \
    } \
    else { \
      return CExprValuePtr(); \
    } \
    double r1 = F(r); \
    return expr->createRealValue(r1); \
  } \
};

class CExprFunctionInt : public CExprFunctionObj {
 public:
  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) {
    assert(values.size() == 1);
    double r = 0.0;
    if      (values[0]->isRealValue()) {
      if (! values[0]->getRealValue(r)) return CExprValuePtr();
    }
    else if (values[0]->isIntegerValue()) {
      long i = 0;
      if (! values[0]->getIntegerValue(i)) return CExprValuePtr();
      r = i;
    }
    else if (values[0]->isComplexValue()) {
      std::complex<double> c;
      if (! values[0]->getComplexValue(c)) return CExprValuePtr();
      r = c.real();
    }
    else {
      return CExprValuePtr();
    }
    int i1 = static_cast<int>(r);
    return expr->createIntegerValue(i1);
  }
};

CEXPR_REALC_TO_REAL_FOBJ(Ceil , std::ceil)
CEXPR_REALC_TO_REAL_FOBJ(Floor, std::floor)
CEXPR_REALC_TO_REAL_FOBJ(Real , static_cast<double>)

#ifdef GNUPLOT_EXPR
class CExprFunctionSPrintF : public CExprFunctionObj {
 public:
  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) {
    assert(values.size() >= 1);

    std::string fmt;

    if (! values[0]->getStringValue(fmt))
      return CExprValuePtr();

    CExprValueArray values1;

    for (uint i = 1; i < values.size(); ++i)
      values1.push_back(values[i]);

    std::string res = expr->printf(fmt, values1);

    return expr->createStringValue(res);
  }
};
#endif

template<typename T, typename R, typename FUNC>
class CExprFunctionObjT1 : public CExprFunctionObj {
 public:
  CExprFunctionObjT1(CExprFunctionMgr *mgr, const std::string &name) {
    std::string argsStr = CExprUtil<T>::argTypeStr();

    func_ = mgr->addObjFunction(name, argsStr, this);
  }

  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) {
    assert(values.size() == 1);
    T v;
    if (CExprUtil<T>::getTypeValue(values[0], v))
      return CExprUtil<R>::createValue(expr, f_(v));
    return CExprValuePtr();
  }

  void setBuiltin(bool b) {
    func_->setBuiltin(b);
  }

 private:
  FUNC             f_;
  CExprFunctionPtr func_;
};

template<typename T1, typename T2, typename R, typename FUNC>
class CExprFunctionObjT2 : public CExprFunctionObj {
 public:
  CExprFunctionObjT2(CExprFunctionMgr *mgr, const std::string &name) {
    std::string argsStr = CExprUtil<T1>::argTypeStr() + "," + CExprUtil<T2>::argTypeStr();

    func_ = mgr->addObjFunction(name, argsStr, this);
  }

  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) {
    assert(values.size() == 2);
    T1 v1; T2 v2;
    if (CExprUtil<T1>::getTypeValue(values[0], v1) &&
        CExprUtil<T2>::getTypeValue(values[1], v2))
      return CExprUtil<R>::createValue(expr, f_(v1, v2));
    return CExprValuePtr();
  }

  void setBuiltin(bool b) {
    func_->setBuiltin(b);
  }

 private:
  FUNC             f_;
  CExprFunctionPtr func_;
};

template<typename T1, typename T2, typename T3, typename R, typename FUNC>
class CExprFunctionObjT3 : public CExprFunctionObj {
 public:
  CExprFunctionObjT3(CExprFunctionMgr *mgr, const std::string &name) {
    std::string argsStr = CExprUtil<T1>::argTypeStr() + "," +
      CExprUtil<T2>::argTypeStr() + "," + CExprUtil<T3>::argTypeStr();

    func_ = mgr->addObjFunction(name, argsStr, this);
  }

  CExprValuePtr operator()(CExpr *expr, const CExprValueArray &values) {
    assert(values.size() == 3);
    T1 v1; T2 v2; T3 v3;
    if (CExprUtil<T1>::getTypeValue(values[0], v1) &&
        CExprUtil<T2>::getTypeValue(values[1], v2) &&
        CExprUtil<T2>::getTypeValue(values[2], v3))
      return CExprUtil<R>::createValue(expr, f_(v1, v2, v3));
    return CExprValuePtr();
  }

  void setBuiltin(bool b) {
    func_->setBuiltin(b);
  }

 private:
  FUNC             f_;
  CExprFunctionPtr func_;
};

CEXPR_REALC_TO_REALC_FUNC(Log  , std::log)
CEXPR_REALC_TO_REALC_FUNC(Log10, std::log10)

CEXPR_ANGLE_TO_REAL_FUNC(Sin, ::sin)
CEXPR_ANGLE_TO_REAL_FUNC(Cos, ::cos)
CEXPR_ANGLE_TO_REAL_FUNC(Tan, ::tan)

CEXPR_REALC_TO_ANGLE_FUNC(ACos, std::acos)
CEXPR_REALC_TO_ANGLE_FUNC(ASin, std::asin)
CEXPR_REALC_TO_ANGLE_FUNC(ATan, std::atan)

CEXPR_REAL2_TO_ANGLE_FUNC(ATan2, ::atan2)

CEXPR_REALC_TO_REALC_FUNC(SinH , std::sinh)
CEXPR_REALC_TO_REALC_FUNC(CosH , std::cosh)
CEXPR_REALC_TO_REALC_FUNC(TanH , std::tanh)
CEXPR_REALC_TO_REALC_FUNC(ASinH, std::asinh)
CEXPR_REALC_TO_REALC_FUNC(ACosH, std::acosh)
CEXPR_REALC_TO_REALC_FUNC(ATanH, std::atanh)

#ifdef GNUPLOT_EXPR
// TODO: besy0, besy1
CEXPR_REAL_TO_REAL_FUNC(BesJ0  , ::j0)
CEXPR_REAL_TO_REAL_FUNC(BesJ1  , ::j1)

CEXPR_REALC_TO_REAL_FUNC(Erf   , ::erf)
CEXPR_REALC_TO_REAL_FUNC(ErfC  , ::erfc)
CEXPR_REALC_TO_REAL_FUNC(InvErf, ::inverf)

CEXPR_COMPLEX_TO_COMPLEX_FUNC(CErf , ::cerf)
CEXPR_COMPLEX_TO_COMPLEX_FUNC(CErfC, ::cerfc)

// TODO: invnorm, norm
CEXPR_REALC_TO_REAL_FUNC(Gamma  , ::gamma)
// TODO: igamma
CEXPR_REALC_TO_REAL_FUNC(LGamma , ::lgamma)
CEXPR_REALC_TO_REAL_FUNC(Norm   , ::norm)
CEXPR_REALC_TO_REAL_FUNC(InvNorm, ::invnorm)
// TODO: lambertw
#endif

static CExprBuiltinFunction
builtinFns[] = {
  { "sqrt"   , "r"  , CExprFunctionSqrt    },
  { "exp"    , "rc" , CExprFunctionExp     },
  { "log"    , "rc" , CExprFunctionLog     },
  { "log10"  , "rc" , CExprFunctionLog10   },
  { "sin"    , "rc" , CExprFunctionSin     },
  { "cos"    , "rc" , CExprFunctionCos     },
  { "tan"    , "rc" , CExprFunctionTan     },
  { "asin"   , "rc" , CExprFunctionASin    },
  { "acos"   , "rc" , CExprFunctionACos    },
  { "atan"   , "rc" , CExprFunctionATan    },
  { "atan2"  , "r,r", CExprFunctionATan2   },
  { "sinh"   , "rc" , CExprFunctionSinH    },
  { "cosh"   , "rc" , CExprFunctionCosH    },
  { "tanh"   , "rc" , CExprFunctionTanH    },
  { "asinh"  , "rc" , CExprFunctionASinH   },
  { "acosh"  , "rc" , CExprFunctionACosH   },
  { "atanh"  , "rc" , CExprFunctionATanH   },
#ifdef GNUPLOT_EXPR
  // EllipticK, EllipticE, EllipticPi
  { "besj0"  , "r"  , CExprFunctionBesJ0   },
  { "besj1"  , "r"  , CExprFunctionBesJ1   },
  // besy0, besy1
  { "erf"    , "rc" , CExprFunctionErf     },
  { "erfc"   , "rc" , CExprFunctionErfC    },
  { "inverf" , "rc" , CExprFunctionInvErf  },
  { "cerf"   , "c"  , CExprFunctionCErf    },
  { "cerfc"  , "c"  , CExprFunctionCErfC   },
  { "gamma"  , "r"  , CExprFunctionGamma   },
  { "lgamma" , "r"  , CExprFunctionLGamma  },
#endif
  { "norm"   , "r"  , CExprFunctionNorm    },
  { "invnorm", "r"  , CExprFunctionInvNorm },
  { "", "", 0 }
};

//------

CExprFunctionMgr::
CExprFunctionMgr(CExpr *expr) :
 expr_(expr)
{
}

void
CExprFunctionMgr::
addFunctions()
{
  for (uint i = 0; builtinFns[i].proc; ++i) {
    CExprFunctionPtr function =
      addProcFunction(builtinFns[i].name, builtinFns[i].args, builtinFns[i].proc);

    function->setBuiltin(true);
  }

  addObjFunction("abs"  , "ric", new CExprFunctionAbs  )->setBuiltin(true);
  addObjFunction("arg"  , "c"  , new CExprFunctionCArg )->setBuiltin(true);
  addObjFunction("ceil" , "rc" , new CExprFunctionCeil )->setBuiltin(true);
  addObjFunction("floor", "rc" , new CExprFunctionFloor)->setBuiltin(true);

  // TODO: use conversion rules
  addObjFunction("int"  , "ric", new CExprFunctionInt  )->setBuiltin(true);
  addObjFunction("real" , "ric", new CExprFunctionReal )->setBuiltin(true);
  addObjFunction("imag" , "c"  , new CExprFunctionImag )->setBuiltin(true);

  addObjFunction("sgn"  , "ric", new CExprFunctionSign )->setBuiltin(true);

#ifdef GNUPLOT_EXPR
  addObjFunction("rand", "i", new CExprFunctionRand)->setBuiltin(true);

  // input types ..., return type, function
  (new CExprFunctionObjT1<std::string,long,CExprStrLen>
         (this, "strlen" ))->setBuiltin(true);
  (new CExprFunctionObjT2<std::string,std::string,long,CExprStrStrT>
         (this, "strstrt"))->setBuiltin(true);
  (new CExprFunctionObjT3<std::string,long,long,std::string,CExprSubStr>
         (this, "substr" ))->setBuiltin(true);
  (new CExprFunctionObjT1<std::string,std::string,CExprSystem>
         (this, "system" ))->setBuiltin(true);
  (new CExprFunctionObjT2<std::string,long,std::string,CExprWord>
         (this, "word"   ))->setBuiltin(true);
  (new CExprFunctionObjT1<std::string,long,CExprWords>
         (this, "words"  ))->setBuiltin(true);

  // gprintf ?
  addObjFunction("sprintf", "s,...", new CExprFunctionSPrintF)->setBuiltin(true);
#endif

  addObjFunction("expr", "s", new CExprFunctionExpr)->setBuiltin(true);
}

CExprFunctionPtr
CExprFunctionMgr::
getFunction(const std::string &name)
{
  for (const auto &func : functions_)
    if (func->name() == name)
      return func;

  return CExprFunctionPtr();
}

void
CExprFunctionMgr::
getFunctions(const std::string &name, Functions &functions)
{
  for (const auto &func : functions_)
    if (func->name() == name)
      functions.push_back(func);
}

CExprFunctionPtr
CExprFunctionMgr::
addProcFunction(const std::string &name, const std::string &argsStr, CExprFunctionProc proc)
{
  Args args;
  bool variableArgs;

  (void) parseArgs(argsStr, args, variableArgs);

  CExprFunctionPtr function(new CExprProcFunction(name, args, proc));

  function->setVariableArgs(variableArgs);

  removeFunction(name);

  functions_.push_back(function);

  resetCompiled(name);

  return function;
}

CExprFunctionPtr
CExprFunctionMgr::
addObjFunction(const std::string &name, const std::string &argsStr,
               CExprFunctionObj *proc, bool resetCompiled)
{
  Args args;
  bool variableArgs;

  (void) parseArgs(argsStr, args, variableArgs);

  CExprFunctionPtr function(new CExprObjFunction(name, args, proc));

  function->setVariableArgs(variableArgs);

  if (! proc->isOverload())
    removeFunction(name);

  functions_.push_back(function);

  if (resetCompiled)
    this->resetCompiled(name);

  return function;
}

CExprFunctionPtr
CExprFunctionMgr::
addUserFunction(const std::string &name, const std::vector<std::string> &args,
                const std::string &proc)
{
  CExprFunctionPtr function(new CExprUserFunction(name, args, proc));

  removeFunction(name);

  functions_.push_back(function);

  resetCompiled(name);

  return function;
}

void
CExprFunctionMgr::
removeFunction(const std::string &name)
{
  removeFunction(getFunction(name));
}

void
CExprFunctionMgr::
removeFunction(CExprFunctionPtr function)
{
  if (function.isValid())
    functions_.remove(function);
}

void
CExprFunctionMgr::
getFunctionNames(std::vector<std::string> &names) const
{
  for (const auto &func : functions_)
    names.push_back(func->name());
}

void
CExprFunctionMgr::
resetCompiled(const std::string &name)
{
  for (const auto &func : functions_) {
    if (func->hasFunction(name))
      func->reset();
  }
}

bool
CExprFunctionMgr::
parseArgs(const std::string &argsStr, Args &args, bool &variableArgs)
{
  variableArgs = false;

  bool rc = true;

  std::vector<std::string> args1;

  CStrUtil::addTokens(argsStr, args1, ", ");

  uint num_args = args1.size();

  for (uint i = 0; i < num_args; ++i) {
    const std::string &arg = args1[i];

    if (arg == "..." && i == num_args - 1) {
      variableArgs = true;
      break;
    }

    uint types = uint(CExprValueType::NONE);

    uint len = arg.size();

    for (uint j = 0; j < len; j++) {
      char c = arg[j];

      if      (c == 'b') types |= uint(CExprValueType::BOOLEAN);
      else if (c == 'i') types |= uint(CExprValueType::INTEGER);
      else if (c == 'r') types |= uint(CExprValueType::REAL);
      else if (c == 's') types |= uint(CExprValueType::STRING);
      else if (c == 'c') types |= uint(CExprValueType::COMPLEX);
      else if (c == 'n') types |= uint(CExprValueType::NUL);
      else {
        CExpr::instance()->
          errorMsg("Invalid argument type char '" + std::string(&c, 1) + "'");
        rc = false;
      }
    }

    args.push_back(CExprFunctionArg((CExprValueType) types));
  }

  return rc;
}

//----------

CExprProcFunction::
CExprProcFunction(const std::string &name, const Args &args, CExprFunctionProc proc) :
 CExprFunction(name), args_(args), proc_(proc)
{
}

bool
CExprProcFunction::
checkValues(const CExprValueArray &values) const
{
  return (values.size() == numArgs());
}

CExprValuePtr
CExprProcFunction::
exec(CExpr *expr, const CExprValueArray &values)
{
  assert(checkValues(values));

  return (*proc_)(expr, values);
}

//----------

CExprObjFunction::
CExprObjFunction(const std::string &name, const Args &args, CExprFunctionObj *proc) :
 CExprFunction(name), args_(args), proc_(proc)
{
}

CExprObjFunction::
~CExprObjFunction()
{
  delete proc_;
}

bool
CExprObjFunction::
checkValues(const CExprValueArray &values) const
{
  if (isVariableArgs())
    return (values.size() >= numArgs());
  else
    return (values.size() == numArgs());
}

CExprValuePtr
CExprObjFunction::
exec(CExpr *expr, const CExprValueArray &values)
{
  assert(checkValues(values));

  return (*proc_)(expr, values);
}

//----------

CExprUserFunction::
CExprUserFunction(const std::string &name, const Args &args, const std::string &proc) :
 CExprFunction(name), args_(args), proc_(proc), compiled_(false)
{
}

bool
CExprUserFunction::
checkValues(const CExprValueArray &values) const
{
  return (values.size() >= numArgs());
}

void
CExprUserFunction::
reset()
{
  compiled_ = false;

  pstack_.clear();
  cstack_.clear();

  itoken_ = CExprITokenPtr();
}

CExprValuePtr
CExprUserFunction::
exec(CExpr *expr, const CExprValueArray &values)
{
  assert(checkValues(values));

  //---

  if (! compiled_) {
    pstack_ = expr->parseLine(proc_);
    itoken_ = expr->interpPTokenStack(pstack_);
    cstack_ = expr->compileIToken(itoken_);

    compiled_ = true;
  }

  //---

  typedef std::map<std::string,CExprValuePtr> VarValues;

  VarValues varValues;

  // set arg values (save previous values)
  for (uint i = 0; i < numArgs(); ++i) {
    const std::string &arg = args_[i];

    CExprVariablePtr var = expr->getVariable(arg);

    if (var.isValid()) {
      varValues[arg] = var->getValue();

      var->setValue(values[i]);
    }
    else {
      varValues[arg] = CExprValuePtr();

      expr->createVariable(arg, values[i]);
    }
  }

  // run proc
  expr->saveCompileState();

  CExprValuePtr value;

//if (! expr->evaluateExpression(proc_, value))
//  value = CExprValuePtr();
//if (! expr->executePTokenStack(pstack_, value))
//  value = CExprValuePtr();
  if (! expr->executeCTokenStack(cstack_, value))
    value = CExprValuePtr();

  expr->restoreCompileState();

  // restore variables
  for (const auto &v : varValues) {
    const std::string varName = v.first;
    CExprValuePtr     value   = v.second;

    if (value.isValid()) {
      CExprVariablePtr var = expr->getVariable(varName);

      var->setValue(value);
    }
    else
      expr->removeVariable(varName);
  }

  return value;
}


/*=============================================================================
    Copyright (c) 2001-2010 Joel de Guzman

    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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A Roman Numerals Parser (demonstrating the symbol table). This is
//  discussed in the "Symbols" chapter in the Spirit User's Guide.
//
//  [ JDG August 22, 2002 ] spirit1
//  [ JDG March 13, 2007 ]  spirit2
//
///////////////////////////////////////////////////////////////////////////////

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/qi.hpp>

#include <iostream>
#include <string>

namespace client {
namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;

///////////////////////////////////////////////////////////////////////////////
//  Parse roman hundreds (100..900) numerals using the symbol table.
//  Notice that the data associated with each slot is the parser's attribute
//  (which is passed to attached semantic actions).
///////////////////////////////////////////////////////////////////////////////
//[tutorial_roman_hundreds
struct hundreds_ : qi::symbols<char, unsigned> {
  hundreds_() {
    add("C", 100)("CC", 200)(
        "CCC", 300)("CD", 400)("D", 500)("DC", 600)("DCC", 700)("DCCC",
                                                                800)("CM", 900);
  }

} hundreds;
//]

///////////////////////////////////////////////////////////////////////////////
//  Parse roman tens (10..90) numerals using the symbol table.
///////////////////////////////////////////////////////////////////////////////
//[tutorial_roman_tens
struct tens_ : qi::symbols<char, unsigned> {
  tens_() {
    add("X", 10)("XX", 20)(
        "XXX", 30)("XL", 40)("L", 50)("LX", 60)("LXX", 70)("LXXX", 80)("XC",
                                                                       90);
  }

} tens;
//]

///////////////////////////////////////////////////////////////////////////////
//  Parse roman ones (1..9) numerals using the symbol table.
///////////////////////////////////////////////////////////////////////////////
//[tutorial_roman_ones
struct ones_ : qi::symbols<char, unsigned> {
  ones_() {
    add("I", 1)("II", 2)(
        "III", 3)("IV", 4)("V", 5)("VI", 6)("VII", 7)("VIII", 8)("IX", 9);
  }

} ones;
//]

///////////////////////////////////////////////////////////////////////////////
//  roman (numerals) grammar
//
//      Note the use of the || operator. The expression
//      a || b reads match a or b and in sequence. Try
//      defining the roman numerals grammar in YACC or
//      PCCTS. Spirit rules! :-)
///////////////////////////////////////////////////////////////////////////////
//[tutorial_roman_grammar
template <typename Iterator> struct roman : qi::grammar<Iterator, unsigned()> {
  roman() : roman::base_type(start) {
    using ascii::char_;
    using qi::_1;
    using qi::_val;
    using qi::eps;
    using qi::lit;

    start = eps[_val = 0] >> (+lit('M')[_val += 1000] || hundreds[_val += _1] ||
                              tens[_val += _1] || ones[_val += _1]);
  }

  qi::rule<Iterator, unsigned()> start;
};
//]
} // namespace client

///////////////////////////////////////////////////////////////////////////////
//  Main program
///////////////////////////////////////////////////////////////////////////////
int main() {
  using namespace std;
  cout << "/////////////////////////////////////////////////////////\n\n";
  cout << "\t\tRoman Numerals Parser\n\n";
  cout << "/////////////////////////////////////////////////////////\n\n";
  cout << "Type a Roman Numeral ...or [q or Q] to quit\n\n";

  typedef string::const_iterator iterator_type;
  typedef client::roman<iterator_type> roman;

  roman roman_parser; // Our grammar

  string str;
  unsigned result;
  while (getline(cin, str)) {
    if (str.empty() || str[0] == 'q' || str[0] == 'Q')
      break;

    string::const_iterator iter = str.begin();
    string::const_iterator end = str.end();
    //[tutorial_roman_grammar_parse
    bool r = parse(iter, end, roman_parser, result);

    if (r && iter == end) {
      cout << "-------------------------\n";
      cout << "Parsing succeeded\n";
      cout << "result = " << result << endl;
      cout << "-------------------------\n";
    } else {
      string rest(iter, end);
      cout << "-------------------------\n";
      cout << "Parsing failed\n";
      cout << "stopped at: \": " << rest << "\"\n";
      cout << "-------------------------\n";
    }
    //]
  }
  cout << "Bye... :-) \n\n";
  return 0;
}