#pragma once

#include <map>
#include <stack>
#include <boost/circular_buffer.hpp>
#include <giecs/ll/arithmetic.hpp>
#include <giecs/ll/io.hpp>

#include <giecs/core.hpp>
#include <giecs/eval.hpp>

namespace forth
{

/**
 * Implements the complete Virtual-Forth-Machine
 */
template <typename MemWord, typename MemContainer, typename DataStackContainer, typename ReturnStackContainer>
class VM : public std::stack<MemWord, DataStackContainer>
{
    public:
        MemWord pc; /// Instruction pointer
        MemContainer mem;
        std::stack<MemWord, ReturnStackContainer> return_stack; /// return stack
        std::map<MemWord, giecs::ProgramBase*> programs; /// external programs

        struct Instruction
        {
            enum Opcode
            {
                compose, branch, exit, // flow
                push, drop, dup, over, swap, pushr, popr, // stack
                load, store, // memory
                noti, andi, ori, xori, // bitwise logic
                addi, subi, muli, divi, // integer arithmetic
                gti, lti, eq, // integer relation
                printi, emit, // out
            };

            using Data = VM<MemWord, MemContainer, DataStackContainer, ReturnStackContainer>;

            Opcode op;
            MemWord pc;

            Opcode fetch(Data& data)
            {
                data.pc = this->pc;
                return op;
            }
        }; // struct Instruction

        giecs::ProgramBase* get_program(MemWord addr)
        {
            auto it = this->programs.find(addr);
            if(it == this->programs.end())
                return nullptr;
            else
                return it->second;
        }

        MemWord pop()
        {
            MemWord a = this->top();
            this->std::stack<MemWord, DataStackContainer>::pop();
            return a;
        }

        /**
         * Executes Opcodes
         */
#define FN(def) ([](VM<MemWord, MemContainer, DataStackContainer, ReturnStackContainer>& state){ def ;})
        GIECS_CORE_OPERATOR(Operator,
                            ((Instruction::Opcode::compose, FN(state.return_stack.push(state.pc); state.pc = state.mem[state.pc];)))
                            ((Instruction::Opcode::branch, FN(MemWord off = state.mem[++state.pc]-1; if(state.pop() == 0) state.pc += off;)))
                            ((Instruction::Opcode::exit, FN(state.pc = state.return_stack.top(); state.return_stack.pop();)))

                            ((Instruction::Opcode::load, FN(MemWord addr=state.pop(); state.push(state.mem[addr]);)))
                            ((Instruction::Opcode::store, FN(MemWord addr=state.pop(); state.mem[addr] = state.pop();)))
                            ((Instruction::Opcode::push, FN(state.push(state.mem[++state.pc]))))
                            ((Instruction::Opcode::drop, FN(state.pop())))
                            ((Instruction::Opcode::dup, FN(state.push(state.top()))))
                            ((Instruction::Opcode::over, FN(MemWord a = state.pop(); MemWord b = state.top(); state.push(a); state.push(b);)))
                            ((Instruction::Opcode::swap, FN(MemWord a = state.pop(); MemWord b = state.pop(); state.push(a); state.push(b);)))
                            ((Instruction::Opcode::pushr, FN(state.return_stack.push(state.pop()))))
                            ((Instruction::Opcode::popr, FN(state.push(state.return_stack.top()); state.return_stack.pop();)))

                            ((Instruction::Opcode::noti, giecs::ll::Bitwise<int>::op_not))
                            ((Instruction::Opcode::andi, giecs::ll::Bitwise<int>::op_and))
                            ((Instruction::Opcode::ori, giecs::ll::Bitwise<int>::op_or))
                            ((Instruction::Opcode::xori, giecs::ll::Bitwise<int>::op_xor))
                            ((Instruction::Opcode::addi, giecs::ll::Arithmetic<int>::add))
                            ((Instruction::Opcode::subi, giecs::ll::Arithmetic<int>::sub))
                            ((Instruction::Opcode::muli, giecs::ll::Arithmetic<int>::mul))
                            ((Instruction::Opcode::divi, giecs::ll::Arithmetic<int>::div))
                            ((Instruction::Opcode::gti, giecs::ll::Relation<int>::gt))
                            ((Instruction::Opcode::lti, giecs::ll::Relation<int>::lt))
                            ((Instruction::Opcode::eq, giecs::ll::Relation<int>::eq))

                            ((Instruction::Opcode::emit, giecs::ll::ConsoleIO<char>::print))
                            ((Instruction::Opcode::printi, giecs::ll::ConsoleIO<int>::print))
                           ); // Operator

        /**
         * Evaluated by giecs::eval
         */
        class Program : public giecs::Program<giecs::Core<Instruction, Operator>, Program>
        {
            private:
                class InstructionDecoder : public boost::circular_buffer<Instruction>
                {
                    private:
                        VM& state;

                    public:
                        InstructionDecoder(VM& s)
                            : boost::circular_buffer<Instruction>(16), state(s)
                        {
                        }

                        Instruction& front(void)
                        {
                            if(this->boost::circular_buffer<Instruction>::empty())
                            {
                                MemWord pc = state.pc;
                                for(Instruction inst; inst.op != Instruction::Opcode::compose && inst.op != Instruction::Opcode::exit && inst.op != Instruction::Opcode::branch && !this->full();)
                                {
                                    ++pc;
                                    inst.op = (typename Instruction::Opcode) this->state.mem[this->state.mem[pc]];
                                    inst.pc = pc;
                                    // We know which instructions take more
                                    if(inst.op == Instruction::Opcode::push)
                                        ++pc;
                                    this->push_back(inst);
                                }
                            }
                            return this->boost::circular_buffer<Instruction>::front();
                        }

                        bool empty(void) const
                        {
                            return (this->state.programs.count(this->state.pc) > 0) || this->state.return_stack.empty();
                        }
                }; // class InstructionDecoder

                VM& state;
                std::queue<Instruction, InstructionDecoder> queue; // could be multiple objects for multithreaded operation

            public:
                Program(VM& vm_, giecs::ProgramBase* ret=nullptr)
                    : state(vm_), queue(InstructionDecoder(vm_))
                {
                }

                std::queue<Instruction, InstructionDecoder>& program(void)
                {
                    return this->queue;
                }

                VM& data(void)
                {
                    return this->state;
                }

                giecs::ProgramBase* next(void)
                {
                    return this->state.get_program(this->state.pc);
                }
        }; // class Word
}; // class VM

} // namespace forth


#include <float.h>
#include <cmath>
#include "get_floor_Hf.h"
#include "normalize2dpts.h"
#include <Eigen/Geometry>

using namespace Eigen;

