//
// Compute jump ahead coefficients for Mersenne Twister RNG.
// Ken-Ichi Ishikawa [ishikawa[at]theo.phys.sci.hiroshima-u.ac.jp]
//
// See also:
//  H. Haramoto, M. Matsumoto, T. Nishimura, F. Panneton, and P. L'Ecuyer,
//   ``Efficient Jump Ahead for F_2-Linear Random Number Generators'',
//  GERAD Report G-2006-62. INFORMS Journal on Computing, 20, 3 (2008), 385-390.
//
// This routine uses;
//  Fast arithmetic in GF(2)[x], [http://wwwmaths.anu.edu.au/~brent/software.html]
//  NTL : A Library for doing Number Theory, [http://www.shoup.net/ntl/index.html]
//
//
// Copyright (c) 2010, Ken-Ichi Ishikawa [ishikawa[at]theo.phys.sci.hiroshima-u.ac.jp]
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright
//   notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer listed
//   in this license in the documentation and/or other materials
//   provided with the distribution.
//
// * Neither the name of the copyright holders nor the names of its
//   contributors may be used to endorse or promote products derived from
//   this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//

#include <NTL/GF2X.h>
NTL_CLIENT

//#define _DEBUG_
#ifdef _DEBUG_
#define DEBUG_PRINTF(format, args...)  fprintf(stderr, format, ## args)
#else
#define DEBUG_PRINTF(format, args...)  ;
#endif

void print_hex(GF2X &f)
{
    int ww = 32;
    int nb = NumBits(f);
    int nn = (int)ceil((double)nb/(double)ww);
    unsigned int pp[nn];
    for(int i = 0; i < nn ; i++) pp[i] = 0;
    for(int i = 0; i < nb ; i++) {
      int iw = i / ww;
      int ib = i % ww;
      if (1 == coeff(f,i)) pp[iw] += (1 << ib);
    }
    if (0 != pp[nn-1]) printf("%X",pp[nn-1]);
    for(int i = nn-2; i > -1 ; i--) {
      printf("%8.8X",pp[i]);
    }
    printf("\n");
}

extern "C"
void get_coeff (const int& nn,     // MT param n
                const int& mm,     //          m
                const int& rr,     //          r
                const int& ww,     //          w => MT period_exp = n*w - r
                const int& avec,   //        aaa => companion matirx component vector a
                const int& nj,     // jump ahead step exponent   : id*(2^nj)
                const int& id,     // jump ahead step id=[0,...] : id*(2^nj)
                unsigned int *pp,  // jump ahead polynomial coefficients, pp[nn]
                int& np)           // jump ahead polynomial order(bit size)
{
  if (id <= 0) return;
  int period_exp = nn*ww-rr;
  int ns = (int)ceil(log((double)period_exp)/log(2.0));

  //
  // Compute MT characteristic polynomial f(x)
  // [see,
  //  M. Matsumoto and T. Nishimura,
  //  "Mersenne Twister: A 623-dimensionally equidistributed uniform
  //                                   pseudorandom number generator",
  //  ACM Trans. on Modeling and Computer Simulation Vol. 8, No. 1,
  //  January pp.3-30 (1998) DOI:10.1145/272991.272995 for explicit form]
  //
  // f(x) =         af^(w-r) * bf^r
  //      + a(0)  * af^(w-r) * bf^(r-1)
  //      + a(1)  * af^(w-r) * bf^(r-2)
  //      + ....
  //      + a(r-2)* af^(w-r) * bf^(1)
  //      + a(r-1)* af^(w-r)
  //      + a(r)  * af^(w-r-1)
  //      + a(r+1)* af^(w-r-2)
  //      + ...
  //      + a(w-2)* af^(1)
  //      + a(w-1)
  // where
  //   af = x^nn     + x^mm;
  //   bf = x^(nn-1) + x^(mm-1);
  //
  GF2X *af, *bf;
  af = new GF2X;
  bf = new GF2X;
  SetCoeff(*af,nn);
  SetCoeff(*af,mm);   // af = x^nn + x^mm;
  SetCoeff(*bf,nn-1);
  SetCoeff(*bf,mm-1); // bf = x^(nn-1) + x^(mm-1)
  GF2X *f;
  f = new GF2X;
  *f = power(*af,ww-rr) * power(*bf,rr);
  for (int i=0;i<rr;i++) {
    int ib = i % ww;
    int a = (avec >> ib) & 0x1;
    if (1 == a) *f += power(*af,ww-rr) * power(*bf,rr-1-i);
  }
  for (int i=rr;i<ww;i++) {
    int ib = i % ww;
    int a = (avec >> ib) & 0x1;
    if (1 == a) *f += power(*af,ww-1-i);
  }
  delete af;
  delete bf;

  //
  // compute r(x) = x^((2^nj)*id) mod f(x)
  //

  //
  // g(x) = x^(2^nj) mod f(x)
  //
  GF2X *g;
  g = new GF2X;
  if ( ns < nj ) {
    SetCoeff(*g,(1 << ns)); // g(x) = x^(2^ns)
    *g = *g % (*f);
    for (int i=ns;i < nj;i++) *g = power(*g,2) % (*f);

  } else {

    SetCoeff(*g,(1 << nj)); // g(x) = x^(2^nj)

  }

#ifdef _DEBUG_
  printf("%8d\n",ns);
  printf("%8d\n",nj);
  printf("@");
  print_hex(*f);
  printf("@");
  print_hex(*g);
#endif

  //
  // r(x) = g(x)^id mod f(x)
  //

  int id_bits = (int)floor(log(double(id))/log(2.0)) + 1; // bit size of id
  DEBUG_PRINTF("id=%d id_bits=%d\n",id,id_bits);
  GF2X *r;
  r = new GF2X;
  SetCoeff(*r,0);  // r(x) = 1
  for (int i=id_bits;i >=0; --i) {

    *r = power(*r,2) % (*f);

    if (1 == ((id >> i)& 1)) *r = ((*r) * (*g)) % (*f);

  }
  delete g;
  delete f;

#ifdef _DEBUG_
  printf("@");
  print_hex(*r);
#endif

  //
  // extract bit sequence (=pp[]) from r(x)
  //
  {
    int nb = NumBits(*r);
    for(int i = 0; i < nn ; i++) pp[i] = 0;
    for(int i = 0; i < nb ; i++) {
      int iw = i / ww;
      int ib = i % ww;
      if (1 == coeff(*r,i)) pp[iw] += (1 << ib);
    }
    np = nb;
  }
  delete r;

}


///////////////////////////////////////////////////////////////////////////////
// ars::point.hpp                                                            //
//                                                                           //
//  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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ARS_POINT_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_POINT_HPP_ER_2009
#include <ostream>
#include <boost/ars/constant.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{

// Given an unnormalized density f, this class is a representation for (x,y,dy),
// where y = log f(x).
template <typename T>
class point{
    typedef constant<T> const_;

    public:
    point(): x_(const_::zero_),y_(const_::zero_),dy_(const_::zero_){}
    point(const T& x,const T& y,const T& dy)
    : x_(x),y_(y),dy_(dy){}

    const T& x()const{ return x_; }
    const T& y()const{ return y_; }
    const T& dy()const{ return dy_; }

    private:
    //abscissa, ordinate, derivative of the log density
    T x_, y_, dy_;
};

template<typename T>
void dump(const point<T>& p,T& x,T& y,T& dy){
    x = p.x();
    y = p.y();
    dy = p.dy();
}

template <typename T>
bool operator<(
    const point<T> &a,
    const point<T> &b
){
    return (a.x() < b.x());
}

template <typename T>
std::ostream&
operator<<(std::ostream &out, const point<T>& p)
{
    out << '(' << p.x() << ',' << p.y() << ',' << p.dy() << ')';
    return out;
}

template<typename T,typename F>
point<T>
create_point(
    const T& x,
    const F& f
){
    typedef point<T> result_t;
    T y, dy;
    f(x,y,dy);
    return result_t(x,y,dy);
}

template<typename T>
T tangent(const point<T>& a, const T& x){
    return a.y() + a.dy() * (x-a.x());
}

template<typename T>
bool is_non_increasing_dy(const point<T>&a,const point<T>& b){
    return !(a.dy()<b.dy());
}

template<typename T>
bool is_concave(const point<T>&a,const point<T>& b){
    T t_b = tangent(a,b.x());
    T t_a = tangent(b,a.x());
    return !( (t_b < b.y()) || (t_a < a.y()) );
}

template<typename T>
T linearly_interpolate(
    const point<T>& a,
    const point<T>& b,
    const T& x
){
    T slope =  (b.y() - a.y()) / (b.x() - a.x());
    return a.y() + (x-a.x()) * slope;
}

}// ars
}// detail
}// statistics
}// boost

#endif // BOOST_STATISTICS_DETAIL_ARS_POINT_HPP_ER_2009


#include <boost/numeric/bindings/traits/ublas_banded.hpp>
#include <boost/numeric/bindings/traits/ublas_vector2.hpp>
#include <boost/numeric/bindings/lapack/gbsv.hpp>
#include <vector>
#include <stdexcept>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

static const char NORMAL = 'N';
static const char TRANSPOSE = 'T';

// solves the equation Ax = B, and puts the solution in B
// A is mutated by this routine
template <typename MatrA, typename MatrB>
void InPlaceSolve(MatrA& a, MatrB& b)
{
  std::vector<integer_t> piv(a.size1());
  int ret = lapack::gbtrf(a, piv);
  if (ret < 0) {
    //CStdString err;
    //err.Format("banded::Solve: argument %d in DGBTRF had an illegal value", -ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: argument %d in DGBTRF had an illegal value");
  }
  if (ret > 0) {
    //CStdString err;
    //err.Format("banded::Solve: the (%d,%d) diagonal element is 0 after DGBTRF", ret, ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: the (%d,%d) diagonal element is 0 after DGBTRF");
  }

  ret = lapack::gbtrs(NORMAL, a, piv, b);
  if (ret < 0) {
    //CStdString err;
    //err.Format("banded::Solve: argument %d in DGBTRS had an illegal value", -ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: argument %d in DGBTRS had an illegal value");
  }
}

template<typename T>
void do_typename()
{
  using namespace boost::numeric::ublas;
  // if the matrix has kl lower and ku upper diagonals, then we should
  // allocate kl lower and kl+ku upper diagonals
  size_t sz = 1000, kl = 1, ku = 1;
  ublas::banded_matrix<T> a(sz, sz, kl, kl+ku);
  ublas::vector<T> b(sz);
  // fill values in a and b
  for (size_t i = 0; i < sz; ++i) {
    a(i,i) = i;
    b(i) = i;
  }
  for (size_t i = 1; i < sz; ++i) {
    a(i,i-1) = i;
    a(i-1,i) = 1;
  }
  InPlaceSolve(a, b);
}

int main()
{
  do_typename<float>();
  do_typename<double>();
  do_typename<std::complex<float> >();
  do_typename<std::complex<double> >();
  return 0;
}


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

using namespace Eigen;
using namespace std;

int main()
{
  MatrixXd m = MatrixXd::Random(3,3);
  m = (m + MatrixXd::Constant(3,3,1.2)) * 50;
  cout << "m =" << endl << m << endl;
  VectorXd v(3);
  v << 1, 2, 3;
  cout << "m * v =" << endl << m * v << endl;
}


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Eigenvalues>
#include <chrono>
#include <cxxopts.hpp>

