#include "get_floor_fHf.h"
#include "ransac_floor_fHf.h"
#include "ransac_data.h"
#include "randperm.h"
#include <Eigen/Geometry>
#include <vector>

using namespace Eigen;

RansacData ransac_floor_fHf(MatrixXd &p1, MatrixXd &p2, Matrix3d &R1, Matrix3d &R2, int nbr_iter, double thresh)
{
    // Init RANSAC loop
    PoseData best_posedata;
    int best_nbr_inliers = 0;
    VectorXi best_inliers;
    best_inliers.setZero();
    int nbr_pts = p1.cols();

    MatrixXd reproj1(2, nbr_pts);
    MatrixXd reproj2(2, nbr_pts);
    MatrixXd reproj(4, nbr_pts);
    VectorXd reproj_mean(nbr_pts);
    MatrixXd H(3,3);
    VectorXi inliers(nbr_pts);
    int nbr_inliers;
    VectorXi history(nbr_iter);

    int nbr_pts_minimal = 3;

    for (int i = 0; i < nbr_iter; i++) {
        std::vector<int> rands = randperm(nbr_pts_minimal, nbr_pts);
        MatrixXd x1(3, nbr_pts_minimal);
        MatrixXd x2(3, nbr_pts_minimal);
        for (int j = 0; j < nbr_pts_minimal; j++) {
            x1.col(j) << p1.col(rands[j]);
            x2.col(j) << p2.col(rands[j]);
        }

        // Get pose
        MatrixXd x1h(2,3);
        MatrixXd x2h(2,3);
        x1h << x1.colwise().hnormalized();
        x2h << x2.colwise().hnormalized();

        PoseData posedata = get_floor_fHf(x1h, x2h, R1, R2);

        // Compute reprojection error, and compare to other solutions.
        H = posedata.homography;
        reproj1 = p2.colwise().hnormalized() - (H * p1).colwise().hnormalized();
        reproj2 = p1.colwise().hnormalized() - (H.lu().solve(p2)).colwise().hnormalized();
        reproj << reproj1.cwiseAbs(), reproj2.cwiseAbs();
        reproj_mean = reproj.colwise().mean().array();

        inliers = (reproj_mean.array() < thresh).cast<int>();
        nbr_inliers = inliers.sum();

        if (best_nbr_inliers < nbr_inliers) {
            best_posedata = posedata;
            best_nbr_inliers = nbr_inliers;
            best_inliers = inliers;
        }
        history(i) = best_nbr_inliers;

    }

    RansacData ransac_data;
    ransac_data.posedata = best_posedata;
    ransac_data.inliers = best_inliers;
    ransac_data.history = history;

    return ransac_data;
}

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

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

    double *nbr_iter_p = mxGetPr(prhs[4]);
    double *thresh = mxGetPr(prhs[5]);
    int nbr_iter = (int) nbr_iter_p[0];

    // Compute output
    RansacData ransac_data = ransac_floor_fHf(x1, x2, R1, R2, nbr_iter, thresh[0]);
    PoseData posedata = ransac_data.posedata;

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

    plhs[2] = mxCreateDoubleMatrix(nbr_pts, 1, mxREAL);
    zr = mxGetPr(plhs[2]);
    for (Index i = 0; i < nbr_pts; i++) {
        zr[i] = ransac_data.inliers(i);
    }

    plhs[3] = mxCreateDoubleMatrix(nbr_iter, 1, mxREAL);
    zr = mxGetPr(plhs[3]);
    for (Index i = 0; i < nbr_iter; i++) {
        zr[i] = ransac_data.history(i);
    }
}
#endif


#include <math.h>
#include <chrono>
#include <stdexcept>
#include <iostream>
#include <Eigen/Dense>
#include "kepler.h"
#include "elements.h"

using std::function;
using std::runtime_error;
using Eigen::Vector3d;

namespace kepler {
    double period(double sma, double mu) {
        return M_PI * 2 * sqrt(pow(sma, 3) / mu);
    }

    double newton(
            double p0,
            function<double(double)> const &func,
            function<double(double)> const &deriv,
            int maxiter = 50,
            double tol = 1e-8
    ) {
        for (auto i = 1; i < maxiter; i++) {
            auto p = p0 - func(p0) / deriv(p0);
            if (fabs(p - p0) < tol) {
                return p;
            }
            p0 = p;
        }
        throw runtime_error("Not converged.");
    }

    double mean2ecc(double M, double ecc) {
        auto E = newton(M, [ecc, M](double E) -> double {
            return E - ecc * sin(E) - M;
        }, [ecc](double E) -> double {
            return 1 - ecc * cos(E);
        });
        return E;
    }

    double ecc2true(double E, double ecc) {
        return 2 * atan2(sqrt(1 + ecc) * sin(E / 2), sqrt(1 - ecc) * cos(E / 2));
    }

    void benchmark(int times) {
        auto mu = 3.986004418e5;
        Vector3d r(8.59072560e+02, -4.13720368e+03, 5.29556871e+03);
        Vector3d v(7.37289205e+00, 2.08223573e+00, 4.39999794e-01);
        auto el = elements::elements(r, v, mu);

        auto best = std::numeric_limits<double>::infinity();
        auto worst = -std::numeric_limits<double>::infinity();
        double all = 0;
        for (auto i=0; i < times; i++) {
            auto begin = std::chrono::high_resolution_clock::now();

            mean2ecc(M_PI, el[1]);

            auto end = std::chrono::high_resolution_clock::now();
            auto current = std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count()/1e9;
            all += current;
            if (current < best) {
                best = current;
            }
            if (current > worst) {
                worst = current;
            }
        }
        std::cout << "[" << all/times << "," << best << "," << worst << "]" << std::endl;
    }
}


#include "common.hpp"
#include "PPXTF.hpp"
#include <sys/stat.h>
#include <Eigen/LU>

using namespace Eigen;
using namespace std;

/*
  #################################### PPMTF ####################################
  # [input1]: R1_observed ([mode1]{mode2, mode3: value})
  # [input2]: R2_observed ([mode2]{mode1, mode3: value})
  # [input3]: R3_observed ([mode3]{mode1, mode2: value})
  # [input4]: A (L1 x K matrix)
  # [input5]: B (L2 x K matrix)
  # [input6]: C (L3 x K matrix)
  # [input7]: L1 -- Length of mode1
  # [input8]: L2 -- Length of mode2
  # [input9]: L3 -- Length of mode3
  # [input10]: X -- tensor no.
  # [output]: A, B, C, mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C
 */
tuple<mat_t, mat_t, mat_t, mat_t, mat_t, mat_t>
pptf(const r_t& r1, const r_t& r2, const r_t& r3,
     vec_mat_t& A, vec_mat_t& B, vec_mat_t& C,
     int L1, int L2, int L3, int K, int ItrNum, double Alp,
     const string& outfile_prefix, const string& X){
  // Hyper-hyper parameters (same as [Salakhutdinov+, ICML08])
  double alpha = Alp;
  double beta0 = 2;
  double mu0 = 0;
  int nu0 = K;
  mat_t W0 = mat_t::Identity(K,K);

  mat_t Lam_A, mu_A, Lam_B, mu_B, Lam_C, mu_C;
  puts("Gibbs Sampling:");
  for(int itr = 0; itr < ItrNum; ++itr){
    if(itr % 10 == 0) printf("itr:%d\n", itr);

    // Sample Lam_{A,B,C,D} & mu_{A,B,C,D}
    tie(Lam_A, mu_A) = sample_Lam_X_mu_X(A[itr], L1, nu0, W0, beta0, mu0, K);
    tie(Lam_B, mu_B) = sample_Lam_X_mu_X(B[itr], L2, nu0, W0, beta0, mu0, K);
    tie(Lam_C, mu_C) = sample_Lam_X_mu_X(C[itr], L3, nu0, W0, beta0, mu0, K);

    // Sample A
    A.push_back(mat_t::Zero(L1, K));
    for(int n = 0; n < L1; ++n){
      mat_t BC_BC, BC_R;
      tie(BC_BC, BC_R) = calc_XYXY_XYR(K, B[itr], C[itr], r1[n]);
      mat_t lam_ast_inv = (Lam_A + alpha * BC_BC).inverse();
      mat_t mu_ast = lam_ast_inv * (alpha * BC_R + Lam_A * mu_A);
      A[itr+1].row(n) = multivariate_normal(mu_ast, lam_ast_inv).transpose();
    }

    // Sample B
    B.push_back(mat_t::Zero(L2, K));
    for(int i = 0; i < L2; ++i){
      mat_t AC_AC, AC_R;
      tie(AC_AC, AC_R) = calc_XYXY_XYR(K, A[itr+1], C[itr], r2[i]);
      mat_t lam_ast_inv = (Lam_B + alpha * AC_AC).inverse();
      mat_t mu_ast = lam_ast_inv * (alpha * AC_R + Lam_B * mu_B);
      B[itr+1].row(i) = multivariate_normal(mu_ast, lam_ast_inv).transpose();
    }

    // Sample C
    C.push_back(mat_t::Zero(L3, K));
    for(int j = 0; j < L3; ++j){
      mat_t AB_AB, AB_R;
      tie(AB_AB, AB_R) = calc_XYXY_XYR(K, A[itr+1], B[itr+1], r3[j]);
      mat_t lam_ast_inv = (Lam_C + alpha * AB_AB).inverse();
      mat_t mu_ast = lam_ast_inv * (alpha * AB_R + Lam_C * mu_C);
      C[itr+1].row(j) = multivariate_normal(mu_ast, lam_ast_inv).transpose();
    }

    // Output model parameters and hyper-parameters
    if(itr == 0 || itr % 10 == 9){
      string pre = outfile_prefix;
      save_parameters(K, itr+1, pre, "A", A, mu_A, Lam_A, X);
      save_parameters(K, itr+1, pre, "B", B, mu_B, Lam_B, X);
      save_parameters(K, itr+1, pre, "C", C, mu_C, Lam_C, X);
    }
  }
  return forward_as_tuple(mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C);
}

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

