// Composite trajectory
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include "compositetrajectory.h"

#include <math.h>
#include <boost/concept_check.hpp>

const double STEP_PATTERN_LENGTH = 6.0 + 5.0 * 0.9;
const double NUM_STEP_PATTERNS = 2.0;

const double TIME_WAIT = 1.0;

const double KICK_DRAWBACK_HIP = 50.0 * M_PI/180.0;
const double KICK_DRAWBACK_KNEE = 45.0 * M_PI/180.0;
const double KICK_DEST_HIP = -60.0 * M_PI/180.0;
const double KICK_DEST_KNEE = 0;
const double KICK_LENGTH = 0.9;
const double KICK_STOP_LENGTH = 0.4;
const double KICK_DRAWBACK_LEN = 2.0;

const double NUM_KICK_PATTERNS = 5.0;

const int ID_KNEE = 14;
const int ID_HIP = 12;

double sinFromTo(double from, double to, double freq, double time)
{
	const double AMPLITUDE = to-from;

	return 0.5 * AMPLITUDE * sin(freq * 2.0 * M_PI * time - M_PI/2.0) + 0.5*AMPLITUDE + from;
}

double CompositeTrajectory::endTime() const
{
	return NUM_STEP_PATTERNS * STEP_PATTERN_LENGTH + TIME_WAIT
		+ NUM_KICK_PATTERNS * (KICK_LENGTH + KICK_STOP_LENGTH + KICK_DRAWBACK_LEN);
}

double stepPattern(int id, double time)
{
	if(time < 6.0)
	{
		const double ANGLE = 60.0 * M_PI / 180.0;
		const double FREQ = 0.5;

		switch(id)
		{
			case ID_HIP: return sinFromTo(0, -ANGLE, FREQ, time);
			case ID_KNEE: return sinFromTo(0, 2.0*ANGLE, FREQ, time);
		}
	}
	else
	{
		const double ANGLE = 30.0 * M_PI / 180.0;
		const double FREQ = 1.0 / 0.9;
		time -= 6.0;
		switch(id)
		{
			case ID_HIP: return sinFromTo(0, -ANGLE, FREQ, time);
			case ID_KNEE: return sinFromTo(0, 2.0*ANGLE, FREQ, time);
		}
	}

	return 0.0;
}

double kickPattern(int id, double time)
{
	double FREQ = 1.0 / (KICK_LENGTH - 0.25) / 2.0;
	double hip_freq = (time / KICK_LENGTH) * FREQ;

	double amplitudeFactor = 0.5 * (KICK_DRAWBACK_HIP / (KICK_LENGTH/2));
	double amplitude = amplitudeFactor * time * time;

	if(time <= KICK_LENGTH)
	{
		switch(id)
		{
			case ID_HIP: return amplitude * sin(2.0 * M_PI * hip_freq * time);
			case ID_KNEE:
			{
				double hipZeroTime = sqrt(0.5 / FREQ * KICK_LENGTH);

				if(time > hipZeroTime)
					return 0;

				return sinFromTo(KICK_DEST_KNEE, KICK_DRAWBACK_KNEE, 1.0 / hipZeroTime, time);
			}
		}
	}
	time -= KICK_LENGTH;

	double endPos = kickPattern(ID_HIP, KICK_LENGTH);
	double endVel = 2.0 * amplitudeFactor * KICK_LENGTH * sin(2.0 * M_PI * FREQ * KICK_LENGTH)
		+ 4.0 * M_PI * FREQ * amplitudeFactor * KICK_LENGTH * KICK_LENGTH * cos(2.0 * M_PI * FREQ * KICK_LENGTH);

	double stopPos = endPos + 0.5 * endVel * KICK_STOP_LENGTH;

	if(time <= KICK_STOP_LENGTH)
	{
		switch(id)
		{
			case ID_HIP:
			{
				return endPos + endVel * time - 0.5 * endVel / KICK_STOP_LENGTH * time*time;
			}
			case ID_KNEE:
				return 0;
		}
	}
	time -= KICK_STOP_LENGTH;

	double k = stopPos;
	switch(id)
	{
		case ID_HIP: return k - k/KICK_DRAWBACK_LEN * time;
		case ID_KNEE: return 0;
	}

	return 0;
}

double CompositeTrajectory::position(int id, double time) const
{
	if(id != ID_HIP && id != ID_KNEE)
		return 0.0;

	if(time < NUM_STEP_PATTERNS * STEP_PATTERN_LENGTH)
	{
		double ptime = time;
		while(ptime >= STEP_PATTERN_LENGTH)
			ptime -= STEP_PATTERN_LENGTH;

		double pos = stepPattern(id, ptime);

		if(id == ID_HIP)
		{
			const double ANGLE = 70.0 * M_PI / 180.0;
			const double FREQ = 1.0 / (NUM_STEP_PATTERNS * STEP_PATTERN_LENGTH);
			pos += 0.5 * ANGLE * sin(FREQ * 2.0 * M_PI * time + M_PI/2.0) - 0.5*ANGLE;
		}

		return pos;
	}
	time -= NUM_STEP_PATTERNS*STEP_PATTERN_LENGTH;

	if(time < TIME_WAIT)
		return 0;
	time -= TIME_WAIT;

	double ptime = time;
	while(ptime >= KICK_LENGTH + KICK_STOP_LENGTH + KICK_DRAWBACK_LEN)
		ptime -= KICK_LENGTH + KICK_STOP_LENGTH + KICK_DRAWBACK_LEN;
	return kickPattern(id, ptime);
}


#ifndef __GUIDANCE_HH__
#define __GUIDANCE_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the GUIDANCE Module On Board)
LIBRARY DEPENDENCY:
      ((../src/Guidance.cpp))
PROGRAMMERS:
      (((Chung-Fan Yang) () () () ))
*******************************************************************************/
#include <armadillo>
#include "aux.hh"

class Guidance {
    TRICK_INTERFACE(Guidance);

 public:
    Guidance();
    Guidance(const Guidance& other);

    Guidance& operator= (const Guidance& other);

    void default_data();
    void initialize();

    void guidance(double int_step);

    arma::vec3 guidance_ltg(int &mprop, double int_step, double time_ltg);
    void guidance_ltg_tgo(double &tgop,
                          arma::mat &BURNTN,
                          arma::mat &L_IGRLN,
                          arma::mat &TGON,
                          double &l_igrl,
                          int &nstmax,
                          double &tgo,
                          int &nst,
                          arma::mat &TAUN,
                          arma::mat VEXN,
                          arma::mat BOTN,
                          double delay_ignition,
                          double vgom,
                          double amag1,
                          double amin,
                          double time_ltg,
                          int num_stages);
    void guidance_ltg_igrl(double &s_igrl,
                           double &j_igrl,
                           double &q_igrl,
                           double &h_igrl,
                           double &p_igrl,
                           double &j_over_l,
                           double &tlam,
                           double &qprime,
                           int nst,
                           int nstmax,
                           arma::mat BURNTN,
                           arma::mat L_IGRLN,
                           arma::mat TGON,
                           arma::mat TAUN,
                           arma::mat VEXN,
                           double l_igrl,
                           double time_ltg);
    void guidance_ltg_trate(arma::mat &ULAM,
                            arma::mat &LAMD,
                            arma::mat &RGO,
                            int &ipas2_flag,
                            arma::mat VGO,
                            double s_igrl,
                            double q_igrl,
                            double j_over_l,
                            double lamd_limit,
                            double vgom,
                            double time_ltg,
                            double tgo,
                            double tgop,
                            arma::mat SDII,
                            arma::mat SBIIC,
                            arma::mat VBIIC,
                            arma::mat RBIAS,
                            arma::mat UD,
                            arma::mat UY,
                            arma::mat UZ,
                            arma::mat &RGRAV);
    void guidance_ltg_trate_rtgo(arma::mat &RGO,
                                 arma::mat &RGRAV,
                                 double tgo,
                                 double tgop,
                                 arma::mat SDII,
                                 arma::mat SBIIC,
                                 arma::mat VBIIC,
                                 arma::mat RBIAS,
                                 arma::mat ULAM,
                                 arma::mat UD,
                                 arma::mat UY,
                                 arma::mat UZ,
                                 double s_igrl);
    void guidance_ltg_pdct(arma::mat &SPII,
                           arma::mat &VPII,
                           arma::mat &RGRAV,
                           arma::mat &RBIAS,
                           arma::mat LAMD,
                           arma::mat ULAM,
                           double l_igrl,
                           double s_igrl,
                           double j_igrl,
                           double q_igrl,
                           double h_igrl,
                           double p_igrl,
                           double j_over_l,
                           double qprime,
                           arma::mat SBIIC,
                           arma::mat VBIIC,
                           arma::mat RGO,
                           double tgo);
    void guidance_ltg_crct(arma::mat &SDII,
                           arma::mat &UD,
                           arma::mat &UY,
                           arma::mat &UZ,
                           arma::mat &VMISS,
                           arma::mat &VGO,
                           double dbi_desired,
                           double dvbi_desired,
                           double thtvdx_desired,
                           arma::mat SPII,
                           arma::mat VPII,
                           arma::mat SBIIC,
                           arma::mat VBIIC);

    std::function<int()>            grab_mprop;

    std::function<double()>         grab_dbi;
    std::function<double()>         grab_dvbi;
    std::function<double()>         grab_thtvdx;
    std::function<double()>         grab_fmassr;
    std::function<arma::vec3()>     grab_VBIIC;
    std::function<arma::vec3()>     grab_SBIIC;
    std::function<arma::mat33()>    grab_TBIC;
    std::function<arma::vec3()>     grab_FSPCB;

    std::function<void()>           set_no_thrust;
    std::function<void()>           set_ltg_thrust;

    ///////////////////////////////////////////////////////////////////////////////
    // Definition of guidance module-variables
    // Member function of class 'Hyper'
    // Module-variable locations are assigned to hyper[400-499]
    // Overflow assignment hyper[850-899]
    //
    //       mguide =  0 no guidance
    //              =  5 linear tangent guidance law (LTG) for rocket ascent
    //
    // 030616 Created by Peter H Zipfel
    // 091214 Modified for ROCKET6, PZi
    ///////////////////////////////////////////////////////////////////////////////
    arma::vec3  get_UTBC();

    double  get_alphacomx();
    double  get_betacomx();

    void    set_degree(double, double);

 private:
    /* Propagative Stats */
    arma::vec  UTBC;        /* *io  (--)        Commanded unit thrust vector in body coor */
    double  _UTBC[3];        /* *io  (--)        Commanded unit thrust vector in body coor */

    /* Diagnostic */
    arma::vec  UTIC;        /* *io  (--)        Commanded unit thrust vector in inertial coor */
    double  _UTIC[3];        /* *io  (--)        Commanded unit thrust vector in inertial coor */

    /* Set by input.py */
    double  alphacomx;      /* *io  (d)         Alpha command */
    double  betacomx;       /* *io  (d)         Beta command */

    /* Internally set parameter */
    int     init_flag;      /* *io  (--)        Flag for initializing LTG flags */
    int     inisw_flag;     /* *io  (--)        Flag to initialize '..._intl()' */
    int     skip_flag;      /* *io  (--)        Flag to delay output */
    int     ipas_flag;      /* *io  (--)        Flag to initialize in '..._tgo()'  */
    int     ipas2_flag;     /* *io  (--)        Flag to initialize in '..._trat()'  */
    int     print_flag;     /* *io  (--)        Flag to cause print-out  */
    double  time_ltg;       /* *io  (s)         Time since initiating LTG */

    int     ltg_count;      /* *io  (--)        Counter of LTG guidance cycles  */

    /* Externally set parameter */
    int     mguide;         /* *io  (--)        Guidance modes, see table */
    double  ltg_step;       /* *io  (s)         LTG guidance time step */

