#include <stdio.h>
#include <time.h>
#include <vector>
#include <fstream>
#include <cassert>
#include <algorithm>
#include <string>
#include <sstream>      // std::stringstream

#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <NTL/ZZ_pX.h>

#include <NTL/mat_ZZ.h>
#include <NTL/matrix.h>
#include <NTL/vec_vec_ZZ.h>
NTL_CLIENT

 int main(){
     cout<<"Hello World"<<endl;
     ifstream ist("polyfile_before.txt");
     if(!ist) cout<<"Can't open file"<< endl;

     string s_temp;
     ZZ mod;
     long size_vec;
     long size_f;
     long size_pt;
     ist>>s_temp;

     conv(mod,s_temp.c_str());
     ZZ_p::init(mod);
     cout<<mod<<endl;
     ZZ_pX f,pt,res;
     res=ZZ_pX(); //zero
     ist>>size_f;
     ist>>size_pt;
     ist>>size_vec;

     //reads the vector t_1
    //l=2*m;
    //for (i = 0; i<l; i++){
    //   ist >> chal_x6->at(i);
    //}

     cout<<"Expecting "<<size_vec*(size_f+size_pt)<<" entries"<<endl;
     //ist>>c;
     int i,j;

     for (i=0;i<size_vec;i++){
         ist>>f;
         //cout<<f;
         ist>>pt;
         //cout<<pt;
         res+=f*pt;
         //cout<<res;
     }
     ofstream ost("polyfile_after.txt");
     //ost<<res;
     stringstream st;     // line A
     st << res;              // line B
     std::string s = st.str();      // Line D
     std::replace( s.begin(), s.end(), ' ', ','); // replace all 'x' to 'y'
     //ost<<"hvec=";
     ost<<s;

    return 1;
}


#include <algorithm>
#include <iostream>
#include <numeric>
#include <sstream>
#include <chrono>

#include <Eigen/SVD>

#include "ICP.h"
#include "ICPPoint.h"
#include "Misc.h"
#include "Point.h"
#include "PointOP.h"

ICP::ICP(std::array<PointCloudData, 2> &pcd) :
    m_point_cloud_data(pcd)
{
    m_LTS = 1.0;
    m_text = NULL;
    m_max_points = 0;
}

void ICP::SetLTS(float a) { m_LTS = a; }

void ICP::Run(Eigen::Matrix4d& Transform)
{
    // Some distance statistics
    std::vector<double> dist_sq;

    auto update_text = [&]() {
        while (m_app->Pending()) {
            m_app->Dispatch();
        }
    };

    if (m_text) {
        m_text->AppendText("Determining square distance to trim at ...\n");
        update_text();
    }

    for (size_t i = 0; i < m_points1.size(); i++) {
        if (i % 100000 == 0) {
            if (m_text) {
                std::stringstream ss;

                ss << i * 100 / (float)m_points1.size() << "% ";
                m_text->AppendText(ss.str());

                update_text();
            }
        }

        double dist;
        int index;

        m_ANN_points2.FindClosest(m_points1[i], dist, index);

        m_points1[i].nearest = m_points2[index];
        m_points1[i].dist_sq = dist;

        dist_sq.push_back(dist);
    }

    if (m_text) {
        m_text->AppendText("100%\n");
        update_text();
    }

    std::sort(dist_sq.begin(), dist_sq.end());

    size_t pos = (int)(dist_sq.size() * m_LTS);

    if (pos >= dist_sq.size()) {
        pos = dist_sq.size() - 1;
    }

    double trimDist = dist_sq[pos];

    if (m_text) {
        std::stringstream ss;
        ss << "Trimming at distance: " << trimDist << "\n";
        m_text->AppendText(ss.str());
        update_text();
    }

    std::vector<ICPPoint> Points2p;

    // Find which points to use based on trimDist and also finds the centroid of
    // the two points
    Point centroid1, centroid2;

    centroid1.x = 0;
    centroid1.y = 0;
    centroid1.z = 0;

    centroid2.x = 0;
    centroid2.y = 0;
    centroid2.z = 0;

    double sum_dist = 0;
    int count = 0;

    for (size_t i = 0; i < m_points1.size(); i++) {
        if (m_points1[i].dist_sq > trimDist) {
            continue;
        }

        sum_dist += m_points1[i].dist_sq;

        centroid1.x += m_points1[i].x;
        centroid1.y += m_points1[i].y;
        centroid1.z += m_points1[i].z;

        centroid2.x += m_points1[i].nearest.x;
        centroid2.y += m_points1[i].nearest.y;
        centroid2.z += m_points1[i].nearest.z;

        count++;
    }

    m_MSE = sum_dist / count;

    centroid1.x /= count;
    centroid1.y /= count;
    centroid1.z /= count;

    centroid2.x /= count;
    centroid2.y /= count;
    centroid2.z /= count;

    /************************************
   * FIND OPTIMAL ROTATION USING SVD
   ************************************/

    Eigen::Matrix3d H;

    H.setZero();

    if (m_text) {
        m_text->AppendText("Calculating optimal transform ... \n");
        update_text();
    }
    std::chrono::system_clock::time_point start = std::chrono::system_clock::now();

    for (size_t i = 0; i < m_points1.size(); i++) {
        if (m_points1[i].dist_sq > trimDist) {
            continue;
        }

        Eigen::Vector3d A, B;

        A(0) = m_points1[i].x - centroid1.x;
        A(1) = m_points1[i].y - centroid1.y;
        A(2) = m_points1[i].z - centroid1.z;

        B(0) = m_points1[i].nearest.x - centroid2.x;
        B(1) = m_points1[i].nearest.y - centroid2.y;
        B(2) = m_points1[i].nearest.z - centroid2.z;

        H += A * B.transpose();
    }

    Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);

    // Optimal rotation
    Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose();

    // Final transformation matrix is T*R*Tc
    // Tc - translates point1 to the centre for finding the rotation
    // R - optimal rotation
    // T - translates point1 back to the centre of point2

    Eigen::Matrix4d Tc, T, RR;

    Tc.setIdentity();
    T.setIdentity();
    RR.setIdentity();

    Tc(0, 3) = -centroid1.x;
    Tc(1, 3) = -centroid1.y;
    Tc(2, 3) = -centroid1.z;

    T(0, 3) = centroid2.x;
    T(1, 3) = centroid2.y;
    T(2, 3) = centroid2.z;

    RR.block(0,0,3,3) = R;

    Transform = T * (RR * Tc);

    PointOP::ApplyTransform(m_points1, Transform);

    // Clean up memory
    {
        std::vector<ICPPoint> Empty;
        Points2p.swap(Empty);
    }
}

void ICP::SetMaxPoints(unsigned int max) { m_max_points = max; }

void ICP::SetPoints(std::vector<Point>& P1, std::vector<Point>& P2,
    float dist_threshold)
{
    const float sq_dist = dist_threshold * dist_threshold;

    if (m_max_points == 0) {
        throw std::runtime_error("ICP: Need to set m_max_points before running SetPoints");
    }

    ANN Point1DB, Point2DB;
    std::vector<Point> filtered1, filtered2;
    Point start, end;

    reverseable_shuffle_forward(P1, m_point_cloud_data[0].table);
    reverseable_shuffle_forward(P2, m_point_cloud_data[1].table);

    auto update_text = [&]() {
        while (m_app->Pending()) {
            m_app->Dispatch();
        }
    };

    // NOTE:
    // The aim of this is to save as much memory as possible.
    // We downsample the points before loading it into the ANN library. At the
    // same time we want to avoid over downsampling. We want the process result
    // in: point cloud2 -> overlapping region filtering -> down sample > initial outlier
    // distance -> max point filtering = number of points point cloud2 is m_max_points So
    // basically, we want all the filtering and downsampling stuff to leave enough
    // points to meet the user's requested MaxPoint parameter if possible

    // Downsample based on a factor of m_max_points
    size_t k = m_max_points * 2;

    if (P1.size() > k) {
        filtered1.resize(k);
    } else {
        filtered1.resize(P1.size());
    }

    for (size_t i = 0; i < filtered1.size(); i++) {
        filtered1[i] = P1[i];
    }

    if (P2.size() > k) {
        filtered2.resize(k);
    } else {
        filtered2.resize(P2.size());
    }

    for (size_t i = 0; i < filtered2.size(); i++) {
        filtered2[i] = P2[i];
    }

    // This consumes the most memory out of the entire program
    // Every 1 million point uses about 200MB of memory when loaded using ANN
    Point2DB.SetPoints(filtered2);

    // for displaying purposes
    if (m_text) {
        m_text->AppendText("Downsampling and filtering first point cloud for ICP ... \n");
    }

    for (size_t i = 0;
         i < filtered1.size() && m_points1.size() < m_max_points; i++) {
        if (i % 100000 == 0) {
            if (m_text) {
                std::stringstream ss;

                ss << i * 100 / (float)filtered1.size() << "% ";
                m_text->AppendText(ss.str());

                update_text();
            }
        }

        double dist;
        int index;

        Point2DB.FindClosest(filtered1[i], dist, index);

        if (dist < sq_dist) {
            m_points1.push_back(ICPPoint(filtered1[i]));
        }
    }

    if (m_text) {
        m_text->AppendText("100%\n");

        std::stringstream ss;
        ss << "Number of points after downsampling/filtering: " << m_points1.size() << "\n";
        m_text->AppendText(ss.str());

        update_text();
    }

    Point2DB.Free();

    {
        std::vector<Point> empty;
        filtered1.swap(empty);
    }

    Point1DB.SetPoints(m_points1);

    if (m_text) {
        m_text->AppendText("Downsampling and filtering second point cloud for ICP ... \n");
    }

    // Do the same again

    for (size_t i = 0;
         i < filtered2.size() && m_points2.size() < m_max_points; i++) {
        if (i % 100000 == 0) {
            if (m_text) {
                std::stringstream ss;
                ss << i * 100 / (float)filtered2.size() << "% ";
                m_text->AppendText(ss.str());

                update_text();
            }
        }

        double dist;
        int index;

        Point1DB.FindClosest(filtered2[i], dist, index);

        if (dist < sq_dist) {
            m_points2.push_back(filtered2[i]);
        }
    }

    if (m_text) {
        m_text->AppendText("100%\n");

        std::stringstream ss;
        ss << "Number of points after downsampling/filtering: " << m_points2.size() << "\n";
        m_text->AppendText(ss.str());

        update_text();
    }

    Point1DB.Free();

    m_ANN_points2.SetPoints(m_points2);

    reverseable_shuffle_backward(P1, m_point_cloud_data[0].table);
    reverseable_shuffle_backward(P2, m_point_cloud_data[1].table);
}

void ICP::Seteps(float e)
{
    m_eps = e;
    m_ANN_points2.Seteps(m_eps);
}

double ICP::GetMSE() { return m_MSE; }
void ICP::SetwxTextCtrl(wxTextCtrl* t) // Used for m_text feedback from ICP
{
    m_text = t;
}
void ICP::SetwxApp(wxApp* a) // Used for m_text feedback from ICP
{
    m_app = a;
}


#include "Position.h"

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

#include "operationVector.h" // ceci va chercher le fichier qui contient les operations sur les vecteurs
#include "alpha.h"

using namespace std;
using namespace boost::numeric::ublas;

matrix<double> Position(matrix<double> Obs, matrix<double> M, std::vector<double> pie) {

    int n = Obs.size1();
    int m = Obs.size2();
    auto alpha = Alpha(Obs, M, pie);
    matrix<double> Probs(n, m);
    double truc=0.0;

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

        double maxx = getMax(ligne(alpha,i));
        for ( int j=0; j<m; ++j){
            truc = truc+ exp(alpha(i,j)-maxx);
        }
        double corr = maxx+log(truc);
        for ( int j=0; j<m; ++j){
            Probs(i,j) = exp(alpha(i,j)-corr);
        }
    }

	for ( int j=0; j<n; ++j) {
		double sumLigne = sum(ligne(Probs,j));
		if ( sumLigne ==0 ) {
			Probs(j,j) = 1;

		} else {
			for ( int i = 0;  i <m; ++i){
				Probs(j,i) = Probs(j,i) / sumLigne;
			}
		}
	}
    return Probs;

}


/*! @file pitts_fixed_tensor3_split.hpp
* @brief contract two simple fixed-dimension rank-3 tensors (along third and first dimension)
* @author Melven Roehrig-Zoellner <Melven.Roehrig-Zoellner@DLR.de>
* @date 2019-12-29
* @copyright Deutsches Zentrum fuer Luft- und Raumfahrt e. V. (DLR), German Aerospace Center
*
**/

// include guard
#ifndef PITTS_FIXED_TENSOR3_SPLIT_HPP
#define PITTS_FIXED_TENSOR3_SPLIT_HPP

// includes
#include <array>
#pragma GCC push_options
#pragma GCC optimize("no-unsafe-math-optimizations")
#include <Eigen/Dense>
#pragma GCC pop_options
#include "pitts_fixed_tensor3.hpp"
#include "pitts_timer.hpp"