int main(int argc, char *argv[]){
  if(argc < 3){
      printf("Usage: PPITF [Dataset] [City] ([Alp (default:200)] [MaxNumTrans (default:100)] [MaxNumVisit (default:100)] [ItrNum (default:100)])\n");
      return -1;
  }

  // Fix a seed
  seeding();

  // ################################# Parameters ##################################
  // Dataset
  const string Dataset = argv[1];
  // City
//  const string City = "TK";
  const string City = argv[2];
  // Hyper-hyper parameter alpha
  double Alp = 200;
  string AlpStr = "200";
  if(argc >= 4){
	  Alp = atof(argv[3]);
	  AlpStr = argv[3];
  }
  // Maximum number of transitions per user (-1: infinity)
  int MaxNumTrans = 100;
  if(argc >= 5){
	  MaxNumTrans = atoi(argv[4]);
  }
  // Maximum number of POI visits per user (-1: infinity)
  int MaxNumVisit = 100;
  if(argc >= 6){
	  MaxNumVisit = atoi(argv[5]);
  }
  // Number of iterations in Gibbs sampling
  int ItrNum = 100;
  if(argc >= 7){
	  ItrNum = atoi(argv[6]);
  }

  // Training user index file (input)
  string TUserIndexFile = "../data/" + Dataset + "/tuserindex_%s.csv";
  // POI index file (input)
  string POIIndexFile = "../data/" + Dataset + "/POIindex_%s.csv";
  // Training transition tensor file (input)
  string TrainTransTensorFile = "../data/" + Dataset + "/traintranstensor_%s_mnt" + std::to_string(MaxNumTrans) + ".csv";
  // Training visit tensor file (input)
  string TrainVisitTensorFile = "../data/" + Dataset + "/trainvisittensor_%s_mnv" + std::to_string(MaxNumVisit) + ".csv";
  // Prefix of the model parameter file (output)
  const string OutDir = "../data/" + Dataset + "/PPITF_" + City + "_alp" + AlpStr + "_mnt" + std::to_string(MaxNumTrans) + "_mnv" + std::to_string(MaxNumVisit);
  mkdir(OutDir.c_str(), 0755);
  const string ModelParameterFile = OutDir + "/modelparameter";

  // Number of time slots
  int T;
  if(Dataset.find("PF") == 0){
	  T = 30;
  }else if(Dataset.find("FS") == 0){
	  T = 12;
  }else{
	  cout << "Wrong Dataset\n";
	  exit(-1);
  }

  // Number of columns in model parameters (A, B, C)
//  const int K = 32;
  const int K = 16;
  // Number of zero elements for each user in a training tensor (-1: all)
  const int ZeroNum = 1000;

  // Replace %s with City
  TUserIndexFile = string_replace(TUserIndexFile, City);
  POIIndexFile = string_replace(POIIndexFile, City);
  TrainTransTensorFile = string_replace(TrainTransTensorFile, City);
  TrainVisitTensorFile = string_replace(TrainVisitTensorFile, City);

  // Number of training users --> N
  int N = line_num(TUserIndexFile) - 1;
  // Number of POIs --> M
  int M = line_num(POIIndexFile) - 1;

  // Read a training transition tensor
  puts("Reading a training transition tensor.");
  r_t RT1_observed, RT2_observed, RT3_observed;
  tie(RT1_observed, RT2_observed, RT3_observed) = ReadTrainTransTensor(N, M, ZeroNum, TrainTransTensorFile);
  // Read a training visit tensor
  puts("Reading a training visit tensor.");
  r_t RV1_observed, RV2_observed, RV3_observed;
  tie(RV1_observed, RV2_observed, RV3_observed) = ReadTrainVisitTensor(N, M, T, ZeroNum, TrainVisitTensorFile);

  {
    // Initialize model parameters (AT, BT, CT) of the training transition tensor by random values in [0,1)
    vec_mat_t A, B, C;
    A.push_back(random_mat(N, K));
    B.push_back(random_mat(M, K));
    C.push_back(random_mat(M, K));
    // PPTF for the training transition tensor --> AT, BT, CT, mu_AT, Lam_AT, mu_BT, Lam_BT, mu_CT, Lam_CT
    mat_t mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C;
    tie(mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C) = pptf(RT1_observed, RT2_observed, RT3_observed,
						      A, B, C, N, M, M, K, ItrNum, Alp, ModelParameterFile, "T");
  }
  {
    // Initialize model parameters (AV, BV, CV) of the training visit tensor by random values in [0,1)
    vec_mat_t A, B, C;
    A.push_back(random_mat(N, K));
    B.push_back(random_mat(M, K));
    C.push_back(random_mat(T, K));
    // PPTF for the training visit tensor --> AV, BV, CV, mu_AV, Lam_AV, mu_BV, Lam_BV, mu_CV, Lam_CV
    mat_t mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C;
    tie(mu_A, Lam_A, mu_B, Lam_B, mu_C, Lam_C) = pptf(RV1_observed, RV2_observed, RV3_observed,
						      A, B, C, N, M, T, K, ItrNum, Alp, ModelParameterFile, "V");
  }
  return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

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

using namespace std;

double f(double) { cout << "double\n"; return 1.0; }
complex<double> f(complex<double>) { cout << "complex\n"; return complex<double>(1.0, -1.0); }

template <typename Matrix>
void singularity_test1(const Matrix& A)
{
    typedef typename mtl::Collection<Matrix>::value_type  Scalar;
    try {
	Matrix B(A);
	B[mtl::iall][0]= Scalar(0);
	// cout << "B is:\n" << B;
	lu(B);
    } catch (mtl::matrix_singular excp) {
	cout << "Exception 1 for singularity successfully caught\n"; return;
    }
    throw "Singularity (test1) not detected";
}

template <typename Matrix>
void singularity_test2(const Matrix& A)
{
    typedef typename mtl::Collection<Matrix>::value_type  Scalar;
    try {
	Matrix B(A);
	B[mtl::iall][0]= Scalar(0);
	mtl::dense_vector<int> p;
	lu(B, p);
	cout << "B is:\n" << B << endl;
    } catch (mtl::matrix_singular excp) {
	cout << "Exception 2 for singularity successfully caught\n"; return;
    }
    throw "Singularity (test2) not detected";
}

template <typename Matrix>
void singularity_test3(const Matrix& A)
{
    typedef typename mtl::Collection<Matrix>::value_type  Scalar;
    try {
	Matrix B(A);
	B[mtl::iall][0]= Scalar(0);
	B[0][0]= Scalar(1e-15);
	lu(B, 2e-15);
    } catch (mtl::matrix_singular excp) {
	cout << "Exception 3 for singularity successfully caught\n"; return;
    }
    throw "Singularity (test3) not detected";
}

template <typename Matrix>
void singularity_test4(const Matrix& A)
{
    typedef typename mtl::Collection<Matrix>::value_type  Scalar;
    try {
	Matrix B(A);
	B[mtl::iall][0]= Scalar(0);
	B[3][0]= Scalar(1e-15);
	mtl::dense_vector<int> p;
	lu(B, p, 2e-15);
    } catch (mtl::matrix_singular excp) {
	cout << "Exception 4 for singularity successfully caught\n"; return;
    }
    throw "Singularity (test4) not detected";
}

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    cout << "\n" << name << "\n";

    typedef typename mtl::Collection<Matrix>::value_type  Scalar;
    typedef typename mtl::dense_vector<Scalar>            Vector;

    unsigned size= unsigned(num_cols(A));
    Matrix L(size, size), U(size, size);

    Scalar c= f(Scalar(1));
    cout << "c is: " << c << "\n";

    for (unsigned i= 0; i < size; i++)
	for(unsigned j= 0; j < size; j++) {
	    U[i][j]= i <= j ? c * Scalar(i+j+2) : Scalar(0);
	    L[i][j]= i > j ? c * Scalar(i+j+1) : (i == j ? Scalar(1) : Scalar(0));
	}

    cout << "L is:\n" << L << "U is:\n" << U;
    A= L * U;

    Vector v(size);
    for (unsigned i= 0; i < size; i++)
	v[i]= Scalar(i);

    Vector w( A*v );

    cout << "A is:\n" << A;

    Matrix LU(A);
    lu(LU);
    cout << "LU decomposition of A is:\n" << LU;

    Matrix I(size, size);
    I= Scalar(1);
    Matrix tmp(I + strict_lower(LU)), A2(tmp * upper(LU));
    cout << "L * U is:\n" << A2;

    Matrix B( lu_f(A) );

    Vector v2( upper_trisolve(upper(LU), unit_lower_trisolve(strict_lower(LU), w)) );

    cout << "LU decomposition of A (as function result) is:\n" << B;
    cout << "upper(LU) is:\n" << upper(LU) << "strict_lower(LU) is:\n" << strict_lower(LU);
    cout << "v2 is " << v2 << "\n";

    MTL_THROW_IF(abs(v[1] - v2[1]) > 0.1, mtl::runtime_error("Error using tri_solve"));

    Vector v3( lu_solve_straight(A, w) );
    MTL_THROW_IF(abs(v[1] - v3[1]) > 0.1, mtl::runtime_error("Error in solve"));

    Vector v4( lu_solve(A, w) );
    cout << "v4 is " << v4 << "\n";
    MTL_THROW_IF(abs(v[1] - v4[1]) > 0.1, mtl::runtime_error("Error in solve"));

    singularity_test1(A);
    singularity_test2(A);
    singularity_test3(A);
    singularity_test4(A);

    mtl::mat::lu_solver<Matrix> lus(A);
    Vector v5(size);

    lus.solve(w, v5);
    cout << "v5 is " << v5 << "\n";
    MTL_THROW_IF(abs(v[1] - v5[1]) > 0.1, mtl::runtime_error("Error in solve"));
}

int main(int, char**)
{
    using namespace mtl;
    unsigned size= 4;

    dense2D<double>                                      dr(size, size);
    dense2D<complex<double> >                            dz(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);

    test(dr, "Row-major dense");
    test(dz, "Row-major dense with complex numbers");
    test(dc, "Column-major dense");

    return 0;
}


//---------------------------------------------------------------------------
//    $Id: product_matrix.cc 27657 2012-11-21 13:19:08Z bangerth $
//
//    Copyright (C) 2005-2006, 2010, 2012 by the deal.II authors
//
//    This file is subject to QPL and may not be distributed without copyright
//    and license information. Please refer to the file
//    deal.II/doc/license.html for the text and further information on this
//    license.
//
//---------------------------------------------------------------------------

// See documentation of ProductMatrix for documentation of this example

#include <deal.II/base/logstream.h>
#include <deal.II/lac/matrix_lib.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/vector.h>

using namespace dealii;

double Adata[] =
{
  .5, .1,
  .4, .2
};

double Bdata[] =
{
  .866, .5,
  -.5, .866
};

int main()
{
  FullMatrix<float> A(2,2);
  FullMatrix<double> B(2,2);

  A.fill(Adata);
  B.fill(Bdata);

  GrowingVectorMemory<Vector<double> > mem;

  ProductMatrix<Vector<double> > AB(A,B,mem);

  Vector<double> u(2);
  Vector<double> v(2);

  u(0) = 1.;
  u(1) = 2.;

  AB.vmult(v,u);

  deallog << v(0) << '\t' << v(1) << std::endl;

  AB.Tvmult(v,u);

  deallog << v(0) << '\t' << v(1) << std::endl;
}


/*
 * test_propagator.cpp
 *
 * Created on: Mar 21, 2018 15:28
 * Description:
 *
 * Copyright (c) 2018 Ruixiang Du (rdu)
 */

#include <iostream>

#include <boost/numeric/odeint.hpp>

#include "model/bicycle_model.hpp"

using namespace robosw;
using namespace boost::numeric::odeint;

int main() {
  double t0 = 0;
  double tf = 10;
  double dt = 0.01;

  BicycleKinematics::state_type x = {0.0, 0.0, 0.0, 0.0};
  BicycleKinematics model({0.8, 0});

  boost::numeric::odeint::integrate_const(
      boost::numeric::odeint::runge_kutta4<BicycleKinematics::state_type>(),
      model, x, 0.0, 10.0, 0.01);

  //   runge_kutta4<std::vector<double>> rk4;
  //   double t = t0;
  //   for (size_t i = 0; i < 1000; ++i, t += dt) {
  //     rk4.do_step(model, x, t, dt);
  //   }

  std::cout << "final state: " << x[0] << " , " << x[1] << std::endl;

  return 0;
}


