#include "mex.h"

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

#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/parse_rhs.h>

#include <igl/PI.h>

#include <Eigen/Core>

// Mesh
Eigen::MatrixXd V;
Eigen::MatrixXi F;

// Constrained faces id
Eigen::VectorXi b;

// Cosntrained faces representative vector
Eigen::MatrixXd bc;

// Degree of the N-RoSy field
int N = 6;

// Converts a representative vector per face in the full set of vectors that describe
// an N-RoSy field
void representative_to_nrosy(
  const Eigen::MatrixXd& V,
  const Eigen::MatrixXi& F,
  const Eigen::MatrixXd& R,
  const int N,
  Eigen::MatrixXd& Y)
{
  using namespace Eigen;
  using namespace std;
  MatrixXd B1, B2, B3;

  igl::local_basis(V,F,B1,B2,B3);

  Y.resize(F.rows()*N,3);
  for (unsigned i=0;i<F.rows();++i)
  {
    double x = R.row(i) * B1.row(i).transpose();
    double y = R.row(i) * B2.row(i).transpose();
    double angle = atan2(y,x);

    for (unsigned j=0; j<N;++j)
    {
      double anglej = angle + 2*igl::PI*double(j)/double(N);
      double xj = cos(anglej);
      double yj = sin(anglej);
      Y.row(i*N+j) = xj * B1.row(i) + yj * B2.row(i);
    }
  }
}

/* The gateway function */
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[])
{
    igl::matlab::parse_rhs_double(prhs,V);
    igl::matlab::parse_rhs_index(prhs+1,F);

    // Threshold faces with high anisotropy
    b.resize(1);
    b << 0;
    bc.resize(1,3);
    bc << 1,1,1;

    // Runs the nrosy
    MatrixXd R; //output field
    VectorXd S; //output singularities

    igl::copyleft::comiso::nrosy(V,F,b,bc,VectorXi(),VectorXd(),MatrixXd(),N,0.5,R,S);

    Eigen::MatrixXd Y;
    representative_to_nrosy(V, F, R, N, Y);

    // Return the matrices to matlab
      switch(nlhs)
      {
        case 2:
          igl::matlab::prepare_lhs_double(S,plhs+1);
        case 1:
          igl::matlab::prepare_lhs_double(Y,plhs);
        default: break;
      }
}


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

/*
 Copyright (C) 2003 RiskMap srl

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

#ifndef quantlib_test_integrals_hpp
#define quantlib_test_integrals_hpp

#include <boost/test/unit_test.hpp>

/* remember to document new and/or updated tests in the Doxygen
   comment block of the corresponding class */

class IntegralTest {
  public:
    static void testSegment();
    static void testTrapezoid();
    static void testMidPointTrapezoid();
    static void testSimpson();
    static void testGaussKronrodAdaptive();
    static void testGaussKronrodNonAdaptive();
    static void testGaussLobatto();
    static void testTwoDimensionalIntegration();
    static void testFolinIntegration();
    static void testDiscreteIntegrals();
    static void testDiscreteIntegrator();
    static void testPiecewiseIntegral();
    static boost::unit_test_framework::test_suite* suite();
};

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

/*
 Copyright (C) 2003 RiskMap srl
 Copyright (C) 2015 Peter Caspers

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

#include "utilities.hpp"
#include <ql/math/functional.hpp>
#include <ql/math/integrals/filonintegral.hpp>
#include <ql/math/integrals/segmentintegral.hpp>
#include <ql/math/integrals/simpsonintegral.hpp>
#include <ql/math/integrals/trapezoidintegral.hpp>
#include <ql/math/integrals/kronrodintegral.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/math/integrals/discreteintegrals.hpp>
#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/termstructures/volatility/abcd.hpp>
#include <ql/math/integrals/twodimensionalintegral.hpp>
#include <ql/experimental/math/piecewisefunction.hpp>
#include <ql/experimental/math/piecewiseintegral.hpp>

#include <boost/make_shared.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/assign/std/vector.hpp>

using namespace QuantLib;
using namespace boost::assign;
using boost::unit_test_framework::test_suite;

namespace {

    Real Tolerance = 1.0e-6;

    template <class T>
    void testSingle(const T& I, const std::string& tag,
                    const boost::function<Real (Real)>& f,
                    Real xMin, Real xMax, Real expected) {
        Real calculated = I(f,xMin,xMax);
        if (std::fabs(calculated-expected) > Tolerance) {
            BOOST_FAIL(std::setprecision(10)
                       << "integrating " << tag
                       << "    calculated: " << calculated
                       << "    expected:   " << expected);
        }
    }

    template <class T>
    void testSeveral(const T& I) {
        testSingle(I, "f(x) = 0",
                   constant<Real,Real>(0.0), 0.0, 1.0, 0.0);
        testSingle(I, "f(x) = 1",
                   constant<Real,Real>(1.0), 0.0, 1.0, 1.0);
        testSingle(I, "f(x) = x",
                   QuantLib::identity<Real>(),           0.0, 1.0, 0.5);
        testSingle(I, "f(x) = x^2",
                   square<Real>(),             0.0, 1.0, 1.0/3.0);
        testSingle(I, "f(x) = sin(x)",
                   std::ptr_fun<Real,Real>(std::sin), 0.0, M_PI, 2.0);
        testSingle(I, "f(x) = cos(x)",
                   std::ptr_fun<Real,Real>(std::cos), 0.0, M_PI, 0.0);
        testSingle(I, "f(x) = Gaussian(x)",
                   NormalDistribution(), -10.0, 10.0, 1.0);
        testSingle(I, "f(x) = Abcd2(x)",
                   AbcdSquared(0.07, 0.07, 0.5, 0.1, 8.0, 10.0), 5.0, 6.0,
                   AbcdFunction(0.07, 0.07, 0.5, 0.1).covariance(5.0, 6.0, 8.0, 10.0));
    }

    template <class T>
    void testDegeneratedDomain(const T& I) {
        testSingle(I, "f(x) = 0 over [1, 1 + macheps]",
                   constant<Real, Real>(0.0), 1.0, 1.0 + QL_EPSILON, 0.0);
    }

}

void IntegralTest::testSegment() {
    BOOST_TEST_MESSAGE("Testing segment integration...");
    testSeveral(SegmentIntegral(10000));
    testDegeneratedDomain(SegmentIntegral(10000));
}

void IntegralTest::testTrapezoid() {
    BOOST_TEST_MESSAGE("Testing trapezoid integration...");
    testSeveral(TrapezoidIntegral<Default>(Tolerance, 10000));
    testDegeneratedDomain(TrapezoidIntegral<Default>(Tolerance, 10000));
}

void IntegralTest::testMidPointTrapezoid() {
    BOOST_TEST_MESSAGE("Testing mid-point trapezoid integration...");
    testSeveral(TrapezoidIntegral<MidPoint>(Tolerance, 10000));
    testDegeneratedDomain(TrapezoidIntegral<MidPoint>(Tolerance, 10000));
}

void IntegralTest::testSimpson() {
    BOOST_TEST_MESSAGE("Testing Simpson integration...");
    testSeveral(SimpsonIntegral(Tolerance, 10000));
    testDegeneratedDomain(SimpsonIntegral(Tolerance, 10000));
}

void IntegralTest::testGaussKronrodAdaptive() {
    BOOST_TEST_MESSAGE("Testing adaptive Gauss-Kronrod integration...");
    Size maxEvaluations = 1000;
    testSeveral(GaussKronrodAdaptive(Tolerance, maxEvaluations));
    testDegeneratedDomain(GaussKronrodAdaptive(Tolerance, maxEvaluations));
}

void IntegralTest::testGaussLobatto() {
    BOOST_TEST_MESSAGE("Testing adaptive Gauss-Lobatto integration...");
    Size maxEvaluations = 1000;
    testSeveral(GaussLobattoIntegral(maxEvaluations, Tolerance));
    // on degenerated domain [1,1+macheps] an exception is thrown
    // which is also ok, but not tested here
}

void IntegralTest::testGaussKronrodNonAdaptive() {
    BOOST_TEST_MESSAGE("Testing non-adaptive Gauss-Kronrod integration...");
    Real precision = Tolerance;
    Size maxEvaluations = 100;
    Real relativeAccuracy = Tolerance;
    GaussKronrodNonAdaptive gaussKronrodNonAdaptive(precision, maxEvaluations,
                                                    relativeAccuracy);
    testSeveral(gaussKronrodNonAdaptive);
    testDegeneratedDomain(gaussKronrodNonAdaptive);
}

void IntegralTest::testTwoDimensionalIntegration() {
    BOOST_TEST_MESSAGE("Testing two dimensional adaptive "
                       "Gauss-Lobatto integration...");

    const Size maxEvaluations = 1000;
    const Real calculated = TwoDimensionalIntegral(
        boost::shared_ptr<Integrator>(
            new TrapezoidIntegral<Default>(Tolerance, maxEvaluations)),
        boost::shared_ptr<Integrator>(
            new TrapezoidIntegral<Default>(Tolerance, maxEvaluations)))(
        std::multiplies<Real>(),
        std::make_pair(0.0, 0.0), std::make_pair(1.0, 2.0));

    const Real expected = 1.0;
    if (std::fabs(calculated-expected) > Tolerance) {
        BOOST_FAIL(std::setprecision(10)
                   << "two dimensional integration: "
                   << "\n    calculated: " << calculated
                   << "\n    expected:   " << expected);
    }
}

namespace {

    class sineF {
      public:
        Real operator()(Real x) const {
            return std::exp(-0.5*(x - M_PI_2/100));
        }
    };

    class cosineF {
      public:
        Real operator()(Real x) const {
            return std::exp(-0.5*x);
        }
    };

}

void IntegralTest::testFolinIntegration() {
    BOOST_TEST_MESSAGE("Testing Folin's integral formulae...");

    // Examples taken from
    // http://www.tat.physik.uni-tuebingen.de/~kokkotas/Teaching/Num_Methods_files/Comp_Phys5.pdf
    const Size nr[] = { 4, 8, 16, 128, 256, 1024, 2048 };
    const Real expected[] = { 4.55229440e-5,4.72338540e-5, 4.72338540e-5,
                              4.78308678e-5,4.78404787e-5, 4.78381120e-5,
                              4.78381084e-5};

    const Real t = 100;
    const Real o = M_PI_2/t;

    const Real tol = 1e-12;

    for (Size i=0; i < LENGTH(nr); ++i) {
        const Size n = nr[i];
        const Real calculatedCosine
            = FilonIntegral(FilonIntegral::Cosine, t, n)(cosineF(),0,2*M_PI);
        const Real calculatedSine
            = FilonIntegral(FilonIntegral::Sine, t, n)
                (sineF(), o,2*M_PI + o);

        if (std::fabs(calculatedCosine-expected[i]) > tol) {
            BOOST_FAIL(std::setprecision(10)
                << "Filon Cosine integration failed: "
                << "\n    calculated: " << calculatedCosine
                << "\n    expected:   " << expected[i]);
        }
        if (std::fabs(calculatedSine-expected[i]) > tol) {
            BOOST_FAIL(std::setprecision(10)
                << "Filon Sine integration failed: "
                << "\n    calculated: " << calculatedCosine
                << "\n    expected:   " << expected[i]);
        }
    }
}

namespace {

    Real f1(Real x) {
        return 1.2*x*x+3.2*x+3.1;
    }

    Real f2(Real x) {
        return 4.3*(x-2.34)*(x-2.34)-6.2*(x-2.34) + f1(2.34);
    }

}

void IntegralTest::testDiscreteIntegrals() {
    BOOST_TEST_MESSAGE("Testing discrete integral formulae...");

    Array x(6), f(6);
    x[0] = 1.0; x[1] = 2.02; x[2] = 2.34; x[3] = 3.3; x[4] = 4.2; x[5] = 4.6;

    std::transform(x.begin(), x.begin()+3, f.begin(),   f1);
    std::transform(x.begin()+3, x.end(),   f.begin()+3, f2);

    const Real expectedSimpson =
        16.0401216 + 30.4137528 + 0.2*f2(4.2) + 0.2*f2(4.6);
    const Real expectedTrapezoid =
          0.5*(f1(1.0)  + f1(2.02))*1.02
        + 0.5*(f1(2.02) + f1(2.34))*0.32
        + 0.5*(f2(2.34) + f2(3.3) )*0.96
        + 0.5*(f2(3.3)  + f2(4.2) )*0.9
        + 0.5*(f2(4.2)  + f2(4.6) )*0.4;

    const Real calculatedSimpson =  DiscreteSimpsonIntegral()(x, f);
    const Real calculatedTrapezoid = DiscreteTrapezoidIntegral()(x, f);

    const Real tol = 1e-12;
    if (std::fabs(calculatedSimpson-expectedSimpson) > tol) {
        BOOST_FAIL(std::setprecision(16)
            << "discrete Simpson integration failed: "
            << "\n    calculated: " << calculatedSimpson
            << "\n    expected:   " << expectedSimpson);
    }

    if (std::fabs(calculatedTrapezoid-expectedTrapezoid) > tol) {
        BOOST_FAIL(std::setprecision(16)
            << "discrete Trapezoid integration failed: "
            << "\n    calculated: " << calculatedTrapezoid
            << "\n    expected:   " << expectedTrapezoid);
    }
}

void IntegralTest::testDiscreteIntegrator() {
    BOOST_TEST_MESSAGE("Testing discrete integrator formulae...");

    testSeveral(DiscreteSimpsonIntegrator(300));
    testSeveral(DiscreteTrapezoidIntegrator(3000));
}

namespace {

std::vector<Real> x, y;

Real pw_fct(const Real t) { return QL_PIECEWISE_FUNCTION(x, y, t); }

void pw_check(const Integrator &in, const Real a, const Real b,
              const Real expected) {
    Real calculated = in(pw_fct, a, b);
    if (!close(calculated, expected))
        BOOST_FAIL(std::setprecision(16)
                   << "piecewise integration over [" << a << "," << b
                   << "] failed: "
                   << "\n   calculated: " << calculated
                   << "\n   expected:   " << expected
                   << "\n   difference: " << (calculated - expected));
}
} // empty namespace

void IntegralTest::testPiecewiseIntegral() {
    BOOST_TEST_MESSAGE("Testing piecewise integral...");
    x += 1.0, 2.0, 3.0, 4.0, 5.0;
    y += 1.0, 2.0, 3.0, 4.0, 5.0, 6.0;
    boost::shared_ptr<Integrator> segment =
        boost::make_shared<SegmentIntegral>(1);
    boost::shared_ptr<Integrator> piecewise =
        boost::make_shared<PiecewiseIntegral>(segment, x);
    pw_check(*piecewise, -1.0, 0.0, 1.0);
    pw_check(*piecewise, 0.0, 1.0, 1.0);
    pw_check(*piecewise, 0.0, 1.5, 2.0);
    pw_check(*piecewise, 0.0, 2.0, 3.0);
    pw_check(*piecewise, 0.0, 2.5, 4.5);
    pw_check(*piecewise, 0.0, 3.0, 6.0);
    pw_check(*piecewise, 0.0, 4.0, 10.0);
    pw_check(*piecewise, 0.0, 5.0, 15.0);
    pw_check(*piecewise, 0.0, 6.0, 21.0);
    pw_check(*piecewise, 0.0, 7.0, 27.0);
    pw_check(*piecewise, 3.5, 4.5, 4.5);
    pw_check(*piecewise, 5.0, 10.0, 30.0);
    pw_check(*piecewise, 9.0, 10.0, 6.0);
}

test_suite* IntegralTest::suite() {
    test_suite* suite = BOOST_TEST_SUITE("Integration tests");
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testSegment));
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testTrapezoid));
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testMidPointTrapezoid));
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testSimpson));
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testGaussKronrodAdaptive));
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testGaussKronrodNonAdaptive));
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testGaussLobatto));
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testTwoDimensionalIntegration));
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testFolinIntegration));
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testDiscreteIntegrals));
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testDiscreteIntegrator));
    suite->add(QUANTLIB_TEST_CASE(&IntegralTest::testPiecewiseIntegral));
    return suite;
}

#endif


#include <stan/math/prim/mat.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>

using Eigen::Matrix;
using Eigen::Dynamic;

TEST(ProbDistributionsMultinomial,RNGSize) {
  boost::random::mt19937 rng;
  Matrix<double,Dynamic,1> theta(5);
  // error in 2.1.0 due to overflow in binomial call due to division
  theta << 0.3, 0.1, 0.2, 0.2, 0.2;
  std::vector<int> sample = stan::math::multinomial_rng(theta,10,rng);
  // bug in 2.1.0 returned 10 rather than 5 for returned size
  EXPECT_EQ(5U, sample.size());
}

TEST(ProbDistributionsMultinomial,Multinomial) {
  std::vector<int> ns;
  ns.push_back(1);
  ns.push_back(2);
  ns.push_back(3);
  Matrix<double,Dynamic,1> theta(3,1);
  theta << 0.2, 0.3, 0.5;
  EXPECT_FLOAT_EQ(-2.002481, stan::math::multinomial_log(ns,theta));
}
TEST(ProbDistributionsMultinomial,Propto) {
  std::vector<int> ns;
  ns.push_back(1);
  ns.push_back(2);
  ns.push_back(3);
  Matrix<double,Dynamic,1> theta(3,1);
  theta << 0.2, 0.3, 0.5;
  EXPECT_FLOAT_EQ(0.0, stan::math::multinomial_log<true>(ns,theta));
}

using stan::math::multinomial_log;

TEST(ProbDistributionsMultinomial, error) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  double inf = std::numeric_limits<double>::infinity();

  std::vector<int> ns;
  ns.push_back(1);
  ns.push_back(2);
  ns.push_back(3);
  Matrix<double,Dynamic,1> theta(3,1);
  theta << 0.2, 0.3, 0.5;

  EXPECT_NO_THROW(multinomial_log(ns, theta));

  ns[1] = 0;
  EXPECT_NO_THROW(multinomial_log(ns, theta));
  ns[1] = -1;
  EXPECT_THROW(multinomial_log(ns, theta), std::domain_error);
  ns[1] = 1;

  theta(0) = 0.0;
  EXPECT_THROW(multinomial_log(ns, theta), std::domain_error);
  theta(0) = nan;
  EXPECT_THROW(multinomial_log(ns, theta), std::domain_error);
  theta(0) = inf;
  EXPECT_THROW(multinomial_log(ns, theta), std::domain_error);
  theta(0) = -inf;
  EXPECT_THROW(multinomial_log(ns, theta), std::domain_error);
  theta(0) = -1;
  theta(1) = 1.5;
  theta(2) = 0.5;
  EXPECT_THROW(multinomial_log(ns, theta), std::domain_error);
  theta(0) = 0.2;
  theta(1) = 0.3;
  theta(2) = 0.5;

  ns.resize(2);
  EXPECT_THROW(multinomial_log(ns, theta), std::invalid_argument);
}

TEST(ProbDistributionsMultinomial, zeros) {
  double result;
  std::vector<int> ns;
  ns.push_back(0);
  ns.push_back(1);
  ns.push_back(2);
  Matrix<double,Dynamic,1> theta(3,1);
  theta << 0.2, 0.3, 0.5;

  result = multinomial_log(ns, theta);
  EXPECT_FALSE(std::isnan(result));

  std::vector<int> ns2;
  ns2.push_back(0);
  ns2.push_back(0);
  ns2.push_back(0);

  double result2 = multinomial_log(ns2, theta);
  EXPECT_FLOAT_EQ(0.0, result2);
}

TEST(ProbDistributionsMultinomial, error_check) {
  boost::random::mt19937 rng;

  Matrix<double,Dynamic,1> theta(3);
  theta << 0.15, 0.45, 0.40;

  EXPECT_THROW(stan::math::multinomial_rng(theta,-3,rng), std::domain_error);

  theta << 0.15, 0.45, 0.50;
  EXPECT_THROW(stan::math::multinomial_rng(theta,3,rng), std::domain_error);
}

TEST(ProbDistributionsMultinomial, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int M = 10;
  int trials = 1000;
  int N = M * trials;

  int K = 3;
  Matrix<double,Dynamic,1> theta(K);
  theta << 0.2, 0.35, 0.45;
  boost::math::chi_squared mydist(K-1);

  double expect[K];
  for (int i = 0 ; i < K; ++i)
    expect[i] = N * theta(i);

  int bin[K];
  for (int i = 0; i < K; ++i)
    bin[i] = 0;

  for (int count = 0; count < M; ++count) {
    std::vector<int> a = stan::math::multinomial_rng(theta,trials,rng);
    for (int i = 0; i < K; ++i)
      bin[i] += a[i];
  }

  double chi = 0;
  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j])) / expect[j];

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}


/*
discrete fourier transform for extension fields
*/