    arma::vec RBIAS;       /* *io  (m)         Range-to-be-gained bias */
    double   _RBIAS[3];       /* *io  (m)         Range-to-be-gained bias */
    int     beco_flag;      /* *io  (--)        Boost engine cut-off flag */
    double  dbi_desired;    /* *io  (m)         Desired orbital end position */
    double  dvbi_desired;   /* *io  (m/s)       Desired orbital end velocity */
    double  thtvdx_desired; /* *io  (d)         Desired orbital flight path angle */
    int     num_stages;     /* *io  (s)         Number of stages in boost phase */
    double  delay_ignition; /* *io  (s)         Delay of motor ignition after staging */
    double  amin;           /* *io  (m/s2)      Minimum longitudinal acceleration */
    double  char_time1;     /* *io  (s)         Characteristic time 'tau' of stage 1 */
    double  char_time2;     /* *io  (s)         Characteristic time 'tau' of stage 2 */
    double  char_time3;     /* *io  (s)         Characteristic time 'tau' of stage 3 */
    double  exhaust_vel1;   /* *io  (m/s)       Exhaust velocity of stage 1 */
    double  exhaust_vel2;   /* *io  (m/s)       Exhaust velocity of stage 2 */
    double  exhaust_vel3;   /* *io  (m/s)       Exhaust velocity of stage 3 */
    double  burnout_epoch1; /* *io  (s)         Burn out of stage 1 at 'time_ltg' */
    double  burnout_epoch2; /* *io  (s)         Burn out of stage 2 at 'time_ltg' */
    double  burnout_epoch3; /* *io  (s)         Burn out of stage 3 at 'time_ltg' */
    double  lamd_limit;     /* *io  (1/s)       Limiter on 'lamd' */
    arma::vec  RGRAV;       /* *io  (m)         Postion loss due to gravity */
    double  _RGRAV[3];       /* *io  (m)         Postion loss due to gravity */
    arma::vec  RGO;         /* *io  (m)         Range-to-go vector */
    double  _RGO[3];         /* *io  (m)         Range-to-go vector */
    arma::vec  VGO;         /* *io  (m/s)       Velocity still to be gained */
    double  _VGO[3];         /* *io  (m/s)       Velocity still to be gained */
    arma::vec  SDII;        /* *io  (m)         Desired inertial position */
    double  _SDII[3];        /* *io  (m)         Desired inertial position */
    arma::vec  UD;          /* *io  (--)        Unit vector of SPII and SDII */
    double  _UD[3];          /* *io  (--)        Unit vector of SPII and SDII */
    arma::vec  UY;          /* *io  (--)        Unit vector normal to traj plane */
    double  _UY[3];          /* *io  (--)        Unit vector normal to traj plane */
    arma::vec  UZ;          /* *io  (--)        Unit vector in traj plane, normal to SBBI_D */
    double  _UZ[3];          /* *io  (--)        Unit vector in traj plane, normal to SBBI_D */
    double  vgom;           /* *io  (m/s)       Velocity to be gained magnitude */
    double  tgo;            /* *io  (s)         Time to go to desired end state */
    int     nst;            /* *io  (--)        N-th stage number */
    arma::vec  ULAM;        /* *io  (--)        Unit thrust vector in VGO direction */
    double  _ULAM[3];        /* *io  (--)        Unit thrust vector in VGO direction */
    arma::vec  LAMD;       /* *io  (1/s)       Thrust vector turning rate */
    double  _LAMD[3];       /* *io  (1/s)       Thrust vector turning rate */
    int     nstmax;         /* *io  (--)        # of stages needed to meet end state */
    double  lamd;           /* *io  (1/s)       Magnitude of LAMD */
    double  dpd;            /* *io  (m)         Distance of the predicted from the desired end-point */
    double  dbd;            /* *io  (m)         Distance of vehicle from the desired end-point */
    double  ddb;            /* *io  (m)         Position error at BECO */
    double  dvdb;           /* *io  (m/s)       Distance of vehicle from the desired end-point */
    double  thtvddbx;       /* *io  (d)         Angle error at BECO */
};

#endif  // __GUIDANCE_HH__


#include <algorithm>
#include <iterator>
#include <stdexcept>
#include <vector>
#include <boost/iterator/counting_iterator.hpp>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <Euclid/Geometry/TriMeshGeometry.h>

namespace Euclid
{

namespace _impl
{

// Construct linear system for mesh
template<typename Mesh, typename FT>
void construct_equation(const Mesh& mesh,
                        const std::vector<int>& ids,
                        Eigen::SparseMatrix<FT>& A,
                        Eigen::SparseMatrix<FT>& B)
{
    using SpMat = Eigen::SparseMatrix<FT>;
    const auto inv_sigma = static_cast<FT>(1.0); // parameter

    auto fimap = get(boost::face_index, mesh);
    auto vpmap = get(boost::vertex_point, mesh);
    auto m = B.cols();
    auto n = B.rows();
    auto sum = 0.0;
    auto n_faces = num_faces(mesh);
    std::vector<int> rows;
    std::vector<int> cols;
    std::vector<FT> edge_len;
    std::vector<FT> ds;
    rows.reserve(3 * n_faces);
    cols.reserve(3 * n_faces);
    edge_len.reserve(3 * n_faces);
    ds.reserve(3 * n_faces);

    // Compute the ingredients of the laplacian matrix
    for (auto fa : faces(mesh)) {
        auto fa_id = ids[get(fimap, fa)];
        auto na = Euclid::face_normal(fa, mesh);

        for (auto ha : CGAL::halfedges_around_face(halfedge(fa, mesh), mesh)) {
            auto hb = opposite(ha, mesh);
            if (!is_border(hb, mesh)) { // Non-boundary

                // Determine whether the incident edge is concave or convex
                auto pa = get(vpmap, target(next(ha, mesh), mesh));
                auto pb = get(vpmap, target(next(hb, mesh), mesh));
                auto p = pb - pa;
                auto eta = p * na <= 0.0 ? 0.2 : 1.0;

                auto fb = face(hb, mesh);
                auto fb_id = ids[get(fimap, fb)];
                auto nb = Euclid::face_normal(fb, mesh);
                auto diff = 0.5 * eta * (na - nb).squared_length();

                cols.push_back(fa_id);
                rows.push_back(fb_id);
                ds.push_back(diff);
                edge_len.push_back(Euclid::edge_length(ha, mesh));
                sum += diff;
            }
            else { // Placeholder
                ds.push_back(-1.0);
            }
        }
    }

    // Normalize
    std::vector<Eigen::Triplet<FT>> values;
    values.reserve(cols.size());
    auto inv_avg = edge_len.size() / (sum * 0.5);
    auto d_iter = ds.begin();
    auto e_iter = edge_len.begin();
    auto c_iter = cols.begin();
    auto r_iter = rows.begin();
    while (d_iter != ds.end()) {
        auto prob_sum = 0.0;
        for (auto j = 0; j < 3; ++j) {
            if (*d_iter != -1.0) {
                auto prob =
                    std::exp(-(*d_iter * inv_avg) * inv_sigma) * *e_iter++;
                *d_iter++ = -prob;
                prob_sum += prob;
            }
            else {
                ++d_iter;
            }
        }
        prob_sum = 1.0 / prob_sum;
        d_iter -= 3;
        for (auto j = 0; j < 3; ++j) {
            if (*d_iter != -1.0) {
                values.emplace_back(*c_iter++, *r_iter++, *d_iter++ * prob_sum);
            }
            else {
                ++d_iter;
            }
        }
    }

    for (unsigned i = 0; i < num_faces(mesh); ++i) {
        values.emplace_back(i, i, static_cast<FT>(1.0));
    }

    // Fill in the matrix
    SpMat L(m + n, m + n);
    L.setFromTriplets(values.begin(), values.end());
    A = L.bottomRightCorner(n, n);
    B = -L.bottomLeftCorner(n, m);
    A.makeCompressed();
    B.makeCompressed();
}

// // Construct linear system for point cloud
// template<typename ForwardIterator,
//          typename Index,
//          typename PPMap,
//          typename NPMap,
//          typename FT>
// void construct_equation(ForwardIterator first,
//                         ForwardIterator beyond,
//                         PPMap point_pmap,
//                         NPMap normal_pmap,
//                         const std::vector<std::vector<int>>& neighbors,
//                         const std::vector<Index>& indices,
//                         const std::vector<int>& ids,
//                         Eigen::SparseMatrix<FT>& A,
//                         Eigen::SparseMatrix<FT>& B)
// {
//     using SpMat = Eigen::SparseMatrix<FT>;

//     const auto inv_sigma1 = 1.0;
//     const auto inv_sigma2 = 1.0;
//     auto m = B.cols();
//     auto n = B.rows();
//     auto n_points = m + n;
//     auto n_neighbors = neighbors[0].size();
//     std::vector<int> rows;
//     std::vector<int> cols;
//     std::vector<FT> d1s;
//     std::vector<FT> d2s;
//     rows.reserve(n_points * n_neighbors);
//     cols.reserve(n_points * n_neighbors);
//     d1s.reserve(n_points * n_neighbors);
//     d2s.reserve(n_points * n_neighbors);
//     auto d2_sum = 0.0;

//     // Compute the ingredients of the laplacian matrix
//     auto i = 0;
//     for (auto iter = first; iter != beyond; ++iter, ++i) {
//         auto pi = get(point_pmap, *iter);
//         auto ni = get(normal_pmap, *iter);
//         auto d1_sum = 0.0;

//         for (auto j = 0; j < n_neighbors; ++j) {
//             auto neighbor = neighbors[i][j];
//             cols.push_back(ids[i]);
//             rows.push_back(ids[neighbor]);

//             auto pj = get(point_pmap, indices[neighbor]);
//             auto nj = get(normal_pmap, indices[neighbor]);
//             auto eta = ((pj - pi) - ((pj - pi) * ni) * ni) * nj >= 0.0
//                            ? 0.2
//                            : 1.0; // convex : concave

//             auto d1 = (pi - pj).squared_length();
//             d1s.push_back(d1);
//             d1_sum += d1;
//             auto d2 = 0.5 * eta * (ni - nj).squared_length();
//             d2s.push_back(d2);
//             d2_sum += d2;
//         }

//         auto d1_inv_avg = n_neighbors / d1_sum;
//         for (auto j = 0; j < n_neighbors; ++j) {
//             auto idx = i * n_neighbors + j;
//             d1s[idx] = std::exp(-d1s[idx] * d1_inv_avg * inv_sigma1);
//         }
//     }

//     auto d2_inv_avg = (n_points * n_neighbors) / d2_sum;
//     for (auto& d2 : d2s) {
//         d2 = std::exp(-d2 * d2_inv_avg * inv_sigma2);
//     }

//     // Normalize
//     std::vector<Eigen::Triplet<FT>> values;
//     values.reserve(n_points * (n_neighbors + 1));
//     for (auto i = 0; i < n_points; ++i) {
//         std::vector<FT> ds(n_neighbors);
//         FT sum = 0.0;
//         for (auto j = 0; j < n_neighbors; ++j) {
//             auto idx = i * n_neighbors + j;
//             ds[j] = d1s[idx] * d2s[idx];
//             sum += ds[j];
//         }
//         FT inv_sum = 1.0 / sum;
//         for (auto j = 0; j < n_neighbors; ++j) {
//             auto idx = i * n_neighbors + j;
//             values.emplace_back(cols[idx], rows[idx], -ds[j] * inv_sum);
//         }
//         values.emplace_back(i, i, static_cast<FT>(1.0));
//     }

//     // Fill in the matrix
//     SpMat L(m + n, m + n);
//     L.setFromTriplets(values.begin(), values.end());
//     A = L.bottomRightCorner(n, n);
//     B = -L.bottomLeftCorner(n, m);
//     A.makeCompressed();
//     B.makeCompressed();
// }

// // Adapt PointPropertyMap to take in integer as key type
// template<typename PPMap>
// class IPMapAdaptor
// {
//     using PointKey = typename boost::property_traits<PPMap>::key_type;
//     using Point_3 = typename boost::property_traits<PPMap>::value_type;

// public:
//     using value_type = Point_3;
//     using reference = const value_type&;
//     using key_type = int;
//     using category = boost::lvalue_property_map_tag;

//     IPMapAdaptor(PPMap ppmap, const std::vector<PointKey>& pks)
//         : _ppmap(ppmap), _pks(pks)
//     {}

//     reference operator[](key_type key) const { return _ppmap[_pks[key]]; }

//     friend reference get(IPMapAdaptor ipmap, key_type key)
//     {
//         return ipmap[key];
//     }

// private:
//     PPMap _ppmap;
//     const std::vector<PointKey>& _pks;
// };

} // namespace _impl

template<typename Mesh>
void random_walk_segmentation(const Mesh& mesh,
                              const std::vector<unsigned>& seeds,
                              std::vector<unsigned>& segments)
{
    using VertexPointMap =
        typename boost::property_map<Mesh, boost::vertex_point_t>::type;
    using Point_3 = typename boost::property_traits<VertexPointMap>::value_type;
    using FT = typename CGAL::Kernel_traits<Point_3>::Kernel::FT;
    using SpMat = Eigen::SparseMatrix<FT>;

    // Construct the linear equation
    auto seed_indices = seeds;
    auto m = static_cast<int>(seed_indices.size()); // Number of seeded
    auto n = static_cast<int>(num_faces(mesh)) - m; // Number of unseeded
    std::sort(seed_indices.begin(), seed_indices.end());
    std::vector<int> ids(m + n, -1); // ids[FacetID] -> MatrixID
    size_t inc = 0;
    while (inc < seed_indices.size()) {
        ids[seed_indices[inc]] = inc;
        ++inc;
    }
    for (auto& id : ids) {
        if (id == -1) {
            id = inc++;
        }
    }
    SpMat A(n, n);
    SpMat B(n, m);
    _impl::construct_equation(mesh, ids, A, B);

    std::vector<int> inv_ids(m + n);
    for (auto i = 0; i < m + n; ++i) {
        inv_ids[ids[i]] = i;
    }

    // Solve the equation to segment
    segments.resize(n + m, 0);
    for (auto s : seed_indices) {
        segments[s] = s;
    }
    std::vector<FT> max_probabilities(n, static_cast<FT>(-1.0));
    Eigen::SparseLU<SpMat> solver;
    solver.compute(A);
    if (solver.info() != Eigen::Success) {
        throw std::runtime_error(solver.lastErrorMessage());
    }
    for (auto i = 0; i < m; ++i) {
        Eigen::Matrix<FT, Eigen::Dynamic, 1> b = B.col(i);
        Eigen::Matrix<FT, Eigen::Dynamic, 1> x = solver.solve(b);
        for (auto j = 0; j < n; ++j) {
            if (x(j, 0) > max_probabilities[j]) {
                max_probabilities[j] = x(j, 0);
                segments[inv_ids[j + m]] = seed_indices[i];
            }
        }
    }
}

// template<typename ForwardIterator, typename PPMap, typename NPMap>
// void random_walk_segmentation(ForwardIterator first,
//                               ForwardIterator beyond,
//                               PPMap point_pmap,
//                               NPMap normal_pmap,
//                               const std::vector<unsigned>& seeds,
//                               std::vector<unsigned>& segments)
// {

//     using Index = std::iterator_traits<ForwardIterator>::value_type;
//     using Point_3 = boost::property_traits<PPMap>::value_type;
//     using Vector_3 = boost::property_traits<NPMap>::value_type;
//     using Kernel = CGAL::Kernel_traits<Point_3>::Kernel;
//     using IPMap = _impl::IPMapAdaptor<PPMap>;
//     using BaseTraits = CGAL::Search_traits_3<Kernel>;
//     using KdTreeTraits = CGAL::Search_traits_adapter<int, IPMap, BaseTraits>;
//     using KNN = CGAL::Orthogonal_k_neighbor_search<KdTreeTraits>;
//     using KdTree = typename KNN::Tree;
//     using Splitter = KdTree::Splitter;
//     using Distance = KNN::Distance;
//     using FT = Kernel::FT;
//     using SpMat = Eigen::SparseMatrix<FT>;
//     const int k = 6; // Use 6 neighbors according to Euler formula

//     // Construct the IPMap
//     std::vector<Index> indices;
//     int n = 0;
//     for (auto iter = first; iter != beyond; ++iter) {
//         indices.push_back(*iter);
//         ++n;
//     }
//     IPMap ipmap(point_pmap, indices);

//     // Construct the k-d tree
//     KdTree tree(boost::counting_iterator<int>(0),
//                 boost::counting_iterator<int>(n),
//                 Splitter(),
//                 KdTreeTraits(ipmap));
//     Distance dist(ipmap);

//     // Query neighbors for all points and store the neighbors' indices
//     std::vector<std::vector<int>> neighbors;
//     for (auto iter = first; iter < beyond; ++iter) {
//         std::vector<int> neighbors_i;
//         neighbors_i.reserve(k);

//         auto p = point_pmap[*iter];
//         KNN knn(tree, p, k + 1, static_cast<FT>(0.00001), true, dist);

//         auto it = knn.begin();
//         ++it; // The first will always return the identical point
//         for (it; it != knn.end(); ++it) {
//             neighbors_i.push_back(it->first);
//         }

//         neighbors.push_back(neighbors_i);
//     }

//     // Construct the linear equation
//     auto seed_indices = seeds;
//     auto m = static_cast<int>(seed_indices.size()); // Number of seeded
//     n -= m;                                         // Number of unseeded
//     std::sort(seed_indices.begin(), seed_indices.end());
//     SpMat A(n, n);
//     SpMat B(n, m);
//     std::vector<int> ids(m + n, -1); // ids[PointID] -> MatrixID
//     int inc = 0;
//     while (inc < seed_indices.size()) {
//         ids[seed_indices[inc]] = inc;
//         ++inc;
//     }
//     for (auto& id : ids) {
//         if (id == -1) { id = inc++; }
//     }
//     _impl::construct_equation(
//         first, beyond, point_pmap, normal_pmap, neighbors, indices, ids, A,
//         B);

//     std::vector<int> inv_ids(m + n);
//     for (auto i = 0; i < m + n; ++i) {
//         inv_ids[ids[i]] = i;
//     }

//     // Solve the equation to segment
//     segments.resize(n + m, 0);
//     for (auto s : seed_indices) {
//         segments[s] = s;
//     }
//     std::vector<FT> max_probabilities(n, static_cast<FT>(-1.0));
//     Eigen::SparseLU<SpMat> solver;
//     solver.compute(A);
//     if (solver.info() != Eigen::Success) {
//         std::cerr << solver.lastErrorMessage() << std::endl;
//         return;
//     }
//     for (auto i = 0; i < m; ++i) {
//         Eigen::Matrix<FT, Eigen::Dynamic, 1> b = B.col(i);
//         Eigen::Matrix<FT, Eigen::Dynamic, 1> x = solver.solve(b);
//         for (auto j = 0; j < n; ++j) {
//             if (x(j, 0) > max_probabilities[j]) {
//                 max_probabilities[j] = x(j, 0);
//                 segments[inv_ids[j + m]] = seed_indices[i];
//             }
//         }
//     }
// }

} // namespace Euclid