/*
 * StokesM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <iomanip>
#include <iostream>

#define DIRECTLAYER 2
#define PI314 (static_cast<double>(3.1415926535897932384626433))
#define E271 (static_cast<double>(2.7182818284590452354))

namespace Laplace3D3DDipole {

using EVec3 = Eigen::Vector3d;
using EMat3 = Eigen::Matrix3d;

inline double ERFC(double x) { return std::erfc(x); }
inline double ERF(double x) { return std::erf(x); }

inline double f(double r, double eta) {
    return ERFC(sqrt(PI314 / eta) * r) / r;
}

inline double fp(double r, double eta) {
    return -ERFC(sqrt(PI314 / eta) * r) / (r * r) -
           2 * exp(-PI314 * r * r / eta) / (r * sqrt(eta));
}

// real and wave sum of 2D Laplace kernel Ewald

// xm: target, xn: source
inline EVec3 realSum(const double eta, const EVec3 &xn, const EVec3 &xm) {
    EVec3 rmn = xm - xn;
    double rnorm = rmn.norm();
    if (rnorm < 1e-14) {
        return EVec3(0, 0, 0);
    }
    return -fp(rnorm, eta) / rnorm * rmn;
}

inline EVec3 gKernelEwald(const EVec3 &xm, const EVec3 &xn) {
    const double eta = 1.0; // recommend for box=1 to get machine precision
    EVec3 target = xm;
    EVec3 source = xn;
    target[0] = target[0] - floor(target[0]); // periodic BC
    target[1] = target[1] - floor(target[1]);
    target[2] = target[2] - floor(target[2]);
    source[0] = source[0] - floor(source[0]);
    source[1] = source[1] - floor(source[1]);
    source[2] = source[2] - floor(source[2]);

    // real sum
    int rLim = 6;
    EVec3 Kreal(0, 0, 0);
    for (int i = -rLim; i <= rLim; i++) {
        for (int j = -rLim; j <= rLim; j++) {
            for (int k = -rLim; k <= rLim; k++) {
                EVec3 rmn = target - source + EVec3(i, j, k);
                if (rmn.norm() < 1e-13) {
                    continue;
                }
                Kreal += realSum(eta, EVec3(0, 0, 0), rmn);
            }
        }
    }

    // wave sum
    int wLim = 6;
    EVec3 Kwave(0, 0, 0);
    EVec3 rmn = target - source;

    for (int i = -wLim; i <= wLim; i++) {
        for (int j = -wLim; j <= wLim; j++) {
            for (int k = -wLim; k <= wLim; k++) {
                if (i == 0 && j == 0 && k == 0) {
                    continue;
                }
                EVec3 kvec = EVec3(i, j, k);
                double knorm = kvec.norm();
                Kwave += 2 * PI314 * sin(2 * PI314 * kvec.dot(rmn)) *
                         exp(-eta * PI314 * knorm * knorm) /
                         (PI314 * knorm * knorm) * kvec;
            }
        }
    }

    return Kreal + Kwave;
}

inline EVec3 gKernel(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < 1e-14) {
        return EVec3(0, 0, 0);
    } else {
        return rst / pow(rnorm, 3);
    }
}

inline Eigen::Matrix3d gKernelGrad(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < 1e-14) {
        return Eigen::Matrix3d::Zero();
    } else {
        return Eigen::Matrix3d::Identity() / pow(rnorm, 3) -
               3 * rst * rst.transpose() / pow(rnorm, 5);
    }
}

// Out of Direct Sum Layer, far field part
inline EVec3 gKernelFF(const EVec3 &target, const EVec3 &source) {
    EVec3 fEwald = gKernelEwald(target, source);
    const int N = DIRECTLAYER;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                EVec3 gFree = gKernel(target, source - EVec3(i, j, k));
                fEwald -= gFree;
            }
        }
    }

    // {
    //   std::cout << "source:" << source << std::endl
    //             << "target:" << target << std::endl
    //             << "gKernalFF" << fEwald << std::endl;
    // }
    return fEwald;
}

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */

template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);

    // testing Ewald routine
    double zeroTest = gKernelEwald(EVec3(0.5, 0.5, 0.5), EVec3(0.5, 0.5, 0.5))
                          .dot(EVec3(1, 1, 1));
    std::cout << std::setprecision(16) << "zeroTest: " << zeroTest << std::endl;

    double centerTest = gKernelEwald(EVec3(0, 0, 0), EVec3(0.5, 0.5, 0.5))
                            .dot(EVec3(0.5, 0.5, 0.5));
    std::cout << std::setprecision(16) << "centerTest: " << centerTest
              << " error: " << centerTest - 0 << std::endl;

    centerTest = gKernelEwald(EVec3(0.2, 0.3, 0.4), EVec3(0.3, 0.6, 0.5))
                     .dot(EVec3(3, 2, 1));
    std::cout << std::setprecision(16) << "centerTest2: " << centerTest
              << " error: " << centerTest + 23.48660380315382667504
              << std::endl;

    centerTest = gKernelEwald(EVec3(0.7, 0.9, 0.7), EVec3(0.2, 0.3, 0.4))
                     .dot(EVec3(0.1, 2, 0.3));
    std::cout << std::setprecision(16) << "centerTest2: " << centerTest
              << " error: " << centerTest + 0.83918927151112920892 << std::endl;

    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {
        -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {
        -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};

    const double scaleLEquiv = 1.05;
    const double scaleLCheck = 2.95;
    const double pCenterLEquiv[3] = {
        -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2};
    const double pCenterLCheck[3] = {
        -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2};

    auto pointMEquiv =
        surface(pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv, 0);
    // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(
        pCheck, (double *)&(pCenterCheck[0]), scaleCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth =0

    auto pointLEquiv = surface(
        pEquiv, (double *)&(pCenterLCheck[0]), scaleLCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth =  0
    auto pointLCheck = surface(
        pCheck, (double *)&(pCenterLEquiv[0]), scaleLEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd M2L(3 * equivN, 3 * equivN); // Laplace, 1->1

    Eigen::MatrixXd A(3 * checkN, 3 * equivN);
    A.setZero();
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                               pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l],
                                         pointLEquiv[3 * l + 1],
                                         pointLEquiv[3 * l + 2]);
            EVec3 temp = gKernel(Cpoint, Lpoint);
            A(3 * k, 3 * l) = temp[0];
            A(3 * k + 1, 3 * l + 1) = temp[1];
            A(3 * k + 2, 3 * l + 2) = temp[2];
            // A.block(k, l, 1, 3) = gKernel(Cpoint, Lpoint).transpose();
        }
    }
    Eigen::MatrixXd ApinvU(A.cols(), A.rows());
    Eigen::MatrixXd ApinvVT(A.cols(), A.rows());
    pinv(A, ApinvU, ApinvVT);

#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1],
                                     pointMEquiv[3 * i + 2]);
        //		std::cout << "debug:" << Mpoint << std::endl;

        // assemble linear system
        Eigen::VectorXd f0(3 * checkN);
        Eigen::VectorXd f1(3 * checkN);
        Eigen::VectorXd f2(3 * checkN);
        f0.setZero();
        f1.setZero();
        f2.setZero();
        for (int k = 0; k < checkN; k++) {
            Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                                   pointLCheck[3 * k + 2]);
            //			std::cout<<"debug:"<<k<<std::endl;
            // sum the images
            EVec3 temp = gKernelFF(Cpoint, Mpoint);
            f0[3 * k] = temp[0];
            f1[3 * k + 1] = temp[1];
            f2[3 * k + 2] = temp[2];
        }
        // std::cout << "debug:" << f0 << std::endl;

        M2L.block(0, 3 * i, 3 * equivN, 1) =
            (ApinvU.transpose() * (ApinvVT.transpose() * f0));
        M2L.block(0, 3 * i + 1, 3 * equivN, 1) =
            (ApinvU.transpose() * (ApinvVT.transpose() * f1));
        M2L.block(0, 3 * i + 2, 3 * equivN, 1) =
            (ApinvU.transpose() * (ApinvVT.transpose() * f2));
    }

    // dump M2L
    for (int i = 0; i < 3 * equivN; i++) {
        for (int j = 0; j < 3 * equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific
                      << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        dipolePoint(1);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        dipoleValue(1);
    dipolePoint[0] = Eigen::Vector3d(0.2, 0.3, 0.4);
    dipoleValue[0] = Eigen::Vector3d(0.1, 0.2, 0.3);

    // solve M
    A.resize(3 * checkN, 3 * equivN);
    ApinvU.resize(A.cols(), A.rows());
    ApinvVT.resize(A.cols(), A.rows());
    Eigen::VectorXd f(3 * checkN);
    for (int k = 0; k < checkN; k++) {
        EVec3 sum(0, 0, 0);
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1],
                               pointMCheck[3 * k + 2]);
        for (size_t p = 0; p < dipolePoint.size(); p++) {
            EVec3 temp = gKernel(Cpoint, dipolePoint[p]);
            sum[0] += temp[0] * dipoleValue[p][0];
            sum[1] += temp[1] * dipoleValue[p][1];
            sum[2] += temp[2] * dipoleValue[p][2];
        }
        f[3 * k] = sum[0];
        f[3 * k + 1] = sum[1];
        f[3 * k + 2] = sum[2];
        for (int l = 0; l < equivN; l++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1],
                                   pointMEquiv[3 * l + 2]);
            // A(k, l) = gKernel(Mpoint, Cpoint);
            EVec3 temp = gKernel(Cpoint, Mpoint);
            A(3 * k, 3 * l) = temp[0];
            A(3 * k + 1, 3 * l + 1) = temp[1];
            A(3 * k + 2, 3 * l + 2) = temp[2];
        }
    }
    pinv(A, ApinvU, ApinvVT);
    Eigen::VectorXd Msource = (ApinvU.transpose() * (ApinvVT.transpose() * f));

    std::cout << "Msource: " << Msource << std::endl;

    Eigen::VectorXd M2Lsource = M2L * (Msource);

    Eigen::Vector3d samplePoint(0.7, 0.9, 0.7);
    //    Eigen::Vector3d samplePoint = dipolePoint[0];
    double Usample = 0;
    double UsampleSP = 0;

    for (int i = -DIRECTLAYER; i < 1 + DIRECTLAYER; i++) {
        for (int j = -DIRECTLAYER; j < 1 + DIRECTLAYER; j++) {
            for (int k = -DIRECTLAYER; k < 1 + DIRECTLAYER; k++) {
                for (size_t p = 0; p < dipolePoint.size(); p++) {
                    Usample +=
                        gKernel(samplePoint, dipolePoint[p] + EVec3(i, j, k))
                            .dot(dipoleValue[p]);
                }
            }
        }
    }

    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1],
                               pointLEquiv[3 * p + 2]);
        EVec3 M2Lsp(M2Lsource[3 * p], M2Lsource[3 * p + 1],
                    M2Lsource[3 * p + 2]);
        UsampleSP += gKernel(samplePoint, Lpoint).dot(M2Lsp);
    }

    std::cout << "samplePoint:" << samplePoint << std::endl;
    std::cout << "Usample NF:" << Usample << std::endl;
    std::cout << "Usample FF:" << UsampleSP << std::endl;
    std::cout << "Usample FF+NF total:" << UsampleSP + Usample << std::endl;

    std::cout
        << "Error : "
        << UsampleSP + Usample -
               gKernelEwald(samplePoint, dipolePoint[0]).dot(dipoleValue[0])
        << std::endl;

    return 0;
}

} // namespace Laplace3D3DDipole

#undef DIRECTLAYER
#undef PI314
#undef E271


#pragma once

#include <Eigen/Dense>
#include <vector>
#include "../flux_worker.hpp"
#include "../outflow_helper.hpp"
#include "aux/eigen2hdf.hpp"
#include "base/numbers.hpp"

namespace boltzmann {
namespace impl_mls {

class DiffusiveReflection : public boltzmann::impl::flux_worker
{
 public:
  /**
   * @param hw Hermite quad. weights (from @see QHermiteW)
   * @param hx Hermite quad. nodes
   * @param vt tangential velocity (moving wall)
   * @param Tw wall temperature
   * @param rho scaling factor
   *
   */
  DiffusiveReflection(const vec_t& hw, const vec_t& hx, double vt, double Tw, double rho = 1.0);

  virtual void apply(mat_t& out,
                     const mat_t& in,
                     const dealii::Point<2>& dummy = dealii::Point<2>(0, 0)) const;

 private:
  boltzmann::impl::outflow_helper outflow_;
  const vec_t w_;
  const vec_t x_;
  double rho_;

  // Eigen::MatrixXd rho_minus_;
  Eigen::MatrixXd Mw_;
};

// --------------------------------------------------------------------------------
DiffusiveReflection::DiffusiveReflection(
    const vec_t& hw, const vec_t& hx, double vt, double Tw, double rho)
    : outflow_(hw, hx)
    , w_(hw)
    , x_(hx)
    , rho_(rho)
{
  unsigned int K = hx.size();
  Mw_.resize(K, K);

  unsigned int khalf = K / 2;

  // normalization factor
  const double fMw = rho_ / std::sqrt(2 * numbers::PI) / std::pow(Tw, 1.5);

  // initialize maxwellian
  for (unsigned int i = 0; i < K; ++i) {
    for (unsigned int j = 0; j < K; ++j) {
      if (i < khalf) {  // inflow boundary
        double x2h = std::pow(x_[i], 2) + std::pow(x_[j] - vt, 2);
        Mw_(i, j) = fMw * std::exp(0.5 * x2h * (-1. / Tw)) * std::sqrt(w_[i] * w_[j]) * x_[i];
      } else {  // outflow
        Mw_(i, j) = 0;
      }
    }
  }

}

// --------------------------------------------------------------------------------
void
DiffusiveReflection::apply(mat_t& out, const mat_t& in, const dealii::Point<2>& dummy) const
{
  int K = out.cols();

  // compute outflow
  double rhom = 0;
  for (int i = 0; i < K; ++i) {
    for (int j = 0; j < K; ++j) {
      rhom += in(i, j) * outflow_.get_y(i) * outflow_.get_x(j);
    }
  }

  int khalf = K / 2;

  out.setZero();

  // outflow
  for (int i = khalf; i < K; ++i) {
    for (int j = 0; j < K; ++j) {
      out(i, j) = in(i, j) * x_[i];
    }
  }

  // inflow M_w
  out += Mw_ * rhom;
}

}  // namespace impl_mls
}  // end namespace boltzmann