#ifndef DFT
#define DFT

#include <boost/operators.hpp>
#include <iostream>
#include <ostream>
#include <vector>
#include <cassert>
#include "polynomial.hpp"

using namespace std;

// class to compute the powers of an element a in an extention field,
// via a lookup table
template<class co_t>
class fieldpower{

public:
unsigned n; // max power
co_t nm;

private:
co_t a; // a is an element of order n
co_t am; // the inverse of a; also has order n
vector<co_t> powers_a; // the lookup table for the powers of a
vector<co_t> powers_am; // the lookup table for the powers of a^(-1)

public:
	fieldpower(){};
	fieldpower(const unsigned n_, const co_t& a_){
		a = a_;
		n = n_;

		nm = co_t(n);
		nm = pow( (co_t) nm,-1  );

		// initialize powers_a
		powers_a.resize( n );
		powers_a[0] = co_t(1);
		powers_a[1] =	a;
		for(int i =2;i < powers_a.size();++i)
			powers_a[i] = powers_a[i-1]*a;

		// initialize powers_am
		am = pow( (co_t) a,-1);
		powers_am.resize( n );
		powers_am[0] = co_t(1);
		powers_am[1] =	am;
		for(int i =2;i < powers_am.size();++i)
			powers_am[i] = powers_am[i-1]*am;

		cout << "filedpower: lookup tables generated.. " << endl;
	}

	co_t copow(int powe) const {
		if(powe >= 0){
			return powers_a[ powe % n ];
		} else
			return powers_am[(-powe) % n ];
	}

};

template<class co_t>
void dft_primitive( const vector<co_t>& xv, vector<co_t>& Xv, fieldpower<co_t>& fp, int prefac){
	//assert(xv.size() == Xv.size());
	for(int j=0; j < Xv.size();++j){
		// compute Xv[j]
		Xv[j] = co_t(0);
		for(int i=0;i < xv.size(); ++i){
			//Xv[j] += xv[i]*pow( (coeff_t)  a, (int)  i*j);
			if(prefac<0)
				Xv[j] += fp.nm*xv[i]*fp.copow(-i*j);
			else
				Xv[j] += xv[i]*fp.copow(i*j);
		}
	}
};

// prefac = 1: normal fft, prefac =-1: inverse fft
template<class co_t>
void fft( const vector<co_t>& xv, vector<co_t>& Xv, const fieldpower<co_t>& fp, int prefac, unsigned Q, unsigned P){
	assert(Q*P == xv.size());

	// compute y_p^(r)
	vector<vector< co_t> > Y(Q,vector< co_t >(P, co_t(0) ));
	for(unsigned r=0;r<Q;++r){
		// for each r, a Fourier transform..
		for(unsigned p=0;p<P;++p){
			for(unsigned q=0;q<Q;++q){
				Y[r][p] += xv[P*q+p]*fp.copow(prefac*P*q*r);
			}
			Y[r][p] *= fp.copow(prefac*p*r);
		}
	}
	// compute A_{Qs+r}
	for(unsigned r=0;r<Q;r++){
	for(unsigned s=0;s<P;s++){
		Xv[Q*s+r] = co_t(0);
		for(unsigned p=0;p<P;++p){
			Xv[Q*s+r] += Y[r][p]*fp.copow(prefac*Q*s*p);
		}
		if(prefac == -1) Xv[Q*s+r] *= fp.nm;
	}
	}
}

////////////////////////////////////////////////
// FFT implementaion, uses fieldpower
template<class co_t>
class DFT_FFT{
private:
fieldpower<co_t> fp;
unsigned n; // the length of the dft
unsigned P;
unsigned Q;

public:
	DFT_FFT(const unsigned n_, const co_t& a_,unsigned P_, unsigned Q_){
		P = P_;
		Q = Q_;
		fp = fieldpower<co_t>(n_,a_);
	}
	DFT_FFT(){};

	void dft( const vector<co_t>& xv, vector<co_t>& Xv) const {
		fft<co_t>(xv,Xv,fp,1,P,Q);
	};

	void idft(vector<co_t>& xv, const vector<co_t>& Xv) const {
		fft<co_t>(Xv,xv,fp,-1,P,Q);
	}

};

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

// DFT implementation of certain lenth that uses a precomputed lookup table to compute the powers of a
template<class co_t>
class DFT_LA{
private:
co_t a; // the Fourier kernal
co_t am;
unsigned n; // the length of the dft
co_t nm;
vector<co_t> powers_a; // the lookup table for the powers of a
vector<co_t> powers_am; // the lookup table for the powers of a^(-1)

public:
	DFT_LA(const unsigned n_, const co_t& a_){
		a = a_;
		n = n_;

		nm = co_t(n);
		nm = pow( (co_t) nm,-1  );

		// initialize powers_a
		powers_a.resize( ((n-1)*(n-1)+1) );
		powers_a[0] = co_t(1);
		powers_a[1] =	a;
		for(int i =2;i < powers_a.size();++i)
			powers_a[i] = powers_a[i-1]*a;

		// initialize powers_am
		am = pow( (co_t) a,-1);
		powers_am.resize( ((n-1)*(n-1)+1) );
		powers_am[0] = co_t(1);
		powers_am[1] =	am;
		for(int i =2;i < powers_am.size();++i)
			powers_am[i] = powers_am[i-1]*am;

		cout << "lookup tables generated.. " << endl;
	}

