#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <vector>
#include <chrono>

#include <getopt.h>

#include "Alpert_Transform.hpp"
#include "Utils.hpp"

#include <boost/numeric/bindings/blas/blas.h>

using namespace std;

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

  if(argc == 1 ){
    cout << argv[0] << " usage is:" << endl;
    cout << argv[0] << " --reduction 'reduction_value' [mesh input_file] [data input_file]" << endl;
    cout << "If the data input file is provided it should have the same number of points as the mesh, if not a default data field will be assigned" << endl;
    cout << "Please refer to README.txt in this folder for more info" << endl;
    exit (0);
  }

    chrono::duration <double> duration;
    chrono::system_clock::time_point t1, t2;

    int d, N;
    double R=10.0e0;
    int c;

    {
          static struct option long_options[] =
        {
          /* These options set a flag. */
          /* These options don’t set a flag.
             We distinguish them by their indices. */
          {"reduction",  required_argument, 0, 'r'},
          {0, 0, 0}
        };
      /* getopt_long stores the option index here. */
      int option_index = 0;

      while (c = getopt_long (argc, argv, "r",
			      long_options, &option_index) != -1){

	if(option_index == 0 && optarg){
	  R  = atof(optarg);
	  cout << "Reduction set to " << R << endl;
	}
      }

    }

    if(optind >= argc ) {
      cout << "Expected an input file name " << endl;
      exit(0);
    }

    cout << "Reading mesh input file" << argv[optind] << endl;
    ifstream f(argv[optind]);
    f >> N >> d;

    cout << "Number of Points " << N << endl;
    cout << "Number of dimensions " << d << endl;
    cout << "R " << R << endl;

    int M=ceil(double(N)/R);
    int NR=N-M;
   // meshpoints
    vector<vector<double> >meshPoints(N, vector<double>(d));

    // Read mesh into array
    for (int i = 0; i < N; i++)
        for (int j = 0; j < d; j++)
            f >> meshPoints[i][j];

    f.close();

    // Setting Wavelet orders
    vector<int> ki(d);
    for (int j = 0; j < d; j++)
      ki[j] = 5;

    boost::numeric::ublas::vector<double> x(N);
    boost::numeric::ublas::vector<double> w(N);
    boost::numeric::ublas::vector<double> xw(N);
    boost::numeric::ublas::vector<double> ys(N);

    // Setting data, this step can be replaced by some commands to read an external data file
    double pi=4.0e0*atan(1.0e0);
    if (argc>4) {
      cout << "Reading data input file" << argv[optind+1] << endl;
      f.open(argv[optind+1]);
      for (int i = 0; i < N; i++)
	f >> x(i);
    }
    else
      for (int i = 0; i < N; i++) x(i)=(4.0e0*sin(8.0e0*pi*meshPoints[i][0]))*(4.0e0*sin(7.0e0*pi*meshPoints[i][1]) )*3.0e0*sin(6.0e0*pi*meshPoints[i][0]);
//     for (int i = 0; i < N; i++) x[i]=(4.0*sin(2.0*pi*p[i][0]) - 4.0*sin(2.0*pi*p[i][1]))*3.0*sin(2.0*pi*p[i][0]);

    int J=-1;
    std::list<std::list<boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major> > > Uj;
    std::list<std::vector<int> > part;
    std::vector<int> G;
    //void Compute_Uj(std::vector<std::vector<double> >meshPoints, std::vector<int> ki, int &J, std::list<std::list<boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major> > > &Uj, std::list<std::vector<int> > &part, std::vector<int> &G)

    // Forward wavelet transform
    t1 = chrono::system_clock::now();
    Compute_Uj(meshPoints,ki,J,Uj, part, G);
    Perform_Alpert_transform(x, ki, J,1, w, Uj, part, G);

    // Sorting, wavelet compression and inverse wavelet transform
    vector<double> yv(N);
    vector<size_t> iyv(N);
    for (int i = 0; i < N; i++) {yv[i]=fabs(w(i));iyv[i]=i;}
    sortandindices(yv, iyv);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total wavelet compression time " << duration.count() << endl;

//     for (int i = 0; i < N; i++)
//       std::cout << w(i) << std::endl;

    t1 = chrono::system_clock::now();
    for (int i = NR; i < N; i++) ys(iyv[i])=w(iyv[i]);

    // xw is the reconstruction by wavelets
    Perform_Alpert_transform(ys,  ki, J,-1, xw, Uj, part, G);

    // Computing NRMSE
    double ew=0.0e0;
    for (int i = 0; i < N; i++)
      ew+=pow(x(i)-xw(i),2.0e0);

    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total wavelet decompression time " << duration.count() << endl;

    // Computing min and max values in the data x
    double mx=x(0);
    double mn=x(0);
    for (int i = 0; i < N; i++) {
      if (x(i)>mx)
	mx=x(i);
      if (x(i)<mn)
	mn=x(i);
    }

    ew=sqrt(ew/double(N))/(mx-mn);

    cout << "Alpert Wavelets Compression NRMSE = " << ew << endl;
}


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

constexpr bool DO_PROJECTION = true;

Bond::Bond(double force_constant, double equilibrium_distance, int first_atom, int second_atom,
           Eigen::VectorXd rail)
    : force_constant(force_constant)
    , equilibrium_distance(equilibrium_distance)
    , atoms { first_atom, second_atom }
    , rail {rail.normalized()} {}

Eigen::VectorXd Bond::project_onto_rail(const Eigen::VectorXd& input) const {
    //! Project a given vector onto this bond rail
    /*!
     *! \param input the vector to do the projection of
     *! \return projected the component of the vector in the direction of the rail
     */
    if constexpr (DO_PROJECTION) {
        return input.dot(rail) * rail;
    }
    return input;
}

double Bond::get_excitement_factor(const Eigen::MatrixXd& positions) const {
    Eigen::VectorXd separation = positions.row(atoms[0]) - positions.row(atoms[1]);
    separation = project_onto_rail(separation);
    double distance = separation.dot(rail);
    return distance / equilibrium_distance;

}

HarmonicBond::HarmonicBond(double force_constant, double equilibrium_distance, int first_atom,
                           int second_atom, Eigen::VectorXd rail)
    : Bond(force_constant, equilibrium_distance, first_atom, second_atom, rail) {}

Eigen::VectorXd HarmonicBond::force(const Eigen::MatrixXd& positions) const {
    const Eigen::VectorXd separation = positions.row(atoms[0]) - positions.row(atoms[1]);
    auto magnitude = separation.norm();
    const Eigen::VectorXd direction = separation / magnitude;
    double extension = magnitude - equilibrium_distance;

    const Eigen::VectorXd force_direction = force_constant * extension * direction;

    return project_onto_rail(force_direction);
}

double HarmonicBond::frequency(const Eigen::VectorXd& masses) const {
    const auto reduced_mass = masses[atoms[0]] * masses[atoms[1]] / (masses[atoms[0]] + masses[atoms[1]]);
    return std::sqrt(force_constant / reduced_mass);
}

double HarmonicBond::period(const Eigen::VectorXd& masses) const {
    return 2 * M_PI / frequency(masses);
}

double HarmonicBond::energy(const Eigen::MatrixXd& positions) const {
    const auto stretch_ratio = get_excitement_factor(positions) - 1.0;
    return  stretch_ratio * stretch_ratio * force_constant * equilibrium_distance * equilibrium_distance / 2.0;
}


#pragma once

#include <cmath>
#include <limits>
#include <Eigen/Core>

namespace kt84 {

// the definition of functions alpha(r) and beta(r) implemented in each kernel |
//-----------------------------------------------------------------------------+
/*

inner product of vectors x and y:
    x * y := x^T y (scalar)
outer product of vectors x and y:
    x % y := x y^T (matrix)

notations:
    x := (x_1, ..., x_N)
    r := |x| = sqrt(x * x)

univariate RBF kernel:
    phi(r): R+ --> R;

first & second derivatives of phi:
    phi'(r) : R+ --> R
    phi"(r) : R+ --> R

examples of phi:
    phi(r) := |r|^3                             // cubic
    phi(r) := exp(-r^2/(2 * sigma^2))           // Gaussian

multivariate RBF kernel:
    f(x): R^N --> R
    f(x) := phi(r)
          = phi(|x|)

gradient:
    g(x): R^N --> R^N
    g(x) := df/dx(x)
          = phi'(r) / r * x

definition of alpha:
    alpha(r) := phi'(r) / r
    g(x) = alpha(r) * x

Hessian:
    H(x): R^N --> R^(NxN)
    H(x) := d^2f/dx^2
          = dg/dx
          = {(r * phi"(r) - phi'(r)) / r^3} * (x % x) + {phi'(r) / r} * I

definition of beta:
    beta(r) := (r * phi"(r) - phi'(r)) / r^3
    H(x) = alpha(r) * I + beta(r) * (x % x);

*/

// Gradient is always defined even for singular (r=0) cases --> can be used with Hermite RBF |
//-------------------------------------------------------------------------------------------+
struct RBFKernel_Gaussian {
    struct Param {
        double sigma;
        Param(double sigma = 1) : sigma(sigma) {}
    } param;
    double operator()       (double r) const { return std::exp(-r * r / (2 * param.sigma * param.sigma)); }
    double derivative_first (double r) const { return operator()(r) * r / (-param.sigma * param.sigma); }
    double derivative_second(double r) const { return operator()(r) * (r * r - param.sigma * param.sigma) / (param.sigma * param.sigma * param.sigma * param.sigma); }
    double alpha_singular() const { return -1 / (param.sigma * param.sigma); }
    static const bool AlphaAlwaysDefined = true;
    static const bool Decaying = true;
};

struct RBFKernel_SquaredInverse {
    struct Param {
        double epsilon;
        Param(double epsilon = 0) : epsilon(epsilon) {}
    } param;
    double operator()(double r) const {
        if (r == 0 && param.epsilon == 0)
            return std::numeric_limits<double>::infinity();         // indication of hard constraint (for MLS)
        return 1 / (r * r + param.epsilon * param.epsilon);
    }
    double derivative_first(double r) const {
        double t = operator()(r);
        return -2 * r * t * t;
    }
    double derivative_second(double r) const {
        double t = operator()(r);
        return (6 * r * r - 2 * param.epsilon * param.epsilon) * t * t * t;
    }
    double alpha_singular() const {
        return -2 / (param.epsilon * param.epsilon * param.epsilon * param.epsilon);
    }
    static const bool AlphaAlwaysDefined = true;
    static const bool Decaying = true;
};

struct RBFKernel_Wendland {
    // Wendland, H.
    // Piecewise polynomial, positive definite and compactly supported radial basis functions of minimal degree.
    // Advances in Computational Mathematics 4, 389--396, 1995.
    struct Param {
        double support;
        Param(double support = 1) : support(support) {}
    } param;
    double operator()(double r) const {
        double t = r / param.support;
        if (t > 1)
            return 0;
        return (1 - t) * (1 - t) * (1 - t) * (1 - t) * (4 * t + 1);
    }
    double derivative_first(double r) const {
        double t = r / param.support;
        if (t > 1)
            return 0;
        return -20 * t * (1 - t) * (1 - t) * (1 - t) / param.support;
    }
    double derivative_second(double r) const {
        double t = r / param.support;
        if (t > 1)
            return 0;
        return 20 * (1 - t) * (1 - t) * (4 * t - 1) / (param.support * param.support);
    }
    double alpha_singular() const { return -20 / (param.support * param.support); }
    static const bool AlphaAlwaysDefined = true;
    static const bool Decaying = true;
};

struct RBFKernel_Cubed {
    struct Param {} param;
    double operator()       (double r) const { return r * r * r; }
    double derivative_first (double r) const { return 3 * r * r; }
    double derivative_second(double r) const { return 6 * r; }
    double alpha_singular() const { return 0; }
    static const bool AlphaAlwaysDefined = true;
    static const bool Decaying = false;
};

// gradient is undefined for singular (r=0) cases --> cannot be used with HermiteRBF |
//-----------------------------------------------------------------------------------+
struct RBFKernel_Identity {
    struct Param {} param;
    double operator()       (double r) const { return r; }
    double derivative_first (double r) const { return 1; }
    double derivative_second(double r) const { return 0; }
    double alpha_singular() const { throw std::logic_error("undefined!"); }
    static const bool AlphaAlwaysDefined = false;
    static const bool Decaying = false;
};

struct RBFKernel_SquaredLog {
    struct Param {} param;
    double operator()(double r) const {
        if (r == 0)
            return 0;
        return r * r * std::log(r);
    }
    double derivative_first(double r) const {
        if (r == 0)
            return 0;
        return 2 * r * std::log(r) + r;
    }
    double derivative_second(double r) const {
        if (r == 0)
            return -std::numeric_limits<double>::infinity();        // not sure if this treatment is correct...
        return 2 * std::log(r) + 3;
    }
    double alpha_singular() const { throw std::logic_error("undefined!"); }
    static const bool AlphaAlwaysDefined = false;
    static const bool Decaying = false;
};

// general template classes |
//--------------------------+

template <class _RBFKernel_Core>
struct RBFKernel_Univariate {
    typedef _RBFKernel_Core Core;