//! namespace for the library PITTS (parallel iterative tensor train solvers)
namespace PITTS
{
  //! split a fixed-size rank-3 tensor into 2 smaller tensors
  //!
  //! Split t3c into t3a and t3b such that
  //!   t3c_(i,k,j) = sum_l t3a_(i,k1,l) * t3b_(l,k2,j)   with k=k2*N+k1
  //!
  //! @tparam T  underlying data type (double, complex, ...)
  //! @tparam N  dimension
  //!
  //! @param[in]  t3c       rank-3 tensor
  //! @param[out] t3a       first part of splitted rank-3 tensor
  //! @param[out] t3b       second part of splitted rank-3 tensor
  //! @param[in]  leftOrtog make left part (t3a) orthogonal if true, otherwise t3b is made orthogonal
  //!
  template<typename T, int N>
  void split(const FixedTensor3<T,N*N>& t3c, FixedTensor3<T,N>& t3a, FixedTensor3<T,N>& t3b, bool leftOrthog = true)
  {
    const auto timer = PITTS::timing::createScopedTimer<FixedTensor3<T,N>>();

    using Matrix = Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>;
    using Stride = Eigen::OuterStride<Eigen::Dynamic>;
    using Map = Eigen::Map<Matrix, Eigen::Aligned128, Stride>;
    using ConstMap = Eigen::Map<const Matrix, Eigen::Aligned128, Stride>;

    const auto r1 = t3c.r1();
    const auto r2 = t3c.r2();
    if( r1*r2 == 0 )
      throw std::invalid_argument("Unsupported dimension of zero!");

    const auto t3cMap = ConstMap(&t3c(0,0,0), N*r1, N*r2, Stride(N*r1));

    // use a faster QR algorithm (at the risk of a badly estimated truncation error)
    if( leftOrthog )
    {
      Eigen::ColPivHouseholderQR<Matrix> qr(t3cMap);
      qr.setThreshold(1.e-10);
      const auto r = std::max(Eigen::Index(1), qr.rank());

      t3a.resize(r1,r);
      t3b.resize(r,r2);

      // A P = Q R
      // => A = Q (R P^(-1))
      const Matrix Q = qr.matrixQ();
      const Matrix R = qr.matrixR().topRows(r).template triangularView<Eigen::Upper>();
      const auto P = qr.colsPermutation();
      Map(&t3a(0,0,0), r1*N, r, Stride(r1*N)) = Q.leftCols(r);
      Map(&t3b(0,0,0), r, r2*N, Stride(r)) = R * P.inverse();
    }
    else // rightOrthog
    {
      Eigen::ColPivHouseholderQR<Matrix> qr(t3cMap.transpose());
      qr.setThreshold(1.e-10);
      const auto r = std::max(Eigen::Index(1), qr.rank());

      t3a.resize(r1,r);
      t3b.resize(r,r2);

      // A^T P = Q R
      // => P^T A = R^T Q^T
      // => A = (R P^(-1))^T Q^T
      const Matrix Q = qr.matrixQ();
      const Matrix R = qr.matrixR().topRows(r).template triangularView<Eigen::Upper>();
      const auto P = qr.colsPermutation();
      Map(&t3a(0,0,0), r1*N, r, Stride(r1*N)) = (R * P.inverse()).transpose();
      Map(&t3b(0,0,0), r, r2*N, Stride(r)) = Q.leftCols(r).transpose();
    }
    /*
    auto svd = Eigen::JacobiSVD<Matrix, Eigen::HouseholderQRPreconditioner>(t3cMap, Eigen::ComputeThinV | Eigen::ComputeThinU);
    svd.setThreshold(1.e-10);
    const auto r = svd.rank();

    t3a.resize(r1,r);
    t3b.resize(r,r2);
    if( leftOrthog )
    {
      Map(&t3a(0,0,0), r1*N,r) = svd.matrixU().leftCols(r);
      Map(&t3b(0,0,0), r,r2*N) = svd.singularValues().head(r).asDiagonal() * svd.matrixV().leftCols(r).adjoint();
    }
    else
    {
      Map(&t3a(0,0,0), r1*N,r) = svd.matrixU().leftCols(r) * svd.singularValues().head(r).asDiagonal();
      Map(&t3b(0,0,0), r,r2*N) = svd.matrixV().leftCols(r).adjoint();
    }
    //std::cout << "Singular value |.|: " << svd.singularValues().head(r).transpose().array().abs() << "\n";
    */
  }

}

#endif // PITTS_FIXED_TENSOR3_SPLIT_HPP


#pragma once

#include <Eigen/Core>

namespace ipc::rigid {

//-------------------------------------------------------------------------
// Signed Distances (useful for CCD)
//-------------------------------------------------------------------------

/// Compute the signed distance between a point and a line
/// WARNING: Produces the same sign as euclidean distance but may be
/// different scales.
template <typename T>
inline T point_line_signed_distance(
    const Vector2<T>& point,
    const Vector2<T>& line_point0,
    const Vector2<T>& line_point1);

/// Compute the signed distance between two lines
/// WARNING: Produces the same sign as euclidean distance but may be
/// different scales.
/// WARNING: Parallel edges results in zero distance
template <typename T>
inline T line_line_signed_distance(
    const Vector3<T>& line0_point0,
    const Vector3<T>& line0_point1,
    const Vector3<T>& line1_point0,
    const Vector3<T>& line1_point1);

/// Compute the distance between a point and a plane.
/// Normal is assumed to be unit length.
/// WARNING: Produces the same sign as euclidean distance but may be
/// different scales.
template <typename T>
inline T point_plane_signed_distance(
    const Vector3<T>& point,
    const Vector3<T>& triangle_vertex0,
    const Vector3<T>& triangle_vertex1,
    const Vector3<T>& triangle_vertex2);

} // namespace ipc::rigid

#include "distance.tpp"


#include <iostream>
#include <vector>

#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace geom = boost::geometry;