	void dft( vector<co_t>& xv, vector<co_t>& Xv) const {
		//assert(xv.size() == Xv.size());
		for(int j=0; j < Xv.size();++j){
			// compute Xv[j]
			Xv[j] = co_t(0);
			for(int i=0;i < xv.size(); ++i){
				//Xv[j] += xv[i]*pow( (coeff_t)  a, (int)  i*j);
				Xv[j] += xv[i]*powers_a[i*j];
			}
		}
	};

	void idft(vector<co_t>& xv, vector<co_t>& Xv) const {
		assert(xv.size() == Xv.size());
		for(int j=0; j < xv.size();++j){
			// compute Xv[j]
			//cout << "adf"<< endl;
			xv[j] = co_t(0);
			//cout << j << endl;
			for(int i=0;i < Xv.size(); ++i){
				//cout << "--" << i << endl;
				//cout << i << "-- Xv: " <<  Xv[i] << " powers " << powers_am[i*j] << endl;
				//cout << "-- xv: " << xv[j] << endl;
				xv[j] += nm*Xv[i]*powers_am[i*j];
			}
		}
	}

};

// primitive DFT
template<class co_t>
class DFT_PRIM{
private:
co_t a; // the Fourier kernal
co_t am;
unsigned n; // the length of the dft
co_t nm;

public:
	DFT_PRIM(){};
	DFT_PRIM(const unsigned n_, const co_t& a_){
		a = a_;
		n = n_;

		nm = co_t(n);
		nm = pow( (co_t) nm,-1  );
	}

	void dft( vector<co_t>& xv, vector<co_t>& Xv) const {

		//assert(xv.size() == Xv.size());
		for(int j=0; j < Xv.size();++j){
			// compute Xv[j]
			Xv[j] = co_t(0);
			for(int i=0;i < xv.size(); ++i){
				//Xv[j] += xv[i]*pow( (coeff_t)  a, (int)  i*j);
				Xv[j] += xv[i]*pow( (co_t) a, i*j);
			}
		}
	};

	void idft(vector<co_t>& xv, vector<co_t>& Xv) const {
		for(int j=0; j < Xv.size();++j){
			// compute Xv[j]
			xv[j] = co_t(0);
			for(int i=0;i < xv.size(); ++i)
				xv[j] += nm*Xv[i]*pow( (co_t) a,i*j);
		}
	}

};

// to do: implement the faster FFT
template<class coeff_t>
void dft( vector<coeff_t>& xv, vector<coeff_t>& Xv, coeff_t a ){

	cout << "startcomplookup" << endl;
	unsigned nn = Xv.size();
	//compute lookup table
	vector<coeff_t> powers( ((nn-1)*(nn-1)+1) );
	powers[0] = coeff_t(1);
	powers[1] =	a;
	for(int i =2;i < powers.size();++i)
		powers[i] = powers[i-1]*a;
	cout << "finishcomplookup" << endl;

	assert(xv.size() == Xv.size());
	for(int j=0; j < Xv.size();++j){
		// compute Xv[j]
		Xv[j] = coeff_t(0);
		for(int i=0;i < xv.size(); ++i){
			//Xv[j] += xv[i]*pow( (coeff_t)  a, (int)  i*j);
			Xv[j] += xv[i]*powers[i*j];
		}
	}
};

// nm: n^-1 ,where n is the lenght of the dft
// am: a^-1, where a is an element of order n
/*
template <class coeff_t>
void idft(vector<coeff_t>& xv, vector<coeff_t>& Xv,coeff_t am, coeff_t nm ){
	assert(xv.size() == Xv.size());
	for(int j=0; j < Xv.size();++j){
		// compute Xv[j]
		xv[j] = coeff_t(0);
		for(int i=0;i < xv.size(); ++i)
			xv[j] += nm*Xv[i]*pow( (coeff_t) am, (int)  i*j);
	}

}
*/

template <class coeff_t>
void idft(vector<coeff_t>& xv, vector<coeff_t>& Xv,coeff_t a ){
	assert(xv.size() == Xv.size());

	unsigned nn = xv.size();
	coeff_t n = coeff_t(nn);
	cout << "n: " << n << endl;
	n = pow(n,-1);

	//compute lookup table
	cout << "startcomplookup" << endl;
	coeff_t am = pow( (coeff_t) a,-1);
	vector<coeff_t> powers( ((nn-1)*(nn-1)+1) );
	powers[0] = coeff_t(1);
	powers[1] =	am;
	for(int i =2;i < powers.size();++i)
		powers[i] = powers[i-1]*am;

	cout << "finishlookuptable.. " << endl;

	for(int j=0; j < Xv.size();++j){
		// compute Xv[j]
		xv[j] = coeff_t(0);
		for(int i=0;i < xv.size(); ++i){

			//xv[j] += n*Xv[i]*pow( (coeff_t) am, (int)  i*j);
			xv[j] += n*Xv[i]*powers[i*j];
		}
	}

}

#endif


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#define BOOST_TEST_MODULE utility

#include <iostream>
#include <boost/units/io.hpp>
#include <boost/units/systems/angle/degrees.hpp>

#include <boost/astronomy/coordinate/coord_sys/horizon_coord.hpp>
#include <boost/astronomy/coordinate/coord_sys/ecliptic_coord.hpp>
#include <boost/astronomy/coordinate/coord_sys/galactic_coord.hpp>
#include <boost/astronomy/coordinate/coord_sys/equatorial_ra_coord.hpp>
#include <boost/astronomy/coordinate/coord_sys/equatorial_ha_coord.hpp>

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/astronomy/coordinate/utility/utility.hpp>

#include <boost/astronomy/time/parser.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/astronomy/time/time_conversions.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include <boost/test/unit_test.hpp>

using namespace boost::units;
using namespace boost::units::si;
using namespace boost::astronomy::coordinate;

namespace bud = boost::units::degree;
namespace bac = boost::astronomy::coordinate;

using namespace boost::numeric::ublas;
namespace bnu = boost::numeric::ublas;

using namespace boost::gregorian;
using namespace boost::posix_time;

BOOST_AUTO_TEST_SUITE(utility)

BOOST_AUTO_TEST_CASE(column_vector) {

  quantity<bud::plane_angle, double> u = 97.638119 * bud::degree;
  quantity<bud::plane_angle, double> v = -17.857969 * bud::degree;

  bac::column_vector<double, quantity<bud::plane_angle>, double> vec(u,v);

  BOOST_CHECK_CLOSE(vec.get()(0,0), -0.126512, 0.001);
  BOOST_CHECK_CLOSE(vec.get()(1,0), 0.943374, 0.001);
  BOOST_CHECK_CLOSE(vec.get()(2,0), -0.306658, 0.001);
}

BOOST_AUTO_TEST_CASE(ha_dec_horizon) {

  /**
   * What are the Horizon Coordinated (Altitude and Azimuth)
   * of a star whose Hour Angle is 5h 51m 44s and
   * Declination is +23◦ 13′ 10′′?
   * The observer’s latitude is 52◦ N.
   */

  double ha = decimal_hour(5,51,44).get() * 15.0;
  double declination = 23.21944444;
  quantity<bud::plane_angle, double> phi1 = 52.0 * bud::degree;

  equatorial_ha_coord<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>>
      eha(ha * bud::degrees, declination * bud::degrees);

  bac::column_vector<double, quantity<bud::plane_angle>, double> vec1(eha.get_ha(),eha.get_dec());

  matrix<double> resultant_vector1 = prod(bac::ha_dec_horizon<double, quantity<bud::plane_angle>, double>(phi1).get(),vec1.get());

  auto coordinates1 = bac::extract_coordinates(resultant_vector1).get_coordinates();
  auto theta1 = coordinates1.first;
  auto gama1 = coordinates1.second;

  BOOST_CHECK_CLOSE(theta1.value() * 180.0 / PI, -76.728973, 0.001);
  BOOST_CHECK_CLOSE(gama1.value() * 180.0 / PI, 19.33434444, 0.001);

  /**
   * What are the Equatorial Coordinates (Hour Angle and Declination)
   * of a star that is observed by an observer at latitude 52◦ N to
   * have an Altitude of 19◦ 20′ 03.64′′ and an Azimuth of 283◦ 16′ 15.7′
   */

  double altitude_a = 19.334344;
  double azimuth_A = 283.271028;
  quantity<bud::plane_angle, double> phi2 = 52.0 * bud::degree;

  horizon_coord<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>>
      hc(altitude_a * bud::degrees, azimuth_A * bud::degrees);

  bac::column_vector<double, quantity<bud::plane_angle>, double> vec2(hc.get_azimuth(),hc.get_altitude());

  matrix<double> resultant_vector2 = prod(bac::ha_dec_horizon<double, quantity<bud::plane_angle>, double>(phi2).get(),vec2.get());

  auto coordinates2 = bac::extract_coordinates(resultant_vector2).get_coordinates();
  auto theta2 = coordinates2.first;
  auto gama2 = coordinates2.second;

  BOOST_CHECK_CLOSE(theta2.value() * 180.0 / PI, 87.933334, 0.001);
  BOOST_CHECK_CLOSE(gama2.value() * 180.0 / PI, 23.219444, 0.001);
}

BOOST_AUTO_TEST_CASE(ha_dec_ra_dec) {

  /**
   * What was the Local Hour Angle of a star whose Right Ascension
   * was 18h 32m 21s on local calendar date 22 April 1980 when
   * observed in time zone −4 h from
   * Longitude 64◦ W at local time 14h 36m 51.67s?
   */
  double ra = decimal_hour(18,32,21).get() * 15.0;
  double declination = 23.21944444;

  std::string ts1("1980-04-22 14:36:51.67");
  ptime t1(time_from_string(ts1));

  decimal_hour d1 = LST(64,DIRECTION::WEST, t1);

  quantity<bud::plane_angle, double> ST = (d1.get() + 4) * 15 * bud::degree;

  equatorial_ra_coord<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>>
      era(ra * bud::degrees, declination * bud::degrees);

  bac::column_vector<double, quantity<bud::plane_angle>, double> vec1(era.get_ra(),era.get_dec());

  matrix<double> resultant_vector1 = prod(bac::ha_dec_ra_dec<double, quantity<bud::plane_angle>, double>(ST).get(),vec1.get());

  auto coordinates1 = bac::extract_coordinates(resultant_vector1).get_coordinates();
  auto theta1 = coordinates1.first;
  auto gama1 = coordinates1.second;

  //Hour Angle converted from degree to hour
  BOOST_CHECK_CLOSE((theta1.value() * 180.0 / PI) / 15.0, 9.873239, 1);
  BOOST_CHECK_CLOSE(gama1.value() * 180.0 / PI, 23.219444, 0.001);

  /**
   * What was the Right Ascension of a star whose Local Hour Angle
   * was 9h 52m 23.66s on local calendar date 22 April 1980 when
   * observed in time zone −4 h from
   * Longitude 64◦ W at local time 14h 36m 51.67s?
   */
  double ha = decimal_hour(9,52,23.66).get() * 15.0;

  equatorial_ha_coord<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>>
      eha(ha * bud::degrees, declination * bud::degrees);

  bac::column_vector<double, quantity<bud::plane_angle>, double> vec2(eha.get_ha(),eha.get_dec());

  matrix<double> resultant_vector2 = prod(bac::ha_dec_ra_dec<double, quantity<bud::plane_angle>, double>(ST).get(),vec2.get());

  auto coordinates2 = bac::extract_coordinates(resultant_vector2).get_coordinates();
  auto theta2 = coordinates2.first;
  auto gama2 = coordinates2.second;

  // Right Ascension converted from degree to hour
  // If Right Ascension negative, add 24.
  long double theta = (theta2.value() * 180.0 / PI) / 15.0;
  long double ra_result = ( theta ) < 0 ? theta + 24.0 : theta;

  BOOST_CHECK_CLOSE(ra_result, 18.539165, 1);
  BOOST_CHECK_CLOSE(gama2.value() * 180.0 / PI, 23.219444, 0.001);
}

BOOST_AUTO_TEST_CASE(ecliptic_to_ra_dec) {

  /**
   * What were the Right Ascension and the declination of a planet
   * whose ecliptic coordinates were longitude 139◦ 41′ 10′′ and
   * latitude 4◦ 52′ 31′′ on 6 July 2009?
   */
  double longitude = 139.6861111;
  double latitude = 4.87527778;

  std::string s("2009-07-6");
  date d(from_simple_string(s));

  auto obliquity = obliquity_of_ecliptic(d).get();
  BOOST_CHECK_CLOSE(obliquity.value() * 180.0 / PI, 23.43805531 , 0.001);

  ecliptic_coord<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>>
      ec(longitude * bud::degrees, latitude * bud::degrees);

  bac::column_vector<double, quantity<bud::plane_angle>, double> vec(ec.get_lat(),ec.get_lon());

  matrix<double> resultant_vector1 = prod(bac::ecliptic_to_ra_dec<>(obliquity).get(),vec.get());

  auto coordinates = bac::extract_coordinates(resultant_vector1).get_coordinates();
  auto theta = coordinates.first;
  auto gama = coordinates.second;

  BOOST_CHECK_CLOSE((theta.value() * 180.0 / PI), 143.722173, 0.001);
  BOOST_CHECK_CLOSE(gama.value() * 180.0 / PI, 19.535003, 0.001);
}