#include "DavidsonSolver.hpp"
#include "DavidsonOperator.hpp"
#include "MatrixFreeOperator.hpp"

#include <iostream>
#include <fstream>
#include <string>

using namespace std;

#define MAXBUFSIZE  ((int) 1e6)

Eigen::MatrixXd readMatrix(const char *filename)
    {
    int cols = 0, rows = 0;
    double buff[MAXBUFSIZE];

    // Read numbers from file into buffer.
    ifstream infile;
    infile.open(filename);
    while (! infile.eof())
        {
        string line;
        getline(infile, line);

        int temp_cols = 0;
        stringstream stream(line);
        while(! stream.eof())
            stream >> buff[cols*rows+temp_cols++];

        if (temp_cols == 0)
            continue;

        if (cols == 0)
            cols = temp_cols;

        rows++;
        }

    infile.close();

    rows--;

    // Populate matrix with numbers.
    Eigen::MatrixXd result(rows,cols);
    for (int i = 0; i < rows; i++)
        for (int j = 0; j < cols; j++)
            result(i,j) = buff[ cols*i+j ];

    return result;
    };

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

    // parse the input
    cxxopts::Options options(argv[0],  "Eigen Davidson Iterative Solver");
    options.positional_help("[optional args]").show_positional_help();
    options.add_options()
        ("size", "dimension of the matrix", cxxopts::value<std::string>()->default_value("100"))
        ("eps", "sparsity of the matrix", cxxopts::value<std::string>()->default_value("0.01"))
        ("neigen", "number of eigenvalues required", cxxopts::value<std::string>()->default_value("5"))
        ("corr", "correction method", cxxopts::value<std::string>()->default_value("DPR"))
        ("mf", "use matrix free", cxxopts::value<bool>())
        ("diag", "diagonal elements are ordered" , cxxopts::value<bool>())
        ("reorder", "reorder diagonal elements" , cxxopts::value<bool>())
        ("linsolve", "method to solve the linear system of JOCC (CG, GMRES, LLT)", cxxopts::value<std::string>()->default_value("CG"))
        ("init", "method to itialize the eigenvector (target, indentity, random)", cxxopts::value<std::string>()->default_value("target"))
        ("tol", "tolerance on the residue norm", cxxopts::value<std::string>()->default_value("1E-4"))
        ("lstol", "tolerance of the linear solver", cxxopts::value<std::string>()->default_value("0.01"))
        ("help", "Print the help", cxxopts::value<bool>());
    auto result = options.parse(argc,argv);

    if (result.count("help"))
    {
        std::cout << options.help({""}) << std::endl;
        exit(0);
    }

    int size = std::stoi(result["size"].as<std::string>(),nullptr);
    int neigen = std::stoi(result["neigen"].as<std::string>(),nullptr);
    bool mf = result["mf"].as<bool>();
    bool odiag = result["diag"].as<bool>();
    bool reorder = result["reorder"].as<bool>();
    std::string linsolve = result["linsolve"].as<std::string>();
    std::string eigen_init = result["init"].as<std::string>();
    std::string correction = result["corr"].as<std::string>();
    bool help = result["help"].as<bool>();
    double eps = std::stod(result["eps"].as<std::string>(),nullptr);
    double davidson_tol = std::stod(result["tol"].as<std::string>(),nullptr);
    double lsolve_tol = std::stod(result["lstol"].as<std::string>(),nullptr);

    // chrono
    std::chrono::time_point<std::chrono::system_clock> start, end;
    std::chrono::duration<double> elapsed_time;

    std::cout << "Matrix size : " << size << "x" << size << std::endl;
    std::cout << "Num Threads : " <<  Eigen::nbThreads() << std::endl;
    std::cout << "eps : " <<  eps << std::endl;

    // Create Operator
    DavidsonOperator Aop(size,eps,odiag,reorder);
    Eigen::MatrixXd Afull = Aop.get_full_mat();
    std::cout << "Afull" << std::endl << Afull.block(0,0,5,5) << std::endl;

    // Davidosn Solver
    start = std::chrono::system_clock::now();
    DavidsonSolver DS;

    DS.set_guess_vectors(eigen_init);
    DS.set_correction(correction);
    DS.set_tolerance(davidson_tol);

    if (correction == "JACOBI") {
        DS.set_jacobi_linsolve(linsolve);
        DS.set_linsolve_tol(lsolve_tol);
    }

    if (mf) DS.solve(Aop,neigen);
    else  DS.solve(Afull,neigen);

    end = std::chrono::system_clock::now();
    elapsed_time = end-start;
    std::cout << std::endl << "Davidson               : " << elapsed_time.count() << " secs" <<  std::endl;

    // normal eigensolver
    start = std::chrono::system_clock::now();
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es2(Afull);
    end = std::chrono::system_clock::now();
    elapsed_time = end-start;
    std::cout << "Eigen                  : " << elapsed_time.count() << " secs" <<  std::endl;

    auto dseigop = DS.eigenvalues();
    auto eig2 = es2.eigenvalues().head(neigen);
    std::cout << std::endl <<  "      Davidson  \tEigen \t\t Error" << std::endl;
    for(int i=0; i< neigen; i++)
        printf("#% 4d %8.7f \t%8.7f \t %4.2e\n",i,dseigop(i),eig2(i),abs(eig2(i)-dseigop(i)));

}


/* ****************** */
/* Include packages   */
/* ****************** */
#include <math.h>
#include <vector>
#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <algorithm>
#include <random>
#include <iostream>
#include <time.h>
#include <stack>
#include <assert.h>
//include Eigen
// #include "eigen3/Eigen/Dense"
// #include "eigen3/Eigen/Sparse"
// #include "eigen3/Eigen/SparseLU"
// #include "eigen3/Eigen/SparseQR"
// #include "eigen3/Eigen/SparseCholesky"
// #include "eigen3/Eigen/IterativeLinearSolvers"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>
#include <Eigen/SparseCholesky>
#include <Eigen/IterativeLinearSolvers>
typedef Eigen::SparseMatrix<double > SpMat;
typedef Eigen::Triplet<double> T;
#include <typeinfo>
#include "MarketIO.h"
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/eigen.h>

//include MEX related files
// #include "mex.h"
// #include "matrix.h"

/* ******************** */
/* State Variable Class */
/* ******************** */

Eigen::MatrixXd empty;
Eigen::ArrayXd emptyAry;

namespace py = pybind11;
using namespace std;
using MatrixXdR = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

/* Timer functions                                    */
/******************************************************/

std::stack<clock_t> tictoc_stack;

void tic() {
    tictoc_stack.push(clock());
}

void toc() {
    std::cout << "Time elapsed: "
    << ((double)(clock() - tictoc_stack.top())) / CLOCKS_PER_SEC
    << std::endl;
    tictoc_stack.pop();
}

class stateVars {

public:
    Eigen::MatrixXd stateMat; //matrix to store state variables
    Eigen::MatrixXd stateMatNorm; //matrix to store normalized state variables [-1,1]
    Eigen::ArrayXd increVec; //vector to record steps
    Eigen::ArrayXd dVec; //vector to record steps
    int N; // num of dimensions
    int S; // number of rows for the grid
    Eigen::ArrayXd upperLims;
    Eigen::ArrayXd lowerLims;
    Eigen::ArrayXd gridSizes;

    stateVars (Eigen::ArrayXd, Eigen::ArrayXd, Eigen::ArrayXd); //constructors with arrays of upper/lower bounds and gridsizes
    stateVars (Eigen::MatrixXd); //constructors by loading in data

};

stateVars::stateVars (Eigen::ArrayXd upper, Eigen::ArrayXd lower, Eigen::ArrayXd gridSizes) {

    upperLims = upper;
    lowerLims = lower;
    N = upperLims.size();
    S = gridSizes.prod();
    stateMat.resize(S,N);
    dVec.resize(N);
    increVec.resize(N);
    increVec(0) = 1;

    //fill in the state object; similar to the ndgrid function in MATLAB

    for (int n = 0; n < N; ++n) {

        if (n != 0) {
            increVec(n) = gridSizes(n - 1) * increVec(n - 1);
        }
        dVec(n) = (upper(n) - lower(n)) / (gridSizes(n) - 1);

        for (int i = 0; i < S; ++i) {
            stateMat(i,n) = lower(n) + dVec(n) * ( int(i /  increVec(n) ) % int( gridSizes(n) ) );
        }

    }

}

stateVars::stateVars (Eigen::MatrixXd preLoad) {

    //fill in stateMat based on data loaded
    N = preLoad.cols();
    S = preLoad.rows();
    stateMat.resize(S,N);
    dVec.resize(N); dVec.setZero();
    increVec.resize(N); increVec.setZero();
    upperLims.resize(N); lowerLims.resize(N);
    for (int j = 0; j < preLoad.cols(); ++j) {
        upperLims(j) = preLoad.col(j).maxCoeff();
        lowerLims(j) = preLoad.col(j).minCoeff();
    }

    stateMat = preLoad;

    //figure out dVec and increVec
    for (int i = 1; i < S; ++i) {
        for (int n = 0; n < N; ++n ) {
            double diff = stateMat(i,n) - stateMat(i-1,n);
            if (diff > 0 && dVec(n) == 0 && increVec(n) == 0) {
                dVec(n) = diff;
                increVec(n) = i;
            }
        }

    }

}

struct bc {
    double a0;
    double a0S;
    bool natural;
    Eigen::ArrayXd level;
    Eigen::ArrayXd first;
    Eigen::ArrayXd second;

    bc(int d) {
        level.resize(d); first.resize(d); second.resize(d);
    }
};

struct elas {
    Eigen::MatrixXd elas1sc;
    Eigen::MatrixXd elas1c; //exposure elas
    Eigen::MatrixXd elas2sc;
    Eigen::MatrixXd elas2c; //exposure elas
    Eigen::MatrixXd elas1p; //price elas
    Eigen::MatrixXd elas2p;  //price elas

    elas(int T, int S) {
        elas1sc.resize(S,T); elas1c.resize(S,T); elas1p.resize(S,T);
        elas2sc.resize(S,T); elas2c.resize(S,T); elas2p.resize(S,T);
    }
};

class linearSysVars {

public:
    double dt;
    int k;
    Eigen::MatrixXd A;
    Eigen::MatrixXd B;
    Eigen::MatrixXd C;
    Eigen::MatrixXd D;

    Eigen::ArrayXd atBoundIndicators;

    std::vector<T> matList;
    std::string solverType;
    SpMat Le;

    //member functions

    //constructor
    linearSysVars(stateVars & state_vars, Eigen::MatrixXd A, Eigen::MatrixXd B, Eigen::MatrixXd C, Eigen::MatrixXd D, double dt);

    //function to construt matrix

    void constructMatFT(stateVars & state_vars);
    void constructMatFK(stateVars & state_vars);

};