    Core core;

    double operator()(double r) const { return core(r); }
    double derivative_first(double r) const { return core.derivative_first(r); }
    double derivative_second(double r) const { return core.derivative_second(r); }
    double alpha(double r) const { return core.derivative_first(r) / r; }
    double beta(double r) const { return (r * core.derivative_second(r) - core.derivative_first(r)) / (r * r * r); }
};

template <int _Dim, class _RBFKernel_Core>
struct RBFKernel_Bivariate {
    enum { Dim  = _Dim };

    typedef Eigen::Matrix<double, Dim, 1> Point;
    typedef Eigen::Matrix<double, 1, Dim> Gradient;
    typedef Eigen::Matrix<double, Dim, Dim> Hessian;
    typedef RBFKernel_Univariate<_RBFKernel_Core> Univariate;

    static const bool GradientAlwaysDefined = Univariate::Core::AlphaAlwaysDefined;
    static const bool Decaying              = Univariate::Core::Decaying;

    Univariate univariate;

    double operator()(const Point& p0, const Point& p1) const { return univariate((p1 - p0).norm()); }
    Gradient gradient(const Point& p_variable, const Point& p_fixed) const {
        Point p_diff = p_variable - p_fixed;
        double r = p_diff.norm();
        if (r == 0)
            return Gradient::Zero();
        return univariate.alpha(r) * p_diff.transpose();
    }
    Hessian hessian(const Point& p_variable, const Point& p_fixed) const {
        Point p_diff = p_variable - p_fixed;
        double r = p_diff.norm();
        if (r == 0)
            return univariate.core.alpha_singular() * Hessian::Identity();
        return univariate.alpha(r) * Hessian::Identity() + univariate.beta(r) * p_diff * p_diff.transpose();
    }

    // easy access to kernel parameters
    typename Univariate::Core::Param& param() { return univariate.core.param; }
    const typename Univariate::Core::Param& param() const { return univariate.core.param; }
};

}


#include "mtf/SSM//AffineEstimator.h"
#include "mtf/Utilities/warpUtils.h"
#include "mtf/Utilities/miscUtils.h"
#include "opencv2/core/core_c.h"
#include "opencv2/calib3d/calib3d.hpp"
#include <boost/random/uniform_int_distribution.hpp>

_MTF_BEGIN_NAMESPACE

AffineEstimator::AffineEstimator(int _modelPoints, bool _use_boost_rng)
	: SSMEstimator(_modelPoints, cvSize(3, 2), 1, _use_boost_rng) {
	assert(_modelPoints >= 3);
	checkPartialSubsets = false;
}

int AffineEstimator::runKernel(const CvMat* m1, const CvMat* m2, CvMat* H) {
	int n_pts = m1->rows * m1->cols;

	//if(n_pts != 3) {
	//    throw invalid_argument(cv::format("Invalid no. of points: %d provided", n_pts));
	//}
	const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
	const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;

	Matrix2Xd in_pts, out_pts;
	in_pts.resize(Eigen::NoChange, n_pts);
	out_pts.resize(Eigen::NoChange, n_pts);
	for(int pt_id = 0; pt_id < n_pts; pt_id++) {
		in_pts(0, pt_id) = M[pt_id].x;
		in_pts(1, pt_id) = M[pt_id].y;

		out_pts(0, pt_id) = m[pt_id].x;
		out_pts(1, pt_id) = m[pt_id].y;
	}
	Matrix3d affine_mat = utils::computeAffineDLT(in_pts, out_pts);

	double *H_ptr = H->data.db;
	H_ptr[0] = affine_mat(0, 0);
	H_ptr[1] = affine_mat(0, 1);
	H_ptr[2] = affine_mat(0, 2);
	H_ptr[3] = affine_mat(1, 0);
	H_ptr[4] = affine_mat(1, 1);
	H_ptr[5] = affine_mat(1, 2);
	return 1;
}

void AffineEstimator::computeReprojError(const CvMat* m1, const CvMat* m2,
	const CvMat* model, CvMat* _err) {
	int n_pts = m1->rows * m1->cols;
	const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
	const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
	const double* H = model->data.db;
	float* err = _err->data.fl;

	for(int pt_id = 0; pt_id < n_pts; pt_id++) {
		double dx = (H[0] * M[pt_id].x + H[1] * M[pt_id].y + H[2]) - m[pt_id].x;
		double dy = (H[3] * M[pt_id].x + H[4] * M[pt_id].y + H[5]) - m[pt_id].y;
		err[pt_id] = (float)(dx * dx + dy * dy);
	}
}

bool AffineEstimator::refine(const CvMat* m1, const CvMat* m2,
	CvMat* model, int maxIters) {
	LevMarq solver(6, 0, cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));
	int n_pts = m1->rows * m1->cols;
	const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
	const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
	CvMat modelPart = cvMat(solver.param->rows, solver.param->cols, model->type, model->data.ptr);
	cvCopy(&modelPart, solver.param);

	for(;;)	{
		const CvMat* _param = 0;
		CvMat *_JtJ = 0, *_JtErr = 0;
		double* _errNorm = 0;

		if(!solver.updateAlt(_param, _JtJ, _JtErr, _errNorm))
			break;

		for(int pt_id = 0; pt_id < n_pts; pt_id++)	{
			const double* h = _param->data.db;
			double Mx = M[pt_id].x, My = M[pt_id].y;
			double _xi = (h[0] * Mx + h[1] * My + h[2]);
			double _yi = (h[3] * Mx + h[4] * My + h[5]);
			double err[] = { _xi - m[pt_id].x, _yi - m[pt_id].y };
			if(_JtJ || _JtErr) {
				double J[][6] = {
					{ Mx, My, 1, 0, 0, 0 },
					{ 0, 0, 0, Mx, My, 1 }
				};
				for(int j = 0; j < 6; j++) {
					for(int k = j; k < 6; k++)
						_JtJ->data.db[j * 6 + k] += J[0][j] * J[0][k] + J[1][j] * J[1][k];
					_JtErr->data.db[j] += J[0][j] * err[0] + J[1][j] * err[1];
				}
			}
			if(_errNorm)
				*_errNorm += err[0] * err[0] + err[1] * err[1];
		}
	}

	cvCopy(solver.param, &modelPart);
	return true;
}

cv::Mat estimateAffine(cv::InputArray _points1, cv::InputArray _points2,
	cv::OutputArray _mask, const SSMEstimatorParams &params){
	cv::Mat points1 = _points1.getMat(), points2 = _points2.getMat();
	int npoints = points1.checkVector(2);
	CV_Assert(npoints >= 0 && points2.checkVector(2) == npoints &&
		points1.type() == points2.type());

	cv::Mat H(2, 3, CV_64F);
	CvMat _pt1 = points1, _pt2 = points2;
	CvMat matH = H, c_mask, *p_mask = 0;
	if(_mask.needed()){
		_mask.create(npoints, 1, CV_8U, -1, true);
		p_mask = &(c_mask = _mask.getMat());
	}
	bool ok = estimateAffine(&_pt1, &_pt2, &matH, p_mask, params) > 0;
	if(!ok)
		H = cv::Scalar(0);
	return H;
}

int	estimateAffine(const CvMat* in_pts, const CvMat* out_pts,
	CvMat* __H, CvMat* mask, const SSMEstimatorParams &params) {
	bool result = false;
	cv::Ptr<CvMat> out_pts_hm, in_pts_hm, tempMask;

	double H[6];
	CvMat matH = cvMat(2, 3, CV_64FC1, H);

	CV_Assert(CV_IS_MAT(out_pts) && CV_IS_MAT(in_pts));

	int n_pts = MAX(out_pts->cols, out_pts->rows);
	CV_Assert(n_pts >= params.n_model_pts);

	out_pts_hm = cvCreateMat(1, n_pts, CV_64FC2);
	cvConvertPointsHomogeneous(out_pts, out_pts_hm);

	in_pts_hm = cvCreateMat(1, n_pts, CV_64FC2);
	cvConvertPointsHomogeneous(in_pts, in_pts_hm);

	if(mask) {
		CV_Assert(CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
			(mask->rows == 1 || mask->cols == 1) &&
			mask->rows * mask->cols == n_pts);
	}
	if(mask || n_pts > params.n_model_pts)
		tempMask = cvCreateMat(1, n_pts, CV_8U);
	if(!tempMask.empty())
		cvSet(tempMask, cvScalarAll(1.));

	AffineEstimator estimator(params.n_model_pts, params.use_boost_rng);

	int method = n_pts == params.n_model_pts ? 0 : params.method_cv;
	if(method == CV_LMEDS)
		result = estimator.runLMeDS(in_pts_hm, out_pts_hm, &matH, tempMask, params.confidence,
		params.max_iters, params.max_subset_attempts);
	else if(method == CV_RANSAC)
		result = estimator.runRANSAC(in_pts_hm, out_pts_hm, &matH, tempMask, params.ransac_reproj_thresh,
		params.confidence, params.max_iters, params.max_subset_attempts);
	else
		result = estimator.runKernel(in_pts_hm, out_pts_hm, &matH) > 0;

	if(result && n_pts > params.n_model_pts) {
		utils::icvCompressPoints((CvPoint2D64f*)in_pts_hm->data.ptr, tempMask->data.ptr, 1, n_pts);
		n_pts = utils::icvCompressPoints((CvPoint2D64f*)out_pts_hm->data.ptr, tempMask->data.ptr, 1, n_pts);
		in_pts_hm->cols = out_pts_hm->cols = n_pts;
		if(method == CV_RANSAC)
			estimator.runKernel(in_pts_hm, out_pts_hm, &matH);
		if(params.refine){
			estimator.refine(in_pts_hm, out_pts_hm, &matH, params.lm_max_iters);
		}
	}

	if(result)
		cvConvert(&matH, __H);

	if(mask && tempMask) {
		if(CV_ARE_SIZES_EQ(mask, tempMask))
			cvCopy(tempMask, mask);
		else
			cvTranspose(tempMask, mask);
	}

	return (int)result;
}

//AffineEstimator::AffineEstimator(int _modelPoints)
//	: SSMEstimator(_modelPoints, cvSize(3, 3), 1)
//{
//	assert(_modelPoints == 4 || _modelPoints == 5);
//	checkPartialSubsets = false;
//}

//int AffineEstimator::runKernel(const CvMat* m1, const CvMat* m2, CvMat* H)
//{
//	int i, count = m1->rows*m1->cols;
//	const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
//	const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;