PoseData get_floor_Hf(MatrixXd &p1, MatrixXd &p2, Matrix3d &R1, Matrix3d &R2, double f1)
{
    int nbr_coeffs = 21;
    int nbr_unknowns = 6;

    // Save copies of the inverse rotation
    Matrix3d R1T = R1.transpose();
    Matrix3d R2T = R2.transpose();

    // Compute normalization matrix
    double scale = normalize2dpts(p2);
    Vector3d s;
    s << scale, scale, 1.0;
    DiagonalMatrix<double, 3> S = s.asDiagonal();

    // Initialize known calibration matrix
    Vector3d k1inv;
    k1inv << 1 / f1, 1 / f1, 1.0;
    DiagonalMatrix<double, 3> K1inv = k1inv.asDiagonal();

    // Normalize data
    Matrix3d y1;
    Matrix3d x2;
    y1 = R1T * K1inv * p1.colwise().homogeneous();
    x2 = p2.colwise().homogeneous();

    x2 = S * x2;

    MatrixXd y1t(2,3);
    y1t << y1.colwise().hnormalized();
    MatrixXd x2t(2,3);
    x2t << x2.colwise().hnormalized();

    // Wrap input data to expected format
    VectorXd input(nbr_coeffs);
    input << y1t.col(0),
             x2t.col(0),
             y1t.col(1),
             x2t.col(1),
             y1t.col(2),
             x2t.col(2),
             Map<VectorXd>(R2T.data(), 9);

    // TODO: Not sure if this is necessary (assure const)
    const Map<VectorXd> input_data(input.data(), nbr_coeffs);

    // Extract solution
    MatrixXcd sols = solver_floor_Hf(input_data);

    // Pre-processing: Remove complex-valued solutions
    double thresh = 1e-5;
    ArrayXd real_sols(7);
    real_sols = sols.imag().cwiseAbs().colwise().sum();
    int nbr_real_sols = (real_sols <= thresh).count();

    // Allocate space for putative (real) homographies
    MatrixXd best_homography(3, 3);
    double best_focal_length;
    double best_algebraic_error = DBL_MAX;
    double algebraic_error;

    // Since this is a 2.5 pt solver, use the last
    // (previously unused) constraint, to discard
    // false solutions.
    ArrayXd xx(nbr_unknowns);
    VectorXd input_algebraic(nbr_coeffs + nbr_unknowns);

    for (int i = 0; i < real_sols.size(); i++) {
        if (real_sols(i) <= thresh) {
            // Compute algebraic error, and compare to other solutions.
            xx = sols.col(i).real();
            input_algebraic << xx, input;
            algebraic_error = get_algebraic_error_floor_Hf(input_algebraic);

            if (algebraic_error < best_algebraic_error) {
                best_algebraic_error = algebraic_error;
                best_homography << xx[0], xx[2], xx[1],
                                       0, xx[3],     0,
                                  -xx[1], xx[4], xx[0];
                best_focal_length = xx[5];
            }
        }
    }

    // Construct homography
    Matrix3d K, H;
    K = Vector3d(best_focal_length, best_focal_length, 1).asDiagonal();
    // Ki = Vector3d(1, 1, best_focal_length).asDiagonal();
    H = S.inverse() * K * R2 * best_homography * R1.transpose() * K1inv;

    // Package output
    PoseData posedata;
    posedata.homography = H;
    posedata.focal_length = best_focal_length / scale;

    return posedata;
}

// ---------------- //
// MATLAB interface //
// ---------------- //
#ifdef MATLAB_MEX_FILE
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (nrhs != 5) {
		mexErrMsgIdAndTxt("get_floor_Hf:nrhs", "Five input arguments are required.");
	}
	if (nlhs != 2) {
		mexErrMsgIdAndTxt("get_floor_Hf:nlhs", "Two output arguments are required.");
	}
	if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0])) {
		mexErrMsgIdAndTxt("get_floor_Hf:notDouble", "Input data must be type double.");
	}
	if(mxGetNumberOfElements(prhs[0]) != 6 && mxGetNumberOfElements(prhs[1]) != 6 && mxGetNumberOfElements(prhs[2]) != 9 && mxGetNumberOfElements(prhs[3]) != 9 && mxGetNumberOfElements(prhs[4]) != 1) {
		mexErrMsgIdAndTxt("get_floor_Hf:incorrectSize", "Input dimensions incorrect.");
	}
    // Convert to expected input
    VectorXd x1_tmp = Map<VectorXd>(mxGetPr(prhs[0]), 6);
    VectorXd x2_tmp = Map<VectorXd>(mxGetPr(prhs[1]), 6);
    MatrixXd x1 = Map<MatrixXd>(x1_tmp.data(), 2, 3);
    MatrixXd x2 = Map<MatrixXd>(x2_tmp.data(), 2, 3);

    VectorXd R1_tmp = Map<VectorXd>(mxGetPr(prhs[2]), 9);
    VectorXd R2_tmp = Map<VectorXd>(mxGetPr(prhs[3]), 9);
    Matrix3d R1 = Map<Matrix3d>(R1_tmp.data(), 3, 3);
    Matrix3d R2 = Map<Matrix3d>(R2_tmp.data(), 3, 3);

    double *f1 = mxGetPr(prhs[4]);

    // Compute output
	PoseData posedata = get_floor_Hf(x1, x2, R1, R2, f1[0]);

    // Wrap it up to Matlab compatible output
	plhs[0] = mxCreateDoubleMatrix(3, 3, mxREAL);
	double* zr = mxGetPr(plhs[0]);
    for (Index i = 0; i < posedata.homography.size(); i++) {
        zr[i] = posedata.homography(i);
    }

    plhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);
    zr = mxGetPr(plhs[1]);
    zr[0] = posedata.focal_length;
}
#endif

// Function that utilizes the last equation of the DLT system to discard false solutions
double get_algebraic_error_floor_Hf(VectorXd &data)
{
    const double* d = data.data();

    // Compute algebraic error
    double error;
    error = -d[0]*d[5]*d[14]*d[25] - d[0]*d[14]*d[16]*d[19] - d[0]*d[14]*d[17]*d[22] - d[1]*d[5]*d[25] - d[1]*d[16]*d[19] - d[1]*d[17]*d[22] - d[2]*d[5]*d[15]*d[25] - d[2]*d[15]*d[16]*d[19] - d[2]*d[15]*d[17]*d[22] + d[3]*d[5]*d[15]*d[24] + d[3]*d[15]*d[16]*d[18] + d[3]*d[15]*d[17]*d[21];

    return abs(error);
}


#include <sequential-line-search/preferenceregressor.h>
#include <sequential-line-search/utils.h>
#include <iostream>
#include <fstream>
#include <cmath>
#include <Eigen/LU>
#include <nlopt-util.hpp>

using Eigen::MatrixXd;
using Eigen::VectorXd;

//#define VERBOSE
//#define NOISELESS

#ifdef VERBOSE
#include <timer.hpp>
#endif

namespace
{
    using namespace sequential_line_search;

#ifdef NOISELESS
    const double b_fixed = 1e-06;
#endif

    inline double calc_grad_a(const VectorXd& y, const MatrixXd& C_inv, const MatrixXd& X, double a, double b, const VectorXd& r)
    {
        const double   a_prior  = PreferenceRegressor::Params::getInstance().a;
        const double   variance = PreferenceRegressor::Params::getInstance().variance;

        const MatrixXd C_grad_a             = Regressor::calc_C_grad_a(X, a, b, r);
        const double   log_p_f_theta_grad_a = 0.5 * y.transpose() * C_inv * C_grad_a * C_inv * y - 0.5 * (C_inv * C_grad_a).trace();
        const double   log_prior            = (std::log(a_prior) - variance - std::log(a)) / (variance * a);
        return log_p_f_theta_grad_a + log_prior;
    }

#ifndef NOISELESS
    inline double calc_grad_b(const VectorXd& y, const MatrixXd& C_inv, const MatrixXd& X, double a, double b, const VectorXd& r)
    {
        const double   b_prior  = PreferenceRegressor::Params::getInstance().b;
        const double   variance = PreferenceRegressor::Params::getInstance().variance;

        const MatrixXd C_grad_b             = Regressor::calc_C_grad_b(X, a, b, r);
        const double   log_p_f_theta_grad_b = 0.5 * y.transpose() * C_inv * C_grad_b * C_inv * y - 0.5 * (C_inv * C_grad_b).trace();
        const double   log_prior            = (std::log(b_prior) - variance - std::log(b)) / (variance * b);
        return log_p_f_theta_grad_b + log_prior;
    }
#endif

    inline VectorXd calc_grad_r(const VectorXd& y, const MatrixXd& C_inv, const MatrixXd& X, double a, double b, const VectorXd& r)
    {
        const double   r_prior  = PreferenceRegressor::Params::getInstance().r;
        const double   variance = PreferenceRegressor::Params::getInstance().variance;

        VectorXd grad = VectorXd::Zero(r.rows());
        for (unsigned i = 0; i < r.rows(); ++ i)
        {
            const MatrixXd C_grad_r               = Regressor::calc_C_grad_r_i(X, a, b, r, i);
            const double   log_p_f_theta_grad_r_i = 0.5 * y.transpose() * C_inv * C_grad_r * C_inv * y - 0.5 * (C_inv * C_grad_r).trace();
            grad(i) += log_p_f_theta_grad_r_i;
        }
        for (unsigned i = 0; i < r.rows(); ++ i)
        {
            const double log_prior = (std::log(r_prior) - variance - std::log(r(i))) / (variance * r(i));
            grad(i) += log_prior;
        }

        return grad;
    }