linearSysVars::linearSysVars(stateVars & state_vars, Eigen::MatrixXd AInput, Eigen::MatrixXd BInput, Eigen::MatrixXd CInput, Eigen::MatrixXd DInput, double dtInput) {

    Le.resize(state_vars.S,state_vars.S);
    A.resize(state_vars.S,1); B.resize(state_vars.S,state_vars.N);
    C.resize(state_vars.S,state_vars.N); D.resize(state_vars.S,1);
    A = AInput; B = BInput; C = CInput; D = DInput;
    dt = dtInput;

}

void linearSysVars::constructMatFT(stateVars & state_vars) {
    matList.clear();
    matList.reserve(10 * state_vars.S);
    atBoundIndicators.resize(state_vars.N);
    double atBound = -1;
    double upperBound = -1;
    //construct matrix

    for (int i = 0; i < state_vars.S; ++i) {
        //level and time deriv

        atBound = -1;
        //check boundaries

        matList.push_back(T(i,i, (1.0 - dt * A(i,0))  ));

        for (int n  = (state_vars.N - 1); n >=0; --n ) {

            atBoundIndicators(n) = -1.0;

            double firstCoefE = B(i,n);

            double secondCoefE = C(i,n);

            //check whether it's at upper or lower boundary
            if ( std::abs(state_vars.stateMat(i,n) - state_vars.upperLims(n)) < state_vars.dVec(n)/2.0 ) {  //upper boundary
                atBoundIndicators(n) = 1.0;

                atBound = 1.0;
                upperBound = 1.0;
                /* Uncomment this section if you want natural boundaries */

                 matList.push_back(T(i, i, - dt * ( firstCoefE/state_vars.dVec(n) + secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - firstCoefE/state_vars.dVec(n) - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                 matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );

                /* Uncomment this section if you want first derivatives = constant
                 matList.push_back(T(i,i, - (1.0 - dt * A(i,0) ) ));
                /*
                 matList.push_back(T(i, i, - dt * ( 1.0/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 1.0/state_vars.dVec(n)  ) ));*/
                 /*
                if ((n == 0) && atBoundIndicators(1) > 0 ) {
                 matList.push_back(T(i, i,  dt * ( firstCoefE/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n),  dt * ( - firstCoefE/state_vars.dVec(n)  ) ));
                }*/
                /* Uncomment this section if you want second derivatives = constant  */
                //matList.push_back(T(i,i, - (1.0 - dt * A(i,0) )  ));
                /*
                matList.push_back(T(i, i, - dt * (  secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                */
                /*
                matList.push_back(T(i, i, - dt * (  1.0 / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 2 *  1.0 / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                */
            } else if ( std::abs(state_vars.stateMat(i,n) - state_vars.lowerLims(n)) < state_vars.dVec(n)/2.0 ) { //lower boundary

                atBoundIndicators(n) = 1.0;
                atBound = 1.0;

                ///* Uncomment this section if you want natural boundaries

                 matList.push_back(T(i, i, - dt * ( - firstCoefE/state_vars.dVec(n) + secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                 matList.push_back(T(i, i + state_vars.increVec(n), - dt * ( firstCoefE/state_vars.dVec(n) - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                 matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );

                //*/
                /* Uncomment this section if you want first derivatives = constant
                 */
                // matList.push_back(T(i,i, - (1.0 - dt * A(i,0) ) ));
                // matList.push_back(T(i, i, - dt * ( - 1/state_vars.dVec(n)  ) ) );
                // matList.push_back(T(i, i + state_vars.increVec(n), - dt * ( 1/state_vars.dVec(n) ) ));
                 /*
                if ((n == 0) && atBoundIndicators(1) > 0 ) {
                 matList.push_back(T(i, i,  dt * ( - firstCoefE/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i + state_vars.increVec(n),  dt * ( firstCoefE/state_vars.dVec(n) ) ));
                }*/
                /* Uncomment this section if you want second derivatives = constant  */
                //matList.push_back(T(i,i, - (1.0 - dt * A(i,0) )/ state_vars.N ));
                /*
                matList.push_back(T(i, i, - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i + state_vars.increVec(n), - dt * (  - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                */
                /*
                    matList.push_back(T(i, i, - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                    matList.push_back(T(i, i + state_vars.increVec(n), - dt * (  - 2 * 1.0 / pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                */

            }

        }

        if (atBound < 0 ) {
          // matList.push_back(T(i,i, (1.0 - dt * A(i,0))  ));
        }
        for (int n = (state_vars.N - 1); n >= 0; --n) {

            //add elements to the vector of triplets for matrix construction
            if ( atBoundIndicators(n) < 0) {
                double firstCoefE = B(i,n);
                double secondCoefE = C(i,n);

                //first derivative
                 matList.push_back(T(i,i, - dt * ( -firstCoefE * ( firstCoefE > 0) + firstCoefE * ( firstCoefE < 0) ) / state_vars.dVec(n)  ) );
                 matList.push_back(T(i,i + state_vars.increVec(n), - dt * firstCoefE * ( firstCoefE > 0) / state_vars.dVec(n) ));
                 matList.push_back(T(i,i - state_vars.increVec(n), - dt *  - firstCoefE * ( firstCoefE < 0) / state_vars.dVec(n) ));

                    matList.push_back(T(i, i, - dt * -2 * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i + state_vars.increVec(n), - dt * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i - state_vars.increVec(n), - dt * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));

            }

        }

    }
    //form matrices

    Le.setFromTriplets(matList.begin(), matList.end());

    //compress
    Le.makeCompressed();
}

void linearSysVars::constructMatFK(stateVars & state_vars) {
    matList.clear();
    matList.reserve(10 * state_vars.S);
    atBoundIndicators.resize(state_vars.N);
    double atBound = -1;
    double upperBound = -1;
    //construct matrix

    for (int i = 0; i < state_vars.S; ++i) {
        //level and time deriv

        atBound = -1;
        //check boundaries

        matList.push_back(T(i,i, (0.0 - dt * A(i,0))  ));

        for (int n  = (state_vars.N - 1); n >=0; --n ) {

            atBoundIndicators(n) = -1.0;

            double firstCoefE = B(i,n);

            double secondCoefE = C(i,n);

            //check whether it's at upper or lower boundary
            if ( std::abs(state_vars.stateMat(i,n) - state_vars.upperLims(n)) < state_vars.dVec(n)/2.0 ) {  //upper boundary
                atBoundIndicators(n) = 1.0;

                atBound = 1.0;
                upperBound = 1.0;
                /* Uncomment this section if you want natural boundaries */

                 matList.push_back(T(i, i, - dt * ( firstCoefE/state_vars.dVec(n) + secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - firstCoefE/state_vars.dVec(n) - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                 matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );

                /* Uncomment this section if you want first derivatives = constant
                 matList.push_back(T(i,i, - (1.0 - dt * A(i,0) ) ));
                /*
                 matList.push_back(T(i, i, - dt * ( 1.0/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 1.0/state_vars.dVec(n)  ) ));*/
                 /*
                if ((n == 0) && atBoundIndicators(1) > 0 ) {
                 matList.push_back(T(i, i,  dt * ( firstCoefE/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n),  dt * ( - firstCoefE/state_vars.dVec(n)  ) ));
                }*/
                /* Uncomment this section if you want second derivatives = constant  */
                //matList.push_back(T(i,i, - (1.0 - dt * A(i,0) )  ));
                /*
                matList.push_back(T(i, i, - dt * (  secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                */
                /*
                matList.push_back(T(i, i, - dt * (  1.0 / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 2 *  1.0 / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                */
            } else if ( std::abs(state_vars.stateMat(i,n) - state_vars.lowerLims(n)) < state_vars.dVec(n)/2.0 ) { //lower boundary

                atBoundIndicators(n) = 1.0;
                atBound = 1.0;

                ///* Uncomment this section if you want natural boundaries

                 matList.push_back(T(i, i, - dt * ( - firstCoefE/state_vars.dVec(n) + secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                 matList.push_back(T(i, i + state_vars.increVec(n), - dt * ( firstCoefE/state_vars.dVec(n) - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                 matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );

                //*/
                /* Uncomment this section if you want first derivatives = constant
                 */
                // matList.push_back(T(i,i, - (1.0 - dt * A(i,0) ) ));
                // matList.push_back(T(i, i, - dt * ( - 1/state_vars.dVec(n)  ) ) );
                // matList.push_back(T(i, i + state_vars.increVec(n), - dt * ( 1/state_vars.dVec(n) ) ));
                 /*
                if ((n == 0) && atBoundIndicators(1) > 0 ) {
                 matList.push_back(T(i, i,  dt * ( - firstCoefE/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i + state_vars.increVec(n),  dt * ( firstCoefE/state_vars.dVec(n) ) ));
                }*/
                /* Uncomment this section if you want second derivatives = constant  */
                //matList.push_back(T(i,i, - (1.0 - dt * A(i,0) )/ state_vars.N ));
                /*
                matList.push_back(T(i, i, - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i + state_vars.increVec(n), - dt * (  - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                */
                /*
                    matList.push_back(T(i, i, - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                    matList.push_back(T(i, i + state_vars.increVec(n), - dt * (  - 2 * 1.0 / pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                */

            }

        }

        if (atBound < 0 ) {
          // matList.push_back(T(i,i, (1.0 - dt * A(i,0))  ));
        }
        for (int n = (state_vars.N - 1); n >= 0; --n) {

            //add elements to the vector of triplets for matrix construction
            if ( atBoundIndicators(n) < 0) {
                double firstCoefE = B(i,n);
                double secondCoefE = C(i,n);

                //first derivative
                 matList.push_back(T(i,i, - dt * ( -firstCoefE * ( firstCoefE > 0) + firstCoefE * ( firstCoefE < 0) ) / state_vars.dVec(n)  ) );
                 matList.push_back(T(i,i + state_vars.increVec(n), - dt * firstCoefE * ( firstCoefE > 0) / state_vars.dVec(n) ));
                 matList.push_back(T(i,i - state_vars.increVec(n), - dt *  - firstCoefE * ( firstCoefE < 0) / state_vars.dVec(n) ));

                    matList.push_back(T(i, i, - dt * -2 * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i + state_vars.increVec(n), - dt * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i - state_vars.increVec(n), - dt * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));

            }

        }

    }
    //form matrices

    Le.setFromTriplets(matList.begin(), matList.end());

    //compress
    Le.makeCompressed();
}

py::tuple solveFT(Eigen::Ref<MatrixXdR> preLoadMat, Eigen::Ref<MatrixXdR> A, Eigen::Ref<MatrixXdR> B, Eigen::Ref<MatrixXdR> C,  Eigen::Ref<MatrixXdR> D, Eigen::Ref<MatrixXdR> v0, double dt, int tol)
{
    py::tuple data(3);
    stateVars stateSpace(preLoadMat);

    linearSysVars linearSys_vars(stateSpace, A,B,C,D,dt);
    linearSys_vars.constructMatFT(stateSpace);

    Eigen::VectorXd rhs;

    rhs = v0.array() + dt * D.array(); // transform v0 into rhs
    /*********************************************/
    /* Change RHS to reflect boundary conditions */
    /*********************************************/

    //construct matrix
    /* uncomment this section if you want to set the boundary conditions to a constant */
    // for (int i = 0; i < stateSpace.S; ++i) {

    //     for (int n = (stateSpace.N - 1); n >=0; --n ) {

    //         //check whether it's at upper or lower boundary
    //         if ( std::abs(stateSpace.stateMat(i,n) - stateSpace.upperLims(n)) < stateSpace.dVec(n)/2 ) {  //upper boundary
    //          //   v0(i) = 0.0001;
    //         } else if ( std::abs( stateSpace.stateMat(i,n) - stateSpace.lowerLims(n)) < stateSpace.dVec(n)/2 ) { //lower boundary
    //          //v0(i) = 0.0001;
    //         }
    //     }
    // }

    /* Initialize Eigen's cg solver */

    Eigen::VectorXd XiEVector;
    Eigen::LeastSquaresConjugateGradient<SpMat > cgE;
    // cgE.setMaxIterations(10000);
    cgE.setTolerance( pow(10,tol) );
    cgE.compute(linearSys_vars.Le);

    XiEVector = cgE.solveWithGuess(rhs, v0);
    data[0] = int(cgE.iterations());
    data[1] = cgE.error();
    data[2] = XiEVector;
    return data;

}

py::tuple solveFK(Eigen::Ref<MatrixXdR> preLoadMat, Eigen::Ref<MatrixXdR> A, Eigen::Ref<MatrixXdR> B, Eigen::Ref<MatrixXdR> C,  Eigen::Ref<MatrixXdR> D, Eigen::Ref<MatrixXdR> v0, int iters)
{
    py::tuple data(3);
    stateVars stateSpace(preLoadMat);
    double dt(1.0);
    linearSysVars linearSys_vars(stateSpace, A,B,C,D,dt);
    linearSys_vars.constructMatFK(stateSpace);

    Eigen::VectorXd rhs;
    rhs =  dt * D.array(); // transform v0 into rhs
    /*********************************************/
    /* Change RHS to reflect boundary conditions */
    /*********************************************/

    //construct matrix
    /* uncomment this section if you want to set the boundary conditions to a constant */
    for (int i = 0; i < stateSpace.S; ++i) {

        for (int n = (stateSpace.N - 1); n >=0; --n ) {

            //check whether it's at upper or lower boundary
            if ( std::abs(stateSpace.stateMat(i,n) - stateSpace.upperLims(n)) < stateSpace.dVec(n)/2 ) {  //upper boundary
             //   v0(i) = 0.0001;
            } else if ( std::abs( stateSpace.stateMat(i,n) - stateSpace.lowerLims(n)) < stateSpace.dVec(n)/2 ) { //lower boundary
             //v0(i) = 0.0001;
            }
        }
    }

    /* Initialize Eigen's cg solver */
    Eigen::VectorXd XiEVector;
    Eigen::LeastSquaresConjugateGradient<SpMat > cgE;
    cgE.setMaxIterations(iters);
    cgE.setTolerance( 0.000001 );
    cgE.compute(linearSys_vars.Le);  // update with Sparse matrix A
    XiEVector = cgE.solveWithGuess(rhs,v0);  // (rhs, guess)
    data[0] = int(cgE.iterations());
    data[1] = cgE.error();
    data[2] = XiEVector;

    return data;

}
/*************************************/
/* Using pybind11 to interface       */
/* with python                       */
/*************************************/

PYBIND11_MODULE(SolveLinSys,m){
    m.doc() = "PDE Solver in cpp";

    m.def("solveFT", &solveFT, py::arg("stateSpace"),
        py::arg("A"), py::arg("B"), py::arg("C"), py::arg("D"),
        py::arg("v0"), py::arg("dt"), py::arg("tol"));

    m.def("solveFK", &solveFK, py::arg("stateSpace"),
        py::arg("A"), py::arg("B"), py::arg("C"), py::arg("D"),
        py::arg("v0"), py::arg("iters"));

}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Press W.H., et al. Numerical Recipes in C++: The Art of Scientific Computing. Cambridge
 *          University Press, February 2002.
 *
 */

#include <boost/multi_array.hpp>

#include "Tudat/Mathematics/Interpolators/linearInterpolator.h"

namespace tudat
{
namespace interpolators
{

//! Compute linear interpolation.
double computeLinearInterpolation( const Eigen::VectorXd& sortedIndependentVariables,
                                   const Eigen::VectorXd& associatedDependentVariables,
                                   const double targetIndependentVariableValue )
{
    // Declare local variables.
    // Declare nearest neighbor.
    int nearestNeighbor;
    double locationTargetIndependentVariableValueInInterval;

    // Compute nearest neighbor in sorted vector of independent variables.
    // Result is always to the left of the target independent variable value.
    nearestNeighbor = basic_mathematics::computeNearestLeftNeighborUsingBinarySearch(
            sortedIndependentVariables, targetIndependentVariableValue );

    // Compute location of target independent variable value in interval
    // between nearest neighbors.
    locationTargetIndependentVariableValueInInterval
            = ( targetIndependentVariableValue
              - sortedIndependentVariables[ nearestNeighbor ] )
             / ( sortedIndependentVariables[ nearestNeighbor + 1 ]
                 - sortedIndependentVariables[ nearestNeighbor ] );

    // Return the computed value of the dependent variable.
    return ( associatedDependentVariables[ nearestNeighbor ]
             * ( 1 - locationTargetIndependentVariableValueInInterval )
             + associatedDependentVariables[ nearestNeighbor + 1 ]
             * locationTargetIndependentVariableValueInInterval );
}

//! Compute linear interpolation.
Eigen::VectorXd computeLinearInterpolation(
        const std::map < double, Eigen::VectorXd >& sortedIndepedentAndDependentVariables,
        const double targetIndependentVariableValue )
{
    // Declare local variables.
    // Declare nearest neighbor.
    int nearestLeftNeighbor;

    // Declare location of target independent variable value in interval.
    double locationTargetIndependentVariableValueInInterval;

    // Declare map iterators
    std::map< double, Eigen::VectorXd >::const_iterator mapIteratorIntervalLeft;
    std::map< double, Eigen::VectorXd >::const_iterator mapIteratorIntervalRight;

    // Compute nearest neighbor in map of data.
    // Result is always to the left of the target independent variable value.
    nearestLeftNeighbor = basic_mathematics::computeNearestLeftNeighborUsingBinarySearch(
                sortedIndepedentAndDependentVariables, targetIndependentVariableValue );

    // Compute location of target independent variable value in interval
    // between nearest neighbors.
    mapIteratorIntervalLeft = sortedIndepedentAndDependentVariables.begin( );
    advance( mapIteratorIntervalLeft, nearestLeftNeighbor );
    mapIteratorIntervalRight = sortedIndepedentAndDependentVariables.begin( );
    advance( mapIteratorIntervalRight, nearestLeftNeighbor + 1 );
    locationTargetIndependentVariableValueInInterval
            = ( targetIndependentVariableValue
              - mapIteratorIntervalLeft->first )
             / ( mapIteratorIntervalRight->first
                 - mapIteratorIntervalLeft->first );

    // Return the computed value of the dependent variable.
    return ( mapIteratorIntervalLeft->second
             * ( 1 - locationTargetIndependentVariableValueInInterval )
             + mapIteratorIntervalRight->second
             * locationTargetIndependentVariableValueInInterval );
}

template class LinearInterpolator< double, Eigen::VectorXd >;
template class LinearInterpolator< double, Eigen::Vector6d >;
template class LinearInterpolator< double, Eigen::MatrixXd >;

template class LinearInterpolator< double, Eigen::Matrix< long double, Eigen::Dynamic, 1 > >;
template class LinearInterpolator< double, Eigen::Matrix< long double, Eigen::Dynamic, 6 > >;
template class LinearInterpolator< double, Eigen::Matrix< long double, Eigen::Dynamic,  Eigen::Dynamic > >;

} // namespace interpolators
} // mamespace tudat


#include "Solvers.h"
#include <Eigen/PardisoSupport>

int Temporalsolver(string typeSolver, double t_Incre, int maxIter, MatrixXd& T)
{
    // ****************************************************************************************************************************************************
    //                          EULER               //                     Solved by Euler integration. It is the fastest but less stable method.
    // ****************************************************************************************************************************************************
    if (typeSolver == "Euler")
    {
        int size;                         //Number on nodes
        int boundaryNodes;                //Number of nodes with boundary condition

        SparseMatrix<double> kl_S;        //Vectors and matrices incluiding boundary conditions
        SparseMatrix<double> kr_S;
        VectorXd T0;
        VectorXd QL;
        VectorXd c;
        double time = 0;

        int count;
        VectorXd T_4;
        VectorXd T_col;
        MatrixXd c_diago;

        count = 0;

        do
        {
            ObjectsDefinition(size, boundaryNodes, kl_S, kr_S, T0, QL, c, T.col(count), time);

            if (count == 0)
            {
                T.col(0) = T0;                   // Initial conditions
            }

            c = c.cwiseInverse();
            c_diago = c.asDiagonal();
            T_col = T.col(count);
            T_4 = T_col.array().pow(4);

            T.col(count + 1) = T.col(count) + t_Incre * c_diago * (kl_S.selfadjointView<Upper>() * T.col(count) + QL + kr_S.selfadjointView<Upper>() * T_4);          //Finite differential equation

            time += t_Incre;
            count++;
            std::cout << "count" << count << "\n";
        } while (count + 1 < maxIter);
    }

    // ****************************************************************************************************************************************************
    //      ADAMS BASHFORTH 2      //      Second order explicit method --- U(n+1) <- U(n) + Dt/2 ( 3 F(n)-F(U(n-1) ) --- x2 times slower than Euler
    // ****************************************************************************************************************************************************
    else if (typeSolver == "AB2")
    {
        int size;                         //Number on nodes
        int boundaryNodes;                //Number of nodes with boundary condition

        SparseMatrix<double> kl_S;        //Vectors and matrices incluiding boundary conditions
        SparseMatrix<double> kr_S;
        VectorXd T0;
        VectorXd QL;
        VectorXd c;
        double time = 0;

        int count;
        VectorXd T_4_0;
        VectorXd T_col_0;
        VectorXd F_0;
        MatrixXd c_diago;

        VectorXd T_4_1;
        VectorXd T_col_1;
        VectorXd F_1;

        count = 0;

        do
        {
            ObjectsDefinition(size, boundaryNodes, kl_S, kr_S, T0, QL, c, T.col(count), time);

            c = c.cwiseInverse();
            c_diago = c.asDiagonal();

            if (count == 0)
            {
                T.col(0) = T0;                             // Initial conditions
            }

            //count
            T_col_0 = T.col(count);
            T_4_0 = T_col_0.array().pow(4);
            F_0 = c_diago * (kl_S.selfadjointView<Upper>() * T.col(count) + QL + kr_S.selfadjointView<Upper>() * T_4_0);

            if (count == 0)
            {
                T.col(1) = T0 + t_Incre * F_0;             //Aproximation of second temperature term with Euler
            }

            //count + 1
            T_col_1 = T.col(count + 1);
            T_4_1 = T_col_1.array().pow(4);
            F_1 = c_diago * (kl_S.selfadjointView<Upper>() * T.col(count + 1) + QL + kr_S.selfadjointView<Upper>() * T_4_1);

            T.col(count + 2) = T.col(count + 1) + t_Incre * (3. / 2. * F_1 - 1. / 2. * F_0);          //Finite differences equation

            time += t_Incre;
            count++;
            std::cout << "count" << count << "\n";
        } while (count + 2 < maxIter);

    }

    // ****************************************************************************************************************************************************
    //                                            RUNGE KUTTA 4         // Fourth order explicit method. x4 times slower than Euler
    // ****************************************************************************************************************************************************
    else if (typeSolver == "RK4")
    {

        int size;                         //Number on nodes
        int boundaryNodes;                //Number of nodes with boundary condition

        SparseMatrix<double> kl_S;        //Vectors and matrices incluiding boundary conditions
        SparseMatrix<double> kr_S;
        VectorXd T0;
        VectorXd QL;
        VectorXd c;
        double time = 0;

        int count;
        VectorXd T_4;
        VectorXd T_col;
        MatrixXd c_diago;

        VectorXd k1, k2, k3, k4;

        count = 0;

        do
        {
            ObjectsDefinition(size, boundaryNodes, kl_S, kr_S, T0, QL, c, T.col(count), time);

            if (count == 0)
            {
                T.col(0) = T0;                   // Initial conditions
            }

            c = c.cwiseInverse();
            c_diago = c.asDiagonal();

            // k1
            T_col = T.col(count);
            T_4 = T_col.array().pow(4);
            k1 = c_diago * (kl_S.selfadjointView<Upper>() * T_col + QL + kr_S.selfadjointView<Upper>() * T_4);

            // k2
            T_col = T.col(count) + t_Incre / 2 * k1;
            T_4 = T_col.array().pow(4);
            k2 = c_diago * (kl_S.selfadjointView<Upper>() * T_col + QL + kr_S.selfadjointView<Upper>() * T_4);

            // k3
            T_col = T.col(count) + t_Incre / 2 * k2;
            T_4 = T_col.array().pow(4);
            k3 = c_diago * (kl_S.selfadjointView<Upper>() * T_col + QL + kr_S.selfadjointView<Upper>() * T_4);

            // k4
            T_col = T.col(count) + t_Incre * k3;
            T_4 = T_col.array().pow(4);
            k4 = c_diago * (kl_S.selfadjointView<Upper>() * T_col + QL + kr_S.selfadjointView<Upper>() * T_4);

            T.col(count + 1) = T.col(count) + 1. / 6. * t_Incre * (k1 + 2 * k2 + 2 * k3 + k4);          //Finite differential equation

            time += t_Incre;
            count++;
            std::cout << "count" << count << "\n";
        } while (count + 1 < maxIter);
    }

    else if (typeSolver == "CN")
    {
        int size;                         //Number on nodes
        int boundaryNodes;                //Number of nodes with boundary condition

        SparseMatrix<double> kl_S;          //Vectors and matrices incluiding boundary conditions
        SparseMatrix<double> kr_S;
        VectorXd T0;
        VectorXd QL;
        VectorXd c;
        double time = 0;

        SparseMatrix<double> kle_S;
        SparseMatrix<double> kre_S;
        VectorXd QLe;

        int count;
        VectorXd T_4;
        VectorXd T_col;
        SparseMatrix<double> c_diago;

        count = 0;

        do
        {
            ObjectsDefinition(size, boundaryNodes, kl_S, kr_S, T0, QL, c, T.col(count), time);

            if (count == 0)
            {
                T.col(0) = T0;                    //Initial conditions
            }

            size = T0.size();

            SparseMatrix<double> AuxS;
            AuxS = kl_S.selfadjointView<Upper>();
            kl_S = AuxS;
            AuxS = kr_S.selfadjointView<Upper>();
            kr_S = AuxS;

            MatrixXd Aux;
            c = c.cwiseInverse();
            Aux = c.asDiagonal();
            c_diago = Aux.sparseView();

            // QLe
            T_col = T.col(count);
            T_4 = T_col.array().pow(4);
            QLe = t_Incre / 2. * c_diago * (kl_S * T_col + 2 * QL + kr_S * T_4) + T_col;

            //kle
            SparseMatrix<double> I(size, size);
            I.setIdentity();
            kle_S = t_Incre / 2. * c_diago * kl_S - I;

            // kre
            kre_S = t_Incre / 2. * c_diago * kr_S;

            StableStationarySolverInternal(kle_S, kre_S, QLe, T_col);
            time += t_Incre;
            count++;

            T.col(count) = T_col;

        } while (count + 1 < maxIter);

    }

    return 0;
}


//******************************************************************************
//  Copyright(C) 2008-2013 Intel Corporation. All Rights Reserved.
//
//  The source code, information  and  material ("Material") contained herein is
//  owned  by Intel Corporation or its suppliers or licensors, and title to such
//  Material remains  with Intel Corporation  or its suppliers or licensors. The
//  Material  contains proprietary information  of  Intel or  its  suppliers and
//  licensors. The  Material is protected by worldwide copyright laws and treaty
//  provisions. No  part  of  the  Material  may  be  used,  copied, reproduced,
//  modified, published, uploaded, posted, transmitted, distributed or disclosed
//  in any way  without Intel's  prior  express written  permission. No  license
//  under  any patent, copyright  or  other intellectual property rights  in the
//  Material  is  granted  to  or  conferred  upon  you,  either  expressly,  by
//  implication, inducement,  estoppel or  otherwise.  Any  license  under  such
//  intellectual  property  rights must  be express  and  approved  by  Intel in
//  writing.
//
//  *Third Party trademarks are the property of their respective owners.
//
//  Unless otherwise  agreed  by Intel  in writing, you may not remove  or alter
//  this  notice or  any other notice embedded  in Materials by Intel or Intel's
//  suppliers or licensors in any way.
//
//******************************************************************************
// Content:
//     Intel(R) Math Kernel Library (MKL) overloaded Boost/uBLAS prod()
//******************************************************************************

#ifndef _MKL_BOOST_UBLAS_MATRIX_PROD_
#define _MKL_BOOST_UBLAS_MATRIX_PROD_

#ifdef NDEBUG

#include <boost/version.hpp>
#if defined (BOOST_VERSION) && (BOOST_VERSION >= 103401)

#include <boost/numeric/ublas/matrix.hpp>

#include "mkl_boost_ublas_gemm.hpp"

namespace boost { namespace numeric { namespace ublas {

    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, m2 )
    prod(const matrix<T,F,A> &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), m2 )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), m2 )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), m2 )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, trans(m2) )
    prod(const matrix<T,F,A> &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), trans(m2) )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), trans(m2) )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), trans(m2) )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, trans(conj(m2)) )
    prod(const matrix<T,F,A> &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), trans(conj(m2)) )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), trans(conj(m2)) )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), trans(conj(m2)) )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, conj(trans(m2)) )
    prod(const matrix<T,F,A> &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), conj(trans(m2)) )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), conj(trans(m2)) )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), conj(trans(m2)) )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }

}}}
#endif  // BOOST_VERSION
#endif  // NDEBUG
#endif  // _MKL_BOOST_UBLAS_MATRIX_PROD_