#ifndef PRODUCTQUANTIZER_HPP
#define PRODUCTQUANTIZER_HPP

#include <Eigen/Dense>
#include "../iterator/iterator.hpp"

/**
 * @brief well known product quantizer
 */
template<typename T, uint D, uint C, uint P>
class productquantizer {

    typedef Eigen::Matrix < T, D / P, 1 > vSegment_t;

    static_assert(D % P == 0, "dim = 0 mod p failed");
    static_assert(C % 2 == 0, "cells = 0 mod 2 failed");

public:

    // ================================================================================
    // Methods
    // ================================================================================

    productquantizer() {
        _raw_centroids = new T[C * D];
        _centroids = Eigen::Map<Eigen::Matrix<T, C, D>>(_raw_centroids, C, D);
    }

    ~productquantizer() {
        delete[] _raw_centroids;
    }

    /**
     * @brief expectation step of Lloyd-iteration
     * @details assign each given vector from the iterator to coarse cluster part-wise
     *
     * @param iter [description]
     */
    void getAssignment(iterator<T, D> &iter) {
        // for each vector ...
        for (uint n = 0, n_e = iter.num(); n < n_e; ++n) {
            // for each part ...
            for (uint p = 0; p < P; ++p) {
                // extract part of base_vector
                vSegment_t vec = iter[n].segment(p * S, S);

                // find minimum
                uint bestIdx = 0;
                T bestDist = HUGE_VAL;

                // for each cluster
                for (uint c = 0, c_e = _step; c < c_e; ++c) {
                    vSegment_t cec = _centroids.row(c).segment(p * S, S);
                    T curDist = (vec - cec).squaredNorm();
                    if ( curDist  < bestDist ) {
                        bestDist = curDist;
                        bestIdx = c;
                    }
                }

                _mapping[n * P + p] = bestIdx;
                _distances[n * P + p] = bestDist;
            }
        }
    }

    /**
     * @brief maximization step of Lloyd-iteration
     * @details centroids should be the mean of all cluster vectors
     *
     * @param iter [description]
     */
    void updateCentroids(iterator<T, D> &iter) {
        _centroids = Eigen::Matrix<T, C, D>::Zero(C, D);
        T centerCounter[C * P] = {0};
        // find mean
        for (uint n = 0, n_e = iter.num(); n < n_e; ++n) {
            for (uint p = 0; p < P; ++p) {
                const uint c = _mapping[n * P + p];
                _centroids.row(c).segment(p * S, S) += iter[n].segment(p * S, S);
                ++centerCounter[p * C + c ];
            }
        }
        for (uint c = 0; c < C; ++c) {
            for (uint p = 0; p < P; ++p) {
                if (centerCounter[p * C + c] != 0)
                    _centroids.row(c).segment(p * S, S).array() /= static_cast<T>(centerCounter[p * C + c]);
            }
        }
    }

    /**
     * @brief split each centroid +- eps
     * @details [long description]
     */
    void augmentCentroids() {
        for (uint i = 0; i < _step; ++i) {
            _centroids.row(i + _step) = _centroids.row(i).array() + 0.001;
            _centroids.row(i) = _centroids.row(i).array() - 0.001;
        }
        _step *= 2;
    }

    /**
     * @brief compute squared loss as distorsion distances
     * @details [long description]
     *
     * @param iter [description]
     * @return squared loss
     */
    T loss(iterator<T, D> &iter) {
        T sum = 0;
        const uint N = iter.num() * P;

        #pragma omp parallel for reduction(+:sum)
        for (uint i = 0; i < N; ++i) {
            sum += _distances[i];
        }
        return sum;
        // Eigen::MatrixXf l = Eigen::Map<  Eigen::MatrixXf >(_distances, iter.num(), P);
        // return l.sum();
    }

    /**
     * @brief start k-means
     * @details [long description]
     *
     * @param iter vector collection that be be clusterized
     */
    void generate(iterator<T, D> &iter) {
        _step = 1;

        _mapping = new uint8_t[iter.num()*P]();
        _distances = new T[iter.num()*P]();

        _centroids.row(0) = iter.center();

        T currentLoss = 0.0;
        T lastLoss    = 0.0;

        do {
            uint run = 1000;
            augmentCentroids();
            do {
                lastLoss = currentLoss;
                getAssignment(iter);    // E step
                updateCentroids(iter);  // M step
                currentLoss = loss(iter);
                //std::cout << "loss "<<currentLoss<<std::endl;
                run--;
            } while (  (abs(lastLoss - currentLoss) >   0.005) && (run > 0) );
        } while (_step < C );

    }

    // ================================================================================
    // Variables
    // ================================================================================

    const uint S = D / P;
    T* _raw_centroids;
    Eigen::Matrix<T, C, D> _centroids;  // centroids of all clusters
    uint _step;                         // current number of centroids
    uint8_t *_mapping;                  // current mapping of all vectors from iter
    T *_distances;                      // current distorsion sq. distances between iter and centroids

};

#endif


//
//  IglUtils.hpp
//  DOT
//
//  Created by Minchen Li on 8/30/17.
//

#ifndef IglUtils_hpp
#define IglUtils_hpp

#include "Mesh.hpp"

