//  (C) Copyright Eric Niebler 2005.
//  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)

// Test case for pot_quantile.hpp (weighted feature)

#define BOOST_NUMERIC_FUNCTIONAL_STD_COMPLEX_SUPPORT
#define BOOST_NUMERIC_FUNCTIONAL_STD_VALARRAY_SUPPORT
#define BOOST_NUMERIC_FUNCTIONAL_STD_VECTOR_SUPPORT

#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

using namespace boost;
using namespace unit_test;
using namespace boost::accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // tolerance in %
    double epsilon = 1.;

    double mu1, mu2, l;

    mu1 = 1.;
    mu2 = -1.;
    l = 0.5;

    // two random number generators
    boost::lagged_fibonacci607 rng;
    boost::normal_distribution<> mean_sigma1(mu1,1);
    boost::normal_distribution<> mean_sigma2(mu2,1);
    boost::exponential_distribution<> lambda(l);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal1(rng, mean_sigma1);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal2(rng, mean_sigma2);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::exponential_distribution<> > exponential(rng, lambda);

    accumulator_set<double, stats<tag::weighted_pot_quantile<right>(with_threshold_value)>, double > acc1(
        pot_threshold_value = 3.
    );
    accumulator_set<double, stats<tag::weighted_pot_quantile<right>(with_threshold_probability)>, double > acc2(
        right_tail_cache_size = 10000
      , pot_threshold_probability = 0.99
    );
    accumulator_set<double, stats<tag::weighted_pot_quantile<left>(with_threshold_value)>, double > acc3(
        pot_threshold_value = -3.
    );
    accumulator_set<double, stats<tag::weighted_pot_quantile<left>(with_threshold_probability)>, double > acc4(
        left_tail_cache_size = 10000
      , pot_threshold_probability = 0.01
    );

    accumulator_set<double, stats<tag::weighted_pot_quantile<right>(with_threshold_value)>, double > acc5(
        pot_threshold_value = 5.
    );
    accumulator_set<double, stats<tag::weighted_pot_quantile<right>(with_threshold_probability)>, double > acc6(
        right_tail_cache_size = 10000
      , pot_threshold_probability = 0.995
    );

    for (std::size_t i = 0; i < 100000; ++i)
    {
        double sample1 = normal1();
        double sample2 = normal2();
        acc1(sample1, weight = std::exp(-mu1 * (sample1 - 0.5 * mu1)));
        acc2(sample1, weight = std::exp(-mu1 * (sample1 - 0.5 * mu1)));
        acc3(sample2, weight = std::exp(-mu2 * (sample2 - 0.5 * mu2)));
        acc4(sample2, weight = std::exp(-mu2 * (sample2 - 0.5 * mu2)));
    }

    for (std::size_t i = 0; i < 100000; ++i)
    {
        double sample = exponential();
        acc5(sample, weight = 1./l * std::exp(-sample * (1. - l)));
        acc6(sample, weight = 1./l * std::exp(-sample * (1. - l)));
    }

    BOOST_CHECK_CLOSE( quantile(acc1, quantile_probability = 0.999), 3.090232, epsilon );
    BOOST_CHECK_CLOSE( quantile(acc2, quantile_probability = 0.999), 3.090232, epsilon );
    BOOST_CHECK_CLOSE( quantile(acc3, quantile_probability = 0.001), -3.090232, epsilon );
    BOOST_CHECK_CLOSE( quantile(acc4, quantile_probability = 0.001), -3.090232, epsilon );

    BOOST_CHECK_CLOSE( quantile(acc5, quantile_probability = 0.999), 6.908, epsilon );
    BOOST_CHECK_CLOSE( quantile(acc6, quantile_probability = 0.999), 6.908, epsilon );
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("weighted_pot_quantile test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2015 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <iostream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"

#include "fem/assemble.hh"
#include "fem/norms.hh"
#include "fem/lagrangespace.hh"
//#include "fem/hierarchicspace.hh"   // ContinuousHierarchicMapper
#include "linalg/direct.hh"
#include "linalg/trivialpreconditioner.hh"
//#include "linalg/partialDirectPreconditioner.hh"
#include "linalg/additiveschwarz.hh"
#include "linalg/iluprecond.hh"      // PrecondType::ILUT, PrecondType::ILUK, PrecondType::ARMS
#include "linalg/iccprecond.hh"
#include "linalg/icc0precond.hh"
#include "linalg/hyprecond.hh"       // BoomerAMG, Euclid
#include "linalg/jacobiPreconditioner.hh"
#include "linalg/cg.hh"
#include "mg/hb.hh"
#include "utilities/enums.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare, createUnitCube
#include "io/vtk.hh"
#include "io/gnuplot.hh"
//#include "io/amira.hh"
#include "utilities/kaskopt.hh"

using namespace Kaskade;
#include "ht.hh"

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

  std::cout << "Start heat transfer tutorial program" << std::endl;

  boost::timer::cpu_timer totalTimer;

  int verbosityOpt = 1;
  bool dump = true;
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  int  refinements = getParameter(pt, "refinements", 5),
       order       =  getParameter(pt, "order", 2),
       verbosity   = getParameter(pt, "verbosity", 1);
  std::cout << "original mesh shall be refined : " << refinements << " times" << std::endl;
  std::cout << "discretization order           : " << order << std::endl;
  std::cout << "output level (verbosity)       : " << verbosity << std::endl;

  int  direct, onlyLowerTriangle = false;

  DirectType directType;
//  IterateType iterateType = IterateType::CG;
  MatrixProperties property = MatrixProperties::SYMMETRIC;
  PrecondType precondType = PrecondType::NONE;
  std::string empty;

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  direct = getParameter(pt, s, 0);

  s = "names.direct." + getParameter(pt, "solver.direct", empty);
  directType = static_cast<DirectType>(getParameter(pt, s, 0));

//  s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
//  iterateType = static_cast<IterateType>(getParameter(pt, s, 0));
  s = "names.preconditioner." + getParameter(pt, "solver.preconditioner", empty);
  precondType = static_cast<PrecondType>(getParameter(pt, s, 0));

  property = MatrixProperties::SYMMETRIC;

  if ( (directType == DirectType::MUMPS)||(directType == DirectType::PARDISO) || ( (precondType == PrecondType::ICC) && !direct ) )
  {
    onlyLowerTriangle = true;
    std::cout <<
      "Note: direct solver MUMPS/PARADISO or PrecondType::ICC preconditioner ===> onlyLowerTriangle is set to true!"
      << std::endl;
  }

  boost::timer::cpu_timer gridTimer;
//   two-dimensional space: dim=2
  constexpr int dim=2;
  using Grid = Dune::UGGrid<dim>;
  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> >;
  // using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<double,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  using VariableDescriptions = boost::fusion::vector<Variable<SpaceIndex<0>,Components<1>,VariableId<0> > >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = HeatFunctional<double,VariableSet>;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  constexpr int neq = Functional::TestVars::noOfVariables;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<0,neq>::type;
  using LinearSpace = VariableSet::CoefficientVectorRepresentation<0,neq>::type;

  GridManager<Grid> gridManager( createUnitSquare<Grid>() );
  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " triangles, " << std::endl;
  std::cout << "      " << gridManager.grid().size(1) << " edges, " << std::endl;
  std::cout << "      " << gridManager.grid().size(2) << " points" << std::endl;
  std::cout << "computing time for generation of initial mesh: " << (double)(gridTimer.elapsed().user)/1e9 << "s\n";

  // construction of finite element space for the scalar solution T.
  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),order);

  Spaces spaces(&temperatureSpace);

  // construct variable list.
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable

  std::string varNames[1] = { "u" };

  VariableSet variableSet(spaces,varNames);

  // construct variational functional

  double kappa = 1.0;
  double q = 1.0;
  Functional F(kappa,q);
  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  std::cout << std::endl << "no of variables = " << nvars << std::endl;
  std::cout << "no of equations = " << neq   << std::endl;
  size_t dofs = variableSet.degreesOfFreedom(0,nvars);
  std::cout << "number of degrees of freedom = " << dofs   << std::endl;

  //construct Galerkin representation

  Assembler assembler(gridManager,spaces);
  VariableSet::VariableSet u(variableSet);
  VariableSet::VariableSet du(variableSet);

  size_t nnz = assembler.nnz(0,neq,0,nvars,onlyLowerTriangle);
  std::cout << "number of nonzero elements in the stiffness matrix: " << nnz << std::endl << std::endl;
  boost::timer::cpu_timer assembTimer;

  CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<0,neq>::init(spaces));
  solution = 0;

  assembler.assemble(linearization(F,u));
  CoefficientVectors rhs(assembler.rhs());
  AssembledGalerkinOperator<Assembler,0,neq,0,nvars> A(assembler, onlyLowerTriangle);
  MatrixAsTriplet<double> tri = A.get<MatrixAsTriplet<double> >();
  std::cout << "computing time for assemble: " << (double)(assembTimer.elapsed().user)/1e9 << "s\n";