BOOST_AUTO_TEST_CASE(ra_dec_to_ecliptic) {
  /**
   * What are the Ecliptic Coordinates of a planet whose
   * Right Ascension and Declination are given as
   * α = 9h 34m 53.32s and δ = 19◦ 32′ 6.01′′
   * when the Greenwich calendar date is 6July2009?
   */

  double ra = decimal_hour(9,34,53.32).get() * 15.0;
  double declination = 19.535003;

  std::string s("2009-07-6");
  date d(from_simple_string(s));

  auto obliquity = obliquity_of_ecliptic(d).get();
  BOOST_CHECK_CLOSE(obliquity.value() * 180.0 / PI, 23.43805531 , 0.001);

  equatorial_ra_coord<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>>
      era(ra * bud::degrees, declination * bud::degrees);

  bac::column_vector<double, quantity<bud::plane_angle>, double> vec(era.get_ra(),era.get_dec());

  matrix<double> resultant_vector = prod(bac::ra_dec_to_ecliptic<>(obliquity).get(),vec.get());

  auto coordinates = bac::extract_coordinates(resultant_vector).get_coordinates();
  auto theta = coordinates.first;
  auto gama = coordinates.second;

  BOOST_CHECK_CLOSE((theta.value() * 180.0 / PI), 139.686106, 0.001);
  BOOST_CHECK_CLOSE(gama.value() * 180.0 / PI, 4.875276, 0.001);
}