    // log p(d_k | f)
    inline double calc_log_likelihood(const Preference& p, const double w, const VectorXd& y)
    {
        const double btl_scale = w * PreferenceRegressor::Params::getInstance().btl_scale;

        VectorXd tmp(p.size()); for (unsigned i = 0; i < p.size(); ++ i) tmp(i) = y(p[i]);
        return std::log(utils::BTL(tmp, btl_scale));
    }

    // Log likelihood that will be maximized
    double objective(const std::vector<double> &x, std::vector<double>& grad, void* data)
    {
        const PreferenceRegressor* regressor = static_cast<PreferenceRegressor*>(data);

        const MatrixXd&                X = regressor->X;
        const std::vector<Preference>& D = regressor->D;
        const VectorXd&                w = regressor->w;
        const unsigned                 M = X.cols();
        const VectorXd                 y = Eigen::Map<const VectorXd>(&x[0], M);

        const double   a = (regressor->use_MAP_hyperparameters) ? x[M + 0] : PreferenceRegressor::Params::getInstance().a;
#ifdef NOISELESS
        const double   b = b_fixed;
#else
        const double   b = (regressor->use_MAP_hyperparameters) ? x[M + 1] : PreferenceRegressor::Params::getInstance().b;
#endif
        const VectorXd r = (regressor->use_MAP_hyperparameters) ? VectorXd(Eigen::Map<const VectorXd>(&x[M + 2], X.rows())) : VectorXd::Constant(X.rows(), PreferenceRegressor::Params::getInstance().r);

        double obj = 0.0;

        // Log likelihood of data
        for (unsigned i = 0; i < D.size(); ++ i)
        {
            obj += calc_log_likelihood(D[i], w(i), y);
        }

        // Log likelihood of y distribution
        const MatrixXd C     = Regressor::calc_C(X, a, b, r);
        const MatrixXd C_inv = C.inverse();
        const double   C_det = C.determinant();
        const double   term1 = - 0.5 * y.transpose() * C_inv * y;
        const double   term2 = - 0.5 * std::log(C_det);
        const double   term3 = - 0.5 * M * std::log(2.0 * M_PI);
        obj += term1 + term2 + term3;

        if (regressor->use_MAP_hyperparameters)
        {
            // Priors for GP parameters
            const double   a_prior  = PreferenceRegressor::Params::getInstance().a;
#ifndef NOISELESS
            const double   b_prior  = PreferenceRegressor::Params::getInstance().b;
#endif
            const double   r_prior  = PreferenceRegressor::Params::getInstance().r;
            const double   variance = PreferenceRegressor::Params::getInstance().variance;

            obj += std::log(utils::log_normal(a, std::log(a_prior), variance));
#ifndef NOISELESS
            obj += std::log(utils::log_normal(b, std::log(b_prior), variance));
#endif
            for (unsigned i = 0; i < r.rows(); ++ i)
            {
                obj += std::log(utils::log_normal(r(i), std::log(r_prior), variance));
            }
        }

        // When the algorithm is gradient-based, compute the gradient vector
        if (grad.size() == x.size())
        {
            VectorXd grad_y = VectorXd::Zero(y.rows());

            // Accumulate per-data derivatives
            const double btl_scale = PreferenceRegressor::Params::getInstance().btl_scale;
            for (unsigned i = 0; i < D.size(); ++ i)
            {
                const Preference& p = D[i];
                const double      s = btl_scale * w(i);
                VectorXd tmp1(p.size()); for (unsigned i = 0; i < p.size(); ++ i) tmp1(i) = y(p[i]);
                const VectorXd tmp2 = utils::derivative_BTL(tmp1, s) / utils::BTL(tmp1, s);
                for (unsigned i = 0; i < p.size(); ++ i) grad_y(p[i]) += tmp2(i);
            }

            // Add GP term
            grad_y += - C_inv * y;

            Eigen::Map<VectorXd>(&grad[0], grad_y.rows()) = grad_y;

            if (regressor->use_MAP_hyperparameters)
            {
                grad[M + 0] = calc_grad_a(y, C_inv, X, a, b, r);
#ifdef NOISELESS
                grad[M + 1] = 0.0;
#else
                grad[M + 1] = calc_grad_b(y, C_inv, X, a, b, r);
#endif
                VectorXd grad_r = calc_grad_r(y, C_inv, X, a, b, r);
                for (unsigned i = 0; i < grad_r.rows(); ++ i)
                {
                    grad[M + 2 + i] = grad_r(i);
                }
            }
            else
            {
                grad[M + 0] = 0.0;
                grad[M + 1] = 0.0;
                grad[M + 2] = 0.0;
            }
        }

        return obj;
    }

}

namespace sequential_line_search
{
    PreferenceRegressor::PreferenceRegressor(const MatrixXd &X, const std::vector<Preference>& D, bool use_MAP_hyperparameters) :
    use_MAP_hyperparameters(use_MAP_hyperparameters),
    X(X),
    D(D)
    {
        if (X.cols() == 0 || D.size() == 0) return;

        w = Eigen::VectorXd::Ones(D.size());

        compute_MAP();

        C     = calc_C(X, a, b, r);
        C_inv = C.inverse();
    }

    PreferenceRegressor::PreferenceRegressor(const MatrixXd &X, const std::vector<Preference>& D, const Eigen::VectorXd &w, bool use_MAP_hyperparameters) :
    use_MAP_hyperparameters(use_MAP_hyperparameters),
    X(X),
    D(D),
    w(w)
    {
        if (X.cols() == 0 || D.size() == 0) return;

        compute_MAP();

        C     = calc_C(X, a, b, r);
        C_inv = C.inverse();
    }

    PreferenceRegressor::PreferenceRegressor(const MatrixXd &X, const std::vector<Preference>& D, const Eigen::VectorXd &w, bool use_MAP_hyperparameters, const PreferenceRegressor* previous) :
    use_MAP_hyperparameters(use_MAP_hyperparameters),
    X(X),
    D(D),
    w(w)
    {
        if (X.cols() == 0 || D.size() == 0) return;

        compute_MAP(previous);

        C     = calc_C(X, a, b, r);
        C_inv = C.inverse();
    }

    double PreferenceRegressor::estimate_y(const VectorXd &x) const
    {
        const VectorXd k = Regressor::calc_k(x, X, a, b, r);
        return k.transpose() * C_inv * y;
    }

    double PreferenceRegressor::estimate_s(const VectorXd &x) const
    {
        const VectorXd k = Regressor::calc_k(x, X, a, b, r);
        return std::sqrt(a + b - k.transpose() * C_inv * k);
    }