//	double LtL[9][9], W[9][1], V[9][9];
//	CvMat _LtL = cvMat(9, 9, CV_64F, LtL);
//	CvMat matW = cvMat(9, 1, CV_64F, W);
//	CvMat matV = cvMat(9, 9, CV_64F, V);
//	CvMat _H0 = cvMat(3, 3, CV_64F, V[8]);
//	CvMat _Htemp = cvMat(3, 3, CV_64F, V[7]);
//	CvPoint2D64f cM = { 0, 0 }, cm = { 0, 0 }, sM = { 0, 0 }, sm = { 0, 0 };

//	for(i = 0; i < count; i++)
//	{
//		cm.x += m[i].x; cm.y += m[i].y;
//		cM.x += M[i].x; cM.y += M[i].y;
//	}

//	cm.x /= count; cm.y /= count;
//	cM.x /= count; cM.y /= count;

//	for(i = 0; i < count; i++)
//	{
//		sm.x += fabs(m[i].x - cm.x);
//		sm.y += fabs(m[i].y - cm.y);
//		sM.x += fabs(M[i].x - cM.x);
//		sM.y += fabs(M[i].y - cM.y);
//	}

//	if(fabs(sm.x) < DBL_EPSILON || fabs(sm.y) < DBL_EPSILON ||
//		fabs(sM.x) < DBL_EPSILON || fabs(sM.y) < DBL_EPSILON)
//		return 0;
//	sm.x = count / sm.x; sm.y = count / sm.y;
//	sM.x = count / sM.x; sM.y = count / sM.y;

//	double invHnorm[9] = { 1. / sm.x, 0, cm.x, 0, 1. / sm.y, cm.y, 0, 0, 1 };
//	double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };
//	CvMat _invHnorm = cvMat(3, 3, CV_64FC1, invHnorm);
//	CvMat _Hnorm2 = cvMat(3, 3, CV_64FC1, Hnorm2);

//	cvZero(&_LtL);
//	for(i = 0; i < count; i++)
//	{
//		double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;
//		double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;
//		double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };
//		double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };
//		int j, k;
//		for(j = 0; j < 9; j++)
//			for(k = j; k < 9; k++)
//				LtL[j][k] += Lx[j] * Lx[k] + Ly[j] * Ly[k];
//	}
//	cvCompleteSymm(&_LtL);

//	//cvSVD( &_LtL, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
//	cvEigenVV(&_LtL, &matV, &matW);
//	cvMatMul(&_invHnorm, &_H0, &_Htemp);
//	cvMatMul(&_Htemp, &_Hnorm2, &_H0);
//	cvConvertScale(&_H0, H, 1. / _H0.data.db[8]);

//	return 1;
//}

//void AffineEstimator::computeReprojError(const CvMat* m1, const CvMat* m2,
//	const CvMat* model, CvMat* _err)
//{
//	int i, count = m1->rows*m1->cols;
//	const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
//	const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
//	const double* H = model->data.db;
//	float* err = _err->data.fl;

//	for(i = 0; i < count; i++)
//	{
//		double ww = 1. / (H[6] * M[i].x + H[7] * M[i].y + 1.);
//		double dx = (H[0] * M[i].x + H[1] * M[i].y + H[2])*ww - m[i].x;
//		double dy = (H[3] * M[i].x + H[4] * M[i].y + H[5])*ww - m[i].y;
//		err[i] = (float)(dx*dx + dy*dy);
//	}
//}

//bool AffineEstimator::refine(const CvMat* m1, const CvMat* m2, CvMat* model, int maxIters)
//{
//	CvLevMarq solver(8, 0, cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, maxIters, DBL_EPSILON));
//	int i, j, k, count = m1->rows*m1->cols;
//	const CvPoint2D64f* M = (const CvPoint2D64f*)m1->data.ptr;
//	const CvPoint2D64f* m = (const CvPoint2D64f*)m2->data.ptr;
//	CvMat modelPart = cvMat(solver.param->rows, solver.param->cols, model->type, model->data.ptr);
//	cvCopy(&modelPart, solver.param);

//	for(;;)
//	{
//		const CvMat* _param = 0;
//		CvMat *_JtJ = 0, *_JtErr = 0;
//		double* _errNorm = 0;

//		if(!solver.updateAlt(_param, _JtJ, _JtErr, _errNorm))
//			break;

//		for(i = 0; i < count; i++)
//		{
//			const double* h = _param->data.db;
//			double Mx = M[i].x, My = M[i].y;
//			double ww = h[6] * Mx + h[7] * My + 1.;
//			ww = fabs(ww) > DBL_EPSILON ? 1. / ww : 0;
//			double _xi = (h[0] * Mx + h[1] * My + h[2])*ww;
//			double _yi = (h[3] * Mx + h[4] * My + h[5])*ww;
//			double err[] = { _xi - m[i].x, _yi - m[i].y };
//			if(_JtJ || _JtErr)
//			{
//				double J[][8] =
//				{
//					{ Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },
//					{ 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }
//				};

//				for(j = 0; j < 8; j++)
//				{
//					for(k = j; k < 8; k++)
//						_JtJ->data.db[j * 8 + k] += J[0][j] * J[0][k] + J[1][j] * J[1][k];
//					_JtErr->data.db[j] += J[0][j] * err[0] + J[1][j] * err[1];
//				}
//			}
//			if(_errNorm)
//				*_errNorm += err[0] * err[0] + err[1] * err[1];
//		}
//	}

//	cvCopy(solver.param, &modelPart);
//	return true;
//}

_MTF_END_NAMESPACE


/** @file
 * @brief NPDE OutputImpedanceBVP
 * @author Erick Schulz
 * @date 12/07/2019
 * @copyright Developed at ETH Zurich
 */

#include "outputimpedancebvp.h"

#include <lf/assemble/assemble.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cassert>

namespace OutputImpedanceBVP {

/* SAM_LISTING_BEGIN_1 */
Eigen::VectorXd solveImpedanceBVP(
    const std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> &fe_space_p,
    Eigen::Vector2d g) {
  // Related implementations:
  // Homework problem ErrorEstimatesForTraces:
  // https://gitlab.math.ethz.ch/ralfh/npdecodes/tree/master/homeworks/ErrorEstimatesForTraces

  // Pointer to current mesh
  std::shared_ptr<const lf::mesh::Mesh> mesh_p = fe_space_p->Mesh();
  // Obtain local->global index mapping for current finite element space
  const lf::assemble::DofHandler &dofh{fe_space_p->LocGlobMap()};
  // Dimension of finite element space
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());
  // Obtain specification for shape functions on edges
  std::shared_ptr<const lf::uscalfe::ScalarReferenceFiniteElement<double>>
      rsf_edge_p = fe_space_p->ShapeFunctionLayout(lf::base::RefEl::kSegment());

  Eigen::VectorXd discrete_solution(N_dofs);

  // I : ASSEMBLY
  // Matrix in triplet format holding Galerkin matrix, zero initially.
  lf::assemble::COOMatrix<double> A(N_dofs, N_dofs);
  // Right hand side vector, must be initialized with 0!
  Eigen::Matrix<double, Eigen::Dynamic, 1> phi(N_dofs);
  phi.setZero();

  // I.i : Computing volume matrix for negative Laplace operator
  //====================
  // Your code goes here
  //====================
  /* SAM_LISTING_END_1 */

  /* SAM_LISTING_BEGIN_2 */
  // I.ii : Computing mass edge matrix resulting from Robin B.C.
  // Obtain an array of boolean flags for the edges of the mesh, 'true'
  // indicates that the edge lies on the boundary
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 1)};
  //====================
  // Your code goes here
  //====================
  /* SAM_LISTING_END_2 */

  /* SAM_LISTING_BEGIN_9 */
  // I.iii : Computing right-hand side vector
  // Right-hand side source function f
  auto mf_f = lf::mesh::utils::MeshFunctionGlobal(
      [](Eigen::Vector2d x) -> double { return 0.0; });
  lf::uscalfe::ScalarLoadElementVectorProvider<double, decltype(mf_f)>
      elvec_builder(fe_space_p, mf_f);
  // Invoke assembly on cells (codim == 0)
  AssembleVectorLocally(0, dofh, elvec_builder, phi);

  // I.iv : Imposing essential boundary conditions
  // Dirichlet data
  auto mf_g = lf::mesh::utils::MeshFunctionGlobal(
      [&g](Eigen::Vector2d x) -> double { return g.dot(x); });
  //====================
  // Your code goes here
  //====================

  // Assembly completed! Convert COO matrix A into CRS format using Eigen's
  // internal conversion routines.
  Eigen::SparseMatrix<double> A_sparse = A.makeSparse();

// II : SOLVING  THE LINEAR SYSTEM
//====================
// Your code goes here
//====================

  discrete_solution.setZero();
  return discrete_solution;
};
/* SAM_LISTING_END_9 */

/* SAM_LISTING_BEGIN_3 */
double computeBoundaryOutputFunctional(
    const Eigen::VectorXd eta,
    const std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> &fe_space_p,
    Eigen::Vector2d d) {
  double func_val = 0.0;
  // Pointer to current mesh
  std::shared_ptr<const lf::mesh::Mesh> mesh_p = fe_space_p->Mesh();
  // Obtain local->global index mapping for current finite element space
  const lf::assemble::DofHandler &dofh{fe_space_p->LocGlobMap()};

  // Obtain an array of boolean flags for the edges of the mesh, 'true'
  // indicates that the edge lies on the boundary
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 1)};

  //====================
  // Your code goes here
  //====================

  // Computing value of the functional
  for (const lf::mesh::Entity *edge : mesh_p->Entities(1)) {
    //====================
    // Your code goes here
    //====================
  }
  return func_val;
};
/* SAM_LISTING_END_3 */

}  // namespace OutputImpedanceBVP


//
//  Copyright Markus Rickert 2008
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <algorithm>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/bindings/blas/blas.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_vector2.hpp>