#include "ScatterDraw.h"
#include <Eigen/Eigen.h>

namespace Upp {
using namespace Eigen;

struct Equation_functor : NonLinearOptimizationFunctor<double> {
	DataSource *series;
	ExplicitEquation *fSource;
	Equation_functor() {}

	int operator()(const VectorXd &b, VectorXd &fvec) const {
		ASSERT(b.size() == unknowns);
		ASSERT(fvec.size() == datasetLen);
		for (int i = 0; i < unknowns; ++i)
			(*fSource).SetCoeffVal(i, b(i));
		for(int64 i = 0; i < datasetLen; i++)
			fvec(ptrdiff_t(i)) = (*fSource).f((*series).x(i)) - (*series).y(i);
		return 0;
	}
};

void ExplicitEquation::SetNumCoeff(int num) {
	coeff.SetCount(num);
	for (int i = 0; i < num; ++i)
		coeff[i] = 0;
}

ExplicitEquation::FitError ExplicitEquation::Fit(DataSource &serie, double &r2) {
	r2 = Null;

	if (serie.IsExplicit() || serie.IsParam())
		return InadequateDataSource;

	if (serie.GetCount() < coeff.GetCount())
		return SmallDataSource;

	ptrdiff_t numUnknowns = coeff.GetCount();

	VectorXd x(numUnknowns);
	for (int i = 0; i < numUnknowns; ++i)
		x(i) = coeff[i];

	Equation_functor functor;
	functor.series = &serie;
	functor.fSource = this;
	functor.unknowns = numUnknowns;
	functor.datasetLen = Eigen::Index(serie.GetCount());

	NumericalDiff<Equation_functor> numDiff(functor);
	LevenbergMarquardt<NumericalDiff<Equation_functor> > lm(numDiff);
// 	ftol is a nonnegative input variable that measures the relative error desired in the sum of squares
	lm.parameters.ftol = 1.E4*NumTraits<double>::epsilon();
//  xtol is a nonnegative input variable that measures the relative error desired in the approximate solution
	lm.parameters.xtol = 1.E4*NumTraits<double>::epsilon();
	lm.parameters.maxfev = maxFitFunctionEvaluations;
	int ret = lm.minimize(x);
	if (ret == LevenbergMarquardtSpace::ImproperInputParameters)
		return ExplicitEquation::ImproperInputParameters;
	else if (ret == LevenbergMarquardtSpace::TooManyFunctionEvaluation)
		return TooManyFunctionEvaluation;

	r2 = R2Y(serie);

	return NoError;
}

double ExplicitEquation::R2Y(DataSource &serie, double mean) {
	if (!IsNum(mean))
		mean = serie.AvgY();
	double sse = 0, sst = 0;
	for (int64 i = 0; i < serie.GetCount(); ++i) {
		double y = serie.y(i);
		if (!!IsNum(y)) {
			double err = y - f(serie.x(i));
			sse += err*err;
			double d = y - mean;
			sst += d*d;
		}
	}
	if (sst < 1E-50 || sse > sst)
		return 0;
	return 1 - sse/sst;
}

int ExplicitEquation::maxFitFunctionEvaluations = 2000;

double PolynomialEquation::f(double x) {
	if (x < 0)
		return Null;
	double y = 0;
	for (int i = 0; i < coeff.GetCount(); ++i)
		y += coeff[i]*pow(x, i);
	return y;
}

String PolynomialEquation::GetEquation(int numDigits) {
	if (coeff.IsEmpty())
		return String();
	String ret = FormatCoeff(0, numDigits);
	if (coeff.GetCount() == 1)
		return ret;
	ret += Format(" + %s*x", FormatCoeff(1, numDigits));
	for (int i = 2; i < coeff.GetCount(); ++i)
		ret += Format(" + %s*x^%s", FormatCoeff(i, numDigits), FormatInt(i));
	ret.Replace("+ -", "- ");
	return ret;
}

double FourierEquation::f(double x) {
	double y = coeff[0];
	double w = coeff[1];
	for (int i = 2; i < coeff.GetCount(); i += 2) {
		int n = 1 + (i - 2)/2;
		y += coeff[i]*cos(n*w*x) + coeff[i+1]*sin(n*w*x);
	}
	return y;
}

String FourierEquation::GetEquation(int numDigits) {
	if (coeff.GetCount() < 4)
		return String();
	String ret = FormatCoeff(0, numDigits);

	for (int i = 2; i < coeff.GetCount(); i += 2) {
		int n = 1 + (i - 2)/2;
		String nwx = Format("%d*%s*x", n, FormatCoeff(1, numDigits));
		ret += Format(" + %s*cos(%s)", FormatCoeff(i, numDigits), nwx);
		ret += Format(" + %s*sin(%s)", FormatCoeff(i + 1, numDigits), nwx);
	}
	ret.Replace("+ -", "- ");
	return ret;
}

static inline double DegToRad(double deg) {return deg*M_PI/180.;}
static inline double RadToDeg(double rad) {return rad*180./M_PI;}

void EvalExpr::EvalThrowError(CParserPP &p, const char *s) {
	CParserPP::Pos pos = p.GetPos();
	CParserPP::Error err(Format("(%d): ", pos.GetColumn()) + String(s));
	throw err;
}

doubleUnit usqrt(doubleUnit val) {
	val.Sqrt();
	return val;
}

doubleUnit ufabs(doubleUnit val) {
	val.val = fabs(val.val);
	return val;
}

doubleUnit uceil(doubleUnit val) {
	val.val = ceil(val.val);
	return val;
}

doubleUnit ufloor(doubleUnit val) {
	val.val = floor(val.val);
	return val;
}

doubleUnit uround(doubleUnit val) {
	val.val = round(val.val);
	return val;
}

doubleUnit usin(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = sin(val.val);
	return val;
}

doubleUnit ucos(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = cos(val.val);
	return val;
}

doubleUnit utan(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = tan(val.val);
	return val;
}

doubleUnit uasin(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = asin(val.val);
	return val;
}

doubleUnit uacos(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = acos(val.val);
	return val;
}

doubleUnit uatan(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = atan(val.val);
	return val;
}

doubleUnit usinh(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = sinh(val.val);
	return val;
}

doubleUnit ucosh(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = cosh(val.val);
	return val;
}

doubleUnit utanh(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = tanh(val.val);
	return val;
}

doubleUnit uexp(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = exp(val.val);
	return val;
}

doubleUnit uDegToRad(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = DegToRad(val.val);
	return val;
}

doubleUnit uRadToDeg(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = RadToDeg(val.val);
	return val;
}

doubleUnit ulog(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = log(val.val);
	return val;
}

doubleUnit ulog10(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = log10(val.val);
	return val;
}

EvalExpr::EvalExpr() {
	noCase = false;
	errorIfUndefined = false;
	allowString = false;

	constants.Add("pi", doubleUnit(M_PI));
	constants.Add("e", doubleUnit(M_E));

	functions.Add("abs", ufabs);
	functions.Add("ceil", uceil);
	functions.Add("floor", ufloor);
	functions.Add("round", uround);
	functions.Add("sqrt", usqrt);
	functions.Add("sin", usin);
	functions.Add("cos", ucos);
	functions.Add("tan", utan);
	functions.Add("asin", uasin);
	functions.Add("acos", uacos);
	functions.Add("atan", uatan);
	functions.Add("sinh", usinh);
	functions.Add("cosh", ucosh);
	functions.Add("tanh", utanh);
	functions.Add("log", ulog);
	functions.Add("log10", ulog10);
	functions.Add("exp", uexp);
	functions.Add("degToRad", uDegToRad);
	functions.Add("radToDeg", uRadToDeg);
}

doubleUnit EvalExpr::Term(CParserPP& p) {
	p.Char('+');
	bool isneg = p.Char('-');
	if (p.IsId()) {
		String strId = p.ReadIdPP();
		if(doubleUnit (*function)(doubleUnit) = functions.Get(strId, 0)) {
			p.PassChar('(');
			doubleUnit x(Exp(p));
			p.PassChar(')');
			doubleUnit ret(function(x));
			if (IsNull(ret))
				EvalThrowError(p, Format(t_("Error in %s(%f)"), strId, x.val));
			if (isneg)
				ret.Neg();
			return ret;
		}
		String strIdSearch;
		if (noCase)
			strIdSearch = ToLower(strId);
		else
			strIdSearch = strId;
		doubleUnit ret(constants.Get(strIdSearch, Null));
		if (IsNull(ret)) {
			int id = FindVariable(strIdSearch);
			if (id >= 0)
				ret = variables[id];
			else {
				if (errorIfUndefined) {
					lastError = Format(t_("Unknown identifier '%s'"), strId);
					return Null;
				}
					//EvalThrowError(p, Format(t_("Unknown identifier '%s'"), strId));
				lastVariableSetId = variables.FindAdd(strIdSearch, 0);
				ret = variables[lastVariableSetId];
			}
		}
		if (isneg)
			ret.Neg();
		return ret;
	} else if (p.Char('(')) {
		doubleUnit x(Exp(p));
		p.PassChar(')');
		if (isneg)
			x.Neg();
		return x;
	} else {
		if (p.IsChar2('.', '.'))
			p.ThrowError("missing number");
		doubleUnit x(p.ReadDouble());
		if (isneg)
			x.Neg();
		return x;
	}
}

doubleUnit EvalExpr::Pow(CParserPP& p) {
	doubleUnit x(Term(p));
	for(;;)
		if(p.Char('^')) {
			//if (x.val < 0)
			//	EvalThrowError(p, t_("Complex number"));
			x.Exp(Term(p));
		} else
			return x;
}

doubleUnit EvalExpr::Mul(CParserPP& p) {
	doubleUnit x(Pow(p));
	for(;;)
		if(p.Char('*'))
			x.Mult(Pow(p));
		else if (p.Char2('|', '|'))
			x.ResParallel(Pow(p));
		else if(memcmp(p.GetPtr(), "·", strlen("·")) == 0) {
			CParserPP::Pos pos = p.GetPos();
			pos.ptr += strlen("·");
			p.SetPos(pos);
			p.Spaces();
			x.Mult(Pow(p));
		} else if(p.Char('/')) {
			x.Div(Pow(p));
		} else if(memcmp(p.GetPtr(), "º", strlen("º")) == 0) {
			CParserPP::Pos pos = p.GetPos();
			pos.ptr += strlen("º");
			p.SetPos(pos);
			p.Spaces();
			x.Mult(doubleUnit(M_PI/180.));
		} else
			return x;
}

doubleUnit EvalExpr::Exp(CParserPP& p) {
	doubleUnit x(Mul(p));
	for(;;)
		if(p.Char('+'))
			x.Sum(Mul(p));
		else if(p.Char('-'))
			x.Sub(Mul(p));
		else if(p.Char(':')) {
			x.Mult(doubleUnit(60));
			x.Sum(Mul(p));
		} else
			return x;
}

doubleUnit EvalExpr::AssignVariable(String var, String expr) {
	doubleUnit ret;
	if (noCase)
		var = ToLower(var);
	int idalloc = FindAddVariable(var);
	try {
		p.Set(expr);

		ret.Set(Exp(p));
		if (!IsNull(ret)) {
			SetVariable(idalloc, ret);
			return ret;
		} else {
			if (allowString) {
				ret.sval = expr;
				SetVariable(idalloc, ret);
				return ret;
			}
			return Null;
		}
	} catch(CParserPP::Error e) {
		if (allowString) {
			ret.sval = expr;
			SetVariable(idalloc, ret);
			return ret;
		}
		lastError = e;
		return Null;
	} catch(Exc e) {
		lastError = e;
		return Null;
	} catch(...) {
		lastError = "Unknown error";
		return Null;
	}
}

void EvalExpr::RenameVariable(String varname, String newvarname) {
	if (noCase) {
		varname = ToLower(varname);
		newvarname = ToLower(newvarname);
	}
	try {
		int id = variables.Find(varname);
		if (id >= 0)
			variables.SetKey(id, newvarname);
	} catch(CParserPP::Error e) {
		lastError = e;
	} catch(Exc e) {
		lastError = e;
	}
}

doubleUnit EvalExpr::AssignVariable(String var, double d) {
	if (noCase)
		var = ToLower(var);
	try {
		doubleUnit ret(d);
		SetVariable(var, ret);
		return ret;
	} catch(CParserPP::Error e) {
		lastError = e;
		return Null;
	} catch(Exc e) {
		lastError = e;
		return Null;
	} catch(...) {
		lastError = "Unknown error";
		return Null;
	}
}

doubleUnit EvalExpr::Eval(String line) {
	line = TrimBoth(line);
	if (line.IsEmpty())
		return Null;

	p.Set(line);
	try {
		if(p.IsId()) {
			CParserPP::Pos pos = p.GetPos();
			String var = p.ReadIdPP();
			if(p.Char('=')) {
				if (noCase)
					var = ToLower(var);
				doubleUnit ret(Exp(p));
				SetVariable(var, ret);
				return ret;
			} else {
				p.SetPos(pos);
				return Exp(p);
			}
		} else
			return Exp(p);
	} catch(CParserPP::Error e) {
		lastError = e;
		return Null;
	} catch(Exc e) {
		lastError = e;
		return Null;
	}
}

String EvalExpr::TermStr(CParserPP& p, int numDigits) {
	if(p.IsId()) {
		String strId = p.ReadIdPP();
		if(functions.Find(strId) >= 0) {
			p.PassChar('(');
			String x = ExpStr(p, numDigits);
			p.PassChar(')');
			return strId + "(" + x + ")";
		}
		if (noCase)
			strId = ToLower(strId);
		if (IsNull(numDigits)) {
			if (constants.Find(strId) < 0)
				lastVariableSetId = variables.FindAdd(strId, 0);
			return strId;
		} else {
			if (constants.Find(strId) >= 0)
				return strId;
			else {
				lastVariableSetId = variables.FindAdd(strId, 0);
				return FormatDoubleFix(variables[lastVariableSetId].val, numDigits);
			}
		}
	}
	if(p.Char('(')) {
		String x = ExpStr(p, numDigits);
		p.PassChar(')');
		return "(" + x + ")";
	}
	return FormatDoubleFix(p.ReadDouble(), IsNull(numDigits) ? 3 : numDigits);
}

String EvalExpr::PowStr(CParserPP& p, int numDigits) {
	String x = TermStr(p, numDigits);
	for(;;)
		if(p.Char('^'))
			x = x + "^" + TermStr(p, numDigits);
		else
			return x;
}

String EvalExpr::MulStr(CParserPP& p, int numDigits) {
	String x = PowStr(p, numDigits);
	for(;;)
		if(p.Char('*'))
			x = x + "*" + MulStr(p, numDigits);
		else if(p.Char('/'))
			x = x + "/" + PowStr(p, numDigits);
		else
			return x;
}

String EvalExpr::ExpStr(CParserPP& p, int numDigits) {
	String x = MulStr(p, numDigits);
	for(;;)
		if(p.Char('+'))
			x = x + " + " + MulStr(p, numDigits);
		else if(p.Char('-'))
			x = x + " - " + MulStr(p, numDigits);
		else if(p.Char(':'))
			x = x + ":" + MulStr(p, numDigits);
		else {
			x.Replace("+ -", "- ");
			return x;
	}
}

String EvalExpr::EvalStr(String line, int numDigits) {
	line = TrimBoth(line);
	if (line.IsEmpty())
		return Null;

	CParserPP p(line);
	try {
		if(p.IsId()) {
			CParserPP::Pos pos = p.GetPos();
			String var = p.ReadIdPP();
			if(p.Char('=')) {
				String ret = ExpStr(p, numDigits);
				lastVariableSetId = variables.FindAdd(var, 0);
				return var + " = " + ret;
			} else {
				p.SetPos(pos);
				return ExpStr(p, numDigits);
			}
		} else
			return ExpStr(p, numDigits);
	} catch(CParserPP::Error e) {
		lastError = Format(t_("Error evaluating '%s': %s"), line, e);
		return Null;
	} catch(Exc e) {
		lastError = Format(t_("Error: %s"), e);
		return Null;
	} catch(String e) {
		lastError = Format(t_("Error: %s"), e);
		return Null;
	} catch(...) {
		lastError = t_("Unknown error");
		return Null;
	}
}

void EvalExpr::ClearVariables() {
	variables.Clear();
}

Vector<int> EvalExpr::FindPattern(String yes, String no) const {
	Vector<int> ret;
	yes = ToLower(yes);
	no = ToLower(no);
	for (int i = 0; i < variables.GetCount(); ++i) {
		String name = ToLower(variables.GetKey(i));
		if (PatternMatch(yes, name) && !PatternMatch(no, name))
			ret << i;
	}
	return ret;
}

Vector<int> EvalExpr::FindPattern(String yes, String no, String yes2) const {
	Vector<int> ret = FindPattern(yes, no);
	if (ret.IsEmpty())
		ret = FindPattern(yes2, no);
	return ret;
}

ExplicitEquation::FitError SplineEquation::Fit(DataSource &data, double &r2) {
	Vector<Pointf> seriesRaw;
	for (int64 i = 0; i < data.GetCount(); ++i) {		// Remove Nulls
		if (!!IsNum(data.x(i)) && !!IsNum(data.y(i)))
			seriesRaw << Pointf(data.x(i), data.y(i));
	}

	if(seriesRaw.IsEmpty())
        return SmallDataSource;

    r2 = 1;

	PointfLess less;
	Sort(seriesRaw, less);								// Sort

	Vector<double> x, y;
	x.Reserve(seriesRaw.GetCount());
	y.Reserve(seriesRaw.GetCount());
	x << seriesRaw[0].x;
	y << seriesRaw[0].y;
	for (int i = 1; i < seriesRaw.GetCount(); ++i) {	// Remove points with duplicate x
		if (seriesRaw[i].x != seriesRaw[i - 1].x) {
			x << seriesRaw[i].x;
			y << seriesRaw[i].y;
		}
	}

	if (x.GetCount() < 2)
		return SmallDataSource;

	Init(x, y);

	coeff.SetCount(1);

	return NoError;
}

void Spline::Init(const double *x, const double *y, int num) {
    nscoeff = num - 1;

    Buffer<double> h(nscoeff);
    for(int i = 0; i < nscoeff; ++i)
        h[i] = x[i+1] - x[i];

    Buffer<double> alpha(nscoeff);
    for(int i = 1; i < nscoeff; ++i)
        alpha[i] = 3*(y[i+1] - y[i])/h[i] - 3*(y[i] - y[i-1])/h[i-1];

    Buffer<double> c(nscoeff+1), l(nscoeff+1), mu(nscoeff+1), z(nscoeff+1);
    l[0] = 1;
    mu[0] = 0;
    z[0] = 0;

    for(int i = 1; i < nscoeff; ++i) {
        l[i] = 2*(x[i+1] - x[i-1]) - h[i-1]*mu[i-1];
        mu[i] = h[i]/l[i];
        z[i] = (alpha[i] - h[i-1]*z[i-1])/l[i];
    }

    l[nscoeff] = 1;
    z[nscoeff] = 0;
    c[nscoeff] = 0;

	scoeff.Alloc(nscoeff);
    for(int i = nscoeff-1; i >= 0; --i) {
        c[i] = z[i] - mu[i] * c[i+1];
        scoeff[i].b = (y[i+1] - y[i])/h[i] - h[i]*(c[i+1] + 2*c[i])/3;
        scoeff[i].d = (c[i+1] - c[i])/3/h[i];
    }

    for(int i = 0; i < nscoeff; ++i) {
        scoeff[i].x = x[i];
        scoeff[i].a = y[i];
        scoeff[i].c = c[i];
    }
    xlast = x[num-1];
}

int Spline::GetPieceIndex(double x) const {
	ASSERT(nscoeff > 0);
    int j;
    for (j = 0; j < nscoeff; j++) {
        if (scoeff[j].x > x) {
            if (j == 0)
                j = 1;
            break;
        }
    }
    return --j;
}

double Spline::f(double x) const {
	int j = GetPieceIndex(x);

    double dx = x - scoeff[j].x;
    double dx2 = dx*dx;
    return scoeff[j].a + scoeff[j].b*dx + scoeff[j].c*dx*dx + scoeff[j].d*dx*dx2;
}

double Spline::df(double x) const {
	int j = GetPieceIndex(x);

    double dx = x - scoeff[j].x;
    return scoeff[j].b + scoeff[j].c*2.*dx + scoeff[j].d*3.*dx*dx;
}

double Spline::d2f(double x) const {
	int j = GetPieceIndex(x);

    double dx = x - scoeff[j].x;
    return scoeff[j].c*2. + scoeff[j].d*6.*dx;
}

double Spline::Integral0(const Coeff &c, double x) {
	double x2 = x*x;
	return c.a*x + c.b*x2/2 + c.c*x*x2/3 + c.d*x2*x2/4;
}

double Spline::Integral(double from, double to) const {
	int ifrom;
	if (!IsNum(from)) {
		ifrom = 0;
		from = scoeff[0].x;
	} else
		ifrom = GetPieceIndex(from);
	int ito;
	if (!IsNum(to)) {
		ito = nscoeff-1;
		to = xlast;
	} else
		ito = GetPieceIndex(to);

	ASSERT(ifrom <= ito);
	if (ifrom > ito)
		return 0;

	double res = 0;
	for (int i = ifrom; i < ito; ++i) {
		double val = Integral0(scoeff[i], scoeff[i+1].x - scoeff[i].x) - Integral0(scoeff[i], from - scoeff[i].x);
		res += val;
		from = scoeff[i+1].x;
	}
	double bal =  Integral0(scoeff[ito], to - scoeff[ito].x) - Integral0(scoeff[ito], from - scoeff[ito].x);
	res += bal;
	return res;
}

INITBLOCK {
	ExplicitEquation::Register<LinearEquation>("LinearEquation");
	ExplicitEquation::Register<PolynomialEquation2>("PolynomialEquation2");
	ExplicitEquation::Register<PolynomialEquation3>("PolynomialEquation3");
	ExplicitEquation::Register<PolynomialEquation4>("PolynomialEquation4");
	ExplicitEquation::Register<PolynomialEquation5>("PolynomialEquation5");
	ExplicitEquation::Register<SinEquation>("SinEquation");
	ExplicitEquation::Register<DampedSinEquation>("DampedSinusoidal");
	ExplicitEquation::Register<Sin_DampedSinEquation>("Sin_DampedSinusoidal");
	ExplicitEquation::Register<ExponentialEquation>("ExponentialEquation");
	ExplicitEquation::Register<RealExponentEquation>("RealExponentEquation");
	ExplicitEquation::Register<Rational1Equation>("Rational1Equation");
	ExplicitEquation::Register<FourierEquation1>("FourierEquation1");
	ExplicitEquation::Register<FourierEquation2>("FourierEquation2");
	ExplicitEquation::Register<FourierEquation3>("FourierEquation3");
	ExplicitEquation::Register<FourierEquation4>("FourierEquation4");
	ExplicitEquation::Register<WeibullEquation>("WeibullEquation");
	ExplicitEquation::Register<WeibullCumulativeEquation>("WeibullCumulativeEquation");
	ExplicitEquation::Register<NormalEquation>("NormalEquation");
}

}