    void PreferenceRegressor::compute_MAP(const PreferenceRegressor *previous)
    {
        const unsigned M = X.cols();
        const unsigned d = X.rows();

        VectorXd upper = VectorXd::Constant(M + 2 + d, + 1e+01);
        VectorXd lower = VectorXd::Constant(M + 2 + d, - 1e+01); lower.block(M, 0, 2 + d, 1) = VectorXd::Constant(2 + d, 1e-05);
        VectorXd x_ini = VectorXd::Constant(M + 2 + d, 0.0);
        x_ini(M + 0) = Params::getInstance().a;
        x_ini(M + 1) = Params::getInstance().b;
        x_ini.block(M + 2, 0, d, 1) = VectorXd::Constant(d, Params::getInstance().r);

        // Use the MAP estimated values in previous regression as initial values
        if (previous != nullptr)
        {
            for (unsigned i = 0; i < M; ++ i)
            {
                x_ini(i) = previous->estimate_y(X.col(i));
            }
            x_ini(M + 0) = previous->a;
            x_ini(M + 1) = previous->b;
            x_ini.block(M + 2, 0, d, 1) = previous->r;
        }

#ifdef VERBOSE
        timer::Timer t("PreferenceRegressor::compute_MAP");
#endif

        const VectorXd x_opt = nloptutil::solve(x_ini, upper, lower, objective, nlopt::LD_TNEWTON, this, 500);

        y = x_opt.block(0, 0, M, 1);

        if (use_MAP_hyperparameters)
        {
            a = x_opt(M + 0);
#ifdef NOISELESS
            b = b_fixed;
#else
            b = x_opt(M + 1);
#endif
            r = x_opt.block(M + 2, 0, d, 1);

#ifdef VERBOSE
            std::cout << "Learned hyperparameters ... a: " << a << ", \tb: " << b << ", \tr: " << r.transpose() << std::endl;
#endif
        }
        else
        {
            a = PreferenceRegressor::Params::getInstance().a;
            b = PreferenceRegressor::Params::getInstance().b;
            r = VectorXd::Constant(d, PreferenceRegressor::Params::getInstance().r);
        }
    }

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

#if 0
    namespace
    {

        double objective_function(const std::vector<double> &x, std::vector<double>& grad, void* data)
        {
            const PreferenceRegressor* regressor = static_cast<PreferenceRegressor*>(data);

            const unsigned M = x.size();

            if (!grad.empty())
            {
                const VectorXd  x_vec = Eigen::Map<const VectorXd>(&x[0], M);
                const MatrixXd& X     = regressor->X;
                const MatrixXd& C_inv = regressor->C_inv;
                const VectorXd& y     = regressor->y;
                const unsigned  N     = y.rows();
                const double    a     = regressor->a;
                const double    l     = regressor->l;

                MatrixXd k_derivative(M, N);

                for (unsigned i = 0; i < N; ++ i)
                {
                    const double   tmp            = (- 0.5 / (l * l)) * (x_vec - X.col(i)).squaredNorm();
                    const VectorXd k_i_derivative = a * std::exp(tmp) * (- 1.0 / (l * l) * (x_vec - X.col(i)));

                    k_derivative.col(i) = k_i_derivative;
                }

                const MatrixXd grad_vec = k_derivative * (C_inv * y);
                Eigen::Map<VectorXd>(&grad[0], M) = grad_vec;
            }

            return regressor->estimate_y(Eigen::Map<const VectorXd>(&x[0], M));
        }

    }
#endif

    VectorXd PreferenceRegressor::find_arg_max()
    {
        const unsigned M = X.rows();

        assert (M != 0);

        int i; y.maxCoeff(&i);
#if 0
        const VectorXd x_initial = X.col(i);
        const VectorXd upper     = VectorXd::Constant(M, 1.0);
        const VectorXd lower     = VectorXd::Constant(M, 0.0);

        return nloptutils::compute(x_initial, upper, lower, objective_function, static_cast<void*>(this), nlopt::LD_TNEWTON, 100);
#else
        return X.col(i);
#endif
    }

    void PreferenceRegressor::dampData(const std::string &dirPath) const
    {
        // Export X using CSV
        utils::exportMatrixToCsv(dirPath + "/X.csv", X);

        // Export D using CSV
        std::ofstream ofs_D(dirPath + "/D.csv");
        for (unsigned i = 0 ; i < D.size(); ++ i)
        {
            for (unsigned j = 0; j < D[i].size(); ++ j)
            {
                ofs_D << D[i][j];
                if (j + 1 != D[i].size()) ofs_D << ",";
            }
            ofs_D << std::endl;
        }
    }
}


/*
   Copyright (C) 2016-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   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.
*/

// Calculating free energy density of quantum antiferomagnetic Heisenberg chain

#pragma once

#include <Eigen/Dense>
#include "standards/exp_number.hpp"
#include "chain.hpp"

namespace afh { namespace free_energy { namespace chain {

class finite {
  typedef std::size_t uint_t;
  typedef Eigen::VectorXd vector_t;
  typedef Eigen::MatrixXd matrix_t;
public:
  finite(uint_t L) : L_(L), eigenvalues_(1 << L_) {
    // lattice
    std::vector<std::pair<uint_t, uint_t> > lattice;
    for (uint_t i = 0; i < L_; ++i) lattice.push_back(std::make_pair(i, (i+1) % L_));

    // generate Hamiltonian
    uint_t dim = 1 << L_;
    matrix_t hamiltonian(dim, dim);
    afh::free_energy::chain::generate(L_, lattice, hamiltonian);

    /* perform eigenvalue decomposition */
    Eigen::SelfAdjointEigenSolver<matrix_t> eigensolver(hamiltonian);
    eigenvalues_ = eigensolver.eigenvalues();
  }
  double gs_energy() const { return eigenvalues_(0) / L_; }
  double gap() const { return eigenvalues_(1) - eigenvalues_(0); }
  std::tuple<double, double, double> free_energy(double t) const {
    // calculate free energy and intternal energy
    uint_t dim = 1 << L_;
    double beta = 1 / t;
    standards::exp_double z = 0;
    standards::exp_double w = 0;
    for (uint_t i = 0; i < dim; ++i) {
      uint_t j = dim - i - 1;
      z += standards::exp_double::exp(-beta * eigenvalues_(j));
      w += eigenvalues_(j) * standards::exp_double::exp(-beta * eigenvalues_(j));
    }
    double f = - log(z) / (beta * L_);
    double e = w / z / L_;
    return std::make_tuple(f, e, beta * (e-f));
  }
private:
  uint_t L_;
  vector_t eigenvalues_;
};

} } }