int
main(int argc, char** argv)
{
	// a * b' = C ; a' * b = d
	{
		boost::numeric::ublas::vector<double> a(3);
		for (std::size_t i = 0; i < a.size(); ++i) a(i) = i;
		std::cout << "a=" << a << std::endl;

		boost::numeric::ublas::vector<double> b(3);
		for (std::size_t i = 0; i < b.size(); ++i) b(i) = i;
		std::cout << "b=" << b << std::endl;

		boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			boost::numeric::bindings::traits::TRANSPOSE,
			1.0, a, b, 0.0, c
		);
		std::cout << "C=" << c << std::endl;

		boost::numeric::ublas::vector<double> d(1);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::TRANSPOSE,
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			1.0, a, b, 0.0, d
		);
		std::cout << "d=" << d << std::endl;
	}

	std::cout << std::endl;

	// a * b' = C ; a' * b = d
	{
		boost::numeric::ublas::bounded_vector<double, 3> a;
		for (std::size_t i = 0; i < a.size(); ++i) a(i) = i;
		std::cout << "a=" << a << std::endl;

		boost::numeric::ublas::bounded_vector<double, 3> b;
		for (std::size_t i = 0; i < b.size(); ++i) b(i) = i;
		std::cout << "b=" << b << std::endl;

		boost::numeric::ublas::bounded_matrix<double, 3, 3, boost::numeric::ublas::column_major> c;
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			boost::numeric::bindings::traits::TRANSPOSE,
			1.0, a, b, 0.0, c
		);
		std::cout << "C=" << c << std::endl;

		boost::numeric::ublas::bounded_vector<double, 1> d;
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::TRANSPOSE,
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			1.0, a, b, 0.0, d
		);
		std::cout << "d=" << d << std::endl;
	}

	std::cout << std::endl;

	// A * B = C
	{
		boost::numeric::ublas::bounded_matrix<double, 4, 3, boost::numeric::ublas::column_major> a;
		for (std::size_t i = 0; i < a.size1(); ++i) for (std::size_t j = 0; j < a.size2(); ++j) a(i, j) = i * a.size2() + j;
		std::cout << "A=" << a << std::endl;

		boost::numeric::ublas::bounded_matrix<double, 3, 4, boost::numeric::ublas::column_major> b;
		for (std::size_t i = 0; i < b.size1(); ++i) for (std::size_t j = 0; j < b.size2(); ++j) b(i, j) = i * b.size2() + j;
		std::cout << "B=" << b << std::endl;

		boost::numeric::ublas::bounded_matrix<double, 4, 4, boost::numeric::ublas::column_major> c;
		boost::numeric::bindings::blas::gemm(a, b, c);
		std::cout << "C=" << c << std::endl;
	}

	std::cout << std::endl;

	// A[0:3;0:2] * B[0:2;0:3] = C
	{
		boost::numeric::ublas::bounded_matrix<double, 4, 3, boost::numeric::ublas::column_major> a;
		for (std::size_t i = 0; i < a.size1(); ++i) for (std::size_t j = 0; j < a.size2(); ++j) a(i, j) = i * a.size2() + j;
		std::cout << "A=" << a << std::endl;

		boost::numeric::ublas::matrix_range<
			boost::numeric::ublas::bounded_matrix<double, 4, 3, boost::numeric::ublas::column_major>
		> a2 = boost::numeric::ublas::subrange(a, 0, 3, 0, 2);
		std::cout << "A2=" << a2 << std::endl;

		boost::numeric::ublas::bounded_matrix<double, 3, 4, boost::numeric::ublas::column_major> b;
		for (std::size_t i = 0; i < b.size1(); ++i) for (std::size_t j = 0; j < b.size2(); ++j) b(i, j) = i * b.size2() + j;
		std::cout << "B=" << b << std::endl;

		boost::numeric::ublas::matrix_range<
			boost::numeric::ublas::bounded_matrix<double, 3, 4, boost::numeric::ublas::column_major>
		> b2 = boost::numeric::ublas::subrange(b, 0, 2, 0, 3);
		std::cout << "B2=" << b2 << std::endl;

		boost::numeric::ublas::bounded_matrix<double, 4, 4, boost::numeric::ublas::column_major> c;
		std::fill(c.data().begin(), c.data().end(), 0.0);
		boost::numeric::ublas::matrix_range<
			boost::numeric::ublas::bounded_matrix<double, 4, 4, boost::numeric::ublas::column_major>
		> c2 = boost::numeric::ublas::subrange(c, 0, 3, 0, 3);
		boost::numeric::bindings::blas::gemm(a2, b2, c2);
		std::cout << "C2=" << c2 << std::endl;
		std::cout << "C=" << c << std::endl;
	}

	std::cout << std::endl;

	// a + b = b ; b - a = b
	{
		boost::numeric::ublas::bounded_vector<double, 3> a;
		for (std::size_t i = 0; i < a.size(); ++i) a(i) = i;
		std::cout << "a=" << a << std::endl;

		boost::numeric::ublas::bounded_vector<double, 3> b;
		for (std::size_t i = 0; i < b.size(); ++i) b(i) = i;
		std::cout << "b=" << b << std::endl;

		boost::numeric::bindings::blas::axpy(1.0, a, b);
		std::cout << "b=" << b << std::endl;

		boost::numeric::bindings::blas::axpy(-1.0, a, b);
		std::cout << "b=" << b << std::endl;
	}

	std::cout << std::endl;

	// b + c = c ; c - b = c
	{
		boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> a(5, 5);
		for (std::size_t i = 0; i < a.size1(); ++i) for (std::size_t j = 0; j < a.size2(); ++j) a(i, j) = i * a.size2() + j;
		std::cout << "A=" << a << std::endl;

		boost::numeric::ublas::matrix_vector_range<
			boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major>
		> b(a, boost::numeric::ublas::range(1, 4), boost::numeric::ublas::range(0, 3));
		std::cout << "b=" << b << std::endl;

		boost::numeric::ublas::matrix_vector_slice<
			boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major>
		> c(a, boost::numeric::ublas::slice(0, 1, 3), boost::numeric::ublas::slice(3, 0, 3));
		std::cout << "c=" << c << std::endl;

		boost::numeric::bindings::blas::axpy(1.0, b, c);
		std::cout << "c=" << c << std::endl;

		boost::numeric::bindings::blas::axpy(-1.0, b, c);
		std::cout << "c=" << c << std::endl;
	}

	std::cout << std::endl;

	// b + c = c ; c - b = c
	{
		boost::numeric::ublas::bounded_matrix<double, 5, 5, boost::numeric::ublas::column_major> a;
		for (std::size_t i = 0; i < a.size1(); ++i) for (std::size_t j = 0; j < a.size2(); ++j) a(i, j) = i * a.size2() + j;
		std::cout << "A=" << a << std::endl;

		boost::numeric::ublas::matrix_vector_range<
			boost::numeric::ublas::bounded_matrix<double, 5, 5, boost::numeric::ublas::column_major>
		> b(a, boost::numeric::ublas::range(1, 4), boost::numeric::ublas::range(0, 3));
		std::cout << "b=" << b << std::endl;

		boost::numeric::ublas::matrix_vector_slice<
			boost::numeric::ublas::bounded_matrix<double, 5, 5, boost::numeric::ublas::column_major>
		> c(a, boost::numeric::ublas::slice(0, 1, 3), boost::numeric::ublas::slice(3, 0, 3));
		std::cout << "c=" << c << std::endl;

		boost::numeric::bindings::blas::axpy(1.0, b, c);
		std::cout << "c=" << c << std::endl;

		boost::numeric::bindings::blas::axpy(-1.0, b, c);
		std::cout << "c=" << c << std::endl;
	}

	return 0;
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"  // Precompiled headers

#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/poses/SO_SE_average.h>
#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::poses;

// -----------   SO_average<2> --------------------
SO_average<2>::SO_average() = default;

void SO_average<2>::clear()
{
	m_count = .0;
	m_accum_x = m_accum_y = .0;
}
void SO_average<2>::append(const double orientation_rad)
{
	append(orientation_rad, 1.0);
}
void SO_average<2>::append(const double orientation_rad, const double weight)
{
	m_count += weight;
	m_accum_x += cos(orientation_rad) * weight;
	m_accum_y += sin(orientation_rad) * weight;
}
double SO_average<2>::get_average() const
{
	ASSERT_ABOVE_(m_count, 0);
	const double x = m_accum_x / m_count;
	const double y = m_accum_y / m_count;
	errno = 0;
	double ang = atan2(y, x);
	if (errno == EDOM)
	{
		if (enable_exception_on_undeterminate)
			throw std::runtime_error(
				"[SO_average<2>::get_average()] Undetermined average value");
		else
			ang = 0;
	}
	return ang;
}

// -----------   SO_average<3> --------------------
SO_average<3>::SO_average() : m_accum_rot() { clear(); }
void SO_average<3>::clear()
{
	m_count = .0;
	m_accum_rot.setZero();
}
void SO_average<3>::append(const mrpt::math::CMatrixDouble33& M)
{
	append(M, 1.0);
}
void SO_average<3>::append(
	const mrpt::math::CMatrixDouble33& M, const double weight)
{
	m_count += weight;
	m_accum_rot.asEigen() += weight * M.asEigen();
}
// See: eq. (3.7) in "MEANS AND AVERAGING IN THE GROUP OF ROTATIONS", MAHER
// MOAKHER, 2002.
mrpt::math::CMatrixDouble33 SO_average<3>::get_average() const
{
	ASSERT_ABOVE_(m_count, 0);
	const Eigen::Matrix3d MtM = m_accum_rot.transpose() * m_accum_rot.asEigen();

	Eigen::JacobiSVD<Eigen::Matrix3d> svd(MtM, Eigen::ComputeFullU);
	const Eigen::Vector3d vs = svd.singularValues();

	errno = 0;
	const double d1 = 1.0 / sqrt(vs[0]);
	const double d2 = 1.0 / sqrt(vs[1]);
	const double d3 = mrpt::sign(m_accum_rot.det()) / sqrt(vs[2]);
	if (errno != 0)
	{
		if (enable_exception_on_undeterminate)
			throw std::runtime_error(
				"[SO_average<3>::get_average()] Undetermined average value");
		else
			return mrpt::math::CMatrixDouble33::Identity();
	}

	mrpt::math::CMatrixDouble33 D = mrpt::math::CMatrixDouble33::Zero();
	D(0, 0) = d1;
	D(1, 1) = d2;
	D(2, 2) = d3;
	return mrpt::math::CMatrixDouble33(
		m_accum_rot.asEigen() * svd.matrixU() * D.asEigen() *
		svd.matrixU().transpose());
}

// -----------   SE_average<2> --------------------
SE_average<2>::SE_average() : m_rot_part() { clear(); }
void SE_average<2>::clear()
{
	m_count = .0;
	m_accum_x = m_accum_y = .0;
	m_rot_part.clear();
}
void SE_average<2>::append(const mrpt::poses::CPose2D& p) { append(p, 1.0); }
void SE_average<2>::append(const mrpt::poses::CPose2D& p, const double weight)
{
	m_count += weight;
	m_accum_x += weight * p.x();
	m_accum_y += weight * p.y();
	m_rot_part.append(p.phi(), weight);
}
void SE_average<2>::append(const mrpt::math::TPose2D& p, const double weight)
{
	m_count += weight;
	m_accum_x += weight * p.x;
	m_accum_y += weight * p.y;
	m_rot_part.append(p.phi, weight);
}
void SE_average<2>::get_average(mrpt::poses::CPose2D& ret_mean) const
{
	ASSERT_ABOVE_(m_count, 0);
	ret_mean.x(m_accum_x / m_count);
	ret_mean.y(m_accum_y / m_count);
	const_cast<SO_average<2>*>(&m_rot_part)->enable_exception_on_undeterminate =
		this->enable_exception_on_undeterminate;
	ret_mean.phi(m_rot_part.get_average());
}

// -----------   SE_average<3> --------------------
SE_average<3>::SE_average() : m_rot_part() { clear(); }
void SE_average<3>::clear()
{
	m_count = .0;
	m_accum_x = m_accum_y = m_accum_z = .0;
	m_rot_part.clear();
}
void SE_average<3>::append(const mrpt::poses::CPose3D& p) { append(p, 1.0); }
void SE_average<3>::append(const mrpt::poses::CPose3D& p, const double weight)
{
	m_count += weight;
	m_accum_x += weight * p.x();
	m_accum_y += weight * p.y();
	m_accum_z += weight * p.z();
	m_rot_part.append(p.getRotationMatrix(), weight);
}
void SE_average<3>::append(const mrpt::math::TPose3D& p, const double weight)
{
	append(CPose3D(p), weight);
}
void SE_average<3>::get_average(mrpt::poses::CPose3D& ret_mean) const
{
	ASSERT_ABOVE_(m_count, 0);
	ret_mean.x(m_accum_x / m_count);
	ret_mean.y(m_accum_y / m_count);
	ret_mean.z(m_accum_z / m_count);
	const_cast<SO_average<3>*>(&m_rot_part)->enable_exception_on_undeterminate =
		this->enable_exception_on_undeterminate;
	ret_mean.setRotationMatrix(m_rot_part.get_average());
}


/**
 * @file
 * @brief NPDE homework TestQuadratureRules
 * @author Erick Schulz, Liaowang Huang (refactoring)
 * @date 08/03/2019, 22/02/2020 (refactoring)
 * @copyright Developed at ETH Zurich
 */

#include "testquadraturerules.h"

#include <lf/base/base.h>
#include <lf/quad/quad.h>

#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace TestQuadratureRules {

double factorial(int i) { return std::tgamma(i + 1); }