#ifndef __PROBABILITY_DISTRIBUTIONS__LAPLACE_IMPL_HPP__
#define __PROBABILITY_DISTRIBUTIONS__LAPLACE_IMPL_HPP__

#include "laplace.hpp"

#include "const_slice.hpp"
#include "slice.hpp"

#include <boost/random/uniform_smallint.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <cmath>

namespace ProbabilityDistributions {
  template <class D, class W, class T>
  Laplace<D,W,T>::Laplace(T mu, T lambda):
    fixed_mu_(false),
    fixed_lambda_(false) {
      set_mu(mu);
      set_lambda(lambda);
    }

  template <class D, class W, class T>
  template <class RNG>
  void Laplace<D,W,T>::sample(MA::Array<D>& samples, size_t n_samples, RNG& rng)
  const {
    MA::Size::SizeType size(2);
    size[0] = n_samples;
    size[1] = 1;
    samples.resize(size);

    boost::random::uniform_smallint<int> dist1(0, 1);
    boost::random::exponential_distribution<T> dist2(lambda_);

    D* ptr = samples.get_pointer();

    for (size_t j = 0; j < n_samples; j++) {
      if (dist1(rng))
        ptr[j] = mu_ + dist2(rng);
      else
        ptr[j] = mu_ - dist2(rng);
    }
  }