#ifdef USE_CLOSEDFORMSVD2D
#include "ClosedFormSVD2d.hpp"
#else
#include "AutoFlipSVD.hpp"
#endif

#include "LinSysSolver.hpp"

#include <Eigen/Eigen>

#include <iostream>
#include <fstream>

namespace DOT {

    // a static class implementing basic geometry processing operations that are not provided in libIgl
    class IglUtils {

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    public:
        static void mapTriangleTo2D(const Eigen::Vector3d v[3], Eigen::Vector2d u[3]);
        static void computeDeformationGradient(const Eigen::Vector3d v[3], const Eigen::Vector2d u[3], Eigen::Matrix2d& F);

        static void mapScalarToColor_bin(const Eigen::VectorXd& scalar, Eigen::MatrixXd& color, double thres);
        static void mapScalarToColor(const Eigen::VectorXd& scalar, Eigen::MatrixXd& color,
                                     double lowerBound, double upperBound, int opt = 0);

        static void addBlockToMatrix(Eigen::SparseMatrix<double>& mtr, const Eigen::MatrixXd& block,
                                     const Eigen::VectorXi& index, int dim);
        template<int blockSize, int indexSize>
        static void addBlockToMatrix(const Eigen::Matrix<double, blockSize, blockSize>& block,
                                     const Eigen::Matrix<int, indexSize, 1>& index,
                                     int dim, Eigen::VectorXd* V,
                                     Eigen::VectorXi* I = NULL,
                                     Eigen::VectorXi* J = NULL)
        {
            assert(indexSize * dim == blockSize);

            int num_free = 0;
            for(int indI = 0; indI < indexSize; indI++) {
                if(index[indI] >= 0) {
                    num_free++;
                }
            }
            if(!num_free) {
                return;
            }

            assert(V);
            int tripletInd = static_cast<int>(V->size());
            const int entryAmt = static_cast<int>(dim * dim * num_free * num_free);
            V->conservativeResize(tripletInd + entryAmt);
            if(I) {
                assert(J);
                assert(I->size() == tripletInd);
                assert(J->size() == tripletInd);
                I->conservativeResize(tripletInd + entryAmt);
                J->conservativeResize(tripletInd + entryAmt);
            }

            for(int indI = 0; indI < indexSize; indI++) {
                if(index[indI] < 0) {
                    continue;
                }
                int startIndI = index[indI] * dim;
                int startIndI_block = indI * dim;

                for(int indJ = 0; indJ < indexSize; indJ++) {
                    if(index[indJ] < 0) {
                        continue;
                    }
                    int startIndJ = index[indJ] * dim;
                    int startIndJ_block = indJ * dim;

                    for(int dimI = 0; dimI < dim; dimI++) {
                        for(int dimJ = 0; dimJ < dim; dimJ++) {
                            (*V)[tripletInd] = block(startIndI_block + dimI, startIndJ_block + dimJ);
                            if(I) {
                                (*I)[tripletInd] = startIndI + dimI;
                                (*J)[tripletInd] = startIndJ + dimJ;
                            }
                            tripletInd++;
                        }
                    }
                }
            }
            assert(tripletInd == V->size());
        }
        static void addDiagonalToMatrix(const Eigen::VectorXd& diagonal,
                                        const Eigen::VectorXi& index,
                                        int dim, Eigen::VectorXd* V,
                                        Eigen::VectorXi* I = NULL,
                                        Eigen::VectorXi* J = NULL);
        template<int dim>
        static void addBlockToMatrix(const Eigen::Matrix<double, dim, dim * (dim + 1)>& block,
                                     const Eigen::Matrix<int, 1, dim + 1>& index, int rowIndI,
                                     Eigen::MatrixXd& hessian)
        {
            int rowStart = index[rowIndI] * dim;
            if(rowStart < 0) {
                rowStart = -rowStart - dim;
                hessian.diagonal().segment(rowStart, dim).setOnes();
                return;
            }

            if(index[0] >= 0) {
                int _dimIndex0 = index[0] * dim;
                hessian.block<dim, dim>(rowStart, _dimIndex0) += block.block(0, 0, dim, dim);
            }

            if(index[1] >= 0) {
                int _dimIndex1 = index[1] * dim;
                hessian.block<dim, dim>(rowStart, _dimIndex1) += block.block(0, dim, dim, dim);
            }

            if(index[2] >= 0) {
                int _2dim = 2 * dim;
                int _dimIndex2 = index[2] * dim;
                hessian.block<dim, dim>(rowStart, _dimIndex2) += block.block(0, _2dim, dim, dim);
            }

            if(dim == 3) {
                if(index[3] >= 0) {
                    int _3dim = 3 * dim;
                    int _dimIndex3 = index[3] * dim;
                    hessian.block<dim, dim>(rowStart, _dimIndex3) += block.block(0, _3dim, dim, dim);
                }
            }
        }
        template<int dim>
        static void addBlockToMatrix(const Eigen::Matrix<double, dim, dim * (dim + 1)>& block,
                                     const Eigen::Matrix<int, 1, dim + 1>& index, int rowIndI,
                                     LinSysSolver<Eigen::VectorXi, Eigen::VectorXd>* linSysSolver)
        {
            int rowStart = index[rowIndI] * dim;
            if(rowStart < 0) {
                rowStart = -rowStart - dim;
                linSysSolver->setCoeff(rowStart, rowStart, 1.0);
                linSysSolver->setCoeff(rowStart + 1, rowStart + 1, 1.0);
                if(dim == 3) {
                    linSysSolver->setCoeff(rowStart + 2, rowStart + 2, 1.0);
                }
                return;
            }

            if(index[0] >= 0) {
                int _dimIndex0 = index[0] * dim;
                linSysSolver->addCoeff(rowStart, _dimIndex0, block(0, 0));
                linSysSolver->addCoeff(rowStart, _dimIndex0 + 1, block(0, 1));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex0, block(1, 0));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex0 + 1, block(1, 1));
                if(dim == 3) {
                    linSysSolver->addCoeff(rowStart, _dimIndex0 + 2, block(0, 2));
                    linSysSolver->addCoeff(rowStart + 1, _dimIndex0 + 2, block(1, 2));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex0, block(2, 0));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex0 + 1, block(2, 1));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex0 + 2, block(2, 2));
                }
            }

            if(index[1] >= 0) {
                int _dimIndex1 = index[1] * dim;
                linSysSolver->addCoeff(rowStart, _dimIndex1, block(0, dim));
                linSysSolver->addCoeff(rowStart, _dimIndex1 + 1, block(0, dim + 1));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex1, block(1, dim));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex1 + 1, block(1, dim + 1));
                if(dim == 3) {
                    linSysSolver->addCoeff(rowStart, _dimIndex1 + 2, block(0, dim + 2));
                    linSysSolver->addCoeff(rowStart + 1, _dimIndex1 + 2, block(1, dim + 2));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex1, block(2, dim));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex1 + 1, block(2, dim + 1));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex1 + 2, block(2, dim + 2));
                }
            }

            if(index[2] >= 0) {
                int _2dim = 2 * dim;
                int _dimIndex2 = index[2] * dim;
                linSysSolver->addCoeff(rowStart, _dimIndex2, block(0, _2dim));
                linSysSolver->addCoeff(rowStart, _dimIndex2 + 1, block(0, _2dim + 1));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex2, block(1, _2dim));
                linSysSolver->addCoeff(rowStart + 1, _dimIndex2 + 1, block(1, _2dim + 1));
                if(dim == 3) {
                    linSysSolver->addCoeff(rowStart, _dimIndex2 + 2, block(0, _2dim + 2));
                    linSysSolver->addCoeff(rowStart + 1, _dimIndex2 + 2, block(1, _2dim + 2));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex2, block(2, _2dim));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex2 + 1, block(2, _2dim + 1));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex2 + 2, block(2, _2dim + 2));
                }
            }

            if(dim == 3) {
                if(index[3] >= 0) {
                    int _3dim = 3 * dim;
                    int _dimIndex3 = index[3] * dim;
                    linSysSolver->addCoeff(rowStart, _dimIndex3, block(0, _3dim));
                    linSysSolver->addCoeff(rowStart, _dimIndex3 + 1, block(0, _3dim + 1));
                    linSysSolver->addCoeff(rowStart, _dimIndex3 + 2, block(0, _3dim + 2));
                    linSysSolver->addCoeff(rowStart + 1, _dimIndex3, block(1, _3dim));
                    linSysSolver->addCoeff(rowStart + 1, _dimIndex3 + 1, block(1, _3dim + 1));
                    linSysSolver->addCoeff(rowStart + 1, _dimIndex3 + 2, block(1, _3dim + 2));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex3, block(2, _3dim));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex3 + 1, block(2, _3dim + 1));
                    linSysSolver->addCoeff(rowStart + 2, _dimIndex3 + 2, block(2, _3dim + 2));
                }
            }
        }
        template<int dim>
        static void addIdBlockToMatrixDiag(const Eigen::VectorXi& index,
                                           LinSysSolver<Eigen::VectorXi, Eigen::VectorXd>* linSysSolver)
        {
            for(int indI = 0; indI < index.size(); indI++) {
                int rowStart = index[indI] * dim;
                assert(rowStart >= 0);
                linSysSolver->addCoeff(rowStart, rowStart, 1.0);
                linSysSolver->addCoeff(rowStart + 1, rowStart + 1, 1.0);
                if(dim == 3) {
                    linSysSolver->addCoeff(rowStart + 2, rowStart + 2, 1.0);
                }
            }
        }

        template<typename Scalar, int size>
        static void symmetrizeMatrix(Eigen::Matrix<Scalar, size, size>& mtr) {
            if(size == Eigen::Dynamic) {
                assert(mtr.rows() == mtr.cols());
            }
            int rows = ((size == Eigen::Dynamic) ? mtr.rows() : size);

            for(int rowI = 0; rowI < rows; rowI++) {
                for(int colI = rowI + 1; colI < rows; colI++) {
                    double &a = mtr(rowI, colI), &b = mtr(colI, rowI);
                    a = b = (a + b) / 2.0;
                }
            }
        }

        // project a symmetric real matrix to the nearest SPD matrix
        template<typename Scalar, int size>
        static void makePD(Eigen::Matrix<Scalar, size, size>& symMtr) {
            Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, size, size>> eigenSolver(symMtr);
            if(eigenSolver.eigenvalues()[0] >= 0.0) {
                return;
            }
            Eigen::DiagonalMatrix<Scalar, size> D(eigenSolver.eigenvalues());
            int rows = ((size == Eigen::Dynamic) ? symMtr.rows() : size);
            for(int i = 0; i < rows; i++) {
                if(D.diagonal()[i] < 0.0) {
                    D.diagonal()[i] = 0.0;
                }
                else {
                    break;
                }
            }
            symMtr = eigenSolver.eigenvectors() * D * eigenSolver.eigenvectors().transpose();
        }
        template<typename Scalar, int size>
        static void makePD2d(Eigen::Matrix<Scalar, size, size>& symMtr)
        {
            // based on http://www.math.harvard.edu/archive/21b_fall_04/exhibits/2dmatrices/

            if(size == Eigen::Dynamic) {
                assert(symMtr.rows() == 2);
            }
            else {
                assert(size == 2);
            }

            const double a = symMtr(0, 0);
            const double b = (symMtr(0, 1) + symMtr(1, 0)) / 2.0;
            const double d = symMtr(1, 1);

            double b2 = b * b;
            const double D = a * d - b2;
            const double T_div_2 = (a + d) / 2.0;
            const double sqrtTT4D = std::sqrt(T_div_2 * T_div_2 - D);
            const double L2 = T_div_2 - sqrtTT4D;
            if(L2 < 0.0) {
                const double L1 = T_div_2 + sqrtTT4D;
                if(L1 <= 0.0) {
                    symMtr.setZero();
                }
                else {
                    if(b2 == 0.0) {
                        symMtr << L1, 0.0, 0.0 ,0.0;
                    }
                    else {
                        const double L1md = L1 - d;
                        const double L1md_div_L1 = L1md / L1;
                        symMtr(0, 0) = L1md_div_L1 * L1md;
                        symMtr(0, 1) = symMtr(1, 0) = b * L1md_div_L1;
                        symMtr(1, 1) = b2 / L1;
                    }
                }
            }
        }
        template<typename Scalar, int size>
        static void flipDet_SVD(Eigen::Matrix<Scalar, size, size>& mtr) {
            Eigen::JacobiSVD<Eigen::Matrix<Scalar, size, size>> svd(mtr, Eigen::ComputeFullU | Eigen::ComputeFullV);

            Eigen::Matrix<Scalar, size, size> U = svd.matrixU(), V = svd.matrixV();
            if(U.determinant() < 0) {
                U.col(U.cols() - 1) *= -1.0;
            }
            if(V.determinant() < 0) {
                V.col(V.cols() - 1) *= -1.0;
            }
            mtr = U * Eigen::DiagonalMatrix<Scalar, size>(svd.singularValues()) * V.transpose();
        }

        static void writeSparseMatrixToFile(const std::string& filePath,
                                            const Eigen::SparseMatrix<double>& mtr,
                                            bool MATLAB = false);
        static void writeSparseMatrixToFile(const std::string& filePath,
                                            const Eigen::VectorXi& I, const Eigen::VectorXi& J,
                                            const Eigen::VectorXd& V, bool MATLAB = false);
        static void writeSparseMatrixToFile(const std::string& filePath,
                                            const std::map<std::pair<int, int>, double>& mtr,
                                            bool MATLAB = false);
        static void writeSparseMatrixToFile(const std::string& filePath,
                                            LinSysSolver<Eigen::VectorXi, Eigen::VectorXd>* linSysSolver,
                                            bool MATLAB = false);
        static void writeDenseMatrixToFile(const std::string& filePath,
                                           const Eigen::MatrixXd& matrix,
                                           bool MATLAB = false);
        static void loadSparseMatrixFromFile(const std::string& filePath,
                                             Eigen::SparseMatrix<double>& mtr);

        static void sparseMatrixToTriplet(const Eigen::SparseMatrix<double>& mtr,
                                          Eigen::VectorXi& I, Eigen::VectorXi& J, Eigen::VectorXd& V);
        static void sparseMatrixToTriplet(const Eigen::SparseMatrix<double>& mtr, Eigen::VectorXd& V);

        static void writeVectorToFile(const std::string& filePath,
                                      const Eigen::VectorXd& vec);
        static void readVectorFromFile(const std::string& filePath,
                                       Eigen::VectorXd& vec);

        static const std::string rtos(double real);

        static double computeRotAngle(const Eigen::RowVector2d& from, const Eigen::RowVector2d& to);

        // test wether 2D segments ab intersect with cd
        static bool Test2DSegmentSegment(const Eigen::RowVector2d& a, const Eigen::RowVector2d& b,
                                         const Eigen::RowVector2d& c, const Eigen::RowVector2d& d,
                                         double eps = 0.0);

        static void addThickEdge(Eigen::MatrixXd& V, Eigen::MatrixXi& F, Eigen::MatrixXd& UV,
                                 Eigen::MatrixXd& seamColor, const Eigen::RowVector3d& color,
                                 const Eigen::RowVector3d& v0, const Eigen::RowVector3d& v1,
                                 double halfWidth, double texScale, bool UVorSurface = false,
                                 const Eigen::RowVector3d& normal = Eigen::RowVector3d());

        static void findSurfaceTris(const Eigen::MatrixXi& TT, Eigen::MatrixXi& F);
        static void buildSTri2Tet(const Eigen::MatrixXi& F, const Eigen::MatrixXi& SF,
                                  std::vector<int>& sTri2Tet);

        static void saveTetMesh(const std::string& filePath,
                                const Eigen::MatrixXd& TV, const Eigen::MatrixXi& TT,
                                const Eigen::MatrixXi& F = Eigen::MatrixXi(),
                                bool findSurface = true);
        static bool readTetMesh(const std::string& filePath,
                                Eigen::MatrixXd& TV, Eigen::MatrixXi& TT,
                                Eigen::MatrixXi& F, bool findSurface = true);
        static void readNodeEle(const std::string& filePath,
                                Eigen::MatrixXd& TV, Eigen::MatrixXi& TT,
                                Eigen::MatrixXi& F);

        static void smoothVertField(const Mesh<DIM>& mesh, Eigen::VectorXd& field);

        static void compute_dF_div_dx(const Eigen::Matrix<double, DIM, DIM>& A,
                                      Eigen::Matrix<double, DIM * (DIM + 1), DIM * DIM>& dF_div_dx);
        template<int colSize>
        static void dF_div_dx_mult(const Eigen::Matrix<double, DIM * DIM, colSize>& right,
                                   const Eigen::Matrix<double, DIM, DIM>& A,
                                   Eigen::Matrix<double, DIM * (DIM + 1), colSize>& result,
                                   bool symmetric)
        {
            if(colSize == Eigen::Dynamic) {
                assert(right.cols() > 0);
            }
            else {
                assert(colSize > 0);
            }
#if(DIM == 2)
            if(symmetric) {
                if(colSize == Eigen::Dynamic) {
                    assert(right.cols() == 6);
                }
                else {
                    assert(colSize == 6);
                }
                // int colI = 0;
                const double _0000 = right(0, 0) * A(0, 0);
                const double _0010 = right(0, 0) * A(1, 0);
                const double _1001 = right(1, 0) * A(0, 1);
                const double _1011 = right(1, 0) * A(1, 1);
                const double _2000 = right(2, 0) * A(0, 0);
                const double _2010 = right(2, 0) * A(1, 0);
                const double _3001 = right(3, 0) * A(0, 1);
                const double _3011 = right(3, 0) * A(1, 1);
                result(2, 0) = result(0, 2) = _0000 + _1001;
                result(3, 0) = result(0, 3) = _2000 + _3001;
                result(4, 0) = result(0, 4) = _0010 + _1011;
                result(5, 0) = result(0, 5) = _2010 + _3011;
                result(0, 0) = -result(2, 0) - result(4, 0);
                result(1, 0) = result(0, 1) = -result(3, 0) - result(5, 0);
                // colI = 1;
                const double _2100 = right(2, 1) * A(0, 0);
                const double _2110 = right(2, 1) * A(1, 0);
                const double _3101 = right(3, 1) * A(0, 1);
                const double _3111 = right(3, 1) * A(1, 1);
                result(2, 1) = result(1, 2) = right(0, 1) * A(0, 0) + right(1, 1) * A(0, 1);
                result(3, 1) = result(1, 3) = _2100 + _3101;
                result(4, 1) = result(1, 4) = right(0, 1) * A(1, 0) + right(1, 1) * A(1, 1);
                result(5, 1) = result(1, 5) = _2110 + _3111;
                result(1, 1) = -result(3, 1) - result(5, 1);
                // colI = 2;
                result(2, 2) = right(0, 2) * A(0, 0) + right(1, 2) * A(0, 1);
                result(3, 2) = result(2, 3) = right(2, 2) * A(0, 0) + right(3, 2) * A(0, 1);
                result(4, 2) = result(2, 4) = right(0, 2) * A(1, 0) + right(1, 2) * A(1, 1);
                result(5, 2) = result(2, 5) = right(2, 2) * A(1, 0) + right(3, 2) * A(1, 1);
                // colI = 3;
                result(3, 3) = right(2, 3) * A(0, 0) + right(3, 3) * A(0, 1);
                result(4, 3) = result(3, 4) = right(0, 3) * A(1, 0) + right(1, 3) * A(1, 1);
                result(5, 3) = result(3, 5) = right(2, 3) * A(1, 0) + right(3, 3) * A(1, 1);
                // colI = 4;
                result(4, 4) = right(0, 4) * A(1, 0) + right(1, 4) * A(1, 1),
                result(5, 4) = result(4, 5) = right(2, 4) * A(1, 0) + right(3, 4) * A(1, 1);
                // colI = 5;
                result(5, 5) = right(2, 5) * A(1, 0) + right(3, 5) * A(1, 1);
            }
            else {
                for(int colI = 0; colI < right.cols(); colI++) {
                    const double _000 = right(0, colI) * A(0, 0);
                    const double _010 = right(0, colI) * A(1, 0);
                    const double _101 = right(1, colI) * A(0, 1);
                    const double _111 = right(1, colI) * A(1, 1);
                    const double _200 = right(2, colI) * A(0, 0);
                    const double _210 = right(2, colI) * A(1, 0);
                    const double _301 = right(3, colI) * A(0, 1);
                    const double _311 = right(3, colI) * A(1, 1);

                    result(2, colI) = _000 + _101;
                    result(3, colI) = _200 + _301;
                    result(4, colI) = _010 + _111;
                    result(5, colI) = _210 + _311;
                    result(0, colI) = -result(2, colI) - result(4, colI);
                    result(1, colI) = -result(3, colI) - result(5, colI);
                }
            }
#else
            //TODO: use symmetric
            for(int colI = 0; colI < right.cols(); colI++) {
                result(3, colI) = (A.row(0) * right.block(0, colI, DIM, 1))[0];
                result(4, colI) = (A.row(0) * right.block(DIM, colI, DIM, 1))[0];
                result(5, colI) = (A.row(0) * right.block(DIM * 2, colI, DIM, 1))[0];
                result(6, colI) = (A.row(1) * right.block(0, colI, DIM, 1))[0];
                result(7, colI) = (A.row(1) * right.block(DIM, colI, DIM, 1))[0];
                result(8, colI) = (A.row(1) * right.block(DIM * 2, colI, DIM, 1))[0];
                result(9, colI) = (A.row(2) * right.block(0, colI, DIM, 1))[0];
                result(10, colI) = (A.row(2) * right.block(DIM, colI, DIM, 1))[0];
                result(11, colI) = (A.row(2) * right.block(DIM * 2, colI, DIM, 1))[0];
                result(0, colI) = - result(3, colI) - result(6, colI) - result(9, colI);
                result(1, colI) = - result(4, colI) - result(7, colI) - result(10, colI);
                result(2, colI) = - result(5, colI) - result(8, colI) - result(11, colI);
            }
#endif
        }
        static void dF_div_dx_mult(const Eigen::Matrix<double, DIM, DIM>& right,
                                   const Eigen::Matrix<double, DIM, DIM>& A,
                                   Eigen::Matrix<double, DIM * (DIM + 1), 1>& result);
        template<int dim>
        static void computeCofactorMtr(const Eigen::Matrix<double, dim, dim>& F,
                                       Eigen::Matrix<double, dim, dim>& A)
        {
            switch(dim) {
                case 2:
                    A(0, 0) = F(1, 1);
                    A(0, 1) = -F(1, 0);
                    A(1, 0) = -F(0, 1);
                    A(1, 1) = F(0, 0);
                    break;

                case 3:
                    A(0, 0) = F(1, 1) * F(2, 2) - F(1, 2) * F(2, 1);
                    A(0, 1) = F(1, 2) * F(2, 0) - F(1, 0) * F(2, 2);
                    A(0, 2) = F(1, 0) * F(2, 1) - F(1, 1) * F(2, 0);
                    A(1, 0) = F(0, 2) * F(2, 1) - F(0, 1) * F(2, 2);
                    A(1, 1) = F(0, 0) * F(2, 2) - F(0, 2) * F(2, 0);
                    A(1, 2) = F(0, 1) * F(2, 0) - F(0, 0) * F(2, 1);
                    A(2, 0) = F(0, 1) * F(1, 2) - F(0, 2) * F(1, 1);
                    A(2, 1) = F(0, 2) * F(1, 0) - F(0, 0) * F(1, 2);
                    A(2, 2) = F(0, 0) * F(1, 1) - F(0, 1) * F(1, 0);
                    break;

                default:
                    assert(0 && "dim not 2 or 3");
                    break;
            }
        }

        static void extractRotation(const Eigen::Matrix3d &A,
                                    Eigen::Quaterniond &q,
                                    const unsigned int maxIter);

        static void sampleSegment(const Eigen::RowVectorXd& vs,
                                  const Eigen::RowVectorXd& ve,
                                  double spacing,
                                  Eigen::MatrixXd& inBetween);

        static void findBorderVerts(const Eigen::MatrixXd& V,
                                    std::vector<std::vector<int>>& borderVerts,
                                    double ratio);

        static void computeSVD_SIMD(std::vector<Eigen::Matrix3d>& testF,
                                    std::vector<Eigen::Matrix3d>& U, std::vector<Eigen::Vector3d>& Sigma, std::vector<Eigen::Matrix3d>& V);

        static void matrixProduct(const std::vector<Eigen::Matrix3d>& left,
                                  const std::vector<Eigen::Matrix3d>& right,
                                  std::vector<Eigen::Matrix3d>& result);

        static void matrixVectorMatrixTProduct(const std::vector<Eigen::Matrix3d>& left,
                                                const std::vector<Eigen::Vector3d>& vec,
                                                const std::vector<Eigen::Matrix3d>& right,
                                                std::vector<Eigen::Matrix3d>& result);
    };

}