/* SAM_LISTING_BEGIN_1 */
bool testQuadOrderTria(const lf::quad::QuadRule &quad_rule,
                       unsigned int order) {
  bool order_isExact = true;  // return variable
  //====================
  // Your code goes here

  // STEP 0: Exception handling
  // mesh shape must match
  if (quad_rule.RefEl()!= lf::base::RefEl::kTria()){
    throw "The quadrature rule must be defined on simplicial cells!";
  }

  // STEP 1: obtain the weights and nodes of the quadrature rule
  Eigen::VectorXd weights_ = quad_rule.Weights();
  Eigen::MatrixXd nodes_ = quad_rule.Points();    // 2 by # nodes matrix

  // std::cout << "It is a " << nodes_.rows() << " by " << nodes_.cols() << " matrix!" << std::endl;

  Eigen::VectorXd x_coords = nodes_.row(0);   // x-coords of nodes stored in a vec
  Eigen::VectorXd y_coords = nodes_.row(1);   // y-coords

  // STEP 2: compute the weights using the analytic formula
  // double volume = 0.5; // area of the reference triangle => not required because gets cancelled out in the formula

  double epsilon = 1e-12; // epsilon used as tolerance
  double exact;     // stores result of the exact integral
  double approx;    // stores result of the approximated integral using quadrature rule

  // helper function for the quadrature rule
  auto F = [&](int i, int j)-> Eigen::VectorXd { return x_coords.array().pow(i) * y_coords.array().pow(j);};

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

      // STEP 3: compute the integrals

      exact = (factorial(i) * factorial(j)) / (factorial(i + j + 2));// analytic: exact integral
      approx = F(i,j).dot(weights_);// numeric:  from quadrature rule
                                 //           dot product of the 2 vectors as sum

      // STEP 4: test difference
      // test failed for "big error" relative to the value tested
      if(std::fabs(exact - approx) > std::fabs(exact) * epsilon){
          return false; // the order is not exact, fails for one test
      }

    }
  }

  //====================
  return order_isExact;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
bool testQuadOrderQuad(const lf::quad::QuadRule &quad_rule,
                       unsigned int order) {
  bool order_isExact = true;  // return variable

  //====================
  // Your code goes here

  // STEP 0: exception handling
  if(quad_rule.RefEl() != lf::base::RefEl::kQuad()){
    throw "Only quadraterial cells are allowed!";
  }

  // STEP 1: obtain weights and nodes of the quadrature rule
  Eigen::VectorXd weights_ = quad_rule.Weights();
  Eigen::MatrixXd nodes_   = quad_rule.Points();   // 2 by # nodes matrix

  // obtain x_coords and y_coords from nodes_
  Eigen::VectorXd x_coords = nodes_.row(0);
  Eigen::VectorXd y_coords = nodes_.row(1);

  // helper function for the numeric formula
  auto F = [&](int i, int j)-> Eigen::VectorXd { return x_coords.array().pow(i) * y_coords.array().pow(j);};

  double exact, approx;
  double epsilon = 1e-12;  // define the epsilon for tolerance

  // STEP 2: loop over the possible combis of basis functions
  for(int i = 0; i < order; ++i){
      for(int j = 0; j < order; ++j){

        exact = 1. / ((i + 1.)* (j + 1.)); // using the exact formula
        approx = F(i, j).dot(weights_); // compute using the quad rule

        // STEP 3: Check the equality with tolerance allowed
        if(std::fabs(exact - approx) >= exact * epsilon){
            return false;
        }
      }
  }

  //====================
  return order_isExact;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
unsigned int calcQuadOrder(const lf::quad::QuadRule &quad_rule) {
  unsigned int maximal_order = quad_rule.Order();

  //====================
  // Your code goes here

  // case distinction
  // based on reference element of the quadrature rule
  const lf::base::RefEl reference  = quad_rule.RefEl();

  // CASE 1: on simplicial cells
  if(reference == lf::base::RefEl::kTria()){

    while(testQuadOrderTria(quad_rule, maximal_order + 1)){
        // change the condition
        ++maximal_order;
    }

  }
  // CASE 2: on tensor product cells
  else if (reference == lf::base::RefEl::kQuad()){

    while(testQuadOrderQuad(quad_rule, maximal_order + 1)){
        // change the condition
        ++maximal_order;
    }

  }
  // default: error handling for exception
  else{
    throw "Only possible tests for mesh with reference elements being simplex or tensor product";
  }

  //====================
  return maximal_order;
}
/* SAM_LISTING_END_3 */

}  // namespace TestQuadratureRules


#include <iostream>
#include <stdio.h>

#include <Eigen/Dense>

#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

using namespace boost::numeric;

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

#include "KalmanFilter.h"

namespace openmht {

     KalmanFilter::KalmanFilter()
     {
     }

     KalmanFilter::KalmanFilter(const Eigen::MatrixXf &F,
				const Eigen::MatrixXf &B,
				const Eigen::MatrixXf &H,
				const Eigen::MatrixXf &Q,
				const Eigen::MatrixXf &R)
     {
	  setModel(F, B, H, Q, R);
     }

     int KalmanFilter::setModel(const Eigen::MatrixXf &F,
				const Eigen::MatrixXf &B,
				const Eigen::MatrixXf &H,
				const Eigen::MatrixXf &Q,
				const Eigen::MatrixXf &R)
     {
	  F_ = F;
	  B_ = B;
	  H_ = H;
          Q_ = Q;
          R_ = R;
	  eye_ = Eigen::MatrixXf::Identity(F.rows(), F.cols());

	  return 0;
     }

     int KalmanFilter::init(const Eigen::MatrixXf &x0,
			    const Eigen::MatrixXf &P0)
     {
	  x_ = x0;
	  P_ = P0;
	  return 0;
     }

     int KalmanFilter::predict(const Eigen::MatrixXf &u)
     {
          x_ = F_*x_ + B_*u;
          P_ = F_*P_*F_.transpose() + Q_;

	  return 0;
     }

     int KalmanFilter::update(const Eigen::MatrixXf &z)
     {
	  K_ = P_*H_.transpose()*(H_*P_*H_.transpose() + R_).inverse();
	  x_ = x_ + K_*(z - H_*x_);
          P_ = (eye_ - K_*H_)*P_;
	  return 0;
     }

     Eigen::MatrixXf KalmanFilter::state() const
     {
	  return x_;
     }

     void KalmanFilter::set_state(const Eigen::MatrixXf &x)
     {
          x_ = x;
     }

     Eigen::MatrixXf KalmanFilter::covariance() const
     {
	  return P_;
     }

     Ellipse KalmanFilter::error_ellipse(double confidence)
     {
          if (confidence < 0) {
               confidence = 0;
          } else if (confidence > 1) {
               confidence = 1;
          }

          // Compute the eigenvectors and eigenvalues of the measurement
          // covariance matrix
          Eigen::MatrixXf B = this->meas_covariance();
          Eigen::EigenSolver<Eigen::MatrixXf> es(B);
          Eigen::EigenSolver< Eigen::MatrixXf >::EigenvectorsType evecs = es.eigenvectors();
          Eigen::EigenSolver< Eigen::MatrixXf >::EigenvectorsType evalues = es.eigenvalues();

          // Find the larger (1st) eigenvalue / eigenvector
          double eig0 = evalues(0).real();
          double eig1 = evalues(1).real();

          double lambda0, lambda1;
          int eig_1st_index;
          if (eig0 >= eig1) {
               eig_1st_index = 0;
               lambda0 = eig0;
               lambda1 = eig1;
          } else {
               eig_1st_index = 1;
               lambda0 = eig1;
               lambda1 = eig0;
          }

          // Determine the angle of the ellipse
          double q0x = evecs.col(eig_1st_index)(0).real();
          double q0y = evecs.col(eig_1st_index)(1).real();
          double angle = 180.0/3.14159265359 * atan2(q0y,q0x);

          double p = confidence; // 0.0 - 1.0

          // The major and minor axes of the ellipse are stored as "half" of
          // the major axes because cv::ellipse accepts half sizes for input
          double r0 = sqrt(-2.0*log(1.0-p/1.00)*lambda0) / 2.0;
          double r1 = sqrt(-2.0*log(1.0-p/1.00)*lambda1) / 2.0;

          Eigen::Vector2d center(x_(0,0),x_(1,0));
          //cv::Point2d center(160,120);
          return Ellipse(center, Eigen::Vector2d(r0,r1), angle);
     }

     bool KalmanFilter::is_within_region(Eigen::MatrixXf Zm, double std)
     {
          Eigen::MatrixXf B = this->meas_covariance();
          Eigen::MatrixXf diff = Zm - H_*x_;
          Eigen::MatrixXf dist_mat = diff.transpose()*B.inverse()*diff;
          double dist = dist_mat(0,0);
          //if (dist <= pow(nsigma,2)) {
          if (dist <= std) {
               return true;
          } else {
               return false;
          }
     }

     Eigen::MatrixXf KalmanFilter::meas_covariance()
     {
          return H_ * P_ * H_.transpose() + R_;
     }

     void KalmanFilter::print()
     {
          cout << "State: " << endl << x_ << endl;
          cout << "Covar: " << endl << P_ << endl;
          cout << "R: " << endl << R_ << endl;
     }
}


// A program to read xyz point set (position) and write off file with CGAL
// Visualization with Open3D in python
// Source: https://doc.cgal.org/latest/Advancing_front_surface_reconstruction/index.html#Chapter_Advancing_Front_Surface_Reconstruction

#include <iostream>
#include <fstream>
#include <algorithm>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Advancing_front_surface_reconstruction.h>
#include <CGAL/tuple.h>
#include <boost/lexical_cast.hpp>
typedef CGAL::Simple_cartesian<double> K;
typedef K::Point_3  Point_3;
typedef std::array<std::size_t,3> Facet;
namespace std {
  std::ostream&
  operator<<(std::ostream& os, const Facet& f)
  {
    os << "3 " << f[0] << " " << f[1] << " " << f[2];
    return os;
  }
}

struct Perimeter {
  double bound;
  Perimeter(double bound)
    : bound(bound)
  {}
  template <typename AdvancingFront, typename Cell_handle>
  double operator() (const AdvancingFront& adv, Cell_handle& c,
                     const int& index) const
  {
    // bound == 0 is better than bound < infinity
    // as it avoids the distance computations
    if(bound == 0){
      return adv.smallest_radius_delaunay_sphere (c, index);
    }
    // If perimeter > bound, return infinity so that facet is not used
    double d  = 0;
    d = sqrt(squared_distance(c->vertex((index+1)%4)->point(),
                              c->vertex((index+2)%4)->point()));
    if(d>bound) return adv.infinity();
    d += sqrt(squared_distance(c->vertex((index+2)%4)->point(),
                               c->vertex((index+3)%4)->point()));
    if(d>bound) return adv.infinity();
    d += sqrt(squared_distance(c->vertex((index+1)%4)->point(),
                               c->vertex((index+3)%4)->point()));
    if(d>bound) return adv.infinity();
    // Otherwise, return usual priority value: smallest radius of
    // delaunay sphere
    return adv.smallest_radius_delaunay_sphere (c, index);
  }
};

