#pragma once
/**
    Declaration of the Mesh class from meshGenerator.py in C++
    @file Mesh.hpp
    @author
    @version 2021-11-06
*/

#include "Defs.hpp"
#include "Element.hpp"
#include <list>
#include <vector>
#include <utility>
#include "core/ElemArray.hpp"
#include <Eigen/Dense>
#include <nanoflann/nanoflann.hpp>

// Placing all lib elements into a sel_map namespace, partly in case this is extended upon later, partly to reduce pollution.
namespace sel_map::mesh{
    /**
     * @brief Mesh class. Contains generic mesh information and is inherited by the TriangularMesh class.
     */
    template <typename TElement_t>
    class Mesh{
        // Found using the compiler
        typedef Eigen::Block<Eigen::Map<PointWithCovArray_t, Eigen::Aligned16>, Eigen::Dynamic, 2> points_to_2d_eigen_t;
        typedef nanoflann::KDTreeEigenMatrixAdaptor<points_to_2d_eigen_t, 2, nanoflann::metric_L2_Simple> kd_tree_t;
        public:
        // Since this is really only accessed for values, and not for maths, don't eigen it.
        // (x, y) origin of the mesh
        double originHeight;
        // (x, y, z) symmetric bounds of the mesh
        double bounds[3];

        // Upper limit to # of points to consider for each element
        unsigned int pointLimit;
        // Parameters to identify a height discrepency and how many elements to select
        // (unsigned int)(1.0/heightPartition) highest height elements are selected if heightSafetyCheck condition is met
        float heightPartition; double heightSafetyCheck;

        // The metric length of one element (not hypotenuse)
        double elementLength;

        // number of vertices along each of the principle axis
        unsigned int verticesLength[2];

        // The points that exist within the mesh, each defined as (x, y, z)
        sel_map::core::ElemArray<double, 4> points;

        // Binning of elementIdx for each point, based on the inner space of each square grid space.
        // Eigen::VectorXi pointsBin;

        //Work the VertMesh out of here
        //VertMesh vertexMesh;
        // The vector describing all the vertexes within the mesh (x, y, mu_z, sigma_z)
        // Structured as (width*height, 4), row-major. Access [x,y] with vector[y*width+x].
        PointWithCovArray_t vertexVector;
        // Eigen::Array<double, Eigen::Dynamic, 2, Eigen::RowMajor> vertexHeights;

        // List of all Element objects
        // std::list<TElement_t> elements;
        std::vector<TElement_t*> elementsVec;
        unsigned int num_elements;

        // Random generator for the mesh
        std::vector<std::mt19937> gens;

        // kd_tree for getting closest points
        const unsigned int max_leaf = 4;
        points_to_2d_eigen_t points_2d;
        kd_tree_t point_index;
        bool updateTree;

        Mesh(const double originHeight, const double bounds[3], double elementLength = 1, unsigned int pointLimit = 20,
             float heightPartition = 0.3, double heightSafetyCheck = 0.5, unsigned int seed = (std::random_device())());

        void makeVertices();

        inline unsigned int unravelIndex(int row, int col){
            // Compute wraparound
            col = ((col % int(verticesLength[0])) + verticesLength[0]) % verticesLength[0];
            row = ((row % int(verticesLength[1])) + verticesLength[1]) % verticesLength[1];
            // Return the correct index
            return row * verticesLength[0] + col;
        };
        inline unsigned int ravelRow(int length){
            // Return the correct row
            return length / verticesLength[0];
        };
        inline unsigned int ravelCol(int length){
            // Return the correct col
            return length % verticesLength[0];
        };
        inline unsigned int unravelElIndex(int row, int col){
            // Compute wraparound
            col = ((col % int(verticesLength[0]-1)) + verticesLength[0]-1) % (verticesLength[0]-1);
            row = ((row % int(verticesLength[1]-1)) + verticesLength[1]-1) % (verticesLength[1]-1);
            // Return the correct index
            return row * (verticesLength[0]-1) + col;
        };
        inline unsigned int ravelElRow(int length){
            // Return the correct row
            return length / (verticesLength[0]-1);
        };
        inline unsigned int ravelElCol(int length){
            // Return the correct col
            return length % (verticesLength[0]-1);
        };

        template <typename Derived>
        Eigen::Array<bool, Eigen::Dynamic, 1> isPointWithinPolygon(const Eigen::ArrayBase<Derived>& points, const Eigen::Array<double, Eigen::Dynamic, 2>& polygonVertexCoords){
            // Process and return an eigen bool array matching the points.
            Eigen::Array<bool, Eigen::Dynamic, 1> inPolygon(points.rows());
            #pragma omp parallel for
            for(unsigned int i = 0; i < points.rows(); ++i){
                // Identify using the singular point function
                Eigen::ArrayXd point = points.row(i);
                inPolygon(i) = isSinglePointWithinPolygon(point, polygonVertexCoords);
            }
            return inPolygon;
        }
        bool isSinglePointWithinPolygon(const Eigen::Ref<Eigen::Array2d>& point, const Eigen::Array<double, Eigen::Dynamic, 2>& polygonVertexCoords);

        virtual Eigen::ArrayXi generatePointBinning(const Eigen::Ref<const RowArray_t, Eigen::Aligned16>& points);

        // Can add (renders a set of idx's to invalid states, sorts, and remove. Doesn't retain sort.)
        // but, with binning, this is redundant.
        //removePointsNotWithinBoundsOfMesh(const Eigen::MatrixBase<Derived>& point)
        void updateKDTreeIfNeeded();

        // NOT USED
        // void updatePointsFromElements();

        // todo, consider utilizing internal binning to avoid searching the entire thing
        std::pair<PointWithCovArray_t, std::vector<double> > getClosestPoints(const Eigen::Ref<const Eigen::Vector2d, Eigen::Aligned16>& targetpoint, int numPoints = AllPoints, double radius = NoRadius, bool checkUpdate = true);

        // Removes points w/in each element to free up memory after vertex height calculation.
        // Also removes points from here to free up memory.
        void clean(bool lazy = false);
    };
}


#ifndef __ADAPTIVE_CONTROL_ALGORITHM__
#define __ADAPTIVE_CONTROL_ALGORITHM__

#include <ros/ros.h>
#include <Eigen/Dense>
#include <math.h>
#include <limits>
#include <stdexcept>

namespace folding_algorithms{
typedef Eigen::Matrix<double, 6, 1> Vector6d;
  /**
    Class that implements the adaptive control algorithm that enables DOF identification.
  **/
  class AdaptiveController
  {
  public:
    AdaptiveController();
    ~AdaptiveController();

    /**
      Computes the relative twist reference for the mechanism and updates the
      estimates. Results in the wrench frame.

      @param wrench Estimation wrench: estimated contact point force and wrench measurement .
      @param v_d The desired linear velocity.
      @param w_d The desired angular velocity.
      @param dt The elapsed time between calls.
      @param force_direction Optional direction for the force controlled component.
      @return The control twist for the relative velocity between parts.
    **/
    Vector6d control(const Vector6d &wrench, double v_d, double w_d, double dt, const Eigen::Vector3d &force_direction = Eigen::Vector3d::Zero());

    /**
      Resets the controller back to an initial state.
    **/
    void reset();

    /**
      Initialize the adaptive controller estimates in the wrench frame.

      @param t Initial translational DOF estimate.
      @param r Initial rotational DOF estimate.
    **/
    void initEstimates(const Eigen::Vector3d &t, const Eigen::Vector3d &r);

    /**
      Sets the magnitude of the desired contact force.

      @param f_d Desired force magnitude.
    **/
    void setReferenceForce(double f_d);

    /**
      Return the current estimates in the wrench frame.

      @param t Current translational DOF estimate.
      @param r Current rotational DOF estimate.
    **/
    void getEstimates(Eigen::Vector3d &t, Eigen::Vector3d &r) const;

    /**
      Returns the current value of the force control components.

      @param v_f The current value of the force control component.
      @param w_f The current value of the torque control component.
    **/
    void getForceControlValues(Eigen::Vector3d &v_f, Eigen::Vector3d &w_f) const;

    /**
      Returns the current measured force and torque errors.

      @param force_e The force error.
      @param torque_e The torque error.
      @param desired_force Force setpoint.
    **/
    void getErrors(Eigen::Vector3d &force_e, Eigen::Vector3d &torque_e, Eigen::Vector3d &desired_force) const;

  private:
    double alpha_force_, beta_force_, alpha_torque_, beta_torque_;
    double f_d_, alpha_adapt_t_, alpha_adapt_r_, torque_slack_, max_force_, max_torque_, force_slack_;
    Eigen::Vector3d t_, r_, int_force_, int_torque_, v_f_, w_f_, force_error_, torque_error_;
    ros::NodeHandle nh_;

    /**
      Compute the integral term in the wrench feedback component of the adaptive controller.

      @param prev The previous value of the integral term.
      @param v The projection vector.
      @param error The force/torque error.
      @param dt The elapsed time.
      @return The updated integral value.
    **/
    Eigen::Vector3d computeIntegralTerm(const Eigen::Vector3d &prev, const Eigen::Vector3d &v, const Eigen::Vector3d &error, double dt) const;

    bool getParams();
  };
}
#endif


#include <gtest/gtest.h>

#include <gen/pregraph.hpp>

#include <math/mathtools.hpp>

#include <boost/graph/iteration_macros.hpp>
#include <gen/graphutils.hpp>

using namespace eXl;

TEST(DunAtk, GraphTest)
{
  {
    ES_RuleSystem sys;
    sys.StartRule().AddNode().AddNode().AddConnection(0, 1)
      .AddNewNode().AddNewConnection(1, 2).End();

    ASSERT_TRUE(!sys.IsRuleSymmetric(0));

    sys.StartRule().AddNode().AddNode().AddConnection(0, 1)/*.AddConnection(1, 0)*/
      .AddNewNode().AddNewConnection(1, 2)/*.AddNewConnection(2, 1)*/.AddNewConnection(0, 2)/*.AddNewConnection(2, 0)*/.End();

    ASSERT_TRUE(sys.IsRuleSymmetric(1));

    sys.StartRule().AddNode().AddNode().AddConnection(0, 1)/*.AddConnection(1, 0)*/
      .AddNewNode().AddNewConnection(1, 2)/*.AddNewConnection(2, 1)*/.End();

    ASSERT_TRUE(!sys.IsRuleSymmetric(2));

    sys.StartRule().AddNode(1).AddCutNode(1).AddNode(1)
      .AddCutConnection(0, 1)/*.AddCutConnection(1, 0)*/
      .AddCutConnection(1, 2)/*.AddCutConnection(2, 1)*/
      .AddNewConnection(0, 2, 0, 1)/*.AddNewConnection(2, 0, 1, 3)*/.End();

    ASSERT_TRUE(sys.IsRuleSymmetric(3));
  }

  uint32_t numNodes = 0;

  auto allocIndex = [&numNodes](ES_RuleSystem::RewriteCtx& iCtx, ES_RuleSystem::GraphVtx iNewVtx)
  {
    boost::put(boost::vertex_index, iCtx.finalGraph, iNewVtx, numNodes++);
  };

  ES_RuleSystem sys;
  sys.StartRule().AddNode().AddNode().AddNode()
    .AddCutConnection(0, 1).AddCutConnection(1, 2).AddCutConnection(2, 0)
    .AddNewNode(0, allocIndex).AddNewNode(0, allocIndex).AddNewNode(0, allocIndex)
    .AddNewConnection(3, 4).AddNewConnection(4, 5).AddNewConnection(5, 3)
    .AddNewConnection(0, 3, 0, -1).AddNewConnection(3, 1, -1, 0)
    .AddNewConnection(1, 4, 1, -1).AddNewConnection(4, 2, -1, 1)
    .AddNewConnection(2, 5, 2, -1).AddNewConnection(5, 0, -1, 2)
    .End();

  ASSERT_TRUE(sys.IsRuleSymmetric(0));

  ES_RuleSystem::Graph testGraph;

  auto vtx1 = boost::add_vertex(testGraph);
  auto vtx2 = boost::add_vertex(testGraph);
  auto vtx3 = boost::add_vertex(testGraph);

  boost::put(boost::vertex_index, testGraph, vtx1, numNodes++);
  boost::put(boost::vertex_index, testGraph, vtx2, numNodes++);
  boost::put(boost::vertex_index, testGraph, vtx3, numNodes++);

  boost::add_edge(vtx1, vtx2, testGraph);
  boost::add_edge(vtx2, vtx3, testGraph);
  boost::add_edge(vtx3, vtx1, testGraph);

  auto browseGraph = [](ES_RuleSystem::Graph const& testGraph)
  {
    auto idxMap = MakeIndexMap(testGraph);

    for (auto edge : EdgesIter(testGraph))
    {
      boost::get(idxMap, edge.m_source);
      boost::get(idxMap, edge.m_target);
    }

    for (auto vtx : VerticesIter(testGraph))
    {
      for (auto edge : OutEdgesIter(testGraph, vtx))
      {
        boost::get(idxMap, edge.m_source);
        boost::get(idxMap, edge.m_target);
      }

      for (auto edge : InEdgesIter(testGraph, vtx))
      {
        boost::get(idxMap, edge.m_source);
        boost::get(idxMap, edge.m_target);
      }

      BGL_FORALL_INEDGES_T(vtx, edge, testGraph, ES_RuleSystem::Graph)
      {
        boost::get(idxMap, edge.m_source);
        boost::get(idxMap, edge.m_target);
      }
    }
  };

  browseGraph(testGraph);
  //sys.Print(std::cout, testGraph, {}, {});

  ES_RuleSystem::Graph newGraph;
  sys.Apply(testGraph, newGraph);

  browseGraph(newGraph);
  //sys.Print(std::cout, newGraph, {}, {});

  ES_RuleSystem::Graph newGraph2;
  sys.Apply(newGraph, newGraph2);

  browseGraph(newGraph2);
  //sys.Print(std::cout, newGraph2, {}, {});
}