#endif /* IglUtils_hpp */


#define BOOST_TEST_MODULE pcraster geo point
#include <boost/test/unit_test.hpp>
#include <algorithm>
#include "geo_point.h"

BOOST_AUTO_TEST_CASE(layout)
{
  using namespace geo;

  // assure size of point is equal to
  // the vector of coordinates
 {
  typedef Point<float,2> Punt;
  BOOST_CHECK(sizeof(Punt)==8);
 }
 {
  typedef Point<double,3> Punt;
  BOOST_CHECK(sizeof(Punt)==24);
 }
}

BOOST_AUTO_TEST_CASE(index_direction)
{
  using namespace geo;

 {
  //  1D:
  //          0
  //    <  1 -c- >= 0
  typedef Point<float,1> Punt;
  Punt c,p;
  c[0]=0;

  p[0]=-1;
  BOOST_CHECK(p.indexDirection(c) == 1);
  p[0]=1;
  BOOST_CHECK(p.indexDirection(c) == 0);
  p[0]=0;
  BOOST_CHECK(p.indexDirection(c) == 0);
 }
 {
 /*  2D:
  *   NW 1(b) | NE 0(a)
  *           0(e)
  *  1(h)-----C---------0(g)
  *   SW 3(c) | SE 2(d)
  *           2(f)
  */
  typedef Point<float,2> Punt;
  Punt c,p;
  c[0]=c[1]=0;
  p[0]=p[1]=1;
  BOOST_CHECK(p.indexDirection(c) == 0); // a
  p[0]=-1;
  BOOST_CHECK(p.indexDirection(c) == 1); // b
  p[0]=p[1]=-1;
  BOOST_CHECK(p.indexDirection(c) == 3); // c
  p[0]=1;
  BOOST_CHECK(p.indexDirection(c) == 2); // d

  p[0]=0;p[1]=1;
  BOOST_CHECK(p.indexDirection(c) == 0); // e
  p[0]=0;p[1]=-1;
  BOOST_CHECK(p.indexDirection(c) == 2); // f
  p[0]=1;p[1]=0;
  BOOST_CHECK(p.indexDirection(c) == 0); // g
  p[0]=-1;p[1]=0;
  BOOST_CHECK(p.indexDirection(c) == 1); // h
 }
}