int main(int argc, char* argv[])
{
  std::ifstream in((argc>1)?argv[1]:"data/atoms.xyz");
  double per = (argc>2)?boost::lexical_cast<double>(argv[2]):0;
  std::vector<Point_3> points;
  std::vector<Facet> facets;
  std::string header;
  in >> header;
  std::istream_iterator<Point_3> in_points_begin(in);
  std::istream_iterator<Point_3> in_points_end;
  std::copy(in_points_begin, in_points_end, std::back_inserter(points));
  Perimeter perimeter(per);
  CGAL::advancing_front_surface_reconstruction(points.begin(),
                                               points.end(),
                                               std::back_inserter(facets),
                                               perimeter);
  std::ofstream outoff;
  outoff.open ("data/atoms_CGAL.off");
  outoff << "OFF\n" << points.size() << " " << facets.size() << " 0\n";
  std::copy(points.begin(),
            points.end(),
            std::ostream_iterator<Point_3>(outoff, "\n"));
  std::copy(facets.begin(),
            facets.end(),
            std::ostream_iterator<Facet>(outoff, "\n"));
  return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    CameraResectioning.cpp
 * @brief   An example of gtsam for solving the camera resectioning problem
 * @author  Duy-Nguyen Ta
 * @date    Aug 23, 2011
 */

#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/make_shared.hpp>

using namespace gtsam;
using namespace gtsam::noiseModel;
using symbol_shorthand::X;

/**
 * Unary factor on the unknown pose, resulting from meauring the projection of
 * a known 3D point in the image
 */
class ResectioningFactor: public NoiseModelFactor1<Pose3> {
  typedef NoiseModelFactor1<Pose3> Base;

  Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters
  Point3 P_;              ///< 3D point on the calibration rig
  Point2 p_;              ///< 2D measurement of the 3D point

public:

  /// Construct factor given known point P and its projection p
  ResectioningFactor(const SharedNoiseModel& model, const Key& key,
      const Cal3_S2::shared_ptr& calib, const Point2& p, const Point3& P) :
      Base(model, key), K_(calib), P_(P), p_(p) {
  }

  /// evaluate the error
  virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
      boost::none) const {
    PinholeCamera<Cal3_S2> camera(pose, *K_);
    return camera.project(P_, H, boost::none, boost::none) - p_;
  }
};

/*******************************************************************************
 * Camera: f = 1, Image: 100x100, center: 50, 50.0
 * Pose (ground truth): (Xw, -Yw, -Zw, [0,0,2.0]')
 * Known landmarks:
 *    3D Points: (10,10,0) (-10,10,0) (-10,-10,0) (10,-10,0)
 * Perfect measurements:
 *    2D Point:  (55,45)   (45,45)    (45,55)     (55,55)
 *******************************************************************************/
int main(int argc, char* argv[]) {
  /* read camera intrinsic parameters */
  Cal3_S2::shared_ptr calib(new Cal3_S2(1, 1, 0, 50, 50));

  /* 1. create graph */
  NonlinearFactorGraph graph;

  /* 2. add factors to the graph */
  // add measurement factors
  SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5));
  boost::shared_ptr<ResectioningFactor> factor;
  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(55, 45), Point3(10, 10, 0));
  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(45, 45), Point3(-10, 10, 0));
  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(45, 55), Point3(-10, -10, 0));
  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(55, 55), Point3(10, -10, 0));

  /* 3. Create an initial estimate for the camera pose */
  Values initial;
  initial.insert(X(1),
      Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2)));

  /* 4. Optimize the graph using Levenberg-Marquardt*/
  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  result.print("Final result:\n");

  return 0;
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2020 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <gudhi/Flag_complex_edge_collapser.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Persistent_cohomology.h>
#include <gudhi/distance_functions.h>
#include <gudhi/Points_off_io.h>
#include <gudhi/graph_simplicial_complex.h>

#include <boost/range/adaptor/transformed.hpp>

#include<utility>  // for std::pair
#include<vector>
#include<tuple>

// Types definition

using Simplex_tree = Gudhi::Simplex_tree<>;
using Filtration_value = Simplex_tree::Filtration_value;
using Vertex_handle = Simplex_tree::Vertex_handle;
using Point = std::vector<Filtration_value>;
using Vector_of_points = std::vector<Point>;

using Proximity_graph = Gudhi::Proximity_graph<Simplex_tree>;

using Field_Zp = Gudhi::persistent_cohomology::Field_Zp;
using Persistent_cohomology = Gudhi::persistent_cohomology::Persistent_cohomology<Simplex_tree, Field_Zp>;

using Persistence_interval = std::tuple<int, Filtration_value, Filtration_value>;
/*
 * Compare two intervals by dimension, then by length.
 */
struct cmp_intervals_by_length {
  explicit cmp_intervals_by_length(Simplex_tree * sc)
      : sc_(sc) { }

  template<typename Persistent_interval>
  bool operator()(const Persistent_interval & p1, const Persistent_interval & p2) {
    return (sc_->filtration(get < 1 > (p1)) - sc_->filtration(get < 0 > (p1))
            > sc_->filtration(get < 1 > (p2)) - sc_->filtration(get < 0 > (p2)));
  }
  Simplex_tree* sc_;
};

std::vector<Persistence_interval> get_persistence_intervals(Simplex_tree& st, int ambient_dim) {
  std::vector<Persistence_interval> persistence_intervals;
  st.expansion(ambient_dim);

  // Sort the simplices in the order of the filtration
  st.initialize_filtration();
  // Compute the persistence diagram of the complex
  Persistent_cohomology pcoh(st);
  // initializes the coefficient field for homology - must be a prime number
  int p = 11;
  pcoh.init_coefficients(p);

  // Default min_interval_length = 0.
  pcoh.compute_persistent_cohomology();
  // Custom sort and output persistence
  cmp_intervals_by_length cmp(&st);
  auto persistent_pairs = pcoh.get_persistent_pairs();
  std::sort(std::begin(persistent_pairs), std::end(persistent_pairs), cmp);
  for (auto pair : persistent_pairs) {
    persistence_intervals.emplace_back(st.dimension(get<0>(pair)),
                                       st.filtration(get<0>(pair)),
                                       st.filtration(get<1>(pair)));
  }
  return persistence_intervals;
}

int main(int argc, char* argv[]) {
  if (argc != 3) {
    std::cerr << "This program requires an OFF file and minimal threshold value as parameter\n";
    std::cerr << "For instance: ./Edge_collapse_conserve_persistence ../../data/points/tore3D_300.off 1.\n";
    exit(-1);  // ----- >>
  }

  std::string off_file_points {argv[1]};
  double threshold {atof(argv[2])};

  Gudhi::Points_off_reader<Point> off_reader(off_file_points);
  if (!off_reader.is_valid()) {
    std::cerr << "Unable to read file " << off_file_points << "\n";
    exit(-1);  // ----- >>
  }

  Vector_of_points point_vector = off_reader.get_point_cloud();
  if (point_vector.size() <= 0) {
    std::cerr << "Empty point cloud." << std::endl;
    exit(-1);  // ----- >>
  }

  Proximity_graph proximity_graph = Gudhi::compute_proximity_graph<Simplex_tree>(off_reader.get_point_cloud(),
                                                                                threshold,
                                                                                Gudhi::Euclidean_distance());

  if (num_edges(proximity_graph) <= 0) {
    std::cerr << "Total number of egdes are zero." << std::endl;
    exit(-1);
  }

  int ambient_dim = point_vector[0].size();

  // ***** Simplex tree from a flag complex built after collapse *****
  auto remaining_edges = Gudhi::collapse::flag_complex_collapse_edges(
    boost::adaptors::transform(edges(proximity_graph), [&](auto&&edge){
      return std::make_tuple(static_cast<Vertex_handle>(source(edge, proximity_graph)),
                             static_cast<Vertex_handle>(target(edge, proximity_graph)),
                             get(Gudhi::edge_filtration_t(), proximity_graph, edge));
      })
  );

  Simplex_tree stree_from_collapse;
  for (Vertex_handle vertex = 0; static_cast<std::size_t>(vertex) < point_vector.size(); vertex++) {
    // insert the vertex with a 0. filtration value just like a Rips
    stree_from_collapse.insert_simplex({vertex}, 0.);
  }
  for (auto remaining_edge : remaining_edges) {
    stree_from_collapse.insert_simplex({std::get<0>(remaining_edge), std::get<1>(remaining_edge)},
                                       std::get<2>(remaining_edge));
  }

  std::vector<Persistence_interval> persistence_intervals_from_collapse = get_persistence_intervals(stree_from_collapse, ambient_dim);

  // ***** Simplex tree from the complete flag complex *****
  Simplex_tree stree_wo_collapse;
  stree_wo_collapse.insert_graph(proximity_graph);

  std::vector<Persistence_interval> persistence_intervals_wo_collapse = get_persistence_intervals(stree_wo_collapse, ambient_dim);

  // ***** Comparison *****
  if (persistence_intervals_wo_collapse.size() != persistence_intervals_from_collapse.size()) {
    std::cerr << "Number of persistence pairs with    collapse is " << persistence_intervals_from_collapse.size() << std::endl;
    std::cerr << "Number of persistence pairs without collapse is " << persistence_intervals_wo_collapse.size()   << std::endl;
    exit(-1);
  }

  int return_value = 0;
  auto ppwoc_ptr = persistence_intervals_wo_collapse.begin();
  for (auto ppfc: persistence_intervals_from_collapse) {
    if (ppfc != *ppwoc_ptr) {
      return_value++;
      std::cerr << "Without collapse: "
                << std::get<0>(*ppwoc_ptr) << " " << std::get<1>(*ppwoc_ptr) << " " << std::get<2>(*ppwoc_ptr)
                << " - With collapse: "
                << std::get<0>(ppfc) << " " << std::get<1>(ppfc) << " " << std::get<2>(ppfc) << std::endl;
    }
    ppwoc_ptr++;
  }
  return return_value;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/SymEigsSolver.h>

namespace fluid {
namespace algorithm {

class SpectralEmbedding
{
public:
  using MatrixXd = Eigen::MatrixXd;
  using ArrayXXd = Eigen::ArrayXXd;
  using SparseMatrixXd = Eigen::SparseMatrix<double>;

  ArrayXXd process(SparseMatrixXd graph, index dims)
  {
    using namespace Eigen;
    using namespace Spectra;
    using namespace std;
    VectorXd diagData = graph * VectorXd::Ones(graph.cols());
    diagData = (1 / diagData.array().sqrt());
    SparseMatrixXd D = SparseMatrixXd(graph.rows(), graph.cols());
    D.reserve(graph.rows());
    for (index i = 0; i < D.rows(); i++) { D.insert(i, i) = diagData(i); }
    SparseMatrixXd I = SparseMatrixXd(D.rows(), D.cols());
    I.setIdentity();
    SparseMatrixXd           L = I - (D * (graph * D));
    int                      k = static_cast<int>(dims + 1);
    index                    ncv = max(2 * k + 1, int(round(sqrt(L.rows()))));
    VectorXd                 initV = VectorXd::Ones(L.rows());
    SparseSymMatProd<double> op(L);
    SymEigsSolver<double, SMALLEST_MAGN, SparseSymMatProd<double>> eigs(&op, k,
                                                                        ncv);
    eigs.init(initV.data());
    auto nConverged = eigs.compute(
        D.cols(), 1e-4, SMALLEST_MAGN); // TODO: failback if not converging
    MatrixXd U = eigs.eigenvectors();
    ArrayXXd Y = U.block(0, 1, U.rows(), dims).array();
    return Y;
  }
};
}; // namespace algorithm
}; // namespace fluid


#ifndef CAMERA_UTILS_INCLUDE_GUARD_HPP
#define CAMERA_UTILS_INCLUDE_GUARD_HPP
/// \file
/// \brief Camera model and camera lidar fusion
///

/*! Definition of extrinsics
 *  map -> ... -> base_link -> velodyne -> camera_link -> camera_color_optical_frame
 *      (obtained from slam)         (extrinsic)    (given by camera)
 *
 *                    z  x                          / z
 *                    | /                          /_ _ x
 *               y _ _|/                          |
 *                                                | y
 *               camera_link              camera_color_optical_frame
 *
*/

// ROS
#include "ros/ros.h"
// Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

namespace disinfection_robot
{

Eigen::Affine3d create_rotation_matrix(double ax, double ay, double az)
{
    Eigen::Affine3d rx =
        Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0)));
    Eigen::Affine3d ry =
        Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0)));
    Eigen::Affine3d rz =
        Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1)));
    return rz * ry * rx;
}

class Camera
{
public:
    float color_cx_;
    float color_cy_;
    float color_fx_;
    float color_fy_; // Camera intrinsics

    Eigen::Matrix4d Tco2l_; // extrinsic, from camera optical to lidar frame transformation

    int image_height_ = 480;
    int image_width_ = 640;

    Camera();