int main()
{
  // point in 2d cartesian c.s.
  typedef geom::model::d2::point_xy<double> point;
  // polygon of points, counterclockwise, open (i.e. last != first)
  typedef geom::model::polygon<point, false, false> polygon;

  polygon q4{{ {0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {0.0, 1.0} }};
  polygon t3{{ {0.2, 0.2}, {0.8, 0.2}, {0.2, 0.8} }};

  std::vector<polygon> N;
  geom::intersection(q4, t3, N);

  for (auto i = 0; i < N.size(); ++i) {
    std::cout << "polygon " << i << std::endl;
    const auto& polynodes = N[i].outer();

    for (auto j = 0; j < polynodes.size(); ++j) {
      const auto& nd = polynodes[j];
      std::cout << "  node " << j << ": "
                << nd.get<0>() << " " << nd.get<1>() << std::endl;
    }

  }

  return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <random>
#include <vector>
#include <sstream>

#include <geometry_test_common.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/extensions/random/uniform_point_distribution.hpp>

typedef bg::model::point<double, 2, bg::cs::cartesian> point2d_cart;
typedef bg::model::point<double, 3, bg::cs::cartesian> point3d_cart;
typedef bg::model::point<double, 2, bg::cs::geographic<bg::degree>> point2d_geog;

void test_geographic()
{
    //We check whether the generated points lie roughly along
    //the great circle segment (<2 km distance on a sphere with
    //the radius of earth) and are distributed uniformly with respect
    //to great circle arc length.
    typedef bg::model::linestring<point2d_geog> linestring;
    linestring ls {{ 0.0, 0.0 }, { 45.0, 45.0 }, { 60.0, 60.0 }};
    bg::random::uniform_point_distribution<linestring> l_dist(ls);
    std::mt19937 generator(0);
    int sample_count = 2000;
    int count_below_45 = 0;
    for (int i = 0 ; i < sample_count ; ++i)
    {
        point2d_geog sample = l_dist(generator);
        BOOST_CHECK( bg::distance(sample, ls) < 2000 );
        if(bg::get<0>(sample) < 45.0) count_below_45++;
    }
    double length_ratio = bg::distance(ls[0], ls[1]) /
        ( bg::distance(ls[0], ls[1]) + bg::distance(ls[1], ls[2]) );
    double sample_ratio = ((double) count_below_45) / sample_count;
    bool in_range = sample_ratio * 0.95 < length_ratio
        && sample_ratio * 1.05 > length_ratio;
    BOOST_CHECK( in_range );

    //We check whether the generated points lie in the spherical box
    //(which is actually a triangle in this case) and whether the latitude
    //is distributed as expected for uniform spherical distribution, using
    //known area ratios of spherical caps.
    typedef bg::model::box<point2d_geog> box;
    box b {{ 0.0, 0.0 }, { 90.0, 90.0 }};
    bg::random::uniform_point_distribution<box> b_dist(b);
    int under_60 = 0;
    for (int i = 0 ; i < sample_count ; ++i)
    {
        point2d_geog sample = b_dist(generator);
        BOOST_CHECK( bg::within(sample, b) );
        if(bg::get<1>(sample) < 60.0) ++under_60;
    }
    BOOST_CHECK_GT(under_60, 0.5 * 0.95 * sample_count);
    BOOST_CHECK_LT(under_60, 0.5 * 1.05 * sample_count);
}

void test_polygon()
{
    //This test will test uniform sampling in polygon, which also checks
    //uniform sampling in boxes. We check whether two equal distributions
    //(copied using operator<< and operator>>) generate the same sequence
    //of points and whether those points are uniformly distributed with
    //respect to cartesian area.
    typedef bg::model::polygon<point2d_cart> polygon;
    polygon poly;
    bg::read_wkt(
        "POLYGON((16 21,17.1226 17.5451,20.7553 17.5451, 17.8164 15.4098,18.9389 11.9549,16 14.0902,13.0611 11.9549, 14.1836 15.4098,11.2447 17.5451,14.8774 17.5451,16 21))",
        poly);
    bg::random::uniform_point_distribution<polygon> poly_dist(poly);
    bg::random::uniform_point_distribution<polygon> poly_dist2;
    BOOST_CHECK( !(poly_dist == poly_dist2) );
    std::stringstream ss;
    ss << poly_dist;
    ss >> poly_dist2;
    BOOST_CHECK( poly_dist == poly_dist2 );
    std::mt19937 generator(0), generator2(0);
    for (int i = 0 ; i < 100 ; ++i)
    {
        point2d_cart sample1 = poly_dist(generator);
        BOOST_CHECK( bg::equals(sample1, poly_dist2(generator2)) );
        BOOST_CHECK( bg::within(sample1, poly) );
    }
    std::vector<point2d_cart> randoms;
    const int uniformity_test_samples = 2000;
    for (int i = 0 ; i < uniformity_test_samples ; ++i)
    {
        randoms.push_back(poly_dist(generator));
    }
    typedef bg::model::box<point2d_cart> box;
    box env, lhalf;
    bg::envelope(poly, env);
    bg::set<bg::min_corner, 0>(lhalf, bg::get<bg::min_corner, 0>(env));
    bg::set<bg::min_corner, 1>(lhalf, bg::get<bg::min_corner, 1>(env));
    bg::set<bg::max_corner, 0>(lhalf, bg::get<bg::max_corner, 0>(env));
    bg::set<bg::max_corner, 1>(lhalf,
        (bg::get<bg::max_corner, 1>(env) + bg::get<bg::min_corner, 1>(env)) / 2);
    std::vector<polygon> lower;
    bg::intersection(lhalf, poly, lower);
    double area_ratio = bg::area(lower[0])/bg::area(poly);
    int in_lower = 0;
    for (int i = 0 ; i < uniformity_test_samples ; ++i)
    {
        if(bg::within(randoms[i], lhalf))
            ++in_lower;
    }
    double sample_ratio = ((double) in_lower ) / uniformity_test_samples;
    BOOST_CHECK_GT( sample_ratio * 1.05, area_ratio );
    BOOST_CHECK_LT( sample_ratio * 0.95, area_ratio );
}

void test_multipoint()
{
    typedef bg::model::multi_point<point3d_cart> multipoint;
    multipoint mp {{0.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}};
    int first = 0;
    bg::random::uniform_point_distribution<multipoint> mp_dist(mp);
    std::mt19937 generator(0);
    int sample_count = 1000;
    for (int i = 0 ; i < sample_count ; ++i)
    {
        point3d_cart sample = mp_dist(generator);
        BOOST_CHECK( bg::within(sample, mp) );
        if(bg::equals(sample, mp[0])) ++first;
    }
    BOOST_CHECK_GT(first * 1.05, sample_count / 3);
    BOOST_CHECK_LT(first * 0.95, sample_count / 3);
}

int test_main(int, char* [])
{
    test_polygon();
    test_geographic();
    test_multipoint();
    return 0;
}


#ifndef SPLINE_FITTER_HPP
#define SPLINE_FITTER_HPP
#include <vector>
#include <Eigen/SparseLU>

/**
 * Quadratic B-spline curve fitter
 * This class computes the control points necessary to make a quadratic B-spline curve
 * interpolating a known set of points.
 *
 */
template<typename T>
class QuadraticSplineFitter
{
public:

  /**
   * Default constructor
   */
  QuadraticSplineFitter() : QuadraticSplineFitter({})
  {
  };

  /**
   * Constructor
   * @param points Points to interpolate
   */
  QuadraticSplineFitter(std::vector<T>& points){
    setPoints(points);
  }

  /**
   * Set points to interpolate
   * @param points points to interpolate
   */
  inline void
  setPoints(std::vector<T>& points)
  {
    m_points = points;
  }

  /**
   * Compute the control points necessary to interpolate the points set by setPoints
   * @return the control points
   */
  std::vector<T>
  compute_control_points() const
  {
    // Here:
    // Build a NxN matrix such that
    // matrix[row][row] =  matrix[row][row+1] = 1/2
    // rest = 0
    // build B such that B = [ep_start points ep_stop]
    // Solve Ax = B
    // x are the knots
    int n = m_points.size()+2;
    Eigen::SparseMatrix<T> A(n,n);

    Eigen::Matrix<T, Eigen::Dynamic, 1> b(n);
    std::vector<Eigen::Triplet<T> > triplets;
    triplets.reserve((n)*2);

    // Initialize coefficients for the points to reach

    for(int i = 1; i < n-1; i++)
    {
      triplets.push_back(Eigen::Triplet<T>(i,i-1,0.125));
      triplets.push_back(Eigen::Triplet<T>(i,i,0.75));
      triplets.push_back(Eigen::Triplet<T>(i,i+1,0.125));
      b(i) = m_points[i-1];
    }

    __insertStartpointCoeffs(triplets,b,0);
    __insertEndpointCoeffs(triplets,b,n-1);
    A.setFromTriplets(triplets.begin(), triplets.end());

    // Now solve the linear system using linear least squares
    Eigen::Matrix<T, Eigen::Dynamic, 1> x(n);
    Eigen::SparseLU<Eigen::SparseMatrix<T> > solver;

    solver.compute(A);
    x = solver.solve(b);

    //     printVector(x,n);
    // Copy back to std::vector
    std::vector<T> ret;

    ret.reserve(n);
    for(int i = 0; i < n; i++)
    {
      ret.push_back(x(i));
    }
    return ret;

  }

private:
  inline void
  __insertStartpointCoeffs( std::vector<Eigen::Triplet<T> >& triplets,
			    Eigen::Matrix<T, Eigen::Dynamic, 1>& b,
			    const int i) const
  {
    triplets.push_back(Eigen::Triplet<T>(i,i,-1));
    triplets.push_back(Eigen::Triplet<T>(i,i+1,1));
    b(i) = 0.0;
  }

  inline void
  __insertEndpointCoeffs( std::vector<Eigen::Triplet<T> >& triplets,
			  Eigen::Matrix<T, Eigen::Dynamic, 1>& b,
			  const int i) const
  {
    triplets.push_back(Eigen::Triplet<T>(i,i-1,-1));
    triplets.push_back(Eigen::Triplet<T>(i,i,1));
    b(i) = 0.0;
  }

  std::vector<T> m_points;

};

#endif


#include <cmath>
#include <vector>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include <BingoCpp/backend.h>

#include "testing_utils.h"
#include "test_fixtures.h"

using namespace bingo;
using namespace backend;

namespace {

const int N_OPS = 13;

struct AGraphValues {
Eigen::ArrayXXd x_vals;
Eigen::VectorXd constants;
AGraphValues() {}
AGraphValues(Eigen::ArrayXXd &x, Eigen::VectorXd &c) :
  x_vals(x), constants(c) {}
};

class AGraphBackend : public ::testing::TestWithParam<int> {
 public:

  const double AGRAPH_VAL_START =-1;
  const double AGRAPH_VAL_END = 0;
  const int N_AGRAPH_VAL = 11;

  AGraphValues sample_agraph_1_values;
  std::vector<Eigen::ArrayXXd> operator_evals_x0;
  std::vector<Eigen::ArrayXXd> operator_x_derivs;
  std::vector<Eigen::ArrayXXd> operator_c_derivs;

  Eigen::ArrayX3i simple_stack;
  Eigen::ArrayX3i simple_stack2;
  Eigen::ArrayXXd x;
  Eigen::ArrayXd constants;

  virtual void SetUp() {
    sample_agraph_1_values = init_agraph_vals(AGRAPH_VAL_START,
                                              AGRAPH_VAL_END,
                                              N_AGRAPH_VAL);
    operator_evals_x0 = init_op_evals_x0(sample_agraph_1_values);
    operator_x_derivs = init_op_x_derivs(sample_agraph_1_values);
    operator_c_derivs = init_op_c_derivs(sample_agraph_1_values);

    simple_stack = testutils::stack_operators_0_to_5();
    simple_stack2 = testutils::stack_unary_operator(4);
    x = testutils::one_to_nine_3_by_3();
    constants = testutils::pi_ten_constants();
  }
  virtual void TearDown() {}

 AGraphValues init_agraph_vals(double begin, double end, int num_points) {
  Eigen::VectorXd constants = Eigen::VectorXd(2);
  constants << 10, 3.14;

  Eigen::ArrayXXd x_vals(num_points, 2);
  x_vals.col(0) = Eigen::ArrayXd::LinSpaced(num_points, begin, 0);
  x_vals.col(1) = Eigen::ArrayXd::LinSpaced(num_points, 0, end);

  return AGraphValues(x_vals, constants);
}

std::vector<Eigen::ArrayXXd> init_op_evals_x0(
    const AGraphValues &sample_agraph_1_values) {
  Eigen::ArrayXXd x_0 = sample_agraph_1_values.x_vals.col(0);
  double constant = sample_agraph_1_values.constants[0];
  Eigen::ArrayXXd c_0 = constant * Eigen::ArrayXd::Ones(x_0.rows());

  const std::vector<Eigen::ArrayXXd> op_evals_x0 = {
    x_0,
    c_0,
    x_0+x_0,
    x_0-x_0,
    x_0*x_0,
    x_0/x_0,
    x_0.sin(),
    x_0.cos(),
    x_0.exp(),
    x_0.abs().log(),
    x_0.abs().pow(x_0),
    x_0.abs(),
    x_0.abs().sqrt()
  };

  return op_evals_x0;
}

std::vector<Eigen::ArrayXXd> init_op_x_derivs(
    const AGraphValues &sample_agraph_1_values) {
  Eigen::ArrayXXd x_0 = sample_agraph_1_values.x_vals.col(0);
  int size = x_0.rows();

  auto last_nan = [](Eigen::ArrayXXd array) {
    array(array.rows() - 1, array.cols() -1) = std::nan("1");
    Eigen::ArrayXXd modified_array = array;
    return modified_array;
  };

  const std::vector<Eigen::ArrayXXd> op_x_derivs = {
    Eigen::ArrayXd::Ones(size),
    Eigen::ArrayXd::Zero(size),
    2.0  * Eigen::ArrayXd::Ones(size),
    Eigen::ArrayXd::Zero(size),
    2.0 * x_0,
    last_nan(Eigen::ArrayXd::Zero(size)),
    x_0.cos(),
    -x_0.sin(),
    x_0.exp(),
    1.0 / x_0,
    last_nan(x_0.abs().pow(x_0)*(x_0.abs().log() + Eigen::ArrayXd::Ones(size))),
    x_0.sign(),
    0.5 * x_0.sign() / x_0.abs().sqrt()
  };

  return op_x_derivs;
}

std::vector<Eigen::ArrayXXd> init_op_c_derivs(
    const AGraphValues &sample_agraph_1_values) {
  int size = sample_agraph_1_values.x_vals.rows();
  Eigen::ArrayXXd c_1 = sample_agraph_1_values.constants[1] * Eigen::ArrayXd::Ones(size);

  std::vector<Eigen::ArrayXXd> op_c_derivs =  {
    Eigen::ArrayXd::Zero(size),
    Eigen::ArrayXd::Ones(size),
    2.0  * Eigen::ArrayXd::Ones(size),
    Eigen::ArrayXd::Zero(size),
    2.0 * c_1,
    (Eigen::ArrayXd::Zero(size)),
    c_1.cos(),
    -c_1.sin(),
    c_1.exp(),
    1.0 / c_1,
    c_1.abs().pow(c_1)*(c_1.abs().log() + Eigen::ArrayXd::Ones(size)),
    c_1.sign(),
    0.5 * c_1.sign() / c_1.abs().sqrt()
  };

  return op_c_derivs;
}
};

TEST_P(AGraphBackend, simplify_and_evaluate) {
  int operator_i = GetParam();
  Eigen::ArrayXXd expected_outcome = operator_evals_x0[operator_i];

  Eigen::ArrayX3i stack(3, 3);
  stack << 0, 0, 0,
           0, 1, 0,
           operator_i, 0, 0;
  Eigen::ArrayXXd f_of_x = SimplifyAndEvaluate(stack,
                                               sample_agraph_1_values.x_vals,
                                               sample_agraph_1_values.constants);
  ASSERT_TRUE(testutils::almost_equal(expected_outcome, f_of_x));
}

TEST_P(AGraphBackend, simplify_and_evaluate_x_deriv) {
  int operator_i = GetParam();
  Eigen::ArrayXXd expected_derivative =
    Eigen::ArrayXXd::Zero(sample_agraph_1_values.x_vals.rows(), 2);
  expected_derivative.col(0) = operator_x_derivs[operator_i];

  Eigen::ArrayX3i stack(4, 3);
  stack << 0, 0, 0,
           0, 0, 0,
           0, 1, 1,
           operator_i, 0, 1;

  Eigen::ArrayXXd x_0 = sample_agraph_1_values.x_vals;
  Eigen::ArrayXXd constants = sample_agraph_1_values.constants;
  std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> res_and_gradient =
    SimplifyAndEvaluateWithDerivative(stack,
                                      x_0,
                                      constants,
                                      true);
  Eigen::ArrayXXd df_dx = res_and_gradient.second;
  ASSERT_TRUE(testutils::almost_equal(expected_derivative, df_dx));
}

TEST_P(AGraphBackend, simplify_and_evaluate_c_deriv) {
  int operator_i = GetParam();
  int num_x_points = sample_agraph_1_values.x_vals.rows();
  int num_consts = sample_agraph_1_values.constants.size();
  int last_col = num_consts - 1;
  Eigen::ArrayXXd expected_derivative =
    Eigen::MatrixXd::Zero(num_x_points, num_consts).array();
  expected_derivative.col(last_col) = operator_c_derivs[operator_i];

  Eigen::ArrayX3i stack(4, 3);
  stack << 1, 1, 1,
           1, 1, 1,
           0, 1, 1,
           operator_i, 1, 0;

  Eigen::ArrayXXd x_0 = sample_agraph_1_values.x_vals;
  Eigen::ArrayXXd constants = sample_agraph_1_values.constants;
  std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> res_and_gradient =
    SimplifyAndEvaluateWithDerivative(stack,
                                      x_0,
                                      constants,
                                      false);
  Eigen::ArrayXXd df_dc = res_and_gradient.second;
  ASSERT_TRUE(testutils::almost_equal(expected_derivative, df_dc));
}
INSTANTIATE_TEST_CASE_P(,AGraphBackend, ::testing::Range(0, N_OPS, 1));

TEST_F(AGraphBackend, evaluate) {
  Eigen::ArrayXXd y = Evaluate(simple_stack, x, constants);
  Eigen::ArrayXXd y_true = x.col(0) * (constants[0] + constants[1]
                          / x.col(1)) - x.col(0);
  ASSERT_TRUE(testutils::almost_equal(y, y_true));
}

TEST_F(AGraphBackend, evaluate_and_derivative) {
  std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> y_and_dy =
    EvaluateWithDerivative(simple_stack, x, constants);
  Eigen::ArrayXXd y_true = x.col(0) * (constants[0] + constants[1]
                          / x.col(1)) - x.col(0);
  Eigen::ArrayXXd dy_true = Eigen::ArrayXXd::Zero(3, 3);
  dy_true.col(0) = constants[0] + constants[1] / x.col(1) - 1.;
  dy_true.col(1) = - x.col(0) * constants[1] / x.col(1) / x.col(1);

  ASSERT_TRUE(testutils::almost_equal(y_and_dy.first, y_true));
  ASSERT_TRUE(testutils::almost_equal(y_and_dy.second, dy_true));
}

TEST_F(AGraphBackend, mask_evaluate) {
  Eigen::ArrayXXd y = Evaluate(simple_stack, x, constants);
  Eigen::ArrayXXd y_simple = SimplifyAndEvaluate(simple_stack, x, constants);
  ASSERT_TRUE(testutils::almost_equal(y, y_simple));
}

TEST_F(AGraphBackend, mask_evaluate_and_derivative) {
  std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> y_and_dy =
    EvaluateWithDerivative(simple_stack, x, constants);
  std::pair<Eigen::ArrayXXd, Eigen::ArrayXXd> y_and_dy_simple =
    SimplifyAndEvaluateWithDerivative(simple_stack, x, constants);
  ASSERT_TRUE(testutils::almost_equal(y_and_dy.first, y_and_dy_simple.first));
  ASSERT_TRUE(testutils::almost_equal(y_and_dy.first, y_and_dy_simple.first));
}

TEST_F(AGraphBackend, get_utilized_commands) {
  std::vector<bool> used_commands = GetUtilizedCommands(simple_stack);
  int num_used_commands = 0;
  for (auto const &command_is_used : used_commands) {
    if (command_is_used) {
      ++num_used_commands;
    }
  }
  ASSERT_EQ(num_used_commands, 8);
}
} // namespace


#pragma once

#include <Eigen/Dense>

namespace dutyroll
{
	Eigen::MatrixXd rollduty_fixed(
		const Eigen::VectorXd data,
		const int window,
		const Eigen::VectorXd thresholds);

	Eigen::MatrixXd rollduty_variable(
		const Eigen::VectorXd time,
		const Eigen::VectorXd data,
		const double window,
		const Eigen::VectorXd thresholds);
}


#include <cmath>
#include <stdexcept>

#include <gtest/gtest.h>
#include <Eigen/Dense>

#include <BingoCpp/equation.h>
#include <BingoCpp/fitness_function.h>
#include <BingoCpp/training_data.h>

#include "test_fixtures.h"
#include "testing_utils.h"

namespace {

const double pi = std::acos(-1);

struct SampleTrainingData : public bingo::TrainingData {
  Eigen::ArrayXXd x;
  Eigen::ArrayXXd y;
  SampleTrainingData() : TrainingData() {}
  ~SampleTrainingData() {}
  SampleTrainingData(Eigen::ArrayXXd &x, Eigen::ArrayXXd &y) :
      x(x), y(y) { }
  SampleTrainingData* GetItem(int) {
    throw new std::logic_error("Not implemented Exception");
  }
  SampleTrainingData* GetItem(const std::vector<int> &) {
    throw new std::logic_error("Not implemented Exception");
  }
  int Size() { return x.rows(); }
};

class SampleFitnessFunction : public bingo::VectorBasedFunction {
 public:
  SampleFitnessFunction(SampleTrainingData* training_data,
      std::string metric = "mae") :
      bingo::VectorBasedFunction(training_data, metric) {}
  ~SampleFitnessFunction() {}
  Eigen::ArrayXXd EvaluateFitnessVector(const bingo::Equation &individual) const {
    Eigen::ArrayXXd f_of_x =
        individual.EvaluateEquationAt(((SampleTrainingData*)training_data_)->x);
    return f_of_x - ((SampleTrainingData*)training_data_)->y;
  }
};

class TestFitnessFunction : public testing::Test {
 public:
  SampleTrainingData training_data_;
  SampleFitnessFunction* sample_fitness_function_;
  void SetUp() {
    training_data_ = init_sample_training_data();
    sample_fitness_function_ = new SampleFitnessFunction(&training_data_);
  }

  void TearDown() {
    delete sample_fitness_function_;
  }

 private:
  SampleTrainingData init_sample_training_data() {
    Eigen::ArrayXXd x(3,3);
    x << ((1./6.) * pi), 1., 1.,
         ((1./2.) * pi), 3., 4.,
         pi            , 9., 16.;
    Eigen::ArrayXXd y(3, 1);
    y << 1.5, 2, 1;
    SampleTrainingData training_data(x, y);
    return training_data;
  }
};

TEST_F(TestFitnessFunction, InvalidTrainingMetric) {
  try {
    SampleFitnessFunction test_function(&training_data_, "invalid_metric");
    FAIL() << "Expecting std::invalid_argument exception\n";
  } catch (std::invalid_argument &exception) {
    SUCCEED();
  }
}

TEST_F(TestFitnessFunction, CorrectMetricSet) {
  SampleFitnessFunction mse(&training_data_, "mse");
  SampleFitnessFunction rmse(&training_data_, "rmse");
  bingo::AGraph agraph = testutils::init_sample_agraph_1();
  double mae_expected = 0.600018;
  double mse_expected = 0.389427;
  double rmse_expected = 0.624041;
  double error_tol = 10e-6;
  ASSERT_NEAR(mae_expected,
              sample_fitness_function_->EvaluateIndividualFitness(agraph),
              error_tol);
  ASSERT_NEAR(mse_expected,
              mse.EvaluateIndividualFitness(agraph),
              error_tol);
  ASSERT_NEAR(rmse_expected,
              rmse.EvaluateIndividualFitness(agraph),
              error_tol);
}
} // namespace (anonymous)


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__IMPL__SE2_HPP_
#define SMOOTH__IMPL__SE2_HPP_

#include <Eigen/Core>

#include "common.hpp"
#include "so2.hpp"

namespace smooth {

/**
 * @brief SE(2) Lie Group represented as C^1 ⋉ R^2
 *
 * Memory layout
 * -------------
 * Group:    x y qz qw
 * Tangent:  vx vy Ωz
 *
 * Lie group Matrix form
 * ---------------------
 * [ qw -qz x ]
 * [ qz  qw y ]
 * [  0   0 1 ]
 *
 * Lie algebra Matrix form
 * -----------------------
 * [ 0 -Ωz vx ]
 * [ Ωz  0 vy ]
 * [  0  0  0 ]
 *
 * Constraints
 * -----------
 * Group:   qz * qz + qw * qw = 1
 * Tangent: -pi < Ωz <= pi
 */
template<typename _Scalar>
class SE2Impl
{
public:
  using Scalar = _Scalar;

  static constexpr Eigen::Index RepSize = 4;
  static constexpr Eigen::Index Dim     = 3;
  static constexpr Eigen::Index Dof     = 3;

  SMOOTH_DEFINE_REFS;

  static void setIdentity(GRefOut g_out) { g_out << Scalar(0), Scalar(0), Scalar(0), Scalar(1); }

  static void setRandom(GRefOut g_out)
  {
    g_out.template head<2>().setRandom();
    SO2Impl<Scalar>::setRandom(g_out.template tail<2>());
  }

  static void matrix(GRefIn g_in, MRefOut m_out)
  {
    m_out.setIdentity();
    SO2Impl<Scalar>::matrix(g_in.template tail<2>(), m_out.template topLeftCorner<2, 2>());
    m_out.template topRightCorner<2, 1>() = g_in.template head<2>();
  }

  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out)
  {
    SO2Impl<Scalar>::composition(
      g_in1.template tail<2>(), g_in2.template tail<2>(), g_out.template tail<2>());
    Eigen::Matrix<Scalar, 2, 2> R1;
    SO2Impl<Scalar>::matrix(g_in1.template tail<2>(), R1);
    g_out.template head<2>() = R1 * g_in2.template head<2>() + g_in1.template head<2>();
  }

  static void inverse(GRefIn g_in, GRefOut g_out)
  {
    Eigen::Matrix<Scalar, 2, 1> so2inv;
    SO2Impl<Scalar>::inverse(g_in.template tail<2>(), so2inv);

    Eigen::Matrix<Scalar, 2, 2> Rinv;
    SO2Impl<Scalar>::matrix(so2inv, Rinv);

    g_out.template head<2>() = -Rinv * g_in.template head<2>();
    g_out.template tail<2>() = so2inv;
  }

  static void log(GRefIn g_in, TRefOut a_out)
  {
    using std::tan;

    Eigen::Matrix<Scalar, 1, 1> so2_log;
    SO2Impl<Scalar>::log(g_in.template tail<2>(), so2_log);
    const Scalar th  = so2_log(0);
    const Scalar th2 = th * th;

    const Scalar B = th / Scalar(2);
    Scalar A;
    if (th2 < Scalar(eps2)) {
      // https://www.wolframalpha.com/input/?i=series+x+%2F+tan+x+at+x%3D0
      A = Scalar(1) - th2 / Scalar(12);
    } else {
      A = B / tan(B);
    }

    Eigen::Matrix<Scalar, 2, 2> Sinv;
    Sinv(0, 0) = A;
    Sinv(1, 1) = A;
    Sinv(0, 1) = B;
    Sinv(1, 0) = -B;

    a_out.template head<2>() = Sinv * g_in.template head<2>();
    a_out(2)                 = th;
  }

  static void Ad(GRefIn g_in, TMapRefOut A_out)
  {
    SO2Impl<Scalar>::matrix(g_in.template tail<2>(), A_out.template topLeftCorner<2, 2>());
    A_out(0, 2) = g_in(1);
    A_out(1, 2) = -g_in(0);
    A_out(2, 0) = Scalar(0);
    A_out(2, 1) = Scalar(0);
    A_out(2, 2) = Scalar(1);
  }

  static void exp(TRefIn a_in, GRefOut g_out)
  {
    using std::cos, std::sin;

    const Scalar th  = a_in.z();
    const Scalar th2 = th * th;

    Scalar A, B;
    if (th2 < Scalar(eps2)) {
      // https://www.wolframalpha.com/input/?i=series+sin+x+%2F+x+at+x%3D0
      A = Scalar(1) - th2 / Scalar(6);
      // https://www.wolframalpha.com/input/?i=series+%28cos+x+-+1%29+%2F+x+at+x%3D0
      B = -th / Scalar(2) + th * th2 / Scalar(24);
    } else {
      A = sin(th) / th;
      B = (cos(th) - Scalar(1)) / th;
    }

    Eigen::Matrix<Scalar, 2, 2> S;
    S(0, 0) = A;
    S(1, 1) = A;
    S(0, 1) = B;
    S(1, 0) = -B;

    g_out.template head<2>() = S * a_in.template head<2>();
    SO2Impl<Scalar>::exp(a_in.template tail<1>(), g_out.template tail<2>());
  }

  static void hat(TRefIn a_in, MRefOut A_out)
  {
    A_out.setZero();
    SO2Impl<Scalar>::hat(a_in.template tail<1>(), A_out.template topLeftCorner<2, 2>());
    A_out.template topRightCorner<2, 1>() = a_in.template head<2>();
  }

  static void vee(MRefIn A_in, TRefOut a_out)
  {
    SO2Impl<Scalar>::vee(A_in.template topLeftCorner<2, 2>(), a_out.template tail<1>());
    a_out.template head<2>() = A_in.template topRightCorner<2, 1>();
  }

  static void ad(TRefIn a_in, TMapRefOut A_out)
  {
    A_out.setZero();
    SO2Impl<Scalar>::hat(a_in.template tail<1>(), A_out.template topLeftCorner<2, 2>());
    A_out(0, 2) = a_in.y();
    A_out(1, 2) = -a_in.x();
  }

  static void dr_exp(TRefIn a_in, TMapRefOut A_out)
  {
    using TangentMap = Eigen::Matrix<Scalar, 3, 3>;
    using std::sin, std::cos;

    const Scalar th2 = a_in.z() * a_in.z();

    Scalar A, B;
    if (th2 < Scalar(eps2)) {
      // https://www.wolframalpha.com/input/?i=series+%281-cos+x%29+%2F+x%5E2+at+x%3D0
      A = Scalar(1) / Scalar(2) - th2 / Scalar(24);
      // https://www.wolframalpha.com/input/?i=series+%28x+-+sin%28x%29%29+%2F+x%5E3+at+x%3D0
      B = Scalar(1) / Scalar(6) - th2 / Scalar(120);
    } else {
      const Scalar th = a_in.z();
      A               = (Scalar(1) - cos(th)) / th2;
      B               = (th - sin(th)) / (th2 * th);
    }

    TangentMap ad_a;
    ad(a_in, ad_a);
    A_out = TangentMap::Identity() - A * ad_a + B * ad_a * ad_a;
  }

  static void dr_expinv(TRefIn a_in, TMapRefOut A_out)
  {
    using TangentMap = Eigen::Matrix<Scalar, 3, 3>;
    using std::sin, std::cos;

    const Scalar th  = a_in.z();
    const Scalar th2 = th * th;

    Scalar A;
    if (th2 < Scalar(eps2)) {
      // https://www.wolframalpha.com/input/?i=series+1%2Fx%5E2+-+%281+%2B+cos+x%29+%2F+%282+*+x+*+sin+x%29+at+x%3D0
      A = Scalar(1) / Scalar(12) + th2 / Scalar(720);
    } else {
      A = (Scalar(1) / th2) - (Scalar(1) + cos(th)) / (Scalar(2) * th * sin(th));
    }

    TangentMap ad_a;
    ad(a_in, ad_a);
    A_out = TangentMap::Identity() + ad_a / 2 + A * ad_a * ad_a;
  }
};

}  // namespace smooth

#endif  // SMOOTH__IMPL__SE2_HPP_


#include <boost/math/special_functions/bessel.hpp>

/**
 * Bessel J_0 function
 *
 * @param  n index
 * @param x argument
 * @return value
 */
extern "C"
double j0(double x) {
    return boost::math::cyl_bessel_j<int, double>(0, x);
}

/**
 * Bessel Y_0 function
 *
 * @param  n index
 * @param x argument
 * @return value
 */
extern "C"
double y0(double x) {
    return boost::math::cyl_neumann<int, double>(0, x);
}

/**
 * Bessel J_1 function
 *
 * @param  n index
 * @param x argument
 * @return value
 */
extern "C"
double j1(double x) {
    return boost::math::cyl_bessel_j<int, double>(1, x);
}

/**
 * Bessel Y_1 function
 *
 * @param  n index
 * @param x argument
 * @return value
 */
extern "C"
double y1(double x) {
    return boost::math::cyl_neumann<int, double>(1, x);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 integration toolbox - quad"

#include <iostream>
#include <nt2/sdk/timing/tic.hpp>
#include <nt2/include/functions/quad.hpp>
#include <nt2/toolbox/integration/output.hpp>
#include <nt2/toolbox/integration/options.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/rowvect.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/expm1.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/dist.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/table.hpp>
struct f
{
  template < class X > inline
  X operator()(const X & x ) const
  {
    return x;
  }
};

NT2_TEST_CASE_TPL( quad_functor, NT2_REAL_TYPES )
{
  using nt2::quad;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(0), T(5));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  nt2::tic();
  BOOST_AUTO_TPL(res, quad<T>(f(), x));
  nt2::toc();
//                                  ,
//                                   options [ nt2::iterations_ = 100,
//                                             nt2::tolerance::absolute_ = T(0.001)
//                                     ]);

   std::cout << "Integrals:" << res.integrals << ") = " << res.errors
             << " after " << res.eval_count <<  " evaluations\n";

   NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::sqr(x)*nt2::Half<T>())), nt2::Sqrteps<T>());

}