  template <class D, class W, class T>
  T Laplace<D,W,T>::log_likelihood(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight) const {
    check_data_and_weight(data, weight);

    D const* ptr = data.get_pointer();

    T ll = 0;
    T lambda_likelihood = std::log(lambda_/2);

    for (size_t j = 0; j < data.total_size(); j++) {
      T w = weight(j);
      T s = ptr[j];
      T local_likelihood = -std::abs(s - mu_) * lambda_;
      local_likelihood += lambda_likelihood;
      ll += w * local_likelihood;
    }

    return ll;
  }

  template <class D, class W, class T>
  void Laplace<D,W,T>::MLE(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight, std::vector<size_t> const& indexes) {
    check_data_and_weight(data, weight);
    assert(data.size()[0] == indexes.size());

    D const* ptr = data.get_pointer();

    if (!fixed_mu_)
      set_mu(Distribution<D,W,T>::get_percentile(0.5, data, weight, indexes));

    if (!fixed_lambda_) {
      T sum_0 = 0, sum_1 = 0;
      for (size_t j = 0; j < data.total_size(); j++) {
        T w = weight(j);
        sum_0 += w;
        sum_1 += w*std::abs(ptr[j] - mu_);
      }

      set_lambda(sum_0 / sum_1);
    }
  }