/**
 * \file dcs/math/random/mersenne_twister.hpp
 *
 * \brief Mersenne Twister Random Number Engine.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_RANDOM_MERSENNE_TWISTER_HPP
#define DCS_MATH_RANDOM_MERSENNE_TWISTER_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(101500) // 1.15
#	error "Required Boost libraries version >= 1.15."
#endif

#include <boost/random/mersenne_twister.hpp>
#include <dcs/math/random/base_generator.hpp>
#include <cstddef>
#include <stdint.h>

namespace dcs { namespace math { namespace random {

/**
 * \brief A mersenne_twister random number engine produces unsigned integer
 * random numbers in the closed interval \f$[0, 2^w − 1]\f$.
 *
 * \tparam UIntT The type of randomly generated numbers.
 * \tparam w The word size.
 * \tparam n The state size.
 * \tparam m The shift size.
 * \tparam r The mask bits.
 * \tparam a The XOR mask.
 * \tparam u The tempering.
 * \tparam s The tempering.
 * \tparam b The tempering.
 * \tparam t The tempering.
 * \tparam c The parameter.
 * \tparam l The tempering.
 *
 * A mersenne_twister random number engine produces unsigned integer random
 * numbers in the closed interval \f$[0, 2^{nw-r} − 1]\f$.
 * The state \f$x_i\f$ of a mersenne_twister object \f$x\f$ is of size \f$n\f$
 * and consists of a sequence \f$X\f$ of \f$n\f$ values of the type delivered by
 * \f$x\f$; all subscripts applied * to \f$X\f$ are to be taken modulo \f$n\f$.
 * The transition algorithm employs a twisted generalized feedback shift
 * register defined by shift values \f$n\f$ and \f$m\f$, a twist value \f$r\f$,
 * and a conditional xor-mask \f$a\f$.
 * To improve the uniformity of the result, the bits of the raw shift register
 * are additionally tempered (i.e., scrambled) according to a bit-scrambling
 * matrix defined by values \f$u\f$, \f$s\f$, \f$b\f$, \f$t\f$,
 * \f$c\f$, and \f$l\f$.
 * The state transition is performed as follows:
 * a) Concatenate the upper \f$w-r\f$ bits of \f$X_{i-n}\f$ with the lower
 *    \f$r\f$ bits of \f$X_{i+1-n}\f$ to obtain an unsigned integer value \f$Y\f$.
 * b) With \f$\alpha=a*(Y \wedge 1)\f$, set \f$X_i\f$ to
 *    \f$X_{i+m-n} \oplus (Y >> 1) xor \alpha\f$
 * The generation algorithm determines the unsigned integer values \f$z_1\f$,
 * \f$z_2\f$, \f$z_3\f$, \f$z_4\f$ as follows, then delivers \f$z_4\f$ as its
 * result:
 * a) Let \f$z_1 = X_i \oplus ((X_i >> u)\f$.
 * b) Let \f$z_2 = z_1 \oplus ((z_1 << s) \wedge b)\f$.
 * c) Let \f$z_3 = z_2 \oplus ((z_2 << t) \wedge c)\f$.
 * d) Let \f$z_4 = z_3 \oplus (z_3 >> l).
 *
 * This class implements the \c RandomNumberEngine concept.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <
	typename UIntT,
	::std::size_t w,
	::std::size_t n,
	::std::size_t m,
	::std::size_t r,
	UIntT a,
	::std::size_t u,
	::std::size_t s,
	UIntT b,
	::std::size_t t,
	UIntT c,
	::std::size_t l
>
class mersenne_twister: public base_generator<UIntT>
{
	private: typedef base_generator<UIntT> base_type;
	public: typedef UIntT result_type;
	public: typedef typename base_type::ulonglong_type ulonglong_type;
	private: typedef ::boost::random::mersenne_twister<UIntT,w,n,m,r,a,u,s,b,t,c,l,0> impl_type;

	public: static const ::std::size_t word_size = w;
	public: static const ::std::size_t state_size = n;
	public: static const ::std::size_t shift_size = m;
	public: static const ::std::size_t mask_bits = r;
	public: static const result_type xor_mask = a;
	public: static const ::std::size_t tempering_u = u;
	public: static const ::std::size_t tempering_s = s;
	public: static const result_type tempering_b = b;
	public: static const ::std::size_t tempering_t = t;
	public: static const result_type parameter_c = c;
	public: static const ::std::size_t tempering_l = l;
	public: static const result_type default_seed = impl_type::default_seed;

	public: mersenne_twister()
		: impl_()
	{
		// empty
	}

	public: explicit mersenne_twister(result_type z)
		: impl_(z)
	{
		// empty
	}

//	public: template <typename ItT>
//		mersenne_twister(ItT& first, ItT& last)
//		: impl_(first,last)
//	{
//		// empty
//	}

//	public: template <typename ItT>
//		void seed(ItT& first, ItT& last)
//	{
//		impl_.seed(first, last);
//	}

	public: static result_type min()
	{
		//return impl_type::min();//FIXME: has not been implemented yet
		return 0;
	}

	public: static result_type max()
	{
		//return impl_type::max();//FIXME: has not been implemented yet
		// 2^w-1
		result_type res = 0;
		for(size_t i = 0; i < w; ++i)
		{
			res |= (1u << i);
		}
		return res;
	}

	private: result_type do_min() const
	{
		return impl_.min();
	}

	private: result_type do_max() const
	{
		return impl_.max();
	}

	private: void do_seed()
	{
		impl_.seed();
	}

	private: void do_seed(result_type z)
	{
		impl_.seed(z);
	}

	private: result_type do_generate()
	{
		return impl_();
	}

	//FIXME: actually cannot use impl_.discard since it is defined only when
	//       BOOST_NO_LONG_LONG is undefined.
	private: void do_discard(ulonglong_type z)
	{
		for ( ; z != 0; --z)
		{
			this->operator()();
		}

//		impl_.discard(z);
	}

	public: friend bool operator==(mersenne_twister const& x, mersenne_twister const& y)
	{
		return x.impl_ == y.impl_;
	}

	public: friend bool operator!=(mersenne_twister const& x, mersenne_twister const& y)
	{
		return !(x == y);
	}

	private: impl_type impl_;
};

typedef mersenne_twister<uint32_t, 32, 351, 175, 19, 0xccab8ee7, 11, 7, 0x31b6ab00, 15, 0xffe50000, 17> mt11213b;
typedef mersenne_twister<uint32_t, 32, 624, 397, 31, 0x9908b0df, 11, 7, 0x9d2c5680, 15, 0xefc60000, 18> mt19937;

}}} // Namespace dcs::math::random

#endif // DCS_MATH_RANDOM_MERSENNE_TWISTER_HPP


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#pragma once
#include <eve/detail/skeleton_calls.hpp>
#include <eve/detail/hz_device.hpp>
#include <eve/concept/value.hpp>
#include <eve/function/if_else.hpp>
#include <eve/function/oneminus.hpp>
#include <eve/function/normal_distribution.hpp>
#include <eve/function/lognormal_distribution.hpp>
#include <eve/function/diff/gamma_p.hpp>
#include <eve/function/abs.hpp>
#include <eve/function/any.hpp>
#include <eve/function/average.hpp>
#include <eve/function/none.hpp>
#include <eve/function/converter.hpp>
#include <eve/function/dec.hpp>
#include <eve/function/is_eqz.hpp>
#include <eve/function/is_not_nan.hpp>
#include <eve/function/is_lez.hpp>
#include <eve/function/is_not_less.hpp>
#include <eve/function/lgamma.hpp>
#include <eve/function/log.hpp>
#include <eve/function/log1p.hpp>
#include <eve/function/max.hpp>
#include <eve/function/min.hpp>
#include <eve/function/rec.hpp>
#include <eve/function/sqr.hpp>
#include <eve/function/sqrt.hpp>
#include <eve/constant/one.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/eps.hpp>
#include <eve/function/ulpdist.hpp>
#include <eve/function/next.hpp>
#include <eve/function/nb_values.hpp>
#include <eve/constant/valmax.hpp>
#include <eve/constant/inf.hpp>
#include <type_traits>
#include <boost/math/special_functions/gamma.hpp>
#include <tts/tts.hpp>
#include <iomanip>
namespace eve::detail
{

  template<real_value T, real_value U>
  EVE_FORCEINLINE  auto gamma_pinv_(EVE_SUPPORTS(cpu_)
                              , T a
                              , U b) noexcept
  requires compatible_values<T, U>
  {
    return arithmetic_call(gamma_pinv, a, b);
  }

  template<floating_real_value T>
  EVE_FORCEINLINE T gamma_pinv_(EVE_SUPPORTS(cpu_), T p, T k) noexcept
  requires has_native_abi_v<T>
  {
    if constexpr(std::is_same_v<T, float>)
    {
      return float32(gamma_pinv(float64(p), float64(k)));
    }
    p = if_else(is_ltz(p) || p > one(as(p)), allbits, p);
    auto iseqzp = is_eqz(p);
    auto iseq1p = p == one(as(p));
    auto x = if_else(iseq1p, inf(as(p)), if_else(iseqzp, zero(as(p)), allbits));
    logical<T> notdone(is_not_nan(p) && !iseqzp && !iseq1p);
    auto d = rec(9*k);
    auto y = oneminus(d + invcdf(normal_distribution_01<T>, oneminus(p)) * eve::sqrt(d));

    x = if_else(notdone, k*sqr(y)*y, x);
    auto x0 = x;
    int i = 10;
    if (eve::none(notdone)) return x;
    while(i)
    {
      auto dx = if_else(notdone, (gamma_p(x, k)-p)/diff(gamma_p)(x, k), zero);
      x -= dx;
      if (i < 7) notdone = notdone && is_not_less(abs(dx), 4*eps(as(x))*max(eve::abs(x), one(as(x))));
      if (eve::none(notdone)) return x;
      --i;
    }
    notdone =  notdone || is_ltz(y);
    x = if_else(notdone, eve::abs(x0), x);
    auto xlo = if_else(notdone, eve::min(x/2, zero(as(x))), x);
    auto xhi = if_else(notdone, eve::min(x*2, eve::valmax(as(x))), x);
    auto inl = ((gamma_p(xlo, k) > p)||(gamma_p(xhi, k) <  p)) && (xlo !=  xhi);
    while (eve::any(inl))
    {
      xlo = if_else(inl, eve::max(xlo/2, zero(as(x))), xlo);
      xhi  = if_else(inl, eve::min(xhi*2, eve::valmax(as(x))), xhi);
      inl = ((gamma_p(xlo, k) > p)||(gamma_p(xhi, k) <  p)) && (xlo !=  xhi);
    }
    auto xmed = average(xlo, xhi);
    while (eve::any(notdone))
    {
      auto test = (gamma_p(xmed, k) <  p);
      xlo = if_else(test, xmed, xlo);
      xhi = if_else(test, xhi, xmed);
      notdone = ulpdist(xlo, xhi) > 1;
      xmed = average(xlo, xhi);
    }
    xmed = if_else(iseq1p, inf(as(p)), if_else(iseqzp, zero(as(p)), xmed));
    return if_else(k == one(as(k)),  -eve::log1p(-p), xmed);
  }
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include <CGAL/Polyhedron_3.h>
#include <CGAL/Surface_mesh.h>

#if defined(CGAL_USE_OPENMESH)
#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/PolyMesh_ArrayKernelT.hh>
#include <CGAL/boost/graph/graph_traits_PolyMesh_ArrayKernelT.h>
#endif

#include <CGAL/boost/graph/copy_face_graph.h>

#include <iostream>
#include <fstream>
#include <iterator>

#include <boost/unordered_map.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

typedef CGAL::Polyhedron_3<Kernel>                       Source;
typedef boost::graph_traits<Source>::vertex_descriptor   sm_vertex_descriptor;
typedef boost::graph_traits<Source>::halfedge_descriptor sm_halfedge_descriptor;
typedef boost::graph_traits<Source>::face_descriptor     sm_face_descriptor;

typedef CGAL::Exact_predicates_exact_constructions_kernel Other_kernel;
typedef Other_kernel::Point_3                             Point;

int main(int argc, char* argv[])
{
  Source S;

  std::ifstream in((argc>1)?argv[1]:CGAL::data_file_path("meshes/cube_poly.off"));
  in >> S;
  assert( CGAL::is_valid_polygon_mesh(S) );

  // Note that the vertex_point property of the Source and Target1
  // come from different kernels.
  typedef CGAL::Surface_mesh<Point> Target1;
  Target1 T1;
  CGAL::copy_face_graph(S, T1);
  assert( CGAL::is_valid_polygon_mesh(T1) );
  assert( vertices(S).size()==vertices(T1).size() );
  assert( halfedges(S).size()==halfedges(T1).size() );
  assert( faces(S).size()==faces(T1).size() );

#if defined(CGAL_USE_OPENMESH)
  typedef OpenMesh::PolyMesh_ArrayKernelT</* MyTraits*/> Target2;
  Target2 T2;
  {
    typedef boost::graph_traits<Target2>::vertex_descriptor   tm_vertex_descriptor;
    typedef boost::graph_traits<Target2>::halfedge_descriptor tm_halfedge_descriptor;
    typedef boost::graph_traits<Target2>::face_descriptor     tm_face_descriptor;

    // Use an unordered_map to keep track of elements.
    boost::unordered_map<sm_vertex_descriptor, tm_vertex_descriptor>     v2v;
    boost::unordered_map<sm_halfedge_descriptor, tm_halfedge_descriptor> h2h;
    boost::unordered_map<sm_face_descriptor, tm_face_descriptor>         f2f;

    CGAL::copy_face_graph(S, T2, CGAL::parameters::vertex_to_vertex_output_iterator(std::inserter(v2v, v2v.end()))
                                 .halfedge_to_halfedge_output_iterator(std::inserter(h2h, h2h.end()))
                                 .face_to_face_output_iterator(std::inserter(f2f, f2f.end())));
    assert( CGAL::is_valid_polygon_mesh(T2) );
    assert( v2v.size()==vertices(T2).size() );
    assert( h2h.size()==halfedges(T2).size() );
    assert( f2f.size()==faces(T2).size() );
    assert( vertices(S).size()==vertices(T2).size() );
    assert( halfedges(S).size()==halfedges(T2).size() );
    assert( faces(S).size()==faces(T2).size() );
  }