#include <cmath>
#include <cstdlib>
#include <memory>

#include <Eigen/Core>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <geometric-vision/linear-triangulation.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

using namespace geometric_vision;  // NOLINT

class GeometricVisionNViewParamTest
    : public ::testing::TestWithParam<Eigen::Vector3d> {
 protected:
  Eigen::Vector2d reprojectPoint(
      const pose::Transformation& G_T_C, const Eigen::Vector3d& G_p_fi) {
    const Eigen::Vector3d C_p_fi = G_T_C.inverse() * G_p_fi;
    return C_p_fi.hnormalized().head<2>();
  }
};

INSTANTIATE_TEST_CASE_P(
    GeometricVision, GeometricVisionNViewParamTest,
    ::testing::Values(
        Eigen::Vector3d(1.5, 0.0, 4.0), Eigen::Vector3d(1.1, 0.1, 3.0),
        Eigen::Vector3d(0.9, -0.05, 1.43), Eigen::Vector3d(1.2, 0.07, 2.73),
        Eigen::Vector3d(2.1, -0.05, 5.2), Eigen::Vector3d(1.8, -0.05, 2.8),
        Eigen::Vector3d(-0.2, -0.25, 1.2), Eigen::Vector3d(3.1, -1.05, 6.1)));

TEST_P(GeometricVisionNViewParamTest, NViewTriangulationTest) {
  const unsigned int number_of_views = 5;

  pose::Quaternion rotations[] = {
      pose::Quaternion(1, 0, 0, 0),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.15, Eigen::Vector3d(0.0, 1.0, 0.1)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.05, Eigen::Vector3d(0.3, 1.0, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.15, Eigen::Vector3d(0.2, 0.3, 0.1)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(-0.1, Eigen::Vector3d(0.1, 1.0, 0.0)))
              .normalized())};

  pose::Position3D positions[] = {
      pose::Position3D(0, 0, 0), pose::Position3D(-3, 0, 0),
      pose::Position3D(0.85, 0.1, -0.3), pose::Position3D(-0.1, -0.05, 0.4),
      pose::Position3D(0.7, 0.3, 0.21)};

  Eigen::Vector3d G_p_fi = GetParam();

  Aligned<std::vector, Eigen::Vector2d> measurements;
  Aligned<std::vector, pose::Transformation> camera_poses;
  camera_poses.resize(number_of_views);
  measurements.resize(number_of_views);
  for (unsigned int i = 0; i < number_of_views; ++i) {
    pose::Transformation G_T_Ci(positions[i], rotations[i]);
    measurements[i] = reprojectPoint(G_T_Ci, G_p_fi);
    camera_poses[i] = G_T_Ci;
  }

  Eigen::Vector3d triangulated_point;
  LinearTriangulation triangulator;
  EXPECT_TRUE(
      triangulator.triangulateFromNormalizedNViews(
          measurements, camera_poses, &triangulated_point));
  EXPECT_NEAR_EIGEN(G_p_fi, triangulated_point, 1e-12);
}