//     for (k=0; k< nnz; k++)
//       {
//         printf("%3d %3d %e\n", tri.ridx[k], tri.cidx[k], tri.data[k]);
//       }

  if (direct)
  {
    boost::timer::cpu_timer directTimer;
    directInverseOperator(A,directType,property).applyscaleadd(-1.0,rhs,solution);
    u.data = solution.data;
    std::cout << "computing time for direct solve: " << (double)(directTimer.elapsed().user)/1e9 << "s\n";
  }
  else
  {
    boost::timer::cpu_timer iteTimer;
    Dune::InverseOperatorResult res;
    const DefaultDualPairing<LinearSpace,LinearSpace> defaultScalarProduct{};
    int iteSteps = getParameter(pt, "solver.iteMax", 2000);
    double iteEps = getParameter(pt, "solver.iteEps", 1.0e-10);
    StrakosTichyPTerminationCriterion<double> termination(iteEps,iteSteps);
    int lookAhead;
    switch (precondType)
    {
      case PrecondType::NONE:
      case PrecondType::HB:   lookAhead=50; break;
      default:                lookAhead=3; break;
    }
    lookAhead = getParameter(pt, "solver.lookAhead", lookAhead);
    termination.setLookAhead(lookAhead);

    switch (precondType)
    {
      case PrecondType::NONE:
      {
        std::cout << "selected preconditioner: NONE" << std::endl;
        TrivialPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > trivial;
        CG<LinearSpace,LinearSpace> cg(A,trivial,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ADDITIVESCHWARZ:
      {
        std::cout << "selected preconditioner: ADDITIVESCHWARZ" << std::endl;
        std::pair<size_t,size_t> idx = temperatureSpace.mapper().globalIndexRange(gridManager.grid().leafIndexSet().geomTypes(dim)[0]);
        AdditiveSchwarzPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > addschwarz(A,idx.first,idx.second,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,addschwarz,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ILUT:
      {
        std::cout << "selected preconditioner: ILUT" << std::endl;
//        std::cout << "Note that this preconditioner combined with the BICGSTAB solver" << std::endl;
        std::cout << "needs matrix.property = GENERAL" << std::endl;
        int lfil = getParameter(pt, "solver.ILUT.lfil", 140);
        double dropTol = getParameter(pt, "solver.ILUT.dropTol", 0.01);
        ILUTPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > ilut(A,lfil,dropTol,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,ilut,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
//         Dune::BiCGSTABSolver<LinearSpace> cg(A,ilut,iteEps,iteSteps,verbosity);
//         cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ILUK:
      {
        std::cout << "selected preconditioner: ILUK" << std::endl;
//        std::cout << "Note that this preconditioner combined with the BICGSTAB solver" << std::endl;
        std::cout << "needs matrix.property = GENERAL" << std::endl;
        int fill_lev = getParameter(pt, "solver.ILUK.fill_lev", 3);
        ILUKPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > iluk(A,fill_lev,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,iluk,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
//         Dune::BiCGSTABSolver<LinearSpace> cg(A,iluk,iteEps,iteSteps,verbosity);
//         cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ARMS:
      {
        int lfil = getParameter(pt, "solver.ARMS.lfil", 140);
        int lev_reord = getParameter(pt, "solver.ARMS.lev_reord", 1);
        double dropTol = getParameter(pt, "solver.ARMS.dropTol", 0.01);
        double tolind = getParameter(pt, "solver.ARMS.tolind", 0.2);
        ARMSPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > iluk(A,lfil,dropTol,lev_reord,tolind,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,iluk,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ICC:
      {
        std::cout << "selected preconditioner: ICC" << std::endl;
        if (property != MatrixProperties::SYMMETRIC)
        {
          std::cout << "PrecondType::ICC preconditioner of TAUCS lib has to be used with matrix.property==MatrixProperties::SYMMETRIC\n";
          std::cout << "i.e., call the executable with option --solver.property MatrixProperties::SYMMETRIC\n\n";
        }
        double dropTol = getParameter(pt, "solver.ICC.dropTol", 0.01);;
        ICCPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > icc(A,dropTol);
        CG<LinearSpace,LinearSpace> cg(A,icc,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::ICC0:
      {
        std::cout << "selected preconditioner: ICC0" << std::endl;
        ICC_0Preconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > icc0(A);
        CG<LinearSpace,LinearSpace> cg(A,icc0,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::HB:
      {
        std::cout << "selected preconditioner: HB" << std::endl;
        HierarchicalBasisPreconditioner<Grid,AssembledGalerkinOperator<Assembler,0,neq,0,nvars>::range_type, AssembledGalerkinOperator<Assembler,0,neq,0,nvars>::range_type > hb(gridManager.grid());
        CG<LinearSpace,LinearSpace> cg(A,hb,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::BOOMERAMG:
      {
        int steps = getParameter(pt, "solver.BOOMERAMG.steps", iteSteps);
        int coarsentype = getParameter(pt, "solver.BOOMERAMG.coarsentype", 21);
        int interpoltype = getParameter(pt, "solver.BOOMERAMG.interpoltype", 0);
        int cycleType = getParameter(pt, "solver.BOOMERAMG.cycleType", 1);
        int relaxType = getParameter(pt, "solver.BOOMERAMG.relaxType", 3);
        int variant = getParameter(pt, "solver.BOOMERAMG.variant", 0);
        int overlap = getParameter(pt, "solver.BOOMERAMG.overlap", 1);
        double tol = getParameter(pt, "solver.BOOMERAMG.tol", iteEps);
        double strongThreshold = getParameter(pt, "solver.BOOMERAMG.strongThreshold", (dim==2)?0.25:0.6);
        BoomerAMG<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> >
                  boomerAMGPrecon(A,steps,coarsentype,interpoltype,tol,cycleType,relaxType,
                  strongThreshold,variant,overlap,1,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,boomerAMGPrecon,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
//         Dune::LoopSolver<LinearSpace> cg(A,boomerAMGPrecon,iteEps,iteSteps,verbosity);
//         cg.apply(solution,rhs,res);
      }
      break;
      case PrecondType::EUCLID:
      {
        std::cout << "selected preconditioner: EUCLID" << std::endl;
        int level      = getParameter(pt, "solver.EUCLID.level",1);
        double droptol = getParameter(pt, "solver.EUCLID.droptol",0.01);
        int printlevel = 0;
        if (verbosity>2) printlevel=verbosity-2;
        printlevel = getParameter(pt,"solver.EUCLID.printlevel",printlevel);
        int bj = getParameter(pt, "solver.EUCLID.bj",0);
        Euclid<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > EuclidPrecon(A,level,droptol,printlevel,bj,verbosity);
        CG<LinearSpace,LinearSpace> cg(A,EuclidPrecon,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
     }
      break;
      case PrecondType::JACOBI:
      default:
      {
        std::cout << "selected preconditioner: JACOBI" << std::endl;
        JacobiPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > jacobi(A,1.0);
        CG<LinearSpace,LinearSpace> cg(A,jacobi,defaultScalarProduct,termination,verbosity);
        cg.apply(solution,rhs,res);
      }
      break;
    }
    solution *= -1.0;
    u.data = solution.data;

    std::cout << "iterative solve eps= " << iteEps << ": "
              << (res.converged?"converged":"failed") << " after "
              << res.iterations << " steps, rate="
              << res.conv_rate << ", computing time=" << (double)(iteTimer.elapsed().user)/1e9 << "s\n";
  }

  // compute L2 norm of the solution
  boost::timer::cpu_timer outputTimer;
  L2Norm l2Norm;
  std::cout << "L2norm(solution) = " << l2Norm(boost::fusion::at_c<0>(u.data)) << std::endl;

  // output of solution in VTK format for visualization,
  // the data are written as ascii stream into file temperature.vtu,
  // possible is also binary
  writeVTKFile(u,"temperature",IoOptions().setOrder(order));
  std::cout << "graphical output finished, data in VTK format is written into file temperature.vtu \n";
  IoOptions gnuplotOptions{};
  //    gnuplotOptions.info = IoOptions::none; // or IoOptions::summary or IoOptions::detail
  writeGnuplotFile(u,"temperature",gnuplotOptions);
  std::cout << "graphical output finished, Gnuplot data are written into file temperature.data \n";

  // output of solution for Amira visualization,
  // the data are written in binary format into file temperature.am,
  // possible is also ascii
  // IoOptions options;
  // options.outputType = IoOptions::ascii;
  // LeafView leafGridView = gridManager.grid().leafGridView();
  // writeAMIRAFile(leafGridView,variableSet,u,"temperature",options);

  std::cout << "computing time for output: " << (double)(outputTimer.elapsed().user)/1e9 << "s\n";

  std::cout << "total computing time: " << (double)(totalTimer.elapsed().user)/1e9 << "s\n";
  std::cout << "End heat transfer tutorial program" << std::endl;
}


#include <stdio.h>
#include <iostream>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/types/slam2d/types_slam2d.h>
#include <Eigen/Core>

typedef struct{
    int s, e;
    Eigen::Vector2d pose;
} Edge;
typedef g2o::BlockSolver< g2o::BlockSolverTraits<2, 2> >  SlamBlockSolver;
typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;

int main(int argc, const char * argv[]) {

    std::vector<Edge> edgeData = {
        {0, 1, {1, 1}},
        {1, 2, {1, -1}},
        {2, 3, {-1, -1}},
        {3, 0, {-0.5, 0.5}},
    };

    std::unique_ptr<SlamLinearSolver> linearSolver = g2o::make_unique<SlamLinearSolver>();
    linearSolver->setBlockOrdering(false);
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<SlamBlockSolver>(std::move(linearSolver)));

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    auto maxEdge = std::max_element(edgeData.begin(), edgeData.end(), [](const Edge& a, const Edge& b){
        return std::max(a.e, a.s) < std::max(b.e, b.s);
    });
    int maxIndex = std::max(maxEdge->s, maxEdge->e);

    for(int i = 0; i < maxIndex+1; i++){
        g2o::VertexPointXY *v = new g2o::VertexPointXY();
        v->setId(i);
        v->setEstimate(g2o::Vector2());
        if(i == 0){
            v->setFixed(true);
        }
        optimizer.addVertex(v);
    }

    for(const auto& pData: edgeData){
        g2o::EdgePointXY* edge = new g2o::EdgePointXY();
        edge->setVertex( 0, optimizer.vertex(pData.s));
        edge->setVertex( 1, optimizer.vertex(pData.e));
        edge->setInformation(  Eigen::Matrix< double, 2,2 >::Identity() );// 信息矩阵表示2维上侧重哪一维，xy是一样重要的，所以就是单位矩阵，但是在6维的位姿中，有可能更侧重优化旋转或者位移，就需要设置信息矩阵
        edge->setMeasurement(pData.pose );
        optimizer.addEdge(edge);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(500);
    for(int i = 0; i < maxIndex+1; i++){
        g2o::VertexPointXY* vertex = dynamic_cast<g2o::VertexPointXY*>(optimizer.vertex( i ));
        g2o::Vector2 pose = vertex->estimate();
        std::cout << i << ":\n" << pose << std::endl ;
    }
    return 0;
}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#define BOOST_TEST_MODULE FCL_MATH
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include <hpp/fcl/data_types.h>
#include <hpp/fcl/math/transform.h>

#include <hpp/fcl/internal/intersect.h>
#include <hpp/fcl/internal/tools.h>

using namespace hpp::fcl;

BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64)
{
  Vec3f v1(1.0, 2.0, 3.0);
  BOOST_CHECK(v1[0] == 1.0);
  BOOST_CHECK(v1[1] == 2.0);
  BOOST_CHECK(v1[2] == 3.0);

  Vec3f v2 = v1;
  Vec3f v3(3.3, 4.3, 5.3);
  v1 += v3;
  BOOST_CHECK(isEqual(v1, v2 + v3));
  v1 -= v3;
  BOOST_CHECK(isEqual(v1, v2));
  v1 -= v3;
  BOOST_CHECK(isEqual(v1, v2 - v3));
  v1 += v3;

  v1.array() *= v3.array();
  BOOST_CHECK(isEqual(v1, v2.cwiseProduct(v3)));
  v1.array() /= v3.array();
  BOOST_CHECK(isEqual(v1, v2));
  v1.array() /= v3.array();
  BOOST_CHECK(isEqual(v1, v2.cwiseQuotient(v3)));
  v1.array() *= v3.array();

  v1 *= 2.0;
  BOOST_CHECK(isEqual(v1, v2 * 2.0));
  v1 /= 2.0;
  BOOST_CHECK(isEqual(v1, v2));
  v1 /= 2.0;
  BOOST_CHECK(isEqual(v1, v2 / 2.0));
  v1 *= 2.0;

  v1.array() += 2.0;
  BOOST_CHECK(isEqual(v1, (v2.array() + 2.0).matrix()));
  v1.array() -= 2.0;
  BOOST_CHECK(isEqual(v1, v2));
  v1.array() -= 2.0;
  BOOST_CHECK(isEqual(v1, (v2.array() - 2.0).matrix()));
  v1.array() += 2.0;

  BOOST_CHECK(isEqual((-Vec3f(1.0, 2.0, 3.0)), Vec3f(-1.0, -2.0, -3.0)));

  v1 = Vec3f(1.0, 2.0, 3.0);
  v2 = Vec3f(3.0, 4.0, 5.0);
  BOOST_CHECK(isEqual((v1.cross(v2)), Vec3f(-2.0, 4.0, -2.0)));
  BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);

  v1 = Vec3f(3.0, 4.0, 5.0);
  BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5);
  BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
  BOOST_CHECK(isEqual(v1.normalized(), v1 / v1.norm()));

  v1 = Vec3f(1.0, 2.0, 3.0);
  v2 = Vec3f(3.0, 4.0, 5.0);
  BOOST_CHECK(isEqual(v1.cross(v2), Vec3f(-2.0, 4.0, -2.0)));
  BOOST_CHECK(v1.dot(v2) == 26);
}

Vec3f rotate (Vec3f input, FCL_REAL w, Vec3f vec) {
  return 2*vec.dot(input)*vec + (w*w - vec.dot(vec))*input + 2*w*vec.cross(input);
}

BOOST_AUTO_TEST_CASE(quaternion)
{
  Quaternion3f q1 (Quaternion3f::Identity()), q2, q3;
  q2 = fromAxisAngle(Vec3f(0,0,1), M_PI/2);
  q3 = q2.inverse();

  Vec3f v(1,-1,0);

  BOOST_CHECK(isEqual(v, q1 * v));
  BOOST_CHECK(isEqual(Vec3f(1,1,0), q2 * v));
  BOOST_CHECK(isEqual(rotate(v, q3.w(), Vec3f(q3.x(), q3.y(), q3.z())), q3 * v));
}

BOOST_AUTO_TEST_CASE(transform)
{
  Quaternion3f q = fromAxisAngle(Vec3f(0,0,1), M_PI/2);
  Vec3f T (0,1,2);
  Transform3f tf (q, T);

  Vec3f v(1,-1,0);

  BOOST_CHECK(isEqual(q * v + T, q * v + T));

  Vec3f rv (q * v);
  // typename Transform3f::transform_return_type<Vec3f>::type output =
    // tf * v;
  // std::cout << rv << std::endl;
  // std::cout << output.lhs() << std::endl;
  BOOST_CHECK(isEqual(rv + T, tf.transform(v)));
}


#include <fmt/format.h>
#include <scn/scn.h>

#include <boost/spirit/include/karma.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <cassert>
#include <charconv>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <iomanip>
#include <numbers>
#include <random>
#include <sstream>
#include <string_view>

constexpr int DEFAULT_PRECISION = 17;

constexpr int BUF_SIZE =
    1 /*'-'*/ +
    (std::numeric_limits<double>::max_exponent10 + 1) /*exponent+1 digits*/
    + 1 /*'.'*/ + DEFAULT_PRECISION /* precision*/ + 1 /*terminating null*/;

template <typename Format, typename... Args>
void error(Format &&format, Args &&...args) {
  throw std::runtime_error(
      fmt::format(format, std::forward<Args>(args)...));
}

template <typename T>
class Rng {
 public:
  explicit Rng(unsigned seed = 0) : gen_{seed} {}

  T operator()() { return dist_(gen_); }

 private:
  std::mt19937 gen_;
  using dist = std::conditional_t<std::is_integral_v<T>,
                                  std::uniform_int_distribution<T>,
                                  std::uniform_real_distribution<T>>;
  dist dist_;
};

template <typename Method>
static size_t verifyValue(double value, Method method,
                          const std::string_view expect = "") {
  auto str = method(value);

  if (not expect.empty() && str != expect) {
    error("Error: expect {} but actual {}", expect, str);
  }

  auto [roundtrip, processed] = method(str);

  if (processed < 0) {
    return 0;
  }

  if (str.size() != static_cast<size_t>(processed)) {
    error("Error: some extra character {} -> '{}'", value, str);
  }

  if (value != roundtrip) {
    error("Error: roundtrip fail {:.17g} -> '{}' -> {:.17g}", value, str,
          roundtrip);
  }

  return str.size();
}

template <typename Method>
static void verify(const std::string_view fname, Method method) try {
  fmt::print("Verifying {:20} ... ", fname);

  // Boundary and simple cases
  verifyValue(0, method);
  verifyValue(0.1, method, "0.1");
  verifyValue(0.12, method, "0.12");
  verifyValue(0.123, method, "0.123");
  verifyValue(0.1234, method, "0.1234");
  verifyValue(1.2345, method, "1.2345");
  verifyValue(1.0 / 3.0, method);
  verifyValue(2.0 / 3.0, method);
  verifyValue(10.0 / 3.0, method);
  verifyValue(20.0 / 3.0, method);
  verifyValue(std::numeric_limits<double>::min(), method);
  verifyValue(std::numeric_limits<double>::max(), method);
  verifyValue(std::numeric_limits<double>::denorm_min(), method);

  Rng<double> r;

  constexpr unsigned kVerifyRandomCount = 100000;

  uint64_t lenSum = 0;
  size_t lenMax = 0;
  for (unsigned i = 0; i < kVerifyRandomCount; i++) {
    double d;
    do {
      d = r();
    } while (std::isnan(d) || std::isinf(d));
    size_t len = verifyValue(d, method);
    lenSum += len;
    lenMax = std::max(lenMax, len);
  }

  double lenAvg = double(lenSum) / kVerifyRandomCount;
  fmt::print("OK. Length Avg = {:2.3f}, Max = {}\n", lenAvg, lenMax);
} catch (const std::exception &ex) {
  fmt::print("{}\n", ex.what());
}

// parse the 2 strings as numbers, add the numbers and return the result as a
// string String add(String lhs, String rhs);

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-conversion"
#pragma GCC diagnostic ignored "-Wconversion"

// https://github.com/dspinellis/unix-history-repo/blob/Research-V6/usr/source/iolib/ftoa.c
void ftoa(double x, char *str, int prec, int format) {
  /* converts a floating point number to an ascii string */
  /* x is stored into str, which should be at least 30 chars long */
  int ie, i, k, ndig, fstyle;
  double y;
  // if (nargs() != 7)
  //   IEHzap("ftoa  ");
  ndig = (prec <= 0) ? 7 : (prec > 22 ? 23 : prec + 1);
  if (format == 'f' || format == 'F')
    fstyle = 1;
  else
    fstyle = 0;
  /* print in e format unless last arg is 'f' */
  ie = 0;
  /* if x negative, write minus and reverse */
  if (x < 0) {
    *str++ = '-';
    x = -x;
  }

  /* put x in range 1 <= x < 10 */
  if (x > 0.0)
    while (x < 1.0) {
      x *= 10.0;
      ie--;
    }
  while (x >= 10.0) {
    x = x / 10.0;
    ie++;
  }

  /* in f format, number of digits is related to size */
  if (fstyle) ndig += ie;

  /* round. x is between 1 and 10 and ndig will be printed to
     right of decimal point so rounding is ... */
  for (y = i = 1; i < ndig; i++) y = y / 10.;
  x += y / 2.;
  if (x >= 10.0) {
    x = 1.0;
    ie++;
  } /* repair rounding disasters */
  /* now loop.  put out a digit (obtain by multiplying by
    10, truncating, subtracting) until enough digits out */
  /* if fstyle, and leading zeros, they go out special */
  if (fstyle && ie < 0) {
    *str++ = '0';
    *str++ = '.';
    if (ndig < 0) ie = ie - ndig; /* limit zeros if underflow */
    for (i = -1; i > ie; i--) *str++ = '0';
  }
  for (i = 0; i < ndig; i++) {
    k = x;
    *str++ = k + '0';
    if (i == (fstyle ? ie : 0)) /* where is decimal point */
      *str++ = '.';
    x -= (y = k);
    x *= 10.0;
  }

  /* now, in estyle,  put out exponent if not zero */
  if (!fstyle && ie != 0) {
    *str++ = 'E';
    if (ie < 0) {
      ie = -ie;
      *str++ = '-';
    }
    for (k = 100; k > ie; k /= 10)
      ;
    for (; k > 0; k /= 10) {
      *str++ = ie / k + '0';
      ie = ie % k;
    }
  }
  *str = '\0';
  return;
}

#pragma GCC diagnostic pop

struct to_double_res {
  double res = 0;
  ptrdiff_t processed;
};

[[maybe_unused]] struct {
  to_double_res operator()(const std::string &str) const {
    return {std::atof(str.c_str()), static_cast<ptrdiff_t>(str.size())};
  }

  std::string operator()(const double d) const {
    char buf[BUF_SIZE];
    ftoa(d, buf, DEFAULT_PRECISION, 'f');
    return buf;
  }
} XtoY;

[[maybe_unused]] struct {
  to_double_res operator()(const std::string &str) const {
    char *end;
    auto res = std::strtod(str.c_str(), &end);
    return {res, errno != 0 ? -1 : (end - str.c_str())};
  }

  std::string operator()(const double d) const {
    char buf[BUF_SIZE];
    gcvt(d, DEFAULT_PRECISION, buf);
    return buf;
  }
} strtoX_gcvt;

[[maybe_unused]] struct {
  to_double_res operator()(const std::string &str) const {
    double res = 0;
    ptrdiff_t processed = -1;
    std::sscanf(str.c_str(), "%lf%tn", &res, &processed);
    return {res, processed};
  }

  std::string operator()(const double d) const {
    char buf[BUF_SIZE];
    std::sprintf(buf, "%g", d);
    return buf;
  }
} sXf;

[[maybe_unused]] struct {
  to_double_res operator()(const std::string &str) const {
    std::istringstream in{str};
    double res = 0;
    in >> res;
    auto processed = [&]() -> ptrdiff_t {
      if (in.eof()) {
        return std::ssize(str);
      }
      return in.tellg();
    }();
    return {res, processed};
  }

  std::string operator()(const double d) const {
    std::ostringstream out;
    out << std::defaultfloat << d;
    return out.str();
  }
} stringstream;

[[maybe_unused]] struct {
  to_double_res operator()(const std::string &str) const {
    std::ios_base::iostate err = std::ios_base::goodbit;
    std::istringstream sst;
    using Facet = std::num_get<char, std::string::const_iterator>;
    static std::locale loc{std::locale::classic(), new Facet};
    double res = 0;
    auto end =
        std::use_facet<Facet>(loc).get(str.begin(), str.end(), sst, err, res);
    return {res, (err & std::ios_base::failbit) ? -1 : (end - str.begin())};
  }

  std::string operator()(const double d) const {
    std::ostringstream sst;
    sst << std::defaultfloat;
    std::string res;
    res.reserve(BUF_SIZE);
    using Facet = std::num_put<char, decltype(std::back_inserter(res))>;
    std::locale loc{sst.getloc(), new Facet};
    std::use_facet<Facet>(loc).put(std::back_inserter(res), sst, sst.fill(), d);
    return res;
  }
} num_X;

[[maybe_unused]] struct {
  to_double_res operator()(const std::string &str) const {
    return {std::stod(str), static_cast<ptrdiff_t>(str.size())};
  }

  std::string operator()(const double d) const { return std::to_string(d); }
} stoX_to_string;

[[maybe_unused]] struct {
  to_double_res operator()(const std::string &str) const {
    double res = 0;
    const auto [end, ec] =
        std::from_chars(str.data(), str.data() + str.size(), res);
    return {res, std::error_condition{ec} ? -1 : (end - str.data())};
  }

  std::string operator()(const double d) const {
    std::string res(BUF_SIZE, 0);
    const auto [end, _] = std::to_chars(res.data(), res.data() + res.size(), d);
    res.resize(static_cast<size_t>(end - res.data()));
    return res;
  }
} X_chars;

[[maybe_unused]] struct {
  to_double_res operator()(const std::string &str) const {
    double res = 0;
    auto scan_result = scn::scan(str, "{}", res);
    return {res, !scan_result ? -1 : (scan_result.begin() - str.data())};
  }

  std::string operator()(const double d) const { return fmt::format("{}", d); }
} scan_format;

[[maybe_unused]] struct {
  template <typename Num>
  struct precision_policy : boost::spirit::karma::real_policies<Num>
  {
    precision_policy(unsigned precision) : precision_{precision} {}
    unsigned precision(Num /*n*/) const { return precision_; }
    unsigned precision_;
  };

  to_double_res operator()(const std::string &str) const {
    namespace qi = boost::spirit::qi;

    using boost::phoenix::ref;
    using qi::_1;
    using qi::double_;

    double res = 0;
    auto first = str.begin();
    auto success = qi::parse(first, str.end(), double_[ref(res) = _1]);
    return { res, !success ? -1 : first - str.begin() };
  }

  std::string operator()(const double d) const {
    namespace karma = boost::spirit::karma;
    using karma::double_;

    std::string res;
    using precision_double_ =
        karma::real_generator<double, precision_policy<double>>;
    karma::generate(std::back_inserter(res), precision_double_{DEFAULT_PRECISION}, d);
    return res;
  }
} qi_karma;

int main() {
  verify("XtoY", XtoY);
  verify("strtoX_gcvt", strtoX_gcvt);
  verify("sXf", sXf);
  verify("stringstream", stringstream);
  verify("num_X", num_X);
  verify("stoX_to_string", stoX_to_string);
  verify("X_chars", X_chars);
  verify("scan_format", scan_format);
  verify("qi_karma", qi_karma);
}


#include <string>
#include <fstream>
#include <vector>
#include <utility> // std::pair
#include <sstream>
#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

using namespace std;

vector<vector<double>> read_csv( string filename ){
    // Reads a CSV file with 2 columns into a vector of <vector<double>>
    vector<vector<double>> result(2);

    // Create an input filestream
	//const char *c = filename.c_str();
    ifstream myFile(filename.c_str());

    // Make sure the file is open
    if(!myFile.is_open()) throw runtime_error("Could not open file");

    // Helper vars
    string line;
    double val;

	if(myFile.good())
	{
		// Read data, line by line
		while(getline(myFile, line))
		{
			// Create a stringstream of the current line
			stringstream ss(line);

			// Keep track of the current column index
			int colIdx = 0;
			// Extract each integer
			while(ss >> val){

				// Add the current integer to the 'colIdx' column's values vector
				result[colIdx].push_back(val);

				// If the next token is a comma, ignore it and move on
				if(ss.peek() == ',') ss.ignore();

				// Increment the column index
				colIdx++;
			}
		}
	}

    // Close file
    myFile.close();
    return result;
}

double getyL(string ref, double positionx , double positiony, bool debug)
{
	typedef boost::geometry::model::d2::point_xy<double> point_type;
	typedef boost::geometry::model::linestring<point_type> linestring_type;

	if (debug) cout << "Entry: Read CSV" << endl;
    vector<vector<double>> gold_ref = read_csv(ref);
	if (debug) cout << "Exit: Read CSV" << endl;

	point_type p(positionx, positiony);
	linestring_type line;
	double x = 0;
	double y = 0;

	for (int i = 0; i < gold_ref[0].size(); ++i)
    {
		for(int j = 0; j < gold_ref.size(); ++j)
		{
			if (j == 0) x = gold_ref[j][i];
			else y = gold_ref[j][i];

		}
		line.push_back(point_type(x,y));
        //cout << x << " , " << y << "\n" ;
    }

	double yl = boost::geometry::distance(p, line);
	//cout << "Point-Line: " << yl << endl;
	return yl;
}

/*int main(int argc , char *argv[])
  {
	typedef boost::geometry::model::d2::point_xy<double> point_type;
	typedef boost::geometry::model::linestring<point_type> linestring_type;
    vector<vector<double>> gold_ref = read_csv("/home/sayandipde/Approx_IBC/hil/client/Webots/worlds/city_ref.csv");

	point_type p(1,2);
	linestring_type line;
	double x = 0;
	double y = 0;

	for (int i = 0; i < gold_ref[0].size(); ++i)
    {
		for(int j = 0; j < gold_ref.size(); ++j)
		{
			//cout << gold_ref[j][i];
			//if(j != gold_ref.size() - 1) cout << ","; // No comma at end of line
			if (j == 0) x = gold_ref[j][i];
			else y = gold_ref[j][i];

		}
		line.push_back(point_type(x,y));
        cout << x << " , " << y << "\n" ;
    }

	cout << "Point-Line: " << boost::geometry::distance(p, line) << endl;
	return 0;
  }*/


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2010: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/

/** Example custom_interval.cpp \file custom_interval.cpp
    \brief Shows how to use interval containers with own interval classes.

    There may be instances, where we want to use interval container with our
    own user defined interval classes. Boost interval containers can be adapted
    to your interval class by partial template specialisation. Only a few lines
    of code are needed to achieve this.

    \include custom_interval_/custom_interval.cpp
*/
//[example_custom_interval
#include <iostream>
#include <boost/icl/interval_set.hpp>

using namespace std;
using namespace boost::icl;

// Here is a typical class that may model intervals in your application.
class MyInterval
{
public:
    MyInterval(): _first(), _past(){}
    MyInterval(int lo, int up): _first(lo), _past(up){}
    int first()const{ return _first; }
    int past ()const{ return _past; }
private:
    int _first, _past;
};

namespace boost{ namespace icl
{
// Class template interval_traits serves as adapter to register and customize your interval class
template<>
struct interval_traits< MyInterval >       //1.  Partially specialize interval_traits for
{                                          //    your class MyInterval
                                           //2.  Define associated types
    typedef MyInterval     interval_type;  //2.1 MyInterval will be the interval_type
    typedef int            domain_type;    //2.2 The elements of the domain are ints
    typedef std::less<int> domain_compare; //2.3 This is the way our element shall be ordered.
                                           //3.  Next we define the essential functions
                                           //    of the specialisation
                                           //3.1 Construction of intervals
    static interval_type construct(const domain_type& lo, const domain_type& up)
    { return interval_type(lo, up); }
                                           //3.2 Selection of values
    static domain_type lower(const interval_type& inter_val){ return inter_val.first(); };
    static domain_type upper(const interval_type& inter_val){ return inter_val.past(); };
};

template<>
struct interval_bound_type<MyInterval>     //4.  Finally we define the interval borders.
{                                          //    Choose between static_open         (lo..up)
    typedef interval_bound_type type;      //                   static_left_open    (lo..up]
    BOOST_STATIC_CONSTANT(bound_type, value = interval_bounds::static_right_open);//[lo..up)
};                                         //               and static_closed       [lo..up]

}} // namespace boost icl

void custom_interval()
{
    // Now we can use class MyInterval with interval containers:
    typedef interval_set<int, std::less, MyInterval> MyIntervalSet;
    MyIntervalSet mySet;
    mySet += MyInterval(1,9);
    cout << mySet << endl;
    mySet.subtract(3) -= 6;
    cout << mySet << "            subtracted 3 and 6\n";
    mySet ^= MyInterval(2,8);
    cout << mySet <<      "  flipped between 2 and 7\n";
}

int main()
{
    cout << ">>Interval Container Library: Sample custom_interval.cpp <<\n";
    cout << "-----------------------------------------------------------\n";
    cout << "This program uses a user defined interval class:\n";
    custom_interval();
    return 0;
}

// Program output:
/*-----------------------------------------------------------------------------
>>Interval Container Library: Sample custom_interval.cpp <<
-----------------------------------------------------------
This program uses a user defined interval class:
{[1,                      9)}
{[1,  3)   [4,  6)   [7,  9)}       subtracted 3 and 6
{[1,2) [3,4)     [6,7) [8,9)}  flipped between 2 and 7
-----------------------------------------------------------------------------*/
//]


#pragma once
#include <cmath>

#include <boost/any.hpp>

#include <SFML/System/Vector2.hpp>

namespace tt
{

using Tile = sf::Vector2f;
using Size = sf::Vector2f;
using Scale = sf::Vector2f;

enum class TileType
{
    NONE,
    ZONE
};

struct TileInfo
{
    TileType        type;
    boost::any      data;
    Tile            tile;
};

namespace tiles
{

inline Tile getTileFromGlobal(const sf::Vector2f& global, const Size& tilesize, const Scale& scale)
{
    sf::Vector2f temp;
    temp.x = global.x / scale.x;
    temp.y = global.y / scale.y;

    temp.x = std::floor(temp.x / tilesize.x);
    temp.y = std::floor(temp.y / tilesize.y);

    return temp;
}

inline sf::Vector2f getGlobalFromTile(const Tile& tilepos, const Size& tileSize, const Scale& scale)
{
    sf::Vector2f temp;

    temp.x = static_cast<float>(tilepos.x * tileSize.x * scale.x);
    temp.y = static_cast<float>(tilepos.y * tileSize.y * scale.y);

    return temp;
}

} // tiles
} // namespace tt


#include <iostream>
#include <cstdio>
#include <ctime>
#include <boost/numeric/mtl/mtl.hpp>
#include "boost/program_options.hpp"

using namespace std;
using namespace mtl;
using namespace mtl::mat;
namespace po = boost::program_options;

// m: numRows, n: numCols
inline double simpleDenseTest_MTL(int m, int n, int num_trials) {

  dense2D<double, parameters<tag::col_major> > A(m, n); random(A);
  dense2D<double, parameters<tag::col_major> > B(m, n); random(B);
  dense2D<double, parameters<tag::col_major> > C(m, n); random(C);
  dense2D<double, parameters<tag::col_major> > D(m, n); random(D);
  dense2D<double, parameters<tag::col_major> > E(m, n); random(E);

  clock_t start;
  double duration = 0.0;

  for (unsigned i = 0; i < num_trials; i++) {
    start = clock();

    (ele_div((A + B), C) - D) * E;

    duration += ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  }
  return duration / num_trials;

}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double gemmDenseTest_MTL(int m, int n, int k, int num_trials) {

  dense2D<double, parameters<tag::col_major> > A(m, n); random(A);
  dense2D<double, parameters<tag::col_major> > B(m, n); random(B);
  dense2D<double, parameters<tag::col_major> > C(n, k); random(C);
  dense2D<double, parameters<tag::col_major> > D(n, k); random(D);
  dense2D<double, parameters<tag::col_major> > E(m, k); random(E);

  clock_t start;
  double duration = 0.0;

  for (unsigned i = 0; i < num_trials; i++) {
    start = clock();

    E += (A + B) * (C - D);

    duration += ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  }
  return duration / num_trials;

}

inline double mulDenseTest_MTL(int a, int b, int c, int d, int num_trials) {

  dense2D<double, parameters<tag::col_major> > A(a, a); random(A);
  dense2D<double, parameters<tag::col_major> > B(a, b); random(B);
  dense2D<double, parameters<tag::col_major> > C(b, c); random(C);
  dense2D<double, parameters<tag::col_major> > D(c, d); random(D);

  clock_t start;
  double duration = 0.0;

  for (unsigned i = 0; i < num_trials; i++) {
    start = clock();

    A * B * C * D;

    duration += ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  }

  return duration / num_trials;

}

void runMTLTests(int num_trials, int m, int n, int k, int a, int b, int c, int d,
    bool skip_vec, bool skip_simple, bool skip_gemm, bool skip_mult) {

   cout << "MTL Simple Test:\t" << simpleDenseTest_MTL(m, n, num_trials) << endl;
   cout << "MTL gemm Test:\t" << gemmDenseTest_MTL(m, n, k, num_trials) << endl;
   cout << "MTL mulDense Test:\t" << mulDenseTest_MTL(a, b, c, d, num_trials) << endl;

}

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

    int l, m, n, k, a, b, c, d, trials;
    bool skip_vec, skip_simple, skip_gemm, skip_mult;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("l", po::value<int>(&l)->default_value(1048576),
                    "length of vectors in vector addition test")
        ("m", po::value<int>(&m)->default_value(1024),
            "numRows of matrices in Simple Test, and gemm Test")
        ("n", po::value<int>(&n)->default_value(1024),
            "numCols of matrices in Simple Test, and gemm Test")
        ("k", po::value<int>(&k)->default_value(1024),
            "numCols of B in gemm Test")
        ("trials", po::value<int>(&trials)->default_value(10), "number of trials")
        ("a", po::value<int>(&a)->default_value(1024),
            "size matrix A in mulDense Test")
        ("b", po::value<int>(&b)->default_value(512),
            "size matrix B in mulDense Test")
        ("c", po::value<int>(&c)->default_value(256),
            "size matrix C in mulDense Test")
        ("d", po::value<int>(&d)->default_value(128),
            "size matrix D in mulDense Test")
        ("skip-vec", po::value<bool>(&skip_vec)->default_value(false),
            "skip vectors Test")
        ("skip-simple", po::value<bool>(&skip_simple)->default_value(false),
            "skip simple Test")
        ("skip-gemm", po::value<bool>(&skip_gemm)->default_value(false),
            "skip gemm Tests")
        ("skip-mult", po::value<bool>(&skip_mult)->default_value(false),
            "skip mulDense Test")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    runMTLTests(trials, l, m, n, k, a, b, c, d, skip_vec, skip_simple, skip_gemm, skip_mult);

    return 0;
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef SPLINEFUNCTION_HPP
#define SPLINEFUNCTION_HPP

#include <algorithm>

#include "Config.hpp"

#include <Eigen/Core>
#include <unsupported/Eigen/Splines>

/*! \file SplineFunction.hpp
 *  \class SplineFunction
 *  \brief Spline interpolation of a function
 *  \author Roberto Di Remigio
 *  \date 2015
 *
 *  Taken from StackOverflow http://stackoverflow.com/a/29825204/2528668
 */

class SplineFunction __final
{
private:
    typedef Eigen::Spline<double, 1> CubicSpline;
public:
    /*! \brief Constructor from abscissa and function values
     *  \param[in] x vector with abscissa values
     *  \param[in] y vector with function values
     *
     *  Interpolation happens in the initialization of the spline_ data
     *  member. We use std::min to set the degree in order to ensure that no
     *  more than a cubic spline is used, but short vectors are still accepted.
     */
    SplineFunction(const Eigen::VectorXd & x, const Eigen::VectorXd & y)
	    : xMin_(x.minCoeff()), xMax_(x.maxCoeff()),
	    spline_(Eigen::SplineFitting<CubicSpline>::Interpolate(y.transpose(),
		      std::min<int>(x.rows() - 1, 3), fitVector(x))
	           )
	{}
    /*! \brief Evaluate spline at given point
     *  \param[in] x evaluation point
     */
    double operator()(double x) const {
	return spline_(fitScalar(x))(0);
    }
private:
    double xMin_;
    double xMax_;
    CubicSpline spline_;
    /*! \brief Scale a scalar value to [0, 1] interval
     *  \param[in] x value to be scaled
     *
     *  val is defined in [min, max] This function returns the variable
     *  as scaled to fit in the [0, 1] interval.
     */
    double fitScalar(double x) const {
	    return (x - xMin_) / (xMax_ - xMin_);
    }
    /*! \brief Scale a vector to [0, 1] interval
     *  \param[in] x_vec a vector
     *
     *  Given column vector, returns a *row* vector with the values scaled
     *  to fit a preset interval. The interval is embedded in the callable object.
     */
#ifdef HAS_CXX11_LAMBDA
    Eigen::RowVectorXd fitVector(const Eigen::VectorXd & x_vec) const {
	    return x_vec.unaryExpr([this](double x) -> double { return fitScalar(x); }).transpose();
    }
#else /* HAS_CXX11_LAMBDA */
    Eigen::RowVectorXd fitVector(const Eigen::VectorXd & x_vec) const {
        pcm::function<double(double)> fit = pcm::bind(&SplineFunction::fitScalar, this, pcm::_1);
	    return x_vec.unaryExpr(fit).transpose();
    }
#endif /*HAS_CXX11_LAMBDA */
};
#endif // SPLINEFUNCTION_HPP


#ifndef _DATA_ITEM_HPP_
#define _DATA_ITEM_HPP_

#include <fstream>
#include <vector>
#include <utility>
#include <Eigen\Dense>

struct LabeledData {
  std::vector<unsigned> labels;
  Eigen::MatrixXd data;
};

struct DataItem {
  unsigned target;
  Eigen::RowVectorXd point;
};

struct PredicitonItem {
  unsigned target;
  unsigned prediction;
};

Eigen::MatrixXd StlVectorsToEigenMatrix(
  const std::vector<std::vector<double> >& data) {
/**
 *
 */
  Eigen::MatrixXd matrix(data.size(), data[0].size());

  unsigned i;
  unsigned j;
  for (i = 0; i < data.size(); ++i) {
    for (j = 0; j < data[0].size(); ++j) {
      matrix(i, j) = data[i][j];
    }
  }

  return matrix;
}

LabeledData
ReadDataFromFile(const char* file_name) {
/**
 *
 */
  std::fstream fin;
  fin.open(file_name);

  std::vector<unsigned> labels;
  std::vector<std::vector<double>> data;
  double dummy;
  while (fin.good()) {
    fin >> dummy;
    labels.push_back((unsigned) dummy);

    std::vector<double> row;
    unsigned i;
    for (i = 0; i < 256; ++i) {
      fin >> dummy;
      row.push_back(dummy);
    }
    data.push_back(row);
  }

  LabeledData labeledData = {
    labels, StlVectorsToEigenMatrix(data)
  };

  return labeledData;
}

Eigen::MatrixXd GetDataSubset(const LabeledData* labeledData, std::vector<int> labels) {
/**
 *
 */

  return Eigen::MatrixXd(1,1);
}

#endif //define _DATA_ITEM_HPP_


#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>

#include "DenseMatrix.hpp"

#define BOOST_TEST_MODULE custom_main
#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_API

#ifndef REAL
#define REAL double
#endif

#define TOL 1.0e-06
const int SIZE = 100;
const int size = 10;

int add(int x, int y);

template<class T>
static T multiply(T x, T y);

// BOOST_AUTO_TEST_SUITE( boost_test_suite )

BOOST_AUTO_TEST_CASE( test_case_00 )
{
  // Seven ways to check and report errors
  // BOOST_CHECK( add(2,3) == 5 );
  // BOOST_REQUIRE( add(2,3) == 5 );
  // if ( add(2,3) != 5 ) BOOST_ERROR("Incorrect answer");
  // if ( add(2,3) != 5 ) BOOST_FAIL("Incorrect answer");
  // if ( add(2,3) != 5 ) throw "Incorrect answer";
  // BOOST_CHECK_MESSAGE( add(2,3) == 5, "add(...) result: " << add(2,3) );
  // BOOST_CHECK_EQUAL( add(2,3), 5);

  REAL *A = (REAL*) calloc (SIZE, sizeof(REAL));
  REAL *B = (REAL*) calloc (SIZE, sizeof(REAL));
  REAL *C = (REAL*) calloc (SIZE, sizeof(REAL));

  boost::numeric::ublas::matrix<double> A_bst(size, size), B_bst(size, size),
    C_bst(size, size);

  randMatrix(A, size, size);
  randMatrix(B, size, size);
  for (int i=0; i<SIZE; ++i) C[i] = 0.0;

  // Assign all element values to Boost matrices
  for (int i=0; i<size; ++i){
    for (int j=0; j<size; ++j){
      int idx = i*size + j;
      A_bst(i, j) = A[idx];
      B_bst(i, j) = B[idx];
    }
  }

  // Manual Version
  dgemm(A, size, size, B, size, size, C);
  // Boost Version
  boost::numeric::ublas::axpy_prod(A_bst, B_bst, C_bst, true);

  for (int i=0; i<size; ++i){
    for (int j=0; j<size; ++j){
      int idx = i*size + j;
      BOOST_CHECK_CLOSE(C_bst(i,j), C[idx], TOL);
    }
  }

}

BOOST_AUTO_TEST_CASE( test_case_01 )
{
  int x = 5; int y = 7; int z = add(x, y);
  BOOST_CHECK_EQUAL( z, 12 );
}

BOOST_AUTO_TEST_CASE( test_case_02 )
{
  long int x = 50; long int y = 30;
  long int z = multiply<long int> (x, y);
  BOOST_CHECK_EQUAL(z, 1500);
}

// BOOST_AUTO_TEST_SUITE_END()

bool init_unit_test(){ return true; }
int main(int argc, char **argv){
  return boost::unit_test::unit_test_main( &init_unit_test, argc, argv );
}

int add(int x, int y) { return (x + y); }

template<class T>
static T multiply(T x, T y)
{ return (x*y); }


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file decorrelated_gaussian.hpp
 * \date JUly 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <vector>
#include <string>
#include <cstddef>
#include <type_traits>

#include <fl/util/traits.hpp>
#include <fl/exception/exception.hpp>
#include <fl/distribution/interface/moments.hpp>
#include <fl/distribution/interface/evaluation.hpp>
#include <fl/distribution/interface/standard_gaussian_mapping.hpp>
#include <fl/distribution/gaussian.hpp>

namespace fl
{

/**
 * \class DecorrelatedGaussian
 *
 * \brief General Decorrelated Gaussian Distribution
 * \ingroup distributions
 * \{
 *
 * The Gaussian is a general purpose distribution representing a multi-variate
 * \f${\cal N}(x; \mu, \Sigma)\f$. It can be used in various
 * ways while maintaining efficienty at the same time. This is due to it's
 * multi-representation structure. The distribution can be represented either by
 *
 *  - the covariance matrix \f$\Sigma\f$,
 *  - the precision matrix \f$\Sigma^{-1} = \Lambda\f$,
 *  - the covariance square root matrix (Cholesky decomposition or LDLT)
 *    \f$\sqrt{\Sigma} = L\sqrt{D}\f$,
 *  - or the diagonal form of the previous three options
 *    \f$diag(\sigma_1, \ldots, \sigma_n)\f$.
 *
 * A change in one representation results in change of all other
 * representations.
 *
 * Two key features of the distribution are its aibility to evaluation the
 * probability of a given sample and to map a noise sample into the distribution
 * sample space.
 *
 * \cond internal
 * The Gaussian internal structure uses lazy assignments or write on read
 * technique. Due to the multi-representation of the Gaussian, modifying one
 * representation affects all remaining ones. If one of the representation is
 * modified, the other representations are only then updated when needed. This
 * minimizes redundant computation and increases efficienty.
 * \endcond
 */
template <typename Variate>
class DecorrelatedGaussian
    : public Moments<Variate, typename DiagonalSecondMomentOf<Variate>::Type>,
      public Evaluation<Variate>,
      public StandardGaussianMapping<Variate, SizeOf<Variate>::Value>
{
public:
    typedef Evaluation<Variate> EvaluationInterface;

    typedef Moments<
                Variate, typename DiagonalSecondMomentOf<Variate>::Type
            > MomentsInterface;

    typedef StandardGaussianMapping<
                Variate, SizeOf<Variate>::Value
            > StdGaussianMappingInterface;

    /**
     * \brief Second moment matrix type, i.e covariance matrix, precision
     *        matrix, and their diagonal and square root representations
     */
    typedef typename DiagonalSecondMomentOf<Variate>::Type DiagonalSecondMoment;

    typedef typename SecondMomentOf<Variate>::Type DenseSecondMoment;

    /**
     * \brief Represents the StandardGaussianMapping standard variate type which
     *        is of the same dimension as the Gaussian Variate. The
     *        StandardVariate type is used to sample from a standard normal
     *        Gaussian and map it to this Gaussian
     */
    typedef
    typename StdGaussianMappingInterface::StandardVariate StandardVariate;

protected:
    /** \cond internal */
    /**
     * \enum Attribute
     * Implementation attributes. The enumeration lists the different
     * representations along with other properties such as the rank of the
     * second moment and the log normalizer.
     */
    enum Attribute
    {
        DiagonalCovarianceMatrix = 0,/**< Diagonal form of of cov. mat. */
        DiagonalPrecisionMatrix,     /**< Diagonal form of inv cov. mat. */
        DiagonalSquareRootMatrix,    /**< Diagonal form of Cholesky decomp. */
        Rank,                        /**< Covariance Rank */
        Normalizer,                  /**< Log probability normalizer */
        Determinant,                 /**< Determinant of covariance */

        Attributes                   /**< Total number of attribute */
    };

    /**
     * \brief Flags array type which contains the content status if different
     *        distribution representation
     */
    typedef std::array<bool, Attributes> FlagArray;
    /** \endcond */

public:
    /**
     * Creates a dynamic or fixed size Gaussian.
     *
     * \param dimension Dimension of the Gaussian. The default is defined by the
     *                  dimension of the variable type \em Vector. If the size
     *                  of the Vector at compile time is fixed, this will be
     *                  adapted. For dynamic-sized Variable the dimension is
     *                  initialized to 0.
     */
    explicit DecorrelatedGaussian(int dim = DimensionOf<Variate>()):
        StdGaussianMappingInterface(dim)
    {
        static_assert(SizeOf<Variate>::Value != 0, "Illegal static dimension");

        std::fill(dirty_.begin(), dirty_.end(), true);
        set_standard();
    }

    /**
     * \brief Overridable default destructor
     */
    virtual ~DecorrelatedGaussian() noexcept { }

    /**
     * \return Gaussian dimension
     */
    virtual int dimension() const
    {
        return StdGaussianMappingInterface::standard_variate_dimension();
    }

    /**
     * \return Gaussian first moment
     */
    virtual const Variate& mean() const
    {
        return mean_;
    }

    /**
     * \return Gaussian second centered moment
     *
     * Computes the covariance from other representation of not available
     *
     * \throws GaussianUninitializedException if the Gaussian is of dynamic-size
     *         and has not been initialized using SetStandard(dimension).
     * \throws InvalidGaussianRepresentationException if non-of the
     *         representation can be used as a source
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#CovarianceMatrix}
     * \endcond
     */
    virtual const DiagonalSecondMoment& covariance() const
    {
        if (dimension() == 0)
        {
            fl_throw(GaussianUninitializedException());
        }

        if (is_dirty(DiagonalCovarianceMatrix))
        {
            switch (select_first_representation<2>(
                        {{ DiagonalSquareRootMatrix,
                           DiagonalPrecisionMatrix }}))
            {
            case DiagonalSquareRootMatrix:
            {
                covariance_.diagonal() = square_root_.diagonal().cwiseProduct(
                                            square_root_.diagonal());
             } break;

            case DiagonalPrecisionMatrix:
            {
                covariance_.diagonal() = precision_.diagonal().cwiseInverse();
            } break;

            default:
                fl_throw(InvalidGaussianRepresentationException());
                break;
            }

            updated_internally(DiagonalCovarianceMatrix);
        }

        return covariance_;
    }

    /**
     * \return Gaussian second centered moment in the precision form (inverse
     * of the covariance)
     *
     * Computes the precision from other representation of not available
     *
     * \throws GaussianUninitializedException if the Gaussian is of dynamic-size
     *         and has not been initialized using SetStandard(dimension).
     * \throws InvalidGaussianRepresentationException if non-of the
     *         representation can be used as a source
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#PrecisionMatrix}
     * \endcond
     */
    virtual const DiagonalSecondMoment& precision() const
    {
        if (dimension() == 0)
        {
            fl_throw(GaussianUninitializedException());
        }

        if (is_dirty(DiagonalPrecisionMatrix))
        {
            switch (select_first_representation<2>(
                        {{ DiagonalCovarianceMatrix,
                           DiagonalSquareRootMatrix}}))
            {
            case DiagonalCovarianceMatrix:
            case DiagonalSquareRootMatrix:
                precision_.diagonal() = covariance().diagonal().cwiseInverse();
                break;

            default:
                fl_throw(InvalidGaussianRepresentationException());
                break;
            }

            updated_internally(DiagonalPrecisionMatrix);
        }

        return precision_;
    }

    /**
     * \return Gaussian second centered moment in the square root form (
     * Cholesky decomposition)
     *
     * Computes the square root from other representation of not available
     *
     * \throws GaussianUninitializedException if the Gaussian is of dynamic-size
     *         and has not been initialized using SetStandard(dimension).
     * \throws InvalidGaussianRepresentationException if non-of the
     *         representation can be used as a source
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#SquareRootMatrix}
     * \endcond
     */
    virtual const DiagonalSecondMoment& square_root() const
    {
        if (dimension() == 0)
        {
            fl_throw(GaussianUninitializedException());
        }

        if (is_dirty(DiagonalSquareRootMatrix))
        {
            switch (select_first_representation<2>(
                        {{ DiagonalCovarianceMatrix,
                           DiagonalPrecisionMatrix }}))
            {
            case DiagonalCovarianceMatrix:
            case DiagonalPrecisionMatrix:
            {
                square_root_.diagonal() = covariance().diagonal().cwiseSqrt();
            } break;

            default:
                fl_throw(InvalidGaussianRepresentationException());
                break;
            }

            updated_internally(DiagonalSquareRootMatrix);
        }

        return square_root_;
    }

    /**
     * \return True if the covariance matrix has a full rank
     *
     * \throws see covariance()
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#CovarianceMatrix}
     * \endcond
     */
    virtual bool has_full_rank() const
    {
        if (is_dirty(Rank))
        {
            full_rank_ = true;

            switch (select_first_representation<3>(
                        {{ DiagonalCovarianceMatrix,
                           DiagonalPrecisionMatrix,
                           DiagonalSquareRootMatrix }}))
            {
            case DiagonalCovarianceMatrix:
                full_rank_ = has_full_rank(covariance());
                break;
            case DiagonalPrecisionMatrix:
                full_rank_ = has_full_rank(precision());
                break;
            case DiagonalSquareRootMatrix:
                full_rank_ = has_full_rank(square_root());
                break;
            default:
                fl_throw(InvalidGaussianRepresentationException());
                break;
            }

            updated_internally(Rank);
        }

        return full_rank_;
    }

    /**
     * \return Log normalizing constant
     *
     * \throws see has_full_rank()
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#CovarianceMatrix}
     * \endcond
     */
    virtual Real log_normalizer() const
    {
        if (is_dirty(Normalizer))
        {
            if (has_full_rank())
            {
                log_norm_ = -0.5
                    * (log(covariance_determinant())
                       + Real(covariance().rows()) * log(2.0 * M_PI));
            }
            else
            {
                log_norm_ = 0.0; // FIXME
            }

            updated_internally(Normalizer);
        }

        return log_norm_;
    }

    /**
     * \return Covariance determinant
     *
     * \throws see covariance
     */
    virtual Real covariance_determinant() const
    {
        if (is_dirty(Determinant))
        {
            determinant_ = covariance().diagonal().prod();

            updated_internally(Determinant);
        }

        return determinant_;
    }

    /**
     * \return Log of the probability of the given sample \c vector
     *
     * \param vector sample which should be evaluated
     *
     * \throws see has_full_rank()
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#PrecisionMatrix}
     * \endcond
     */
    virtual Real log_probability(const Variate& vector) const
    {
        if(has_full_rank())
        {
            return log_normalizer() - 0.5
                    * (vector - mean()).transpose()
                    * precision()
                    * (vector - mean());
        }

        return -std::numeric_limits<Real>::infinity();
    }

    /**
     * \return a Gaussian sample of the type \c Vector determined by mapping a
     * noise sample into the Gaussian sample space
     *
     * \param sample    Noise Sample
     *
     * \throws see square_root()
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#SquareRootMatrix}
     * \endcond
     */
    virtual Variate map_standard_normal(const StandardVariate& sample) const
    {
        return mean() + square_root() * sample;
    }

    /**
     * Sets the Gaussian to a standard distribution with zero mean and identity
     * covariance.
     *
     * \cond internal
     * \pre {}
     * \post
     *  - Fully ranked covariance
     *  - {Valid representations} = {#CovarianceMatrix}
     * \endcond
     */
    virtual void set_standard()
    {
        mean_.resize(dimension());
        covariance_.resize(dimension());
        precision_.resize(dimension());
        square_root_.resize(dimension());

        mean(Variate::Zero(dimension()));

        auto cov = DiagonalSecondMoment(dimension());
        cov.setIdentity(dimension());
        covariance(cov);

        full_rank_ = true;
        updated_internally(Rank);
    }

    /**
     * Changes the dimension of the dynamic-size Gaussian and sets it to a
     * standard distribution with zero mean and identity covariance.
     *
     * \param new_dimension New dimension of the Gaussian
     *
     * \cond internal
     * \pre {}
     * \post
     *  - Fully ranked covariance
     *  - {Valid representations} = {#CovarianceMatrix}
     * \endcond
     *
     * \throws ResizingFixedSizeEntityException
     *         see GaussianMap::standard_variate_dimension(int)
     */
    virtual void dimension(int new_dimension)
    {
        StdGaussianMappingInterface::standard_variate_dimension(new_dimension);
        set_standard();
    }

    /**
     * Sets the mean
     *
     * \param mean New Gaussian mean
     *
     * \throws WrongSizeException
     */
    virtual void mean(const Variate& mean) noexcept
    {
        if (mean_.size() != mean.size())
        {
            fl_throw(fl::WrongSizeException(mean.size(), mean_.size()));
        }

        mean_ = mean;
    }

    /**
     * Sets the covariance matrix as a diagonal matrix
     *
     * \param diag_covariance New diagonal covariance matrix
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalCovarianceMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void covariance(
        const DiagonalSecondMoment& diag_covariance) noexcept
    {
        if (diag_covariance.size() != covariance_.size())
        {
            fl_throw(
                fl::WrongSizeException(
                    diag_covariance.size(), covariance_.size()));
        }

        covariance_ = diag_covariance;
        updated_externally(DiagonalCovarianceMatrix);
    }

    /**
     * Sets the covariance matrix in its diagonal square root form
     *
     * \param diag_square_root New diagonal square root of the covariance
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalSquareRootMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void square_root(
        const DiagonalSecondMoment& diag_square_root) noexcept
    {
        if (diag_square_root.size() != square_root_.size())
        {
            fl_throw(
                fl::WrongSizeException(
                    diag_square_root.size(), square_root_.size()));
        }

        square_root_ = diag_square_root;
        updated_externally(DiagonalSquareRootMatrix);
    }

    /**
     * Sets the covariance matrix in its diagonal precision form
     *
     * \param diag_precision New diagonal precision matrix
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalPrecisionMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void precision(
        const DiagonalSecondMoment& diag_precision) noexcept
    {
        if (diag_precision.size() != precision_.size())
        {
            fl_throw(
                fl::WrongSizeException(
                    diag_precision.size(), precision_.size()));
        }

        precision_ = diag_precision;
        updated_externally(DiagonalPrecisionMatrix);
    }

    /**
     * Sets the covariance matrix as a diagonal matrix
     *
     * \param diag_covariance New diagonal covariance matrix
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalCovarianceMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void covariance(
        const Eigen::MatrixBase<DenseSecondMoment>& cov) noexcept
    {
        covariance(cov.diagonal().asDiagonal());
    }

    /**
     * Sets the covariance matrix in its diagonal square root form
     *
     * \param diag_square_root New diagonal square root of the covariance
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalSquareRootMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void square_root(
        const Eigen::MatrixBase<DenseSecondMoment>& sqrt) noexcept
    {
        square_root(sqrt.diagonal().asDiagonal());
    }

    /**
     * Sets the covariance matrix in its diagonal precision form
     *
     * \param diag_precision New diagonal precision matrix
     *
     * \cond internal
     * \pre |{Valid Representations}| > 0
     * \post {Valid Representations}
     *       = {Valid Representations} \f$ \cup \f$ {#DiagonalPrecisionMatrix}
     * \endcond
     *
     * \throws WrongSizeException
     */
    virtual void precision(
        const Eigen::MatrixBase<DenseSecondMoment>& prec) noexcept
    {
        precision(prec.diagonal().asDiagonal());
    }

protected:
    /** \cond internal */
    /**
     * Flags the specified attribute as valid and the rest of attributes as
     * dirty.
     *
     * \param attribute Modified attribute
     */
    virtual void updated_externally(Attribute attribute) const noexcept
    {
        std::fill(dirty_.begin(), dirty_.end(), true);
        updated_internally(attribute);
    }

    /**
     * Flags the specified attribute as valid.
     *
     * \param attribute Modified attribute
     */
    virtual void updated_internally(Attribute attribute) const noexcept
    {
        dirty_[attribute] = false;
    }

    /**
     * \return True if any of the other representation was modified.
     * \param attribute     Attribute in question
     */
    virtual bool is_dirty(Attribute attribute) const noexcept
    {
        return dirty_[int(attribute)];
    }

    /**
     * \return First representation ID that is available
     *
     * \param representations   Representation list
     *
     * Example:
     * If the last invoked functions were
     *
     * \code
     * diagonal_covariance(my_diagonal);
     * my_covariance = covariance();
     * \endcode
     *
     * Now, the representation is set to \c DiagonalCovarianceMatrix and
     * \c CovarianceMatrix since \c diagonal_covariance() was used to set the
     * covariance matrix followed by requesting \c covariance().
     * The following subsequent call
     *
     * \code
     * Attribute att = SelectRepresentation({SquareRoot,
     *                                       DiagonalCovarianceMatrix,
     *                                       CovarianceMatrix});
     * \endcode
     *
     * will assign att to DiagonalCovarianceMatrix since that is the first
     * available representation within the initializer-list
     * <tt>{#SquareRoot, #DiagonalCovarianceMatrix, #CovarianceMatrix}</tt>.
     *
     * This method is used to determine the best suitable representation
     * for conversion. It is recommanded to put the diagonal forms at the
     * beginning of the initialization-list. Diagonal forms can be converted
     * most efficiently other  representations.
     */
    template <int AttributeCount>
    Attribute select_first_representation(
        const std::array<Attribute, AttributeCount>& representations
    ) const noexcept
    {
        for (auto& rep: representations)  if (!is_dirty(rep)) return rep;
        return Attributes;
    }

    /**
     * \brief has_full_rank check implementation
     */
    virtual bool has_full_rank(const DiagonalSecondMoment& mat) const
    {
        bool full_rank = true;

        const auto& diag = mat.diagonal();

        for (int i = 0; i < diag.size(); ++i)
        {
            if (std::fabs(diag(i)) < 1e-24)
            {
                full_rank = false;
                break;
            }
        }

        return full_rank;
    }
    /** \endcond */

protected:
    /** \cond internal */
    Variate mean_;                            /**< \brief first moment vector */
    mutable DiagonalSecondMoment covariance_; /**< \brief cov. form */
    mutable DiagonalSecondMoment precision_;  /**< \brief cov. inverse form */
    mutable DiagonalSecondMoment square_root_;/**< \brief cov. square root  */
    mutable bool full_rank_;                  /**< \brief full rank flag */
    mutable Real log_norm_;                   /**< \brief log normalizing const */
    mutable Real determinant_;         /**< \brief determinant of covariance */
    mutable FlagArray dirty_;          /**< \brief data validity flags */
    /** \endcond */
};

/** \} */

}


#ifndef PWALK_UTIL_MATH_FUNCTIONS_HPP_
#define PWALK_UTIL_MATH_FUNCTIONS_HPP_

#include <Eigen/Dense>

namespace pwalk {

template <typename Dtype>
void sample_gaussian(const int n, const Dtype mu, const Dtype sigma, Eigen::Matrix<Dtype, Eigen::Dynamic, 1>& r);

template <typename Dtype>
Dtype rng_uniform(const Dtype a, const Dtype b);

// unnormalized gaussian density
template <typename Dtype>
Dtype gaussian_density(const Eigen::Matrix<Dtype, Eigen::Dynamic, 1>& x, const Eigen::Matrix<Dtype, Eigen::Dynamic, 1>& mu, const Eigen::Matrix<Dtype, Eigen::Dynamic, Eigen::Dynamic>& sqrt_inv_cov);

} // namespace pwalk

#endif // PWALK_UTIL_MATH_FUNTIONS_HPP_