BOOST_AUTO_TEST_CASE(ra_dec_to_galactic) {

  /**
   * What are the Galactic Coordinates of a star whose
   * right ascension and declination are
   * α = 10h 21m 00s and δ = 10◦ 03′ 11′′?
   */

  double ra = decimal_hour(10,21,0).get() * 15.0;
  double declination = 10.053056;

  equatorial_ra_coord<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>>
      era(ra * bud::degrees, declination * bud::degrees);

  bac::column_vector<double, quantity<bud::plane_angle>, double> vec(era.get_ra(),era.get_dec());

  matrix<double> resultant_vector = prod(bac::ra_dec_to_galactic<double>().get(),vec.get());

  auto coordinates = bac::extract_coordinates(resultant_vector).get_coordinates();
  auto theta = coordinates.first;
  auto gama = coordinates.second;

  long double longitude_result = theta.value() * 180.0 / PI;
  long double longitude = ( longitude_result ) < 0 ? longitude_result + 360.0 : longitude_result;

  BOOST_CHECK_CLOSE(longitude, 232.247881, 0.001);
  BOOST_CHECK_CLOSE(gama.value() * 180.0 / PI, 51.122268, 0.001);
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by Vlad Argunov on 17/01/2022.
//

#include "OptimalExecution.h"

#include <iostream>
#include <Eigen/Dense>
#include <random>
#include <string>
#include <cmath>
#include "Simulation.h"

OptimalExecution::OptimalExecution(double a_coeff_, double b_coeff_, double sigma_coeff_, double k_coeff_, double phi_coeff_) {
    a_coeff = a_coeff_;
    b_coeff = b_coeff_;
    sigma_coeff = sigma_coeff_;
    k_coeff = k_coeff_;
    phi_coeff = phi_coeff_;
}

void OptimalExecution::compute_liquidation(const std::string& trading_speed, double impact_nonlinearity) {
    int num_rows = num_diff_q + 1;
    int num_cols = num_diff_t + 1;
    sim = Simulation();
    value_matrix.resize(num_rows, num_cols);
    optimal_speed_matrix.resize(num_rows, num_cols);
    double diff_q = 1.0 / num_diff_q;
    double diff_t = 1.0 / num_diff_t;
    double quantity;
    double h_diff_q_up;
    double h_diff_q_down;
    double partial_diff_q;

    Eigen::VectorXd diffusion; diffusion.resize(num_cols - 1);
    diffusion = sim.generate_normal_variable(num_cols - 1, 0, pow(diff_t, 2));

    stock.resize(num_cols); inventory.resize(num_cols);
    cash.resize(num_cols); optimal_speed.resize(num_cols);
    value_full.resize(num_cols);

    if (trading_speed == "Nonlinear" || trading_speed == "Linear") {

        // That part computes the matrix of value_matrix function for each value of time and quantity.
        for (int col = (num_cols - 1); col > -1; --col) { // Time space
            for (int row = 0; row < num_rows; ++row) { // Stock space
                quantity = row * diff_q;

                if (trading_speed == "Linear"){
                    double gamma = sqrt(phi_coeff / k_coeff);
                    double zeta = (a_coeff - 0.5 * b_coeff + sqrt(k_coeff * phi_coeff)) / (a_coeff - 0.5 * b_coeff - sqrt(k_coeff * phi_coeff));
                    double remaining_time = (num_cols - col) * diff_t;
                    optimal_speed_matrix(row, col) = gamma * (zeta * exp(gamma * remaining_time) + exp(- gamma * remaining_time)) / (zeta * exp(gamma) - exp(- gamma));
                }

                if (col == (num_cols - 1)){
                    value_matrix(row, col) = - a_coeff * quantity * quantity;

                    if (trading_speed == "Nonlinear") {
                        optimal_speed_matrix(row, col) = pow(abs(-(b_coeff * quantity - 2 * a_coeff * quantity) / ((1 + a_coeff) * k_coeff))
                                ,1.0 / impact_nonlinearity);
                    }
                } else {

                    if (row == 0){
                        h_diff_q_up =  value_matrix(row + 1, col + 1);
                        h_diff_q_down =  value_matrix(row, col + 1);
                        partial_diff_q = (h_diff_q_up - h_diff_q_down) / (diff_q);
                    } else if (row == (num_rows - 1)) {
                        h_diff_q_up =  value_matrix(row, col + 1);
                        h_diff_q_down =  value_matrix(row - 1, col + 1);
                        partial_diff_q = (h_diff_q_up - h_diff_q_down) / (diff_q);
                    } else {
                        h_diff_q_up =  value_matrix(row + 1, col + 1);
                        h_diff_q_down =  value_matrix(row - 1, col + 1);
                        partial_diff_q = (h_diff_q_up - h_diff_q_down) / (2 * diff_q);
                    }

                    value_matrix(row, col) = value_matrix(row, col + 1) -
                            diff_t * (phi_coeff * pow(quantity, 2) - a_coeff * k_coeff *
                            pow(abs(-(b_coeff * quantity + partial_diff_q ) / ((1 + a_coeff) * k_coeff))
                                ,1.0 + 1.0/ impact_nonlinearity));

                    if (trading_speed == "Nonlinear"){
                        optimal_speed_matrix(row, col) = pow(abs(-(b_coeff * quantity + partial_diff_q) / ((1 + a_coeff) * k_coeff))
                                ,1.0 / impact_nonlinearity);
                    }

                }
            }
        }

        // That part simulates stock, cash, and inventory processes
        for (int step = 0; step < num_cols; ++step) {
            if (step == 0){
                inventory(step) = 1;
                stock(step) = 1;
                cash(step) = 0;

                optimal_speed(step) = optimal_speed_matrix(num_rows - 1, step);
                value_full(step) = cash(step) + inventory(step) * stock(step) + value_matrix(num_rows - 1, step);

            } else {
                inventory(step) = std::max(inventory(step - 1) - optimal_speed(step - 1) * diff_t, 0.0);
                stock(step) = stock(step - 1) - b_coeff * optimal_speed(step - 1) * diff_t
                        + sigma_coeff * diffusion(step - 1);
                cash(step) = cash(step - 1) + (stock(step - 1)
                        - k_coeff * pow(optimal_speed(step - 1), impact_nonlinearity)) * optimal_speed(step - 1) * diff_t;

                // Determine the relevant trading speed and value function
                for (int row = 0; row < num_rows; ++row) {
                    if ((row * diff_q <= inventory(step)) && ((row + 1) * diff_q > inventory(step))){
                        if (row != (num_rows - 1)){
                            optimal_speed(step) =
                                    (((row + 1) * diff_q - inventory(step)) / diff_q) * optimal_speed_matrix(row, step) +
                                    ((inventory(step) - row * diff_q) / diff_q) * optimal_speed_matrix(row + 1, step);

                            value_full(step) = cash(step) + inventory(step) * stock(step) +
                                    (((row + 1) * diff_q - inventory(step)) / diff_q) * value_matrix(row, step) +
                                    ((inventory(step) - row * diff_q) / diff_q) * value_matrix(row + 1, step);
                        } else {
                            optimal_speed(step) = optimal_speed_matrix(num_rows - 1, step);
                            value_full(step) = cash(step) + inventory(step) * stock(step) + value_matrix(num_rows - 1, step);
                        }

                    }
                }
            }
        }
    }

    if (inventory(num_cols - 1) > 0.0){
        cash(num_cols - 1) += inventory(num_cols - 1) * (stock(num_cols - 1) - a_coeff * inventory(num_cols - 1));
    }
    }

void OptimalExecution::set_num_steps(int num_diff_t_, int num_diff_q_) {
    num_diff_t = num_diff_t_;
    num_diff_q = num_diff_q_;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2019, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_UTILS_MATH_HPP_
#define CROCODDYL_CORE_UTILS_MATH_HPP_

#include <Eigen/Dense>
#include <algorithm>
#include <limits>

template <typename MatrixType>
MatrixType pseudoInverse(const MatrixType& a, double epsilon = std::numeric_limits<double>::epsilon()) {
  Eigen::JacobiSVD<MatrixType> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
  double tolerance =
      epsilon * static_cast<double>(std::max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
  return svd.matrixV() *
         (svd.singularValues().array().abs() > tolerance)
             .select(svd.singularValues().array().inverse(), 0)
             .matrix()
             .asDiagonal() *
         svd.matrixU().adjoint();
}

#endif  // CROCODDYL_CORE_UTILS_MATH_HPP_


#include "ros_utils.hpp"
#include "kinematics.hpp"
#include <boost/array.hpp>

#include <casadi/casadi.hpp>

#include <mhe_estimator/CanData.h>
#include <mhe_estimator/ArticulatedAngles.h>
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "ackermann_msgs/AckermannDrive.h"

namespace mhe_estimator
{
    using namespace casadi;

    void mheSetupCar(casadi::Function& solver, const mhe_estimator::CarParams& params, const mhe_estimator::MheParams& mheParams,const long unsigned int N_mhe)
    {

        double A = N_mhe;
        double B = N_mhe+1;
        SX theta = SX::sym("theta"); SX x = SX::sym("x"); SX y = SX::sym("y"); SX beta0 = SX::sym("beta0");
        SX u1 = SX::sym("u1"); SX u2 = SX::sym("u2");

        SX states = vertcat(theta, x, y, beta0);
        SX controls = vertcat(u1, u2);  //u1 =dbeta     u2= linear vel
        unsigned int n_states = states.size1(); unsigned int n_controls = controls.size1();

        SX rhs = vertcat(u2*(1/params.L)*tan(beta0), u2*cos(theta), u2*sin(theta), u1); //system r.h.s  theta, x, y, beta0
        Function f = Function("f",{states,controls}, {rhs}); //nonlinear mapping function f(x,u)

        SX U = SX::sym("U",n_controls,A);     //A = N_MHE
        SX X = SX::sym("X",n_states,B);       //B = N_MHE +1
        SX P = SX::sym("P",n_states,B+A+B+A); //state,control,covOfState,covOfControl

        SX obj = 0;
        SX g;

        for(int k = 0; k < B; k++) // Create states objective function
        {
            SX h_x = vertcat(X(0,k), X(1,k), X(2,k), X(3,k));
            SX y_tilde = vertcat(P(0,k), P(1,k), P(2,k), P(3,k));
            SX V = SX::zeros(n_states,n_states);
            V(0,0) = P(0,B+A+k);
            V(1,1) = P(1,B+A+k);
            V(2,2) = P(2,B+A+k);
            V(3,3) = P(3,B+A+k);
            obj = obj + mtimes(mtimes(((y_tilde - h_x).T()), V), (y_tilde - h_x));
        }
        for(int k = 0; k < A; k++) // Create control objective function
        {
            SX con = vertcat(U(0,k), U(1,k));
            SX u_tilde = vertcat(P(0,B+k), P(1,B+k));
            SX W = SX::zeros(n_controls,n_controls);
            W(0,0) = P(0,B+A+B+k);
            W(1,1) = P(1,B+A+B+k);
            obj = obj + mtimes(mtimes(((u_tilde - con).T()), W), (u_tilde - con));
        }

        for(int k = 0; k < A; k++) //  multiple shooting constraints
        {
            SX con = vertcat(U(0,k), U(1,k));
            SX st = vertcat(X(0,k), X(1,k), X(2,k), X(3,k));
            SX st_next = vertcat(X(0,k+1), X(1,k+1), X(2,k+1), X(3,k+1));
            SXVector func = f(SXVector{st,con});
            SX f_value = SX::vertcat(func);
            SX st_next_euler = st + ((1/mheParams.loopRate)*f_value);
            g = vertcat(g, (st_next-st_next_euler));
        }

        SX OPT_variables; //optimization variable

        for(int j = 0; j < X.size2(); j++) //reshape X and apend
        {
            for(int i = 0; i < X.size1(); i++)
            {
                OPT_variables = vertcat(OPT_variables, X(i,j));
            }
        }
        for(int j = 0; j < U.size2(); j++) //reshape U and apend
        {
            for(int i = 0; i < U.size1(); i++)
            {
                OPT_variables = vertcat(OPT_variables, U(i,j));
            }
        }

        SXDict nlp = {{"f", obj}, {"x", OPT_variables}, {"g", g}, {"p", P}};
        Dict opts;
        /*
        opts["qpsol"] = "qpoases";
//      opts["max_iter"] =2;

        opts["print_iteration"] = true;
        Dict qopts;
        qopts["sparse"]=true;

        opts["qpsol_options"] = qopts;

        */
        opts["ipopt.max_iter"] = 2000;
        opts["ipopt.print_level"] = 0;
        opts["print_time"] = false;
        opts["ipopt.acceptable_tol"] = 1e-6;
        opts["ipopt.acceptable_obj_change_tol"] = 1e-4;
        opts["ipopt.print_timing_statistics"] = "no";
        opts["ipopt.print_info_string"] = "no";

        solver = nlpsol("nlpsol", "ipopt", nlp, opts);

        //solver = nlpsol("nlpsol", "sqpmethod", nlp, opts);

    }

    void mheSetupTrailer(casadi::Function& solver,const mhe_estimator::CarParams& params,const mhe_estimator::MheParams& mheParams,const long unsigned int N_mhe)
    {
        double A = N_mhe;
        double B = N_mhe+1;
        SX beta1 = SX::sym("beta1"); SX theta = SX::sym("theta"); SX x = SX::sym("x"); SX y = SX::sym("y"); SX beta0 = SX::sym("beta0");
        SX u1 = SX::sym("u1"); SX u2 = SX::sym("u2");

        SX states1 = vertcat(beta1,theta, x);
        SX states2 = vertcat(y, beta0);
        SX states = vertcat(states1, states2); // due to casadi limitation
        SX controls = vertcat(u1, u2);  //u1 =dbeta     u2= linear vel
        unsigned int n_states = states.size1(); unsigned int n_controls = controls.size1();

        SX rhs;
        SX k1 = (1/params.L1)*tan(beta1 - atan((params.Lh1/params.L)*tan(beta0)));
        if(!params.moveGuidancePoint)  //OneTrailerKinematicsGPRear
        {
            SX dq0 = u2 * (sin(beta1)/params.Lh1 - (1 + (params.L1/params.Lh1)*cos(beta1))*(k1)); //beta1
            SX dq1 = u2 * k1; //theta
            SX dq2 = u2 * cos(theta); //x
            SX dq3 = u2 * sin(theta); //y
            SX dq4 = u1; //beta0

            SX rhs1 = vertcat(dq0, dq1, dq2); //due to casadi limitation
            SX rhs2 = vertcat(dq3, dq4);
            rhs = vertcat(rhs1, rhs2); //system r.h.s
        }
        else    //OneTrailerKinematicsGPFront
        {
            SX dq0 = u2 * (sin(beta1)/params.Lh1 - (1 + (params.L1/params.Lh1)*cos(beta1))*k1); //beta1
            SX dq1 = u2 * ( -(params.L1/params.Lh1)*cos(beta1)*k1 + sin(beta1)/params.Lh1 ); //theta
            SX dq2 = u2 * cos(theta) * ( params.L1*sin(beta1)*k1 + cos(beta1) ); //x
            SX dq3 = u2 * sin(theta) * ( params.L1*sin(beta1)*k1 + cos(beta1) ); //y
            SX dq4 = u1; //beta0

            SX rhs1 = vertcat(dq0, dq1, dq2); //due to casadi limitation
            SX rhs2 = vertcat(dq3, dq4);
            rhs = vertcat(rhs1, rhs2); //system r.h.s
        }

        //ROS_INFO_STREAM("rhstRAILER: " << rhs <<" ");

        Function f = Function("f",{states,controls}, {rhs}); //nonlinear mapping function f(x,u)

        SX U = SX::sym("U",n_controls,A); //A = N_MHE
        SX X = SX::sym("X",n_states,B); //B = N_MHE +1
        SX P = SX::sym("P",n_states,B+A+B+A);

        SX obj = 0;
        SX g;

        for(int k = 0; k < B; k++) // Create states objective function
        {
            SX h_x1 = vertcat(X(0,k), X(1,k), X(2,k));
            SX h_x2 = vertcat(X(3,k), X(4,k) );
            SX h_x = vertcat(h_x1, h_x2);

            SX y_tilde1 = vertcat(P(0,k), P(1,k), P(2,k));
            SX y_tilde2 = vertcat(P(3,k), P(4,k));
            SX y_tilde = vertcat(y_tilde1, y_tilde2);

            SX V = SX::zeros(n_states,n_states);
            V(0,0) = P(0,B+A+k);
            V(1,1) = P(1,B+A+k);
            V(2,2) = P(2,B+A+k);
            V(3,3) = P(3,B+A+k);
            V(4,4) = P(4,B+A+k);
            obj = obj + mtimes(mtimes(((y_tilde - h_x).T()), V), (y_tilde - h_x));
        }
        for(int k = 0; k < A; k++) // Create control objective function
        {
            SX con = vertcat(U(0,k), U(1,k));
            SX u_tilde = vertcat(P(0,B+k), P(1,B+k));
            SX W = SX::zeros(n_controls,n_controls);
            W(0,0) = P(0,B+A+B+k);
            W(1,1) = P(1,B+A+B+k);
            obj = obj + mtimes(mtimes(((u_tilde - con).T()), W), (u_tilde - con));
        }

        for(int k = 0; k < A; k++) //  multiple shooting constraints
        {
            SX con = vertcat(U(0,k), U(1,k));

            SX st1 = vertcat(X(0,k), X(1,k), X(2,k));
            SX st2 = vertcat(X(3,k), X(4,k));
            SX st = vertcat(st1,st2);

            SX st_next1 = vertcat(X(0,k+1), X(1,k+1), X(2,k+1));
            SX st_next2 = vertcat(X(3,k+1), X(4,k+1));
            SX st_next = vertcat(st_next1, st_next2);

            SXVector func = f(SXVector{st,con});
            SX f_value = SX::vertcat(func);
            SX st_next_euler = st + ((1/mheParams.loopRate)*f_value);
            g = vertcat(g, (st_next-st_next_euler));
        }

        //ROS_INFO_STREAM("g " << g <<" ");

        SX OPT_variables; //optimization variable

        for(int j = 0; j < X.size2(); j++) //reshape X and apend
        {
            for(int i = 0; i < X.size1(); i++)
            {
                OPT_variables = vertcat(OPT_variables, X(i,j));
            }
        }
        for(int j = 0; j < U.size2(); j++) //reshape U and apend
        {
            for(int i = 0; i < U.size1(); i++)
            {
                OPT_variables = vertcat(OPT_variables, U(i,j));
            }
        }
        //ROS_INFO_STREAM("g.size " << g.size1() <<" ");

        SXDict nlp = {{"f", obj}, {"x", OPT_variables}, {"g", g}, {"p", P}};
        Dict opts;

//        opts["qpsol"] = "gurobi";
//        opts["hessian_approximation"] = "limited-memory";

//        opts["max_iter"] =10;

//        opts["print_iteration"] = true;
//        opts["warn_initial_bounds"] = true;
//        Dict qopts;
//       qopts["gurobi.Threads"]=4;
//       qopts["verbose"]=false;

////        qopts["sparse"]=true;
////         qopts["printLevel"]="low";
//        qopts["linsol_plugin"]="gurobi";

//        opts["qpsol_options"] = qopts;
//       opts["qpsol_options"] = qopts;

        opts["ipopt.max_iter"] = 2000;
        opts["ipopt.print_level"] = 0;
        opts["print_time"] = false;
        opts["ipopt.acceptable_tol"] = 1e-6;
        opts["ipopt.acceptable_obj_change_tol"] = 1e-4;
        opts["ipopt.print_timing_statistics"] = "no";
        opts["ipopt.print_info_string"] = "no";

        solver = nlpsol("nlpsol", "ipopt", nlp, opts);
    }

  ////////////////////////////////////////////////////////////////////////////////////////////////////////
    template <class T, class T2, long unsigned int A, long unsigned int B>
    void estimateMhe(casadi::DM& argx0, const boost::array<T, B>& q4wLoc,
        const boost::array<T2, A>& control2w,const boost::array<T, B>& q4wCov,
        const boost::array<T2, A>& control2wCov, const mhe_estimator::CarParams& carParams,
        const mhe_estimator::MheParams& mheParams,const casadi::Function& solver)
    {
        unsigned int n_states = 4; unsigned int n_controls = 2;
        std::map<std::string, DM> arg, res;
        arg["lbg"] = SX::zeros(n_states*A); //size = n_state * N_Mhe
        arg["ubg"] = SX::zeros(n_states*A);

        std::vector<double> lbx((n_controls*A)+(n_states*B), 0.0);
        std::vector<double> ubx((n_controls*A)+(n_states*B), 0.0);

        for(int i = 0; i < (n_states*B); i += n_states)
        {
            lbx[i] = carParams.lowerTh;  //theta lower bound
            lbx[i+1]   = carParams.lowerX;    //x lower bound
            lbx[i+2] = carParams.lowerY;  //y lower bound
            lbx[i+3] = -carParams.steeringLimit;

            ubx[i] = carParams.upperTh;   //theta upper bound
            ubx[i+1]   = carParams.upperX;    //x upper bound
            ubx[i+2] = carParams.upperY;  //y upper bound
            ubx[i+3] = carParams.steeringLimit; //beta upper bound

        }
        for(int i = ((n_states*B)); i < ((n_controls*A)+(n_states*B)); i += n_controls)
        {
            lbx[i] = -carParams.SteeringVelLimit;    //v lower bound
            lbx[i+1] = -carParams.LinearVelLimit;  //beta lower bound

            ubx[i] = carParams.SteeringVelLimit;    //v upper bound
            ubx[i+1] = carParams.LinearVelLimit;  //beta upper bound

        }
        arg["lbx"] = lbx;
        arg["ubx"] = ubx;
        //--------------ALL OF THE ABOVE IS JUST A PROBLEM SET UP-------------//

        DM argp = SX::zeros(n_states,B+A+B+A);
        for(int j = 0; j < B; j++) //measured states
        {
            boost::array<double, 4> qloc = q4wLoc[j];
            argp(0,j) = qloc[0];
            argp(1,j) = qloc[1];
            argp(2,j) = qloc[2];
            argp(3,j) = qloc[3];
        }
        for(int j = 0; j < A; j++) //measured controls
        {
            boost::array<double, 2> qctr = control2w[j];
            //argp(0,j + B) = 0.0;   //we do not have measured control dbeta
            argp(1,B+j) = qctr[1];
        }
        for(int j = 0; j < B; j++) //state cov matrix
        {
            boost::array<double, 4> qCov = q4wCov[j];
            argp(0,B+A+j) = qCov[0];
            argp(1,B+A+j) = qCov[1];
            argp(2,B+A+j) = qCov[2];
            argp(3,B+A+j) = qCov[3];
        }
        for(int j = 0; j < A; j++) //control cov matrix
        {
            boost::array<double, 2> ctrCov = control2wCov[j];
            argp(0,B+A+B+j) = 0.0;   //we do not have measured control dbeta
            argp(1,B+A+B+j) = ctrCov[1];
        }

        arg["p"] = argp;
        arg["x0"] = argx0; //Comes as function parameter

        res = solver(arg); // solve MHE
        argx0 = res.at("x");

    }

    template <class T, class T2, long unsigned int A, long unsigned int B>
    void estimateMheTrailer(casadi::DM& argx0Trailer,const boost::array<T, B>& q5wLocTrailer,
        const boost::array<T2, A>& control2wTrailer,const boost::array<T, B>& q5wCovTrailer,
        const boost::array<T2, A>& control2wCovTrailer,const mhe_estimator::CarParams& carParams,
        const mhe_estimator::MheParams& mheParams,const casadi::Function& solver)  //check the ref and const
    {

        unsigned int n_states = 5; unsigned int n_controls = 2;
        std::map<std::string, DM> arg, res;
        arg["lbg"] = SX::zeros(n_states*A); //size = n_state * N_Mhe
        arg["ubg"] = SX::zeros(n_states*A);

        std::vector<double> lbx((n_controls*A)+(n_states*B), 0.0);
        std::vector<double> ubx((n_controls*A)+(n_states*B), 0.0);

        for(int i = 0; i < (n_states*B); i += n_states)
        {
            lbx[i] = -carParams.trailer1Limit;
            lbx[i+1] = carParams.lowerTh;   //theta lower bound
            lbx[i+2] = carParams.lowerX;    //x lower bound
            lbx[i+3] = carParams.lowerY;    //y lower bound
            lbx[i+4] = -carParams.steeringLimit;

            ubx[i] = carParams.trailer1Limit;
            ubx[i+1] = carParams.upperTh;   //theta upper bound
            ubx[i+2] = carParams.upperX;    //x upper bound
            ubx[i+3] = carParams.upperY;    //y upper bound
            ubx[i+4] = carParams.steeringLimit;
        }

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

        for(int i = (n_states*B); i < ((n_controls*A)+(n_states*B)); i += n_controls)
        {
            lbx[i] = -carParams.SteeringVelLimit;    //dbeta lower bound
            lbx[i+1] = -carParams.LinearVelLimit;  //linear.vel lower bound

            ubx[i] = carParams.SteeringVelLimit;  //dbeta upper bound
            ubx[i+1] = carParams.LinearVelLimit;  //linear.vel upper bound
        }
        arg["lbx"] = lbx;
        arg["ubx"] = ubx;
        //--------------ALL OF THE ABOVE IS JUST A PROBLEM SET UP-------------//

        DM argp = SX::zeros(n_states,B+A+B+A);
        for(int j = 0; j < B; j++) //measured state
        {
            boost::array<double, 5> qloc = q5wLocTrailer[j];
            argp(0,j) = qloc[0]; //beta1
            argp(1,j) = qloc[1]; //theta
            argp(2,j) = qloc[2]; //x
            argp(3,j) = qloc[3]; //y
            argp(4,j) = qloc[4]; //beta0
        }
        for(int j = 0; j < A; j++) //measured control
        {
            boost::array<double, 2> qctr = control2wTrailer[j];
            //argp(0,j+B) = 0.0;   //we do not have measured control dbeta
            argp(1,j+B) = qctr[1];
        }
        for(int j = 0; j < B; j++) //state cov matrix
        {
            boost::array<double, 5> qCov = q5wCovTrailer[j];
            argp(0,B+A+j) = qCov[0]; //beta1
            argp(1,B+A+j) = qCov[1]; //theta
            argp(2,B+A+j) = qCov[2]; //x
            argp(3,B+A+j) = qCov[3]; //y
            argp(4,B+A+j) = qCov[4]; //beta0
        }
        for(int j = 0; j < A; j++) //control cov matrix
        {
            boost::array<double, 2> ctrCov = control2wCovTrailer[j];
            argp(0,B+A+B+j) = 0.0;   //we do not have measured control dbeta
            argp(1,B+A+B+j) = ctrCov[1];
        }

        arg["p"] = argp;
        arg["x0"] = argx0Trailer; //Comes as parameter

        try
        {
          res = solver(arg); // solve MHE
          argx0Trailer = res.at("x");
        } catch (casadi::CasadiException& ex)
        {
          ROS_ERROR_STREAM(ex.what());

        }

    }

    void estimateEst(Vec3& qCarEst,const Vec3& qLoc,const Vec3& qPred,const MheParams& estParams)
    {

        qCarEst[0] = estParams.WeightTh*qPred[0] + (1-estParams.WeightTh)*qLoc[0];
        qCarEst[1] = estParams.WeightPos*qPred[1] + (1-estParams.WeightPos)*qLoc[1];
        qCarEst[2] = estParams.WeightPos*qPred[2] + (1-estParams.WeightPos)*qLoc[2];

    }

    void estimateEstTrailer(Vec4& qTrailerEst,const Vec4& qTrailerLoc,const Vec4& qTrailerPred,const MheParams& estParams)
    {   qTrailerEst[0] = estParams.WeightTrailer1*qTrailerPred[0] + (1-estParams.WeightTrailer1)*qTrailerLoc[0];
        qTrailerEst[1] = estParams.WeightTh*qTrailerPred[1] + (1-estParams.WeightTh)*qTrailerLoc[1];
        qTrailerEst[2] = estParams.WeightPos*qTrailerPred[2] + (1-estParams.WeightPos)*qTrailerLoc[2];
        qTrailerEst[3] = estParams.WeightPos*qTrailerPred[3] + (1-estParams.WeightPos)*qTrailerLoc[3];
    }

}


///////////////////////////////////////////////////////////////////////////////
// kernel::example::mono_bw_kernel_rp.cpp                                    //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/foreach.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/special_functions/fpclassify.hpp> //needed?
#include <boost/math/tools/precision.hpp>

#include <boost/statistics/detail/kernel/kernels/scalar/gaussian.hpp>
#include <boost/statistics/detail/kernel/kernels/multivariate/mono_bw.hpp>
#include <boost/statistics/detail/kernel/estimation/rp_visitor.hpp>
#include <boost/statistics/detail/kernel/estimation/estimator.hpp>
#include <libs/statistics/detail/kernel/example/scalar_rp.h>

void example_mv_mono_bw_rp(std::ostream& out){
    out << "-> example_mono_bw_kernel_rp : ";

    // This example shows how to compute a Rosenblatt-Parzen estimate of the
    // density, p(x). The type used for each data-unit, x, is a vector of
    // doubles, and the kernel uses the same bandwidth throughout all
    // coordinates

    using namespace boost;
    namespace kernel = boost::statistics::detail::kernel;

    // Types
    typedef double                                          val_;
    typedef std::vector<val_>                               vec_;
    typedef vec_                                            x_;
    typedef std::vector<x_>                                 dataset_;
    typedef mt19937                                         urng_;
    typedef normal_distribution<val_>                       norm_;
    typedef variate_generator<urng_&,norm_>                 gen_;
    typedef kernel::scalar::gaussian_kernel<val_>                  gauss_k_;

    const unsigned dim = 2;
    typedef kernel::multivariate::mono_bw_kernel<gauss_k_,dim> mono_bw_kernel_k_;
    // Use of a const reference is not necessary but probably improves speed
    typedef kernel::rp_visitor<mono_bw_kernel_k_,const x_&>  rp_visitor_;

    // Constants
    const val_ bandwidth = 0.5;
    const val_ eps = math::tools::epsilon<val_>();
    const unsigned n = 10;

    // Generate n samples, each drawn from prod{N(0,1):i=1,...,dim}
    dataset_ dataset; dataset.reserve(n);
    vec_ vec_rp; vec_rp.reserve(n);
    urng_ urng;
    norm_ norm;
    gen_ gen(urng,norm);
    for(unsigned i = 0; i<n; i++){
        vec_ tmp(dim);
        std::generate_n(
            boost::begin(tmp),
            dim,
            gen
        );
        dataset.push_back( tmp );
    }

    // Density estimate for each x in dataset
    BOOST_FOREACH(const x_& x,dataset){
        val_ rp = std::for_each(
            boost::begin(dataset),
            boost::end(dataset),
            rp_visitor_(bandwidth,x)
        ).estimate();
        vec_rp.push_back(rp);
    }
    typedef sub_range<dataset_> sub_;
    typedef kernel::estimator<
        sub_,
        kernel::rp_visitor,
        mono_bw_kernel_k_
    > estimator_;
    estimator_ estimator(bandwidth);
    estimator.train(sub_(dataset));
    vec_ vec_rp2; vec_rp2.reserve(n);

    // Same as previous but calls estimator instead of for_each
    for(unsigned i = 0; i<n; i++){
        x_ x = dataset[i];
        val_ rp = vec_rp[i];
        val_ rp2 = estimator.predict(x);
        BOOST_ASSERT(fabs(rp-rp2)<eps);
    }
    out << "<-" << std::endl;
}


#include "Eigen/Dense"
#include <Eigen/Eigenvalues>
#include <cmath>
#include <vector>
#include <cstdio>
#include <fstream>
#include <iostream>
#include <string>

typedef Eigen::Matrix< double , Eigen::Dynamic , Eigen::Dynamic > MatrixXd;

void load_int_scalar(int &in, std::string filename) {
  std::ifstream input_file(filename);
  input_file >> in;
}

void load_double_scalar(double &in, std::string filename) {
  std::ifstream input_file(filename);
  input_file >> in;
}

void load_double(std::vector<double> &in, std::string filename) {
  std::ifstream input_file(filename);
  double temp_val;
  std::string line;
  while (std::getline(input_file, line)) {
    std::istringstream ss(line);
    while (ss >> temp_val) {
      in.push_back(temp_val);
    }
  }
}

size_t idx2(size_t &i, size_t &j) {
  if (i >= j) {
    return i * (i + 1) / 2 + j;
  } else {
    return j * (j + 1) / 2 + i;
  }
}

size_t idx4(size_t &i, size_t &j, size_t &k, size_t &l) {
  size_t temp_ij = idx2(i, j);
  size_t temp_kl = idx2(k, l);
  return idx2(temp_ij, temp_kl);
}

int main(int argc, char const *argv[]) {
  int iteration_max;
  int num_elec_alpha;
  int num_elec_beta;
  int num_ao;

  load_int_scalar(num_elec_alpha, "../../data/num_elec_alpha.txt");
  load_int_scalar(num_elec_beta, "../../data/num_elec_beta.txt");
  load_int_scalar(iteration_max, "../../data/iteration_max.txt");
  load_int_scalar(num_ao, "../../data/num_ao.txt");

  std::vector<double> S_vec;
  load_double(S_vec, "../../data/S.txt");
  Eigen::Map<MatrixXd> S(S_vec.data(), num_ao, num_ao);

  std::vector<double> T_vec;
  load_double(T_vec, "../../data/T.txt");
  Eigen::Map<MatrixXd> T(T_vec.data(), num_ao, num_ao);

  std::vector<double> V_vec;
  load_double(V_vec, "../../data/V.txt");
  Eigen::Map<MatrixXd> V(V_vec.data(), num_ao, num_ao);

  std::vector<double> eri_vec;
  load_double(eri_vec, "../../data/eri.txt");
  Eigen::Map<MatrixXd> eri(eri_vec.data(),45150,1);

  double convergence_DM;
  load_double_scalar(convergence_DM, "../../data/convergence_DM.txt");
  double convergence_E;
  load_double_scalar(convergence_DM, "../../data/convergence_E.txt");
  double E_nuc;
  load_double_scalar(E_nuc, "../../data/E_nuc.txt");

  MatrixXd D = MatrixXd::Zero(num_ao,num_ao);
  MatrixXd D_last = MatrixXd::Zero(num_ao,num_ao);
  // loop variables
  int iteration_num = 0;
  double E_total = 0.0;
  double E_elec = 0.0;
  double E_elec_last = 0.0;
  double iteration_E_diff = 0.0;
  double iteration_rmsc_dm = 0.0;
  bool converged = false;
  bool exceeded_iterations = false;
  Eigen::SelfAdjointEigenSolver<MatrixXd> eigen_solver(num_ao);
  eigen_solver.compute(S);
  auto s = eigen_solver.eigenvalues().transpose();
  auto L = eigen_solver.eigenvectors();
  MatrixXd X = MatrixXd::Zero(num_ao,num_ao);
  for (size_t i = 0; i < s.size(); i++) {
    X(i, i) = 1.0 / std::sqrt(s(i));
  }
  X = L*X*L.transpose();
  auto H = T + V;
  MatrixXd G = MatrixXd::Zero(num_ao,num_ao);
  MatrixXd F = MatrixXd::Zero(num_ao,num_ao);
  MatrixXd F_prime = MatrixXd::Zero(num_ao,num_ao);
  MatrixXd C = MatrixXd::Zero(num_ao,num_ao);
  while (!converged && !exceeded_iterations) {
    E_elec_last = E_elec;
    D_last = D;

    iteration_num++;
    // form G matrix
    MatrixXd G = MatrixXd::Zero(num_ao,num_ao);
    for (size_t i = 0; i < num_ao; i++) {
      for (size_t j = 0; j < num_ao; j++) {
        for (size_t k = 0; k < num_ao; k++) {
          for (size_t l = 0; l < num_ao; l++) {
            G(i, j) += D(k, l) * ((2.0 * (eri(idx4(i, j, k, l)))) - (eri(idx4(i, k, j, l))));
          }
        }
      }
    }

    F = H + G;
    F_prime = X*F*X;

    eigen_solver.compute(F_prime);
    auto E_orbitals = eigen_solver.eigenvalues().transpose();
    auto C_prime = eigen_solver.eigenvectors();

    C = X*C_prime;
    D.setZero();

    for (size_t i = 0; i < num_ao; i++) {
      for (size_t j = 0; j < num_ao; j++) {
        for (size_t k = 0; k < num_elec_alpha; k++) {
          D(i, j) += C(i, k) * C(j, k);
        }
      }
    }
    E_elec = D.cwiseProduct(H + F).sum();
    iteration_E_diff = std::abs(E_elec - E_elec_last);
    iteration_rmsc_dm = std::sqrt((D-D_last).cwiseProduct(D-D_last).sum());
    if (iteration_E_diff < convergence_E &&
        iteration_rmsc_dm < convergence_DM) {
      converged = true;
    }
    if (iteration_num == iteration_max) {
      exceeded_iterations = true;
    }
  }
  E_total = E_elec + E_nuc;
  printf("%20.15f\n", E_total);
  return 0;
}


#include "Lshape.hpp"
#include "convergence.hpp"
#include "fem_solve.hpp"
#include "stiffness_matrix.hpp"
#include "writer.hpp"
#include <Eigen/Core>
#include <sstream>
#include <tuple>

// constant sigma
double sigma(double x, double y) {
	std::ignore = x;
	std::ignore = y;
	return 1.0;
}

// Data for square
double f_square(double x, double y) {
	return 2 * M_PI * M_PI * sin(M_PI * x) * sin(M_PI * y);
}

double g_square(double x, double y) {
	std::ignore = x;
	std::ignore = y;
	return 0;
}

double uex_square(double x, double y) {
	return sin(M_PI * x) * sin(M_PI * y);
}

Eigen::Vector2d uex_grad_square(double x, double y) {
	Eigen::Vector2d grad;
	grad << M_PI * cos(M_PI * x) * sin(M_PI * y),
	    M_PI * sin(M_PI * x) * cos(M_PI * y);
	return grad;
}

Eigen::Vector2d u_square_ex_grad(double x, double y) {
	Eigen::Vector2d gradient;
	gradient << M_PI * cos(M_PI * x) * sin(M_PI * y), M_PI * sin(M_PI * x) * cos(M_PI * y);
	return gradient;
}

int main(int, char **) {
	try {
		solveL(0.5);

		std::cout << "Convergence Analysis" << std::endl;
		convergenceAnalysis("square", 7, f_square, sigma, g_square, 0, uex_square, uex_grad_square);
		convergenceAnalysis("Lshape", 7, f_lshape, sigma, g_lshape, 0, g_lshape, g_grad_lshape);
	} catch (std::runtime_error &e) {
		std::cerr << "An error occurred. Error message: " << std::endl;
		std::cerr << "    \"" << e.what() << "\"" << std::endl;
		return EXIT_FAILURE;
	} catch (...) {
		std::cerr << "An unknown error occurred." << std::endl;
		throw;
	}

	return EXIT_SUCCESS;
}


#include <boost/numeric/interval.hpp>

// Exception codes
#define EXC_NONE                          0
#define EXC_UNDEFINED_OPERATION          -1
#define EXC_POSSIBLY_UNDEFINED_OPERATION -2
#define EXC_INTERVAL_PART_OF_NAI         -3
#define EXC_INVALID_OPERAND              -4

using namespace boost::numeric;
using namespace interval_lib;

typedef interval<double,
                 policies<save_state<rounded_transc_std<double> >,
                          checking_base<double> > > I;

extern "C" {

void test_boost_setup() {
}

int test_boost_neg(I* x, I* r) {
    *r = - *x;
    return EXC_NONE;
}

int test_boost_add(I* x, I* y, I* r) {
    *r = *x + *y;
    return EXC_NONE;
}

int test_boost_sub(I* x, I* y, I* r) {
    *r = *x - *y;
    return EXC_NONE;
}

int test_boost_mul(I* x, I* y, I* r) {
    *r = *x * *y;
    return EXC_NONE;
}

int test_boost_div(I* x, I* y, I* r) {
    *r = *x / *y;
    return EXC_NONE;
}

int test_boost_recip(I* x, I* r) {
    *r = multiplicative_inverse( *x );
    return EXC_NONE;
}

int test_boost_sqrt(I* x, I* r) {
    *r = sqrt( *x ) ;
    return EXC_NONE;
}

int test_boost_hypot(I* x, I *y, I* r) {
    *r = sqrt( square( *x ) + square( *y ));
    return EXC_NONE;
}

int test_boost_fma(I* x, I *y, I *z, I* r) {
    *r = *x * *y + *z;
    return EXC_NONE;
}

int test_boost_sqr(I* x, I* r) {
    *r = square( *x );
    return EXC_NONE;
}

int test_boost_pown(I* x, int *p, I* r) {
    *r = pow( *x, *p );
    return EXC_NONE;
}

//int test_boost_pow(I* x, int *y, I* r) {
//    return EXC_NONE;
//}

int test_boost_exp(I* x, I* r) {
    *r = exp( *x );
    return EXC_NONE;
}

//int test_boost_exp2(I* x, I* r) {
//    return EXC_NONE;
//}

//int test_boost_exp10(I* x, I* r) {
//    return EXC_NONE;
//}

int test_boost_log(I* x, I* r) {
    *r = log( *x );
    return EXC_NONE;
}

//int test_boost_log2(I* x, I* r) {
//    return EXC_NONE;
//}

//int test_boost_log10(I* x, I* r) {
//}

int test_boost_sin(I* x, I* r) {
    *r = sin( *x );
    return EXC_NONE;
}

int test_boost_cos(I* x, I* r) {
    *r = cos( *x );
    return EXC_NONE;
}

int test_boost_tan(I* x, I* r) {
    *r = tan( *x );
    return EXC_NONE;
}

int test_boost_asin(I* x, I* r) {
    *r = asin( *x );
    return EXC_NONE;
}

int test_boost_acos(I* x, I* r) {
    *r = acos( *x );
    return EXC_NONE;
}

int test_boost_atan(I* x, I* r) {
    *r = atan( *x );
    return EXC_NONE;
}

//int test_boost_atan2(I* x, I *y, I* r) {
//    return EXC_NONE;
//}

int test_boost_sinh(I* x, I* r) {
    *r = sinh( *x );
    return EXC_NONE;
}

int test_boost_cosh(I* x, I* r) {
    *r = cosh( *x );
    return EXC_NONE;
}

int test_boost_tanh(I* x, I* r) {
    *r = tanh( *x );
    return EXC_NONE;
}

int test_boost_asinh(I* x, I* r) {
    *r = asinh( *x );
    return EXC_NONE;
}

int test_boost_acosh(I* x, I* r) {
    *r = acosh( *x );
    return EXC_NONE;
}

int test_boost_atanh(I* x, I* r) {
    *r = atanh( *x );
    return EXC_NONE;
}

//int test_boost_sign(I* x, I* r) {
//    return EXC_NONE;
//}

//int test_boost_ceil(I* x, I* r) {
//    return EXC_NONE;
//}

//int test_boost_floor(I* x, I* r) {
//    return EXC_NONE;
//}

//int test_boost_round(I* x, I* r) {
//    return EXC_NONE;
//}

//int test_boost_trunc(I* x, I* r) {
//    return EXC_NONE;
//}

int test_boost_abs(I* x, I* r) {
    *r = abs( *x );
    return EXC_NONE;
}

int test_boost_min(I* x, I *y, I* r) {
    *r = min( *x, *y );
    return EXC_NONE;
}

int test_boost_max(I* x, I *y, I* r) {
    *r = max( *x, *y );
    return EXC_NONE;
}

int test_boost_intersection(I* x, I *y, I* r) {
    *r = intersect( *x, *y );
    return EXC_NONE;
}

int test_boost_convexHull(I* x, I *y, I* r) {
    *r = hull( *x, *y );
    return EXC_NONE;
}

}


//  Copyright John Maddock 2006
//  Copyright Paul A. Bristow 2007
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <pch.hpp>

#ifdef _MSC_VER
#  pragma warning(disable : 4267) // conversion from 'size_t' to 'const unsigned int', possible loss of data
#  pragma warning(disable : 4180) // qualifier applied to function type has no meaning; ignored
#  pragma warning(disable : 4224) // nonstandard extension used : formal parameter 'function_ptr' was previously defined as a type
#  pragma warning(disable : 4100) // unreferenced formal parameter (in ublas/functional)
#endif

#include <boost/math/tools/remez.hpp>
#include <boost/test/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/expm1.hpp>

void test_polynomial()
{
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   double (*f)(double) = boost::math::expm1<double>;
#else
   double (*f)(double) = boost::math::expm1;
#endif
   std::cout << "Testing expm1 approximation, pinned to origin, abolute error, 6 term polynomial\n";
   boost::math::tools::remez_minimax<double> approx1(f, 6, 0, -1, 1, true, false);
   std::cout << "Interpolation Error: " << approx1.max_error() << std::endl;
   for(unsigned i = 0; i < 7; ++i)
   {
      approx1.iterate();
      std::cout << approx1.error_term() << " " << approx1.max_error() << " " << approx1.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
   std::cout << "Testing expm1 approximation, pinned to origin, relative error, 6 term polynomial\n";
   boost::math::tools::remez_minimax<double> approx2(f, 6, 0, -1, 1, true, true);
   std::cout << "Interpolation Error: " << approx1.max_error() << std::endl;
   for(unsigned i = 0; i < 7; ++i)
   {
      approx2.iterate();
      std::cout << approx2.error_term() << " " << approx2.max_error() << " " << approx2.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;

   f = std::exp;
   std::cout << "Testing exp approximation, not pinned to origin, abolute error, 6 term polynomial\n";
   boost::math::tools::remez_minimax<double> approx3(f, 6, 0, -1, 1, false, false);
   std::cout << "Interpolation Error: " << approx1.max_error() << std::endl;
   for(unsigned i = 0; i < 7; ++i)
   {
      approx3.iterate();
      std::cout << approx3.error_term() << " " << approx3.max_error() << " " << approx3.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
   std::cout << "Testing exp approximation, not pinned to origin, relative error, 6 term polynomial\n";
   boost::math::tools::remez_minimax<double> approx4(f, 6, 0, -1, 1, false, true);
   std::cout << "Interpolation Error: " << approx1.max_error() << std::endl;
   for(unsigned i = 0; i < 7; ++i)
   {
      approx4.iterate();
      std::cout << approx4.error_term() << " " << approx4.max_error() << " " << approx4.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;

   f = std::cos;
   std::cout << "Testing cos approximation, not pinned to origin, abolute error, 5 term polynomial\n";
   boost::math::tools::remez_minimax<double> approx5(f, 5, 0, -1, 1, false, false);
   std::cout << "Interpolation Error: " << approx1.max_error() << std::endl;
   for(unsigned i = 0; i < 7; ++i)
   {
      approx5.iterate();
      std::cout << approx5.error_term() << " " << approx5.max_error() << " " << approx5.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
   std::cout << "Testing cos approximation, not pinned to origin, relative error, 5 term polynomial\n";
   boost::math::tools::remez_minimax<double> approx6(f, 5, 0, -1, 1, false, true);
   for(unsigned i = 0; i < 7; ++i)
   {
      approx6.iterate();
      std::cout << approx6.error_term() << " " << approx6.max_error() << " " << approx6.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;

   f = std::sin;
   std::cout << "Testing sin approximation, pinned to origin, abolute error, 4 term polynomial\n";
   boost::math::tools::remez_minimax<double> approx7(f, 4, 0, 0, 1, true, false);
   for(unsigned i = 0; i < 7; ++i)
   {
      approx7.iterate();
      std::cout << approx7.error_term() << " " << approx7.max_error() << " " << approx7.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
   std::cout << "Testing sin approximation, pinned to origin, relative error, 4 term polynomial\n";
   boost::math::tools::remez_minimax<double> approx8(f, 4, 0, 0, 1, true, true);
   for(unsigned i = 0; i < 7; ++i)
   {
      approx8.iterate();
      std::cout << approx8.error_term() << " " << approx8.max_error() << " " << approx8.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}

void test_rational()
{
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   double (*f)(double) = boost::math::expm1<double>;
#else
   double (*f)(double) = boost::math::expm1;
#endif
   std::cout << "Testing expm1 approximation, pinned to origin, abolute error, 3+3 term rational\n";
   boost::math::tools::remez_minimax<double> approx1(f, 3, 3, -1, 1, true, false);
   std::cout << "Interpolation Error: " << approx1.max_error() << std::endl;
   for(unsigned i = 0; i < 7; ++i)
   {
      approx1.iterate();
      std::cout << approx1.error_term() << " " << approx1.max_error() << " " << approx1.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
#if 0
   //
   // This one causes UBLAS to fail on some systems, so disabled for now.
   //
   std::cout << "Testing expm1 approximation, pinned to origin, relative error, 3+3 term rational\n";
   boost::math::tools::remez_minimax<double> approx2(f, 3, 3, -1, 1, true, true);
   std::cout << "Interpolation Error: " << approx1.max_error() << std::endl;
   for(unsigned i = 0; i < 7; ++i)
   {
      approx2.iterate();
      std::cout << approx2.error_term() << " " << approx2.max_error() << " " << approx2.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
#endif
   f = std::exp;
   std::cout << "Testing exp approximation, not pinned to origin, abolute error, 3+3 term rational\n";
   boost::math::tools::remez_minimax<double> approx3(f, 3, 3, -1, 1, false, false);
   std::cout << "Interpolation Error: " << approx1.max_error() << std::endl;
   for(unsigned i = 0; i < 7; ++i)
   {
      approx3.iterate();
      std::cout << approx3.error_term() << " " << approx3.max_error() << " " << approx3.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
   std::cout << "Testing exp approximation, not pinned to origin, relative error, 3+3 term rational\n";
   boost::math::tools::remez_minimax<double> approx4(f, 3, 3, -1, 1, false, true);
   std::cout << "Interpolation Error: " << approx1.max_error() << std::endl;
   for(unsigned i = 0; i < 7; ++i)
   {
      approx4.iterate();
      std::cout << approx4.error_term() << " " << approx4.max_error() << " " << approx4.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;

   f = std::cos;
   std::cout << "Testing cos approximation, not pinned to origin, abolute error, 2+2 term rational\n";
   boost::math::tools::remez_minimax<double> approx5(f, 2, 2, 0, 1, false, false);
   std::cout << "Interpolation Error: " << approx1.max_error() << std::endl;
   for(unsigned i = 0; i < 7; ++i)
   {
      approx5.iterate();
      std::cout << approx5.error_term() << " " << approx5.max_error() << " " << approx5.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
   std::cout << "Testing cos approximation, not pinned to origin, relative error, 2+2 term rational\n";
   boost::math::tools::remez_minimax<double> approx6(f, 2, 2, 0, 1, false, true);
   std::cout << "Interpolation Error: " << approx1.max_error() << std::endl;
   for(unsigned i = 0; i < 7; ++i)
   {
      approx6.iterate();
      std::cout << approx6.error_term() << " " << approx6.max_error() << " " << approx6.max_change() << std::endl;
   }
   std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}

int test_main(int, char* [])
{
   test_polynomial();
   test_rational();
   return 0;
}


/* integrate.hpp header file
 *
 * Copyright Jens Maurer 2000
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * $Id: integrate.hpp,v 1.4 2001/11/19 22:13:04 jmaurer Exp $
 *
 * Revision history
 *   01 April 2001: Modified to use new <boost/limits.hpp> header. (JMaddock)
 */

#ifndef INTEGRATE_HPP
#define INTEGRATE_HPP

#include <boost/limits.hpp>

template<class UnaryFunction>
inline typename UnaryFunction::result_type
trapezoid(UnaryFunction f, typename UnaryFunction::argument_type a,
          typename UnaryFunction::argument_type b, int n)
{
  typename UnaryFunction::result_type tmp = 0;
  for(int i = 1; i <= n-1; ++i)
    tmp += f(a+(b-a)/n*i);
  return (b-a)/2/n * (f(a) + f(b) + 2*tmp);
}

template<class UnaryFunction>
inline typename UnaryFunction::result_type
simpson(UnaryFunction f, typename UnaryFunction::argument_type a,
        typename UnaryFunction::argument_type b, int n)
{
  typename UnaryFunction::result_type tmp1 = 0;
  for(int i = 1; i <= n-1; ++i)
    tmp1 += f(a+(b-a)/n*i);
  typename UnaryFunction::result_type tmp2 = 0;
  for(int i = 1; i <= n ; ++i)
    tmp2 += f(a+(b-a)/2/n*(2*i-1));

  return (b-a)/6/n * (f(a) + f(b) + 2*tmp1 + 4*tmp2);
}

// compute b so that f(b) = y; assume f is monotone increasing
template<class UnaryFunction>
inline typename UnaryFunction::argument_type
invert_monotone_inc(UnaryFunction f, typename UnaryFunction::result_type y,
                    typename UnaryFunction::argument_type lower = -1,
                    typename UnaryFunction::argument_type upper = 1)
{
  while(upper-lower > 1e-6) {
    double middle = (upper+lower)/2;
    if(f(middle) > y)
      upper = middle;
    else
      lower = middle;
  }
  return (upper+lower)/2;
}

// compute b so that  I(f(x), a, b) == y
template<class UnaryFunction>
inline typename UnaryFunction::argument_type
quantil(UnaryFunction f, typename UnaryFunction::argument_type a,
        typename UnaryFunction::result_type y,
        typename UnaryFunction::argument_type step)
{
  typedef typename UnaryFunction::result_type result_type;
  if(y >= 1.0)
    return std::numeric_limits<result_type>::infinity();
  typename UnaryFunction::argument_type b = a;
  for(result_type result = 0; result < y; b += step)
    result += step*f(b);
  return b;
}

#endif /* INTEGRATE_HPP */


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

#ifndef NOMAD_COMMON_MATHUTILS_H
#define NOMAD_COMMON_MATHUTILS_H

// C System Files
#include <math.h>

// C++ System Files

// Third Party Includes
#include <Eigen/Dense>
// Project Include Files

namespace Common::Math
{
    template <typename T>
    int sgn(T val)
    {
        return (T(0) < val) - (val < T(0));
    }

    // https://en.wikipedia.org/wiki/Skew-symmetric_matrix#Cross_product
    Eigen::Matrix3d SkewSymmetricCrossProduct(const Eigen::Vector3d& a);

    void rpyToR(Eigen::Matrix3d &R, double *rpy_in);

        // Convert Rotation -> Euler RPY
    Eigen::Vector3d RotationMatrixToEuler(const Eigen::Matrix3d& R);

    // Convert Euler RPY -> Rotation Matrix.  TODO: Add support for rotation order
    Eigen::Matrix3d EulerToRotationMatrix(const Eigen::Vector3d& euler);

    // Convert Euler RPY -> Quaternion.  TODO: Add support for rotation order
    Eigen::Quaterniond EulerToQuaternion(const Eigen::Vector3d& euler);

    // Convert Euler RPY -> Quaternion.  TODO: Add support for rotation order
    Eigen::Vector3d QuaterionToEuler(const Eigen::Quaterniond& q);

    // Compute Orientation Error between 2 Euler orientations
    Eigen::Vector3d ComputeOrientationError(const Eigen::Vector3d& theta_1, const Eigen::Vector3d& theta_2);

    Eigen::Matrix3d RotationX(double rad);
    Eigen::Matrix3d RotationY(double rad);
    Eigen::Matrix3d RotationZ(double rad);

    namespace EigenHelpers
    {

        // Typedefs
        typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMatrixXd;
        // TODO: Make this more generic
        // Block Matrix Class
        class BlockMatrixXd
        {
        public:
            BlockMatrixXd() {}
            BlockMatrixXd(const unsigned int Rows, const unsigned int Cols, const unsigned int BlockHeight, const unsigned int BlockWidth, const int init_val);
            BlockMatrixXd(const unsigned int Rows, const unsigned int Cols, const unsigned int BlockHeight, const unsigned int BlockWidth, Eigen::MatrixXd &matrix);
            BlockMatrixXd(const unsigned int Rows, const unsigned int Cols, const unsigned int BlockHeight, const unsigned int BlockWidth);

            friend std::ostream &operator<<(std::ostream &os, BlockMatrixXd const &bm) { return os << bm.Matrix_; }
            // Operator Overload:
            void operator()(const unsigned int Row, const unsigned int Col, const Eigen::MatrixXd &block_val);

            // TODO: Fix Templated Hack so we can set this directly... i.e. matrix(1,1) = block
            Eigen::Block<Eigen::MatrixXd, -1, -1, false> operator()(const unsigned int Row, const unsigned int Col);

            // Cast Overload
            operator Eigen::MatrixXd() { return Matrix_; }

            Eigen::MatrixXd MatrixXd() const { return Matrix_; }
            // Set Block Matrix Value
            // TODO: Should be Matrix(1,1) = value
            void SetBlock(const unsigned int Row, const unsigned int Col, const Eigen::MatrixXd &block_val);

            // Fill Diagonal with a block
            void FillDiagonal(const Eigen::MatrixXd &block_val, const int k = 0);

        protected:
            unsigned int Rows_;
            unsigned int Cols_;
            unsigned int BlockWidth_;
            unsigned int BlockHeight_;

            Eigen::MatrixXd Matrix_;

        };
    } // namespace EigenHelpers

} // namespace Common::Math
#endif // NOMAD_COMMON_STATISTICS_H


///
/// File:   eigen.cpp
/// Author: Open Risk
///

#include <iostream>
#include <sstream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, const char **argv) {

    //
    // Get eigen library version
    //

    std::stringstream ss;
    ss.str("");
    ss << EIGEN_MAJOR_VERSION;
    ss << ".";
    ss << EIGEN_MINOR_VERSION;
    std::string eigen_version = ss.str();
    std::cout << eigen_version << std::endl;

    MatrixXf A = MatrixXf::Random(3, 2);
    cout << "Here is the matrix A:\n" << A << endl;
    VectorXf b = VectorXf::Random(3);
    cout << "Here is the right hand side b:\n" << b << endl;
    cout << "The least-squares solution is:\n"
         << A.bdcSvd(ComputeThinU | ComputeThinV).solve(b) << endl;

    return 0;
}


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

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

struct poisson2D_dirichlet
{
    poisson2D_dirichlet(int m, int n) : m(m), n(n) {}

    template <typename Vector>
    Vector operator*(const Vector& v) const
    {
	assert(int(size(v)) == m * n);
	Vector w(m * n, typename Vector::value_type(0));

	for (int i= 0; i < m; i++)
	    for (int j= 0; j < n; j++) {
		int k= i * n + j; // offset
		w[k]= 4 * v[k];
		if (i > 0) w[k]-= v[k-n];   // upper neighbor
		if (i < m-1) w[k]-= v[k+n]; // lower neighbor
		if (j > 0) w[k]-= v[k-1];   // left neighbor
		if (j < n-1) w[k]-= v[k+1]; // right neighbor
	    }
	return w;
    }
    int m, n;
};

namespace mtl { namespace ashape {
    template <> struct ashape_aux<poisson2D_dirichlet>
    {	typedef nonscal type;    };
}}

int main()
{
    mtl::vampir_trace<9999> tracer;
  // For a more realistic example set size to 1000 or larger
  const int size = 10, N = size * size;

  poisson2D_dirichlet A(size, size);

  itl::pc::identity<poisson2D_dirichlet>     P(A);

  mtl::dense_vector<double> x(N, 1.0), b(N);

  b = A * x;
  x= 0;
  itl::cyclic_iteration<double> iter(b, 100, 1.e-11, 0.0, 5);
  cg(A, x, b, P, iter);

  return 0;
}


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

#include <iostream>
#include <boost/test/minimal.hpp>

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

using namespace std;

template <typename T, typename F>
mtl::maybe<T> inline bisection(T a, T b, const F& f, const T& tau)
{
    if (f(a) * f(b) > 0)
	return false;
    while (b - a > tau) {
	T m= (a + b) / 2;
	// cout << "f(m) is " << f(m) << '\n';
	if (f(m) * f(a) < 0)
	    b= m;
	else
	    a= m;
    }
    return (a + b) / 2;
}

class caterpillar
{
public:
    caterpillar(double r, double K, double a, double b) : r(r), K(K), a(a), b(b) {}

    double operator()(double N) const
    {
	return r * N * (1. - N / K) - a * N * N / (b + N * N);
    }
private:
    double r, K, a, b;
};

template <typename Matrix>
class ev_bisection
{
    typedef typename mtl::Collection<Matrix>::value_type value_type;
public:
    explicit ev_bisection(const Matrix& A) : A(A), D(A), Q(A), R(A)
    {
	assert(num_rows(A) == num_cols(A));
    }

    value_type operator()(const value_type& lambda)
    {
	// D= A - lamdda * identity
	D= -lambda; D+= A;

	return lambda; // to make it compile answer is nonsense
    }

private:
    const Matrix &A;
    Matrix D, Q, R;
};

void inline find_eigenvalue(const ev_bisection<mtl::dense2D<double> >& bis, double a, double b)
{
    cout << "Search eigenvalue in interval [" << a << ", " << b << "]\n";
    //mtl::maybe<double> lambda= ;
}

int test_main(int argc, char* argv[])
{
#if 0
    caterpillar population1(1.3, 100, 20, 50);
    cout << "Equilibrium of population 1 in interval [0.1, 10] is " << bisection(0.1, 10.0, population1, 0.0001).value() << '\n';
    cout << "Equilibrium of population 1 in interval [10, 20] is " << bisection(10.0, 20.0, population1, 0.0001).value() << '\n';
    cout << "Equilibrium of population 1 in interval [20, 100] is " << bisection(20.0, 100.0, population1, 0.0001).value() << '\n';

    caterpillar population2(2.0, 80, 25, 10);
    cout << "\nEquilibrium of population 2 in interval [0.1, 10] is " << bisection(0.1, 10.0, population2, 0.0001).value() << '\n';
    cout << "Equilibrium of population 2 in interval [10, 20] is " << bisection(10.0, 20.0, population2, 0.0001).value() << '\n';
    cout << "Equilibrium of population 2 in interval [20, 100] is " << bisection(20.0, 100.0, population2, 0.0001).value() << '\n';
#endif

    mtl::dense2D<double> A(3, 3);
    A= 0.0; A[0][0]= 1.0; A[1][1]= 2.0; A[2][2]= 3;

    ev_bisection<mtl::dense2D<double> > bis(A);
    find_eigenvalue(bis, 0, 1.5);

    return 0;
}