BOOST_AUTO_TEST_CASE(closer)
{
  using namespace geo;

  typedef Point<float,2> P;
  typedef Closer<P>      C;

  C c(P(-5,0));

  BOOST_CHECK( c(P(1,0),P(2,0)));
  BOOST_CHECK(!c(P(2,0),P(1,0)));

  std::vector<P> l;
  l.push_back(P(2,0));
  l.push_back(P(1,0));

  BOOST_CHECK(l[0][X]==2);
  BOOST_CHECK(l[1][X]==1);
  std::sort(l.begin(),l.end(), C(P(-5,0)));
  BOOST_CHECK(l[0][X]==1);
  BOOST_CHECK(l[1][X]==2);
  // reverse
  std::sort(l.begin(),l.end(), std::not2(c));
  BOOST_CHECK(l[0][X]==2);
  BOOST_CHECK(l[1][X]==1);
}

BOOST_AUTO_TEST_CASE(distance)
{
  using namespace geo;

  typedef Point<float,2> P;

  // test compilation of enum
  BOOST_CHECK(P::Dim == 2);

  P p1(2,20),p2(3,21);
  BOOST_CHECK(p1.squaredDistance(p2) == 2);
  double d=p1.distance(p2);
  BOOST_CHECK(d>1.4 && d<1.43); // sqrt(2)
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, log2_fun) {
  using stan::math::log2;

  EXPECT_FLOAT_EQ(std::log(2.0), log2());
}

TEST(MathFunctions, log2) {
  using stan::math::log2;

  EXPECT_FLOAT_EQ(1.0, log2(2.0));
  EXPECT_FLOAT_EQ(2.0, log2(4.0));
  EXPECT_FLOAT_EQ(3.0, log2(8.0));
}

TEST(MathFunctions, log2_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::log2(nan));
}


///////////////////////////////////////////////////////////////////////////////
// survival::modelss::exponential::scalar::model.hpp                   		 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_MODEL_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_MODEL_HPP_ER_2009
#include <boost/operators.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/statistics/detail/mpl/nested_type.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/include.hpp>
#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/log_rate/identity/identity.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace survival{

    // TODO add an error policy

    template<
        typename T,
        typename L = exponential_model_::log_rate::identity<T>,
        typename Err = boost::math::policies::policy<>
    >
    class exponential_model : L{
    	// TODO Err

        public:
        typedef T value_type;

        //typedef exponential_model<T,L> distribution_type;

        exponential_model():L(){}
        exponential_model(const exponential_model& that)
            : L(static_cast<const L&>(that)),b_(that.b_){}
        exponential_model& operator=(const exponential_model& that)
        {
            if(&that!=this)
            {
                static_cast<L&>(*this) = static_cast<const L&>(that);
                b_ = that.b_;
            }
            return (*this);
        }

        template<typename X>
        T log_rate(const X& x)const
        {
            T cross_prod =  static_cast<T>( x );
            cross_prod *= this->parameter() ;
            return this->log_rate_impl( cross_prod );
        }

        const T& parameter()const{ return this-> b_; }
        template<typename B>
        void set_parameter(const B& b)const{ this->b_ = b; }

        protected:

        friend class boost::serialization::access;
        template<class Archive>
        void serialize(Archive & ar, const unsigned int version){
            ar & b_;
        }

        mutable T b_;
    };

}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


#include "OccupancyGrid/occgrid.hpp"
#include "OccupancyGrid/cvmat_serialization.h"
#include <opencv2/opencv.hpp>
#include <boost/format.hpp>

/// Override class to override is_occupied function so that it can copy the
/// ground truth map everytime a laser crosses a cell.
template<typename real_t, typename int_t>
class OccupancyGrid2DInverseSensor : public OccupancyGrid2D<real_t, int_t> {
  public:
    using OccupancyGrid2D<real_t, int_t>::og_;
    using OccupancyGrid2D<real_t, int_t>::cell_size_;
    using OccupancyGrid2D<real_t, int_t>::min_pt_;
    using OccupancyGrid2D<real_t, int_t>::FREE;
    int_t observed_manh_range_;
    cv::Vec<int_t, 2> robot_position_;
    cv::Mat_<real_t> log_odds_map_;
    const real_t LOG_ODDS_OCCUPIED;
    const real_t LOG_ODDS_FREE;

    OccupancyGrid2DInverseSensor(real_t min_x, real_t min_y, real_t cell_size_x, real_t
        cell_size_y, int_t ncells_x, int_t ncells_y) :
      OccupancyGrid2D<real_t, int_t>(min_x, min_y, cell_size_x, cell_size_y,
          ncells_x, ncells_y),
      observed_manh_range_(),
      robot_position_(),
      log_odds_map_(ncells_x, ncells_y, 0.0L),
      LOG_ODDS_OCCUPIED(1.3863),
      LOG_ODDS_FREE(-1.3863)
    {
    };

    void set_up_ray_trace(
        real_t px,
        real_t py,
        real_t ptheta,
        real_t observed_range) {
        robot_position_(0) =
            (int)floor((px - min_pt_(0)) / cell_size_(0));
        robot_position_(1) =
            (int)floor((py - min_pt_(1)) / cell_size_(1));
        real_t dx_abs = fabs(cos(ptheta));
        real_t dy_abs = fabs(sin(ptheta));
        real_t dmag = sqrt(dx_abs * dx_abs + dy_abs * dy_abs);
        observed_manh_range_ =
          floor(observed_range * dx_abs / dmag / cell_size_(0)) +
          floor(observed_range * dy_abs / dmag / cell_size_(1));
        //printf("-----------------\n");
    }

    inline int_t manh_distance(int_t i, int_t j) {
        return std::abs(i - robot_position_(0)) + std::abs(j - robot_position_(1));
    }

    virtual bool is_occupied(int_t i, int_t j) {
        uint8_t val = og_.ptr(i)[j];
        bool retval = (val != FREE);
        int_t d = manh_distance(i, j);
        // update step
        // printf("%d < %d\n", d, observed_manh_range_);
        log_odds_map_(i, j) +=
          (d < observed_manh_range_) ?  LOG_ODDS_FREE
          : (d == observed_manh_range_) ? LOG_ODDS_OCCUPIED
          : 0; // unknown
        return retval;
    }

    inline void show(int r) {
        cv::Mat vis;
        cv::exp(log_odds_map_, vis);
        vis = 1 / (1 + vis);
        vis *= 255;
        vis.convertTo(vis, CV_8U);
        cv::imshow("c", vis);
        //cv::imwrite((boost::format("out-%d.png") % r).str(), vis);
        cv::waitKey(1);
        cv::imwrite("/tmp/two_assumption_algo.png", vis);
    }
};

int main(int argc, char** argv) {
    if (argc != 4) {
        std::cout << "Sample Usage:" << std::endl;
        std::cout << "bin/two_assumption_alg Data/player_sim/laser_pose_all.bin Data/player_sim/laser_range_all.bin Data/player_sim/scan_angles_all.bin" << std::endl;
        exit(1);
    }
    cv::Mat laser_pose;
    loadMat(laser_pose, argv[1]);
    cv::Mat laser_ranges;
    loadMat(laser_ranges, argv[2]);
    cv::Mat scan_angles;
    loadMat(scan_angles, argv[3]);

    cv::Vec2d min_pt(-9, -9);
    cv::Vec2d range = -2 * min_pt;
    cv::Vec2i gridsize(100, 100);
    cv::Vec2d cellsize;
    cv::divide(range, gridsize, cellsize);
    //std::cout << cellsize(0) << cellsize(1) << std::endl;
    cv::Vec2i ncells;
    cv::divide(min_pt, cellsize, ncells, -2);

    //std::cout << ncells(0) << ncells(1) << std::endl;

    OccupancyGrid2DInverseSensor<double, int> map(
        min_pt(0),
        min_pt(1),
        cellsize(0),
        cellsize(1),
        ncells(0),
        ncells(1));

    double MAX_RANGE = 8;

    int r;
    for (r = 0; r < laser_pose.rows; r++) {
        double* pose = laser_pose.ptr<double>(r);
        double* ranges = laser_ranges.ptr<double>(r);
        double* angles = scan_angles.ptr<double>(r);
        double robot_angle = pose[2];
        for (int c = 0; c < scan_angles.cols; c++) {
            double total_angle = robot_angle + angles[c];
            cv::Vec2d final_pos;
            map.set_up_ray_trace(pose[0], pose[1], total_angle, ranges[c]);
            bool reflectance;
            map.ray_trace(pose[0], pose[1], total_angle, MAX_RANGE, final_pos, reflectance);
        }
    }
    map.show(r);
}


/* compile_time_power.hpp
 *
*/

#ifndef _COMPILE_TIME_POWER_HPP_
#define _COMPILE_TIME_POWER_HPP_

#include <type_traits>

#include <boost/multiprecision/cpp_int.hpp>