  template <class D, class W, class T>
  void Laplace<D,W,T>::check_data_and_weight(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight) const {
    assert(data.size().size() == 2);
    assert(data.size()[0] > 0);
    assert(data.size()[1] == 1);
    assert(weight.size().size() == 1);
    assert(weight.size()[0] == data.size()[0]);
  }
};

#endif


#include "utils.h"

#include <NTL/GF2EX.h>
#include <NTL/GF2X.h>
#include <stdexcept>

using namespace NTL;

namespace utils {

static GF2X modulus;
static std::array<GF2E, 256> lifting_lut;

static void init_lifting_lut(const GF2E &generator) {
  clear(lifting_lut[0]); // lut(0) = 0
  set(lifting_lut[1]);   // lut(1) = 1

  GF2E pow = generator;
  for (size_t bit = 1; bit < 8; bit++) {
    size_t start = (1ULL << bit);
    // copy last half of LUT and add current generator power
    for (size_t idx = 0; idx < start; idx++) {
      lifting_lut[start + idx] = lifting_lut[idx] + pow;
    }
    pow = pow * generator;
  }
}

void init_extension_field(const banquet_instance_t &instance) {
  switch (instance.lambda) {
  case 4: {
    // modulus = x^32 + x^7 + x^3 + x^2 + 1
    clear(modulus);
    SetCoeff(modulus, 32);
    SetCoeff(modulus, 7);
    SetCoeff(modulus, 3);
    SetCoeff(modulus, 2);
    SetCoeff(modulus, 0);
    // Ring morphism:
    //   From: Finite Field in x of size 2^8
    //   To:   Finite Field in y of size 2^32
    //   Defn: x |--> y^30 + y^23 + y^21 + y^18 + y^14 + y^13 + y^11 + y^9 + y^7
    //   + y^6 + y^5 + y^4 + y^3 + y
    GF2X gen;
    clear(gen);
    SetCoeff(gen, 30);
    SetCoeff(gen, 23);
    SetCoeff(gen, 21);
    SetCoeff(gen, 18);
    SetCoeff(gen, 14);
    SetCoeff(gen, 13);
    SetCoeff(gen, 11);
    SetCoeff(gen, 9);
    SetCoeff(gen, 7);
    SetCoeff(gen, 6);
    SetCoeff(gen, 5);
    SetCoeff(gen, 4);
    SetCoeff(gen, 3);
    SetCoeff(gen, 1);

    GF2E::init(modulus);
    init_lifting_lut(conv<GF2E>(gen));
  } break;
  case 5: {
    // modulus = x^40 + x^5 + x^4 + x^3 + 1
    clear(modulus);
    SetCoeff(modulus, 40);
    SetCoeff(modulus, 5);
    SetCoeff(modulus, 4);
    SetCoeff(modulus, 3);
    SetCoeff(modulus, 0);
    // Ring morphism:
    //   From: Finite Field in x of size 2^8
    //   To:   Finite Field in y of size 2^40
    //   Defn: x |--> y^31 + y^30 + y^27 + y^25 + y^22 + y^21 + y^20 + y^18 +
    //   y^15 + y^9 + y^6 + y^4 + y^2
    GF2X gen;
    clear(gen);
    SetCoeff(gen, 31);
    SetCoeff(gen, 30);
    SetCoeff(gen, 27);
    SetCoeff(gen, 25);
    SetCoeff(gen, 22);
    SetCoeff(gen, 21);
    SetCoeff(gen, 20);
    SetCoeff(gen, 18);
    SetCoeff(gen, 15);
    SetCoeff(gen, 9);
    SetCoeff(gen, 6);
    SetCoeff(gen, 4);
    SetCoeff(gen, 2);

    GF2E::init(modulus);
    init_lifting_lut(conv<GF2E>(gen));
  } break;
  case 6: {
    // modulus = x^48 + x^5 + x^3 + x^2 + 1
    clear(modulus);
    SetCoeff(modulus, 48);
    SetCoeff(modulus, 5);
    SetCoeff(modulus, 3);
    SetCoeff(modulus, 2);
    SetCoeff(modulus, 0);
    // Ring morphism:
    //   From: Finite Field in x of size 2^8
    //   To:   Finite Field in y of size 2^48
    //   Defn: x |--> y^45 + y^43 + y^40 + y^37 + y^36 + y^35 + y^34 + y^33 +
    //   y^31 + y^30 + y^29 + y^28 + y^24 + y^21 + y^20 + y^19 + y^16 + y^14 +
    //   y^13 + y^11 + y^10 + y^7 + y^3 + y^2
    GF2X gen;
    clear(gen);
    SetCoeff(gen, 45);
    SetCoeff(gen, 43);
    SetCoeff(gen, 40);
    SetCoeff(gen, 37);
    SetCoeff(gen, 36);
    SetCoeff(gen, 35);
    SetCoeff(gen, 34);
    SetCoeff(gen, 33);
    SetCoeff(gen, 31);
    SetCoeff(gen, 30);
    SetCoeff(gen, 29);
    SetCoeff(gen, 28);
    SetCoeff(gen, 24);
    SetCoeff(gen, 21);
    SetCoeff(gen, 20);
    SetCoeff(gen, 19);
    SetCoeff(gen, 16);
    SetCoeff(gen, 14);
    SetCoeff(gen, 13);
    SetCoeff(gen, 11);
    SetCoeff(gen, 10);
    SetCoeff(gen, 7);
    SetCoeff(gen, 3);
    SetCoeff(gen, 2);

    GF2E::init(modulus);
    init_lifting_lut(conv<GF2E>(gen));
  } break;
  default:
    throw std::runtime_error(
        "modulus for that specific lambda not implemented.");
  }
}

const GF2E &lift_uint8_t(uint8_t value) { return lifting_lut[value]; }

GF2E GF2E_from_bytes(const std::vector<uint8_t> &value) {
  // assumes value is already smaller than current modulus
  GF2X inner = GF2XFromBytes(value.data(), value.size());
  // GF2E result(INIT_NO_ALLOC);
  // result.LoopHole() = inner;
  // return result;
  return conv<GF2E>(inner);
}

vec_GF2E get_first_n_field_elements(size_t n) {
  vec_GF2E result;
  result.SetLength(n);
  GF2X gen;
  SetX(gen);
  for (size_t i = 0; i < n; i++) {
    result[i] = conv<GF2E>(gen);
    gen = MulByX(gen);
  }
  return result;
}
std::vector<GF2EX> precompute_lagrange_polynomials(const vec_GF2E &x_values) {
  size_t m = x_values.length();
  std::vector<GF2EX> precomputed_lagrange_polynomials;
  precomputed_lagrange_polynomials.reserve(m);

  GF2EX full_poly = BuildFromRoots(x_values);
  GF2EX lagrange_poly;
  GF2EX missing_term;
  SetX(missing_term);
  for (size_t k = 0; k < m; k++) {
    SetCoeff(missing_term, 0, -x_values[k]);
    lagrange_poly = full_poly / missing_term;
    lagrange_poly = lagrange_poly / eval(lagrange_poly, x_values[k]);
    precomputed_lagrange_polynomials.push_back(lagrange_poly);
  }

  return precomputed_lagrange_polynomials;
}

GF2EX interpolate_with_precomputation(
    const std::vector<GF2EX> &precomputed_lagrange_polynomials,
    const vec_GF2E &y_values) {
  if (precomputed_lagrange_polynomials.size() != (size_t)y_values.length())
    throw std::runtime_error("invalid sizes for interpolation");

  GF2EX res;
  size_t m = y_values.length();
  for (size_t k = 0; k < m; k++) {
    res += precomputed_lagrange_polynomials[k] * y_values[k];
  }
  return res;
}
field::GF2E ntl_to_custom(const GF2E &element) {
  const GF2X &poly_rep = rep(element);
  std::vector<uint8_t> buffer(8);
  BytesFromGF2X(buffer.data(), poly_rep, buffer.size());
  field::GF2E a;
  a.from_bytes(buffer.data());
  return a;
}
GF2E custom_to_ntl(const field::GF2E &element) {
  std::vector<uint8_t> buffer(8);
  element.to_bytes(buffer.data());
  GF2X inner = GF2XFromBytes(buffer.data(), buffer.size());
  return conv<GF2E>(inner);
}
} // namespace utils


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