TEST_P(GeometricVisionNViewParamTest, NoisyNViewTriangulationTest) {
  srand(1);
  const unsigned int number_of_views = 20;

  pose::Quaternion rotations[] = {
      pose::Quaternion(1, 0, 0, 0),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.15, Eigen::Vector3d(0.0, 1.0, 0.1)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.05, Eigen::Vector3d(0.3, 1.0, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.02, Eigen::Vector3d(-0.2, 0.3, 0.1)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(-0.1, Eigen::Vector3d(0.12, 1.0, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(-0.1, Eigen::Vector3d(-0.1, 1.0, 0.2)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.01, Eigen::Vector3d(0.1, 1.0, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(-0.3, Eigen::Vector3d(0.1, 1.0, 0.1)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(-1.5, Eigen::Vector3d(-0.1, 1.0, 0.3)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(-0.3, Eigen::Vector3d(0.1, 1.0, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.1, Eigen::Vector3d(0.1, 1.0, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.0, Eigen::Vector3d(0.0, 1.0, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.02, Eigen::Vector3d(0.1, 1.0, 0.1)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.01, Eigen::Vector3d(0.1, 1.0, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.0, Eigen::Vector3d(0.1, 1.0, 0.1)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.8, Eigen::Vector3d(0.0, 1.0, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.0, Eigen::Vector3d(0.1, 1.0, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.1, Eigen::Vector3d(0.5, 0.5, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(0.0, Eigen::Vector3d(0.0, 1.0, 0.0)))
              .normalized()),
      pose::Quaternion(
          Eigen::Quaterniond(
              Eigen::AngleAxisd(-0.02, Eigen::Vector3d(0.1, 1.0, 0.1)))
              .normalized())};

  pose::Position3D positions[] = {
      pose::Position3D(0, 0, 0),           pose::Position3D(-3, 0, 0),
      pose::Position3D(-1.85, 0.6, -0.3),  pose::Position3D(-0.1, -0.05, 0.4),
      pose::Position3D(0.7, 0.1, -0.05),   pose::Position3D(-3.7, -1.1, 0.05),
      pose::Position3D(-0.53, 1.3, 0.13),  pose::Position3D(-2.5, -0.3, 0.21),
      pose::Position3D(-3.8, 0.1, 1),      pose::Position3D(0.7, -0.6, 1),
      pose::Position3D(-5, 0.43, 1),       pose::Position3D(-1.7, -0.6, 1),
      pose::Position3D(-4, -0.13, 1),      pose::Position3D(-3.5, 0, 0.1),
      pose::Position3D(-3, 0.1, 0),        pose::Position3D(-1.85, 0.12, -0.32),
      pose::Position3D(-0.1, -0.25, 0.41), pose::Position3D(-0.89, 0.12, -0.05),
      pose::Position3D(-3.7, -0.3, 0.02),  pose::Position3D(2, 0.2, 0.19)};

  Eigen::Vector3d G_p_fi = GetParam();

  Aligned<std::vector, Eigen::Vector2d> measurements;
  Aligned<std::vector, pose::Transformation> camera_poses;
  camera_poses.resize(number_of_views);
  measurements.resize(number_of_views);
  for (unsigned int i = 0; i < number_of_views; ++i) {
    pose::Transformation G_T_Ci(positions[i], rotations[i]);
    measurements[i] = reprojectPoint(G_T_Ci, G_p_fi);
    measurements[i] += Eigen::Vector2d::Random() / 1e4;
    camera_poses[i] = G_T_Ci;
  }

  Eigen::Vector3d triangulated_point;
  LinearTriangulation triangulator;
  EXPECT_TRUE(
      triangulator.triangulateFromNormalizedNViews(
          measurements, camera_poses, &triangulated_point));

  // Check consistency of reprojection errors.
  for (unsigned int i = 0; i < number_of_views; ++i) {
    Eigen::Vector2d reprojected_point =
        reprojectPoint(camera_poses[i], triangulated_point);
    EXPECT_NEAR_EIGEN(
        reprojected_point, reprojectPoint(camera_poses[i], G_p_fi), 1e-3);
  }
  // Check retriangulated point coordinates.
  EXPECT_NEAR_EIGEN(G_p_fi, triangulated_point, 1e-3);
}

MAPLAB_UNITTEST_ENTRYPOINT


// Compile:
// g++ -I /usr/include/eigen3/ softmaxspeed.cpp -o softmaxspeed
// Run:
// ./softmaxspeed 10 20 30 40

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

using namespace Eigen;
using namespace std;

int main(int argc, char *argv[])
{
  if ( argc != 5 ) {
    cout << "Usage: softmaxspeed T U N n_trials" << endl;
    return 0;
  }

  // Number of timesteps in the input
  const int T = atoi(argv[1]);
  // Number of output timesteps
  const int U = atoi(argv[2]);
  // Dimensionality of the input/output
  const int N = atoi(argv[3]);
  // Number of trials to run for timing
  const int n_trials = atoi(argv[4]);

  // Initializers
  double energy_max;
  VectorXd energy;
  VectorXd attention;
  VectorXd context;
  MatrixXd x = MatrixXd::Random(T, N);
  MatrixXd s = MatrixXd::Random(U, N);

  // Time it
  clock_t begin = clock();
  for (int trial = 0; trial < n_trials; trial++) {
    for (int i = 0; i < U; i++) {
      // Compute dot product of the i'th row of s against all rows of x
      energy = x * s.row(i).transpose();
      // Compute softmax(energy)_n = exp(energy[n] - max(energy))/sum(exp(energy))
      energy_max = energy.maxCoeff();
      energy -= energy_max*VectorXd::Ones(energy.size());
      attention = energy.array().exp();
      attention /= attention.array().sum();
      // Compute weighted sum of each entry in x
      context = (x.array().colwise() * attention.array()).colwise().sum();
    }
  }
  clock_t end = clock();
  cout << double(end - begin) / CLOCKS_PER_SEC << endl;
}


#include "Mesh_Data_Struct.h"
#include "igl/circulation.h"
#include "igl/vertex_triangle_adjacency.h"
#include <Eigen/LU>

Mesh_Data_Struct::Mesh_Data_Struct(std::string filename, Eigen::MatrixXd& data_F_normals)
{
	igl::read_triangle_mesh(filename, OV, OF);
	V = OV;
	F = OF;
	F_normals = data_F_normals;
	igl::edge_flaps(F, E, EMAP, EF, EI);
	Qit->resize(E.rows());

	C.resize(E.rows(), V.cols());

	Q->clear();
	QV->resize(V.rows());

	std::vector<std::vector<int>> VF;
	std::vector<std::vector<int>> VFi;

	igl::vertex_triangle_adjacency(V, F, VF, VFi);

	for (int v = 0; v < V.rows(); v++) {
		(*QV)[v] = Eigen::Matrix4d::Zero();
		for (int f = 0; f < VF[v].size(); f++) {
			Eigen::Vector3d norm = F_normals.row(VF[v][f]).normalized();
			double d = V.row(v) * norm;
			double a = norm[0];
			double b = norm[1];
			double c = norm[2];
			d *= -1;
			Eigen::Matrix4d Kp;
			Kp.row(0) = Eigen::Vector4d(a * a, a * b, a * c, a * d);
			Kp.row(1) = Eigen::Vector4d(a * b, b * b, b * c, b * d);
			Kp.row(2) = Eigen::Vector4d(a * c, c * b, c * c, c * d);
			Kp.row(3) = Eigen::Vector4d(a * d, d * b, d * c, d * d);
			(*QV)[v] += Kp;
		}
	}

	for (int e = 0; e < E.rows(); e++)
		{
		double cost = e;
		Eigen::RowVector3d p;

		Eigen::Matrix4d QUAD = (*QV)[E(e,0)] + (*QV)[E(e, 1)];
		Eigen::Matrix4d QUADp = Eigen::Matrix4d::Identity();
		QUADp.row(0) = QUAD.row(0);
		QUADp.row(1) = QUAD.row(1);
		QUADp.row(2) = QUAD.row(2);
		//QUADp.block(0, 0, 3, 4) = QUAD3.block(0, 0, 3, 4);
		Eigen::Vector4d temp;
		if (QUADp.fullPivLu().isInvertible()) {
			temp = QUADp.inverse() * (Eigen::Vector4d(0.0, 0.0, 0.0, 1.0));
			p[0] = temp[0];
			p[1] = temp[1];
			p[2] = temp[2];

		}
		else {
			p = V.row(E(e, 0));
			temp[0] = p[0];
			temp[1] = p[1];
			temp[2] = p[2];
			temp[3] = 1.0;

		}

		cost = temp.transpose() * QUAD * temp;

		C.row(e) = p;
		(*Qit)[e] = Q->insert(std::pair<double, int>(cost, e)).first;
	}
	num_collapsed = 0;

}

Mesh_Data_Struct:: ~Mesh_Data_Struct() {}


/**
    @file bayes_classifier.cpp

    @author Terence Henriod

    Project 1: Bayesian Minimum Error Classification

    @brief Class implementations for the BayesClassifier defined in
           bayes_classifier.h.

    @version Original Code 1.00 (3/8/2014) - T. Henriod
*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   HEADER FILES / NAMESPACES
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

// Class Declaration
#include "bayes_classifier.h"

// Other Dependencies
#include <cassert>
#include <iostream>
#include <fstream>
#include <Eigen/Dense>  // -I /home/thenriod/Desktop/cpp_libs/Eigen_lib

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
================================================================================
                   CLASS FUNCTION IMPLEMENTATIONS
================================================================================
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   CONSTRUCTOR(S) / DESTRUCTOR
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/**
BayesClassifier

Description

@pre
-# The GameState object is given an appropriate identifier.

@post
-# A new, empty GameState will be initialized.

@code
@endcode
*/
BayesClassifier::BayesClassifier()
{
  // variables
  int ndx = 0;
  Eigen::Vector2d temp_mean;
    temp_mean << 1, 1;
  Eigen::Matrix2d temp_matrix;
    temp_matrix << 1, 0,
                   0, 1;

  // initialize all members
  mean_vector_ << 1, 1;
  covariance_matrix_ << 1, 0,
                       0, 1;
  inverse_covariance_matrix_ = covariance_matrix_.inverse();
  covariance_determinant_ = covariance_matrix_.determinant();
  prior_probability_ = 0.5;
  class_name_ = "Give me a name!";

  // no return - constructor
}

BayesClassifier::BayesClassifier( const BayesClassifier& other )
{
  // no return - copy constructor
}

BayesClassifier& BayesClassifier::operator=( const BayesClassifier& other )
{
  // return *this
  return *this;
}

BayesClassifier::~BayesClassifier()
{
  // currently nothing to destruct

  // no return - destructor
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   MUTATORS
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

void BayesClassifier::clear()
{
  // no return - void
}

void BayesClassifier::setMean( const Eigen::Vector2d& new_mean_vector )
{
  // set the appropriate mean vector
  mean_vector_ = new_mean_vector;

  // no return - void
}

void BayesClassifier::setMean( const vector<DataItem>& data )
{
  // variables
  Eigen::Vector2d new_mean;
    new_mean << 0, 0;
  int i = 0;
  int num_data = 0;

  // sum the values of the features over all of the data
  for( i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sum
      new_mean( 0 ) += data[i].feature_vector( 0 );
      new_mean( 1 ) += data[i].feature_vector( 1 );
      num_data++;
    }
  }

  // scale the data
  new_mean( 0 ) /= num_data;
  new_mean( 1 ) /= num_data;

  // update the new mean member
  mean_vector_ = new_mean;

  // no return - void
}

void BayesClassifier::setCovariance(
    const Eigen::Matrix2d& new_covariance_matrix )
{
  // set the appropriate covariance matrix
  covariance_matrix_ = new_covariance_matrix;

  // update the other covariance related members
  inverse_covariance_matrix_ = covariance_matrix_.inverse();
  covariance_determinant_ = covariance_matrix_.determinant();

  // no return - void
}

void BayesClassifier::setCovariance( const vector<DataItem>& data,
                                     const Eigen::Vector2d& mean )
{
  // variables
  Eigen::Matrix2d new_covariance;
     new_covariance << 0, 0,
                       0, 0;
  double temp = 0;
  int i = 0;
  int num_data = 0;

  // sum the values of the features over all of the data
  for( num_data = 0, temp = 0, i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sum
      temp += ( data[i].feature_vector( 0 ) - mean( 0 ) ) *
              ( data[i].feature_vector( 0 ) - mean( 0 ) );
      num_data++;
    }
  }

  // scale the result
  new_covariance( 0, 0 ) /= (num_data - 1);

  // sum the values of the features over all of the data
  for( temp = 0, i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sum
      temp += ( data[i].feature_vector( 0 ) - mean( 0 ) ) *
              ( data[i].feature_vector( 1 ) - mean( 1 ) );
    }
  }

  // scale the result
  new_covariance( 0, 1 ) /= (num_data - 1);
  new_covariance( 1, 0 ) = new_covariance( 0, 1 );

  // sum the values of the features over all of the data
  for( temp = 0, i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sum
      temp += ( data[i].feature_vector( 1 ) - mean( 1 ) ) *
              ( data[i].feature_vector( 1 ) - mean( 1 ) );
    }
  }

  // scale the result
  new_covariance( 1, 1 ) /= (num_data - 1);

  // update the new mean member
  covariance_matrix_ = new_covariance;

  // update the other covariance related members
  inverse_covariance_matrix_ = covariance_matrix_.inverse();
  covariance_determinant_ = covariance_matrix_.determinant();

  // no return - void
}

void BayesClassifier::setPriorProbability( const double new_probability )
{
  // assert pre-conditions
  assert( ( new_probability >= 0.0 ) && ( new_probability <= 1.0 ) );

  // set the new prior probability
  prior_probability_ = new_probability;

  // no return - void
}

void BayesClassifier::set_class_name( const string& new_name )
{
  // set the class name member
  class_name_ = new_name;
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   ACCESSORS
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
double BayesClassifier::prior_probability() const
{
  // return the prior probability of the class
  return prior_probability_;
}

Eigen::Vector2d BayesClassifier::mean_vector() const
{
  // return the prior mean feature vector of the class
  return mean_vector_;
}

Eigen::Matrix2d BayesClassifier::covariance_matrix() const
{
  // return the covariance matrix of the class
  return covariance_matrix_;
}

Eigen::Matrix2d BayesClassifier::inverse_covariance_matrix() const
{
  // return the inverse of the covariance matrix of the class
  return inverse_covariance_matrix_;
}

double BayesClassifier::covariance_determinant() const
{
  // return the determinant of the covariance matrix of the class
  return covariance_determinant_;
}

string BayesClassifier::class_name() const
{
  // return the class name
  return class_name_;
}

void BayesClassifier::performAnalysis(
    const string input_file,
    const string output_file,
    const vector<BayesClassifier>& classifiers )
{

/*
  // variables
  fstream file;
  TestData temp;
  char delimiter;
  int num_data = 0;
  int ndx = 0;
  int num_misclassified = 0;
  double test_error_rate = 0.5;
  double beta_start = 0.5;
  vector<TestData> data;
  Chernoff chernoff_bound;

  // read in all of the data
  file.clear();
  file.open( input_file.c_str(), fstream::in );
  while( file.good() )
  {
    // read in a line of data
    file >> temp.feature_vector(0) >> delimiter
         >> temp.feature_vector(1) >> delimiter
         >> temp.actual_class;

    // store the data
    data.push_back( temp );

    // count the data
    num_data++;
  }
  file.close();

  // classify all of the data
  for( ndx = 0; ndx < num_data; ndx++ )
  {
    // classify an object
    data[ndx].classified_as = assignToClass( data[ndx].feature_vector,
                                             classifiers );

    // case: it was classified correctly
    if( data[ndx].classified_as == data[ndx].actual_class )
    {
      // mark and count this as a correct classification
      data[ndx].correctly_classified = CORRECT;
    }
    // case: it was not classified correctly
    else
    {
      // mark this as an incorrect classification
      data[ndx].correctly_classified = INCORRECT;
      num_misclassified++;
    }
  }

  // compute the error rate of the test
  test_error_rate = double( num_misclassified ) / double( num_data );

  // find chernoff bound
  chernoff_bound = findChernoffBound( beta_start, 0 );

  // output the results to file
  file.clear();
  file.open( output_file.c_str(), fstream::out );
  file << "Number of data: " << ( num_data - 1 ) << endl
       << "Number of incorrect classifications: "
           << num_misclassified << endl
       << "Test Sample Error Rate: " << test_error_rate << endl
       << "Battacharyya bound: " << findBattacharyyaBound() << endl
       << "Chernoff bound: " << chernoff_bound.bound << endl
       << "         beta*: " << chernoff_bound.beta << endl;
  for( ndx = 0; ndx < num_data; ndx++ )
  {
    // write the delimited data to the file
    file << data[ndx].feature_vector(0) << ", "
         << data[ndx].feature_vector(1) << ", "
         << data[ndx].actual_class << ", "
         << data[ndx].classified_as << ", "
         << data[ndx].correctly_classified << endl;
  }
  file.close();
*/
  // no return - void
}

string BayesClassifier::assignToClass(
    Eigen::Vector2d& input_vector,
    vector<BayesClassifier>& classifiers )
{
  // variables
  string classification_result = "NO_RESULT";
  int i = 0;
  double largest_discriminant = 0;
  double largest_discriminant_ndx = 0;

  // calculate each of the discriminants, keep track of the most likely class
  for( i = 0; i < classifiers.size(); ++i )
  {
    // case: this discriminant is larger than the previously largest one
    if( largest_discriminant <
        classifiers[i].calculateDiscriminant( input_vector ) )
    {
      // store the index of the class
      largest_discriminant_ndx = i;
    }
  }

  // whichever class produced the largest discriminant is the most likely
  classification_result = classifiers[largest_discriminant_ndx].class_name();

  // return the resulting assignment
  return classification_result;
}

double BayesClassifier::calculateDiscriminant(
    const Eigen::Vector2d& input_vector )
{
  // variables
  double discriminant_result = 0;
  double first_sum_term = 0;
  double second_sum_term = 0;
  double third_sum_term = 0;
  double fourth_sum_term = 0;
  double fifth_sum_term = 0;
  Eigen::Vector2d intermediate_row;
  Eigen::Vector2d intermediate_col;
  Eigen::Matrix2d intermediate_matrix;

  // compute the first summative term of the discriminant function
  intermediate_matrix = -0.5 * inverse_covariance_matrix_;
  intermediate_row = ( input_vector.transpose() * intermediate_matrix );
  first_sum_term = intermediate_row.dot( input_vector );

  // compute the second summative term of the discriminant function
  second_sum_term =
  ( inverse_covariance_matrix_ * mean_vector_ ).transpose().dot( input_vector );

  // compute the third summative term of the discriminant function
  intermediate_row = -0.5 * mean_vector_;
  intermediate_row = intermediate_row.transpose() * inverse_covariance_matrix_;
  third_sum_term = intermediate_row.dot( mean_vector_ );

  // compute the fourth summative term of the discriminant function
  fourth_sum_term = -0.5 * log( covariance_determinant_ );

/*  // case: we are assuming case 1 assumptions
  else
  {
    // compute the first term ( 1/s^2 * mean * x )
    intermediate_row = ( 1.0 / variance ) * mean;
    first_sum_term = intermediate_row.transpose().dot( input_vector );

    // compute the second term
    intermediate_row = ( -1.0 / ( 2 * variance ) ) * mean;
    second_sum_term = intermediate_row.transpose().dot( mean );
  }
 */

  // compute the last summative term of the discriminant function
  fifth_sum_term = log( prior_probability_ );

  // sum the terms to get the discriminant result
  discriminant_result = first_sum_term + second_sum_term + third_sum_term +
                        fourth_sum_term + fifth_sum_term;

  // return the discriminant result
  return discriminant_result;
}

Chernoff BayesClassifier::findChernoffBound(
    const vector<BayesClassifier>& classifiers )
{
  // variables
  Chernoff chernoff_bound;
    chernoff_bound.bound = 1.0;
    chernoff_bound.beta_star = 1.0;
  double beta = 0;
  double prior_product = 0.0;
  double kappa_of_beta = 0;
  double new_attempt;

  // test many possibilities to find the ideal beta*
  for( beta = 0.0; beta < 1.0; beta += EPSILON )
  {
    // compute the prior product
    prior_product = pow( classifiers[0].prior_probability(), beta ) *
                    pow( classifiers[1].prior_probability(), ( 1.0 - beta ) );

    // kappa( beta* )
    kappa_of_beta = kappaF( beta, classifiers );

    // attempt to find a lower bound
    new_attempt = prior_product * exp( -1.0 * kappa_of_beta );

    // case: the new bound is a tighter one
    if( new_attempt < chernoff_bound.bound )
    {
      // set the new bound and beta_star
      chernoff_bound.bound = new_attempt;
      chernoff_bound.beta_star = beta;
    }
  }

  // return the Chernoff bound
  return chernoff_bound;
}

double BayesClassifier::findBattacharyyaBound(
    const vector<BayesClassifier>& classifiers )
{
  // variables
  double battacharyya_bound = 1;
  double kappa_of_beta = 0;
  double root_prior_product = 0;
  double root_covariance_det_product = 0;
  Eigen::Vector2d mean_difference;
  Eigen::Matrix2d covariance_sum;

  // compute the square root term
  root_prior_product = sqrt( classifiers[0].prior_probability() *
                             classifiers[1].prior_probability() );

  // compute kappa( 0.5 )
  kappa_of_beta = kappaF( 0.5, classifiers );

  // compute sqrt( P( w1 ) * P( w2 ) ) * e^( -kappa( 0.5 ) )
  battacharyya_bound = root_prior_product * exp( -1.0 * kappa_of_beta );

  // return the Battacharrya bound
  return battacharyya_bound;
}

double BayesClassifier::kappaF( const double beta,
                                const vector<BayesClassifier>& classifiers )
{
  // variables
  double kappa_of_beta = 0;
  double beta_complement = 1.0 - beta;
  double beta_product_over_two = 0;
  double root_prior_product = 0;
  double root_covariance_det_product = 0;
  double log_denominator = 0;
  Eigen::Vector2d mean_difference;
  Eigen::Vector2d intermediate_row;
  Eigen::Matrix2d scaled_covariance_sum;

  // compute (beta * beta^c) / 2
  beta_product_over_two = ( beta * beta_complement ) / 2;

  // compute the mean difference u2 - u1 (to be used later)
  mean_difference = classifiers[0].mean_vector() - classifiers[1].mean_vector();

  // compute the scaled covariance sum beta^c * E1 + beta * E2
  // (to be used later)
  scaled_covariance_sum =
      ( beta_complement * classifiers[0].covariance_matrix() ) +
      ( beta * classifiers[1].covariance_matrix() );

  // compute the logarithm denominator (to be used later)
  log_denominator = pow( classifiers[0].covariance_determinant(),
                         beta_complement );
  log_denominator *= pow( classifiers[1].covariance_determinant(),
                          beta );

  // compute the first term in the sum
  intermediate_row = beta_product_over_two * mean_difference;
  intermediate_row = intermediate_row.transpose() *
                     scaled_covariance_sum.inverse();
  kappa_of_beta = intermediate_row.transpose() * mean_difference;

  // compute the second term in the sum
  kappa_of_beta += 0.5 * log( scaled_covariance_sum.determinant() /
                              log_denominator );

  // return the result
  return kappa_of_beta;
}


//  Copyright 2020 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <iostream>
#include <benchmark/benchmark.h>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/gmp.hpp>

#include <boost/math/special_functions/prime.hpp>
#include <boost/math/special_functions/pow.hpp>

#include <gmpxx.h>

template <class Rational, class Integer = typename Rational::value_type>
Rational zeta18()
{
   Rational result = 1;

   for (unsigned i = 0; i < 10; ++i)
   {
      result /= 1 - Rational(1, boost::math::pow<18>(Integer(boost::math::prime(i))));
   }
   return result;
}

template <class Rational, class Integer = typename Rational::value_type>
static void BM_zeta18(benchmark::State& state)
{
   for (auto _ : state)
   {
      benchmark::DoNotOptimize(zeta18<Rational, Integer>());
   }
}

BENCHMARK_TEMPLATE(BM_zeta18, boost::multiprecision::cpp_rational);
BENCHMARK_TEMPLATE(BM_zeta18, boost::multiprecision::mpq_rational);
BENCHMARK_TEMPLATE(BM_zeta18, boost::multiprecision::number<boost::multiprecision::rational_adaptor<boost::multiprecision::gmp_int>>);
BENCHMARK_TEMPLATE(BM_zeta18, mpq_class, mpz_class);

BENCHMARK_MAIN();


//
// Created by Cephas Svosve on 17/6/2021.
//

#include "dividend.h"
#include <iostream>
#include <vector>
#include <math.h>
#include <iostream>
#include <boost/random.hpp>

//get dividend growth rate i.e average of the percentage change in dividend paid-out, dD(t)/D(t-1).

double Div::getGrowthRate(int tickerID)
{
 return growthRate[tickerID];
}

//get volatility i.e stdev of the percentage changes in dividend paid-out, dD(t)/D(t-1).

double Div::getVolatility(int tickerID)
{
    return volatility[tickerID];
}

//we retrieve the autocorrelation of a particular asset by this function

double Div::getAutoCorr(int tickerID)
{
    return autoCorr[tickerID];
}

//this is a setter for the average growth rate of an asset's growth rate

void Div::setGrowthRate(double x)
{
 growthRate.push_back(x);
}

//this is a setter for saving the volatility of an asset's dividend process

void Div::setVolatility(double x)
{
 volatility.push_back(x);
}

//this is a setter for saving the auto-correlation of an asset's dividend process

void Div::setAutoCorr(double x)
{
 autoCorr.push_back(x);
}

//work-in-progress-*the function is meant to retrieve the cross-sectional correlation from the market object

void Div::setCrossCorr()
{
 // std::cout <<  "Div " << crossCorrMat << std::endl;
}

//because our system depends on accuracy of the randomness of the random matrix,
//we create a function that carefully generates a purely random matrix,
//i.e. auto-correlation is 0 precise to the order of e-03 as well as the cross-sectional correlation

MatrixXd Div::generateWhiteNoise(int numOfAssets, int numOfTicks)
{

    MatrixXd randoms(numOfAssets,numOfTicks);
    VectorXd a;

    //algorithm for generating random numbers that are shuffled continuously through seeding them on time

    time_t now = time(0);
    boost::random::mt19937 gen{static_cast<uint32_t>(now)};
    boost::normal_distribution<> nd(0.0, clock.getDt());
    boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<> > var_nor(gen, nd);

    //we generate the matrix of random numbers

    for(int rw=0; rw < numOfAssets; rw++)
    {

        //we make sure the naturally occurring auto-correlation is sufficiently small by using a do-while loop

     do
        {
            //here we load each row with appropriate vector of random numbers
            a = VectorXd(numOfTicks);

            for(int i = 0; i < numOfTicks; ++i)
            {
                a(i) = var_nor();
            }

        }
        while (abs(lateralcorrcoef(a)) > 0.001);

        randoms.row(rw) = a;

    }

    //We then remove any cross-sectional correlation

    return verticallyWhiten(randoms);
}

//this function removes cross-sectional correlation by multiplying a matrix
// by the inverse of the lower cholesky decomposition of its correlation matrix

MatrixXd Div::verticallyWhiten(MatrixXd mx)
{
    Matrix crossCorrs = crossCorr(mx);
    LLT<MatrixXd> llt(crossCorrs);
    MatrixXd L = llt.matrixL();
    MatrixXd vWhite = L.inverse() * mx;

    return vWhite;
}

//this function calculates the coefficient of determination -correlation- between two vectors
//it requires the two vectors x and y, then computes the variables required for the formula
//i.e. variables xx = x^2  ,xy = x*y  and yy = y^2

double Div::corrcoef(VectorXd x, VectorXd y)
{
    if(x.size() == y.size()) {

        int n = x.size();

        //variable xx represents x^2

        double xx[n];
        double xy[n];
        double yy[n];

        //sumx is the sum of random variables x

        double sumx = x.sum();
        double sumy = y.sum();

        //sumxx is the sum of random variables x^2

        double sumxx;
        double sumxy;
        double sumyy;

        for (int i=0; i < x.size(); i++)
        {
            xx[i] = pow(x[i],2);
            xy[i] = x[i] * y[i];
            yy[i] = pow(y[i],2);

            sumxx = sumxx + xx[i];
            sumxy = sumxy + xy[i];
            sumyy = sumyy + yy[i];
        }

        //this is the formula used to calculate coefficient of determination, *see
        //https://www.jstor.org/stable/2965177?seq=6#metadata_info_tab_contents
        //Reed, William Gardner. “The Coefficient of Correlation.” Publications of the American Statistical Association,
        //vol. 15, no. 118, 1917, pp. 675–684. JSTOR, www.jstor.org/stable/2965177. Accessed 20 June 2021.

        double r = (n*sumxy - sumx*sumy)/sqrt((n*sumxx-pow(sumx,2))*(n*sumyy-pow(sumy,2)));

        return r;
    }
    else
    {
     cout << "found vectors of different size on computing correlation";
    }

}

//this function calculates the auto-correlation coefficient of a vector of random numbers
//we use it to verify that the naturally occuring auto-correlation in the random numbers-
//-we are using is sufficiently small i.e. of order e-03

double Div::lateralcorrcoef(VectorXd a){

    int n = a.size()-1;

    double x[n];
    double y[n];
    double xx[n];
    double xy[n];
    double yy[n];

    double sumx;
    double sumy;
    double sumxx;
    double sumxy;
    double sumyy;

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

        x[i] = a[i+1];
        y[i] = a[i];
        xx[i] = pow(x[i],2);
        xy[i] = x[i] * y[i];
        yy[i] = pow(y[i],2);

        sumx = sumx + x[i];
        sumy = sumy + y[i];

        sumxx = sumxx + xx[i];
        sumxy = sumxy + xy[i];
        sumyy = sumyy + yy[i];

    }

    double r = (n*sumxy - sumx*sumy)/sqrt((n*sumxx-pow(sumx,2))*(n*sumyy-pow(sumy,2)));
    return r;

}

//this function calculates the correlation matrix from the random matrix

MatrixXd Div::crossCorr(MatrixXd x){
    int m = x.col(0).size();
    int n = x.row(0).size();

    MatrixXd corrMat(m,m);

    for(int i = 0; i < m; i++){
        for(int j = 0; j <= i; j++){

            corrMat(i,j) = corrcoef( x.row(i),x.row(j));
            corrMat(j,i) = corrMat(i,j);
        }
    }

    return corrMat;
}

//work-in-progress

MatrixXd Div::verticallyColor(MatrixXd x){}

VectorXd Div::laterallyColor(VectorXd x){}

VectorXd Div::standardize(VectorXd x){}

MatrixXd Div::generateColoredNoise(int numOfAssets, int numOfTicks)
{
    MatrixXd X = generateWhiteNoise(numOfAssets, numOfTicks);
    MatrixXd Y = verticallyWhiten(X);

    //vector<double> lC = laterallyColor(Y(0:));
}


#include <boost/test/unit_test.hpp>
#include "functions/vector_field.hh"
#include "functions/std_functions.hh"
#include "functions/polynomial.hh"

BOOST_AUTO_TEST_CASE(vector_field_test) {
  using namespace manifolds;
  auto v1 = GetVectorField(1_c, 1_c);
  auto f1 = v1(Sin()(x * y));
  auto f1_check = Cos()(x * y) * (x + y);
  BOOST_CHECK_EQUAL(f1(2, 1), f1_check(2, 1));
  BOOST_CHECK_CLOSE(f1(4, 3), f1_check(4, 3), 1E-13);
}


//
// Copyright (c) 2019 INRIA
//

#include "pinocchio/autodiff/casadi.hpp"

#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include "pinocchio/parsers/sample-models.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_jacobian)
{
  typedef double Scalar;
  typedef casadi::SX ADScalar;

  typedef pinocchio::ModelTpl<Scalar> Model;
  typedef Model::Data Data;

  typedef pinocchio::ModelTpl<ADScalar> ADModel;
  typedef ADModel::Data ADData;

  Model model;
  pinocchio::buildModels::humanoidRandom(model);
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  Data data(model);

  typedef Model::ConfigVectorType ConfigVector;
  typedef Model::TangentVectorType TangentVector;
  ConfigVector q(model.nq);
  q = pinocchio::randomConfiguration(model);
  TangentVector v(TangentVector::Random(model.nv));

  typedef ADModel::ConfigVectorType ConfigVectorAD;
  typedef ADModel::TangentVectorType TangentVectorAD;
  ADModel ad_model = model.cast<ADScalar>();
  ADData ad_data(ad_model);

  Model::Index joint_id = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
  Data::Matrix6x jacobian_local(6,model.nv), jacobian_world(6,model.nv);
  jacobian_local.setZero(); jacobian_world.setZero();

  BOOST_CHECK(jacobian_local.isZero() && jacobian_world.isZero());

  pinocchio::computeJointJacobians(model,data,q);
  pinocchio::getJointJacobian(model,data,joint_id,pinocchio::WORLD,jacobian_world);
  pinocchio::getJointJacobian(model,data,joint_id,pinocchio::LOCAL,jacobian_local);

  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
  ConfigVectorAD q_ad(model.nq);
  for(Eigen::DenseIndex k = 0; k < model.nq; ++k)
  {
    q_ad[k] = cs_q(k);
  }
  std::cout << "q =\n " << q_ad << std::endl;

  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
  TangentVectorAD v_ad(model.nv);
  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    v_ad[k] = cs_v(k);
  }
  std::cout << "v =\n " << v_ad << std::endl;

  pinocchio::forwardKinematics(ad_model, ad_data, q_ad, v_ad);
  typedef pinocchio::MotionTpl<ADScalar> MotionAD;
  MotionAD & v_local = ad_data.v[(size_t)joint_id];
  MotionAD v_world = ad_data.oMi[(size_t)joint_id].act(v_local);

  casadi::SX cs_v_local(6,1), cs_v_world(6,1);
  for(Eigen::DenseIndex k = 0; k < 6; ++k)
  {
    cs_v_local(k) = v_local.toVector()[k];
    cs_v_world(k) = v_world.toVector()[k];
  }
  std::cout << "v_local = " << cs_v_local << std::endl;
  std::cout << "v_world = " << cs_v_world << std::endl;

  casadi::Function eval_velocity_local("eval_velocity_local",
                                       casadi::SXVector {cs_q, cs_v},
                                       casadi::SXVector {cs_v_local});
  std::cout << "eval_velocity_local = " << eval_velocity_local << std::endl;

  casadi::Function eval_velocity_world("eval_velocity_world",
                                       casadi::SXVector {cs_q, cs_v},
                                       casadi::SXVector {cs_v_world});
  std::cout << "eval_velocity_world = " << eval_velocity_world << std::endl;

  casadi::SX dv_dv_local = jacobian(cs_v_local, cs_v);
  casadi::Function eval_jacobian_local("eval_jacobian_local",
                                       casadi::SXVector {cs_q,cs_v},
                                       casadi::SXVector {dv_dv_local});
  std::cout << "eval_jacobian_local = " << eval_jacobian_local << std::endl;

  casadi::SX dv_dv_world = jacobian(cs_v_world, cs_v);
  casadi::Function eval_jacobian_world("eval_jacobian_world",
                                       casadi::SXVector {cs_q,cs_v},
                                       casadi::SXVector {dv_dv_world});

  std::vector<double> q_vec((size_t)model.nq);
  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;

  std::vector<double> v_vec((size_t)model.nv);
  Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;

  casadi::DMVector v_local_res = eval_velocity_local(casadi::DMVector {q_vec,v_vec});
  casadi::DMVector J_local_res = eval_jacobian_local(casadi::DMVector {q_vec,v_vec});
  std::cout << "J_local_res:" << J_local_res << std::endl;

  std::vector<double> v_local_vec(static_cast< std::vector<double> >(v_local_res[0]));
  BOOST_CHECK((jacobian_local*v).isApprox(Eigen::Map<pinocchio::Motion::Vector6>(v_local_vec.data())));

  casadi::DMVector v_world_res = eval_velocity_world(casadi::DMVector {q_vec,v_vec});
  casadi::DMVector J_world_res = eval_jacobian_world(casadi::DMVector {q_vec,v_vec});

  std::vector<double> v_world_vec(static_cast< std::vector<double> >(v_world_res[0]));
  BOOST_CHECK((jacobian_world*v).isApprox(Eigen::Map<pinocchio::Motion::Vector6>(v_world_vec.data())));

  Data::Matrix6x J_local_mat(6,model.nv), J_world_mat(6,model.nv);

  std::vector<double> J_local_vec(static_cast< std::vector<double> >(J_local_res[0]));
  J_local_mat = Eigen::Map<Data::Matrix6x>(J_local_vec.data(),6,model.nv);
  BOOST_CHECK(jacobian_local.isApprox(J_local_mat));

  std::vector<double> J_world_vec(static_cast< std::vector<double> >(J_world_res[0]));
  J_world_mat = Eigen::Map<Data::Matrix6x>(J_world_vec.data(),6,model.nv);
  BOOST_CHECK(jacobian_world.isApprox(J_world_mat));
}

  BOOST_AUTO_TEST_CASE(test_fk)
  {
    typedef double Scalar;
    typedef casadi::SX ADScalar;

    typedef pinocchio::ModelTpl<Scalar> Model;
    typedef Model::Data Data;

    typedef pinocchio::ModelTpl<ADScalar> ADModel;
    typedef ADModel::Data ADData;

    Model model;
    pinocchio::buildModels::humanoidRandom(model);
    model.lowerPositionLimit.head<3>().fill(-1.);
    model.upperPositionLimit.head<3>().fill(1.);
    Data data(model);

    typedef Model::ConfigVectorType ConfigVector;
    typedef Model::TangentVectorType TangentVector;
    ConfigVector q(model.nq);
    q = pinocchio::randomConfiguration(model);
    TangentVector v(TangentVector::Random(model.nv));
    TangentVector a(TangentVector::Random(model.nv));

    pinocchio::forwardKinematics(model,data,q);

    typedef ADModel::ConfigVectorType ConfigVectorAD;
    typedef ADModel::TangentVectorType TangentVectorAD;
    ADModel ad_model = model.cast<ADScalar>();
    ADData ad_data(ad_model);

    casadi::SX cs_q = casadi::SX::sym("q", model.nq);
    ConfigVectorAD q_ad(model.nq);
    pinocchio::casadi::copy(cs_q,q_ad);

    casadi::SX cs_v = casadi::SX::sym("v", model.nv);
    TangentVectorAD v_ad(model.nv);
    pinocchio::casadi::copy(cs_v,v_ad);

    casadi::SX cs_a = casadi::SX::sym("a", model.nv);
    TangentVectorAD a_ad(model.nv);
    pinocchio::casadi::copy(cs_a,a_ad);

    pinocchio::forwardKinematics(ad_model, ad_data, q_ad, v_ad, a_ad);
    pinocchio::updateGlobalPlacements(ad_model, ad_data);
    pinocchio::updateFramePlacements(ad_model, ad_data);
//    typedef pinocchio::MotionTpl<ADScalar> MotionAD;
  }

BOOST_AUTO_TEST_CASE(test_rnea)
{
  typedef double Scalar;
  typedef casadi::SX ADScalar;

  typedef pinocchio::ModelTpl<Scalar> Model;
  typedef Model::Data Data;

  typedef pinocchio::ModelTpl<ADScalar> ADModel;
  typedef ADModel::Data ADData;

  Model model;
  pinocchio::buildModels::humanoidRandom(model);
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  Data data(model);

  typedef Model::ConfigVectorType ConfigVector;
  typedef Model::TangentVectorType TangentVector;
  ConfigVector q(model.nq);
  q = pinocchio::randomConfiguration(model);
  TangentVector v(TangentVector::Random(model.nv));
  TangentVector a(TangentVector::Random(model.nv));

  typedef ADModel::ConfigVectorType ConfigVectorAD;
  typedef ADModel::TangentVectorType TangentVectorAD;
  ADModel ad_model = model.cast<ADScalar>();
  ADData ad_data(ad_model);

  pinocchio::rnea(model,data,q,v,a);

  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
  ConfigVectorAD q_ad(model.nq);
  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);

  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
  TangentVectorAD v_ad(model.nv);
  v_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_v).data(),model.nv,1);

  casadi::SX cs_a = casadi::SX::sym("a", model.nv);
  TangentVectorAD a_ad(model.nv);
  a_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_a).data(),model.nv,1);

  rnea(ad_model,ad_data,q_ad,v_ad,a_ad);
  casadi::SX tau_ad(model.nv,1);
  //    Eigen::Map<TangentVectorAD>(tau_ad->data(),model.nv,1)
  //    = ad_data.tau;
  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
    tau_ad(k) = ad_data.tau[k];
  casadi::Function eval_rnea("eval_rnea",
                             casadi::SXVector {cs_q, cs_v, cs_a},
                             casadi::SXVector {tau_ad});

  std::vector<double> q_vec((size_t)model.nq);
  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;

  std::vector<double> v_vec((size_t)model.nv);
  Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;

  std::vector<double> a_vec((size_t)model.nv);
  Eigen::Map<TangentVector>(a_vec.data(),model.nv,1) = a;
  casadi::DM tau_res = eval_rnea(casadi::DMVector {q_vec,v_vec,a_vec})[0];
  std::cout << "tau_res = " << tau_res << std::endl;
  Data::TangentVectorType tau_vec = Eigen::Map<Data::TangentVectorType>(static_cast< std::vector<double> >(tau_res).data(),model.nv,1);

  BOOST_CHECK(data.tau.isApprox(tau_vec));
}

BOOST_AUTO_TEST_CASE(test_crba)
{
  typedef double Scalar;
  typedef casadi::SX ADScalar;

  typedef pinocchio::ModelTpl<Scalar> Model;
  typedef Model::Data Data;

  typedef pinocchio::ModelTpl<ADScalar> ADModel;
  typedef ADModel::Data ADData;

  Model model;
  pinocchio::buildModels::humanoidRandom(model);
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  Data data(model);

  typedef Model::ConfigVectorType ConfigVector;
  typedef Model::TangentVectorType TangentVector;
  ConfigVector q(model.nq);
  q = pinocchio::randomConfiguration(model);
  TangentVector v(TangentVector::Random(model.nv));
  TangentVector a(TangentVector::Random(model.nv));

  typedef ADModel::ConfigVectorType ConfigVectorAD;
  typedef ADModel::TangentVectorType TangentVectorAD;
  ADModel ad_model = model.cast<ADScalar>();
  ADData ad_data(ad_model);

  pinocchio::crba(model,data,q);
  data.M.triangularView<Eigen::StrictlyLower>()
  = data.M.transpose().triangularView<Eigen::StrictlyLower>();
  pinocchio::rnea(model,data,q,v,a);

  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
  ConfigVectorAD q_ad(model.nq);
  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);

  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
  TangentVectorAD v_ad(model.nv);
  v_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_v).data(),model.nv,1);

  casadi::SX cs_a = casadi::SX::sym("a", model.nv);
  TangentVectorAD a_ad(model.nv);
  a_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_a).data(),model.nv,1);

  // RNEA
  rnea(ad_model,ad_data,q_ad,v_ad,a_ad);
  casadi::SX cs_tau(model.nv,1);
  //    Eigen::Map<TangentVectorAD>(tau_ad->data(),model.nv,1)
  //    = ad_data.tau;
  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
    cs_tau(k) = ad_data.tau[k];
  casadi::Function eval_rnea("eval_rnea",
                             casadi::SXVector {cs_q, cs_v, cs_a},
                             casadi::SXVector {cs_tau});
  // CRBA
  crba(ad_model,ad_data,q_ad);
  ad_data.M.triangularView<Eigen::StrictlyLower>()
  = ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
  casadi::SX M_ad(model.nv,model.nv);
  for(Eigen::DenseIndex j = 0; j < model.nv; ++j)
  {
    for(Eigen::DenseIndex i = 0; i < model.nv; ++i)
    {
      M_ad(i,j) = ad_data.M(i,j);
    }
  }

  std::vector<double> q_vec((size_t)model.nq);
  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;

  std::vector<double> v_vec((size_t)model.nv);
  Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;

  std::vector<double> a_vec((size_t)model.nv);
  Eigen::Map<TangentVector>(a_vec.data(),model.nv,1) = a;

  casadi::Function eval_crba("eval_crba",
                             casadi::SXVector {cs_q},
                             casadi::SXVector {M_ad});
  casadi::DM M_res = eval_crba(casadi::DMVector {q_vec})[0];
  Data::MatrixXs M_mat = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(M_res).data(),
                                                    model.nv,model.nv);

  BOOST_CHECK(data.M.isApprox(M_mat));

  casadi::SX dtau_da = jacobian(cs_tau,cs_a);
  casadi::Function eval_dtau_da("eval_dtau_da",
                                casadi::SXVector {cs_q,cs_v,cs_a},
                                casadi::SXVector {dtau_da});
  casadi::DM dtau_da_res = eval_dtau_da(casadi::DMVector {q_vec, v_vec, a_vec})[0];
  Data::MatrixXs dtau_da_mat = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(dtau_da_res).data(),
                                                          model.nv,model.nv);
  BOOST_CHECK(data.M.isApprox(dtau_da_mat));
}

  BOOST_AUTO_TEST_CASE(test_aba)
  {
    typedef double Scalar;
    typedef casadi::SX ADScalar;

    typedef pinocchio::ModelTpl<Scalar> Model;
    typedef Model::Data Data;

    typedef pinocchio::ModelTpl<ADScalar> ADModel;
    typedef ADModel::Data ADData;

    Model model;
    pinocchio::buildModels::humanoidRandom(model);
    model.lowerPositionLimit.head<3>().fill(-1.);
    model.upperPositionLimit.head<3>().fill(1.);
    Data data(model);

    typedef Model::ConfigVectorType ConfigVector;
    typedef Model::TangentVectorType TangentVector;
    ConfigVector q(model.nq);
    q = pinocchio::randomConfiguration(model);
    TangentVector v(TangentVector::Random(model.nv));
    TangentVector tau(TangentVector::Random(model.nv));

    typedef ADModel::ConfigVectorType ConfigVectorAD;
    typedef ADModel::TangentVectorType TangentVectorAD;
    ADModel ad_model = model.cast<ADScalar>();
    ADData ad_data(ad_model);

    pinocchio::aba(model,data,q,v,tau);

    casadi::SX cs_q = casadi::SX::sym("q", model.nq);
    ConfigVectorAD q_ad(model.nq);
    q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);

    casadi::SX cs_v = casadi::SX::sym("v", model.nv);
    TangentVectorAD v_ad(model.nv);
    v_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_v).data(),model.nv,1);

    casadi::SX cs_tau = casadi::SX::sym("tau", model.nv);
    TangentVectorAD tau_ad(model.nv);
    tau_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_tau).data(),model.nv,1);

    // ABA
    aba(ad_model,ad_data,q_ad,v_ad,tau_ad);
    casadi::SX cs_ddq(model.nv,1);
    for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
      cs_ddq(k) = ad_data.ddq[k];
    casadi::Function eval_aba("eval_aba",
                              casadi::SXVector {cs_q, cs_v, cs_tau},
                              casadi::SXVector {cs_ddq});

    std::vector<double> q_vec((size_t)model.nq);
    Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;

    std::vector<double> v_vec((size_t)model.nv);
    Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;

    std::vector<double> tau_vec((size_t)model.nv);
    Eigen::Map<TangentVector>(tau_vec.data(),model.nv,1) = tau;

    casadi::DM ddq_res = eval_aba(casadi::DMVector {q_vec, v_vec, tau_vec})[0];
    Data::TangentVectorType ddq_mat = Eigen::Map<Data::TangentVectorType>(static_cast< std::vector<double> >(ddq_res).data(),
                                                            model.nv,1);

    BOOST_CHECK(ddq_mat.isApprox(data.ddq));
  }