namespace alex::utils::math
{
    namespace concepts
    {
    #if __cplusplus > 201703L && __cpp_concepts >= 201907L
        template<typename T>
        concept NumberType = std::is_integral_v<T> ||
                            std::is_floating_point_v<T> ||
                            boost::multiprecision::is_number<T>::value;
    #else
        // Legacy SFINAE
        template<typename T> struct IsNumberType
        { static constexpr bool value{std::is_integral<T>::value || std::is_floating_point<T>::value}; };
    #endif
    } // namespace alex::utils::math::concepts

#if 0
    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::NumberType number_type,
        number_type number,
        size_t pow
#else
        typename number_type,
        number_type number,
        size_t pow,
        typename = std::enable_if_t<concepts::IsNumberType<number_type>::value>
#endif
    >
    struct ct_power
    { static constexpr number_type value{number * ct_power<number_type, number, pow - 1>::value}; };

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::NumberType number_type,
        number_type number
#else
        typename number_type,
        number_type number
#endif
    > struct ct_power<number_type, number, 1> { static constexpr number_type value{number}; };

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::NumberType number_type,
        number_type number
#else
        typename number_type,
        number_type number
#endif
    > struct ct_power<number_type, number, 0> { static constexpr number_type value{static_cast<number_type>(1)}; };
#else
    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::NumberType number_type
#else
        typename number_type, typename = std::enable_if_t<concepts::IsNumberType<number_type>::value>
#endif
    >
    constexpr number_type ct_power(number_type number, size_t pow)
    {
        if (pow == 0) return 1;
        if (pow == 1) return number;
        return number * ct_power(number, pow - 1);
    }
#endif
} // namespace alex::utils::math

#endif // _COMPILE_TIME_POWER_HPP_


#define CATCH_CONFIG_MAIN
#include <Eigen/Dense>
#include <catch.hpp>
#include <random>

#include "EDP/ConstructSparseMat.hpp"
#include "EDP/LocalHamiltonian.hpp"

#include "yavque/Operators/HamEvol.hpp"
#include "yavque/utils.hpp"

#include "common.hpp"

template<typename RandomEngine>
void test_single_qubit(const uint32_t N, const Eigen::SparseMatrix<double>& m,
                       RandomEngine& re)
{
	using namespace Eigen;
	using std::cos;
	using std::exp;
	using std::sin;
	using std::sqrt;

	constexpr yavque::cx_double I(0., 1.);
	std::normal_distribution<> nd;

	edp::LocalHamiltonian<double> ham_ct(N, 2);
	for(uint32_t i = 0; i < N; i++)
	{
		ham_ct.addOneSiteTerm(i, m);
	}

	auto ham
		= yavque::Hamiltonian(edp::constructSparseMat<yavque::cx_double>(1 << N, ham_ct));
	auto hamEvol = yavque::HamEvol(ham);
	auto var = hamEvol.get_variable();

	for(int i = 0; i < 100; i++)
	{
		VectorXcd ini = VectorXcd::Random(1 << N);
		ini.normalize();
		double t = nd(re);
		var = t;
		VectorXcd out_test = hamEvol * ini;

		MatrixXcd mevol = (cos(t) * MatrixXcd::Identity(2, 2) - I * sin(t) * m);
		VectorXcd out = apply_kronecker(N, mevol, ini);

		REQUIRE((out - out_test).norm() < 1e-6);
	}

	hamEvol.dagger_in_place();
	for(int i = 0; i < 100; i++)
	{
		VectorXcd ini = VectorXcd::Random(1 << N);
		ini.normalize();

		double t = nd(re);
		var = t;
		VectorXcd out_test = hamEvol * ini;

		MatrixXcd mevol = (cos(t) * MatrixXcd::Identity(2, 2) + I * sin(t) * m);
		VectorXcd out = apply_kronecker(N, mevol, ini);

		REQUIRE((out - out_test).norm() < 1e-6);
	}
}

TEST_CASE("test single qubit Hamiltonian", "[one-site]")
{
	constexpr uint32_t N = 8; // number of qubits

	// ini is |+>^N
	std::random_device rd;
	std::default_random_engine re{rd()};

	SECTION("test using pauli X") { test_single_qubit(N, yavque::pauli_x(), re); }

	SECTION("test using pauli Z") { test_single_qubit(N, yavque::pauli_z(), re); }
}

TEST_CASE("Test basic operations", "[basic]")
{
	using namespace Eigen;
	using namespace yavque;
	using std::cos;
	using std::exp;
	using std::sin;
	using std::sqrt;
	constexpr uint32_t N = 8; // number of qubits

	// ini is |+>^N
	VectorXcd ini = VectorXcd::Ones(1 << N);
	ini /= sqrt(1 << N);

	std::random_device rd;
	std::default_random_engine re{rd()};
	std::normal_distribution<> nd;

	constexpr yavque::cx_double I(0., 1.);

	edp::LocalHamiltonian<double> ham_ct(N, 2);
	for(uint32_t i = 0; i < N; i++)
	{
		ham_ct.addOneSiteTerm(i, yavque::pauli_x());
	}

	auto ham
		= yavque::Hamiltonian(edp::constructSparseMat<yavque::cx_double>(1 << N, ham_ct));
	auto hamEvol = HamEvol(ham);

	auto copied = hamEvol.clone();

	REQUIRE(hamEvol.get_variable()
	        != dynamic_cast<HamEvol*>(copied.get())->get_variable());

	REQUIRE(hamEvol.hamiltonian().is_same_ham(
		dynamic_cast<HamEvol*>(copied.get())->hamiltonian()));
}

TEST_CASE("Test gradient", "[log-deriv]")
{
	using namespace Eigen;
	using namespace yavque;
	using std::cos;
	using std::exp;
	using std::sin;
	using std::sqrt;
	constexpr uint32_t N = 8; // number of qubits

	std::random_device rd;
	std::default_random_engine re{rd()};

	std::vector<Eigen::SparseMatrix<double>> pauli_ops
		= {yavque::pauli_xx(), yavque::pauli_yy(), yavque::pauli_zz()};

	std::uniform_int_distribution<> uid(0, 2);
	std::normal_distribution<> nd;

	for(uint32_t k = 0; k < 100; ++k) // instances
	{
		// ini is random
		VectorXcd ini = VectorXcd::Random(1 << N);
		ini.normalize();

		edp::LocalHamiltonian<double> ham_ct(N, 2);
		ham_ct.addTwoSiteTerm(random_connection(N, re), pauli_ops[uid(re)]);

		auto ham = yavque::Hamiltonian(
			edp::constructSparseMat<yavque::cx_double>(1 << N, ham_ct));
		auto hamEvol = HamEvol(ham);

		double val = nd(re);

		hamEvol.set_variable_value(val);
		Eigen::VectorXcd grad1
			= hamEvol.log_deriv()->apply_right(hamEvol.apply_right(ini));

		hamEvol.get_variable() += M_PI / 2;
		Eigen::VectorXcd grad2 = hamEvol.apply_right(ini);
		hamEvol.get_variable() = val - M_PI / 2;
		grad2 -= hamEvol.apply_right(ini);
		grad2 /= 2.0;

		REQUIRE((grad1 - grad2).norm() < 1e-6);
	}
}


// random.cpp
//
#include <stdint.h>
#include <ctime>
#include <iostream>

#include <boost/random.hpp>
#include <boost/random/random_device.hpp>