NT2_TEST_CASE_TPL( quad_tag, NT2_REAL_TYPES )
{
  using nt2::quad;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(0), T(5));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  BOOST_AUTO_TPL(res, quad<T>(nt2::functor<nt2::tag::exp_>(), x));
//                                  ,
//                                   options [ nt2::iterations_ = 100,
//                                             nt2::tolerance::absolute_ = T(0.001)
//                                     ]);

   std::cout << "Integrals: " << res.integrals << " with " << res.errors
             << " after " << res.eval_count <<  " evaluations\n";

   NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::expm1(x))), nt2::Sqrteps<T>());

}
NT2_TEST_CASE_TPL( quad_tag_reverse, NT2_REAL_TYPES )
{
  using nt2::quad;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(5), T(-1), T(0));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  BOOST_AUTO_TPL(res, quad<T>(nt2::functor<nt2::tag::exp_>(), x));
//                                  ,
//                                   options [ nt2::iterations_ = 100,
//                                             nt2::tolerance::absolute_ = T(0.001)
//                                     ]);

  std::cout << "Integrals: " << res.integrals << " with " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";

  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::exp(x)-nt2::exp(T(5)))), nt2::Sqrteps<T>());

}

NT2_TEST_CASE_TPL( quad_2, NT2_REAL_TYPES )
{
  using nt2::quad;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
   tab_t x = nt2::_(T(0), T(5), T(5));
  NT2_DISPLAY(x);

  nt2::tic();
  BOOST_AUTO_TPL(res, quad<T>(nt2::functor<nt2::tag::exp_>(), T(0), T(5)
                              ,
                              options [ nt2::iterations_ = 100,
                                        nt2::tolerance::absolute_ = T(1.0e-6)
                                ]));
  nt2::toc();
  std::cout << "Integrals: " << res.integrals << " with " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";

  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, expm1(x))), nt2::Sqrteps<T>());

}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CLTV_PROBLEM_HPP_
#define CLTV_PROBLEM_HPP_