#include <gtest/gtest.h>
#include <cmath> // pow
#include <Eigen/Dense>
#include "../src/InputInterface.h"
#include "../src/Decomposition.h"
#include "../src/matrix_definition.h"

namespace {

// helper function for getting a digit of a decimal number
// index is from the back and zero-based
int get_digit(double x, int index) {
    int n = (int) x;
    int larger = (int) pow(10, index+1);
    int smaller = (int) pow(10, index);
    return (int) (n%larger)/smaller;
    //return (int)(n%pow(10,index+1))/pow(10,index);
}

// Test whether global rows are calculated correctly
TEST(GlobalInputInterfaceTest, GlobalRowsTest) {
  std::vector<int> dimensions = {3,4,5,6};
  std::vector<bool> reduced(4, false);
  int fields = 1;

  reduced[2] = true;
  InputInterface<float, double> interface1(dimensions, reduced, fields);
  EXPECT_EQ(3*4*6, interface1.global_rows);

  reduced[3] = true;
  InputInterface<float, double> interface2(dimensions, reduced, fields);
  EXPECT_EQ(3*4, interface2.global_rows);

  reduced[0] = true;
  InputInterface<float, double> interface3(dimensions, reduced, fields);
  EXPECT_EQ(4, interface3.global_rows);
}

// The fixture for testing class Foo.
class SingleFileInputInterfaceTest : public ::testing::Test {
 protected:

  SingleFileInputInterfaceTest() {
    std::vector<int> dimensions = {3,4,5,3};
    std::vector<bool> reduced(4, false);
    reduced[0] = true;
    reduced[1] = true;
    int fields = 1;
    InputInterface<float, double> interface(dimensions, reduced, fields);

    std::vector<std::string> filenames = {"../test/test_x1.out"};
    bool multiple = false;
    matrix = interface.read(filenames, multiple);
  }

  // virtual ~InputInterfaceTest() { }
  // virtual void SetUp() { }
  // virtual void TearDown() { }

  // Objects declared here can be used by all tests in the test case for Foo.
  Matrix<double> matrix;
};

TEST_F(SingleFileInputInterfaceTest, RowsEqualTest) {
    for (int i=0; i<matrix.rows(); i++) {
        int n2 = get_digit(matrix(i,0),2);
        int n3 = get_digit(matrix(i,0),3);
        for (int j=1; j<matrix.cols(); j++) {
            EXPECT_EQ(n2, get_digit(matrix(i,j),2));
            EXPECT_EQ(n3, get_digit(matrix(i,j),3));
        }
    }
}

TEST_F(SingleFileInputInterfaceTest, ColsEqualTest) {
    for (int j=0; j<matrix.cols(); j++) {
        int n0 = get_digit(matrix(0,j),0);
        int n1 = get_digit(matrix(0,j),1);
        for (int i=1; i<matrix.rows(); i++) {
            EXPECT_EQ(n0, get_digit(matrix(i,j),0));
            EXPECT_EQ(n1, get_digit(matrix(i,j),1));
        }
    }
}

TEST_F(SingleFileInputInterfaceTest, RowsIncreasingTest) {
    for (int i=0; i<matrix.rows(); i++) {
        int n0 = get_digit(matrix(i,0),0);
        int n1 = get_digit(matrix(i,0),1);
        int n = 10*n1+n0;
        for (int j=1; j<matrix.cols(); j++) {
            n0 = get_digit(matrix(i,j),0);
            n1 = get_digit(matrix(i,j),1);
            int n_new = 10*n1+n0;
            EXPECT_GT(n_new, n);
            n = n_new;
        }
    }
}

TEST_F(SingleFileInputInterfaceTest, ColsIncreasingTest) {
    for (int j=0; j<matrix.cols(); j++) {
        int n2 = get_digit(matrix(0,j),2);
        int n3 = get_digit(matrix(0,j),3);
        int n = 10*n3+n2;
        for (int i=1; i<matrix.rows(); i++) {
            n2 = get_digit(matrix(i,j),2);
            n3 = get_digit(matrix(i,j),3);
            int n_new = 10*n3+n2;
            EXPECT_GT(n_new, n);
            n = n_new;
        }
    }
}

class MultiFileInputInterfaceTest : public ::testing::Test {
 protected:

  MultiFileInputInterfaceTest() {
    std::vector<int> dimensions = {3,4,5,6};
    std::vector<bool> reduced(4, false);
    reduced[1] = true;
    reduced[3] = true;
    int fields = 1;
    InputInterface<float, double> interface(dimensions, reduced, fields);

    std::vector<std::string> filenames = {"../test/test_x.lst"};
    bool multiple = true;
    matrix = interface.read(filenames, multiple);
    //std::cout << matrix << std::endl;
  }

  // virtual ~InputInterfaceTest() { }
  // virtual void SetUp() { }
  // virtual void TearDown() { }

  // Objects declared here can be used by all tests in the test case for Foo.
  Matrix<double> matrix;
};

TEST_F(MultiFileInputInterfaceTest, RowsEqualTest) {
    for (int i=0; i<matrix.rows(); i++) {
        int n0 = get_digit(matrix(i,0),0);
        int n2 = get_digit(matrix(i,0),2);
        for (int j=1; j<matrix.cols(); j++) {
            EXPECT_EQ(n0, get_digit(matrix(i,j),0));
            EXPECT_EQ(n2, get_digit(matrix(i,j),2));
        }
    }
}

TEST_F(MultiFileInputInterfaceTest, ColsEqualTest) {
    for (int j=0; j<matrix.cols(); j++) {
        int n1 = get_digit(matrix(0,j),1);
        int n3 = get_digit(matrix(0,j),3);
        for (int i=1; i<matrix.rows(); i++) {
            EXPECT_EQ(n1, get_digit(matrix(i,j),1));
            EXPECT_EQ(n3, get_digit(matrix(i,j),3));
        }
    }
}

TEST_F(MultiFileInputInterfaceTest, RowsIncreasingTest) {
    for (int i=0; i<matrix.rows(); i++) {
        int n1 = get_digit(matrix(i,0),1);
        int n3 = get_digit(matrix(i,0),3);
        int n = 10*n3+n1;
        for (int j=1; j<matrix.cols(); j++) {
            n1 = get_digit(matrix(i,j),1);
            n3 = get_digit(matrix(i,j),3);
            int n_new = 10*n3+n1;
            EXPECT_GT(n_new, n);
            n = n_new;
        }
    }
}

TEST_F(MultiFileInputInterfaceTest, ColsIncreasingTest) {
    for (int j=0; j<matrix.cols(); j++) {
        int n0 = get_digit(matrix(0,j),0);
        int n2 = get_digit(matrix(0,j),2);
        int n = 10*n2+n0;
        for (int i=1; i<matrix.rows(); i++) {
            n0 = get_digit(matrix(i,j),0);
            n2 = get_digit(matrix(i,j),2);
            int n_new = 10*n2+n0;
            EXPECT_GT(n_new, n);
            n = n_new;
        }
    }
}

class MultiFieldInputInterfaceTest : public ::testing::Test {
 protected:

  MultiFieldInputInterfaceTest() {
    std::vector<int> dimensions = {3,4,5,6};
    std::vector<bool> reduced(4, false);
    reduced[0] = true;
    reduced[2] = true;
    int fields = 2;
    InputInterface<float, double> interface(dimensions, reduced, fields);

    std::vector<std::string> filenames = {"../test/test_x.lst","../test/test_y.lst"};
    bool multiple = true;
    matrix = interface.read(filenames, multiple);
    //std::cout << matrix << std::endl << std::endl;
  }

  // virtual ~InputInterfaceTest() { }
  // virtual void SetUp() { }
  // virtual void TearDown() { }

  // Objects declared here can be used by all tests in the test case for Foo.
  Matrix<double> matrix;
};

TEST_F(MultiFieldInputInterfaceTest, RowsEqualTest) {
    for (int i=0; i<matrix.rows(); i++) {
        int n1 = get_digit(matrix(i,0),1);
        int n2 = get_digit(matrix(i,0),3);
        for (int j=1; j<matrix.cols(); j++) {
            EXPECT_EQ(n1, get_digit(matrix(i,j),1));
            EXPECT_EQ(n2, get_digit(matrix(i,j),3));
        }
    }
}

TEST_F(MultiFieldInputInterfaceTest, ColsEqualTest) {
    for (int j=0; j<matrix.cols(); j++) {
        int n1 = get_digit(matrix(0,j),0);
        int n2 = get_digit(matrix(0,j),2);
        for (int i=1; i<matrix.rows(); i++) {
            EXPECT_EQ(n1, get_digit(matrix(i,j),0));
            EXPECT_EQ(n2, get_digit(matrix(i,j),2));
        }
    }
}

TEST_F(MultiFieldInputInterfaceTest, RowsIncreasingTest) {
    for (int i=0; i<matrix.rows(); i++) {
        int n1 = get_digit(matrix(i,0),0);
        int n2 = get_digit(matrix(i,0),2);
        int n = 10*n2+n1;
        for (int j=1; j<matrix.cols(); j++) {
            n1 = get_digit(matrix(i,j),0);
            n2 = get_digit(matrix(i,j),2);
            int n_new = 10*n2+n1;
            EXPECT_GT(n_new, n);
            n = n_new;
        }
    }
}

TEST_F(MultiFieldInputInterfaceTest, ColsIncreasingTest) {
    for (int j=0; j<matrix.cols(); j++) {
        int n1 = get_digit(matrix(0,j),1);
        int n2 = get_digit(matrix(0,j),3);
        int n3 = get_digit(matrix(0,j),4);
        int n = 100*n3+10*n2+n1;
        for (int i=1; i<matrix.rows(); i++) {
            n1 = get_digit(matrix(i,j),1);
            n2 = get_digit(matrix(i,j),3);
            n3 = get_digit(matrix(i,j),4);
            int n_new = 100*n3+10*n2+n1;
            EXPECT_GT(n_new, n);
            n = n_new;
        }
    }
}

}  // namespace

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

  // initialize mpi
  MPI::Init(argc, argv);

  // run tests
  ::testing::InitGoogleTest(&argc, argv);
  int result = RUN_ALL_TESTS();
  MPI::Finalize();
  return result;
}


#ifndef SPLX_BEZIER_HPP
#define SPLX_BEZIER_HPP

#include <splx/curve/ParametricCurve.hpp>
#include <iostream>
#include <numeric>
#include <Eigen/StdVector>
#include <algorithm>
#include <memory>
#include <stdexcept>
#include <utility>
#include <splx/internal/combinatorics.hpp>
#include <splx/internal/bezier.hpp>

namespace splx {

template<typename T, unsigned int DIM>
class Bezier : public ParametricCurve<T, DIM> {
  public:
    using Base = ParametricCurve<T, DIM>;

    using VectorDIM = typename ParametricCurve<T, DIM>::VectorDIM;
    using Vector = typename ParametricCurve<T, DIM>::Vector;
    using Hyperplane = typename ParametricCurve<T, DIM>::Hyperplane;
    using Matrix = typename ParametricCurve<T, DIM>::Matrix;
    using Row = typename ParametricCurve<T, DIM>::Row;
    using ControlPoints = typename ParametricCurve<T, DIM>::ControlPoints;
    using CurveType = typename ParametricCurve<T, DIM>::CurveType;

    Bezier(): Base(CurveType::BEZIER), m_a(0) {

    }

    Bezier(T a, const ControlPoints& cpts) : Base(CurveType::BEZIER),
                                                      m_a(a),
                                                      m_controlPoints(cpts)

    {
      if(m_a < 0) {
        throw std::domain_error(
          std::string("max parameter should be non-negative. given ")
          + std::to_string(m_a)
        );
      }

    }

    Bezier(T a) : Base(CurveType::BEZIER), m_a(a) {
      if(m_a < 0) {
        throw std::domain_error(
          std::string("max parameter should be non-negative. given ")
          + std::to_string(m_a)
        );
      }
    }

    /*
    * Construct from curve pointer
    * curve->m_type should be beizer
    */
    Bezier(const std::shared_ptr<ParametricCurve<T, DIM>> curve): Base(CurveType::BEZIER, curve->a) {
      if(curve->type != CurveType::Bezier) {
        throw std::domain_error(
          std::string("tried to initialize bezier curve with another type of curve")
        );
      }

      auto bezpt = std::static_pointer_cast<Bezier<T, DIM>>(curve);
      m_controlPoints = bezpt->m_controlPoints;
      m_a = bezpt->m_a;
    }

    ~Bezier() {

    }