BOOST_AUTO_TEST_SUITE_END()


#include <CGAL/config.h>
#define CGAL_EIGEN3_ENABLED
#if defined(BOOST_GCC) && (__GNUC__ <= 4) && (__GNUC_MINOR__ < 4)
#include <iostream>
int main()
{
  std::cerr << "NOTICE: This test requires G++ >= 4.4, and will not be compiled." << std::endl;
}
#else
#include <CGAL/Epick_d.h>
#include <eigen3/Eigen/Core>
#include <CGAL/Delaunay_triangulation.h>
#include <CGAL/IO/Triangulation_off_ostream.h>
#include <CGAL/point_generators_d.h>
#include <CGAL/Timer.h>
#include <CGAL/algorithm.h>
#include <CGAL/Memory_sizer.h>

#include <vector>
#include <string>
#include <fstream>
#include <iostream>
#include <cstdlib>
#include <iterator>
#include <algorithm>
#include <unistd.h>
#include <boost/algorithm/string.hpp>
#define OUTPUT_STATS

//function to read in data from csv file
std::vector<std::vector<double> > read_data(std::string file_name, int dim,
std::string delimeter = " ")
{
	std::ifstream file(file_name);

	std::vector<std::vector<double> > data_list;

	std::string line = "";
	// Iterate through each line split the content using delimeter
  // convert it to double
	while (getline(file, line))
	{
		std::vector<std::string> vec;
		boost::algorithm::split(vec, line, boost::is_any_of(delimeter));
        std::vector<double> dd;
        dd.reserve(dim);
        for(std::vector<std::string>::iterator it = vec.begin();
        it != vec.end(); ++it) {
          dd.push_back(std::stod(*it));
        }

		data_list.push_back(dd);

	}
	// Close the File
	file.close();

	return data_list;
}