#include <Eigen/Dense>
#include <cstdint>

struct CltvOcpDi
{
  // Must be defined
  constexpr static double T = 1.;
  constexpr static std::size_t nx = 2;
  constexpr static std::size_t nu = 1;

  // Custom types
  using state_t = Eigen::Matrix<double, nx, 1>;
  using input_t = Eigen::Matrix<double, nu, 1>;
  using A_t = Eigen::Matrix<double, nx, nx>;
  using B_t = Eigen::Matrix<double, nx, nu>;
  using Q_t = Eigen::Matrix<double, nx, nx>;
  using R_t = Eigen::Matrix<double, nu, nu>;

  // Constants
  const A_t A = (A_t() << 0., 1., 0., 0.).finished();
  const B_t B = (B_t() << 0., 1.).finished();
  const state_t E = (state_t() << 0., 0.).finished();
  const Q_t Q = (Q_t() << 1000., 0., 0., 10.).finished();
  const Q_t QT = (Q_t() << 100., 0., 0., 100.).finished();
  const R_t R = (R_t() << 0.0001).finished();

  void get_x0(Eigen::Ref<state_t> x0) const
  {
    x0 << 1., 0.;
  }

  void get_T(double & t) const
  {
    t = T;
  }

  void get_state_lb(double, Eigen::Ref<state_t> state_lb) const
  {
    state_lb <<
      -1000.,
      -1.;
  }

  void get_state_ub(double, Eigen::Ref<state_t> state_ub) const
  {
    state_ub <<
      1000.,
      1.;
  }

  void get_input_lb(double, Eigen::Ref<input_t> input_lb) const
  {
    input_lb << -1.;
  }

  void get_input_ub(double, Eigen::Ref<input_t> input_ub) const
  {
    input_ub << 1.;
  }

  const A_t & get_A(double) const
  {
    return A;
  }

  const B_t & get_B(double) const
  {
    return B;
  }

  const state_t & get_E(double) const
  {
    return E;
  }

  const Q_t & get_Q(double) const
  {
    return Q;
  }

  const R_t & get_R(double) const
  {
    return R;
  }

  const Q_t & get_QT() const
  {
    return QT;
  }

  state_t get_xldot(double) const {return state_t::Zero();}
};

#endif  // CLTV_PROBLEM_HPP_


#ifndef newton_raphson_hpp
#define newton_raphson_hpp 3141592

#include <iostream>
#include <Eigen/SparseCholesky>

template<typename U, typename V>
void newton_raphson(U& F,V& J,VectorXd& x0,double tol)
{
  int it = 0;
  int maxit = 1000;
  double res = tol+1;
  VectorXd x1,step;
  SparseLU<SpMat> solver;
  while(res > tol && it <maxit)
  {
    solver.analyzePattern(J(x0));
    solver.factorize(J(x0));
    x1 = x0- solver.solve(F(x0));
    res = (x1-x0).squaredNorm();
    x0 = x1;
    std::cout << "Residu: "<< res << std::endl;
    it += 1;
  }
}

template<typename U, typename V>
void quasi_newton_raphson(U& F,V& J,VectorXd& x0,double tol)
{
  int it = 0;
  int maxit = 1000;
  double res = tol+1;
  VectorXd x1,step;
  SparseLU<SpMat> solver;
  solver.analyzePattern(J(x0));
  solver.factorize(J(x0));
  while(res > tol && it <maxit)
  {
    x1 = x0- solver.solve(F(x0));
    res = (x1-x0).squaredNorm();
    x0 = x1;
    std::cout << "Residu: "<< res << std::endl;
    it += 1;
  }
}

#endif


#ifndef THREEDIMUTIL_MESH_HPP
#define THREEDIMUTIL_MESH_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace threedimutil
{
    template <typename Scalar, typename UInt>
    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> CalcVertexNormals(const Eigen::Matrix<Scalar, 3, Eigen::Dynamic>& vertex_data,
                                                               const Eigen::Matrix<UInt, 3, Eigen::Dynamic>&   index_data)
    {
        Eigen::Matrix<Scalar, 3, Eigen::Dynamic> normal_data = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>::Zero(vertex_data.rows(), vertex_data.cols());

        for (unsigned face = 0; face < index_data.cols(); ++face)
        {
            const auto& x_0 = vertex_data.col(index_data(0, face));
            const auto& x_1 = vertex_data.col(index_data(1, face));
            const auto& x_2 = vertex_data.col(index_data(2, face));

            const auto area_scaled_face_normal = (x_1 - x_0).cross(x_2 - x_0);

            normal_data.col(index_data(0, face)) += area_scaled_face_normal;
            normal_data.col(index_data(1, face)) += area_scaled_face_normal;
            normal_data.col(index_data(2, face)) += area_scaled_face_normal;
        }

        for (unsigned vertex = 0; vertex < vertex_data.cols(); ++vertex)
        {
            normal_data.col(vertex).normalize();
        }

        return normal_data;
    }
}

#endif // THREEDIMUTIL_MESH_HPP


#ifndef FARHADIFARDIFFERENTIALFORCE_HPP_
#define FARHADIFARDIFFERENTIALFORCE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include "Exception.hpp"

#include "AbstractForce.hpp"
#include "VertexBasedCellPopulation.hpp"
#include "CellLabel.hpp"

#include <iostream>

/**
 * A force class for use in Vertex-based simulations. This force is based on the
 * Energy function proposed by Farhadifar et al in  Curr. Biol., 2007, 17, 2095-2104.
 */

template<unsigned DIM>
class FarhadifarDifferentialForce : public AbstractForce<DIM>
{
friend class TestForces;

private:

    friend class boost::serialization::access;
    /**
     * Boost Serialization method for archiving/checkpointing.
     * Archives the object and its member variables.
     *
     * @param archive  The boost archive.
     * @param version  The current version of this class.
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractForce<DIM> >(*this);
        archive & mCellAreaElasticityParameter;
        archive & mLabelledCellAreaElasticityParameter;
        archive & mCellPerimeterContractilityParameter;
        archive & mLabelledCellPerimeterContractilityParameter;
        archive & mCellCellLineTensionParameter;
        archive & mLabelledCellLabelledCellLineTensionParameter;
        archive & mLabelledCellCellLineTensionParameter;
        archive & mCellBoundaryLineTensionParameter;
        archive & mLabelledCellBoundaryLineTensionParameter;
    }

protected:

    /**
     * The strength of the area term in the model. Corresponds to K_alpha in
     * Farhadifar's paper.
     */
    double mCellAreaElasticityParameter;

    /**
     * The strength of the area term in the model for a labelled cell.
     * */
    double mLabelledCellAreaElasticityParameter;

    /**
     * The strength of the perimeter term in the model. Corresponds to
     * Gamma_alpha in Farhadifar's paper.
     */
    double mCellPerimeterContractilityParameter;

    /**
     * The strength of the perimeter term in the model for a labelled cell.
     */
    double mLabelledCellPerimeterContractilityParameter;

    /**
     * The strength of the cell-cell line tension between two cells.
     * Lambda_{i,j} in Farhadifar's paper.
     */
    double mCellCellLineTensionParameter;

    /**
     * The strength of the cell-cell line tension between two labelled cells.
     */
    double mLabelledCellLabelledCellLineTensionParameter;

    /**
     * The strength of the cell-cell line tension between a labelled and
     * non-labelled cell.
     */
    double mLabelledCellCellLineTensionParameter;

    /**
     * The strength of the line tension at the boundary. This term does
     * correspond to Lambda_{i,j} in Farhadifar's paper.
     */
    double mCellBoundaryLineTensionParameter;

    /**
     * The strength of the line tension at the boundary for labelled cells.
     */
    double mLabelledCellBoundaryLineTensionParameter;

public:

    /**
     * Constructor.
     */
    FarhadifarDifferentialForce();

    /**
     * Destructor.
     */
    virtual ~FarhadifarDifferentialForce() override;

    /**
     * Overridden AddForceContribution() method.
     *
     * Calculates the force on each node in the vertex-based cell population based on the energy function
     * Farhadifar's model.
     *
     * @param rCellPopulation reference to the cell population
     */
    virtual void AddForceContribution(AbstractCellPopulation<DIM>& rCellPopulation) override;

    /**
     * Get the line tension parameter for the edge between two given nodes.
     *
     * @param pNodeA one node
     * @param pNodeB the other node
     * @param rVertexCellPopulation reference to the cell population
     *
     * @return the line tension parameter for this edge.
     */
    double GetLineTensionParameter(Node<DIM>* pNodeA, Node<DIM>* pNodeB, VertexBasedCellPopulation<DIM>& rVertexCellPopulation);

    /**
     * @return mCellAreaElasticityParameter
     */
    double GetCellAreaElasticityParameter();

    /**
     * @return mLabelledCellAreaElasticityParameter
     */
    double GetLabelledCellAreaElasticityParameter();

    /**
     * @return mCellPerimeterContractilityParameter
     */
    double GetCellPerimeterContractilityParameter();

    /**
     * @return mLabelledCellPerimeterContractilityParameter
     */
    double GetLabelledCellPerimeterContractilityParameter();

    /**
     * @return mCellCellLineTensionParameter
     */
    double GetCellCellLineTensionParameter();

    /**
     * @return mLabelledCellLabelledCellLineTensionParameter
     */
    double GetLabelledCellLabelledCellLineTensionParameter();

    /**
     * @return mLabelledCellCellLineTensionParameter
     */
    double GetLabelledCellCellLineTensionParameter();

    /**
     * @return mCellBoundaryLineTensionParameter
     */
    double GetCellBoundaryLineTensionParameter();

    /**
     * @return mLabelledCellBoundaryLineTensionParameter
     */
    double GetLabelledCellBoundaryLineTensionParameter();

    /**
     * Set mCellAreaElasticityParameter.
     *
     * @param cellAreaElasticityParameter the new value of
     * mCellAreaElasticityParameter
     */
    void SetCellAreaElasticityParameter(double cellAreaElasticityParameter);

    /**
     * Set mLabelledCellAreaElasticityParameter.
     *
     * @param labelledCellAreaElasticityParameter the new value of
     * mLabelledCellAreaElasticityParameter
     */
    void SetLabelledCellAreaElasticityParameter(double labelledCellAreaElasticityParameter);

    /**
     * Set mCellPerimeterContractilityParameter.
     *
     * @param cellPerimeterContractilityParameter the new value of
     * cellPerimeterContractilityParameter
     */
    void SetCellPerimeterContractilityParameter(double cellPerimeterContractilityParameter);

    /**
     * Set mLabelledCellPerimeterContractilityParameter.
     *
     * @param labelledCellPerimeterContractilityParameter the new value of
     * labelledCellPerimeterContractilityParameter
     */
    void SetLabelledCellPerimeterContractilityParameter(
            double labelledCellPerimeterContractilityParameter);

    /**
     * Set mCellCellLineTensionParameter.
     *
     * @param cellLineTensionParameter the new value of mCellCellLineTensionParameter
     */
    void SetCellCellLineTensionParameter(double cellCellLineTensionParameter);

    /**
     * Set mLabelledCellLabelledCellLineTensionParameter.
     *
     * @param labelledCellLabelledCellLineTensionParameter the new value of
     * mLabelledCellLabelledCellLineTensionParameter
     */
    void SetLabelledCellLabelledCellLineTensionParameter(
            double labelledCellLabelledCellLineTensionParameter);

    /**
     * Set mLabelledCellCellLineTensionParameter.
     *
     * @param labelledCellCellLineTensionParameter the new value of
     * mLabelledCellCellLineTensionParameter
     */
    void SetLabelledCellCellLineTensionParameter(
            double labelledCellCellLineTensionParameter);

    /**
     * Set mCellBoundaryLineTensionParameter.
     *
     * @param cellBoundaryLineTensionParameter the new value of mCellBoundaryLineTensionParameter
     */
    void SetCellBoundaryLineTensionParameter(double cellBoundaryLineTensionParameter);

    /**
     * Set mLabelledCellBoundaryLineTensionParameter.
     *
     * @param labelledCellBoundaryLineTensionParameter the new value of
     * mLabelledCellBoundaryLineTensionParameter
     */
    void SetLabelledCellBoundaryLineTensionParameter(
            double labelledCellBoundaryLineTensionParameter);