    Bezier(const Bezier<T, DIM>& rhs): Base(CurveType::BEZIER),
                                       m_a(rhs.m_a),
                                       m_controlPoints(rhs.m_controlPoints) {

    }

    Bezier(Bezier<T, DIM>&& rhs): Base(CurveType::BEZIER),
                                  m_a(rhs.m_a),
                                  m_controlPoints(std::move(rhs.m_controlPoints)) {
    }

    Bezier& operator=(const Bezier<T, DIM>& rhs) {
      this->m_a = rhs.m_a;
      this->m_controlPoints = rhs.m_controlPoints;
    }

    Bezier& operator=(Bezier<T, DIM>&& rhs) {
      this->m_a = rhs.m_a;
      this->m_controlPoints = std::move(rhs.m_controlPoints);
    }

    std::size_t numControlPoints() const override {
      return m_controlPoints.size();
    }

    VectorDIM& operator[](std::size_t i) override {
      return this->m_controlPoints[i];
    }

    const VectorDIM& operator[](std::size_t i) const override {
      return this->m_controlPoints[i];
    }

    void appendControlPoint(const VectorDIM& cpt) override {
      this->m_controlPoints.push_back(cpt);
    }

    void removeControlPoint(std::size_t idx) override {
      if(idx >= this->m_controlPoints.size()) {
        throw std::domain_error(
          std::string("given index out of range for removal")
        );
      }

      this->m_controlPoints.erase(this->m_controlPoints.begin() + idx);
    }

    T maxParameter() const override {
      return m_a;
    }

    void maxParameter(T nw) override {
      if(nw < 0) {
        throw std::domain_error(
          std::string("max parameter should be non-negative. given ")
          + std::to_string(nw)
        );
      }

      m_a = nw;
    }

    unsigned int degree() const {
      return this->numControlPoints() - 1;
    }

    /*
     * Evaluate k^th derivative of bezier curve at u = u.
     *
     * @fails if u > m_a
    */
    VectorDIM eval(T u, unsigned int k) const override {
      if(u < 0 || u > maxParameter()) {
        throw std::domain_error(
          std::string("u is outside of the range [0, ")
          + std::to_string(maxParameter())
          + std::string("]")
        );
      }

      if(this->numControlPoints() == 0) {
        VectorDIM zero;
        zero.setZero();
        return zero;
      }

      Row basis = splx::internal::bezier::getBasisRow(this->degree(), this->maxParameter(), u, k);
      VectorDIM result;
      result.setZero();

      for(typename std::size_t i = 0; i < this->numControlPoints(); i++) {
        result += (*this)[i] * basis(i);
      }

      return result;
    }

    /*
    * Returns true if the curve is in the negative side of the hyperplane hp
    * leverage convex hull property
    */
    bool onNegativeSide(const Hyperplane& hp) const override {
      for(const auto& cp: m_controlPoints) {
        if(hp.signedDistance(cp) >= 0)
          return false;
      }

      if(this->numControlPoints() == 0) {
        VectorDIM zeros;
        zeros.setZero();
        return hp.signedDistance(zeros) < 0;
      }

      return true;
    }

    /*
    * Returns true if the curve is in the negative side of the hyperplane hp
    * leverage convex hull property
    */
    bool onNonPositiveSide(const Hyperplane& hp) const override {
      for(const auto& cp: m_controlPoints) {
        if(hp.signedDistance(cp) > 0)
          return false;
      }

      if(this->numControlPoints() == 0) {
        VectorDIM zeros;
        zeros.setZero();
        return hp.signedDistance(zeros) <= 0;
      }

      return true;
    }

  private:
    /**
     * Bezier curve is defined for u \in [0, m_a]
    */
    T m_a;

    /**
     * Control points of the bezier curve.
     * Implicitly defines the order of the curve
     *
     * General equation of bezier curves is
     * f(u | m_a) = \sum_{i=0}^{d} P_i C(d, i) (u/m_a)^i (1-u/m_a)^(d-i)
     * Therefore, the degree d of the bezier is m_controlPoints.size() - 1
    */
    ControlPoints m_controlPoints;
};

}

#endif


#ifndef _IO_MATH_TESTS_PRINT_TVECTOR_3_HPP
#define _IO_MATH_TESTS_PRINT_TVECTOR_3_HPP

#include "../../src/types/vectors/TVector3.hpp"

#include <iostream>
#include <limits>
#include <boost/type_index.hpp>

namespace IOMath
{
    namespace Types
    {
        template <typename T>
        std::ostream& boost_test_print_type(std::ostream &outputStream, TVector<3, T> const &object)
        {
            outputStream.precision(std::numeric_limits<T>::max_digits10);
            outputStream << "TVector<" << 3 << ", " << boost::typeindex::type_id<T>().pretty_name() << "> ";
            outputStream << "{ " << "x: " << object.x;
            outputStream << ", y: " << object.y;
            outputStream << ", z: " << object.z << " }";
            outputStream.flush();

            return outputStream;
        }
    }
}

#endif


/**
 * @file scanUtils.hpp
 * @author (Enigmatisms:https://github.com/Enigmatisms) @copyright Qianyue He
 * @brief Utilities for 2d scan simulator
 * @date 2021-08-27
 * @copyright Copyright (c) 2021
 */

#pragma once
#include <chrono>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/Imu.h>

extern double K_P;
extern double K_I;
extern double K_D;

// scan带有漂移的odometry transform
void odomTFSimulation(
    const Eigen::Vector4d& noise_level, Eigen::Vector3d delta_p, tf::StampedTransform& tf,
    std::string frame_id, std::string child_id
);

void stampedTransform2TFMsg(const tf::StampedTransform& transf, tf::tfMessage& msg);

/// @brief create transform - tf messages
void makeTransform(Eigen::Vector3d p, std::string frame_id, std::string child_frame_id, tf::StampedTransform& transf);

/// @brief 2D LiDAR scan messages
void makeScan(
    const std::vector<double>& range, const Eigen::Vector3d& angles,
    sensor_msgs::LaserScan& scan, std::string frame_id, double scan_time
);

/// @brief 2D Odometry with noise
/// @param noise_level the first elem of this var is the noise level in translation, the second is for rotation
void makePerturbedOdom(
    const Eigen::Vector4d& noise_level, Eigen::Vector3d delta_p, nav_msgs::Odometry& odom,
    double duration, std::string frame_id, std::string child_id
);

void sendTransform(Eigen::Vector3d p, std::string frame_id, std::string child_frame_id);

void sendStampedTranform(const tf::StampedTransform& _tf);

double pidAngle(const Eigen::Vector2d& orient, const Eigen::Vector2d& obs, double now);

std::pair<double, double> makeImuMsg(
    const Eigen::Vector2d& speed,
    std::string frame_id,
    double now_ang,
    sensor_msgs::Imu& msg,
    Eigen::Vector2d vel_var,
    Eigen::Vector2d ang_var
);

double makeImuMsg(
    const Eigen::Vector2d& speed,
    std::string frame_id,
    double now_ang,
    sensor_msgs::Imu& msg,
    std::ofstream* file = nullptr
);

class TicToc {
private:
    std::chrono::_V2::system_clock::time_point start;
public:
    void tic() {
        start = std::chrono::system_clock::now();
    }

    // return in milliseconds
    double toc() {
        auto end = std::chrono::system_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
        return static_cast<double>(duration.count()) / 1000.0;
    }
};


//////////////////////////////////////////////////////////////////////////////
// detail::multi_array::serialization::load.hpp                             //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are 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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_MULTI_ARRAY_SERIALIZATION_LOAD_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_MULTI_ARRAY_SERIALIZATION_LOAD_HPP_ER_2009
#include <boost/multi_array.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/nvp.hpp>
//#include <boost/archive/text_iarchive.hpp>

namespace boost{ namespace serialization{

    template<class Archive,class T>
    inline void load(
        Archive & ar,
        boost::multi_array<T,2> & t,
        const unsigned int file_version
    )
    {
		typedef boost::multi_array<T,2> multi_array_;
		typedef typename multi_array_::size_type size_;

        size_ n0;
        ar >> BOOST_SERIALIZATION_NVP(n0);
        size_ n1;
        ar >> BOOST_SERIALIZATION_NVP(n1);

        t.resize(boost::extents[n0][n1]);
        ar >> make_array(t.data(), t.num_elements());
    }
}}

#endif


//  Copyright John Maddock 2015.
//  Use, modification and distribution are 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)

#ifdef _MSC_VER
#  pragma warning (disable : 4224)
#endif

#include <boost/array.hpp>
#include <boost/lexical_cast.hpp>
#include "../../test/table_type.hpp"
#include "table_helper.hpp"
#include "performance.hpp"
#include <iostream>

#define evaluate_polynomial_c_imp evaluate_polynomial_c_imp_1
#undef BOOST_MATH_TOOLS_POLY_EVAL_20_HPP
#include <boost/math/tools/detail/polynomial_horner1_20.hpp>
#undef evaluate_polynomial_c_imp
#undef BOOST_MATH_TOOLS_POLY_EVAL_20_HPP
#define evaluate_polynomial_c_imp evaluate_polynomial_c_imp_2
#include <boost/math/tools/detail/polynomial_horner2_20.hpp>
#undef evaluate_polynomial_c_imp
#undef BOOST_MATH_TOOLS_POLY_EVAL_20_HPP
#define evaluate_polynomial_c_imp evaluate_polynomial_c_imp_3
#include <boost/math/tools/detail/polynomial_horner3_20.hpp>
#undef evaluate_polynomial_c_imp

#undef BOOST_MATH_TOOLS_POLY_RAT_20_HPP
#define evaluate_rational_c_imp evaluate_rational_c_imp_1
#include <boost/math/tools/detail/rational_horner1_20.hpp>
#undef evaluate_rational_c_imp
#undef BOOST_MATH_TOOLS_POLY_RAT_20_HPP
#define evaluate_rational_c_imp evaluate_rational_c_imp_2
#include <boost/math/tools/detail/rational_horner2_20.hpp>
#undef evaluate_rational_c_imp
#undef BOOST_MATH_TOOLS_RAT_EVAL_20_HPP
#define evaluate_rational_c_imp evaluate_rational_c_imp_3
#include <boost/math/tools/detail/rational_horner3_20.hpp>
#undef evaluate_rational_c_imp
#undef BOOST_MATH_TOOLS_POLY_RAT_20_HPP

static const double num[21] = {
   static_cast<double>(56906521.91347156388090791033559122686859L),
   static_cast<double>(103794043.1163445451906271053616070238554L),
   static_cast<double>(86363131.28813859145546927288977868422342L),
   static_cast<double>(43338889.32467613834773723740590533316085L),
   static_cast<double>(14605578.08768506808414169982791359218571L),
   static_cast<double>(3481712.15498064590882071018964774556468L),
   static_cast<double>(601859.6171681098786670226533699352302507L),
   static_cast<double>(75999.29304014542649875303443598909137092L),
   static_cast<double>(6955.999602515376140356310115515198987526L),
   static_cast<double>(449.9445569063168119446858607650988409623L),
   static_cast<double>(19.51992788247617482847860966235652136208L),
   static_cast<double>(0.5098416655656676188125178644804694509993L),
   static_cast<double>(0.006061842346248906525783753964555936883222L),
   0.0
};
static const double denom[20] = {
   static_cast<double>(0u),
   static_cast<double>(39916800u),
   static_cast<double>(120543840u),
   static_cast<double>(150917976u),
   static_cast<double>(105258076u),
   static_cast<double>(45995730u),
   static_cast<double>(13339535u),
   static_cast<double>(2637558u),
   static_cast<double>(357423u),
   static_cast<double>(32670u),
   static_cast<double>(1925u),
   static_cast<double>(66u),
   static_cast<double>(1u),
   0.0
};
static const boost::uint32_t denom_int[20] = {
   static_cast<boost::uint32_t>(0u),
   static_cast<boost::uint32_t>(39916800u),
   static_cast<boost::uint32_t>(120543840u),
   static_cast<boost::uint32_t>(150917976u),
   static_cast<boost::uint32_t>(105258076u),
   static_cast<boost::uint32_t>(45995730u),
   static_cast<boost::uint32_t>(13339535u),
   static_cast<boost::uint32_t>(2637558u),
   static_cast<boost::uint32_t>(357423u),
   static_cast<boost::uint32_t>(32670u),
   static_cast<boost::uint32_t>(1925u),
   static_cast<boost::uint32_t>(66u),
   static_cast<boost::uint32_t>(1u),
   0
};

std::string make_order_string(int n)
{
   std::string result = boost::lexical_cast<std::string>(n);
   if (result.size() < 2)
      result.insert(result.begin(), ' ');
   return result;
}

void test_poly_1(const std::integral_constant<int, 1>&)
{
}