int main(int argc, char* argv[])
{
    namespace random = boost::random;

    std::time_t now = std::time(0);
    random::mt19937 rand(static_cast<uint32_t>(now));
    std::cout << rand() << '\n';

    random::random_device rand_dev;
    random::uniform_int_distribution<> dist(1, 100);
    std::cout << dist(rand_dev) << '\n';
    random::bernoulli_distribution<> dist1;
    std::cout << dist1(rand_dev) << '\n';
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file dcs/fog/confidence_intervals.hpp
 *
 * \brief Confidence interval estimation
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2017 Marco Guazzone (marco.guazzone@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.
 */

#ifndef DCS_FOG_CONFIDENCE_INTERVALS_HPP
#define DCS_FOG_CONFIDENCE_INTERVALS_HPP

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <dcs/assert.hpp>
#include <dcs/debug.hpp>
#include <dcs/logging.hpp>
#include <dcs/macro.hpp>
#include <dcs/math/function/iszero.hpp>
#include <dcs/math/function/sqr.hpp>
#include <dcs/math/traits/float.hpp>
#include <cstddef>
#include <limits>
#include <stdexcept>
#include <string>
#include <vector>

namespace dcs { namespace fog {

template <typename RealT>
class ci_mean_estimator
{
private:
    typedef boost::accumulators::accumulator_set<RealT,
                                                 boost::accumulators::stats<boost::accumulators::tag::count,
                                                                            boost::accumulators::tag::mean,
                                                                            boost::accumulators::tag::variance>> accumulator_type;

public:
    static const RealT default_ci_level;
    static const RealT default_relative_precision;
    static const std::size_t default_min_sample_size;
    static const std::size_t default_max_sample_size;

    explicit ci_mean_estimator(RealT confidence_level = default_ci_level,
                                       RealT relative_precision = default_relative_precision,
                                       std::size_t min_sample_size = default_min_sample_size,
                                       std::size_t max_sample_size = default_max_sample_size)
    : ci_level_(confidence_level),
      target_rel_prec_(relative_precision),
      n_min_(min_sample_size),
      n_max_(max_sample_size),
      name_("Unnamed"),
      n_target_(std::numeric_limits< std::size_t >::max()),
      n_detected_(false),
      n_aborted_(false),
      n_first_call_(true),
      unstable_(false),
      done_(false)
    {
        // pre: min sample size >= 2
        DCS_ASSERT(n_min_ >= 2,
                   DCS_EXCEPTION_THROW(std::invalid_argument,
                                       "Min sample size must be >= 2"));
        // pre: min sample size <= max sample size
        DCS_ASSERT(n_min_ <= n_max_,
                   DCS_EXCEPTION_THROW(std::invalid_argument,
                                       "Min sample size must be <= max sample size"));
    }

    void name(std::string const& s)
    {
        name_ = s;
    }

    std::string name() const
    {
        return name_;
    }

    std::size_t size() const
    {
        return boost::accumulators::count(stat_);
    }

    std::size_t target_size() const
    {
        return n_target_;
    }

    RealT estimate() const
    {
        return boost::accumulators::mean(stat_);
    }

    RealT variance() const
    {
        //FIXME: Boost.Accumulators variance computes the biased sample variance
        const std::size_t n = this->size();
        return (n/static_cast<RealT>(n-1))*boost::accumulators::variance(stat_);
    }

    RealT standard_deviation() const
    {
        return std::sqrt(this->variance());
    }

    RealT half_width() const
    {
        const std::size_t n = this->size();

        if (n > 1)
        {
            boost::math::students_t_distribution<RealT> t_dist(n-1);
            const RealT t = boost::math::quantile(t_dist, (1+ci_level_)*0.5);

            return t*(this->standard_deviation()/std::sqrt(n));
        }

        return std::numeric_limits<RealT>::infinity();
    }

    RealT target_relative_precision() const
    {
        return target_rel_prec_;
    }

    RealT relative_precision() const
    {
        if (!::dcs::math::iszero(this->estimate()) && this->size() > 1)
        {
            return this->half_width() / std::abs(this->estimate());
        }

        return std::numeric_limits<RealT>::infinity();
    }

    RealT lower() const
    {
        return this->estimate() - this->half_width();
    }

    RealT upper() const
    {
        return this->estimate() + this->half_width();
    }

    bool done() const
    {
        return done_;
    }

    bool unstable() const
    {
        return unstable_;
    }

    void collect(RealT obs)
    {
        if (n_aborted_)
        {
            return;
        }

        stat_(obs);

        //this->check_precision();
        this->check_precision_alt();
DCS_DEBUG_TRACE("(" << name_ << ") Statistic Info: estimate: " << this->estimate() << ", s.d.: " << this->standard_deviation() << ", size: " << this->size() << ", n_target_: " << n_target_ << ", n_min_: " << n_min_ << ", n_max_: " << n_max_ << ", rel.prec.: " << this->relative_precision() << ", n_detected_: " << std::boolalpha << n_detected_ << ", n_aborted_: " << n_aborted_ << ", unstable: " << unstable_ << ", done: " << done_ << ")");//XXX
    }

    void reset()
    {
        stat_ = accumulator_type();
        n_aborted_ = n_detected_
                   = false;
        n_first_call_ = true;
        unstable_ = false;
        done_ = false;
        n_target_ = std::numeric_limits< std::size_t >::max();
    }

private:
    void check_precision()
    {
        const std::size_t n = this->size();

        if (n_detected_ && n >= n_target_)
        {
            if (std::isinf(target_rel_prec_))
            {
                done_ = true;
            }
            else
            {
                done_ = ::dcs::math::float_traits<RealT>::definitely_less_equal(this->relative_precision(), target_rel_prec_);
            }
        }
        if (!n_detected_ || (n >= n_target_ && !done_))
        {
            // Sample size is still to be detected...
            // ... or needs to be redetected since, after having performed the
            //        detection, the precision has not been reached yet.

            DCS_DEBUG_TRACE("(" << name_ << ") Detecting sample size...");

            this->detect();

            if (n_detected_)
            {
                if (n >= n_target_)
                {
                    if (done_)
                    {
                        // Ooops! The new detected number of replications is the
                        // same of or greater than the one previously detected
                        // AND we have already performed this number of replications
                        // AND precision has not been reached yet.
                        // This means that we are unable to reach the target
                        // precision. So disable this statistic.

                        ::dcs::log_warn(DCS_LOGGING_AT, "Statistic '" + name_ + "' will be disabled: unable to reach the wanted precision.");

                        unstable_ = true;
                    }

                    done_ = true;
                }

                DCS_DEBUG_TRACE("(" << name_ << ") Sample size detected: " << n_target_ << " (already collected: " << n << ")");
            }
        }

#ifdef DCS_DEBUG
        if (done_)
        {
            DCS_DEBUG_TRACE("(" << name_ << ") [Sample #" << n << "] Detected precision: mean = " << this->estimate() << " - reached precision = " << this->relative_precision() << " - target precision: " << target_rel_prec_);
        }
        else if (n_detected_)
        {
            //DCS_DEBUG_TRACE("(" << name_ << ") Sample size detected: " << n_target_ << " (already collected: " << n << ")");
            if (n >= n_target_)
            {
                DCS_DEBUG_TRACE("(" << name_ << ") [Sample #" << n << "] Failed to detect precision: mean = " << this->estimate() << " - reached precision = " << this->relative_precision() << " - target precision: " << target_rel_prec_);
            }
            else
            {
                DCS_DEBUG_TRACE("(" << name_ << ") [Sample #" << n << "] Precision not yet reached: not enough replications (done: " << n << " - needed: " << n_target_ << ")");
            }
        }
#endif
    }

    bool detect()
    {
        std::size_t n = this->size();

        if (n < n_min_)
        {
            n_detected_ = false;
            return false;
        }
        if (n >= n_max_)
        {
            n_aborted_ = true;
            return false;
        }
        if (std::isinf(target_rel_prec_) && !n_detected_)
        {
            n_target_ = n;
            n_detected_ = true;
            return true;
        }

        // Use the procedure described in [1], chapter 11.
        //
        // References
        // 1. J. Banks et al.
        //    "Discrete-Event System Simulations,"
        //    4th Edition, Prentice-Hall, 2005
        //

        const RealT mean = this->estimate();
        const RealT sd = this->standard_deviation();

        if (sd < 0 || std::isinf(sd))
        {
            ::dcs::log_warn(DCS_LOGGING_AT, "Standard deviation is negative or infinite");
            n_detected_ = false;
            return false;
        }

        const RealT half_alpha = (1-ci_level_)*0.5;

        // Compute an initial estimate of sample size
        if (n_first_call_)
        {
            n_first_call_ = false;

            boost::math::normal_distribution<RealT> norm;
            const RealT z = boost::math::quantile(norm, half_alpha);
            n =  static_cast< std::size_t >(::dcs::math::sqr(z*sd/(target_rel_prec_*mean)));

            if (n < n_min_)
            {
                n = n_min_;
            }
        }

        RealT n_want = 0;

        // Compute the real estimate of sample size
        do
        {
            boost::math::students_t_distribution<RealT> student_t(n-1);
            const RealT t = boost::math::quantile(student_t, half_alpha);
            n_want = ::dcs::math::sqr(t*sd/(target_rel_prec_*mean));

            if (n < n_want)
            {
                ++n;
            }
        }
        while (n < n_want && n < n_max_);

        if (n <= n_max_)
        {
            if (n_detected_ && n >= n_target_ && !done_)
            {
                // Ooops! The new detected sample size is the
                // same of or greater than the one previously detected
                // AND we have already collected this number of samples
                // AND precision has not been reached yet.
                // This means that we are unable to reach the target
                // precision. So disable this statistic.

                ::dcs::log_warn(DCS_LOGGING_AT, "Statistic '" + name_ + "' will be disabled: unable to reach the wanted precision.");

                unstable_ = true;
            }

            n_target_ = n;
            n_detected_ = true;
            //done_ = true;
        }
        else
        {
            n_target_ = n_max_;
            n_detected_ = false;
            n_aborted_ = true;
        }

DCS_DEBUG_TRACE("(" << name_ << ") Detecting Sample Size --> " << std::boolalpha << n_detected_ << " (n_target_: " << n_target_ << " - n_want: " << n_want << " - n_max_: " << n_max_ << " - n_aborted_: " << n_aborted_ << " - unstable: " << unstable_ << " - done: " << done_ << ")");//XXX

        return n_detected_;
    }

    bool check_precision_alt()
    {
        std::size_t n = this->size();

        if (n < n_min_)
        {
            n_detected_ = false;
            return false;
        }
        if (n >= n_max_)
        {
            n_aborted_ = true;
            return false;
        }
        if (std::isinf(target_rel_prec_))
        {
            n_target_ = n;
            n_detected_ = true;
            done_ = true;
            return true;
        }

        // Use the procedure described in [1], chapter 11.
        //
        // References
        // 1. J. Banks et al.
        //    "Discrete-Event System Simulations,"
        //    4th Edition, Prentice-Hall, 2005
        //

        const RealT mean = this->estimate();
        const RealT sd = this->standard_deviation();

        if (sd < 0 || std::isinf(sd))
        {
            ::dcs::log_warn(DCS_LOGGING_AT, "Standard deviation is negative or infinite");
            n_detected_ = false;
            return false;
        }

        const RealT half_alpha = (1-ci_level_)*0.5;

        // Compute an initial estimate of sample size
        if (n_first_call_)
        {
            n_first_call_ = false;

            boost::math::normal_distribution<RealT> norm;
            const RealT z = boost::math::quantile(norm, half_alpha);
            n =  static_cast< std::size_t >(::dcs::math::sqr(z*sd/(target_rel_prec_*mean)));

            if (n < n_min_)
            {
                n = n_min_;
            }
        }

        RealT n_want = 0;

        // Compute the real estimate of sample size
        do
        {
            boost::math::students_t_distribution<RealT> student_t(n-1);
            const RealT t = boost::math::quantile(student_t, half_alpha);
            n_want = ::dcs::math::sqr(t*sd/(target_rel_prec_*mean));

            if (n < n_want)
            {
                ++n;
            }
        }
        while (n < n_want && n < n_max_);

        if (n <= n_max_)
        {
//          if (n_detected_ && n >= n_target_ && !done_)
//          {
//              // Ooops! The new detected sample size is the
//              // same of or greater than the one previously detected
//              // AND we have already collected this number of samples
//              // AND precision has not been reached yet.
//              // This means that we are unable to reach the target
//              // precision. So disable this statistic.
//
//              ::dcs::log_warn(DCS_LOGGING_AT, "Statistic '" + name_ + "' will be disabled: unable to reach the wanted precision.");
//
//              unstable_ = true;
//          }

            if (n <= this->size())
            {
//              n_target_ = this->size();
                done_ = true;
            }
//          else
//          {
//              n_target_ = n;
//          }
            n_target_ = n;
            n_detected_ = true;
            //done_ = true;
        }
        else
        {
            n_target_ = n_max_;
            n_detected_ = false;
            n_aborted_ = true;
        }

DCS_DEBUG_TRACE("(" << name_ << ") Detecting Sample Size --> " << std::boolalpha << n_detected_ << " (n_target_: " << n_target_ << " - n_want: " << n_want << " - n_max_: " << n_max_ << " - n_aborted_: " << n_aborted_ << " - unstable: " << unstable_ << " - done: " << done_ << ")");//XXX

        return n_detected_;
    }

private:
    RealT ci_level_;
    RealT target_rel_prec_;
    std::size_t n_min_;
    std::size_t n_max_;
    std::string name_;
    accumulator_type stat_; ///< The accumulated statistics
    std::size_t n_target_; ///< The sample size needed to reach the target relative precision
    bool n_detected_; ///< Tells if the sample size needed to reach the target relative precision has been achieved
    bool n_aborted_; ///< Tells if the sample size detection process has been aborted
    bool n_first_call_; ///< Tells if this is the first invocation of the sample size detection process
    bool unstable_; ///< Tells if this statistics has shown an unstable behavior
    bool done_; ///< Tells if this statistics has reached the target precision
}; // ci_mean_estimator

template <typename RT>
const RT ci_mean_estimator<RT>::default_ci_level = 0.95;

template <typename RT>
const RT ci_mean_estimator<RT>::default_relative_precision = 0.04;

template <typename RT>
const std::size_t ci_mean_estimator<RT>::default_min_sample_size = 2;

template <typename RT>
const std::size_t ci_mean_estimator<RT>::default_max_sample_size = std::numeric_limits< std::size_t >::max();

}} // Namespace dcs::fog

#endif // DCS_FOG_CONFIDENCE_INTERVALS_HPP


/*
 * checks.hpp
 *
 *  Created on: Mar 1, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   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.
 */

#pragma once

#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace math {

inline static
bool is_power_of_two(int number){
	return !(number == 0) && !(number & (number - 1));
}

template<typename ScalarA, typename ScalarB>
inline static
bool are_dimensions_equal(
		const Eigen::Matrix<ScalarA,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>& container_a,
		const Eigen::Matrix<ScalarB,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>& container_b){
	return container_a.rows() == container_b.rows() && container_a.cols() == container_b.cols();
}

template<typename ScalarA, typename ScalarB, typename ScalarC>
inline static
bool are_dimensions_equal(
		const Eigen::Matrix<ScalarA,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>& container_a,
		const Eigen::Matrix<ScalarB,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>& container_b,
		const Eigen::Matrix<ScalarC,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor>& container_c){
	return container_a.rows() == container_b.rows() && container_a.cols() == container_b.cols()
			&& container_c.rows() == container_b.rows() && container_c.cols() == container_b.cols();
}

template<typename ScalarA, typename ScalarB>
inline static
bool are_dimensions_equal(
		const Eigen::Tensor<ScalarA,3,Eigen::ColMajor>& container_a,
		const Eigen::Tensor<ScalarB,3,Eigen::ColMajor>& container_b){
	for (int i_dim = 0; i_dim < 3; i_dim++) {
		if (container_a.dimension(i_dim) != container_b.dimension(i_dim)) {
			return false;
		}
	}
	return true;
}

template<typename ScalarA, typename ScalarB, typename ScalarC>
inline static
bool are_dimensions_equal(
		const Eigen::Tensor<ScalarA,3,Eigen::ColMajor>& container_a,
		const Eigen::Tensor<ScalarB,3,Eigen::ColMajor>& container_b,
		const Eigen::Tensor<ScalarC,3,Eigen::ColMajor>& container_c){
	for (int i_dim = 0; i_dim < 3; i_dim++) {
		if (container_a.dimension(i_dim) != container_b.dimension(i_dim) ||
				container_c.dimension(i_dim) != container_c.dimension(i_dim)) {
			return false;
		}
	}
	return true;
}

} //namespace math