    /**
     * Overridden OutputForceParameters() method.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    void OutputForceParameters(out_stream& rParamsFile);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(FarhadifarDifferentialForce)

#endif /*FARHADIFARDIFFERENTIALFORCE_HPP_*/


#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include <srook/math/matrix.hpp>

BOOST_AUTO_TEST_SUITE(srook_math_matrix_test)

namespace sm = srook::math;
namespace stv = srook::tmpl::vt;

BOOST_AUTO_TEST_CASE(matrix_construct)
{
    typedef sm::row<int, int> r2;
    typedef stv::transfer_t<sm::matrix, stv::replicate_t<2, stv::transfer_t<sm::row, stv::replicate_t<2, int>>>> mtx2x2_int; // 2 x 2, int matrix type

    constexpr mtx2x2_int m1 {
       r2{ 1, 3 },
       r2{ 4, 2 }
    };
    SROOK_ATTRIBUTE_UNUSED constexpr auto m2 = m1;

    constexpr mtx2x2_int m3 { // initialized as like the above.
        1, 3,
        4, 2
    };
    SROOK_ST_ASSERT(m1 == m2 && m1 == m3);
}

/*

int main()
{
    constexpr srook::math::matrix<
        srook::math::row<int, int, int, int>,
        srook::math::row<int, int, int, int>,
        srook::math::row<int, int, int, int>,
        srook::math::row<int, int, int, int>
    > m1 {
        1, 2, 7, 6,
        2, 4, 4, 2,
        1, 8, 5, 2,
        2, 4, 3, 3
    };

    constexpr auto res = m1.compute_equations(srook::math::make_vector(6, 2, 12, 5), srook::math::gaussian_elimination_eq());
    std::cout << res << std::endl;

    constexpr srook::math::matrix<
        srook::math::row<int, int, int>,
        srook::math::row<int, int, int>,
        srook::math::row<int, int, int>
    > m2 {
        2, 2, 1,
        3, 0, 2,
        4, 3, 2
    };
    constexpr auto inv = m2.inverse();
    std::cout << inv << std::endl;

    constexpr auto det = m2.determinant();
    std::cout << det << std::endl;
}*/

BOOST_AUTO_TEST_SUITE_END()


/*!
     \brief Complex a template implementation for complex arithmetics
        Copyright (C) 2012 Sebastian Schlenkrich
        Version 1.0
*/

#ifndef Cpx_Complex_hpp
#define Cpx_Complex_hpp

//#define _USE_MATH_DEFINES // for Visual Studio

#include <iostream>
#include <iomanip>

#include <map>
#include <vector>
#include <iterator>
//#include <cmath>
#include <complex>

//#define DEBUG              // low-level io debugging
//#define EXECUTABLE               // compile example and main function
#define MS_VISUAL_STUDIO     // math functions not in global scope

#ifdef MS_VISUAL_STUDIO
    #include <boost/math/special_functions/erf.hpp>
#endif

namespace Cpx {

#ifdef DEBUG
    #define MSG(msg_stream) std::cerr << msg_stream << std::endl ;
#else
    #define MSG(msg_stream)
#endif

#ifdef MS_VISUAL_STUDIO
    //! Some compilers don't find intrinsic double functions
    inline double exp(double x) { return std::exp(x); }
    inline double log(double x) { return std::log(x); }
    inline double sqrt(double x) { return std::sqrt(x); }
    inline double sin(double x) { return std::sin(x); }
    inline double cos(double x) { return std::cos(x); }
    inline double tan(double x) { return std::tan(x); }
    inline double atan2(double y, double x) { return std::atan2(y,x); }
    inline double erf(double x)     { return boost::math::erf(x); }
    inline double erf_inv(double x) { return boost::math::erf_inv(x); }
#endif

    //! Variable defines the class template for derivative evaluations
    template <class Type = double>
    class Complex {
    protected:
        Type real_;
        Type imag_;
    public:
        //! "standard" constructor
        Complex( const Type& real, const Type& imag) : real_(real), imag_(imag) {}
        //! Type conversion constructor
        Complex( const Type& real) : real_(real), imag_(0) {}
        //! no standard constructor and default copy constructor
        //! inspectors
        const Type& real()  const { return real_;  }
        const Type& imag() const { return imag_; }
        //! IO steaming
        //! Apply IO to the real and imaginary component
        inline friend std::ostream& operator << (std::ostream &output, const Complex<Type> &x) {
            return output << "(" << x.real_ << "," << x.imag_ << ")";
        }
        //! Intrinsic functions
        inline friend Complex<Type> exp(const Complex<Type> &x) {
            Type r = exp(x.real_);
            return Complex<Type>( r*cos(x.imag_), r*sin(x.imag_) );
        }
        inline friend Complex<Type> log(const Complex<Type> &x) {
            return Complex<Type>( 0.5*log(x.real_*x.real_ + x.imag_*x.imag_), atan2(x.imag_,x.real_) );
        }
        inline friend Complex<Type> sqrt(const Complex<Type> &x) {
            if (x.imag_==0) {
                if (x.real_<0) return Complex<Type>( 0, sqrt(-x.real_) );
                else           return Complex<Type>( sqrt(x.real_), 0 );
            }
            if (x.real_==0) {
                if (x.imag_<0) {
                    Type tmp  = sqrt(-x.imag_)*M_SQRT1_2;
                    return Complex<Type>( tmp, -tmp );
                } else {
                    Type tmp  = sqrt(x.imag_)*M_SQRT1_2;
                    return Complex<Type>( tmp, tmp );
                }
            }
            Type sqr_r  = sqrt(sqrt(x.real_*x.real_ + x.imag_*x.imag_));
            Type phi_2  = atan2(x.imag_,x.real_) / 2;
            return Complex<Type>( sqr_r*cos(phi_2), sqr_r*sin(phi_2) );
        }

        //! Unary operators
        inline friend Complex<Type> operator + (const Complex<Type> &x ) {
            return x;
        }
        inline friend Complex<Type> operator - (const Complex<Type> &x ) {
            return Complex<Type>( -x.real_, -x.imag_ );
        }
        //! Binary operators
        //! Complex x Complex
        inline friend Complex<Type> operator + (const Complex<Type> &x, const Complex<Type> &y ) {
            return Complex<Type>( x.real_ + y.real_, x.imag_ + y.imag_ );
        }
        inline friend Complex<Type> operator - (const Complex<Type> &x, const Complex<Type> &y ) {
            return Complex<Type>( x.real_ - y.real_, x.imag_ - y.imag_ );
        }
        inline friend Complex<Type> operator * (const Complex<Type> &x, const Complex<Type> &y ) {
            return Complex<Type>( x.real_*y.real_ - x.imag_*y.imag_, x.real_*y.imag_ + x.imag_*y.real_ );
        }
        inline friend Complex<Type> operator / (const Complex<Type> &x, const Complex<Type> &y ) {
            Type den = y.real_*y.real_ + y.imag_*y.imag_;
            return Complex<Type>( (x.real_*y.real_ + x.imag_*y.imag_)/den,
                          (x.imag_*y.real_ - x.real_*y.imag_)/den );
        }
        //! Complex x Type
        inline friend Complex<Type> operator + (const Complex<Type> &x, const Type &y ) {
            return Complex<Type>( x.real_ + y, x.imag_ );
        }
        inline friend Complex<Type> operator - (const Complex<Type> &x, const Type &y ) {
            return Complex<Type>( x.real_ - y, x.imag_ );
        }
        inline friend Complex<Type> operator * (const Complex<Type> &x, const Type &y ) {
            return Complex<Type>( x.real_*y, x.imag_*y );
        }
        inline friend Complex<Type> operator / (const Complex<Type> &x, const Type &y ) {
            return Complex<Type>( x.real_/y, x.imag_/y );
        }
        //! Type x Complex
        inline friend Complex<Type> operator + (const Type &x, const Complex<Type> &y ) {
            return Complex<Type>( x + y.real_, y.imag_ );
        }
        inline friend Complex<Type> operator - (const Type &x, const Complex<Type> &y ) {
            return Complex<Type>( x - y.real_, -y.imag_ );
        }
        inline friend Complex<Type> operator * (const Type &x, const Complex<Type> &y ) {
            return Complex<Type>( x*y.real_, x*y.imag_ );
        }
        inline friend Complex<Type> operator / (const Type &x, const Complex<Type> &y ) {
            Type den = y.real_*y.real_ + y.imag_*y.imag_;
            return Complex<Type>( x*y.real_/den, -x*y.imag_/den );
        }
    };

    //! Type x Complex
    template <class Type>
    inline Complex<Type> operator + (const double &x, const Complex<Type> &y ) {
        return Complex<Type>( x + y.real(), y.imag() );
    }
    template <class Type>
    inline Complex<Type> operator - (const double &x, const Complex<Type> &y ) {
        return Complex<Type>( x - y.real(), -y.imag() );
    }
    template <class Type>
    inline Complex<Type> operator * (const double &x, const Complex<Type> &y ) {
        return Complex<Type>( x*y.real(), x*y.imag() );
    }
    template <class Type>
    inline Complex<Type> operator / (const double &x, const Complex<Type> &y ) {
        Type den = y.real()*y.real() + y.imag()*y.imag();
        return Complex<Type>( x*y.real()/den, -x*y.imag()/den );
    }

}   // namespace Cpx

#endif   // Cpx_Complex_hpp

#ifdef EXECUTABLE

int main() {
    typedef Cpx::Complex<double> complex;
    //typedef std::complex<double> complex;

    std::cout << "Cpx Complex (C) Sebastian Schlenktich (2012)"<< std::endl;
    std::cout << std::endl;

    complex a(0,4), b(1,2);
    complex c(0); // = a/b;
    c = a + complex(5.0);
    std::cout << "c = " << (c = sqrt(a)) << std::endl;
    double z;
    std::cout << (z = atan2(-1,-1)) << std::endl;

    return 0;
}

#endif   // EXECUTABLE

/*  Example output

*/


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing sqrt capabilities

    Computes the square root of its parameter. For integers it is the
    truncation of the real square root.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = sqrt(x);
    @endcode

    @par Decorators

    - std_ calls std::sqrt

    - raw_ for floating entries can gain some speed with less accuracy on some architectures.