template <int N>
void test_poly_1(const std::integral_constant<int, N>&)
{
   test_poly_1(std::integral_constant<int, N - 1>());

   double time = exec_timed_test([](const std::vector<double>& v)
   {
      double result = 0;
      for (unsigned i = 0; i < 10; ++i)
         result += boost::math::tools::detail::evaluate_polynomial_c_imp_1(denom, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
      return result;
   });
   report_execution_time(time, std::string("Polynomial Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 1[br](Double Coefficients)");

   time = exec_timed_test([](const std::vector<double>& v)
   {
      double result = 0;
      for (unsigned i = 0; i < 10; ++i)
         result += boost::math::tools::detail::evaluate_polynomial_c_imp_1(denom_int, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
      return result;
   });
   report_execution_time(time, std::string("Polynomial Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 1[br](Integer Coefficients)");
}

void test_poly_2(const std::integral_constant<int, 1>&)
{
}

template <int N>
void test_poly_2(const std::integral_constant<int, N>&)
{
   test_poly_2(std::integral_constant<int, N - 1>());

   double time = exec_timed_test([](const std::vector<double>& v)
   {
      double result = 0;
      for (unsigned i = 0; i < 10; ++i)
         result += boost::math::tools::detail::evaluate_polynomial_c_imp_2(denom, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
      return result;
   });
   report_execution_time(time, std::string("Polynomial Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 2[br](Double Coefficients)");

   time = exec_timed_test([](const std::vector<double>& v)
   {
      double result = 0;
      for (unsigned i = 0; i < 10; ++i)
         result += boost::math::tools::detail::evaluate_polynomial_c_imp_2(denom_int, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
      return result;
   });
   report_execution_time(time, std::string("Polynomial Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 2[br](Integer Coefficients)");
}

void test_poly_3(const std::integral_constant<int, 1>&)
{
}

template <int N>
void test_poly_3(const std::integral_constant<int, N>&)
{
   test_poly_3(std::integral_constant<int, N - 1>());

   double time = exec_timed_test([](const std::vector<double>& v) {  double result = 0;
   for (unsigned i = 0; i < 10; ++i)
      result += boost::math::tools::detail::evaluate_polynomial_c_imp_3(denom, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
   return result;
   });
   report_execution_time(time, std::string("Polynomial Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 3[br](Double Coefficients)");

   time = exec_timed_test([](const std::vector<double>& v) {  double result = 0;
   for (unsigned i = 0; i < 10; ++i)
      result += boost::math::tools::detail::evaluate_polynomial_c_imp_3(denom_int, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
   return result;
   });
   report_execution_time(time, std::string("Polynomial Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 3[br](Integer Coefficients)");
}

template <class T, class U>
U evaluate_polynomial_0(const T* poly, U const& z, std::size_t count)
{
   U sum = static_cast<U>(poly[count - 1]);
   for (int i = static_cast<int>(count) - 2; i >= 0; --i)
   {
      sum *= z;
      sum += static_cast<U>(poly[i]);
   }
   return sum;
}

void test_rat_1(const std::integral_constant<int, 1>&)
{
}

template <int N>
void test_rat_1(const std::integral_constant<int, N>&)
{
   test_rat_1(std::integral_constant<int, N - 1>());

   double time = exec_timed_test([](const std::vector<double>& v)
   {
      double result = 0;
      for (unsigned i = 0; i < 10; ++i)
         result += boost::math::tools::detail::evaluate_rational_c_imp_1(num, denom, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
      return result;
   });
   report_execution_time(time, std::string("Rational Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 1[br](Double Coefficients)");

   time = exec_timed_test([](const std::vector<double>& v)
   {
      double result = 0;
      for (unsigned i = 0; i < 10; ++i)
         result += boost::math::tools::detail::evaluate_rational_c_imp_1(num, denom_int, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
      return result;
   });
   report_execution_time(time, std::string("Rational Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 1[br](Integer Coefficients)");
}

void test_rat_2(const std::integral_constant<int, 1>&)
{
}

template <int N>
void test_rat_2(const std::integral_constant<int, N>&)
{
   test_rat_2(std::integral_constant<int, N - 1>());

   double time = exec_timed_test([](const std::vector<double>& v)
   {
      double result = 0;
      for (unsigned i = 0; i < 10; ++i)
         result += boost::math::tools::detail::evaluate_rational_c_imp_2(num, denom, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
      return result;
   });
   report_execution_time(time, std::string("Rational Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 2[br](Double Coefficients)");

   time = exec_timed_test([](const std::vector<double>& v)
   {
      double result = 0;
      for (unsigned i = 0; i < 10; ++i)
         result += boost::math::tools::detail::evaluate_rational_c_imp_2(num, denom_int, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
      return result;
   });
   report_execution_time(time, std::string("Rational Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 2[br](Integer Coefficients)");
}

void test_rat_3(const std::integral_constant<int, 1>&)
{
}

template <int N>
void test_rat_3(const std::integral_constant<int, N>&)
{
   test_rat_3(std::integral_constant<int, N - 1>());

   double time = exec_timed_test([](const std::vector<double>& v)
   {
      double result = 0;
      for (unsigned i = 0; i < 10; ++i)
         result += boost::math::tools::detail::evaluate_rational_c_imp_3(num, denom, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
      return result;
   });
   report_execution_time(time, std::string("Rational Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 3[br](Double Coefficients)");

   time = exec_timed_test([](const std::vector<double>& v)
   {
      double result = 0;
      for (unsigned i = 0; i < 10; ++i)
         result += boost::math::tools::detail::evaluate_rational_c_imp_3(num, denom_int, v[0] + i, static_cast<std::integral_constant<int, N>*>(0));
      return result;
   });
   report_execution_time(time, std::string("Rational Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(N), "Method 3[br](Integer Coefficients)");
}

template <class T, class U, class V>
V evaluate_rational_0(const T* num, const U* denom, const V& z_, std::size_t count)
{
   V z(z_);
   V s1, s2;
   if (z <= 1)
   {
      s1 = static_cast<V>(num[count - 1]);
      s2 = static_cast<V>(denom[count - 1]);
      for (int i = (int)count - 2; i >= 0; --i)
      {
         s1 *= z;
         s2 *= z;
         s1 += num[i];
         s2 += denom[i];
      }
   }
   else
   {
      z = 1 / z;
      s1 = static_cast<V>(num[0]);
      s2 = static_cast<V>(denom[0]);
      for (unsigned i = 1; i < count; ++i)
      {
         s1 *= z;
         s2 *= z;
         s1 += num[i];
         s2 += denom[i];
      }
   }
   return s1 / s2;
}

int main()
{
   double val = 0.001;
   while (val < 1)
   {
      std::vector<double> v;
      v.push_back(val);
      data.push_back(v);
      val *= 1.1;
   }

   for (unsigned i = 3; i <= 20; ++i)
   {
      double time = exec_timed_test([&](const std::vector<double>& v) {
         double result = 0;
         for (unsigned j = 0; j < 10; ++j)
            result += evaluate_polynomial_0(denom, v[0] + j, i);
         return result;
      });
      report_execution_time(time, std::string("Polynomial Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(i), "Method 0[br](Double Coefficients)");

      time = exec_timed_test([&](const std::vector<double>& v) {
         double result = 0;
         for (unsigned j = 0; j < 10; ++j)
            result += evaluate_polynomial_0(denom_int, v[0] + j, i);
         return result;
      });
      report_execution_time(time, std::string("Polynomial Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(i), "Method 0[br](Integer Coefficients)");
   }

   test_poly_1(std::integral_constant<int, 20>());
   test_poly_2(std::integral_constant<int, 20>());
   test_poly_3(std::integral_constant<int, 20>());

   for (unsigned i = 3; i <= 20; ++i)
   {
      double time = exec_timed_test([&](const std::vector<double>& v) {
         double result = 0;
         for (unsigned j = 0; j < 10; ++j)
            result += evaluate_rational_0(num, denom, v[0] + j, i);
         return result;
      });
      report_execution_time(time, std::string("Rational Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(i), "Method 0[br](Double Coefficients)");

      time = exec_timed_test([&](const std::vector<double>& v) {
         double result = 0;
         for (unsigned j = 0; j < 10; ++j)
            result += evaluate_rational_0(num, denom_int, v[0] + j, i);
         return result;
      });
      report_execution_time(time, std::string("Rational Method Comparison with ") + compiler_name() + std::string(" on ") + platform_name(), "Order " + make_order_string(i), "Method 0[br](Integer Coefficients)");
   }

   test_rat_1(std::integral_constant<int, 20>());
   test_rat_2(std::integral_constant<int, 20>());
   test_rat_3(std::integral_constant<int, 20>());

   return 0;
}


// Boost.Geometry Index
//
// Quickbook Examples
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// 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)

//[rtree_interprocess

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>

#include <boost/geometry/index/rtree.hpp>

#include <boost/foreach.hpp>

#include <boost/interprocess/managed_shared_memory.hpp>
#include <boost/interprocess/allocators/allocator.hpp>

#include <vector>
#include <string>
#include <cstdlib> //std::system

//For parent process argc == 1, for child process argc > 1
int main(int argc, char *argv[])
{
    using namespace boost::interprocess;
    namespace bg = boost::geometry;
    namespace bgm = bg::model;
    namespace bgi = bg::index;

    typedef bgm::point<float, 2, bg::cs::cartesian> P;
    typedef bgm::box<P> B;

    typedef bgi::linear<32, 8> Par;
    typedef bgi::indexable<B> I;
    typedef bgi::equal_to<B> E;
    typedef allocator<B, managed_shared_memory::segment_manager> Alloc;
    typedef bgi::rtree<B, Par, I, E, Alloc> Rtree;

    //Parent process
    if ( argc == 1 )
    {
        struct shm_remove
        {
            shm_remove() { shared_memory_object::remove("MySharedMemory"); }
            ~shm_remove(){ shared_memory_object::remove("MySharedMemory"); }
        } remover;

        managed_shared_memory segment(create_only, "MySharedMemory", 65536);

        std::cout << "Parent: Constructing container\n";

        Rtree * tree = segment.construct<Rtree>("Rtree")(Par(), I(), E(), Alloc(segment.get_segment_manager()));

        std::cout << "Parent: Filling container with 100 boxes\n";

        for ( float i = 0 ; i < 100 ; i += 1 )
            tree->insert(B(P(i, i), P(i+0.5f, i+0.5f)));

        std::cout << "Parent: Tree content\n";
        Rtree::bounds_type bb = tree->bounds();
        std::cout << "[(" << bg::get<0>(bb.min_corner()) << ", " << bg::get<1>(bb.min_corner())
                  << ")(" << bg::get<0>(bb.max_corner()) << ", " << bg::get<1>(bb.max_corner()) << ")]\n";

        std::cout << "Parent: Running child process\n";

        std::string s(argv[0]); s += " child ";
        if ( 0 != std::system(s.c_str()) )
            return 1;

        if ( segment.find<Rtree>("Rtree").first )
            return 1;

        std::cout << "Parent: Container was properly destroyed by the child process\n";
   }
   //Child process
   else
   {
      managed_shared_memory segment(open_only, "MySharedMemory");

      std::cout << "Child: Searching of the container in shared memory\n";

      Rtree * tree = segment.find<Rtree>("Rtree").first;

      std::cout << "Child: Querying for objects intersecting box = [(45, 45)(55, 55)]\n";

      std::vector<B> result;
      unsigned k = tree->query(bgi::intersects(B(P(45, 45), P(55, 55))), std::back_inserter(result));

      std::cout << "Child: Found objects:\n";
      std::cout << k << "\n";
      BOOST_FOREACH(B const& b, result)
      {
        std::cout << "[(" << bg::get<0>(b.min_corner()) << ", " << bg::get<1>(b.min_corner())
                  << ")(" << bg::get<0>(b.max_corner()) << ", " << bg::get<1>(b.max_corner()) << ")]\n";
      }
      std::cout << "\n";

      std::cout << "Child: Destroying container\n";

      segment.destroy<Rtree>("Rtree");
   }

   return 0;
};

//]


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * 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.
 */

#include <iostream>
#include <iomanip>
#include <stdexcept>

#include "test_tomographer.h"

#include <tomographer/histogram.h>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

template<typename T>
inline Eigen::Matrix<T,4,1> inline_vector_4(T a1, T a2, T a3, T a4)
{
  Eigen::Matrix<T,4,1> v;
  (v << a1, a2, a3, a4).finished();
  return v;
}

BOOST_AUTO_TEST_SUITE(test_histogram);
// =============================================================================

BOOST_AUTO_TEST_SUITE(uniform_bins_histogram);

BOOST_AUTO_TEST_CASE(basic)
{
  Tomographer::Histogram<float, long> hist(0.0f, 1.0f, 10);
  hist.record(0.42323f);
  hist.record(0.933f);
  hist.record(0.5f);
  hist.record(0.55555232f);
  hist.record(0.4999f);
  hist.record(0.52f);
  hist.record(1.2f);

  BOOST_CHECK_EQUAL(hist.numBins(), 10);
  BOOST_CHECK_SMALL(hist.params.min, tol_f);
  BOOST_CHECK_CLOSE(hist.params.max, 1.0f, tol_percent_f);

  BOOST_CHECK_EQUAL(hist.count(0), 0);
  BOOST_CHECK_EQUAL(hist.count(1), 0);
  BOOST_CHECK_EQUAL(hist.count(2), 0);
  BOOST_CHECK_EQUAL(hist.count(3), 0);
  BOOST_CHECK_EQUAL(hist.count(4), 2);
  BOOST_CHECK_EQUAL(hist.count(5), 3);
  BOOST_CHECK_EQUAL(hist.count(6), 0);
  BOOST_CHECK_EQUAL(hist.count(7), 0);
  BOOST_CHECK_EQUAL(hist.count(8), 0);
  BOOST_CHECK_EQUAL(hist.count(9), 1);
  BOOST_CHECK_EQUAL(hist.off_chart, 1);

  for (int k = 0; k < 10; ++k) {
    BOOST_CHECK_EQUAL(hist.count(k), hist.bins(k));
  }

  typedef Tomographer::Histogram<float, long> MyHistogramType;
  BOOST_CHECK( ! MyHistogramType::HasErrorBars );
}

BOOST_AUTO_TEST_CASE(boundaries)
{
  Tomographer::Histogram<float, int> hist(0.0f, 1.f, 10);

  BOOST_CHECK(hist.isWithinBounds(0.43f));
  BOOST_CHECK(!hist.isWithinBounds(-0.01f));
  BOOST_CHECK(!hist.isWithinBounds(1.2f));
  BOOST_CHECK(!hist.isWithinBounds(std::numeric_limits<float>::quiet_NaN()));
  BOOST_CHECK(!hist.isWithinBounds(std::numeric_limits<float>::infinity()));
  BOOST_CHECK(!hist.isWithinBounds(-std::numeric_limits<float>::infinity()));

  BOOST_CHECK_EQUAL(hist.binIndex(0.13f), 1);
  BOOST_CHECK_EQUAL(hist.binIndex(0.99f), 9);
  BOOST_CHECK_EQUAL(hist.binIndex(0.34f), 3);

  BOOST_CHECK_SMALL(hist.binLowerValue(0), tol_f);
  BOOST_CHECK_CLOSE(hist.binLowerValue(1), 0.1f, tol_percent_f);
  BOOST_CHECK_CLOSE(hist.binLowerValue(6), 0.6f, tol_percent_f);

  BOOST_CHECK_CLOSE(hist.binUpperValue(0), 0.1f, tol_percent_f);
  BOOST_CHECK_CLOSE(hist.binUpperValue(5), 0.6f, tol_percent_f);
  BOOST_CHECK_CLOSE(hist.binUpperValue(9), 1.0f, tol_percent_f);

  BOOST_CHECK_CLOSE(hist.binCenterValue(1), 0.15f, tol_percent_f);

  BOOST_CHECK_CLOSE(hist.binResolution(), 0.1f, tol_percent_f);

  {
    EigenAssertTest::setting_scope settingvariable(true); // eigen_assert() should throw an exception.
    BOOST_CHECK_THROW(
        hist.binLowerValue(-1),
        ::Tomographer::Tools::EigenAssertException
        );
    BOOST_CHECK_THROW(
        hist.binLowerValue(11),
        ::Tomographer::Tools::EigenAssertException
        );
    BOOST_CHECK_THROW(
        hist.binCenterValue(-1),
        ::Tomographer::Tools::EigenAssertException
        );
    BOOST_CHECK_THROW(
        hist.binUpperValue(-1),
        ::Tomographer::Tools::EigenAssertException
        );
  }
}

BOOST_AUTO_TEST_CASE(values)
{
  Tomographer::HistogramParams<double> params(0.0f, 1.f, 10);

  auto vcenter = params.valuesCenter();
  BOOST_CHECK_EQUAL(vcenter.size(), 10);
  MY_BOOST_CHECK_FLOATS_EQUAL(vcenter(0), 0.05, tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(vcenter(4), 0.45, tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(vcenter(9), 0.95, tol);

  auto vlower = params.valuesLower();
  BOOST_CHECK_EQUAL(vlower.size(), 10);
  MY_BOOST_CHECK_FLOATS_EQUAL(vlower(0), 0.0, tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(vlower(9), 0.9, tol);

  auto vupper = params.valuesUpper();
  BOOST_CHECK_EQUAL(vupper.size(), 10);
  MY_BOOST_CHECK_FLOATS_EQUAL(vupper(0), 0.1, tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(vupper(9), 1.0, tol);
}

BOOST_AUTO_TEST_CASE(add_load_reset)
{
  Tomographer::Histogram<float, long> hist(0.0f, 1.0f, 10);
  hist.record(0.42323f);
  hist.record(0.933f);
  hist.record(0.5f);
  hist.record(0.55555232f);
  hist.record(0.4999f);
  hist.record(0.52f);
  hist.record(1.2f);

  Tomographer::Histogram<double, unsigned int> hist2(0.0, 1.0, 10);
  hist2.add(hist);

  int k;

  for (k = 0; k < 10; ++k) {
    BOOST_CHECK_EQUAL(hist.count(k), (long)hist2.count(k));
  }
  BOOST_CHECK_EQUAL(hist.off_chart, (long)hist2.off_chart);

  hist2.load(Eigen::Matrix<int,10,1>::Constant(80));
  for (k = 0; k < 10; ++k) {
    BOOST_CHECK_EQUAL(hist2.count(k), 80u);
  }

  Eigen::Matrix<unsigned int,10,1> m;
  (m << 0,    1,  4, 30, 95,
        150, 77, 18,  5,  1).finished();

  hist2.load(m, 42);

  MY_BOOST_CHECK_EIGEN_EQUAL(hist2.bins, m, tol);
  BOOST_CHECK_EQUAL(hist2.off_chart, 42u);

  Eigen::Matrix<unsigned int,10,1> m2;
  (m2 <<  0,  0,  0, 10, 10,
         10, 10, 10,  0,  0).finished();
  hist2.add(m2.array());

  MY_BOOST_CHECK_EIGEN_EQUAL(hist2.bins, m+m2, tol);
  BOOST_CHECK_EQUAL(hist2.off_chart, 42u);

  hist2.reset();
  auto zeros = Eigen::Array<unsigned int,10,1>::Zero();
  MY_BOOST_CHECK_EIGEN_EQUAL(hist2.bins, zeros, tol);
  BOOST_CHECK_EQUAL(hist2.off_chart, 0u);
}

BOOST_AUTO_TEST_CASE(floatcounttype)
{
  Tomographer::Histogram<float, double> hist(0.0f, 1.0f, 10);

  hist.record(0.21f);
  hist.record(0.55f, 2.01);
  hist.record(0.743f);
  hist.record(1.334f, 120.399);
  hist.record(0.781f, 380.4);
  hist.record(0.58f);
  hist.record(0.64f, 1.2);

  Eigen::Array<double,10,1> a;
  //   0.0  0.1  0.2  0.3  0.4   0.5  0.6    0.7  0.8  0.9
  a <<   0,   0, 1.0,   0,   0, 3.01, 1.2, 381.4,   0,   0;

  BOOST_MESSAGE(hist.prettyPrint(100));

  MY_BOOST_CHECK_EIGEN_EQUAL(hist.bins, a, tol);
  BOOST_CHECK_CLOSE(hist.off_chart, 120.399, tol_percent);
}

BOOST_AUTO_TEST_CASE(normalization)
{
  { Tomographer::Histogram<float, int> hist(0.0f, 4.0f, 4);
    hist.load( inline_vector_4<int>(0, 3, 19, 24), 10 );
    MY_BOOST_CHECK_FLOATS_EQUAL(hist.normalization(), 0.f+3+19+24+10, tol_f); }

  { Tomographer::Histogram<float, int> hist(0.0f, 1.0f, 4);
    hist.load( inline_vector_4<int>(0, 3, 19, 24), 10 );
    BOOST_CHECK_EQUAL(hist.normalization<int>(), int((0+3+19+24)*0.25 + 10)); }

  { Tomographer::Histogram<float, int> hist(0.0f, 1.0f, 4);
    hist.load( inline_vector_4<int>(0, 3, 19, 24), 10 );
    MY_BOOST_CHECK_FLOATS_EQUAL(hist.normalization<double>(), (0.0+3+19+24)*0.25 + 10, tol); }
}
BOOST_AUTO_TEST_CASE(normalized)
{
  { Tomographer::Histogram<float, int> hist(0.0f, 4.0f, 4);
    hist.load( inline_vector_4<int>(0, 3, 19, 24), 10 );
    auto hn = hist.normalized();
    auto n = hist.normalization();
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.bins, inline_vector_4<float>(0, 3, 19, 24)/n, tol_f);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.off_chart, 10.f/n, tol_f);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.normalization(), 1.f, tol_f);
  }
  { Tomographer::Histogram<float, int> hist(0.0f, 1.0f, 4);
    hist.load( inline_vector_4<int>(0, 3, 19, 24), 10 );
    auto hn = hist.normalized<double>();
    auto n = hist.normalization<double>();
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.bins, inline_vector_4<double>(0, 3, 19, 24)/n, tol);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.off_chart, 10.0/n, tol);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.normalization(), 1.0, tol);
  }
}
BOOST_AUTO_TEST_CASE(normalized_counts)
{
  { Tomographer::Histogram<float, int> hist(0.0f, 4.0f, 4);
    hist.load( inline_vector_4<int>(0, 3, 19, 24), 10 );
    BOOST_CHECK_EQUAL(hist.totalCounts(), hist.bins.sum() + hist.off_chart);
    auto hn = hist.normalizedCounts();
    auto n = hist.totalCounts();
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.bins, inline_vector_4<float>(0, 3, 19, 24)/n, tol_f);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.off_chart, 10.f/n, tol_f);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.totalCounts(), 1.f, tol_f);
  }
  { Tomographer::Histogram<float, int> hist(0.0f, 1.0f, 4);
    hist.load( inline_vector_4<int>(0, 3, 19, 24), 10 );
    BOOST_CHECK_EQUAL(hist.totalCounts(), hist.bins.sum() + hist.off_chart);
    auto hn = hist.normalizedCounts<double>();
    double n = hist.totalCounts();
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.bins, inline_vector_4<double>(0, 3, 19, 24)/n, tol);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.off_chart, 10.0/n, tol);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.totalCounts(), 1.0, tol);
  }
}

BOOST_AUTO_TEST_CASE(copy)
{
  Tomographer::Histogram<float, int> hist(0.0f, 1.0f, 5);
  hist.bins << 0, 3, 19, 24, 8;

  auto histtgt = Tomographer::Histogram<float, double>::copy(hist);

  MY_BOOST_CHECK_EIGEN_EQUAL( hist.bins.cast<double>(), histtgt.bins, tol );
}

BOOST_AUTO_TEST_CASE(move)
{
  Tomographer::Histogram<float, int> hist(0.0f, 1.0f, 5);
  hist.bins << 0, 3, 19, 24, 8;

  Tomographer::Histogram<float, int> hist2(std::move(hist));

  BOOST_CHECK_EQUAL( hist2.bins.size(), 5 );
  MY_BOOST_CHECK_FLOATS_EQUAL(hist2.bins.cast<double>()(0), 0., tol );
  MY_BOOST_CHECK_FLOATS_EQUAL(hist2.bins.cast<double>()(1), 3., tol );
  MY_BOOST_CHECK_FLOATS_EQUAL(hist2.bins.cast<double>()(2), 19., tol );
  MY_BOOST_CHECK_FLOATS_EQUAL(hist2.bins.cast<double>()(3), 24., tol );
  MY_BOOST_CHECK_FLOATS_EQUAL(hist2.bins.cast<double>()(4), 8., tol );
}

BOOST_AUTO_TEST_SUITE_END(); // uniform_bins_histogram

// -----------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(uniform_bins_histogram_with_error_bars)

BOOST_AUTO_TEST_CASE(basic)
{
  Tomographer::HistogramWithErrorBars<double, float> hist(-1.0, 1.0, 4);

  // this doesn't make sense!! what about the error bars?
  //  hist.record(0.01, 1.2f);
  //  hist.record(-0.56, 0.1f);
  (hist.bins << 0.1f, 0.f, 1.2f, 0.f).finished();

  (hist.delta << 0.1f, 0.f, 0.4f, 0.f).finished();

  Eigen::Array<float,4,1> a;
  a << 0.1f, 0.f, 1.2f, 0.f;
  MY_BOOST_CHECK_EIGEN_EQUAL(hist.bins, a, tol_f);

  int k;

  for (k = 0; k < 4; ++k) {
    BOOST_CHECK_CLOSE(hist.errorBar(k), hist.delta(k), tol_percent);
  }

  hist.reset();

  auto zeros4 = Eigen::Array<float,4,1>::Zero();
  MY_BOOST_CHECK_EIGEN_EQUAL(hist.bins, zeros4, tol_f);
  MY_BOOST_CHECK_EIGEN_EQUAL(hist.delta, zeros4, tol_f);
  BOOST_CHECK_CLOSE(hist.off_chart, 0.f, tol_percent_f);

  typedef Tomographer::HistogramWithErrorBars<double,float> MyHistType;
  BOOST_CHECK( MyHistType::HasErrorBars );
}

BOOST_AUTO_TEST_CASE(load_reset)
{
  Tomographer::HistogramWithErrorBars<double, float> hist(0.0f, 1.0f, 10);
  Tomographer::HistogramWithErrorBars<double, double> hist2(0.0, 1.0, 10);

  Eigen::Matrix<int,10,1> m;
  (m << 0,    1,  4, 30, 95,
        150, 77, 18,  5,  1).finished();
  Eigen::Matrix<float,10,1> m2;
  (m2 << 0,    1,  1, 4, 2,
         5, 7, 1,  3,  0.5).finished();

  hist.load(m, m2, 42);
  MY_BOOST_CHECK_EIGEN_EQUAL(hist.bins, m.cast<float>(), tol);
  MY_BOOST_CHECK_EIGEN_EQUAL(hist.delta, m2, tol);
  BOOST_CHECK_EQUAL(hist.off_chart, 42);

  hist2.load(m, m2, 42);
  MY_BOOST_CHECK_EIGEN_EQUAL(hist2.bins, m.cast<double>(), tol);
  MY_BOOST_CHECK_EIGEN_EQUAL(hist2.delta, m2.cast<double>(), tol);
  BOOST_CHECK_EQUAL(hist2.off_chart, 42);

  hist2.reset();
  auto zeros = Eigen::Array<double,10,1>::Zero();
  MY_BOOST_CHECK_EIGEN_EQUAL(hist2.bins, zeros, tol);
  MY_BOOST_CHECK_EIGEN_EQUAL(hist2.delta, zeros, tol);
  BOOST_CHECK_EQUAL(hist2.off_chart, 0);
}

#if __GNUC__ == 4 && __GNUC_MINOR__ <= 6 && !defined(__clang__)
#define IS_GCC_4_6 1
#else
#define IS_GCC_4_6 0
#endif

BOOST_AUTO_TEST_CASE(normalized)
{
  { Tomographer::HistogramWithErrorBars<float, int> hist(0.0f, 4.0f, 4);
    hist.load( inline_vector_4<int>(0, 3, 19, 24),
               inline_vector_4<float>(0, 1, 4, 3),
               10 );
    auto hn = hist.normalized();
    auto n = hist.normalization();
#if ! IS_GCC_4_6  // gcc 4.6 doesn't like decltype(...) it appears:
    typedef std::is_same<decltype(hn)::Scalar, decltype(hist)::Scalar> test1td;
    TOMO_STATIC_ASSERT_EXPR(test1td::value) ;
    typedef std::is_same<decltype(hn)::CountType, decltype(float(1)+int(1))> test2td;
    TOMO_STATIC_ASSERT_EXPR(test2td::value) ;
#endif
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.bins, inline_vector_4<float>(0, 3, 19, 24)/n, tol_f);
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.delta, inline_vector_4<float>(0, 1, 4, 3)/n, tol_f);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.off_chart, 10.f/n, tol_f);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.normalization(), 1.f, tol_f);
  }
  { Tomographer::HistogramWithErrorBars<float, int> hist(0.0f, 1.0f, 4);
    hist.load( inline_vector_4<int>(0, 3, 19, 24),
               inline_vector_4<float>(0, 1, 4, 3),
               10 );
    auto hn = hist.normalized<double>();
    // the x-axis Scalar doesn't change, it's the count type that does.
#if ! IS_GCC_4_6  // gcc 4.6 doesn't like decltype(...) it appears:
    typedef std::is_same<decltype(hn)::Scalar, decltype(hist)::Scalar> td1;
    TOMO_STATIC_ASSERT_EXPR(td1::value) ;
    typedef std::is_same<decltype(hn)::CountType, double> td2;
    TOMO_STATIC_ASSERT_EXPR(td2::value) ;
#endif
    auto n = hist.normalization<double>();
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.bins, inline_vector_4<double>(0, 3, 19, 24)/n, tol);
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.delta, inline_vector_4<double>(0, 1, 4, 3)/n, tol);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.off_chart, 10.0/n, tol);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.normalization(), 1.0, tol);
  }
}

BOOST_AUTO_TEST_CASE(normalized_counts)
{
  { Tomographer::HistogramWithErrorBars<float, int> hist(0.0f, 4.0f, 4);
    hist.load( inline_vector_4<int>(0, 3, 19, 24),
               inline_vector_4<float>(0, 1, 4, 3),
               10 );
    auto hn = hist.normalizedCounts();
    float n = hist.totalCounts();
#if ! IS_GCC_4_6  // gcc 4.6 doesn't like decltype(...) it appears:
    typedef std::is_same<decltype(hn)::Scalar, decltype(hist)::Scalar> test1td;
    TOMO_STATIC_ASSERT_EXPR(test1td::value) ;
    typedef std::is_same<decltype(hn)::CountType, decltype(float(1)+int(1))> test2td;
    TOMO_STATIC_ASSERT_EXPR(test2td::value) ;
#endif
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.bins, inline_vector_4<float>(0, 3, 19, 24)/n, tol_f);
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.delta, inline_vector_4<float>(0, 1, 4, 3)/n, tol_f);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.off_chart, 10.f/n, tol_f);
  }
  { Tomographer::HistogramWithErrorBars<float, int> hist(0.0f, 1.0f, 4);
    hist.load( inline_vector_4<int>(0, 3, 19, 24),
               inline_vector_4<float>(0, 1, 4, 3),
               10 );
    auto hn = hist.normalizedCounts<double>();
    double n = hist.totalCounts();
    // the x-axis Scalar doesn't change, it's the count type that does.
#if ! IS_GCC_4_6  // gcc 4.6 doesn't like decltype(...) it appears:
    typedef std::is_same<decltype(hn)::Scalar, decltype(hist)::Scalar> td1;
    TOMO_STATIC_ASSERT_EXPR(td1::value) ;
    typedef std::is_same<decltype(hn)::CountType, double> td2;
    TOMO_STATIC_ASSERT_EXPR(td2::value) ;
#endif
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.bins, inline_vector_4<double>(0, 3, 19, 24)/n, tol);
    MY_BOOST_CHECK_EIGEN_EQUAL(hn.delta, inline_vector_4<double>(0, 1, 4, 3)/n, tol);
    MY_BOOST_CHECK_FLOATS_EQUAL(hn.off_chart, 10.0/n, tol);
  }
}

BOOST_AUTO_TEST_SUITE_END(); // uniform_bins_histogram_with_error_bars

// -----------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(averaged_histogram);

BOOST_AUTO_TEST_CASE(no_underlying_error_bars)
{
  typedef Tomographer::Histogram<double, int> SimpleHistogramType;
  typedef Tomographer::AveragedHistogram<SimpleHistogramType, float> AvgHistogramType;

  typename SimpleHistogramType::Params p(0.0, 1.0, 4);

  AvgHistogramType avghist;

  avghist.reset(p);

  BOOST_CHECK_SMALL(avghist.params.min, tol);
  BOOST_CHECK_CLOSE(avghist.params.max, 1.0, tol_percent);
  BOOST_CHECK_EQUAL(avghist.numBins(), 4);

  { SimpleHistogramType hist(p);
    hist.load( inline_vector_4<double>(15, 45, 42, 12) , 36 ); // sum=150
    avghist.addHistogram(hist);
  }
  { SimpleHistogramType hist(p);
    hist.load( inline_vector_4<double>(17, 43, 40, 18) , 32 );
    avghist.addHistogram(hist);
  }
  { SimpleHistogramType hist(p);
    hist.load( inline_vector_4<double>(20, 38, 47, 10) , 35 );
    avghist.addHistogram(hist);
  }
  { SimpleHistogramType hist(p);
    hist.load( inline_vector_4<double>(18, 44, 43, 13) , 32 );
    avghist.addHistogram(hist);
  }

  avghist.finalize();

  BOOST_CHECK_EQUAL(avghist.num_histograms, 4);

  BOOST_MESSAGE(avghist.prettyPrint());

  auto cheap_stddev = [](float x, float y, float z, float w) -> float {
    return (float) std::sqrt( ((x*x+y*y+z*z+w*w)/4.f - std::pow((x+y+z+w)/4.f, 2)) / (4.f - 1) );
  };

  BOOST_CHECK_CLOSE(avghist.bins.sum() + avghist.off_chart, 150.0, tol_percent);
  auto vecbins = inline_vector_4<float>(70/4.f,170/4.f,172/4.f,53/4.f);
  MY_BOOST_CHECK_EIGEN_EQUAL(avghist.bins, vecbins, tol_f);
  auto vecdelta = inline_vector_4<float>(
      cheap_stddev(15, 17, 20, 18),
      cheap_stddev(45, 43, 38, 44),
      cheap_stddev(42, 40, 47, 43),
      cheap_stddev(12, 18, 10, 13)
      );
  MY_BOOST_CHECK_EIGEN_EQUAL(avghist.delta, vecdelta, tol_f);
}

BOOST_AUTO_TEST_CASE(with_underlying_error_bars)
{
  typedef Tomographer::HistogramWithErrorBars<double, float> BaseHistogramType;
  typedef Tomographer::AveragedHistogram<BaseHistogramType, float> AvgHistogramType;

  typename BaseHistogramType::Params p(0.0, 1.0, 4);

  AvgHistogramType avghist;

  avghist.reset(p);

  BOOST_CHECK_SMALL(avghist.params.min, tol);
  BOOST_CHECK_CLOSE(avghist.params.max, 1.0, tol_percent);
  BOOST_CHECK_EQUAL(avghist.numBins(), 4);

  { BaseHistogramType hist(p);
    hist.load( inline_vector_4<double>(15, 45, 42, 12) ,
               inline_vector_4<float>(1, 1, 1, 1), 36 ); // sum=150
    avghist.addHistogram(hist);
  }
  { BaseHistogramType hist(p);
    hist.load( inline_vector_4<double>(17, 43, 40, 18) ,
               inline_vector_4<float>(2, 2, 5, 2), 32 );
    avghist.addHistogram(hist);
  }
  { BaseHistogramType hist(p);
    hist.load( inline_vector_4<double>(20, 38, 47, 10) ,
               inline_vector_4<float>(1, 2, 13, 4), 35 );
    avghist.addHistogram(hist);
  }
  { BaseHistogramType hist(p);
    hist.load( inline_vector_4<double>(18, 44, 43, 13) ,
               inline_vector_4<float>(2, 1, 24, 3), 32 );
    avghist.addHistogram(hist);
  }

  avghist.finalize();

  BOOST_CHECK_EQUAL(avghist.num_histograms, 4);

  BOOST_TEST_MESSAGE(avghist.prettyPrint());

  BOOST_CHECK_CLOSE(avghist.bins.sum() + avghist.off_chart, 150.f, tol_percent_f);
  auto vecbins = inline_vector_4<float>(70/4.f,170/4.f,172/4.f,53/4.f);
  MY_BOOST_CHECK_EIGEN_EQUAL(avghist.bins, vecbins, tol_f);
  auto vecdelta = inline_vector_4<float>(
      std::sqrt(1.f+4+1+4)/4.f,
      std::sqrt(1.f+4+4+1)/4.f,
      std::sqrt(1.f+5*5+13*13+24*24)/4.f,
      std::sqrt(1.f+4.f+16.f+9.f)/4.f
      );
  MY_BOOST_CHECK_EIGEN_EQUAL(avghist.delta, vecdelta, tol_f);
}

BOOST_AUTO_TEST_SUITE_END(); // averaged_histogram

// -----------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(aggregated_histogram);

BOOST_AUTO_TEST_CASE(simple)
{
  typedef Tomographer::Histogram<double, int> SimpleHistogramType;
  typedef Tomographer::AveragedHistogram<SimpleHistogramType, float> AvgHistogramType;

  typename SimpleHistogramType::Params p(0.0, 1.0, 4);

  SimpleHistogramType hist1(p);
  hist1.load( inline_vector_4<double>(15, 45, 42, 12) , 36 ); // sum=150
  SimpleHistogramType hist2(p);
  hist2.load( inline_vector_4<double>(17, 43, 40, 18) , 32 );
  SimpleHistogramType hist3(p);
  hist3.load( inline_vector_4<double>(20, 38, 47, 10) , 35 );
  SimpleHistogramType hist4(p);
  hist4.load( inline_vector_4<double>(18, 44, 43, 13) , 32 );

  AvgHistogramType avghist;
  avghist.reset(p);
  avghist.addHistogram(hist1);
  avghist.addHistogram(hist2);
  avghist.addHistogram(hist3);
  avghist.addHistogram(hist4);
  avghist.finalize();

  std::vector<const SimpleHistogramType*> hist_list;
  hist_list.push_back(&hist1);
  hist_list.push_back(&hist2);
  hist_list.push_back(&hist3);
  hist_list.push_back(&hist4);

  Tomographer::AggregatedHistogramSimple<SimpleHistogramType, float> aggregated
    = Tomographer::AggregatedHistogramSimple<SimpleHistogramType, float>::aggregate(
        p,
        hist_list,
        [](const SimpleHistogramType * item) -> const SimpleHistogramType& { return *item; }
        );

  MY_BOOST_CHECK_FLOATS_EQUAL(aggregated.final_histogram.params.min, p.min, tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(aggregated.final_histogram.params.max, p.max, tol);
  BOOST_CHECK_EQUAL(aggregated.final_histogram.params.num_bins, p.num_bins);
  MY_BOOST_CHECK_EIGEN_EQUAL(aggregated.final_histogram.bins, avghist.bins, tol_f);
  MY_BOOST_CHECK_EIGEN_EQUAL(aggregated.final_histogram.delta, avghist.delta, tol_f);
}

BOOST_AUTO_TEST_CASE(binning)
{
  typedef Tomographer::HistogramWithErrorBars<double, float> BaseHistogramType;
  typedef Tomographer::Histogram<double, float> SimpleHistogramType;
  typedef Tomographer::AveragedHistogram<BaseHistogramType, float> AvgHistogramType;
  typedef Tomographer::AveragedHistogram<SimpleHistogramType, float>
    SimpleAvgHistogramType;

  typename BaseHistogramType::Params p(0.0, 1.0, 4);

  BaseHistogramType hist1(p);
  hist1.load( inline_vector_4<double>(15, 45, 42, 12) ,
              inline_vector_4<float>(1, 1, 1, 1), 36 ); // sum=150
  BaseHistogramType hist2(p);
  hist2.load( inline_vector_4<double>(17, 43, 40, 18) ,
              inline_vector_4<float>(2, 2, 5, 2), 32 );
  BaseHistogramType hist3(p);
  hist3.load( inline_vector_4<double>(20, 38, 47, 10) ,
              inline_vector_4<float>(1, 2, 13, 4), 35 );
  BaseHistogramType hist4(p);
  hist4.load( inline_vector_4<double>(18, 44, 43, 13) ,
              inline_vector_4<float>(2, 1, 24, 3), 32 );

  AvgHistogramType avghist;
  avghist.reset(p);
  avghist.addHistogram(hist1);
  avghist.addHistogram(hist2);
  avghist.addHistogram(hist3);
  avghist.addHistogram(hist4);
  avghist.finalize();

  SimpleHistogramType shist1(p);
  shist1.load( inline_vector_4<double>(15, 45, 42, 12) , 36 ); // sum=150
  SimpleHistogramType shist2(p);
  shist2.load( inline_vector_4<double>(17, 43, 40, 18) , 32 );
  SimpleHistogramType shist3(p);
  shist3.load( inline_vector_4<double>(20, 38, 47, 10) , 35 );
  SimpleHistogramType shist4(p);
  shist4.load( inline_vector_4<double>(18, 44, 43, 13) , 32 );

  SimpleAvgHistogramType simpleavghist;
  simpleavghist.reset(p);
  simpleavghist.addHistogram(shist1);
  simpleavghist.addHistogram(shist2);
  simpleavghist.addHistogram(shist3);
  simpleavghist.addHistogram(shist4);
  simpleavghist.finalize();

  std::vector<const BaseHistogramType*> hist_list;
  hist_list.push_back(&hist1);
  hist_list.push_back(&hist2);
  hist_list.push_back(&hist3);
  hist_list.push_back(&hist4);

  Tomographer::AggregatedHistogramWithErrorBars<BaseHistogramType, float> aggregated
    = Tomographer::AggregatedHistogramWithErrorBars<BaseHistogramType, float>::aggregate(
        p,
        hist_list,
        [](const BaseHistogramType * item) -> const BaseHistogramType& { return *item; }
        );

  MY_BOOST_CHECK_FLOATS_EQUAL(aggregated.final_histogram.params.min, p.min, tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(aggregated.final_histogram.params.max, p.max, tol);
  BOOST_CHECK_EQUAL(aggregated.final_histogram.params.num_bins, p.num_bins);
  MY_BOOST_CHECK_EIGEN_EQUAL(aggregated.final_histogram.bins, avghist.bins, tol_f);
  MY_BOOST_CHECK_EIGEN_EQUAL(aggregated.final_histogram.delta, avghist.delta, tol_f);

  MY_BOOST_CHECK_FLOATS_EQUAL(aggregated.simple_final_histogram.params.min, p.min, tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(aggregated.simple_final_histogram.params.max, p.max, tol);
  BOOST_CHECK_EQUAL(aggregated.simple_final_histogram.params.num_bins, p.num_bins);
  MY_BOOST_CHECK_EIGEN_EQUAL(aggregated.simple_final_histogram.bins, simpleavghist.bins, tol_f);
  MY_BOOST_CHECK_EIGEN_EQUAL(aggregated.simple_final_histogram.delta, simpleavghist.delta, tol_f);
}

BOOST_AUTO_TEST_SUITE_END()

// -----------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(formatting)

BOOST_AUTO_TEST_SUITE(histogram_pretty_print)

BOOST_AUTO_TEST_CASE(basic)
{
  Tomographer::Histogram<double> hist(0.0, 1.0, 5);
  hist.load( (Eigen::VectorXi(5) << 0, 1, 4, 6, 2).finished() );

  const int max_width = 80;

  const std::string correct_str =
    "0.1000 |                                                                       0\n"
    "0.3000 |***********                                                            1\n"
    "0.5000 |*********************************************                          4\n"
    "0.7000 |******************************************************************     6\n"
    "0.9000 |**********************                                                 2\n"
    ;

  const std::string s = Tomographer::histogramPrettyPrint(hist, max_width);
  BOOST_CHECK_EQUAL(s, correct_str) ;
  const std::string s2 = hist.prettyPrint(max_width);
  BOOST_CHECK_EQUAL(s2, correct_str) ;

  std::ostringstream ss;
  Tomographer::histogramPrettyPrint(ss, hist, max_width);
  BOOST_CHECK_EQUAL(ss.str(), correct_str) ;
}
BOOST_AUTO_TEST_CASE(errbars)
{
  Tomographer::HistogramWithErrorBars<double> hist(0.0, 1.0, 5);
  // make the values small (<1) to make sure that there hasn't been a conversion to int
  // somewhere in the process
  hist.load( 0.01 * (Eigen::VectorXd(5) << 0.0, 1.0, 4.53, 6.5, 2.2).finished(),
             0.01 * (Eigen::VectorXd(5) << 0, 0.4, 0.5, 0.3, 0.15).finished() );

  const int max_width = 80;

  const std::string correct_str =
    "0.1000 ||                                                       0.0000 +- 0.0000\n"
    "0.3000 |*****|-----|                                            0.0100 +- 0.0040\n"
    "0.5000 |********************************|-------|               0.0453 +- 0.0050\n"
    "0.7000 |*************************************************|---|  0.0650 +- 0.0030\n"
    "0.9000 |****************|--|                                    0.0220 +- 0.0015\n"
    ;

  const std::string s = Tomographer::histogramPrettyPrint(hist, max_width);
  BOOST_CHECK_EQUAL(s, correct_str) ;
  const std::string s2 = hist.prettyPrint(max_width);
  BOOST_CHECK_EQUAL(s2, correct_str) ;

  std::ostringstream ss;
  Tomographer::histogramPrettyPrint(ss, hist, max_width);
  BOOST_CHECK_EQUAL(ss.str(), correct_str) ;
}
BOOST_AUTO_TEST_CASE(mednumbers)
{
  Tomographer::HistogramWithErrorBars<double> hist(0.0, 1.0, 5);
  // make the values small (<1) to make sure that there hasn't been a conversion to int
  // somewhere in the process
  hist.load( (Eigen::VectorXd(5) << 0, 181.6, 427.3, 1051.4, 1394.8).finished(),
             (Eigen::VectorXd(5) << 0, 32.069439, 46.908433, 46.468675, 32.718598).finished() );

  const int max_width = 80;

  const std::string correct_str =
    "0.1000 ||                                                             0 +-     0\n"
    "0.3000 |******|-|                                                   182 +-    32\n"
    "0.5000 |***************|---|                                        427 +-    47\n"
    "0.7000 |***************************************|---|               1051 +-    46\n"
    "0.9000 |*****************************************************|-|   1395 +-    33\n"
    ;

  const std::string s = Tomographer::histogramPrettyPrint(hist, max_width);
  BOOST_CHECK_EQUAL(s, correct_str) ;
  const std::string s2 = hist.prettyPrint(max_width);
  BOOST_CHECK_EQUAL(s2, correct_str) ;

  std::ostringstream ss;
  Tomographer::histogramPrettyPrint(ss, hist, max_width);
  BOOST_CHECK_EQUAL(ss.str(), correct_str) ;
}
BOOST_AUTO_TEST_CASE(largenumbers)
{
  Tomographer::HistogramWithErrorBars<double> hist(0.0, 1.0, 5);
  // make the values small (<1) to make sure that there hasn't been a conversion to int
  // somewhere in the process
  hist.load( (Eigen::VectorXd(5) << 0, 100033.931, 4538205.111, 6501842.882, 221045.155).finished(),
             (Eigen::VectorXd(5) << 0, 40342.111, 51044.209, 30114.315, 90104.919).finished() );

  const int max_width = 80;

  const std::string correct_str =
    "0.1000 ||                                                             0 +-     0\n"
    "0.3000 |||                                                       100034 +- 40342\n"
    "0.5000 |*************************************||                 4538205 +- 51044\n"
    "0.7000 |*****************************************************|  6501843 +- 30114\n"
    "0.9000 |*|-|                                                     221045 +- 90105\n"
    ;

  const std::string s = Tomographer::histogramPrettyPrint(hist, max_width);
  BOOST_CHECK_EQUAL(s, correct_str) ;
  const std::string s2 = hist.prettyPrint(max_width);
  BOOST_CHECK_EQUAL(s2, correct_str) ;

  std::ostringstream ss;
  Tomographer::histogramPrettyPrint(ss, hist, max_width);
  BOOST_CHECK_EQUAL(ss.str(), correct_str) ;
}

BOOST_AUTO_TEST_SUITE(nobug)

BOOST_AUTO_TEST_CASE(toolargeerrbar)
{
  Tomographer::HistogramWithErrorBars<double> hist(-2.0, 2.0, 2);
  // make the values small (<1) to make sure that there hasn't been a conversion to int
  // somewhere in the process
  hist.load( (Eigen::VectorXd(2) << 3.0, 4.0).finished() ,
             (Eigen::VectorXd(2) << 4.0, 2.0).finished() );

  const int max_width = 80;

  const std::string correct_str =
    "-1.000 ||--------------------------------------------------------|  3.00 +- 4.00\n"
    " 1.000 |*****************|--------------------------------|         4.00 +- 2.00\n"
    ;

  const std::string s = Tomographer::histogramPrettyPrint(hist, max_width);
  BOOST_CHECK_EQUAL(s, correct_str) ;

  const std::string s2 = hist.prettyPrint(max_width);
  BOOST_CHECK_EQUAL(s2, correct_str) ;

  std::ostringstream ss;
  Tomographer::histogramPrettyPrint(ss, hist, max_width);
  BOOST_CHECK_EQUAL(ss.str(), correct_str) ;
}
BOOST_AUTO_TEST_CASE(withinf)
{
  Tomographer::HistogramWithErrorBars<double> hist(-2.0, 2.0, 2);
  // make the values small (<1) to make sure that there hasn't been a conversion to int
  // somewhere in the process
  hist.load( (Eigen::VectorXd(2) << 3.0, std::numeric_limits<double>::infinity()).finished(),
             (Eigen::VectorXd(2) << 4.0, 1.0).finished() );

  const int max_width = 80;

  const std::string correct_str =
    "-1.000 ||--------------------------------------------------------|  3.00 +- 4.00\n"
    " 1.000 ||                                                            inf +- 1.00\n"
    ;

  const std::string s = Tomographer::histogramPrettyPrint(hist, max_width);
  BOOST_CHECK_EQUAL(s, correct_str) ;
  const std::string s2 = hist.prettyPrint(max_width);
  BOOST_CHECK_EQUAL(s2, correct_str) ;
  std::ostringstream ss;
  Tomographer::histogramPrettyPrint(ss, hist, max_width);
  BOOST_CHECK_EQUAL(ss.str(), correct_str) ;
}
BOOST_AUTO_TEST_CASE(withinf2)
{
  Tomographer::HistogramWithErrorBars<double> hist(-2.0, 2.0, 2);
  // make the values small (<1) to make sure that there hasn't been a conversion to int
  // somewhere in the process
  hist.load( (Eigen::VectorXd(2) << 3.0, 2.0).finished() ,
             (Eigen::VectorXd(2) << 4.0, std::numeric_limits<double>::infinity()).finished() );

  const int max_width = 80;

  const std::string correct_str =
    "-1.000 ||--------------------------------------------------------|  3.00 +- 4.00\n"
    " 1.000 ||                                                           2.00 +-  inf\n"
    ;

  const std::string s = Tomographer::histogramPrettyPrint(hist, max_width);
  BOOST_CHECK_EQUAL("\n"+s, "\n"+correct_str) ;
  const std::string s2 = hist.prettyPrint(max_width);
  BOOST_CHECK_EQUAL(s2, correct_str) ;
  std::ostringstream ss;
  Tomographer::histogramPrettyPrint(ss, hist, max_width);
  BOOST_CHECK_EQUAL(ss.str(), correct_str) ;
}
BOOST_AUTO_TEST_CASE(withnan)
{
  Tomographer::HistogramWithErrorBars<double> hist(-2.0, 2.0, 2);
  // make the values small (<1) to make sure that there hasn't been a conversion to int
  // somewhere in the process
  hist.load( (Eigen::VectorXd(2) << 3.0, std::numeric_limits<double>::quiet_NaN()).finished() ,
             (Eigen::VectorXd(2) << 4.0, 1.0).finished() );

  const int max_width = 80;

  const std::string correct_str =
    "-1.000 ||--------------------------------------------------------|  3.00 +- 4.00\n"
    " 1.000 ||                                                            nan +- 1.00\n"
    ;

  const std::string s = Tomographer::histogramPrettyPrint(hist, max_width);
  BOOST_CHECK_EQUAL("\n"+s, "\n"+correct_str) ;
  const std::string s2 = hist.prettyPrint(max_width);
  BOOST_CHECK_EQUAL(s2, correct_str) ;
  std::ostringstream ss;
  Tomographer::histogramPrettyPrint(ss, hist, max_width);
  BOOST_CHECK_EQUAL(ss.str(), correct_str) ;
}
BOOST_AUTO_TEST_CASE(withnan2)
{
  Tomographer::HistogramWithErrorBars<double> hist(-2.0, 2.0, 2);
  // make the values small (<1) to make sure that there hasn't been a conversion to int
  // somewhere in the process
  hist.load( (Eigen::VectorXd(2) << 3.0, 2.0).finished() ,
             (Eigen::VectorXd(2) << 4.0, std::numeric_limits<double>::quiet_NaN()).finished() );

  const int max_width = 80;

  const std::string correct_str =
    "-1.000 ||--------------------------------------------------------|  3.00 +- 4.00\n"
    " 1.000 ||                                                           2.00 +-  nan\n"
    ;

  const std::string s = Tomographer::histogramPrettyPrint(hist, max_width);
  BOOST_CHECK_EQUAL("\n"+s, "\n"+correct_str) ;
  const std::string s2 = hist.prettyPrint(max_width);
  BOOST_CHECK_EQUAL(s2, correct_str) ;
  std::ostringstream ss;
  Tomographer::histogramPrettyPrint(ss, hist, max_width);
  BOOST_CHECK_EQUAL(ss.str(), correct_str) ;
}

BOOST_AUTO_TEST_SUITE_END() // nobug

BOOST_AUTO_TEST_SUITE_END() // histogram_pretty_print

BOOST_AUTO_TEST_CASE(histogram_short_bar)
{
  Tomographer::HistogramWithErrorBars<double> hist(0.0, 1.0, 5);
  // make the values small (<1) to make sure that there hasn't been a conversion to int
  // somewhere in the process
  hist.load( 0.01*(Eigen::VectorXd(5) << 0.0, 1.0, 4.53, 6.5, 2.2).finished(),
             0.01*(Eigen::VectorXd(5) << 0, 0.4, 0.5, 0.3, 0.15).finished() );

  const int max_width = 80;

  const std::string correct_str = "0| -x#+|1";

  const std::string s = Tomographer::histogramShortBar(hist, false, max_width);
  BOOST_CHECK_EQUAL(s, correct_str) ;

  std::ostringstream ss;
  Tomographer::histogramShortBar(ss, hist, false, max_width);
  BOOST_CHECK_EQUAL(ss.str(), correct_str) ;
}

BOOST_AUTO_TEST_CASE(histogram_short_bar_log)
{
  Tomographer::HistogramWithErrorBars<double> hist(0.0, 1.0, 5);
  hist.load( 0.01*(Eigen::VectorXd(5) << 0.0, 1.0, 4.53, 6.5, 2.2).finished(),
             0.01*(Eigen::VectorXd(5) << 0, 0.4, 0.5, 0.3, 0.15).finished() );

  const int max_width = 80;

  const std::string correct_str = "0| .++-|1";

  const std::string s = Tomographer::histogramShortBar(hist, true, max_width);
  BOOST_CHECK_EQUAL(s, correct_str) ;

  std::ostringstream ss;
  Tomographer::histogramShortBar(ss, hist, true, max_width);
  BOOST_CHECK_EQUAL(ss.str(), correct_str) ;
}

BOOST_AUTO_TEST_SUITE_END(); // formatting

// =============================================================================
BOOST_AUTO_TEST_SUITE_END();


#include "modelvisu.h"

#ifndef GLM_ENABLE_EXPERIMENTAL
#define GLM_ENABLE_EXPERIMENTAL
#endif
#include <glm/gtx/euler_angles.hpp>

#include "constants.h"
#include "recenter.h"

#include <iostream>

// eigen stuf
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include "barycenter.h"

ModelVisu::ModelVisu( QWidget *parent )
    : QOpenGLWidget( parent )
{
    int major = 3;
    int minor = 2;

    QSurfaceFormat format;

    format.setDepthBufferSize( 24 );
    format.setStencilBufferSize( 8 );
    format.setVersion( major, minor );
    format.setProfile( QSurfaceFormat::CoreProfile );

    setFormat( format );

    create();
}

void ModelVisu::initializeGL()
{
    glewExperimental = GL_TRUE;
    GLenum initGlew{glewInit()};

    if ( initGlew != GLEW_OK )
    {
        throw std::runtime_error(
            reinterpret_cast<const char *>( glewGetErrorString( initGlew ) ) );
    }

    simpleShader_.setVertexShader( "shader/color.vert" );
    simpleShader_.setFragmentShader( "shader/color.frag" );
    try
    {
        simpleShader_.Load();
    }
    catch ( std::exception const &e )
    {
        std::cerr << e.what() << "\n";
    }
    catch ( std::string const &errorStr )
    {
        std::cerr << errorStr << "\n";
    }
    catch ( ... )
    {
        std::cerr << "unknown exception\n";
    }

    glClearColor( .0f, 0.f, 0.f, .0f );

    computeCloudAndPca_();

#if 0
    ellipsoid.setScale( glm::vec3{2.f, 1.f, 3.f} );
    ellipsoid.setCenter( glm::vec3{-5.f, 0.f, 0.f} );
    ellipsoid.setOrientation( transformation_.rotate( glm::vec3{0.f, 1.f, 0.f}, M_PI / 6.f ) );
    cloud_ = ellipsoid.computeTransform();
    cloud_.generateCloud();

#endif
    look_ = transformation_.lookAt( glm::vec3{0.f, -10.f, 0.f}, glm::vec3{0.f, 0.f, 0.f},
                                    glm::vec3{0.f, 0.f, 1.f} );

#if 0

    cloudNode_ = std::make_shared<MeshNode<Nuage>>( cloud_ );
    cloud_.refreshBuffer();

    cloudNode_->updateVertexBuffer();

    pca.computePca( std::cbegin( cloud_ ), std::cend( cloud_ ) );

    std::cout << "Eigen values are: \n";
    auto const &eigenValues = pca.eigenValues();
    std::cout << eigenValues << "\n";

    std::cout << "Eigen vectors are: \n";
    auto const &eigenVector = pca.eigenVectors();
    std::cout << eigenVector << "\n";
#endif
}

void ModelVisu::setCenterX( double x )
{
    center_[ dirX ] = static_cast<float>( x );
}
void ModelVisu::setCenterY( double y )
{
    center_[ dirY ] = static_cast<float>( y );
}
void ModelVisu::setCenterZ( double z )
{
    center_[ dirZ ] = static_cast<float>( z );
}

void ModelVisu::setScaleX( double a )
{
    scale_[ dirX ] = 2.f * a;
}
void ModelVisu::setScaleY( double b )
{
    scale_[ dirY ] = 2.f * b;
}
void ModelVisu::setScaleZ( double c )
{
    scale_[ dirZ ] = 2.f * c;
}

void ModelVisu::setRotateAxisX( double x )
{
    rotateAxis_[ dirX ] = static_cast<float>( x );
}
void ModelVisu::setRotateAxisY( double y )
{
    rotateAxis_[ dirY ] = static_cast<float>( y );
}
void ModelVisu::setRotateAxisZ( double z )
{
    rotateAxis_[ dirZ ] = static_cast<float>( z );
}

void ModelVisu::setRotateAngle( double theta )
{
    rotateAngle_ = static_cast<float>( theta );
}

void ModelVisu::setNpoints( int nPoint )
{
    nPoints_ = nPoint;
}

void ModelVisu::setDmin( double dmin )
{
    dmin_ = static_cast<float>( dmin );
}

void ModelVisu::computeCloud()
{
    makeCurrent();
    computeCloudAndPca_();
    doneCurrent();
    update();
}

void ModelVisu::setCamPos1X( double x )
{
    pos1_[ dirX ] = static_cast<float>( x );
}
void ModelVisu::setCamPos1Y( double y )
{
    pos1_[ dirY ] = static_cast<float>( y );
}
void ModelVisu::setCamPos1Z( double z )
{
    pos1_[ dirZ ] = static_cast<float>( z );
}

void ModelVisu::setCamTarget1X( double x )
{
    target1_[ dirX ] = static_cast<float>( x );
}
void ModelVisu::setCamTarget1Y( double y )
{
    target1_[ dirY ] = static_cast<float>( y );
}
void ModelVisu::setCamTarget1Z( double z )
{
    target1_[ dirZ ] = static_cast<float>( z );
}

void ModelVisu::setCamUp1X( double x )
{
    up1_[ dirX ] = static_cast<float>( x );
}
void ModelVisu::setCamUp1Y( double y )
{
    up1_[ dirY ] = static_cast<float>( y );
}
void ModelVisu::setCamUp1Z( double z )
{
    up1_[ dirZ ] = static_cast<float>( z );
}

void ModelVisu::computeCam1()
{
    look1_ = transformation_.lookAt( pos1_, target1_, up1_ );
}

void ModelVisu::showCam1()
{
    look_ = look1_;
    update();
}

void ModelVisu::computeCloudAndPca_()
{
    ellipsoid.setScale( scale_ );
    ellipsoid.setCenter( center_ );
    ellipsoid.setOrientation(
        transformation_.rotate( glm::normalize( rotateAxis_ ), rotateAngle_ ) );

    ellipsoid.computeCloud( nPoints_, dmin_ );

    cloud_ = ellipsoid.computeTransform();
    cloud_.generateCloud();

    cloudNode_ = std::make_shared<MeshNode<Nuage>>( cloud_ );
    cloud_.refreshBuffer();

    cloudNode_->updateVertexBuffer();

    pca.computePca( std::cbegin( cloud_ ), std::cend( cloud_ ) );

    std::cout << "Eigen values are: \n";
    auto const &eigenValues = pca.eigenValues();
    std::cout << eigenValues << "\n";

    std::cout << "Eigen vectors are: \n";
    auto const &eigenVector = pca.eigenVectors();
    std::cout << eigenVector << "\n";
}

void ModelVisu::resizeGL( int width, int height )
{
    //
    float near = 0.01;
    float far  = 100;
    float fov  = 70.;

    projection_
        = transformation_.perspective( fov, static_cast<float>( width ) / height, near, far );
}

void ModelVisu::paintGL()
{
    //
    glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

    glEnable( GL_DEPTH_TEST );

    simpleShader_.Enable();

    glm::mat4 mvp = projection_ * look_;

    auto mvpLoc = simpleShader_.GetUniformLocation( "MVP" );

    glUniformMatrix4fv( mvpLoc, 1, GL_FALSE, glm::value_ptr( mvp ) );

    cloudNode_->drawPoints();

    simpleShader_.Disable();
}


#ifndef BOOST_SAFE_FLOAT_POLICY_CHECK_MULTIPLICATION_OVERFLOW_HPP
#define BOOST_SAFE_FLOAT_POLICY_CHECK_MULTIPLICATION_OVERFLOW_HPP

#include <boost/safe_float/policy/check_base_policy.hpp>

#ifdef FENV_AVAILABLE
#pragma STDC FENV_ACCESS ON
#include <fenv.h>
#endif

namespace boost {
namespace safe_float{
namespace policy{

template<class FP>
class check_multiplication_overflow : public check_policy<FP> {
    bool precond=true;
public:
    bool pre_multiplication_check(const FP& lhs, const FP& rhs){
#ifndef FENV_AVAILABLE
        precond = std::isinf(lhs) || std::isinf(rhs);
        return true;
#else
        return ! std::feclearexcept(FE_OVERFLOW);
#endif
    }
    bool post_multiplication_check(const FP& rhs){
#ifndef FENV_AVAILABLE
        return precond || ! std::isinf(rhs);
#else
        return ! std::fetestexcept(FE_OVERFLOW);
#endif
    }
    std::string multiplication_failure_message(){
        return std::string("Overflow to infinite on multiplication operation");
    }
};

}
}
}

#endif // BOOST_SAFE_FLOAT_POLICY_CHECK_MULTIPLICATION_OVERFLOW_HPP


#pragma once
#include <Eigen/Core>

namespace patchgen {

    struct Permutation {
        int num_sides;  // number of sides
        int id;         // 0 <= id <=   num_sides-1 : permutation with original order and offset id
        // n <= id <= 2*num_sides-1 : permutation with reversed order and offset id-num_sides

        Permutation() {
            init(0);
        }
        void init(int num_sides_, int id_ = 0) {
            num_sides = num_sides_;
            id = id_;
        }
        bool is_flipped() const {
            return id >= num_sides;
        }
        bool operator==(const Permutation& rhs) const {
            return num_sides == rhs.num_sides && id == rhs.id;
        }
        // loop utility
        void next(bool is_forward = true) {
            id += is_forward ? 1 : -1;
        }
        bool is_valid() const {
            return 0 <= id && id < 2 * num_sides;
        }
        // accessor
        int operator[](int index) const {
            return id < num_sides ? ((index + id) % num_sides)
                : (num_sides - 1 - (index + id - num_sides) % num_sides);
        }
        Eigen::VectorXi operator()(const Eigen::VectorXi& l) const {
            Eigen::VectorXi l_permuted(num_sides);
            for (int i = 0; i < num_sides; ++i)
                l_permuted[i] = l[this->operator[](i)];
            return l_permuted;
        }
    };

}