void test(int dim, std::string file_name, std::string output_file)
{
    typedef CGAL::Epick_d<CGAL::Dynamic_dimension_tag> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point Point;
    typedef CGAL::Random_points_in_cube_d<Point> Random_points_iterator;
    CGAL::Timer timer;  // timer

    //TODO: Change the path_prefix before running the function!
    std::string path_prefix = "/Users/angelynaye/desktop/research/data/";
    std::string full_file_name = path_prefix + file_name;

    std::vector<Point> points;

    // CSVReader reader(full_file_name);
    std::cout <<"          Reading file: " << file_name << std::endl;
    // Get the data from CSV File
    std::vector<std::vector<double> > data_vec = read_data(full_file_name, dim);

    std::size_t N  = data_vec.size();

    std::cout << N <<std::endl;
    int i = 0;

    // construct points
    for(int i = 0; i < data_vec.size(); i++) {
      std::vector<double> cur = data_vec.at(i);
      double temp[cur.size()];
      std::copy(cur.begin(), cur.end(), temp);
      Point p(&temp[0], &temp[cur.size()]);
      points.push_back(p);
    }

    std::size_t mem_before = CGAL::Memory_sizer().virtual_size();
    timer.reset();
    timer.start();

    // Build the Regular Triangulation
    DT dt(dim);

    // std::istream_iterator<Point> begin (iFile), end;
    dt.insert(points.begin(), points.end());

    std::cout << "Delaunay triangulation of " << N <<
    " points in dim " << dim << ":" << std::endl;

    std::size_t mem = CGAL::Memory_sizer().virtual_size() - mem_before;
    double timing = timer.time();
    std::cout << "  Triangles Complete in " << timing << " seconds." << std::endl;
    std::cout << "  Memory consumption: " << (mem >> 10) << " KB.\n";
    std::size_t nbfc= dt.number_of_finite_full_cells();
    std::size_t nbc= dt.number_of_full_cells();
    std::cout << "There are " << dt.number_of_vertices() << " vertices, "
            << nbfc << " finite simplices and "
            << (nbc-nbfc) << " convex hull Facets.\n"
            << std::endl;

    #ifdef OUTPUT_STATS
    path_prefix += "stats/";
    std::string output_name = path_prefix + output_file;
    std::ofstream csv_file(output_name);
    csv_file
        << "Dimension: " << dim << "; "
        << "Numbers of Pts: " << N << "; "
        << "Completion Time: "<< timing << " seconds; "
        << "Used Memory: " << mem << " KB; "
        << "Numbers of Facets: "<< nbfc << "\n "
        << std::flush;
    #endif

}