  **/
  Value sqrt(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/sqrt.hpp>
#include <boost/simd/function/simd/sqrt.hpp>

#endif


#include <boost/test/unit_test.hpp>

#include <ayla/geometry/triangle.hpp>
#include <ayla/geometry/vector.hpp>

BOOST_AUTO_TEST_SUITE(ayla)
BOOST_AUTO_TEST_SUITE(triangle)

BOOST_AUTO_TEST_CASE( basicTest ) {
	Triangle t0(glm::vec3(-11, 0, 0), glm::vec3(0, 11, 0), glm::vec3(22,-11, 0));

	BOOST_CHECK(glm::epsilonEqual(t0.calculatePerimeter(), 81.4541f, 0.001f));
	BOOST_CHECK(glm::epsilonEqual(t0.calculateArea(), 242.0f, 0.001f));

	BOOST_CHECK(epsilonEqual(t0.getNormal(), -glm::vec3(0,0,1), 0.0000001f));
}

BOOST_AUTO_TEST_CASE( barymetric_coordinates_coplanar ) {
	glm::vec3 A(-3, -4, 0);
	glm::vec3 B(0, 5, 0);
	glm::vec3 C(3, -4, 0);
	Triangle t0(A,B,C);

	glm::vec3 P(0.0f,0.0f,0.0f);
	Float a, b, c;
	t0.getBarycentricCoordinates(P, a, b, c);
	glm::vec3 result = a*A + b*B + c*C;

	BOOST_CHECK(isZero(result.x));
	BOOST_CHECK(isZero(result.y));
	BOOST_CHECK(isZero(result.z));

	glm::vec3 halfEdge = (B-A);
	halfEdge /= 2;
	P = A + halfEdge;
	t0.getBarycentricCoordinates(P, a, b, c);

	BOOST_CHECK_CLOSE( a, 1/2.0f, 0.00001f );
	BOOST_CHECK_CLOSE( b, 1/2.0f, 0.00001f );
	BOOST_CHECK_CLOSE( c, 0.0f, 0.00001f );
}

BOOST_AUTO_TEST_CASE( closest_point_coplanar ) {
	glm::vec3 A(-3, -4, 0);
	glm::vec3 B(0, 5, 0);
	glm::vec3 C(3, -4, 0);
	Triangle t0(A,B,C);
	Float a, b, c;

	//Point is inside the triangle
	glm::vec3 P(0,0,0);
	glm::vec3 closest = t0.getClosestPoint(P, a, b, c);

	glm::vec3 result = A*a + B*b + C*c;

	BOOST_CHECK(isZero(closest.x));
	BOOST_CHECK(isZero(closest.y));
	BOOST_CHECK(isZero(closest.z));
	BOOST_CHECK(isZero(result.x));
	BOOST_CHECK(isZero(result.y));
	BOOST_CHECK(isZero(result.z));

	//Point outside the triangle, closest to one vertex
	P = glm::vec3( 0, 10, 0 );
	closest = t0.getClosestPoint(P, a, b, c);
	result = A*a + B*b + C*c;

	//Result and Closest should be equal to B
	BOOST_CHECK_CLOSE( closest.x, B.x, 0.0001f );
	BOOST_CHECK_CLOSE( closest.y, B.y, 0.0001f );
	BOOST_CHECK_CLOSE( closest.z, B.z, 0.0001f );
	BOOST_CHECK_CLOSE( result.x, B.x, 0.0001f );
	BOOST_CHECK_CLOSE( result.y, B.y, 0.0001f );
	BOOST_CHECK_CLOSE( result.z, B.z, 0.0001f );

	//Point outside the triangle, colinear with the middle of the edge AB and the point (0,0,0).
	//The closes point should be the half of the edge.
	glm::vec3 halfEdge = (B-A);
	halfEdge /= 2.0f;
	halfEdge = A + halfEdge;
	P = halfEdge + halfEdge;
	closest = t0.getClosestPoint(P, a, b, c);
	result = A*a + B*b + C*c;

	//Result and Closest should be equal to halfEdge
	BOOST_CHECK_CLOSE( closest.x, halfEdge.x, 0.0001f );
	BOOST_CHECK_CLOSE( closest.y, halfEdge.y, 0.0001f );
	BOOST_CHECK_CLOSE( closest.z, halfEdge.z, 0.0001f );
	BOOST_CHECK_CLOSE( result.x, halfEdge.x, 0.0001f );
	BOOST_CHECK_CLOSE( result.y, halfEdge.y, 0.0001f );
	BOOST_CHECK_CLOSE( result.z, halfEdge.z, 0.0001f );
}

BOOST_AUTO_TEST_CASE( closest_point_not_coplanar ) {
	glm::vec3 A(-3, -4, 0);
	glm::vec3 B(0, 5, 0);
	glm::vec3 C(3, -4, 0);
	Triangle t0(A,B,C);
	Float a, b, c;

	//Point is "over" the triangle
	//Should be mapped to (0,0,0)
	glm::vec3 P(0,0,20);
	glm::vec3 closest = t0.getClosestPoint(P, a, b, c);

	glm::vec3 result = A*a + B*b + C*c;

	BOOST_CHECK(isZero(closest.x));
	BOOST_CHECK(isZero(closest.y));
	BOOST_CHECK(isZero(closest.z));
	BOOST_CHECK(isZero(result.x));
	BOOST_CHECK(isZero(result.y));
	BOOST_CHECK(isZero(result.z));

	//Point outside the triangle, closest to one vertex
	P = glm::vec3( 0, 10, 7 );
	closest = t0.getClosestPoint(P, a, b, c);
	result = A*a + B*b + C*c;

	//Result and Closest should be equal to B
	BOOST_CHECK_CLOSE( closest.x, B.x, 0.0001f );
	BOOST_CHECK_CLOSE( closest.y, B.y, 0.0001f );
	BOOST_CHECK_CLOSE( closest.z, B.z, 0.0001f );
	BOOST_CHECK_CLOSE( result.x, B.x, 0.0001f );
	BOOST_CHECK_CLOSE( result.y, B.y, 0.0001f );
	BOOST_CHECK_CLOSE( result.z, B.z, 0.0001f );

	//Point outside the triangle, colinear with the middle of the edge AB and the point (0,0,0).
	//The closes point should be the half of the edge.
	glm::vec3 halfEdge = (B-A);
	halfEdge /= 2.0f;
	halfEdge = A + halfEdge;
	P = halfEdge + halfEdge + glm::vec3(0,0,5);
	closest = t0.getClosestPoint(P, a, b, c);
	result = A*a + B*b + C*c;

	//Result and Closest should be equal to halfEdge
	BOOST_CHECK_CLOSE( closest.x, halfEdge.x, 0.0001f );
	BOOST_CHECK_CLOSE( closest.y, halfEdge.y, 0.0001f );
	BOOST_CHECK_CLOSE( closest.z, halfEdge.z, 0.0001f );
	BOOST_CHECK_CLOSE( result.x, halfEdge.x, 0.0001f );
	BOOST_CHECK_CLOSE( result.y, halfEdge.y, 0.0001f );
	BOOST_CHECK_CLOSE( result.z, halfEdge.z, 0.0001f );
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()


#include <gtest/gtest.h>
#include "pitts_parallel.hpp"
#include "pitts_tensortrain_from_dense.hpp"
#include "pitts_tensortrain_dot.hpp"
#include "pitts_tensortrain_norm.hpp"
#include "pitts_multivector.hpp"
#include "pitts_multivector_random.hpp"
#include "pitts_multivector_eigen_adaptor.hpp"
#include <Eigen/Dense>

namespace
{
  auto toMultiVector(const double* begin, const double* end, const std::vector<int>& dimensions)
  {
    int size = 1;
    for(auto d: dimensions)
      size *= d;
    assert(end - begin == size);
    PITTS::MultiVector<double> result(size / dimensions.back(), dimensions.back());
    for(int j = 0; j < result.cols(); j++)
      for(int i = 0; i < result.rows(); i++)
        result(i,j) = *(begin++);
    return result;
  }
}

TEST(PITTS_TensorTrain_fromDense, scalar)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  const std::array<double,1> scalar = {5};
  const std::vector<int> dimensions = {1};

  PITTS::MultiVector<double> work, data = toMultiVector(begin(scalar), end(scalar), dimensions);
  TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

  ASSERT_EQ(TT.dimensions(), dimensions);
  ASSERT_NEAR(5., TT.subTensors()[0](0,0,0), eps);
}

TEST(PITTS_TensorTrain_fromDense, vector_1d)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  const std::array<double,7> scalar = {1,2,3,4,5,6,7};
  const std::vector<int> dimensions = {7};

  PITTS::MultiVector<double> work, data = toMultiVector(begin(scalar), end(scalar), dimensions);
  TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

  ASSERT_EQ(TT.dimensions(), dimensions);
  ASSERT_EQ(1, TT.subTensors()[0].r1());
  ASSERT_EQ(7, TT.subTensors()[0].n());
  ASSERT_EQ(1, TT.subTensors()[0].r2());
  ASSERT_NEAR(1., TT.subTensors()[0](0,0,0), eps);
  ASSERT_NEAR(2., TT.subTensors()[0](0,1,0), eps);
  ASSERT_NEAR(3., TT.subTensors()[0](0,2,0), eps);
  ASSERT_NEAR(4., TT.subTensors()[0](0,3,0), eps);
  ASSERT_NEAR(5., TT.subTensors()[0](0,4,0), eps);
  ASSERT_NEAR(6., TT.subTensors()[0](0,5,0), eps);
  ASSERT_NEAR(7., TT.subTensors()[0](0,6,0), eps);
}

TEST(PITTS_TensorTrain_fromDense, matrix_2d_1x1)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  const std::array<double,1> M = {7.};
  const std::vector<int> dimensions = {1,1};

  PITTS::MultiVector<double> work, data = toMultiVector(begin(M), end(M), dimensions);
  TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

  ASSERT_EQ(TT.dimensions(), dimensions);
  ASSERT_EQ(1, TT.subTensors()[0].r1());
  ASSERT_EQ(1, TT.subTensors()[0].n());
  ASSERT_EQ(1, TT.subTensors()[0].r2());
  ASSERT_EQ(1, TT.subTensors()[1].r1());
  ASSERT_EQ(1, TT.subTensors()[1].n());
  ASSERT_EQ(1, TT.subTensors()[1].r2());
  if( TT.subTensors()[0](0,0,0) > 0 )
  {
    ASSERT_NEAR(7., TT.subTensors()[0](0,0,0), eps);
    ASSERT_NEAR(1., TT.subTensors()[1](0,0,0), eps);
  }
  else
  {
    // sign flip is ok
    ASSERT_NEAR(-7., TT.subTensors()[0](0,0,0), eps);
    ASSERT_NEAR(-1., TT.subTensors()[1](0,0,0), eps);
  }
}

TEST(PITTS_TensorTrain_fromDense, matrix_2d_1x5)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  const std::array<double,5> M = {1., 2., 3., 4., 5.};
  const std::vector<int> dimensions = {1,5};

  PITTS::MultiVector<double> work, data = toMultiVector(begin(M), end(M), dimensions);
  TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

  ASSERT_EQ(TT.dimensions(), dimensions);
  ASSERT_EQ(1, TT.subTensors()[0].r1());
  ASSERT_EQ(1, TT.subTensors()[0].n());
  ASSERT_EQ(1, TT.subTensors()[0].r2());
  ASSERT_EQ(1, TT.subTensors()[1].r1());
  ASSERT_EQ(5, TT.subTensors()[1].n());
  ASSERT_EQ(1, TT.subTensors()[1].r2());
  ASSERT_NEAR(1., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(2., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,1,0), eps);
  ASSERT_NEAR(3., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,2,0), eps);
  ASSERT_NEAR(4., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,3,0), eps);
  ASSERT_NEAR(5., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,4,0), eps);
}

