/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2021 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
/**
 * @file   covariance.cpp
 * @brief  Covariance estimation methods for ICP results
 * @author Jose Luis Blanco Claraco
 * @date   Jun 9, 2020
 */

#include <mp2p_icp/covariance.h>
#include <mp2p_icp/errorTerms.h>
#include <mrpt/math/CVectorDynamic.h>
#include <mrpt/math/num_jacobian.h>

#include <Eigen/Dense>

using namespace mp2p_icp;

mrpt::math::CMatrixDouble66 mp2p_icp::covariance(
    const Pairings& in, const mrpt::poses::CPose3D& finalAlignSolution,
    const CovarianceParameters& param)
{
    // If we don't have pairings, we can't provide an estimation:
    if (in.empty())
    {
        mrpt::math::CMatrixDouble66 cov;
        cov.setDiagonal(1e6);
        return cov;
    }

    mrpt::math::CMatrixDouble61 xInitial;
    xInitial[0] = finalAlignSolution.x();
    xInitial[1] = finalAlignSolution.y();
    xInitial[0] = finalAlignSolution.x();
    xInitial[3] = finalAlignSolution.yaw();
    xInitial[4] = finalAlignSolution.pitch();
    xInitial[5] = finalAlignSolution.roll();

    mrpt::math::CMatrixDouble61 xIncrs;
    for (int i = 0; i < 3; i++) xIncrs[i] = param.finDif_xyz;
    for (int i = 0; i < 3; i++) xIncrs[3 + i] = param.finDif_angles;

    struct LambdaParams
    {
    };

    LambdaParams lmbParams;

    auto errorLambda = [&](const mrpt::math::CMatrixDouble61& x,
                           const LambdaParams&,
                           mrpt::math::CVectorDouble& err) {
        mrpt::poses::CPose3D pose;
        pose.setFromValues(x[0], x[1], x[2], x[3], x[4], x[5]);

        const auto nPt2Pt = in.paired_pt2pt.size();
        const auto nPt2Ln = in.paired_pt2ln.size();
        const auto nPt2Pl = in.paired_pt2pl.size();
        const auto nPl2Pl = in.paired_pl2pl.size();
        const auto nLn2Ln = in.paired_ln2ln.size();

        const auto nErrorTerms =
            (nPt2Pt + nPl2Pl + nPt2Ln + nPt2Pl) * 3 + nLn2Ln * 4;
        ASSERT_(nErrorTerms > 0);
        err.resize(nErrorTerms);

        // Point-to-point:
        for (size_t idx_pt = 0; idx_pt < nPt2Pt; idx_pt++)
        {
            // Error:
            const auto&                       p = in.paired_pt2pt[idx_pt];
            mrpt::math::CVectorFixedDouble<3> ret =
                mp2p_icp::error_point2point(p, pose);
            err.block<3, 1>(idx_pt * 3, 0) = ret.asEigen();
        }
        auto base_idx = nPt2Pt * 3;

        // Point-to-line
        for (size_t idx_pt = 0; idx_pt < nPt2Ln; idx_pt++)
        {
            // Error
            const auto&                       p = in.paired_pt2ln[idx_pt];
            mrpt::math::CVectorFixedDouble<3> ret =
                mp2p_icp::error_point2line(p, pose);
            err.block<3, 1>(base_idx + idx_pt * 3, 0) = ret.asEigen();
        }
        base_idx += nPt2Ln * 3;

        // Line-to-Line
        // Minimum angle to approach zero
        for (size_t idx_ln = 0; idx_ln < nLn2Ln; idx_ln++)
        {
            const auto&                       p = in.paired_ln2ln[idx_ln];
            mrpt::math::CVectorFixedDouble<4> ret =
                mp2p_icp::error_line2line(p, pose);
            err.block<4, 1>(base_idx + idx_ln * 4, 0) = ret.asEigen();
        }
        base_idx += nLn2Ln;

        // Point-to-plane:
        for (size_t idx_pl = 0; idx_pl < nPt2Pl; idx_pl++)
        {
            // Error:
            const auto&                       p = in.paired_pt2pl[idx_pl];
            mrpt::math::CVectorFixedDouble<3> ret =
                mp2p_icp::error_point2plane(p, pose);
            err.block<3, 1>(idx_pl * 3 + base_idx, 0) = ret.asEigen();
        }
        base_idx += nPt2Pl * 3;

        // Plane-to-plane (only direction of normal vectors):
        for (size_t idx_pl = 0; idx_pl < nPl2Pl; idx_pl++)
        {
            // Error term:
            const auto&                       p = in.paired_pl2pl[idx_pl];
            mrpt::math::CVectorFixedDouble<3> ret =
                mp2p_icp::error_plane2plane(p, pose);
            err.block<3, 1>(idx_pl * 3 + base_idx, 0) = ret.asEigen();
        }
    };

    // Do NOT use "Eigen::MatrixXd", it may have different alignment
    // requirements than MRPT matrices:
    mrpt::math::CMatrixDouble jacob;
    mrpt::math::estimateJacobian(
        xInitial,
        std::function<void(
            const mrpt::math::CMatrixDouble61&, const LambdaParams&,
            mrpt::math::CVectorDouble&)>(errorLambda),
        xIncrs, lmbParams, jacob);

    const mrpt::math::CMatrixDouble66 hessian(
        jacob.asEigen().transpose() * jacob.asEigen());

    const mrpt::math::CMatrixDouble66 cov = hessian.inverse_LLt();

    return cov;
}

// other ideas?
// See: http://censi.mit.edu/pub/research/2007-icra-icpcov-slides.pdf


#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/icp.h>

#include <Eigen/Dense>

#include "myndt.h"

//#include <math.h>

#include <stdio.h>

bool registerWithNormals(int npoints1, double *xyz1, double *normals1, int npoints2, double *xyz2, double *normals2, double *vec,
                         bool usemaxiter, int maxiter,
                         bool usemaxcorrdist, double maxcorrdist,
                         bool usetepsilon, double tepsilon,
                         bool usefepsilon, double fepsilon) {
  pcl::PointCloud<pcl::PointNormal>::Ptr cloud1 (new pcl::PointCloud<pcl::PointNormal>);
  pcl::PointCloud<pcl::PointNormal>::Ptr cloud2 (new pcl::PointCloud<pcl::PointNormal>);
  pcl::PointCloud<pcl::PointNormal>::Ptr final  (new pcl::PointCloud<pcl::PointNormal>);
  pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal>::Matrix4 transf;

  printf("doing cloud 1\n");
  cloud1->width = npoints1;
  cloud1->height = 1;
  cloud1->is_dense = false;
  cloud1->points.resize(npoints1);
  for (int i=0; i<npoints1; i++) {
    cloud1->points[i].x = *xyz1++;
    cloud1->points[i].y = *xyz1++;
    cloud1->points[i].z = *xyz1++;
    cloud1->points[i].normal[0] = *normals1++;
    cloud1->points[i].normal[1] = *normals1++;
    cloud1->points[i].normal[2] = *normals1++;
  }

  printf("doing cloud 2\n");
  cloud2->width = npoints2;
  cloud2->height = 1;
  cloud2->is_dense = false;
  cloud2->points.resize(npoints2);
  for (int i=0; i<npoints2; i++) {
    cloud2->points[i].x = *xyz2++;
    cloud2->points[i].y = *xyz2++;
    cloud2->points[i].z = *xyz2++;
    cloud2->points[i].normal[0] = *normals2++;
    cloud2->points[i].normal[1] = *normals2++;
    cloud2->points[i].normal[2] = *normals2++;
  }

  printf("after doing clouds\n");
  pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal> icp;

  if (usemaxiter) {
    icp.setMaximumIterations(maxiter);
  }
  if (usemaxcorrdist) {
    icp.setMaxCorrespondenceDistance(maxcorrdist);
  }
  if (usetepsilon) {
    icp.setTransformationEpsilon(tepsilon);
  }
  if (usefepsilon) {
    icp.setTransformationEpsilon(fepsilon);
  }

  printf("hola10\n");
  icp.setInputCloud(cloud1);
  icp.setInputTarget(cloud2);

  printf("hola11\n");
  icp.align(*final);
  printf("hola12\n");

  transf = icp.getFinalTransformation();
  for (int k=0;k<16;k++) {
    vec[k] = transf.data()[k];
  }

  return icp.hasConverged();

}

double executeNDT(pcl::PointCloud<pcl::PointXYZ>::Ptr &source, pcl::PointCloud<pcl::PointXYZ>::Ptr &target, //pcl::PointCloud<pcl::PointXYZ> *output,
                bool useepsilon, double epsilon,
                bool usestep, double step,
                bool useresolution, double resolution,
                bool useiters, double maxiters, double *vec) {
  // Initializing Normal Distributions Transform (NDT).
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Matrix4 transf;
  pcl::PointCloud<pcl::PointXYZ> output;
//  Eigen::Matrix4f identity;
//
//  for (int k=0;k<16;k++) {
//    identity.data()[k] = 0.0;
//  }
//  for (int k=0;k<16;k+=5) {
//    identity.data()[k] = 1.0;
//  }

  // Setting scale dependent NDT parameters
  if (useepsilon) {
    // Setting minimum transformation difference for termination condition.
    //ndt.setTransformationEpsilon (0.01);
    ndt.setTransformationEpsilon (epsilon);
  }
  if (usestep) {
    // Setting maximum step size for More-Thuente line search.
    //ndt.setStepSize (0.1);
    ndt.setStepSize (step);
  }
  if (useresolution) {
    //Setting Resolution of NDT grid structure (VoxelGridCovariance).
    //ndt.setResolution (1.0);
    ndt.setResolution (resolution);
  }
  if (useiters) {
    // Setting max number of registration iterations.
    //ndt.setMaximumIterations (35);
    ndt.setMaximumIterations (maxiters);
  }

  // Setting point cloud to be aligned.
  ndt.setInputSource (source);
  // Setting point cloud to be aligned to.
  ndt.setInputTarget (target);

  printf("hola11\n");
  ndt.align (output);//, identity);//, init_guess);
  printf("hola2\n");

  transf = ndt.getFinalTransformation();
  for (int k=0;k<16;k++) {
    vec[k] = transf.data()[k];
  }

  return ndt.getTransformationProbability();

}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, 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/poses/CPose2DInterpolator.h>
#include <mrpt/serialization/stl_serialization.h>
#include <Eigen/Dense>
#include "CPoseInterpolatorBase.hpp"  // templ impl

using namespace mrpt::poses;