int main()
{
  int dims[13] = {
    // 2, 8,
    2, 10, 25, 25, 25, 14, 22, 22, 24, 54, 54, 60, 7};
  std::string names[13] = {
      // "fourclass.csv",
      // "diabetes.csv",
      "halfmoon.csv",
      "cancer.csv",
      "mnist17_test.csv",
      "f-mnist06_test.csv",
      "f-mnist35.csv",
      "australian.csv",
      "svmguide3.csv",
      "ijcnn1.csv",
      "german.csv",
      "covtype_bi.csv",
      "cov_dat.csv",
      "splice.csv",
      "abalone.csv",
  };
  std::string output[13] = {
      // "fourclass_stat.csv",
      // "diabetes_stat.csv",
      "halfmoon_stat.csv",
      "breast_cancer_stat.csv",
      "mnist17_test_stat.csv",
      "f-mnist06_test_stat.csv",
      "f-mnist35_stat.csv",
      "australian_stat.csv",
      "svmguide3_stat.csv",
      "ijcnn1_stat.csv",
      "german_stat.csv",
      "covtype_bi_stat.csv",
      "cov_dat_stat.csv",
      "splice_stat.csv",
      "abalone_stat.csv"
  };

  for(int i = 0; i < sizeof(names)/sizeof(names[0]); i++) {
    test(dims[i], names[i], output[i]);
  }

  return 0;
}
#endif


#include <iostream>
#include <Eigen/Dense>
#include "../include/utils.h"

int main() {
  Eigen::Array<double, 1, -1> a(10);
  double b[] = {0., 1., 2., 3., 4., 5., 6., 7., 8., 9.};
  a << 0., 1., 2., 3., 4., 5., 6., 7., 8., 9.;
  std::cout << "Array []logsumexp(range(10)) = " << logsumexp(b, 10) << "\n";
  std::cout << "Eigen - logsumexp(range(10)) = " << logsumexp(a, 10) << "\n";
  Eigen::ArrayXd a_copy = a;
  normalize(a_copy);
  std::cout << "normalize(a) = \n" << a_copy << "\n";

  Eigen::ArrayXXd A(3, 3);
  A << 1, 2, 3,
       4, 5, 6,
       7, 8, 9;
  Eigen::ArrayXXd A_log = A;
  Eigen::ArrayXXd A_ = A;
  normalize(A_);
  std::cout << "normalize(A(3x3)) = \n";
  std::cout << A_ << "\n";
  log_normalize(A_log);
  std::cout << "log_normalize(A(3x3)) = \n";
  std::cout << A_log << "\n";

  double a_scaler = 2.0, b_scaler = 4.0;
  std::cout << "logaddexp(" << a_scaler <<
    " + " << b_scaler << ") = "
    << logaddexp(a_scaler, b_scaler) << "\n";

  Eigen::ArrayXd means(2);
  means << 0, 5;
  Eigen::ArrayXd covar = Eigen::ArrayXd::Ones(2);
  std::vector<double> X(b, b + 10);
  Eigen::ArrayXXd logprob(X.size(), 2);
  log_univariate_normal_density(X, means, covar, logprob);
  std::cout << "log_univariate_normal_density(X) = \n"
    << logprob << "\n";

  std::cout << "==================================Forward - backward Test\n";
  size_t n_observations = 10;
  size_t n_components = 2;
  Eigen::ArrayXd log_start(2);
  Eigen::ArrayXXd log_trans(2, 2);
  log_start << std::log(0.5), std::log(0.5);
  log_trans << std::log(0.5), std::log(0.5),
               std::log(0.5), std::log(0.5);
  Eigen::ArrayXXd alpha(10, 2);
  forward(n_observations, n_components, log_start,
          log_trans, logprob, alpha);
  std::cout << "forward -> alpha: \n"
            << alpha << "\n";
  Eigen::ArrayXXd beta(10, 2);
  backward(n_observations, n_components,
           log_trans, logprob, beta);
  std::cout << "backward -> beta: \n"
            << beta << "\n";

  std::cout << "================================Compute log xi\n";
  Eigen::ArrayXXd log_xi_sum(2, 2);
  log_xi_sum = -INFINITY * Eigen::ArrayXXd::Ones(2, 2);
  compute_log_xi_sum(n_observations, n_components, alpha,
                     log_trans, beta, logprob, log_xi_sum);
  std::cout << "Compute_log_xi_sum: \n"
    << log_xi_sum << "\n";

  return 0;
}