#endif
  S.clear();
  {
    typedef boost::graph_traits<Target1>::vertex_descriptor   source_vertex_descriptor;
    typedef boost::graph_traits<Target1>::halfedge_descriptor source_halfedge_descriptor;
    typedef boost::graph_traits<Target1>::face_descriptor source_face_descriptor;

    typedef boost::graph_traits<Source>::vertex_descriptor   tm_vertex_descriptor;
    typedef boost::graph_traits<Source>::halfedge_descriptor tm_halfedge_descriptor;
    typedef boost::graph_traits<Source>::face_descriptor   tm_face_descriptor;

    boost::unordered_map<source_vertex_descriptor, tm_vertex_descriptor> v2v;
    boost::unordered_map<source_halfedge_descriptor, tm_halfedge_descriptor> h2h;
    boost::unordered_map<source_face_descriptor, tm_face_descriptor> f2f;
    CGAL::copy_face_graph(T1, S, CGAL::parameters::vertex_to_vertex_map(boost::make_assoc_property_map(v2v))
                          .halfedge_to_halfedge_output_iterator(std::inserter(h2h, h2h.end()))
                          .face_to_face_map(boost::make_assoc_property_map(f2f)));

    assert( CGAL::is_valid_polygon_mesh(S) );
    assert( vertices(S).size()==vertices(T1).size() );
    assert( halfedges(S).size()==halfedges(T1).size() );
    assert( faces(S).size()==faces(T1).size() );
  }
  return 0;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file analytichestonadengine.hpp
  \brief analytic pricing engine for a heston option
  based on fourier transformation
*/

#include <ql/math/functional.hpp>
#include <ql/instruments/payoffs.hpp>
#include <ql/math/integrals/gaussianadquadratures.hpp>
#include <ql/pricingengines/vanilla/analytichestonadengine.hpp>

#include <boost/assign/std/vector.hpp>
using namespace boost::assign;

namespace QuantLib {

	namespace {
		std::complex<CppAD::AD<Real> > operator*(
			Real a, const std::complex<CppAD::AD<Real> >& b ) {
			return std::complex<CppAD::AD<Real> >(a*b.real(), a*b.imag());
		}
		std::complex<CppAD::AD<Real> > operator*(
			const std::complex<CppAD::AD<Real> >& b, Real a) {
			return a*b;
		}
		std::complex<CppAD::AD<Real> > operator-(
			Real a, const std::complex<CppAD::AD<Real> >& b) {
			return std::complex<CppAD::AD<Real> >(a - b.real(), -b.imag());
		}
	}

    // helper class for integration
    class AnalyticHestonADEngine::Fj_Helper
        : public std::unary_function<Real, Real>
    {
    public:
        Fj_Helper(
        	CppAD::AD<Real>& kappa,
        	CppAD::AD<Real>& theta,
        	CppAD::AD<Real>& sigma,
        	CppAD::AD<Real>& v0,
        	CppAD::AD<Real>& s0,
        	CppAD::AD<Real>& rho,
            const AnalyticHestonADEngine* const engine,
            ComplexLogFormula cpxLog,
            Time term,
            Real strike,
            Real ratio,
            Size j);

        CppAD::AD<Real> operator()(Real phi)      const;

    private:
        const Size j_;
        const CppAD::AD<Real> &kappa_, &theta_, &sigma_, &v0_;
        const ComplexLogFormula cpxLog_;

        // helper variables
        const Time term_;
        CppAD::AD<Real> x_;
        const Real sx_;
        CppAD::AD<Real> dd_;
        const CppAD::AD<Real> sigma2_, rsigma_;
        const CppAD::AD<Real> t0_;

        // log branch counter
        mutable int  b_;     // log branch counter
        mutable Real g_km1_; // imag part of last log value

        const AnalyticHestonADEngine* const engine_;
    };