TEST(PITTS_TensorTrain_fromDense, matrix_2d_5x1)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  const std::array<double,5> M = {1., 2., 3., 4., 5.};
  const std::vector<int> dimensions = {5,1};

  PITTS::MultiVector<double> work, data = toMultiVector(begin(M), end(M), dimensions);
  TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

  ASSERT_EQ(TT.dimensions(), dimensions);
  ASSERT_EQ(1, TT.subTensors()[0].r1());
  ASSERT_EQ(5, TT.subTensors()[0].n());
  ASSERT_EQ(1, TT.subTensors()[0].r2());
  ASSERT_EQ(1, TT.subTensors()[1].r1());
  ASSERT_EQ(1, TT.subTensors()[1].n());
  ASSERT_EQ(1, TT.subTensors()[1].r2());
  ASSERT_NEAR(1., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(2., TT.subTensors()[0](0,1,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(3., TT.subTensors()[0](0,2,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(4., TT.subTensors()[0](0,3,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(5., TT.subTensors()[0](0,4,0)*TT.subTensors()[1](0,0,0), eps);
}

TEST(PITTS_TensorTrain_fromDense, matrix_2d_5x2_rank1)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  const std::array<double,10> M = {1., 2., 3., 4., 5., 2., 4., 6., 8., 10.};
  const std::vector<int> dimensions = {5,2};

  PITTS::MultiVector<double> work, data = toMultiVector(begin(M), end(M), dimensions);
  TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

  ASSERT_EQ(TT.dimensions(), dimensions);
  ASSERT_EQ(1, TT.subTensors()[0].r1());
  ASSERT_EQ(5, TT.subTensors()[0].n());
  ASSERT_EQ(1, TT.subTensors()[0].r2());
  ASSERT_EQ(1, TT.subTensors()[1].r1());
  ASSERT_EQ(2, TT.subTensors()[1].n());
  ASSERT_EQ(1, TT.subTensors()[1].r2());
  ASSERT_NEAR(1., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(2., TT.subTensors()[0](0,1,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(3., TT.subTensors()[0](0,2,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(4., TT.subTensors()[0](0,3,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(5., TT.subTensors()[0](0,4,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(2., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,1,0), eps);
  ASSERT_NEAR(4., TT.subTensors()[0](0,1,0)*TT.subTensors()[1](0,1,0), eps);
  ASSERT_NEAR(6., TT.subTensors()[0](0,2,0)*TT.subTensors()[1](0,1,0), eps);
  ASSERT_NEAR(8., TT.subTensors()[0](0,3,0)*TT.subTensors()[1](0,1,0), eps);
  ASSERT_NEAR(10., TT.subTensors()[0](0,4,0)*TT.subTensors()[1](0,1,0), eps);
}

TEST(PITTS_TensorTrain_fromDense, matrix_2d_2x5_rank1)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  const std::array<double,10> M = {1., 2., 2., 4., 3., 6., 4., 8., 5., 10.};
  const std::vector<int> dimensions = {2,5};

  PITTS::MultiVector<double> work, data = toMultiVector(begin(M), end(M), dimensions);
  TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

  ASSERT_EQ(TT.dimensions(), dimensions);
  ASSERT_EQ(1, TT.subTensors()[0].r1());
  ASSERT_EQ(2, TT.subTensors()[0].n());
  ASSERT_EQ(1, TT.subTensors()[0].r2());
  ASSERT_EQ(1, TT.subTensors()[1].r1());
  ASSERT_EQ(5, TT.subTensors()[1].n());
  ASSERT_EQ(1, TT.subTensors()[1].r2());
  ASSERT_NEAR(1., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(2., TT.subTensors()[0](0,1,0)*TT.subTensors()[1](0,0,0), eps);
  ASSERT_NEAR(2., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,1,0), eps);
  ASSERT_NEAR(4., TT.subTensors()[0](0,1,0)*TT.subTensors()[1](0,1,0), eps);
  ASSERT_NEAR(3., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,2,0), eps);
  ASSERT_NEAR(6., TT.subTensors()[0](0,1,0)*TT.subTensors()[1](0,2,0), eps);
  ASSERT_NEAR(4., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,3,0), eps);
  ASSERT_NEAR(8., TT.subTensors()[0](0,1,0)*TT.subTensors()[1](0,3,0), eps);
  ASSERT_NEAR(5., TT.subTensors()[0](0,0,0)*TT.subTensors()[1](0,4,0), eps);
  ASSERT_NEAR(10., TT.subTensors()[0](0,1,0)*TT.subTensors()[1](0,4,0), eps);
}

TEST(PITTS_TensorTrain_fromDense, matrix_2d_4x5)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  std::array<double,4*5> M;
  const std::vector<int> dimensions = {4,5};
  for(int i = 0; i < 4; i++)
    for(int j = 0; j < 5; j++)
      M[i+j*4] = i + j*4;

  PITTS::MultiVector<double> work, data = toMultiVector(begin(M), end(M), dimensions);
  TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

  ASSERT_EQ(TT.dimensions(), dimensions);

  // check result with dot products
  TensorTrain_double testTT(dimensions);
  for(int i = 0; i < 4; i++)
    for(int j = 0; j < 5; j++)
    {
      testTT.setUnit({i,j});
      EXPECT_NEAR(i+j*4., dot(testTT, TT), eps);
    }
}

TEST(PITTS_TensorTrain_fromDense, tensor_3d_rank1)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  std::array<double,3*4*5> M = {};
  const std::vector<int> dimensions = {3,4,5};
  for(int i = 0; i < 3; i++)
    for(int j = 0; j < 4; j++)
      for(int k = 0; k < 5; k++)
        M[i+j*3+k*3*4] = 1.;

  PITTS::MultiVector<double> work, data = toMultiVector(begin(M), end(M), dimensions);
  TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

  ASSERT_EQ(TT.dimensions(), dimensions);
  std::vector<int> ones = {1,1};
  ASSERT_EQ(ones, TT.getTTranks());

  // check result with dot products
  TensorTrain_double testTT(dimensions);
  for(int i = 0; i < 3; i++)
    for(int j = 0; j < 4; j++)
      for(int k = 0; k < 5; k++)
      {
        testTT.setUnit({i,j,k});
        EXPECT_NEAR(1., dot(testTT, TT), eps);
      }
}

TEST(PITTS_TensorTrain_fromDense, tensor_3d_3x4x5)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  std::array<double,3*4*5> M;
  const std::vector<int> dimensions = {3,4,5};
  for(int i = 0; i < 3; i++)
    for(int j = 0; j < 4; j++)
      for(int k = 0; k < 5; k++)
        M[i+j*3+k*3*4] = i + j*10 + k*100;

  PITTS::MultiVector<double> work, data = toMultiVector(begin(M), end(M), dimensions);
  TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

  ASSERT_EQ(TT.dimensions(), dimensions);

  // check result with dot products
  TensorTrain_double testTT(dimensions);
  for(int i = 0; i < 3; i++)
    for(int j = 0; j < 4; j++)
      for(int k = 0; k < 5; k++)
      {
        testTT.setUnit({i,j,k});
        EXPECT_NEAR(i + j*10. + k*100., dot(testTT, TT), eps);
      }
}

TEST(PITTS_TensorTrain_fromDense, tensor_5d_2x3x4x2x3_unit)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  std::array<double,2*3*4*2*3> M = {};
  const std::vector<int> dimensions = {2,3,4,2,3};
  const std::vector<int> dir = {1,0,2,0,2};
  M[ dir[0] + 2*dir[1] + 2*3*dir[2] + 2*3*4*dir[3] + 2*3*4*2*dir[4] ] = 1.;

  PITTS::MultiVector<double> work, data = toMultiVector(begin(M), end(M), dimensions);
  TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

  ASSERT_EQ(TT.dimensions(), dimensions);
  std::vector<int> ones = {1,1,1,1};
  ASSERT_EQ(ones, TT.getTTranks());

  TensorTrain_double refTT(dimensions);
  refTT.setUnit(dir);

  EXPECT_NEAR(1., norm2(TT), eps);
  EXPECT_NEAR(1., norm2(refTT), eps);
  EXPECT_NEAR(1., dot(TT, refTT), eps);
}

TEST(PITTS_TensorTrain_fromDense, matrix_2d_4x5_maxRank)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  constexpr auto eps = 1.e-10;

  std::array<double,4*5> M;
  const std::vector<int> dimensions = {4,5};
  for(int i = 0; i < 4; i++)
    for(int j = 0; j < 5; j++)
      M[i+j*4] = (i == j ? 10.-i : 0.);

  {
    // full / exact
  PITTS::MultiVector<double> work, data = toMultiVector(begin(M), end(M), dimensions);
    TensorTrain_double TT = PITTS::fromDense(data, work, dimensions);

    ASSERT_EQ(TT.dimensions(), dimensions);

    // check result with dot products
    TensorTrain_double testTT(dimensions);
    for(int i = 0; i < 4; i++)
      for(int j = 0; j < 5; j++)
      {
        testTT.setUnit({i,j});
        EXPECT_NEAR((i == j ? 10.-i : 0.), dot(testTT, TT), eps);
      }
  }

  {
    // truncated
  PITTS::MultiVector<double> work, data = toMultiVector(begin(M), end(M), dimensions);
    TensorTrain_double TT = PITTS::fromDense(data, work, dimensions, 1.e-16, 3);

    ASSERT_EQ(TT.dimensions(), dimensions);

    // check result with dot products
    TensorTrain_double testTT(dimensions);
    for(int i = 0; i < 4; i++)
      for(int j = 0; j < 5; j++)
      {
        testTT.setUnit({i,j});
        if( i == j && i < 3 )
        {
          EXPECT_NEAR(10.-i, dot(testTT, TT), eps);
        }
        else
        {
          EXPECT_NEAR(0., dot(testTT, TT), eps);
        }
      }
  }

}

TEST(PITTS_TensorTrain_fromDense, tensor5d_random_maxRank)
{
  using TensorTrain_double = PITTS::TensorTrain<double>;
  using MultiVector_double = PITTS::MultiVector<double>;

  std::vector<int> shape = {2,3,4,2,3};
  MultiVector_double M(2*3*4*2, 3);
  randomize(M);

  MultiVector_double work;
  TensorTrain_double TT = PITTS::fromDense(M, work, shape, 1.e-16, 2);

  for(auto r: TT.getTTranks())
  {
    ASSERT_LE(r, 2);
  }
}

// anonymous namespace with helper functions
namespace
{
  // check that the distributed (MPI parallel) algorithm obtains the same result as the "serial" algorithm
  void check_mpiGlobal_result(const std::vector<int>& localShape)
  {
    using TensorTrain_double = PITTS::TensorTrain<double>;
    using MultiVector_double = PITTS::MultiVector<double>;
    constexpr auto eps = 1.e-8;

    ASSERT_GE(localShape.size(), 2);
    const auto nDim = localShape.size();

    const auto& [iProc,nProcs] = PITTS::internal::parallel::mpiProcInfo();

    std::vector<int> globalShape = localShape;
    globalShape[0] *= nProcs;
    const long long nLocal = std::accumulate(localShape.begin(), localShape.end(), 1, std::multiplies<long long>());
    const long long nGlobal = nLocal * nProcs;

    // generate random data and distribute it
    using mat = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
    mat globalData = mat::Random(nGlobal/globalShape.back(), globalShape.back());
    MPI_Bcast(globalData.data(), nGlobal, MPI_DOUBLE, 0, MPI_COMM_WORLD);

    // calculate the global solution on each process
    MultiVector_double Mglobal(nGlobal/globalShape.back(), globalShape.back());
    {
      auto mapMglobal = EigenMap(Mglobal);
      mapMglobal = globalData;
    }
    MultiVector_double work;
    const auto globalTT = PITTS::fromDense(Mglobal, work, globalShape, 1.e-12, 5, false);

    // calculate distributed solution
    MultiVector_double Mlocal(nLocal/localShape.back(), localShape.back());
    {
      auto mapMlocal = EigenMap(Mlocal);
      Eigen::Map<mat> mapGlobalData = Eigen::Map<mat>(globalData.data(), nProcs, nGlobal/nProcs);
      mat localData = mapGlobalData.row(iProc);
      Eigen::Map<mat> mapLocalData = Eigen::Map<mat>(localData.data(), nLocal/localShape.back(), localShape.back());
      mapMlocal = mapLocalData;
    }
    const auto distributedTT = PITTS::fromDense(Mlocal, work, localShape, 1.e-12, 5, true);

    // distributedTT and globalTT should be identical, only the first sub-tensor is distributed onto multiple processes...
    for(int iDim = 1; iDim < nDim; iDim++)
    {
      const auto& subT_ref = globalTT.subTensors()[iDim];
      const auto& subT = distributedTT.subTensors()[iDim];

      ASSERT_EQ(subT_ref.r1(), subT.r1());
      ASSERT_EQ(subT_ref.n(), subT.n());
      ASSERT_EQ(subT_ref.r2(), subT.r2());

      for(int i = 0; i < subT.r1(); i++)
        for(int j = 0; j < subT.n(); j++)
          for(int k = 0; k < subT.r2(); k++)
          {
            // only compare absolute values as the sign of singular vectors is not well defined
            EXPECT_NEAR(std::abs(subT_ref(i,j,k)), std::abs(subT(i,j,k)), eps);
          }
    }

    // first dimension is distributed
    {
      const auto& subT_ref = globalTT.subTensors()[0];
      const auto& subT = distributedTT.subTensors()[0];

      ASSERT_EQ(subT_ref.r1(), subT.r1());
      ASSERT_EQ(globalShape[0], subT_ref.n());
      ASSERT_EQ(localShape[0], subT.n());
      ASSERT_EQ(subT_ref.r2(), subT.r2());

      for(int i = 0; i < subT.r1(); i++)
        for(int k = 0; k < subT.r2(); k++)
          for(int j = 0; j < subT.n(); j++)
          {
            // only compare absolute values as the sign of singular vectors is not well defined
            EXPECT_NEAR(std::abs(subT_ref(i,iProc+j*nProcs,k)), std::abs(subT(i,j,k)), eps);
          }
    }
  }
}

TEST(PITTS_TensorTrain_fromDense, tensor2d_mpiGlobal)
{
  check_mpiGlobal_result({1, 10});
}

TEST(PITTS_TensorTrain_fromDense, another_tensor2d_mpiGlobal)
{
  check_mpiGlobal_result({3, 1});
}

TEST(PITTS_TensorTrain_fromDense, larger_tensor2d_mpiGlobal)
{
  check_mpiGlobal_result({7, 15});
}

TEST(PITTS_TensorTrain_fromDense, tensor3d_mpiGlobal)
{
  check_mpiGlobal_result({1, 5, 5});
}

TEST(PITTS_TensorTrain_fromDense, another_tensor3d_mpiGlobal)
{
  check_mpiGlobal_result({3, 5, 5});
}

TEST(PITTS_TensorTrain_fromDense, tensor5d_mpiGlobal)
{
  check_mpiGlobal_result({1, 5, 4, 5, 3});
}

TEST(PITTS_TensorTrain_fromDense, another_tensor5d_mpiGlobal)
{
  check_mpiGlobal_result({2, 5, 4, 5, 3});
}


#define BOOST_TEST_MODULE WeightsTest
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include "sparse_vector.h"

using namespace std;

BOOST_AUTO_TEST_CASE(Dot) {
  SparseVector<double> x;
  SparseVector<double> y;
  x.set_value(1,0.8);
  y.set_value(1,5);
  x.set_value(2,-2);
  y.set_value(2,1);
  x.set_value(3,80);
  BOOST_CHECK_CLOSE(x.dot(y), 2.0, 1e-9);
}

BOOST_AUTO_TEST_CASE(Equality) {
  SparseVector<double> x;
  SparseVector<double> y;
  x.set_value(1,-1);
  y.set_value(1,-1);
  BOOST_CHECK(x == y);
}

BOOST_AUTO_TEST_CASE(Division) {
  SparseVector<double> x;
  SparseVector<double> y;
  x.set_value(1,1);
  y.set_value(1,-1);
  BOOST_CHECK(!(x == y));
  x /= -1;
  BOOST_CHECK(x == y);
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Nbexponentbits Nbexponentbits (function template)

  Generates a constant representing the number of exponent bits of a floating point type.

  @headerref{<boost/simd/constant/nbexponentbits.hpp>}

  @par Description

  1.  @code
      template<typename T> as_integer_t<T> Nbexponentbits();
      @endcode

  2.  @code
      template<typename T> as_integer_t<T> Nbexponentbits( boost::simd::as_<T> const& target );
      @endcode

    Generates a value of type `as_integer_t<T>` that evaluates to the number of bits used to
    represents the exponent of an IEEE754 floating-point value.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c as_integer_t<T> that evaluates to:

  | Type        | double      | float         |
  |:------------|:------------|---------------|
  | **Values**  |   11        |      8        |

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/nbexponentbits.hpp>
#include <boost/simd/constant/simd/nbexponentbits.hpp>

#endif


// Compile me: g++ -std=c++1z main.cpp -o test
#include <iostream>
#include <iterator>
#include <vector>

#include <boost/format.hpp>
#include <boost/range/irange.hpp>

template<class Size>
std::tuple<Size> make_cum_product(Size size)
{
    return std::make_tuple(size);
}

template<class Size, class... Sizes>
std::tuple<Sizes..., Size, Size> make_cum_product(Size product, Size second, Sizes... tail)
{
    return std::tuple_cat(std::make_tuple(product), make_cum_product(second * product, tail...));
}

template<class Functor, class Ranges, class Sizes, class CumProd, std::size_t... Is>
void dereference(Functor f, std::size_t index,
                 const Ranges& ranges, const Sizes& sizes, const CumProd& cum_prod,
                 std::index_sequence<Is...>)
{
    f(std::get<Is>(ranges)[(index / std::get<Is>(cum_prod)) % std::get<Is>(sizes)]...);
}

template<class Functor, class... Ranges>
void MultipleForLoop(Functor f, const Ranges&... rs)
{
    auto sizes = std::make_tuple(std::size(rs)...);
    auto cum_prod = make_cum_product(std::size_t(1), std::size(rs)...);
    auto size = std::get<sizeof...(Ranges)>(cum_prod);
    auto ranges = std::make_tuple(rs...);

    for (std::size_t i = 0; i < size; ++i)
        dereference(f, i, ranges, sizes, cum_prod, std::index_sequence_for<Ranges...>());
}

void func(int i, int j, int k)
{
    std::cout << boost::format("Received (%1%, %2%, %3%)\n") % i % j % k;
}

int main(int argc, char** argv)
{
    MultipleForLoop(func, boost::irange(0, 3), std::vector<int>({-2, 0, 1}),
                    std::vector<int>({10, 11, 12, 13}));
    return 0;
}