IMPLEMENTS_SERIALIZABLE(CPose2DInterpolator, CSerializable, mrpt::poses)

uint8_t CPose2DInterpolator::serializeGetVersion() const { return 0; }
void CPose2DInterpolator::serializeTo(mrpt::serialization::CArchive& out) const
{
	out << m_path;
}
void CPose2DInterpolator::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		{
			in >> m_path;
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

namespace mrpt::poses
{
// Specialization for DIM=2
template <>
void CPoseInterpolatorBase<2>::impl_interpolation(
	const TTimePosePair& p1, const TTimePosePair& p2, const TTimePosePair& p3,
	const TTimePosePair& p4, const TInterpolatorMethod method,
	const mrpt::Clock::time_point& t, pose_t& out_interp) const
{
	using mrpt::math::TPose2D;
	using doubleDuration = std::chrono::duration<double>;
	doubleDuration durationT(t.time_since_epoch());
	double td = durationT.count();
	mrpt::math::CVectorFixedDouble<4> ts;
	ts[0] =
		std::chrono::duration_cast<doubleDuration>(p1.first.time_since_epoch())
			.count();
	ts[1] =
		std::chrono::duration_cast<doubleDuration>(p2.first.time_since_epoch())
			.count();
	ts[2] =
		std::chrono::duration_cast<doubleDuration>(p3.first.time_since_epoch())
			.count();
	ts[3] =
		std::chrono::duration_cast<doubleDuration>(p4.first.time_since_epoch())
			.count();

	mrpt::math::CVectorFixedDouble<4> X, Y, yaw;
	X[0] = p1.second.x;
	Y[0] = p1.second.y;
	yaw[0] = p1.second.phi;
	X[1] = p2.second.x;
	Y[1] = p2.second.y;
	yaw[1] = p2.second.phi;
	X[2] = p3.second.x;
	Y[2] = p3.second.y;
	yaw[2] = p3.second.phi;
	X[3] = p4.second.x;
	Y[3] = p4.second.y;
	yaw[3] = p4.second.phi;

	unwrap2PiSequence(yaw);

	// Target interpolated values:
	switch (method)
	{
		case imSpline:
		{
			// ---------------------------------------
			//    SPLINE INTERPOLATION
			// ---------------------------------------
			out_interp.x = math::spline(td, ts, X);
			out_interp.y = math::spline(td, ts, Y);
			out_interp.phi = math::spline(td, ts, yaw, true);  // Wrap 2pi
		}
		break;

		case imLinear2Neig:
		{
			out_interp.x =
				math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
			out_interp.y =
				math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
			out_interp.phi = math::interpolate2points(
				td, ts[1], yaw[1], ts[2], yaw[2], true);  // Wrap 2pi
		}
		break;

		case imLinear4Neig:
		{
			out_interp.x =
				math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, X);
			out_interp.y =
				math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
			out_interp.phi =
				math::leastSquareLinearFit<double, decltype(ts), 4>(
					td, ts, yaw, true);  // Wrap 2pi
		}
		break;

		case imSSLLLL:
		{
			out_interp.x = math::spline(td, ts, X);
			out_interp.y = math::spline(td, ts, Y);
			out_interp.phi =
				math::leastSquareLinearFit<double, decltype(ts), 4>(
					td, ts, yaw, true);  // Wrap 2pi
		}
		break;

		case imSSLSLL:
		{
			out_interp.x = math::spline(td, ts, X);
			out_interp.y = math::spline(td, ts, Y);
			out_interp.phi = math::spline(td, ts, yaw, true);  // Wrap 2pi
		}
		break;

		case imLinearSlerp:
		{
			const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
			const double Aang = mrpt::math::angDistance(yaw[1], yaw[2]);
			out_interp.phi = yaw[1] + ratio * Aang;

			out_interp.x =
				math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
			out_interp.y =
				math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
		}
		break;

		case imSplineSlerp:
		{
			const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
			const double Aang = mrpt::math::angDistance(yaw[1], yaw[2]);
			out_interp.phi = yaw[1] + ratio * Aang;

			out_interp.x = math::spline(td, ts, X);
			out_interp.y = math::spline(td, ts, Y);
		}
		break;

		default:
			THROW_EXCEPTION("Unknown value for interpolation method!");
	};  // end switch
}

// Explicit instantations:
template class CPoseInterpolatorBase<2>;
}  // namespace mrpt::poses


#ifndef CX_DQMC_CHECKERBOARD_HPP
#define CX_DQMC_CHECKERBOARD_HPP

#include "la.hpp"
#include "parameters.hpp"

#include <boost/multi_array.hpp>

#include <exception>
#include <cmath>
#include <memory>

namespace cx_dqmc {
    namespace checkerboard {