    AnalyticHestonADEngine::Fj_Helper::Fj_Helper(
		CppAD::AD<Real>& kappa,
		CppAD::AD<Real>& theta,
		CppAD::AD<Real>& sigma,
		CppAD::AD<Real>& v0,
		CppAD::AD<Real>& s0,
		CppAD::AD<Real>& rho,
        const AnalyticHestonADEngine* const engine,
        ComplexLogFormula cpxLog,
        Time term,
        Real strike,
        Real ratio,
        Size j)
        :
        j_(j),
        kappa_(kappa),
        theta_(theta),
        sigma_(sigma),
        v0_(v0),
        cpxLog_(cpxLog),
        term_(term),
        x_(log(s0)),
        sx_(std::log(strike)),
        dd_(x_-std::log(ratio)),
        sigma2_(sigma_*sigma_),
        rsigma_(rho*sigma_),
        t0_(kappa - ((j== 1)? rho*sigma : 0)),
        b_(0),
        g_km1_(0),
        engine_(engine) {
    }

    CppAD::AD<Real> AnalyticHestonADEngine::Fj_Helper::operator()(
    	Real phi) const {

        const CppAD::AD<Real> rpsig(rsigma_*phi);

        const std::complex<CppAD::AD<Real> > t1 =
        	t0_+std::complex<CppAD::AD<Real> >(0, -rpsig);
        const std::complex<CppAD::AD<Real> > d =
            std::sqrt(t1*t1 - sigma2_*phi
                      *std::complex<CppAD::AD<Real> >(-phi, (j_== 1)? 1 : -1));
        const std::complex<CppAD::AD<Real> > ex = exp(-d*term_);

        if (cpxLog_ == Gatheral) {
            if (phi != 0.0) {
                if (sigma_ > 1e-5) {
                    const std::complex<CppAD::AD<Real> > p = (t1-d)/(t1+d);
                    const std::complex<CppAD::AD<Real> > g
                        = log((1.0 - p*ex)/(1.0 - p));

                    const std::complex<CppAD::AD<Real> > c(
                    	v0_*(t1-d)*(1.0-ex)/(sigma2_*(1.0-ex*p))
                    	+ (kappa_*theta_)/sigma2_*((t1-d)*term_-2.0*g));

                    CppAD::AD<Real> im(c.imag()), re(c.real());

                    return
                        exp(std::complex<CppAD::AD<Real> >(re, im)
							 + std::complex<CppAD::AD<Real> >(0.0, phi*(dd_-sx_))
							 ).imag()/phi;
                }
                else {
                	QL_FAIL("ouch, sigma < 1e-5");
                }
            }
            else {
            	QL_FAIL("ouch, phi = 0");
            }
        }
        else if (cpxLog_ == BranchCorrection) {
        	QL_FAIL("ouch, BranchCorrection");
        }
        else {
            QL_FAIL("unknown complex logarithm formula");
        }
    }

    AnalyticHestonADEngine::AnalyticHestonADEngine(
                              const boost::shared_ptr<HestonModel>& model,
                              Size integrationOrder)
    : GenericModelEngine<HestonModel,
                         VanillaOption::arguments,
                         VanillaOption::results>(model),
      evaluations_(0),
      cpxLog_     (Gatheral),
      integration_(new Integration(
                          Integration::gaussLaguerre(integrationOrder))) {
    }

    Size AnalyticHestonADEngine::numberOfEvaluations() const {
        return evaluations_;
    }

    void AnalyticHestonADEngine::doCalculation(Real riskFreeDiscount,
                                             Real dividendDiscount,
                                             Real spotPrice,
                                             Real strikePrice,
                                             Real term,
                                             Real kappa, Real theta, Real sigma, Real v0, Real rho,
                                             const TypePayoff& type,
                                             const Integration& integration,
                                             const ComplexLogFormula cpxLog,
                                             const AnalyticHestonADEngine* const enginePtr,
                                             VanillaOption::results& results,
                                             Size& evaluations)
    {
        std::vector<CppAD::AD<Real> > params;
        params += spotPrice, v0, kappa, theta, sigma, rho;
        CppAD::Independent(params);

        std::vector<Real> vp;
        vp += spotPrice, v0, kappa, theta, sigma, rho;
        const Real ratio = riskFreeDiscount/dividendDiscount;

        evaluations = 0;

        const CppAD::AD<Real> p1 = integration.calculate(
            Fj_Helper(params[2], params[3], params[4],
            		  params[1], params[0], params[5], enginePtr,
                      cpxLog, term, strikePrice, ratio, 1))/M_PI;
        evaluations+= integration.numberOfEvaluations();

        const CppAD::AD<Real> p2 = integration.calculate(
            Fj_Helper(params[2], params[3], params[4],
            		  params[1], params[0], params[5], enginePtr,
                      cpxLog, term, strikePrice, ratio, 2))/M_PI;
        evaluations+= integration.numberOfEvaluations();

        std::vector<CppAD::AD<Real> > y(1);

        switch (type.optionType())
        {
          case Option::Call:
            y[0] = params[0]*dividendDiscount*(p1+0.5)
            	- strikePrice*riskFreeDiscount*(p2+0.5);
            break;
          case Option::Put:
        	  y[0] = params[0]*dividendDiscount*(p1-0.5)
                - strikePrice*riskFreeDiscount*(p2-0.5);
            break;
          default:
            QL_FAIL("unknown option type");
        }

        std::vector<Real> moreResults
        	= CppAD::ADFun<Real>(params, y).Reverse(1, std::vector<Real>(1, 1.0));

        results.value = CppAD::Value(y[0]);

        results.delta = moreResults[0];
        results.additionalResults["delta"] = moreResults[0];
        results.additionalResults["v0"]    = moreResults[1];
        results.additionalResults["kappa"] = moreResults[2];
        results.additionalResults["theta"] = moreResults[3];
        results.additionalResults["sigma"] = moreResults[4];
        results.additionalResults["rho"]   = moreResults[5];
    }

    void AnalyticHestonADEngine::calculate() const
    {
        // this is a european option pricer
        QL_REQUIRE(arguments_.exercise->type() == Exercise::European,
                   "not an European option");

        // plain vanilla
        boost::shared_ptr<PlainVanillaPayoff> payoff =
            boost::dynamic_pointer_cast<PlainVanillaPayoff>(arguments_.payoff);
        QL_REQUIRE(payoff, "non plain vanilla payoff given");

        const boost::shared_ptr<HestonProcess>& process = model_->process();

        const Real riskFreeDiscount = process->riskFreeRate()->discount(
                                            arguments_.exercise->lastDate());
        const Real dividendDiscount = process->dividendYield()->discount(
                                            arguments_.exercise->lastDate());

        const Real spotPrice = process->s0()->value();
        QL_REQUIRE(spotPrice > 0.0, "negative or null underlying given");

        const Real strikePrice = payoff->strike();
        const Real term = process->time(arguments_.exercise->lastDate());

        doCalculation(riskFreeDiscount,
                      dividendDiscount,
                      spotPrice,
                      strikePrice,
                      term,
                      model_->kappa(),
                      model_->theta(),
                      model_->sigma(),
                      model_->v0(),
                      model_->rho(),
                      *payoff,
                      *integration_,
                      cpxLog_,
                      this,
                      results_,
                      evaluations_);
    }

    AnalyticHestonADEngine::Integration::Integration(
            Algorithm intAlgo,
            const boost::shared_ptr<GaussianADQuadrature>& gaussianQuadrature)
    : intAlgo_(intAlgo),
      gaussianQuadrature_(gaussianQuadrature) { }

    AnalyticHestonADEngine::Integration
    AnalyticHestonADEngine::Integration::gaussLaguerre(Size intOrder) {
        QL_REQUIRE(intOrder <= 192, "maximum integraton order (192) exceeded");
        return Integration(GaussLaguerre,
                           boost::shared_ptr<GaussianADQuadrature>(
                               new GaussLaguerreADIntegration(intOrder)));
    }