#include <votca/tools/linspline.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <votca/tools/linalg.h>
#include <iostream>

namespace votca { namespace tools {

using namespace std;

void LinSpline::Interpolate(ub::vector<double> &x, ub::vector<double> &y)
{
    if(x.size() != y.size())
        throw std::invalid_argument("error in LinSpline::Interpolate : sizes of vectors x and y do not match");

    if(x.size()<2)
        throw std::invalid_argument("error in LinSpline::Interpolate : vectors x and y have to contain at least 2 points");

    const int N = x.size();

    // adjust the grid
    _r.resize(N);

    // copy the grid points into f
    _r = x;

    // LINEAR SPLINE: a(i) * x + b(i)
    // where i=number of interval

    // initialize vectors a,b
    a = ub::zero_vector<double>(N);
    b = ub::zero_vector<double>(N);

    // boundary conditions not applicable

    // calculate a,b for all intervals 0..(N-2), where interval
    // [x(i),x(i+1)] shall have number i (this means that the last interval
    // has number N-2)
    for (int i=0; i<N-1; i++) {
        a(i) = (y(i+1)-y(i))/(x(i+1)-x(i));
        b(i) = y(i)-a(i)*x(i);
    }
}

void LinSpline::Fit(ub::vector<double> &x, ub::vector<double> &y)
{
    if(x.size() != y.size())
        throw std::invalid_argument("error in LinSpline::Fit : sizes of vectors x and y do not match");

    const int N = x.size();
    const int ngrid = _r.size();

    // construct the equation
    // A*u = b
    // The matrix A contains all conditions
    // s_i(x) = (y(i+1)-y(i)) * (x-r(i))/(r(i+1)-r(i)) + y(i)
    // where y(i) are the unknown values at grid points r(i), and
    // the condition y=s_i(x) is to be satisfied at all input points:
    // therefore b=y and u=vector of all unknown y(i)

    ub::matrix<double> A(N, ngrid);
    A = ub::zero_matrix<double>(N, ngrid);
    int interval;

    // construct matrix A
    for (int i=0; i<N; i++) {
        interval = getInterval(x(i));
        A(i,interval)   = 1 - (x(i)-_r(interval))/(_r(interval+1)-_r(interval));
        A(i,interval+1) = (x(i)-_r(interval))/(_r(interval+1)-_r(interval));
    }

    // now do a qr solve
    ub::vector<double> sol(ngrid);
    votca::tools::linalg_qrsolve(sol, A, y);

    // vector "sol" contains all y-values of fitted linear splines at each
    // interval border
    // get a(i) and b(i) for piecewise splines out of solution vector "sol"
    a = ub::zero_vector<double>(ngrid-1);
    b = ub::zero_vector<double>(ngrid-1);
    for (int i=0; i<ngrid-1; i++) {
        a(i) = (sol(i+1)-sol(i))/(_r(i+1)-_r(i));
        b(i) = -a(i)*_r(i) + sol(i);
    }
}
}}


#include <iostream>

#include <NTL/ZZ.h>

using namespace std;
using namespace NTL;

/*
 * This program calculates a blinded message from m and from its
 * signature S' calculates S, the signature of m.
 */

int main() {
	ZZ m, e, n, blind_m, blind_S, S, r;
	r = 2;

	cout << "m> ";
	cin >> m;
	cout << "e> ";
	cin >> e;
	cout << "n> ";
	cin >> n;

	// m' = m*r^e (mod n)
	mul(blind_m, m, PowerMod(r, e, n));
	cout << "m': " << blind_m << endl << endl;

	cout << "S'> ";
	cin >> blind_S;

	// S = S' * r^(-1)
	MulMod(S, blind_S, InvMod(r, n), n);

	cout << "S: " << S << endl;
}