	inline void graph_to_checkerboard(dqmc::parameters& p, cx_dqmc::workspace& ws) {
	    for (int i = 0; i < p.graph.num_bonds(); ++i) {
		ws.bond_used[i] = -1;
	    }

	    int bonds_used = 0;
	    int sites_used = 0;
	    int first_unused_bond = 0;

	    alps::graph_helper<>::bond_iterator itr1, itr1_end;
	    int b, s1, s2;
	    double t, im_t;
	    while (bonds_used < p.graph.num_bonds()) {
		sp_mat hopping(p.N, p.N);
		sp_mat hopping_inv(p.N, p.N);

		cx_sp_mat cx_hopping(p.N, p.N);
		cx_sp_mat cx_hopping_inv(p.N, p.N);

		int elements = 0;

		for (int i = 0; i < p.N; ++i) { ws.site_used[i] = -1; }

		for (boost::tie(itr1, itr1_end) = p.graph.bonds(); itr1 != itr1_end; ++itr1) {
		    b = p.graph.index(*itr1);
		    s1 = p.graph.source(*itr1);
		    s2 = p.graph.target(*itr1);

		    if (ws.bond_used[b] == 1) continue;
		    if (ws.site_used[s1] == 1 || ws.site_used[s2] == 1) continue;

		    ws.bond_used[b] = 1;
		    ws.site_used[s1] = 1;
		    ws.site_used[s2] = 1;
		    t = p.ts[p.graph.bond_type(*itr1)];

		    if (p.complex_hoppings == false) {
			hopping.insert(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping.insert(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping.insert(s1, s2) = sinh(t * p.delta_tau/2.);
			hopping.insert(s2, s1) = sinh(t * p.delta_tau/2.);

			hopping_inv.insert(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping_inv.insert(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping_inv.insert(s1, s2) = -sinh(t * p.delta_tau/2.);
			hopping_inv.insert(s2, s1) = -sinh(t * p.delta_tau/2.);
		    } else {
			im_t = p.im_ts[p.graph.bond_type(*itr1)];
			std::complex<double> cx_t(t, im_t);
			std::complex<double> abs_t(sqrt(t*t + im_t*im_t), 0);
			cx_hopping.insert(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping.insert(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping.insert(s1, s2) = sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t;
			cx_hopping.insert(s2, s1) = sinh(abs_t * p.delta_tau/2.) * std::conj(cx_t / abs_t);

			cx_hopping_inv.insert(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_inv.insert(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_inv.insert(s1, s2) = -(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);
			cx_hopping_inv.insert(s2, s1) = -sinh(abs_t * p.delta_tau/2.) * std::conj(cx_t / abs_t);
		    }
		    // std::cout << "Using " << s1 << " and " << s2 << std::endl;
		    ++bonds_used;
		    ++elements;
		}

		if (elements != 0) {
		    for (int i = 0; i < p.N; ++i) {
			if(ws.site_used[i] == -1) {
			    if (p.complex_hoppings == false) {
				hopping.insert(i, i) = 1.;
				hopping_inv.insert(i, i) = 1.;
			    } else {
				cx_hopping.insert(i, i) = std::complex<double>(1., 0.);
				cx_hopping_inv.insert(i, i) = std::complex<double>(1., 0.);
			    }
			}
		    }
		    // arma::mat(hopping).print("A hopping matrix");

		    if (p.complex_hoppings == false) {
			ws.sparse_hoppings.push_back(hopping);
			ws.sparse_hoppings_inv.push_back(hopping_inv);
		    } else {
			ws.cx_sparse_hoppings.push_back(cx_hopping);
			ws.cx_sparse_hoppings_inv.push_back(cx_hopping_inv);
		    }
		}
	    }
	}

	inline void graph_to_checkerboard_renyi(dqmc::parameters& p, cx_dqmc::workspace& ws) {
	    for (int i = 0; i < p.graph.num_bonds(); ++i) { ws.bond_used[i] = -1; }

	    int bonds_used = 0;
	    int sites_used = 0;
	    int first_unused_bond = 0;

	    alps::graph_helper<>::bond_iterator itr1, itr1_end;
	    int b, s1, s2, s1a, s2a;
	    double t;
	    std::complex<double> one(1., 0.);

	    while (bonds_used < p.graph.num_bonds()) {
		sp_mat hopping_0(ws.vol, ws.vol), hopping_0_inv(ws.vol, ws.vol),
		    hopping_1(ws.vol, ws.vol), hopping_1_inv(ws.vol, ws.vol),
		    hopping_2(ws.vol, ws.vol), hopping_2_inv(ws.vol, ws.vol),
		    hopping_3(ws.vol, ws.vol), hopping_3_inv(ws.vol, ws.vol);

		cx_sp_mat cx_hopping_0(ws.vol, ws.vol), cx_hopping_0_inv(ws.vol, ws.vol),
		    cx_hopping_1(ws.vol, ws.vol), cx_hopping_1_inv(ws.vol, ws.vol),
		    cx_hopping_2(ws.vol, ws.vol), cx_hopping_2_inv(ws.vol, ws.vol),
		    cx_hopping_3(ws.vol, ws.vol), cx_hopping_3_inv(ws.vol, ws.vol);

		int elements = 0;

		for (int i = 0; i < p.N; ++i) ws.site_used[i] = -1;
		for (int i = 0; i < p.N + p.n_B; ++i) {
		    if (p.complex_hoppings == false) {
			hopping_0.insert(i, i) = 1.;
			hopping_0_inv.insert(i, i) = 1.;
			hopping_1.insert(i, i) = 1.;
			hopping_1_inv.insert(i, i) = 1.;
			hopping_2.insert(i, i) = 1.;
			hopping_2_inv.insert(i, i) = 1.;
			hopping_3.insert(i, i) = 1.;
			hopping_3_inv.insert(i, i) = 1.;
		    } else {
			cx_hopping_0.insert(i, i) = one;
			cx_hopping_0_inv.insert(i, i) = one;
			cx_hopping_1.insert(i, i) = one;
			cx_hopping_1_inv.insert(i, i) = one;
			cx_hopping_2.insert(i, i) = one;
			cx_hopping_2_inv.insert(i, i) = one;
			cx_hopping_3.insert(i, i) = one;
			cx_hopping_3_inv.insert(i, i) = one;
		    }
		}

		for (boost::tie(itr1, itr1_end) = p.graph.bonds(); itr1 != itr1_end; ++itr1) {
		    b = p.graph.index(*itr1);
		    s1 = p.graph.source(*itr1);
		    s2 = p.graph.target(*itr1);
		    s1a = s1;
		    s2a = s2;

		    if (ws.bond_used[b] == 1) continue;
		    if (ws.site_used[s1] == 1 || ws.site_used[s2] == 1) continue;

		    ws.bond_used[b] = 1;
		    ws.site_used[s1] = 1;
		    ws.site_used[s2] = 1;

		    t = p.ts[p.graph.bond_type(*itr1)];

		    if (s1 >= p.n_A) s1a = s1 + p.n_B;
		    if (s2 >= p.n_A) s2a = s2 + p.n_B;

		    if (p.complex_hoppings == false) {
			hopping_0.coeffRef(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping_0.coeffRef(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping_0.coeffRef(s1, s2) = sinh(t * p.delta_tau/2.);
			hopping_0.coeffRef(s2, s1) = sinh(t * p.delta_tau/2.);

			hopping_0_inv.coeffRef(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping_0_inv.coeffRef(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping_0_inv.coeffRef(s1, s2) = -sinh(t * p.delta_tau/2.);
			hopping_0_inv.coeffRef(s2, s1) = -sinh(t * p.delta_tau/2.);

			hopping_1.coeffRef(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping_1.coeffRef(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping_1.coeffRef(s1, s2) = sinh(t * p.delta_tau/2.);
			hopping_1.coeffRef(s2, s1) = sinh(t * p.delta_tau/2.);

			hopping_1_inv.coeffRef(s1, s1) = cosh(t * p.delta_tau/2.);
			hopping_1_inv.coeffRef(s2, s2) = cosh(t * p.delta_tau/2.);
			hopping_1_inv.coeffRef(s1, s2) = -sinh(t * p.delta_tau/2.);
			hopping_1_inv.coeffRef(s2, s1) = -sinh(t * p.delta_tau/2.);

			hopping_2.coeffRef(s1a, s1a) = cosh(t * p.delta_tau/2.);
			hopping_2.coeffRef(s2a, s2a) = cosh(t * p.delta_tau/2.);
			hopping_2.coeffRef(s1a, s2a) = sinh(t * p.delta_tau/2.);
			hopping_2.coeffRef(s2a, s1a) = sinh(t * p.delta_tau/2.);

			hopping_2_inv.coeffRef(s1a, s1a) = cosh(t * p.delta_tau/2.);
			hopping_2_inv.coeffRef(s2a, s2a) = cosh(t * p.delta_tau/2.);
			hopping_2_inv.coeffRef(s1a, s2a) = -sinh(t * p.delta_tau/2.);
			hopping_2_inv.coeffRef(s2a, s1a) = -sinh(t * p.delta_tau/2.);

			hopping_3.coeffRef(s1a, s1a) = cosh(t * p.delta_tau/2.);
			hopping_3.coeffRef(s2a, s2a) = cosh(t * p.delta_tau/2.);
			hopping_3.coeffRef(s1a, s2a) = sinh(t * p.delta_tau/2.);
			hopping_3.coeffRef(s2a, s1a) = sinh(t * p.delta_tau/2.);

			hopping_3_inv.coeffRef(s1a, s1a) = cosh(t * p.delta_tau/2.);
			hopping_3_inv.coeffRef(s2a, s2a) = cosh(t * p.delta_tau/2.);
			hopping_3_inv.coeffRef(s1a, s2a) = -sinh(t * p.delta_tau/2.);
			hopping_3_inv.coeffRef(s2a, s1a) = -sinh(t * p.delta_tau/2.);
		    } else {
			double im_t = p.im_ts[p.graph.bond_type(*itr1)];
			std::complex<double> cx_t(t, im_t);
			std::complex<double> abs_t(sqrt(t*t + im_t*im_t), 0);
			cx_hopping_0.coeffRef(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_0.coeffRef(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_0.coeffRef(s1, s2) = sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t;
			cx_hopping_0.coeffRef(s2, s1) = sinh(abs_t * p.delta_tau/2.) * std::conj(cx_t / abs_t);

			cx_hopping_0_inv.coeffRef(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_0_inv.coeffRef(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_0_inv.coeffRef(s1, s2) = -(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t ) ;
			cx_hopping_0_inv.coeffRef(s2, s1) = -sinh(abs_t * p.delta_tau/2.) * std::conj(cx_t / abs_t );

			cx_hopping_1.coeffRef(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_1.coeffRef(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_1.coeffRef(s1, s2) = sinh(abs_t * p.delta_tau/2.)  * cx_t / abs_t;
			cx_hopping_1.coeffRef(s2, s1) = std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);

			cx_hopping_1_inv.coeffRef(s1, s1) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_1_inv.coeffRef(s2, s2) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_1_inv.coeffRef(s1, s2) = -(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);
			cx_hopping_1_inv.coeffRef(s2, s1) = -std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);

			cx_hopping_2.coeffRef(s1a, s1a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_2.coeffRef(s2a, s2a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_2.coeffRef(s1a, s2a) = sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t;
			cx_hopping_2.coeffRef(s2a, s1a) = std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);

			cx_hopping_2_inv.coeffRef(s1a, s1a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_2_inv.coeffRef(s2a, s2a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_2_inv.coeffRef(s1a, s2a) = -(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);
			cx_hopping_2_inv.coeffRef(s2a, s1a) = -std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);

			cx_hopping_3.coeffRef(s1a, s1a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_3.coeffRef(s2a, s2a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_3.coeffRef(s1a, s2a) = sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t;
			cx_hopping_3.coeffRef(s2a, s1a) = std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);

			cx_hopping_3_inv.coeffRef(s1a, s1a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_3_inv.coeffRef(s2a, s2a) = cosh(abs_t * p.delta_tau/2.);
			cx_hopping_3_inv.coeffRef(s1a, s2a) = -(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);
			cx_hopping_3_inv.coeffRef(s2a, s1a) = -std::conj(sinh(abs_t * p.delta_tau/2.) * cx_t / abs_t);
		    }
		    ++bonds_used;
		    ++elements;
		}

		if (elements != 0) {
		    if (p.complex_hoppings == false) {
			ws.sparse_hoppings_0.push_back(hopping_0);
			ws.sparse_hoppings_0_inv.push_back(hopping_0_inv);

			ws.sparse_hoppings_1.push_back(hopping_1);
			ws.sparse_hoppings_1_inv.push_back(hopping_1_inv);

			ws.sparse_hoppings_2.push_back(hopping_2);
			ws.sparse_hoppings_2_inv.push_back(hopping_2_inv);

			ws.sparse_hoppings_3.push_back(hopping_3);
			ws.sparse_hoppings_3_inv.push_back(hopping_3_inv);
		    } else {
			ws.cx_sparse_hoppings_0.push_back(cx_hopping_0);
			ws.cx_sparse_hoppings_0_inv.push_back(cx_hopping_0_inv);

			ws.cx_sparse_hoppings_1.push_back(cx_hopping_1);
			ws.cx_sparse_hoppings_1_inv.push_back(cx_hopping_1_inv);

			ws.cx_sparse_hoppings_2.push_back(cx_hopping_2);
			ws.cx_sparse_hoppings_2_inv.push_back(cx_hopping_2_inv);

			ws.cx_sparse_hoppings_3.push_back(cx_hopping_3);
			ws.cx_sparse_hoppings_3_inv.push_back(cx_hopping_3_inv);
		    }
		}
	    }
	}

	inline void hop_left(cx_dqmc::workspace * ws, cx_mat_t& M, double pref) {
	    using namespace std;
	    int par = 0;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings[i] * M;
			else
			    M = ws->sparse_hoppings[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings[i] * M;
			else
			    M = ws->sparse_hoppings[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings_inv[i] * M;
			else
			    M = ws->sparse_hoppings_inv[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings_inv[i] * M;
			else
			    M = ws->sparse_hoppings_inv[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings[i] * M;
			else
			    M = ws->cx_sparse_hoppings[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings[i] * M;
			else
			    M = ws->cx_sparse_hoppings[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings_inv[i] * M;
			else
			    M = ws->cx_sparse_hoppings_inv[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings_inv[i] * M;
			else
			    M = ws->cx_sparse_hoppings_inv[i] * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }

	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_left_t(cx_dqmc::workspace * ws, cx_mat_t& M, double pref) {
	    using namespace std;
	    int par = 0;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings[i].transpose() * M;
			else
			    M = ws->sparse_hoppings[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings[i].transpose() * M;
			else
			    M = ws->sparse_hoppings[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings_inv[i].transpose() * M;
			else
			    M = ws->sparse_hoppings_inv[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->sparse_hoppings_inv[i].transpose() * M;
			else
			    M = ws->sparse_hoppings_inv[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
	    } else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings[i].transpose() * M;
			else
			    M = ws->cx_sparse_hoppings[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings[i].transpose() * M;
			else
			    M = ws->cx_sparse_hoppings[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings_inv[i].transpose() * M;
			else
			    M = ws->cx_sparse_hoppings_inv[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = ws->cx_sparse_hoppings_inv[i].transpose() * M;
			else
			    M = ws->cx_sparse_hoppings_inv[i].transpose() * ws->hop_temp;
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }

	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_right(cx_dqmc::workspace * ws, cx_mat_t& M, double pref) {
	    int par = 0;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = M * ws->sparse_hoppings[i];
			else
			    M = ws->hop_temp * ws->sparse_hoppings[i];
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = M * ws->sparse_hoppings[i];
			else
			    M = ws->hop_temp * ws->sparse_hoppings[i];
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = M * ws->sparse_hoppings_inv[i];
			else
			    M = ws->hop_temp * ws->sparse_hoppings_inv[i];
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = M * ws->sparse_hoppings_inv[i];
			else
			    M = ws->hop_temp * ws->sparse_hoppings_inv[i];
			par = (par == 0) ? 1 : 0;
		    }
		}
	    } else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = M * ws->cx_sparse_hoppings[i];
			else
			    M = ws->hop_temp * ws->cx_sparse_hoppings[i];
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = M * ws->cx_sparse_hoppings[i];
			else
			    M = ws->hop_temp * ws->cx_sparse_hoppings[i];
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings.size(); ++i) {
			if (par == 0)
			    ws->hop_temp = M * ws->cx_sparse_hoppings_inv[i];
			else
			    M = ws->hop_temp * ws->cx_sparse_hoppings_inv[i];
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings.size() - 1; i >=0; --i) {
			if (par == 0)
			    ws->hop_temp = M * ws->cx_sparse_hoppings_inv[i];
			else
			    M = ws->hop_temp * ws->cx_sparse_hoppings_inv[i];
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_left_renyi(cx_dqmc::workspace * ws,
				   cx_mat_t& M,
				   double pref,
				   int section) {
	    using namespace std;
	    int par = 0;
	    // cout << "section " << section << endl;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i] * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i] * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i] * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i] * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0[i] * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1[i] * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2[i] * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i] * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i] * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i] * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i] * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0[i] * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1[i] * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2[i] * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i] * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i] * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i] * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i] * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0_inv[i] * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1_inv[i] * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2_inv[i] * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3_inv[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i] * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i] * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i] * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i] * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0_inv[i] * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1_inv[i] * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2_inv[i] * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3_inv[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    } else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0[i] * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1[i] * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2[i] * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3[i] * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0[i] * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1[i] * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2[i] * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0[i] * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1[i] * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2[i] * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3[i] * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0[i] * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1[i] * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2[i] * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0_inv[i] * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1_inv[i] * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2_inv[i] * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3_inv[i] * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0_inv[i] * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1_inv[i] * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2_inv[i] * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3_inv[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0_inv[i] * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1_inv[i] * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2_inv[i] * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3_inv[i] * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0_inv[i] * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1_inv[i] * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2_inv[i] * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3_inv[i] * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_left_renyi_t(cx_dqmc::workspace * ws,
				   cx_mat_t& M,
				   double pref,
				   int section) {
	    using namespace std;
	    int par = 0;
	    // cout << "section " << section << endl;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0_inv[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1_inv[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2_inv[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3_inv[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->sparse_hoppings_0_inv[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->sparse_hoppings_1_inv[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->sparse_hoppings_2_inv[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->sparse_hoppings_3_inv[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    } else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0_inv[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1_inv[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2_inv[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3_inv[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0_inv[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1_inv[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2_inv[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3_inv[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }

		    // cx_dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = ws->cx_sparse_hoppings_0_inv[i].transpose() * M;
			    if (section == 1) ws->hop_temp = ws->cx_sparse_hoppings_1_inv[i].transpose() * M;
			    if (section == 2) ws->hop_temp = ws->cx_sparse_hoppings_2_inv[i].transpose() * M;
			    if (section == 3) ws->hop_temp = ws->cx_sparse_hoppings_3_inv[i].transpose() * M;
			} else {
			    if (section == 0) M = ws->cx_sparse_hoppings_0_inv[i].transpose() * ws->hop_temp ;
			    if (section == 1) M = ws->cx_sparse_hoppings_1_inv[i].transpose() * ws->hop_temp ;
			    if (section == 2) M = ws->cx_sparse_hoppings_2_inv[i].transpose() * ws->hop_temp ;
			    if (section == 3) M = ws->cx_sparse_hoppings_3_inv[i].transpose() * ws->hop_temp ;
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_right_renyi(cx_dqmc::workspace * ws,
				    cx_mat_t&__restrict__ M,
				    double pref, int section) {
	    int par = 0;

	    if (ws->complex_hoppings == false) {
		if (pref > 0) {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0[i];
			    if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1[i];
			    if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2[i];
			    if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0[i];
			    if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1[i];
			    if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2[i];
			    if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3[i];
			}
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0[i];
			    if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1[i];
			    if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2[i];
			    if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0[i];
			    if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1[i];
			    if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2[i];
			    if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3[i];
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0_inv[i];
			    if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1_inv[i];
			    if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2_inv[i];
			    if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3_inv[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0_inv[i];
			    if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1_inv[i];
			    if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2_inv[i];
			    if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3_inv[i];
			}
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0_inv[i];
			    if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1_inv[i];
			    if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2_inv[i];
			    if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3_inv[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0_inv[i];
			    if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1_inv[i];
			    if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2_inv[i];
			    if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3_inv[i];
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    else {
		if (pref > 0) {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->cx_sparse_hoppings_0[i];
			    if (section == 1) ws->hop_temp = M * ws->cx_sparse_hoppings_1[i];
			    if (section == 2) ws->hop_temp = M * ws->cx_sparse_hoppings_2[i];
			    if (section == 3) ws->hop_temp = M * ws->cx_sparse_hoppings_3[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->cx_sparse_hoppings_0[i];
			    if (section == 1) M = ws->hop_temp * ws->cx_sparse_hoppings_1[i];
			    if (section == 2) M = ws->hop_temp * ws->cx_sparse_hoppings_2[i];
			    if (section == 3) M = ws->hop_temp * ws->cx_sparse_hoppings_3[i];
			}
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->cx_sparse_hoppings_0[i];
			    if (section == 1) ws->hop_temp = M * ws->cx_sparse_hoppings_1[i];
			    if (section == 2) ws->hop_temp = M * ws->cx_sparse_hoppings_2[i];
			    if (section == 3) ws->hop_temp = M * ws->cx_sparse_hoppings_3[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->cx_sparse_hoppings_0[i];
			    if (section == 1) M = ws->hop_temp * ws->cx_sparse_hoppings_1[i];
			    if (section == 2) M = ws->hop_temp * ws->cx_sparse_hoppings_2[i];
			    if (section == 3) M = ws->hop_temp * ws->cx_sparse_hoppings_3[i];
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
		else {
		    for (int i = 0; i < ws->cx_sparse_hoppings_0.size(); ++i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->cx_sparse_hoppings_0_inv[i];
			    if (section == 1) ws->hop_temp = M * ws->cx_sparse_hoppings_1_inv[i];
			    if (section == 2) ws->hop_temp = M * ws->cx_sparse_hoppings_2_inv[i];
			    if (section == 3) ws->hop_temp = M * ws->cx_sparse_hoppings_3_inv[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->cx_sparse_hoppings_0_inv[i];
			    if (section == 1) M = ws->hop_temp * ws->cx_sparse_hoppings_1_inv[i];
			    if (section == 2) M = ws->hop_temp * ws->cx_sparse_hoppings_2_inv[i];
			    if (section == 3) M = ws->hop_temp * ws->cx_sparse_hoppings_3_inv[i];
			}
			par = (par == 0) ? 1 : 0;
		    }

		    for (int i = ws->cx_sparse_hoppings_0.size() - 1; i >=0; --i) {
			if (par == 0) {
			    if (section == 0) ws->hop_temp = M * ws->cx_sparse_hoppings_0_inv[i];
			    if (section == 1) ws->hop_temp = M * ws->cx_sparse_hoppings_1_inv[i];
			    if (section == 2) ws->hop_temp = M * ws->cx_sparse_hoppings_2_inv[i];
			    if (section == 3) ws->hop_temp = M * ws->cx_sparse_hoppings_3_inv[i];
			}
			else {
			    if (section == 0) M = ws->hop_temp * ws->cx_sparse_hoppings_0_inv[i];
			    if (section == 1) M = ws->hop_temp * ws->cx_sparse_hoppings_1_inv[i];
			    if (section == 2) M = ws->hop_temp * ws->cx_sparse_hoppings_2_inv[i];
			    if (section == 3) M = ws->hop_temp * ws->cx_sparse_hoppings_3_inv[i];
			}
			par = (par == 0) ? 1 : 0;
		    }
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}
    }
}
#endif


#include <armadillo>
#include <boost/program_options.hpp>
#include <HSMM.hpp>
#include <json.hpp>
#include <ProMPs_emission.hpp>

using namespace arma;
using namespace hsmm;
using namespace robotics;
using namespace std;
using json = nlohmann::json;
namespace po = boost::program_options;

field<mat> fromMatToField(const mat& obs) {
    field<mat> ret(obs.n_cols);
    for(int i = 0; i < obs.n_cols; i++)
        ret(i) = obs.col(i);
    return ret;
}

// Running the Viterbi algorithm.
void ViterbiAlgorithm(HSMM& promp_hsmm, const field<field<mat>>& seq_obs,
        string filename) {
    int nseq = seq_obs.n_elem;
    for(int s = 0; s < nseq; s++) {
        const field<mat>& obs = seq_obs(s);
        int nstates = promp_hsmm.nstates_;
        int nobs = obs.n_elem;
        imat psi_duration(nstates, nobs, fill::zeros);
        imat psi_state(nstates, nobs, fill::zeros);
        mat delta(nstates, nobs, fill::zeros);
        cout << "Before pdf" << endl;
        cube log_pdf = promp_hsmm.computeEmissionsLogLikelihood(obs);
        cout << "After pdf" << endl;
        Viterbi(promp_hsmm.transition_, promp_hsmm.pi_, promp_hsmm.duration_,
                log_pdf, delta, psi_duration, psi_state,
                promp_hsmm.min_duration_, nobs);
        cout << "Delta last column" << endl;
        cout << delta.col(nobs - 1) << endl;
        ivec viterbiStates, viterbiDurations;
        viterbiPath(psi_duration, psi_state, delta, viterbiStates,
                viterbiDurations);
        cout << "Viterbi states and durations" << endl;
        imat states_and_durations = join_horiz(viterbiStates, viterbiDurations);
        cout << states_and_durations << endl;
        cout << "Python states list representation" << endl;
        cout << "[";
        for(int i = 0; i < viterbiStates.n_elem; i++)
            cout << viterbiStates[i] <<
                    ((i + 1 == viterbiStates.n_elem)? "]" : ",");
        cout << endl;
        cout << "Python duration list representation" << endl;
        cout << "[";
        for(int i = 0; i < viterbiDurations.n_elem; i++)
            cout << viterbiDurations[i] <<
                    ((i + 1 == viterbiDurations.n_elem)? "]" : ",");
        cout << endl;

        // Saving the matrix of joint states and durations.
        string viterbi_filename(filename);
        if (nseq > 1)
            viterbi_filename += string(".") + to_string(s);
        states_and_durations.save(viterbi_filename, raw_ascii);
    }
}

int main(int argc, char *argv[]) {
    po::options_description desc("Options");
    desc.add_options()
        ("help,h", "Produce help message")
        ("input,i", po::value<string>(), "Path to the input obs")
        ("params,p", po::value<string>(), "Path to the json input params")
        ("output,o", po::value<string>(), "Path to the output viterbi file(s)")
        ("polybasisfun", po::value<int>()->default_value(1), "Order of the "
                "poly basis functions")
	    ("norbf", "Flag to deactivate the radial basis functions")
        ("nsequences,n", po::value<int>()->default_value(1),
                "Number of sequences used for training");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    if (!vm.count("input") || !vm.count("output") || !vm.count("params")) {
        cerr << "Error: You should provide input and output files" << endl;
        return 1;
    }
    string input_filename = vm["input"].as<string>();
    string output_filename = vm["output"].as<string>();
    string params = vm["params"].as<string>();
    int nseq = vm["nsequences"].as<int>();

    field<field<mat>> seq_obs(nseq);
    int njoints;
    for(int i = 0; i < nseq; i++) {
        string name = input_filename;
        if (nseq != 1)
            name += string(".") + to_string(i);
        mat obs;
        obs.load(name, raw_ascii);
        ifstream input_params_file(params);
        json input_params;
        input_params_file >> input_params;
        njoints = obs.n_rows;
        int nobs = obs.n_cols;
        cout << "Time series shape: (" << njoints << ", " << nobs << ")." << endl;
        seq_obs(i) = fromMatToField(obs);
    }

    ifstream input_params_file(params);
    json input_params;
    input_params_file >> input_params;
    int min_duration = input_params["min_duration"];
    int nstates = input_params["nstates"];
    int ndurations = input_params["ndurations"];
    mat transition(nstates, nstates);
    transition.fill(1.0 / (nstates - 1));
    transition.diag().zeros(); // No self-transitions.
    vec pi(nstates);
    pi.fill(1.0/nstates);
    mat durations(nstates, ndurations);
    durations.fill(1.0 / ndurations);

    // Setting a combination of polynomial and rbf basis functions.
    auto rbf = shared_ptr<ScalarGaussBasis>(new ScalarGaussBasis(
                {0.25,0.5,0.75},0.25));
    auto poly = make_shared<ScalarPolyBasis>(vm["polybasisfun"].as<int>());
    auto comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({rbf, poly}));
    if (vm.count("norbf"))
        comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({poly}));
    int n_basis_functions = comb->dim();

    // Instantiating as many ProMPs as hidden states.
    vector<FullProMP> promps;
    for(int i = 0; i < nstates; i++) {
        vec mu_w(n_basis_functions * njoints);
        mu_w.randn();
        mat Sigma_w = eye<mat>(n_basis_functions * njoints,
                    n_basis_functions * njoints);
        mat Sigma_y = 0.01*eye<mat>(njoints, njoints);
        ProMP promp(mu_w, Sigma_w, Sigma_y);
        FullProMP poly(comb, promp, njoints);
        promps.push_back(poly);
    }

    // Creating the ProMP emission and parsing the model parameters as json.
    shared_ptr<AbstractEmission> ptr_emission(new ProMPsEmission(promps));
    HSMM promp_hsmm(ptr_emission, transition, pi, durations, min_duration);
    promp_hsmm.from_stream(input_params);

    ViterbiAlgorithm(promp_hsmm, seq_obs, output_filename);
    return 0;
}


#ifndef QST_WAVEFUNCTIONCOMPLEX_HPP
#define QST_WAVEFUNCTIONCOMPLEX_HPP
#include <iostream>
#include <Eigen/Dense>
#include <random>
#include <fstream>
#include <iostream>

namespace qst{

class WavefunctionComplex{

    int N_;            // Number of degrees of freedom (visible units)
    int npar_;         // Number of parameters
    int nparLambda_;   // Number of amplitude parameters
    int nparMu_;       // Number of phase parameters
    Rbm rbmAm_;        // RBM for the amplitude
    Rbm rbmPh_;        // RBM for the phases

    const std::complex<double> I_; // Imaginary unit

    //Random number generator
    std::mt19937 rgen_;

public:
    // Constructor
    WavefunctionComplex(Parameters &par):rbmAm_(par),
                                  rbmPh_(par),
                                  I_(0,1){
        npar_ = rbmAm_.Npar() + rbmPh_.Npar();  // Total number of parameters
        nparLambda_ = rbmAm_.Npar();
        nparMu_ = rbmPh_.Npar();
        N_ = rbmAm_.Nvisible();
        std::random_device rd;
        //rgen_.seed(rd());
        rgen_.seed(13579);
    }

    // Private members access functions
    inline int N()const{
        return N_;
    }
    inline int Npar()const{
        return npar_;
    }
    inline int NparLambda()const{
        return nparLambda_;
    }
    inline int NparMu()const{
        return nparMu_;
    }
    inline int Nchains(){
        return rbmAm_.Nchains();
    }
    inline Eigen::VectorXd VisibleStateRow(int s){
        return rbmAm_.VisibleStateRow(s);
    }

    // Set the state of the wavefunction's degrees of freedom
    inline void SetVisibleLayer(Eigen::MatrixXd v){
        rbmAm_.SetVisibleLayer(v);
    }

    // Initialize the wavefunction parameters
    void InitRandomPars(int seed,double sigma){
        rbmAm_.InitRandomPars(seed,sigma);
        rbmPh_.InitRandomPars(seed,sigma);
    }

    // Amplitude
    double amplitude(const Eigen::VectorXd & v){
        return std::sqrt(rbmAm_.prob(v));//exp(0.5*rbmAm_.LogVal(v));
    }
    // Phase
    double phase(const Eigen::VectorXd & v){
        return std::log(rbmPh_.prob(v));
    }
    // Psi
    std::complex<double> psi(const Eigen::VectorXd & v){
        return amplitude(v)*exp(0.5*I_*phase(v));
    }

    //---- SAMPLING ----/

    // Perform k steps of Gibbs sampling
    void Sample(int steps){
        rbmAm_.Sample(steps);
    }

    //---- DERIVATIVES ----//

    //Compute gradient of effective energy wrt Lambda
    Eigen::VectorXd LambdaGrad(const Eigen::VectorXd & v){
        return rbmAm_.VisEnergyGrad(v);
    }
    //Compute gradient of effective energy wrt all parameters
    Eigen::VectorXd Grad(const Eigen::VectorXd & v){
        Eigen::VectorXd der(npar_);
        der<<rbmAm_.VisEnergyGrad(v),rbmPh_.VisEnergyGrad(v);
        return der;
    }
    //Compute the gradient of the effective energy in an arbitrary basis given by U
    void rotatedGrad(const std::vector<std::string> & basis,
                            const Eigen::VectorXd & state,//VectorRbmT & gradR){
                            std::map<std::string,Eigen::MatrixXcd> & Unitaries,
                            Eigen::VectorXcd &gradR ){
        int t=0,counter=0;
        std::complex<double> U=1.0,den=0.0;
        std::bitset<16> bit;
        std::bitset<16> st;
        std::vector<int> basisIndex;
        Eigen::VectorXd v(N_);
        Eigen::VectorXcd num(npar_);
        //Eigen::VectorXcd gradR(npar_);
        num.setZero();
        basisIndex.clear();
        // Extract the sites where the rotation is non-trivial
        for(int j=0;j<N_;j++){
            if (basis[j]!="Z"){
                t++;
                basisIndex.push_back(j);
            }
        }

        // Loop over the states of the local Hilbert space
        for(int i=0;i<1<<t;i++){
            counter =0;
            bit = i;
            v=state;
            for(int j=0;j<N_;j++){
                if (basis[j] != "Z"){
                    v(j) = bit[counter];
                    counter++;
                }
            }
            U=1.0;
            //Compute the product of the matrix elements of the unitary rotations
            for(int ii=0;ii<t;ii++){
                U = U * Unitaries[basis[basisIndex[ii]]](int(state(basisIndex[ii])),int(v(basisIndex[ii])));
            }
            num += U*Grad(v)*psi(v);
            den += U*psi(v);
        }
        gradR = num/den;
    }

    //---- UTILITIES ----//

    //Get RBM parameters
    Eigen::VectorXd GetParameters(){
        Eigen::VectorXd pars(npar_);
        pars<<rbmAm_.GetParameters(),rbmPh_.GetParameters();
        return pars;
    }

    // Set RBM parameters
    void SetParameters(const Eigen::VectorXd & pars){
        Eigen::VectorXd parsAm(nparLambda_);
        Eigen::VectorXd parsPh(npar_-nparLambda_);

        for(int i=0;i<nparLambda_;i++){
            parsAm(i)=pars(i);
        }
        for(int i=0;i<npar_-nparLambda_;i++){
            parsPh(i)=pars(nparLambda_+i);
        }
        rbmAm_.SetParameters(parsAm);
        rbmPh_.SetParameters(parsPh);
    }

    void LoadWeights(std::string &fileName){
        std::ifstream fin(fileName);
        rbmAm_.LoadWeights(fin);
        rbmPh_.LoadWeights(fin);
    }
};
}

#endif


/*
This file is part of Bohrium and Copyright (c) 2012 the Bohrium team:
http://bohrium.bitbucket.org

Bohrium is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation, either version 3
of the License, or (at your option) any later version.

Bohrium is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the
GNU Lesser General Public License along with bohrium.

If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
#include <Eigen/Dense>
#include <bp_util.h>

using namespace std;
using namespace Eigen;

template <typename T>
Array<T, Dynamic, 1>& cnd(const Array<T, Dynamic, 1>& x)
{
    size_t samples = x.size();
    Array<T, Dynamic, 1> l(samples), k(samples), w(samples);
    Array<T, Dynamic, 1> mask(samples);
    Array<bool, Dynamic, 1> mask_bool(samples);

    T a1 = 0.31938153,
      a2 =-0.356563782,
      a3 = 1.781477937,
      a4 =-1.821255978,
      a5 = 1.330274429,
      pp = 2.5066282746310002; // sqrt(2.0*PI)

    l = abs(x);
    //k = (T)1.0 / ((T)1.0 + (T)0.2316419 * l);
    //k = (T)0.5 / l / (T)0.5;
    k = l / (T)0.5;
    w = (T)1.0 - (T)1.0 / (T)pp * exp((T)-(T)1.0*l*l/(T)2.0) * \
        ((T)a1*k + \
         (T)a2*(pow(k,(T)2)) + \
         (T)a3*(pow(k,(T)3)) + \
         (T)a4*(pow(k,(T)4)) + \
         (T)a5*(pow(k,(T)5)));

    mask_bool= (x < (T)0.0);
    mask = mask_bool.cast<T>();
    return w * (mask) + ((T)1.0-w);
    //return w * (mask) + ((T)1.0-w)* mask;
    //return w * (!mask) + (1.0-w)* mask;
}

template <typename T>
T* pricing(size_t samples, size_t iterations, char flag, T x, T d_t, T r, T v)
{
    T* p    = (T*)malloc(sizeof(T)*samples);    // Intermediate results
    T t     = d_t;                              // Initial delta

    Array<T, Dynamic, 1> d1(samples), d2(samples), res(samples);
    Array<T, Dynamic, 1> s = Array<T, Dynamic, 1>::Random(samples)*4.0 +58.0;      // Model between 58-62

    for(size_t i=0; i<iterations; i++) {
        d1 = (log(s/x) + (r+v*v/2.0)*t) / (v*sqrt(t));
        d2 = d1-v*sqrt(t);

        if (flag == 'c') {

            size_t samples = x.size();
            Array<T, Dynamic, 1> l(samples), k(samples), w(samples);
            Array<T, Dynamic, 1> mask(samples);
            Array<bool, Dynamic, 1> mask_bool(samples);

            T a1 = 0.31938153,
              a2 =-0.356563782,
              a3 = 1.781477937,
              a4 =-1.821255978,
              a5 = 1.330274429,
              pp = 2.5066282746310002; // sqrt(2.0*PI)

            l = abs(x);
            //k = (T)1.0 / ((T)1.0 + (T)0.2316419 * l);
            //k = (T)0.5 / l / (T)0.5;
            k = l / (T)0.5;
            w = (T)1.0 - (T)1.0 / (T)pp * exp((T)-(T)1.0*l*l/(T)2.0) * \
                ((T)a1*k + \
                 (T)a2*(pow(k,(T)2)) + \
                 (T)a3*(pow(k,(T)3)) + \
                 (T)a4*(pow(k,(T)4)) + \
                 (T)a5*(pow(k,(T)5)));

            mask_bool= (x < (T)0.0);
            mask = mask_bool.cast<T>();
            res =  w * (!mask) + (1.0-w)* mask;

            //cnd<T>(d1);
            //res = s * cnd<T>(d1) -x * exp(-r*t) * cnd<T>(d2);
        } else {
/*
            Array<T, Dynamic, 1> tmp1(samples), tmp2(samples);
            tmp1 = -1.0*d2;
            tmp2 = -1.0*d1;
            res = x * exp(-r*t) * cnd<T>(tmp1) - s*cnd<T>(tmp2);
    */
        }

        t += d_t;                               // Increment delta
        p[i] = res.sum() / (T)samples;           // Result from timestep
    }

    return p;
}

int main(int argc, char* argv[])
{
    bp_util_type bp = bp_util_create(argc, argv, 2);    // Grab arguments
    if (bp.args.has_error) {
        return 1;
    }
    const size_t samples    = bp.args.sizes[0];
    const size_t iterations = bp.args.sizes[1];

    bp.timer_start();                                   // Start timer
    double* prices = pricing(                           // Run...
        samples, iterations,
        'c', 65.0, 1.0 / 365.0,
        0.08, 0.3
    );
    bp.timer_stop();                                    // Stop timer

    bp.print("black_scholes(cpp11_bxx)");               // Print restults..
    if (bp.args.verbose) {                              // ..verbosely.
        cout << ", \"output\": [";
        for(size_t i=0; i<iterations; i++) {
            cout << prices[i];
            if (iterations-1!=i) {
                cout << ", ";
            }
        }
        cout << "]" << endl;
    }
    free(prices);                                       // Cleanup

    return 0;
}


#include <boost/numeric/odeint/stepper/symplectic_rkn_sb3a_mclachlan.hpp>


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_GENERIC_D_EXPO_REDUCTION_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_GENERIC_D_EXPO_REDUCTION_HPP_INCLUDED

#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/horn1.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/constant/invlog10_2.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/detail/constant/log10_2hi.hpp>
#include <boost/simd/detail/constant/log10_2lo.hpp>
#include <boost/simd/constant/log_10.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/detail/constant/maxlog10.hpp>
#include <boost/simd/detail/constant/maxlog2.hpp>
#include <boost/simd/detail/constant/minlog.hpp>
#include <boost/simd/detail/constant/minlog10.hpp>
#include <boost/simd/detail/constant/minlog2.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/is_greater_equal.hpp>
#include <boost/simd/function/is_less_equal.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

    template < class A0> struct exp_reduction < A0, bs::tag::exp_, double>
    {
      static BOOST_FORCEINLINE auto isgemaxlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_greater_equal(a0, Maxlog<A0>()))
      {
        return is_greater_equal(a0, Maxlog<A0>());
      }

      static BOOST_FORCEINLINE auto isleminlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_less_equal(a0, Minlog<A0>()))
      {
        return is_less_equal(a0, Minlog<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce( A0 const& a0
                                        , A0& hi, A0& lo, A0& x) BOOST_NOEXCEPT
      {
        A0 k = nearbyint(Invlog_2<A0>()*a0);
        hi = fnms(k, Log_2hi<A0>(), a0); //a0-k*L
        lo = k*Log_2lo<A0>();
        x  = hi-lo;
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 const& x) BOOST_NOEXCEPT
      {
        A0 const t = sqr(x);
        return fnms(t,
                    horn<A0
                         , 0x3fc555555555553eull
                         , 0xbf66c16c16bebd93ull
                         , 0x3f11566aaf25de2cull
                         , 0xbebbbd41c5d26bf1ull
                         , 0x3e66376972bea4d0ull
                    >(t), x); //x-h*t
    }

      static BOOST_FORCEINLINE A0 finalize(A0 const& x, A0 const& c, A0 const& hi, A0 const& lo) BOOST_NOEXCEPT
      {
        return One<A0>()-(((lo-(x*c)/(Two<A0>()-c))-hi));
      }

    };

    template < class A0 > struct exp_reduction < A0, bs::tag::exp2_, double>
    {
      static BOOST_FORCEINLINE auto isgemaxlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_greater_equal(a0, Maxlog2<A0>()))
      {
        return is_greater_equal(a0, Maxlog2<A0>());
      }

      static BOOST_FORCEINLINE auto isleminlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_less_equal(a0, Minlog2<A0>()))
      {
        return is_less_equal(a0, Minlog2<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce(A0 const& a0, A0 const&, A0 const&, A0& x) BOOST_NOEXCEPT
      {
        A0 k = nearbyint(a0);
        x = (a0 - k)*Log_2<A0>();
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 const& x) BOOST_NOEXCEPT
      {
        const A0 t =  sqr(x);
        return fnms(t,
                    horn<A0
                         , 0x3fc555555555553eull
                         , 0xbf66c16c16bebd93ull
                         , 0x3f11566aaf25de2cull
                         , 0xbebbbd41c5d26bf1ull
                         , 0x3e66376972bea4d0ull
                    > (t), x); //x-h*t
      }

      static BOOST_FORCEINLINE A0 finalize(A0 const& x, A0 const& c, A0 const&, A0& ) BOOST_NOEXCEPT
      {
        return oneminus(((-(x*c)/(Two<A0>()-c))-x));
      }
    };

    template < class A0 > struct exp_reduction < A0, bs::tag::exp10_, double>
    {
      static BOOST_FORCEINLINE auto isgemaxlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_greater_equal(a0, Maxlog10<A0>()))
      {
        return is_greater_equal(a0, Maxlog10<A0>());
      }

      static BOOST_FORCEINLINE auto isleminlog(A0 const& a0) BOOST_NOEXCEPT
        -> decltype(is_less_equal(a0, Minlog10<A0>()))
      {
        return is_less_equal(a0, Minlog10<A0>());
      }

      static BOOST_FORCEINLINE A0 reduce(A0 const& a0, A0&, A0&, A0& x) BOOST_NOEXCEPT
      {
        A0 k  = nearbyint(Invlog10_2<A0>()*a0);
        x = fnms(k, Log10_2hi<A0>(), a0);
        x = fnms(k, Log10_2lo<A0>(), x);
        return k;
      }

      static BOOST_FORCEINLINE A0 approx(A0 x) BOOST_NOEXCEPT
      {
        A0 xx = sqr(x);
        A0 px = x*horn<A0,
                       0x40a2b4798e134a01ull,
                       0x40796b7a050349e4ull,
                       0x40277d9474c55934ull,
                       0x3fa4fd75f3062dd4ull
                       > (xx);
        A0 x2 =  px/(horn1<A0,
                          0x40a03f37650df6e2ull,
                          0x4093e05eefd67782ull,
                          0x405545fdce51ca08ull
                     //   0x3ff0000000000000ull
                          > (xx)-px);
        return inc(x2+x2);
      }

      static BOOST_FORCEINLINE A0 finalize(A0 const&, A0 const& c, A0 const&, A0& ) BOOST_NOEXCEPT
      {
        return c;
      }
    };
  }
} }
#endif


/***************************************************************************
 *
 *   Copyright (C) 2021 by fxzjshm
 *   Licensed under the GNU General Public License, version 2.0
 *
 ***************************************************************************/

#include "benchmark.hpp"
#include "checks.hpp"
#include "io.hpp"
#include <boost/compute/command_queue.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/utility/source.hpp>
#include <clFFT.h>

extern Stopwatch setup_timer, generate_timer, fft_timer, normalize_timer, copy_timer, write_timer;

std::string kernel_source = BOOST_COMPUTE_STRINGIZE_SOURCE(
    __kernel void generate(__global data_type *d_in, ulong nsamp /*, data_type dt*/) {
        size_t i = get_global_id(0);
        /*
        data_type t = dt * ((data_type)i);
        data_type x = t / 400.0f;
        d_in[i] = sin(2*M_PI_F*40*x) + ((data_type)0.5) * sin(2*M_PI_F*90*x);
        */
        uint m = 0xCAFEBABEDEADCAFE; //
        uint t = i & 0x3F;
        for (uint i = 0; i < t; i++) {
            m = (m << 1) | (m >> 63);
        }
        d_in[i] = m / 1e18;
    }

    __kernel void normalize_complex_number(__global data_type *d_out_complex, __global data_type *d_out_real) {
        size_t i = get_global_id(0);
        d_out_real[i] = (sqrt(d_out_complex[2 * i] * d_out_complex[2 * i] + d_out_complex[2 * i + 1] * d_out_complex[2 * i + 1]));
    }

    __kernel void pick_real_in_complex_number(__global data_type *d_out_complex, __global data_type *d_out_real) {
        size_t i = get_global_id(0);
        d_out_real[i] = d_out_complex[2 * i];
    });

template <typename data_type>
class clfft_caller {

public:
    boost::compute::command_queue queue;
    size_t in_nsamp_seg, out_nsamp_seg;
    size_t seg_count;
    size_t in_nsamp, out_nsamp;
    boost::compute::vector<data_type> d_in, d_out_complex, d_out_real, d_in_tmp;
    clfftPlanHandle plan_handle;
    boost::compute::kernel generate_kernel;
    boost::compute::kernel normalize_kernel;
    boost::compute::kernel pick_real_kernel;
    bool inverse;

    clfft_caller(boost::compute::command_queue queue_, size_t in_nsamp_seg_, size_t seg_count_, size_t out_nsamp_seg_, bool inverse_)
        : queue(queue_), in_nsamp_seg(in_nsamp_seg_), seg_count(seg_count_), out_nsamp_seg(out_nsamp_seg_), inverse(inverse_),
          in_nsamp(in_nsamp_seg * seg_count), out_nsamp(out_nsamp_seg * seg_count), d_in(in_nsamp), d_out_complex(2 * out_nsamp), d_out_real(out_nsamp) {

        namespace bc = boost::compute;
        bc::context context = queue.get_context();
        bc::device device = queue.get_device();
        clfftSetupData clfft_setup_data;
        clfftInitSetupData(&clfft_setup_data);
        clfftSetup(&clfft_setup_data);
        clfftDim dim = CLFFT_1D;
        size_t cl_lengths[1];
        if (!inverse) {
            cl_lengths[0] = in_nsamp_seg;
        } else {
            cl_lengths[0] = out_nsamp_seg;
        }
        if (!check_fft_length(cl_lengths[0])) {
            throw std::runtime_error("Unsupported fft length " + std::to_string(cl_lengths[0]));
        }
        clfftCreateDefaultPlan(&plan_handle, context.get(), dim, cl_lengths);
        clfftSetPlanPrecision(plan_handle, CLFFT_SINGLE);
        if (!inverse) {
            clfftSetLayout(plan_handle, CLFFT_REAL, CLFFT_HERMITIAN_INTERLEAVED);
        } else {
            clfftSetLayout(plan_handle, CLFFT_HERMITIAN_PLANAR, CLFFT_REAL);
            d_in_tmp = bc::vector<data_type>(in_nsamp);
            bc::fill(d_in_tmp.begin(), d_in_tmp.end(), static_cast<data_type>(0));
        }
        clfftSetResultLocation(plan_handle, CLFFT_OUTOFPLACE);
        clfftSetPlanBatchSize(plan_handle, seg_count);
        clfftSetPlanDistance(plan_handle, in_nsamp_seg, out_nsamp_seg);
        clfftBakePlan(plan_handle, 1, &(queue.get()), NULL, NULL);

        bc::program program = bc::program::build_with_source(kernel_source, context, std::string("-Ddata_type=") + bc::type_name<data_type>());
        generate_kernel = bc::kernel(program, "generate");
        normalize_kernel = bc::kernel(program, "normalize_complex_number");
        pick_real_kernel = bc::kernel(program, "pick_real_in_complex_number");
    }

    void print_info() {
        /* clang-format off */
        std::cout << "in_nsamp_seg = " << in_nsamp_seg << "  " << "out_nsamp_seg = " << out_nsamp_seg << "  " << "seg_count = " << seg_count << std::endl
                  << "in_nsamp = " << in_nsamp << "  " << "out_nsamp = " << out_nsamp << std::endl;
        /* clang-format on */
        std::cout << "d_in size : " << d_in.get_buffer().get_memory_size() << " bytes" << std::endl;
    }

    void generate() {
        generate_kernel.set_args(d_in.get_buffer().get(), static_cast<cl_ulong>(in_nsamp));
        queue.enqueue_1d_range_kernel(generate_kernel, 0, in_nsamp, 0);
    }

    void call_fft(std::vector<data_type> &h_in, std::vector<data_type> &h_out_complex, std::vector<data_type> &h_out_real) {
        namespace bc = boost::compute;
        size_t iteration = h_in.size() / in_nsamp;
        size_t in_offset = 0, out_offset = 0, out_part_offset = 0;

        std::vector<data_type> h_in_tmp(in_nsamp), h_out_complex_tmp(2 * out_nsamp), h_out_real_tmp(out_nsamp);
        for (size_t i = 0; i < iteration; i++) {
            start_timer(copy_timer);
            bc::copy(h_in.begin() + in_offset, h_in.begin() + in_offset + in_nsamp, d_in.begin());
            bc::copy(h_in.begin() + in_offset, h_in.begin() + in_offset + in_nsamp, h_in_tmp.begin());
            stop_timer(copy_timer);

            start_timer(fft_timer);
            if (!inverse) {
                clfftEnqueueTransform(plan_handle, CLFFT_FORWARD, 1, &(queue.get()), 0, NULL, NULL, &(d_in.get_buffer().get()), &(d_out_complex.get_buffer().get()), NULL);
            } else {
                cl_mem d_ins[2] = {(d_in.get_buffer().get()), d_in_tmp.get_buffer().get()};
                clfftEnqueueTransform(plan_handle, CLFFT_BACKWARD, 1, &(queue.get()), 0, NULL, NULL, &d_ins[0], &(d_out_real.get_buffer().get()), NULL);
            }
            stop_timer(fft_timer);

            start_timer(normalize_timer);
            if (!inverse) {
                normalize_kernel.set_args(d_out_complex.get_buffer().get(), d_out_real.get_buffer().get());
                queue.enqueue_1d_range_kernel(normalize_kernel, 0, out_nsamp, 0);
            }
            stop_timer(normalize_timer);

            start_timer(copy_timer);
            // bc::copy(d_in.begin(), d_in.end(), h_in.begin() + in_offset);
            bc::copy(d_out_complex.begin(), d_out_complex.end(), h_out_complex.begin() + 2 * out_offset);
            bc::copy(d_out_real.begin(), d_out_real.end(), h_out_real.begin() + out_offset);
            if (i == 0) {
                bc::copy(h_out_complex.begin() + 2 * out_offset, h_out_complex.begin() + 2 * out_offset + 2 * out_nsamp, h_out_complex_tmp.begin());
                bc::copy(h_out_real.begin() + out_offset, h_out_real.begin() + out_offset + out_nsamp, h_out_real_tmp.begin());
            }
            // queue.enqueue_read_buffer_rect(d_out_real.get_buffer(),
            //                                /* buffer_offset = */ bc::dim(fmin_id, 0, 0).data(),
            //                                /* host_offset = */ bc::dim(fmin_id, seg_count * i, 0).data(),
            //                                /* region = */ bc::dim(out_part_nsamp_seg, seg_count, 1).data(),
            //                                /* buffer_row_pitch = */ out_nsamp_seg,
            //                                /* buffer_slice_pitch = */ 0,
            //                                /* host_row_pitch = */ out_part_nsamp_seg,
            //                                /* host_slice_pitch = */ 0,
            //                                &h_out_part[0]);
            stop_timer(copy_timer);
            if (i == 0) {
                start_timer(write_timer);
                write_vector(h_in_tmp, in_nsamp_seg, seg_count, "h_in_tmp-" + std::to_string(i) + ".txt");
                write_vector(h_out_complex_tmp, out_nsamp_seg * 2, seg_count, "h_out_complex_tmp-" + std::to_string(i) + ".txt");
                write_vector(h_out_real_tmp, out_nsamp_seg, seg_count, "h_out_real_tmp-" + std::to_string(i) + ".txt");
                stop_timer(write_timer);
            }

            std::cout << "fft_timer: " << fft_timer.getTime() << "  "
                      << "normalize_timer: " << normalize_timer.getTime() << "  "
                      << "copy_timer: " << copy_timer.getTime()
                      << "    \r";
            std::cout.flush();

            in_offset += in_nsamp;
            out_offset += out_nsamp;
            //out_part_offset += out_part_nsamp;
        }
    }

    void teardown() {
        clfftDestroyPlan(&plan_handle);
        clfftTeardown();
    }
};


#pragma once

#include "Rational.hpp"
#include "Utils.hpp"

#include <Eigen/Core>

#include <array>
#include <vector>

namespace eccd
{

class TriPtF
{
public:
    TriPtF(const Vector3d &pts,
           const Vector3d &v1s, const Vector3d &v2s, const Vector3d &v3s,
           const Vector3d &pte,
           const Vector3d &v1e, const Vector3d &v2e, const Vector3d &v3e);

    void save(std::ostream &out) const;

    Vector3r operator()(double u, double v, double t) const;

    std::array<Vector3r, 4> corners(int i) const;
    inline int n_patches() const { return 3; }

    std::vector<std::array<Vector3r, 3>> top_bottom_faces() const;

private:
    const Vector3d &pts_;
    const Vector3d &v1s_, &v2s_, &v3s_;

    const Vector3d &pte_;
    const Vector3d &v1e_, &v2e_, &v3e_;

    Vector3r ptsr_;
    Vector3r v1sr_, v2sr_, v3sr_;

    Vector3r pter_;
    Vector3r v1er_, v2er_, v3er_;

    void init_rationals();
};

class EdgeEdgeF
{
public:
    //edge [a0, a1] and [b0, b1]
    EdgeEdgeF(const Vector3d &a0s, const Vector3d &a1s,
              const Vector3d &b0s, const Vector3d &b1s,
              const Vector3d &a0e, const Vector3d &a1e,
              const Vector3d &b0e, const Vector3d &b1e);

    void save(std::ostream &out) const;

    Vector3r operator()(double ta, double tb, double t) const;

    std::array<Vector3r, 4> corners(int i) const;
    inline int n_patches() const { return 6; }

    std::vector<std::array<Vector3r, 3>> top_bottom_faces() const { return {}; }

private:
    const Vector3d &a0s_, &a1s_;
    const Vector3d &b0s_, &b1s_;

    const Vector3d &a0e_, &a1e_;
    const Vector3d &b0e_, &b1e_;

    Vector3r a0rs_, a1rs_;
    Vector3r b0rs_, b1rs_;

    Vector3r a0re_, a1re_;
    Vector3r b0re_, b1re_;

    void init_rationals();
};

Rational phi(const Vector3r x, const std::array<Vector3r, 4> &corners);

bool is_origin_in_tet(const std::array<Vector3r, 4> &corners, const std::array<std::array<int, 3>, 4> &tet_faces);

int ray_flat_patch(const std::array<Vector3r, 4> &corners, const Vector3d &dir);

template <typename FuncF>
int ray_patch(const FuncF &func, int patch, const Vector3d &dir);

template <typename FuncF>
int ccd(const FuncF &func, const Vector3d &dir);

template <typename FuncF>
bool retrial_ccd(const FuncF &func);

bool vertexFaceCCD(const Vector3d &pts,
                   const Vector3d &v1s, const Vector3d &v2s, const Vector3d &v3s,
                   const Vector3d &pte,
                   const Vector3d &v1e, const Vector3d &v2e, const Vector3d &v3e);

bool edgeEdgeCCD(const Vector3d &a0s, const Vector3d &a1s,
                 const Vector3d &b0s, const Vector3d &b1s,
                 const Vector3d &a0e, const Vector3d &a1e,
                 const Vector3d &b0e, const Vector3d &b1e);

} // namespace eccd


/*
    Copyright 2012 Ulrik Mikaelsson <ulrik.mikaelsson@gmail.com>

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

/// TreeStore as described briefly in http://www.lucidchart.com/publicSegments/view/4f5910e5-22dc-4b22-ba2c-6fee0a7c6148

#ifndef BITHORDED_TREESTORE_H
#define BITHORDED_TREESTORE_H

#include <math.h>
#include <stdint.h>
#include <ostream>

#include <boost/assert.hpp>
#include <boost/shared_ptr.hpp>

inline uint32_t parentlayersize(uint32_t nodes) {
	if (nodes > 1)
		return (nodes+1)/2;
	else
		return 0;
}

// Calculates the required number of nodes in a tree, with a leaf-layer of n leaves
inline uint32_t treesize(uint32_t leafs) {
	if (leafs > 1)
		return leafs + treesize(parentlayersize(leafs));
	else
		return leafs;
}

uint32_t calc_leaves(uint32_t treesize);

struct NodeIdx {
	uint32_t nodeIdx;
	uint32_t layerSize;

	NodeIdx(uint32_t nodeIdx, uint32_t layerSize)
		: nodeIdx(nodeIdx), layerSize(layerSize)
	{}

	NodeIdx parent() const {
		BOOST_ASSERT( not isRoot() );
		return NodeIdx(nodeIdx/2, parentlayersize(layerSize));
	}

	NodeIdx sibling() const {
		return NodeIdx(nodeIdx ^ 0x01, layerSize);
	}

	bool operator<(const NodeIdx& other) {
		BOOST_ASSERT(this->layerSize == other.layerSize);
		return this->nodeIdx < other.nodeIdx;
	}

	bool isValid() const {
		return nodeIdx < layerSize;
	}

	bool operator==(const NodeIdx& other) const {
		return (this->nodeIdx == other.nodeIdx)
			&& (this->layerSize == other.layerSize);
	}

	bool isRoot() const {
		return (layerSize == 1);
	}
};
const NodeIdx TREE_ROOT_NODE(0,1);

std::ostream& operator<<(std::ostream& str,const NodeIdx& idx);

template <typename Node, typename BackingStore>
class TreeStore
{
public:
	typedef typename BackingStore::NodePtr NodePtr;

	TreeStore(BackingStore& backingStore)
		: _storage(backingStore), _leaves(calc_leaves(backingStore.size()))
	{
		BOOST_ASSERT(backingStore.size() >= treesize(_leaves));
	}

	NodeIdx leaf(uint32_t i) const {
		return NodeIdx(i, _leaves);
	};

	NodePtr operator[](const NodeIdx& idx) {
		int layer_offset = treesize(parentlayersize(idx.layerSize));
		return _storage[layer_offset + idx.nodeIdx];
	}
	const NodePtr operator[](const NodeIdx& idx) const {
		int layer_offset = treesize(parentlayersize(idx.layerSize));
		return _storage[layer_offset + idx.nodeIdx];
	}

	uint32_t leaves() const {
		return _leaves;
	}

private:
	BackingStore& _storage;
	uint32_t _leaves;
};

#endif // BITHORDED_TREESTORE_H


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include <igl/parallel_transport_angles.h>
#include <Eigen/Geometry>

template <typename DerivedV, typename DerivedF, typename DerivedK>
IGL_INLINE void igl::parallel_transport_angles(
const Eigen::PlainObjectBase<DerivedV>& V,
const Eigen::PlainObjectBase<DerivedF>& F,
const Eigen::PlainObjectBase<DerivedV>& FN,
const Eigen::MatrixXi &E2F,
const Eigen::MatrixXi &F2E,
Eigen::PlainObjectBase<DerivedK> &K)
{
  int numE = E2F.rows();

  Eigen::VectorXi isBorderEdge;
  isBorderEdge.setZero(numE,1);
  for(unsigned i=0; i<numE; ++i)
  {
    if ((E2F(i,0) == -1) || ((E2F(i,1) == -1)))
      isBorderEdge[i] = 1;
  }

  K.setZero(numE);
  // For every non-border edge
  for (unsigned eid=0; eid<numE; ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = E2F(eid,0);
      int fid1 = E2F(eid,1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N0 = FN.row(fid0);
//      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N1 = FN.row(fid1);

      // find common edge on triangle 0 and 1
      int fid0_vc = -1;
      int fid1_vc = -1;
      for (unsigned i=0;i<3;++i)
      {
        if (F2E(fid0,i) == eid)
          fid0_vc = i;
        if (F2E(fid1,i) == eid)
          fid1_vc = i;
      }
      assert(fid0_vc != -1);
      assert(fid1_vc != -1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> common_edge = V.row(F(fid0,(fid0_vc+1)%3)) - V.row(F(fid0,fid0_vc));
      common_edge.normalize();

      // Map the two triangles in a new space where the common edge is the x axis and the N0 the z axis
      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> P;
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> o = V.row(F(fid0,fid0_vc));
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> tmp = -N0.cross(common_edge);
      P << common_edge, tmp, N0;
      //      P.transposeInPlace();

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V0;
      V0.row(0) = V.row(F(fid0,0)) -o;
      V0.row(1) = V.row(F(fid0,1)) -o;
      V0.row(2) = V.row(F(fid0,2)) -o;

      V0 = (P*V0.transpose()).transpose();

      //      assert(V0(0,2) < 1e-10);
      //      assert(V0(1,2) < 1e-10);
      //      assert(V0(2,2) < 1e-10);

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V1;
      V1.row(0) = V.row(F(fid1,0)) -o;
      V1.row(1) = V.row(F(fid1,1)) -o;
      V1.row(2) = V.row(F(fid1,2)) -o;
      V1 = (P*V1.transpose()).transpose();

      //      assert(V1(fid1_vc,2) < 10e-10);
      //      assert(V1((fid1_vc+1)%3,2) < 10e-10);

      // compute rotation R such that R * N1 = N0
      // i.e. map both triangles to the same plane
      double alpha = -atan2(V1((fid1_vc+2)%3,2),V1((fid1_vc+2)%3,1));

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> R;
      R << 1,          0,            0,
      0, cos(alpha), -sin(alpha) ,
      0, sin(alpha),  cos(alpha);
      V1 = (R*V1.transpose()).transpose();

      //      assert(V1(0,2) < 1e-10);
      //      assert(V1(1,2) < 1e-10);
      //      assert(V1(2,2) < 1e-10);

      // measure the angle between the reference frames
      // k_ij is the angle between the triangle on the left and the one on the right
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref0 = V0.row(1) - V0.row(0);
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref1 = V1.row(1) - V1.row(0);

      ref0.normalize();
      ref1.normalize();

      double ktemp = atan2(ref1(1),ref1(0)) - atan2(ref0(1),ref0(0));

      // just to be sure, rotate ref0 using angle ktemp...
      Eigen::Matrix<typename DerivedV::Scalar,2,2> R2;
      R2 << cos(ktemp), -sin(ktemp), sin(ktemp), cos(ktemp);

//      Eigen::Matrix<typename DerivedV::Scalar, 1, 2> tmp1 = R2*(ref0.head(2)).transpose();

      //      assert(tmp1(0) - ref1(0) < 1e-10);
      //      assert(tmp1(1) - ref1(1) < 1e-10);

      K[eid] = ktemp;
    }
  }

}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::parallel_transport_angles<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1> const&, Eigen::Matrix<int, -1, -1, 0, -1, -1> const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
#endif


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/cospi.hpp>
#include <boost/simd/function/fast.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/quarter.hpp>
#include <boost/simd/constant/sqrt_2.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

STF_CASE_TPL (" cospi",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cospi;

  using r_t = decltype(cospi(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(cospi(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cospi(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cospi(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(cospi(-bs::Quarter<T>()), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cospi(bs::Half<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cospi(bs::Mhalf<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cospi(bs::One<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cospi(bs::Quarter<T>()), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cospi(bs::Zero<T>()), bs::One<r_t>(), 0.5);
}

STF_CASE_TPL (" cospi fast",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cospi;

  using r_t = decltype(cospi(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::fast_(cospi)(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cospi)(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cospi)(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(bs::fast_(cospi)(-bs::Quarter<T>()), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cospi)(bs::Half<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cospi)(bs::Mhalf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cospi)(bs::One<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cospi)(bs::Quarter<T>()), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cospi)(bs::Zero<T>()), bs::One<r_t>(), 0.5);
}

STF_CASE_TPL (" cospi unsigned",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::cospi;
  using r_t = decltype(cospi(T()));
  using result_t = bd::as_floating_t<T>;

  // return type conformity test
  STF_TYPE_IS(r_t, result_t);

  // specific values tests
  STF_ULP_EQUAL(cospi(bs::One<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cospi(bs::Zero<T>()), bs::One<r_t>(), 0.5);
} // end of test for unsigned_int_

STF_CASE_TPL (" cospi signed",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::cospi;
  using r_t = decltype(cospi(T()));
  using result_t = bd::as_floating_t<T>;

  // return type conformity test
  STF_TYPE_IS(r_t, result_t);

  // specific values tests
  STF_ULP_EQUAL(cospi(bs::Mone<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cospi(bs::One<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cospi(bs::Zero<T>()), bs::One<r_t>(), 0.5);
} // end of test for signed_int_


#pragma once

#include <Eigen/Dense>

namespace icarus
{
    template<typename T, size_t N>
    struct GaussianDistribution
    {
        Eigen::Matrix<T, N, 1> mean;
        Eigen::Matrix<T, N, N> covariance;
    };
}