    AnalyticHestonADEngine::Integration
    AnalyticHestonADEngine::Integration::gaussLegendre(Size intOrder) {
        return Integration(GaussLegendre,
                           boost::shared_ptr<GaussianADQuadrature>(
                               new GaussLegendreADIntegration(intOrder)));
    }

    AnalyticHestonADEngine::Integration
    AnalyticHestonADEngine::Integration::gaussChebyshev(Size intOrder) {
        return Integration(GaussChebyshev,
                           boost::shared_ptr<GaussianADQuadrature>(
                               new GaussChebyshevADIntegration(intOrder)));
    }

    AnalyticHestonADEngine::Integration
    AnalyticHestonADEngine::Integration::gaussChebyshev2nd(Size intOrder) {
        return Integration(GaussChebyshev2nd,
                           boost::shared_ptr<GaussianADQuadrature>(
                               new GaussChebyshev2ndADIntegration(intOrder)));
    }

    Size AnalyticHestonADEngine::Integration::numberOfEvaluations() const {
    	return gaussianQuadrature_->order();
    }

    CppAD::AD<Real> AnalyticHestonADEngine::Integration::calculate(
	    const boost::function<CppAD::AD<Real>(Real)>& f) const {

    	CppAD::AD<Real> retVal;

        switch(intAlgo_) {
          case GaussLaguerre:
            retVal = gaussianQuadrature_->operator()(f);
            break;
          default:
              QL_FAIL("unknwon integration algorithm");
        }

        return retVal;
     }
}


#include "unscented.h"

#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <unsupported/Eigen/MatrixFunctions>

using namespace uvdar;

double angdiff(double x,double y){
  double d = y-x;
  if ((d>-M_PI)&&(d<M_PI)) return d;
  return fmod(d+M_PI,2*M_PI)-M_PI;
}

unscented::measurement unscented::unscentedTransform(e::VectorXd x,e::MatrixXd Px, const boost::function<e::VectorXd(e::VectorXd,e::VectorXd,int)> &fcn,double fleft,double fright, double fcenter, int camera_index){
  int L = x.rows();
  double W0=1.0/3.0;
  Eigen::MatrixXd X(L,2*L+1);
  X.leftCols(1) = x;
  X.rightCols(2*L).Ones(L,2*L);

  Eigen::VectorXd W(2*L+1);
  W =e::VectorXd::Ones(2*L+1)*((1-W0)/(2*L));
  W(0)=W0;

  e::MatrixXd sf = ((L/(1-W0))*Px).sqrt();

  for (int i=0; i<L; i++){
    X.col(i*2+1) = (x+(sf.row(i)).transpose());
    X.col(i*2+2) = (x-(sf.row(i)).transpose());// check
  }
  e::MatrixXd Y(6,2*L+1);

  e::VectorXd expFrequencies;
  if (fcenter>0){
    expFrequencies = e::VectorXd(3);
    expFrequencies << fleft,fright,fcenter;
  }
  else {
    expFrequencies = e::VectorXd(2);
    expFrequencies << fleft,fright;
  }

  for (int i=0; i<(1+2*L); i++){
    Y.col(i)=fcn(X.col(i),expFrequencies, camera_index); //this is weird, check please
  }
  int nan_index = -1;
  int nan_count = 0;
  for (int i=0; i<(1+2*L); i++){
    if (Y.col(i).array().isNaN().any()){
      nan_count++;
      nan_index = i;
    }
  }

  if (nan_count == 1){
    Y.col(nan_index) = Y.col(0);
  }
  if (false){
  /* if (true){ */
    std::cout << "unscented W: "<< std::endl;
    std::cout << W.transpose() << std::endl;
    std::cout << "unscented X: "<< std::endl;
    std::cout << X << std::endl;
    std::cout << "unscented Y: "<< std::endl;
    std::cout << Y << std::endl;
  }
  e::Vector3d mr;
    mr << 0,0,0;
  e::VectorXd y = Y*W;

  e::MatrixXd Ye = (Y-y.replicate(1,2*L+1));
  e::MatrixXd Py = ((Ye*W.asDiagonal())*Ye.transpose());

  for (int i=0; i<3; i++){
      if (y(3+i)>M_PI){
        y(3+i)=-2*M_PI+y(3+i);
    }
  }
  struct measurement output;
  output.x = y;
  output.C = Py;
  return output;
}

std::vector<Eigen::VectorXd> unscented::getSigmaPtsSource(e::VectorXd x,e::MatrixXd Px){
  int L = x.rows();
  double W0=1.0/3.0;
  std::vector<e::VectorXd> output;
  output.push_back(x);
  Eigen::VectorXd W(2*L+1);
  W =e::VectorXd::Ones(2*L+1)*((1-W0)/(2*L));
  W(0)=W0;
  e::MatrixXd sf = ((L/(1-W0))*Px).sqrt();
  for (int i=0; i<L; i++){
    output.push_back((x+(sf.row(i)).transpose()));
    output.push_back((x-(sf.row(i)).transpose()));
  }
  return output;
}

/* } */


#include "utility.h"
#include <random>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <Eigen/Core>
#include "slicesampler.h"

using Eigen::VectorXd;
using Eigen::MatrixXd;

namespace
{
std::random_device seed;
std::default_random_engine gen(seed());
std::uniform_real_distribution<double> uniform_dist(0.0, 1.0);
std::normal_distribution<> normal_dist(0.0, 1.0);
}

namespace Utility
{

Eigen::VectorXd generateRandomVector(unsigned n)
{
    Eigen::VectorXd x(n);
    for (unsigned i = 0; i < n; ++ i)
    {
        x(i) = uniform_dist(gen);
    }
    return x;
}

double generateUniformReal()
{
    return uniform_dist(gen);
}

double generateStandardNormal()
{
    return normal_dist(gen);
}

double temp(const Eigen::VectorXd& x, const void* data)
{
    const MatrixXd& Sigma_inv = static_cast<const std::pair<MatrixXd, double>*>(data)->first;
    const double    Sigma_det = static_cast<const std::pair<MatrixXd, double>*>(data)->second;
    return Utility::gauss(x, VectorXd::Zero(x.rows()), Sigma_inv, Sigma_det);
}

VectorXd generateNormal(const Eigen::VectorXd &mu, const Eigen::MatrixXd &Sigma)
{
    VectorXd x = VectorXd::Zero(mu.rows());
    std::pair<MatrixXd, double> data(Sigma.inverse(), Sigma.determinant());
    x = SliceSampler::sampling(temp, &data, x, VectorXd::Constant(x.rows(), Sigma.maxCoeff() * 3.0));
    x = SliceSampler::sampling(temp, &data, x, VectorXd::Constant(x.rows(), Sigma.maxCoeff() * 3.0));
    x = SliceSampler::sampling(temp, &data, x, VectorXd::Constant(x.rows(), Sigma.maxCoeff() * 3.0));
    x = SliceSampler::sampling(temp, &data, x, VectorXd::Constant(x.rows(), Sigma.maxCoeff() * 3.0));
    x = SliceSampler::sampling(temp, &data, x, VectorXd::Constant(x.rows(), Sigma.maxCoeff() * 3.0));
    return x + mu;
}

void exportMatrixToCsv(const std::string& filePath, const Eigen::MatrixXd& X)
{
    std::ofstream ofs(filePath);
    for (unsigned i = 0; i < X.rows(); ++ i)
    {
        for (unsigned j = 0; j < X.cols(); ++ j)
        {
            ofs << X(i, j);
            if (j + 1 != X.cols()) ofs << ",";
        }
        ofs << std::endl;
    }
}

std::string getCurrentTimeInString()
{
    const std::time_t t = std::time(nullptr);
    std::stringstream s; s << std::put_time(std::localtime(&t), "%Y%m%d%H%M%S");
    return s.str();
}

}