    /*! \brief construct camera object with instrinsics
    */
    Camera(ros::NodeHandle &nh)
    {
        nh.getParam("/color_cx", color_cx_);
        nh.getParam("/color_cy", color_cy_);
        nh.getParam("/color_fx", color_fx_);
        nh.getParam("/color_fy", color_fy_);

        nh.getParam("/image_height", image_height_);
        nh.getParam("/image_width", image_width_);

        std::vector<double> rpy_lc;
        std::vector<double> t_lc;
        nh.getParam("/rpy_lc", rpy_lc);
        nh.getParam("/t_lc", t_lc);
        Eigen::Affine3d Rot_lc = create_rotation_matrix(rpy_lc[0], rpy_lc[1], rpy_lc[2]);
        Eigen::Affine3d Trans_lc(Eigen::Translation3d(Eigen::Vector3d(t_lc[0], t_lc[1], t_lc[2])));
        // transformation from camera link to lidar
        Eigen::Affine3d T_cl = (Trans_lc * Rot_lc).inverse();

        std::vector<double> rpy_c2co;
        std::vector<double> t_c2co;
        nh.getParam("/rpy_c2co", rpy_c2co);
        nh.getParam("/t_c2co", t_c2co);
        Eigen::Affine3d Rot_c2co = create_rotation_matrix(rpy_c2co[0], rpy_c2co[1], rpy_c2co[2]);
        Eigen::Affine3d Trans_c2co(Eigen::Translation3d(Eigen::Vector3d(t_c2co[0], t_c2co[1], t_c2co[2])));
        // transformation from camera optical frame to camera link
        Eigen::Affine3d T_co2c = (Trans_c2co * Rot_c2co).inverse();

        Tco2l_ = (T_co2c * T_cl).matrix();

        // validated by comparing to ros tf
        // std::cout << Tco2l_ << std::endl;
    }

    /*! \brief return 3 by 3 intrinsic matrix
    */
    Eigen::Matrix<double, 3, 3> K() const
    {
        Eigen::Matrix<double, 3, 3> k;
        k << color_fx_, 0, color_cx_, 0, color_fy_, color_cy_, 0, 0, 1;
        return k;
    }

    /*! \brief transform a point from lidar coordinate to camera coordinate
    */
    Eigen::Matrix<double, 3, 1> lidar2camera(const Eigen::Matrix<double, 3, 1> &p_l)
    {
        return (Tco2l_ * p_l.colwise().homogeneous()).colwise().hnormalized();
    }

    /*! \brief transform a point from camera coordinate to sensor coordinate
    */
    Eigen::Matrix<int, 2, 1> camera2pixel(const Eigen::Matrix<double, 3, 1> &p_c)
    {
        return Eigen::Matrix<int, 2, 1>(
            color_fx_ * p_c(0, 0) / p_c(2, 0) + color_cx_,
            color_fy_ * p_c(1, 0) / p_c(2, 0) + color_cy_);
    }

    /*! \brief transform a point in the lidar coordinate to pixel position (sensor coordinate)
    */
    Eigen::Matrix<int, 2, 1> lidar2pixel(const Eigen::Matrix<double, 3, 1> &p_l)
    {
        return camera2pixel(lidar2camera(p_l));
    }

    /*! \brief transform a point from lidar coordinate to map coordinate
    *           Given map to lidar transform and 3D point
    */
    Eigen::Matrix<double, 3, 1> lidar2map(const Eigen::Matrix<double, 3, 1> &p_l, const Eigen::Matrix<double, 4, 4> &Tml)
    {
        return (Tml * p_l.colwise().homogeneous()).colwise().hnormalized();
    }

    /*! \brief given a pixel position and a depth, transfer it into a 3D point in camere frame (all in color camera frame)
    */
    Eigen::Matrix<float, 3, 1> depth2camera(const Eigen::Matrix<int, 2, 1> &p_s, unsigned int depth)
    {
        float z = float(depth);
        float x = (p_s[0] - color_cx_) * z / color_fx_;
        float y = (p_s[1] - color_cy_) * z / color_fy_;

        return Eigen::Matrix<float, 3, 1>(x, y, z);
    }
};

} // namespace disinfection_robot
#endif


//
// Created by aoool on 15.11.18.
//

#include "trajectory_layer.hpp"

#include <typeinfo>
#include <Eigen/Dense>

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

std::vector<double>
TrajectoryLayer::GetJerkMinimizingTrajectory(std::vector<double> start, std::vector<double> end, double t) const
{
  /*
  Calculate the Jerk Minimizing Trajectory that connects the initial state
  to the final state in time t.

  INPUTS

  start - the vehicles start location given as a length three array
      corresponding to initial values of [s, s_dot, s_double_dot]

  end   - the desired end state for vehicle. Like "start" this is a
      length three array.

  t     - The duration, in seconds, over which this maneuver should occur.

  OUTPUT
  an array of length 6, each value corresponding to a coefficent in the polynomial
  s(t) = a_0 + a_1 * t + a_2 * t**2 + a_3 * t**3 + a_4 * t**4 + a_5 * t**5

  EXAMPLE

  > JMT( [0, 10, 0], [10, 10, 0], 1)
  [0.0, 10.0, 0.0, 0.0, 0.0, 0.0]
  */

  MatrixXd A = MatrixXd(3, 3);
  A << t*t*t, t*t*t*t, t*t*t*t*t,
       3*t*t, 4*t*t*t, 5*t*t*t*t,
         6*t,  12*t*t,  20*t*t*t;

  MatrixXd B = MatrixXd(3,1);
  B << end[0]-(start[0]+start[1]*t+.5*start[2]*t*t),
       end[1]-(start[1]+start[2]*t),
       end[2]-start[2];

  MatrixXd Ai = A.inverse();

  MatrixXd C = Ai*B;

  std::vector<double> result = {start[0], start[1], 0.5*start[2]};
  for(int i = 0; i < C.size(); i++)
  {
    result.push_back(C.data()[i]);
  }

  return result;

}

TrajectoryLayer::TrajectoryLayer(const PathPlannerConfig& config, LocalizationLayer& localization_layer,
                                 PredictionLayer& prediction_layer, BehaviorLayer& behavior_layer):
  pp_config_{config},
  localization_layer_{localization_layer},
  prediction_layer_{prediction_layer},
  behavior_layer_{behavior_layer},
  initialized_{false},
  ego_car_{},
  next_cars_{}
{

}

std::vector<Car>
TrajectoryLayer::GetTrajectory(size_t num_points)
{
  if (!initialized_) {
    throw std::logic_error("TrajectoryLayer::Initialize should be invoked before TrajectoryLayer::GetTrajectory");
  }

  auto&& predictions = prediction_layer_.GetPredictions(num_points * pp_config_.frequency_s, ego_car_.T());

  auto&& cur_other_cars_current_lane = ego_car_.CarsInCurrentLane(map_keys(predictions));
  std::optional<Car> cur_other_car_ahead_current_lane_opt = ego_car_.NearestCarAhead(cur_other_cars_current_lane);

  if (cur_other_car_ahead_current_lane_opt.has_value()) {
    auto cur_other_car_ahead = cur_other_car_ahead_current_lane_opt.value();

    if (ego_car_.IsFrontBufferViolatedBy(cur_other_car_ahead, 0.5)) {
      std::cout << __PRETTY_FUNCTION__ << " identified the front buffer violation of ego car\n"
                << ego_car_ << "\n by other car\n" << cur_other_car_ahead << std::endl;
      next_cars_.resize(0);
    }
  }

  if (next_cars_.size() >= pp_config_.path_len) {
    std::vector<Car> to_return{next_cars_.rbegin(), next_cars_.rbegin() + num_points};
    ego_car_ = to_return[to_return.size() - 1];
    next_cars_.resize(next_cars_.size() - num_points);

    return to_return;
  }

  if (!next_cars_.empty()) {
    ego_car_ = next_cars_[0];
  }

  Car planned_ego_car = behavior_layer_.Plan(ego_car_)[0];

  auto ego_car_s = static_cast<double>(ego_car_.S());
  auto planned_ego_car_s = static_cast<double>(planned_ego_car.S());
  if (planned_ego_car_s < ego_car_s) {
    planned_ego_car_s += pp_config_.max_s_m;
  }

  double planning_time_horizon = planned_ego_car.T() - ego_car_.T();

  std::vector<double> s_coeffs = GetJerkMinimizingTrajectory(
      {ego_car_s, ego_car_.Vs(), ego_car_.As(), },
      {planned_ego_car_s, planned_ego_car.Vs(), planned_ego_car.As(), },
      planning_time_horizon
  );

  std::vector<double> d_coeffs = GetJerkMinimizingTrajectory(
      { ego_car_.D(), ego_car_.Vd(), ego_car_.Ad(), },
      { planned_ego_car.D(), planned_ego_car.Vd(), planned_ego_car.Ad(), },
      planning_time_horizon
  );

  double t = pp_config_.frequency_s;
  const double t_diff = pp_config_.frequency_s;
  double s_prev = static_cast<double>(ego_car_.S());
  double d_prev = ego_car_.D();
  double vs_prev = ego_car_.Vs();
  double vd_prev = ego_car_.Vd();

  for (int i = 0; i < pp_config_.trajectory_layer_queue_len - next_cars_.size(); ++i) {
    double s = CalcPolynomial(s_coeffs, t);
    double d = CalcPolynomial(d_coeffs, t);
    double vs = Calc1DVelocity(s_prev, s, t_diff);
    double vd = Calc1DVelocity(d_prev, d, t_diff);

    next_cars_.push_front(
      Car::Builder(planned_ego_car)
        .SetTime(ego_car_.T() + t)
        .SetCoordinateS(s)
        .SetCoordinateD(d)
        .SetVelocityS(vs)
        .SetVelocityD(vd)
        .SetAccelerationS(Calc1DAcc(vs_prev, vs, t_diff))
        .SetAccelerationD(Calc1DAcc(vd_prev, vd, t_diff))
      .Build()
    );
    s_prev = s;
    d_prev = d;
    vs_prev = vs;
    vd_prev = vd;

    t += t_diff;
  }

  std::vector<Car> to_return{next_cars_.rbegin(), next_cars_.rbegin() + num_points};
  ego_car_ = to_return[to_return.size() - 1];
  next_cars_.resize(next_cars_.size() - num_points);

  return to_return;
}

void TrajectoryLayer::Initialize(const Car& car)
{
  ego_car_ = car;
  initialized_ = true;
}

TrajectoryLayer::~TrajectoryLayer() = default;


#ifndef MATHEVAL_IMPLEMENTATION
#error "Do not include parser_def.hpp directly!"
#endif

#pragma once

#include "ast.hpp"
#include "ast_adapted.hpp"
#include "../math.hpp"
#include "parser.hpp"

#include <boost/math/constants/constants.hpp>
#include <boost/spirit/home/x3.hpp>

#include <cmath>
#include <iostream>
#include <limits>
#include <string>

namespace matheval {

namespace x3 = boost::spirit::x3;

namespace parser {

// LOOKUP

struct constant_ : x3::symbols<double> {
    constant_() {
        // clang-format off
        add
            ("e"      , boost::math::constants::e<double>())
            ("epsilon", std::numeric_limits<double>::epsilon())
            ("phi"    , boost::math::constants::phi<double>())
            ("pi"     , boost::math::constants::pi<double>())
            ;
        // clang-format on
    }
} constant;

struct ufunc_ : x3::symbols<double (*)(double)> {
    ufunc_() {
        // clang-format off
        add
            ("abs"   , static_cast<double (*)(double)>(&std::abs))
            ("acos"  , static_cast<double (*)(double)>(&math::acos))
            ("acosh" , static_cast<double (*)(double)>(&math::acosh))
            ("asin"  , static_cast<double (*)(double)>(&math::asin))
            ("asinh" , static_cast<double (*)(double)>(&std::asinh))
            ("atan"  , static_cast<double (*)(double)>(&std::atan))
            ("atanh" , static_cast<double (*)(double)>(&math::atanh))
            ("cbrt"  , static_cast<double (*)(double)>(&std::cbrt))
            ("ceil"  , static_cast<double (*)(double)>(&std::ceil))
            ("cos"   , static_cast<double (*)(double)>(&math::cos))
            ("cosh"  , static_cast<double (*)(double)>(&std::cosh))
            ("deg"   , static_cast<double (*)(double)>(&math::deg))
            ("erf"   , static_cast<double (*)(double)>(&std::erf))
            ("erfc"  , static_cast<double (*)(double)>(&std::erfc))
            ("exp"   , static_cast<double (*)(double)>(&std::exp))
            ("exp2"  , static_cast<double (*)(double)>(&std::exp2))
            ("floor" , static_cast<double (*)(double)>(&std::floor))
            ("isinf" , static_cast<double (*)(double)>(&math::isinf))
            ("isnan" , static_cast<double (*)(double)>(&math::isnan))
            ("log"   , static_cast<double (*)(double)>(&math::log))
            ("log2"  , static_cast<double (*)(double)>(&math::log2))
            ("log10" , static_cast<double (*)(double)>(&math::log10))
            ("rad"   , static_cast<double (*)(double)>(&math::rad))
            ("round" , static_cast<double (*)(double)>(&std::round))
            ("sgn"   , static_cast<double (*)(double)>(&math::sgn))
            ("sin"   , static_cast<double (*)(double)>(&math::sin))
            ("sinh"  , static_cast<double (*)(double)>(&std::sinh))
            ("sqrt"  , static_cast<double (*)(double)>(&math::sqrt))
            ("tan"   , static_cast<double (*)(double)>(&math::tan))
            ("tanh"  , static_cast<double (*)(double)>(&std::tanh))
            ("tgamma", static_cast<double (*)(double)>(&math::tgamma))
            ;
        // clang-format on
    }
} ufunc;

struct tfunc_ : x3::symbols<double (*)(double, double, double)> {
    tfunc_() {
        // clang-format off
        add
	  ("ifelse", static_cast<double (*)(double, double, double)>(&math::ifelse))
            ;
        // clang-format on
    }
} tfunc;

struct bfunc_ : x3::symbols<double (*)(double, double)> {
    bfunc_() {
        // clang-format off
        add
            ("atan2", static_cast<double (*)(double, double)>(&std::atan2))
            ("max"  , static_cast<double (*)(double, double)>(&std::fmax))
            ("min"  , static_cast<double (*)(double, double)>(&std::fmin))
            ("pow"  , static_cast<double (*)(double, double)>(&math::pow))
            ;
        // clang-format on
    }
} bfunc;

struct unary_op_ : x3::symbols<double (*)(double)> {
    unary_op_() {
        // clang-format off
        add
            ("+", static_cast<double (*)(double)>(&math::plus))
            ("-", static_cast<double (*)(double)>(&math::minus))
            ("!", static_cast<double (*)(double)>(&math::unary_not))
            ;
        // clang-format on
    }
} unary_op;

struct additive_op_ : x3::symbols<double (*)(double, double)> {
    additive_op_() {
        // clang-format off
        add
            ("+", static_cast<double (*)(double, double)>(&math::plus))
            ("-", static_cast<double (*)(double, double)>(&math::minus))
            ;
        // clang-format on
    }
} additive_op;

struct multiplicative_op_ : x3::symbols<double (*)(double, double)> {
    multiplicative_op_() {
        // clang-format off
        add
            ("*", static_cast<double (*)(double, double)>(&math::multiplies))
            ("/", static_cast<double (*)(double, double)>(&math::divides))
            ("%", static_cast<double (*)(double, double)>(&math::fmod))
            ;
        // clang-format on
    }
} multiplicative_op;

struct logical_op_ : x3::symbols<double (*)(double, double)> {
    logical_op_() {
        // clang-format off
        add
            ("&&", static_cast<double (*)(double, double)>(&math::logical_and))
            ("||", static_cast<double (*)(double, double)>(&math::logical_or))
            ;
        // clang-format on
    }
} logical_op;

struct relational_op_ : x3::symbols<double (*)(double, double)> {
    relational_op_() {
        // clang-format off
        add
            ("<" , static_cast<double (*)(double, double)>(&math::less))
            ("<=", static_cast<double (*)(double, double)>(&math::less_equals))
            (">" , static_cast<double (*)(double, double)>(&math::greater))
            (">=", static_cast<double (*)(double, double)>(&math::greater_equals))
            ;
        // clang-format on
    }
} relational_op;

struct equality_op_ : x3::symbols<double (*)(double, double)> {
    equality_op_() {
        // clang-format off
        add
            ("==", static_cast<double (*)(double, double)>(&math::equals))
            ("!=", static_cast<double (*)(double, double)>(&math::not_equals))
            ;
        // clang-format on
    }
} equality_op;

struct power_ : x3::symbols<double (*)(double, double)> {
    power_() {
        // clang-format off
        add
            ("**", static_cast<double (*)(double, double)>(&math::pow))
            ;
        // clang-format on
    }
} power;

// ADL markers

struct expression_class;
struct logical_class;
struct equality_class;
struct relational_class;
struct additive_class;
struct multiplicative_class;
struct factor_class;
struct primary_class;
struct unary_class;
struct binary_class;
struct ternary_class;
struct variable_class;

// clang-format off

// Rule declarations

auto const expression     = x3::rule<expression_class    , ast::expression>{"expression"};
auto const logical        = x3::rule<logical_class       , ast::expression>{"logical"};
auto const equality       = x3::rule<equality_class      , ast::expression>{"equality"};
auto const relational     = x3::rule<relational_class    , ast::expression>{"relational"};
auto const additive       = x3::rule<additive_class      , ast::expression>{"additive"};
auto const multiplicative = x3::rule<multiplicative_class, ast::expression>{"multiplicative"};
auto const factor         = x3::rule<factor_class        , ast::expression>{"factor"};
auto const primary        = x3::rule<primary_class       , ast::operand   >{"primary"};
auto const unary          = x3::rule<unary_class         , ast::unary_op  >{"unary"};
auto const binary         = x3::rule<binary_class        , ast::binary_op >{"binary"};
auto const ternary        = x3::rule<ternary_class       , ast::ternary_op>{"ternary"};
auto const variable       = x3::rule<variable_class      , std::string    >{"variable"};

// Rule defintions

auto const expression_def =
    logical
    ;

auto const logical_def =
    equality >> *(logical_op > equality)
    ;

auto const equality_def =
    relational >> *(equality_op > relational)
    ;

auto const relational_def =
    additive >> *(relational_op > additive)
    ;

auto const additive_def =
    multiplicative >> *(additive_op > multiplicative)
    ;

auto const multiplicative_def =
    factor >> *(multiplicative_op > factor)
    ;

auto const factor_def =
    primary >> *( power > factor )
    ;

auto const unary_def =
    ufunc > '(' > expression > ')'
    ;

auto const binary_def =
    bfunc > '(' > expression > ',' > expression > ')'
    ;

auto const ternary_def =
    tfunc > '(' > expression > ',' > expression > ',' > expression > ')'
    ;

auto const variable_def =
    x3::raw[x3::lexeme[x3::alpha >> *(x3::alnum | '_')]]
    ;

auto const primary_def =
      x3::double_
    | ('(' > expression > ')')
    | (unary_op > primary)
    | ternary
    | binary
    | unary
    | constant
    | variable
    ;

BOOST_SPIRIT_DEFINE(
    expression,
    logical,
    equality,
    relational,
    additive,
    multiplicative,
    factor,
    primary,
    unary,
    binary,
    ternary,
    variable
)

// clang-format on

struct expression_class {
    template <typename Iterator, typename Exception, typename Context>
    x3::error_handler_result on_error(Iterator &, Iterator const &last,
                                      Exception const &x, Context const &) {
        std::cout << "Expected " << x.which() << " at \""
                  << std::string{x.where(), last} << "\"" << std::endl;
        return x3::error_handler_result::fail;
    }
};

} // namespace parser

parser::expression_type grammar() { return parser::expression; }

} // namespace matheval


#include <random>
#include <Eigen/Dense>
#include "HODLR_Tree.hpp"
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <squaredeMat.hpp> //squared exponential kernel
#include <squaredeP1Mat.hpp>

using std::normal_distribution;
namespace py = pybind11;

// AZ: draw random samples from fstar
Eigen::MatrixXd predict(Eigen::MatrixXd X, Eigen::MatrixXd Y, Eigen::MatrixXd Xtest, Eigen::VectorXd sig_samps, Eigen::VectorXd rho_samps, Eigen::VectorXd tau_samps, double multiplier, int M, double tol, int nsamps) {
    /*
    Temporarily assumes regression = True
    */

  //////////// PASS THE GENERATOR //////////

    std::cout << "hullo there" << std::endl;

    // Create the standard normal generator
    normal_distribution<double> norm(0, 1);
    std::mt19937 rng;
    auto r_std_normal = bind(norm, rng);

    ////Number of observations in train and test
    int Ntest =  Xtest.rows();
    int N =  X.rows();
    int D =  X.cols();

    int n_levels = log(N / M) / log(2);
    bool is_sym = true;
    bool is_pd  = true;

    double tau;
    double rho;
    double sig;

    // Sigma square and SSR
    double sigsq; // = pow(sig, 2.0);
    double tmpSSR;

    // Allocate fstarsamp
    Eigen::MatrixXd fstarsamp(nsamps, Ntest);
    Eigen::VectorXd KobsNew(N);

    for (int s = 0; s < nsamps; s++) {

      sig = sig_samps(s);
      tau = tau_samps(s);
      rho = rho_samps(s);

      if (s % 100 == 0) {
        // printf("PREDICT: Sig, Tau, Rho: s = %d, %.2f \t%.2f \t%.2f\n", s, sig, tau, rho);
      }

      sigsq = pow(sig, 2.0);

      SQRExponentialP1_Kernel* L  = new SQRExponentialP1_Kernel(X, N, sig, rho, tau);
      HODLR_Tree* T = new HODLR_Tree(n_levels, tol, L); // With noise (i.e. Sigma + I/tau)
      T->assembleTree(is_sym, is_pd);
      T->factorize();

      // VO: Should be able to do this through matrix operations
      // AZ: cycle through the observations in the test set
      for (int i = 0; i < Ntest; i++) {
        ////// Sample a draw of the GP function f*|f,sig,rho,tau,x*.
        ////// Assume a squared exponential Gaussian Process with params sigf and rho,
        ////// based on observed function f at new test points x*.
        Eigen::RowVectorXd Xtest_i = Xtest.row(i);
        // Get covariance between X and Xtest
        for (int j = 0; j < N; j++) {
          Eigen::RowVectorXd tmp = X.row(j) - Xtest_i;
          tmpSSR = 0.0;
          for (int d = 0; d < D; d++) {
            tmpSSR = tmpSSR + pow(tmp(d), 2.0);
          }
          KobsNew(j) = sigsq * exp(- tmpSSR * rho);
        }
        // Get variance at Xtest
        double kNewNew = sigsq + 1e-8;
        // Get posterior mean and variance of f* at point xtest(i)
        double sdstar = pow(kNewNew - (multiplier * KobsNew.transpose() * T->solve(tau * KobsNew))(0,0), 0.5);
        double mustar = (multiplier * KobsNew.transpose() * T->solve(tau * Y))(0,0);

        auto normal_samp = r_std_normal();

        fstarsamp(s, i) =  sdstar * normal_samp + mustar;
        //std::normal_distribution<double> distribution(mustar, sdstar);
        //fstarsamp(i) = distribution(generator);
      }
    }
    return fstarsamp;
}

void predict_module(py::module &m) {
    m.def("predict_f", &predict, "predicted samples of f at new X");
}