/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "mitkNumericTwoTissueCompartmentModel.h"
#include "mitkAIFParametrizerHelper.h"
#include "mitkTimeGridHelper.h"
#include "mitkTwoTissueCompartmentModelDifferentialEquations.h"
#include <vnl/algo/vnl_fft_1d.h>
#include <boost/numeric/odeint.hpp>
#include <fstream>

const std::string mitk::NumericTwoTissueCompartmentModel::MODEL_DISPLAY_NAME =
  "Numeric Two Tissue Compartment Model";

const std::string mitk::NumericTwoTissueCompartmentModel::NAME_PARAMETER_K1 = "K1";
const std::string mitk::NumericTwoTissueCompartmentModel::NAME_PARAMETER_k2 = "k2";
const std::string mitk::NumericTwoTissueCompartmentModel::NAME_PARAMETER_k3 = "k3";
const std::string mitk::NumericTwoTissueCompartmentModel::NAME_PARAMETER_k4 = "k4";
const std::string mitk::NumericTwoTissueCompartmentModel::NAME_PARAMETER_VB = "V_B";

const std::string mitk::NumericTwoTissueCompartmentModel::UNIT_PARAMETER_K1 = "1/min";
const std::string mitk::NumericTwoTissueCompartmentModel::UNIT_PARAMETER_k2 = "1/min";
const std::string mitk::NumericTwoTissueCompartmentModel::UNIT_PARAMETER_k3 = "1/min";
const std::string mitk::NumericTwoTissueCompartmentModel::UNIT_PARAMETER_k4 = "1/min";
const std::string mitk::NumericTwoTissueCompartmentModel::UNIT_PARAMETER_VB = "ml/ml";

const unsigned int mitk::NumericTwoTissueCompartmentModel::POSITION_PARAMETER_K1 = 0;
const unsigned int mitk::NumericTwoTissueCompartmentModel::POSITION_PARAMETER_k2 = 1;
const unsigned int mitk::NumericTwoTissueCompartmentModel::POSITION_PARAMETER_k3 = 2;
const unsigned int mitk::NumericTwoTissueCompartmentModel::POSITION_PARAMETER_k4 = 3;
const unsigned int mitk::NumericTwoTissueCompartmentModel::POSITION_PARAMETER_VB = 4;

const unsigned int mitk::NumericTwoTissueCompartmentModel::NUMBER_OF_PARAMETERS = 5;

std::string mitk::NumericTwoTissueCompartmentModel::GetModelDisplayName() const
{
  return MODEL_DISPLAY_NAME;
};

std::string mitk::NumericTwoTissueCompartmentModel::GetModelType() const
{
  return "Dynamic.PET";
};

mitk::NumericTwoTissueCompartmentModel::NumericTwoTissueCompartmentModel()
{

}

mitk::NumericTwoTissueCompartmentModel::~NumericTwoTissueCompartmentModel()
{

}

mitk::NumericTwoTissueCompartmentModel::ParameterNamesType
mitk::NumericTwoTissueCompartmentModel::GetParameterNames() const
{
  ParameterNamesType result;

  result.push_back(NAME_PARAMETER_K1);
  result.push_back(NAME_PARAMETER_k2);
  result.push_back(NAME_PARAMETER_k3);
  result.push_back(NAME_PARAMETER_k4);
  result.push_back(NAME_PARAMETER_VB);

  return result;
}

mitk::NumericTwoTissueCompartmentModel::ParametersSizeType
mitk::NumericTwoTissueCompartmentModel::GetNumberOfParameters() const
{
  return NUMBER_OF_PARAMETERS;
}

mitk::NumericTwoTissueCompartmentModel::ParamterUnitMapType
mitk::NumericTwoTissueCompartmentModel::GetParameterUnits() const
{
  ParamterUnitMapType result;

  result.insert(std::make_pair(NAME_PARAMETER_K1, UNIT_PARAMETER_K1));
  result.insert(std::make_pair(NAME_PARAMETER_k2, UNIT_PARAMETER_k2));
  result.insert(std::make_pair(NAME_PARAMETER_k3, UNIT_PARAMETER_k3));
  result.insert(std::make_pair(NAME_PARAMETER_k4, UNIT_PARAMETER_k4));
  result.insert(std::make_pair(NAME_PARAMETER_VB, UNIT_PARAMETER_VB));

  return result;
};

mitk::NumericTwoTissueCompartmentModel::ModelResultType
mitk::NumericTwoTissueCompartmentModel::ComputeModelfunction(const ParametersType& parameters) const
{
  typedef itk::Array<double> ConcentrationCurveType;
  typedef std::vector<double> ConcentrationVectorType;

  if (this->m_TimeGrid.GetSize() == 0)
  {
    itkExceptionMacro("No Time Grid Set! Cannot Calculate Signal");
  }

  AterialInputFunctionType aterialInputFunction;
  aterialInputFunction = GetAterialInputFunction(this->m_TimeGrid);

  unsigned int timeSteps = this->m_TimeGrid.GetSize();

  /** @brief Boost::numeric::odeint works with type std::vector<double> thus, aif and grid are converted to ModelParameters( of type std::vector)
   */
  mitk::TwoTissueCompartmentModelDifferentialEquations::AIFType aif = mitk::convertArrayToParameter(
        aterialInputFunction);
  mitk::TwoTissueCompartmentModelDifferentialEquations::AIFType grid = mitk::convertArrayToParameter(
        m_TimeGrid);

  mitk::TwoTissueCompartmentModelDifferentialEquations::AIFType aifODE = aif;
  aifODE.push_back(aif[timeSteps - 1]);
  mitk::TwoTissueCompartmentModelDifferentialEquations::AIFType gridODE = grid;
  gridODE.push_back(grid[timeSteps - 1] + (grid[timeSteps - 1] - grid[timeSteps - 2]));

  //Model Parameters
  double K1 = (double)parameters[POSITION_PARAMETER_K1] / 60.0;
  double k2 = (double)parameters[POSITION_PARAMETER_k2] / 60.0;
  double k3 = (double)parameters[POSITION_PARAMETER_k3] / 60.0;
  double k4 = (double)parameters[POSITION_PARAMETER_k4] / 60.0;
  double VB = parameters[POSITION_PARAMETER_VB];

  /** @brief Initialize class TwpTissueCompartmentModelDifferentialEquations defining the differential equations. AIF and Grid must be set so that at step t the aterial Concentration Ca(t) can be interpolated from AIF*/
  mitk::TwoTissueCompartmentModelDifferentialEquations ode;
  ode.initialize(K1, k2, k3, k4);
  ode.setAIF(aifODE);
  ode.setAIFTimeGrid(gridODE);

  state_type x(2);
  x[0] = 0.0;
  x[1] = 0.0;
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<state_type> error_stepper_type;
  //typedef boost::numeric::odeint::runge_kutta4< state_type > stepper_type;

  /** @brief Results of odeeint x[0] and x[1]*/
  ConcentrationVectorType C1;
  ConcentrationVectorType C2;
  ConcentrationVectorType odeTimeGrid;

  error_stepper_type stepper;
  /** @brief Stepsize. Should be adapted by stepper (runge_kutta_cash_karp54) */
  const double dt = 0.1;
  /** @brief perform Step t -> t+dt to calculate approximate value x(t+dt)*/

  double T = this->m_TimeGrid(timeSteps - 1) + (grid[timeSteps - 1] - grid[timeSteps - 2]);

  for (double t = 0.0; t < T; t += dt)
  {
    stepper.do_step(ode, x, t, dt);
    C1.push_back(x[0]);
    C2.push_back(x[1]);
    odeTimeGrid.push_back(t);

  }

  /** @brief transfom result of Differential equations back to itk::Array and interpolate to m_TimeGrid (they are calculated on a different grid defined by stepsize of odeint)*/
  ConcentrationCurveType ConcentrationCompartment1 = mitk::convertParameterToArray(C1);
  ConcentrationCurveType ConcentrationCompartment2 = mitk::convertParameterToArray(C2);
  ConcentrationCurveType rungeKuttaTimeGrid = mitk::convertParameterToArray(odeTimeGrid);

  mitk::ModelBase::ModelResultType C_1 = mitk::InterpolateSignalToNewTimeGrid(
      ConcentrationCompartment1, rungeKuttaTimeGrid, m_TimeGrid);
  mitk::ModelBase::ModelResultType C_2 = mitk::InterpolateSignalToNewTimeGrid(
      ConcentrationCompartment2, rungeKuttaTimeGrid, m_TimeGrid);

  //Signal that will be returned by ComputeModelFunction
  mitk::ModelBase::ModelResultType signal(timeSteps);
  signal.fill(0.0);
  mitk::ModelBase::ModelResultType::iterator signalPos = signal.begin();
  mitk::ModelBase::ModelResultType::const_iterator C1Pos = C_1.begin();
  mitk::ModelBase::ModelResultType::const_iterator C2Pos = C_2.begin();

  for (AterialInputFunctionType::const_iterator aifpos = aterialInputFunction.begin();
       aifpos != aterialInputFunction.end(); ++aifpos, ++C1Pos, ++C2Pos, ++signalPos)
  {
    *signalPos = VB * (*aifpos) + (1 - VB) * (*C1Pos + *C2Pos);
  }

  return signal;

}

itk::LightObject::Pointer mitk::NumericTwoTissueCompartmentModel::InternalClone() const
{
  NumericTwoTissueCompartmentModel::Pointer newClone = NumericTwoTissueCompartmentModel::New();

  newClone->SetTimeGrid(this->m_TimeGrid);

  return newClone.GetPointer();
}

void mitk::NumericTwoTissueCompartmentModel::PrintSelf(std::ostream& os, ::itk::Indent indent) const
{
  Superclass::PrintSelf(os, indent);

}


//
// Created by Xinyu Zhang on 3/26/21.
//
// gcc -I/Users/xinyuzhang/Desktop/Spring2021/c++pattern/project/cosan /Users/xinyuzhang/Desktop/Spring2021/c++pattern/project/cosan/test/model/LinearRegressionTest.cpp
#include <iostream>
// #include <Eigen/Dense>
// #include <cosan/io/utils.h>
#include <cosan/data/CosanData.h>
#include <cosan/model/CosanLinearRegression.h>
#include <cosan/model/CosanRidgeRegression.h>
//using namespace Eigen;
//using namespace std;
#include <fmt/format.h>
int main() {
   Cosan::CosanRawData CD("./example_data/toy/X.csv","./example_data/toy/y.csv");
   Cosan::CosanLinearRegression CLRwbias(true);
   CLRwbias.fit(CD.GetInput(),CD.GetTarget());
   std::cout<<CLRwbias.GetBeta()<<std::endl;
   std::cout<<(CLRwbias.predict(CD.GetInput())-CD.GetTarget()).norm()<<std::endl;
   Cosan::CosanLinearRegression CLRwobias(false);
   CLRwobias.fit(CD.GetInput(),CD.GetTarget());
   std::cout<<CLRwobias.GetBeta()<<std::endl;
   std::cout<<(CLRwobias.predict(CD.GetInput())-CD.GetTarget()).norm()<<std::endl;

   std::string output = fmt::format("The answer is {}.", 42);
   return 0;
}


#include <stan/math/mix/mat.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/mat/prob/lkj_corr_cholesky_test_functors.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <test/unit/math/mix/mat/prob/higher_order_utils.hpp>
#include <vector>

TEST(ProbDistributionsLkjCorr, fvar_var) {
  using stan::math::fvar;
  using stan::math::var;
  boost::random::mt19937 rng;
  int K = 4;
  Eigen::Matrix<fvar<var>, Eigen::Dynamic, Eigen::Dynamic> Sigma(K, K);
  Sigma.setZero();
  Sigma.diagonal().setOnes();
  for (int i = 0; i < K * K; i++)
    Sigma(i).d_ = 1.0;
  fvar<var> eta = stan::math::uniform_rng(0, 2, rng);
  fvar<var> f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_.val(),
                  stan::math::lkj_corr_log(Sigma, eta).val_.val());
  EXPECT_FLOAT_EQ(2.5177896, stan::math::lkj_corr_log(Sigma, eta).d_.val());
  eta = 1.0;
  f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_.val(),
                  stan::math::lkj_corr_log(Sigma, eta).val_.val());
  EXPECT_FLOAT_EQ(f.d_.val(), stan::math::lkj_corr_log(Sigma, eta).d_.val());
}

TEST(ProbDistributionsLkjCorrCholesky, fvar_var) {
  using stan::math::fvar;
  using stan::math::var;
  boost::random::mt19937 rng;
  int K = 4;
  Eigen::Matrix<fvar<var>, Eigen::Dynamic, Eigen::Dynamic> Sigma(K, K);
  Sigma.setZero();
  Sigma.diagonal().setOnes();
  for (int i = 0; i < K * K; i++)
    Sigma(i).d_ = 1.0;
  fvar<var> eta = stan::math::uniform_rng(0, 2, rng);
  fvar<var> f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_.val(),
                  stan::math::lkj_corr_cholesky_log(Sigma, eta).val_.val());
  EXPECT_FLOAT_EQ(6.7766843,
                  stan::math::lkj_corr_cholesky_log(Sigma, eta).d_.val());
  eta = 1.0;
  f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_.val(),
                  stan::math::lkj_corr_cholesky_log(Sigma, eta).val_.val());
  EXPECT_FLOAT_EQ(3, stan::math::lkj_corr_cholesky_log(Sigma, eta).d_.val());
}

TEST(ProbDistributionsLkjCorr, fvar_fvar_var) {
  using stan::math::fvar;
  using stan::math::var;
  boost::random::mt19937 rng;
  int K = 4;
  Eigen::Matrix<fvar<fvar<var> >, Eigen::Dynamic, Eigen::Dynamic> Sigma(K, K);
  Sigma.setZero();
  Sigma.diagonal().setOnes();
  for (int i = 0; i < K * K; i++)
    Sigma(i).d_.val_ = 1.0;
  fvar<fvar<var> > eta = stan::math::uniform_rng(0, 2, rng);
  fvar<fvar<var> > f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_.val_.val(),
                  stan::math::lkj_corr_log(Sigma, eta).val_.val_.val());
  EXPECT_FLOAT_EQ(2.5177896,
                  stan::math::lkj_corr_log(Sigma, eta).d_.val_.val());
  eta = 1.0;
  f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(f.val_.val_.val(),
                  stan::math::lkj_corr_log(Sigma, eta).val_.val_.val());
  EXPECT_FLOAT_EQ(f.d_.val_.val(),
                  stan::math::lkj_corr_log(Sigma, eta).d_.val_.val());
}

TEST(ProbDistributionsLkjCorrCholesky, fvar_fvar_var) {
  using stan::math::fvar;
  using stan::math::var;
  boost::random::mt19937 rng;
  int K = 4;
  Eigen::Matrix<fvar<fvar<var> >, Eigen::Dynamic, Eigen::Dynamic> Sigma(K, K);
  Sigma.setZero();
  Sigma.diagonal().setOnes();
  for (int i = 0; i < K * K; i++)
    Sigma(i).d_.val_ = 1.0;
  fvar<fvar<var> > eta = stan::math::uniform_rng(0, 2, rng);
  fvar<fvar<var> > f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(
      f.val_.val_.val(),
      stan::math::lkj_corr_cholesky_log(Sigma, eta).val_.val_.val());
  EXPECT_FLOAT_EQ(6.7766843,
                  stan::math::lkj_corr_cholesky_log(Sigma, eta).d_.val_.val());
  eta = 1.0;
  f = stan::math::do_lkj_constant(eta, K);
  EXPECT_FLOAT_EQ(
      f.val_.val_.val(),
      stan::math::lkj_corr_cholesky_log(Sigma, eta).val_.val_.val());
  EXPECT_FLOAT_EQ(3,
                  stan::math::lkj_corr_cholesky_log(Sigma, eta).d_.val_.val());
}

TEST(ProbDistributionsLkjCorrCholesky, hessian) {
  int dim_mat = 3;
  Eigen::Matrix<double, Eigen::Dynamic, 1> x1(dim_mat);
  Eigen::Matrix<double, Eigen::Dynamic, 1> x2(1);
  Eigen::Matrix<double, Eigen::Dynamic, 1> x3(dim_mat + 1);

  x2(0) = 2.0;

  for (int i = 0; i < dim_mat; ++i) {
    x1(i) = i / 10.0;
    x3(i + 1) = x1(i);
  }
  x3(0) = 0.5;

  stan::math::lkj_corr_cholesky_dc test_func_1(dim_mat);
  stan::math::lkj_corr_cholesky_cd test_func_2(dim_mat);
  stan::math::lkj_corr_cholesky_dd test_func_3(dim_mat);

  using stan::math::finite_diff_hessian;
  using stan::math::hessian;

  Eigen::Matrix<double, Eigen::Dynamic, 1> grad_hess_3;
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_3;
  double fx_hess_3;
  Eigen::Matrix<double, Eigen::Dynamic, 1> grad_hess_ad_3;
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_ad_3;
  double fx_hess_ad_3;

  finite_diff_hessian(test_func_3, x3, fx_hess_3, grad_hess_3, hess_3);
  hessian(test_func_3, x3, fx_hess_ad_3, grad_hess_ad_3, hess_ad_3);

  test_hess_eq(hess_3, hess_ad_3);
  EXPECT_FLOAT_EQ(fx_hess_3, fx_hess_ad_3);

  Eigen::Matrix<double, Eigen::Dynamic, 1> grad_hess_2;
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_2;
  double fx_hess_2;
  Eigen::Matrix<double, Eigen::Dynamic, 1> grad_hess_ad_2;
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_ad_2;
  double fx_hess_ad_2;

  finite_diff_hessian(test_func_2, x2, fx_hess_2, grad_hess_2, hess_2);
  hessian(test_func_2, x2, fx_hess_ad_2, grad_hess_ad_2, hess_ad_2);

  test_hess_eq(hess_2, hess_ad_2);
  EXPECT_FLOAT_EQ(fx_hess_2, fx_hess_ad_2);

  Eigen::Matrix<double, Eigen::Dynamic, 1> grad_hess_1;
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_1;
  double fx_hess_1;
  Eigen::Matrix<double, Eigen::Dynamic, 1> grad_hess_ad_1;
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_ad_1;
  double fx_hess_ad_1;

  finite_diff_hessian(test_func_1, x1, fx_hess_1, grad_hess_1, hess_1);
  hessian(test_func_1, x1, fx_hess_ad_1, grad_hess_ad_1, hess_ad_1);

  test_hess_eq(hess_1, hess_ad_1);
  EXPECT_FLOAT_EQ(fx_hess_1, fx_hess_ad_1);
}

TEST(ProbDistributionsLkjCorrCholesky, grad_hessian) {
  int dim_mat = 3;
  Eigen::Matrix<double, Eigen::Dynamic, 1> x1(dim_mat);
  Eigen::Matrix<double, Eigen::Dynamic, 1> x2(1);
  Eigen::Matrix<double, Eigen::Dynamic, 1> x3(dim_mat + 1);

  x2(0) = 2.0;

  for (int i = 0; i < dim_mat; ++i) {
    x1(i) = i / 10.0;
    x3(i + 1) = x1(i);
  }
  x3(0) = 0.5;

  stan::math::lkj_corr_cholesky_dc test_func_1(dim_mat);
  stan::math::lkj_corr_cholesky_cd test_func_2(dim_mat);
  stan::math::lkj_corr_cholesky_dd test_func_3(dim_mat);

  using stan::math::finite_diff_grad_hessian;
  using stan::math::grad_hessian;

  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_gh_3;
  std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > gh_3;
  double fx_gh_3;
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_gh_ad_3;
  std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > gh_ad_3;
  double fx_gh_ad_3;

  finite_diff_grad_hessian(test_func_3, x3, fx_gh_3, hess_gh_3, gh_3);
  grad_hessian(test_func_3, x3, fx_gh_ad_3, hess_gh_ad_3, gh_ad_3);

  test_grad_hess_eq(gh_3, gh_ad_3);
  EXPECT_FLOAT_EQ(fx_gh_3, fx_gh_ad_3);

  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_gh_2;
  std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > gh_2;
  double fx_gh_2;
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_gh_ad_2;
  std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > gh_ad_2;
  double fx_gh_ad_2;

  finite_diff_grad_hessian(test_func_2, x2, fx_gh_2, hess_gh_2, gh_2);
  grad_hessian(test_func_2, x2, fx_gh_ad_2, hess_gh_ad_2, gh_ad_2);

  test_grad_hess_eq(gh_2, gh_ad_2);
  EXPECT_FLOAT_EQ(fx_gh_2, fx_gh_ad_2);

  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_gh_1;
  std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > gh_1;
  double fx_gh_1;
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> hess_gh_ad_1;
  std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > gh_ad_1;
  double fx_gh_ad_1;

  finite_diff_grad_hessian(test_func_1, x1, fx_gh_1, hess_gh_1, gh_1);
  grad_hessian(test_func_1, x1, fx_gh_ad_1, hess_gh_ad_1, gh_ad_1);

  test_grad_hess_eq(gh_1, gh_ad_1);
  EXPECT_FLOAT_EQ(fx_gh_1, fx_gh_ad_1);
}


/**
 *  Testival.cpp
 *
 *  Test interval class
 *
 *  Created by Yinan Li on July 20, 2016.
 *
 *  Hybrid Systems Group, University of Waterloo.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE IntervalClass

#include <boost/test/unit_test.hpp>
//#include <boost/test/unit_test_log.hpp>
#include <cmath>
#include <iomanip>
#include "src/interval.h"

/*
  test case 1: initialization
*/
BOOST_AUTO_TEST_CASE(test_init_basics)
{
    rocs::interval a(2,1);
    rocs::interval b(0.2, 1.5);
    rocs::interval c(b);
    rocs::interval d = b;
    rocs::interval e(rocs::PINF, rocs::PINF);

    /* test isempty() */
    BOOST_CHECK(a.isempty());
    BOOST_CHECK(!b.isempty());
    BOOST_CHECK(!c.isempty());
    BOOST_CHECK(!e.isempty());

    /* test equivalence function */
    BOOST_CHECK(c == b);
    BOOST_CHECK(d == b);
    BOOST_CHECK(a != b);

    /* test set operations */
    BOOST_CHECK(b.isin(0.2));

    /* test print function */
//    std::cout << b<< std::endl;
}

/*
  test case 2: + -
*/
BOOST_AUTO_TEST_CASE(test_plus_minus)
{
    using namespace rocs;

    interval e(2,1); // empty interval

    interval a(0.2, 1.5);
    interval b(-0.3, -0.0001);
    interval c = interval(-2.33, 1.09);

    /* test plus */
    BOOST_CHECK(interval(0.4, 3) == a + a);
    BOOST_CHECK(interval(-0.1, 1.4999) == b + a);
    BOOST_CHECK(interval(-0.1, 1.4999) == a + b);
    BOOST_CHECK(interval(NAN, NAN) == a + e);
    BOOST_CHECK(interval(7.2, 8.5) == a + 7);
    BOOST_CHECK(interval(-102.33, -98.91) == -100 + c);
    BOOST_CHECK(interval(NINF, PINF) == c + interval(NINF, PINF)); // support [-oo,+oo]

    /* test minus */
    BOOST_CHECK(interval(0.2001, 1.8) == a - b);
    BOOST_CHECK(interval(-1.8, -0.2001) == b - a);
    BOOST_CHECK(interval(-1.39, 2.3299) == b - c);
    BOOST_CHECK(interval(NAN, NAN) == a - e); // support NAN
    BOOST_CHECK(interval(-11.95, -11.6501) == b - 11.65);
    BOOST_CHECK(interval(-104.13, -100.71) == -103.04 - c);
    BOOST_CHECK(interval(NINF, PINF) == c - interval(NINF, PINF)); // support [-oo, +oo]

    //std::cout << PINF - PINF << "\n";
    //std::cout << (PINF == PINF);

}

/*
  test case 3: * /
*/
BOOST_AUTO_TEST_CASE(test_mul_div)
{
    using namespace rocs;

    interval e(2,1); // empty interval

    interval a(0.2, 1.5);
    interval b(-0.3, -0.0001);
    interval c(-2.33, 1.09);
    interval d(0, 1);
    interval g(-1.15, 0);

    interval p1(-1, PINF);
    interval p2(0,PINF);
    interval p3(3, PINF);

    interval np(NINF, PINF);

    interval n1(NINF, 5);
    interval n2(NINF,0);
    interval n3(NINF, -0.02);

    /* mul */
    BOOST_CHECK(interval(0.04, 2.25) == a * a);
    BOOST_CHECK(interval(-2.33*1.5, 1.09*1.5) == a * c);
    BOOST_CHECK(interval(-0.45, -0.00002) == a * b);
    BOOST_CHECK(interval(-2.33*1.5, 1.09*1.5) == c * a);
    BOOST_CHECK(interval(-2.5397, 5.4289) == c * c);
    BOOST_CHECK(interval(-0.327, 0.699) == c * b);
    BOOST_CHECK(interval(-0.45, -0.00002) == b * a);
    BOOST_CHECK(interval(-0.327, 0.699) == b * c);
    BOOST_CHECK(interval(0.00000001, 0.09) == b * b);
    BOOST_CHECK(interval(-0.3, 0) == d * b); //contain 0
    BOOST_CHECK(interval(-2.33, 1.09) == c * d);
    BOOST_CHECK(interval(-1.15*1.5, 0) == g * a);
    BOOST_CHECK(interval(0, 1.15*0.3) == g * b);
    BOOST_CHECK(interval(-1.15*1.09, 2.33*1.15) == c * g);
    BOOST_CHECK(interval(-1.15*1.09, 2.33*1.15) == g * c);
    BOOST_CHECK(interval(NAN, NAN) == e * a); //empty
    BOOST_CHECK(interval(-1.5, PINF) == p1 * a); //infinity
    BOOST_CHECK(interval(0, PINF) == p2 * a);
    BOOST_CHECK(interval(NINF, -0.0003) == b * p3);
    BOOST_CHECK(interval(NINF, 7.5) == a * n1);
    BOOST_CHECK(interval(0, PINF) == n2 * b);
    BOOST_CHECK(interval(NINF, PINF) == c * n3);
    BOOST_CHECK(interval(NINF, PINF) == p1 * n1);
    BOOST_CHECK(interval(NINF, PINF) == n1 * n2);
    BOOST_CHECK(interval(NINF, PINF) == n1 * n3);
    BOOST_CHECK(interval(0, PINF) == n2 * n3);
    BOOST_CHECK(interval(NINF, PINF) == np * np);
    BOOST_CHECK(interval(0, 0) == 0 * a);
    BOOST_CHECK(interval(0, 0) == a * 0);
    BOOST_CHECK(interval(0, 0) == 0 * p1);
    BOOST_CHECK(interval(0, 0) == 0 * np);
    BOOST_CHECK(interval(-1, PINF) == 1 * p1);
    BOOST_CHECK(interval(0.00005, 0.15) == -0.5 * b);
    BOOST_CHECK(interval(0, PINF) == -0.5 * n2);

    /* div */
    BOOST_CHECK(interval(0.2/1.5, 1.5/0.2) == a / a);
    BOOST_CHECK(interval(-15000, -0.2/0.3) == a / b);
    BOOST_CHECK(interval(-0.3/0.2, -0.0001/1.5) == b / a);
    BOOST_CHECK(interval(NINF, PINF) == a / c);
    BOOST_CHECK(interval(-2.33/0.2, 1.09/0.2) == c / a);
    BOOST_CHECK(interval(-10900, 23300) == c / b);
    BOOST_CHECK(interval(0, 5) == d / a);
    BOOST_CHECK(interval(0.2, PINF) == a / d);
    BOOST_CHECK(interval(-5.75, 0) == g / a);
    BOOST_CHECK(interval(NINF, -0.2/1.15) == a / g);
    BOOST_CHECK(interval(0, 11500) == g / b);
    BOOST_CHECK(interval(NINF, PINF) == d / g);
    BOOST_CHECK(interval(NAN, NAN) == a / interval(0, 0));
    BOOST_CHECK(interval(0, 0) == 0 / a);
    BOOST_CHECK(interval(0, 0) == 0 / b);
    BOOST_CHECK(interval(NAN, NAN) == 0 / c); // num = 0, 0\in den
    BOOST_CHECK(interval(NAN, NAN) == 0 / g);
    BOOST_CHECK(interval(NAN, NAN) == 0 / d);
    BOOST_CHECK(interval(1/1.5, 5) == 1 / a); // num > 0
    BOOST_CHECK(interval(-10000, -1/0.3) == 1 / b);
    BOOST_CHECK(interval(NINF, PINF) == 1 / c);
    BOOST_CHECK(interval(1, PINF) == 1 / d);
    BOOST_CHECK(interval(NINF, -1/1.15) == 1 / g);
    BOOST_CHECK(interval(-5, -1/1.5) == -1 / a);
    BOOST_CHECK(interval(1/0.3, 10000) == -1 / b);
    BOOST_CHECK(interval(NINF, PINF) == -1 / c);
    BOOST_CHECK(interval(NINF, -1) == -1 / d);
    BOOST_CHECK(interval(1/1.15, PINF) == -1 / g);
}

/*
  test case 4: sin(), cos(), tan(), atan()
*/
BOOST_AUTO_TEST_CASE(test_trigonometric_fcns)
{
    using namespace rocs;

    /* test sin() [cos() is implemented by sin()] */
    BOOST_CHECK(interval(-1, sin(-1.1)) == sin(interval(-1.6, -1.1))); //wid = 0.5
    BOOST_CHECK(interval(sin(-1), sin(-0.5)) == sin(interval(-1, -0.5)));
    BOOST_CHECK(interval(sin(-0.2), sin(0.3)) == sin(interval(-0.2, 0.3))); //0
    std::cout << sin(interval(-0.2, 0.3)) << '\n';
    BOOST_CHECK(interval(-1, sin(5)) == sin(interval(4.5, 5))); //3pi/2

    BOOST_CHECK(interval(sin(-1), sin(0.57)) == sin(interval(-1, 0.57))); //wid = 1.57
    BOOST_CHECK(interval(0, 1) == sin(interval(0, 1.571))); //pi/2
    BOOST_CHECK(interval(-1, sin(3.14)) == sin(interval(3.14, 4.72))); //3pi/2

    BOOST_CHECK(interval(-1, sin(-3.2)) == sin(interval(-3.2, -3.2+PIIVAL))); //wid = pi
    std::cout << sin(interval(-3.2, -3.2+PIIVAL)) << '\n';
    BOOST_CHECK(interval(-1, 0) == sin(interval(-PIIVAL, 0)));
    BOOST_CHECK(interval(-1, 1) == sin(interval(-PIIVAL/2, PIIVAL/2)));
    BOOST_CHECK(interval(sin(-1), 1) == sin(interval(-1, -1+PIIVAL)));

    BOOST_CHECK(interval(sin(-1), 1) == sin(interval(-1, 3.4))); //wid = 4.3
    BOOST_CHECK(interval(-1, 1) == sin(interval(1.5, 5.8)));
    BOOST_CHECK(interval(-1, sin(7.4)) == sin(interval(3.1, 7.4)));
    BOOST_CHECK(interval(-1, 1) == sin(interval(7, 11.3)));

    BOOST_CHECK(interval(-1, 1) == sin(interval(0, PI2IVAL))); //wid = 2pi
    BOOST_CHECK(interval(-1, 1) == sin(interval(PI2IVAL, 2*PI2IVAL)));
    BOOST_CHECK(interval(-1, 1) == sin(interval(-0.5, -0.5+PI2IVAL)));

    BOOST_CHECK(interval(-1, 1) == sin(interval(PIIVAL, 5*PIIVAL))); //wid = 4pi
    BOOST_CHECK(interval(-1, 1) == sin(interval(-6, 1))); //wid = 7

    /* tan() */
    BOOST_CHECK(interval(NINF, PINF) == tan(interval(-1.6, 2.54))); //wid = 4.14 > pi
    // wid = 3.14 < pi, pi/2 \in [inf, sup]
    BOOST_CHECK(interval(NINF, PINF) == tan(interval(-5.14, -2)));
    BOOST_CHECK(interval(tan(4.3), tan(4.5)) == tan(interval(4.3, 4.5)));  // k+=1 branch
    std::cout << tan(interval(4.3, 4.5)) << '\n';
    BOOST_CHECK(interval(NINF, PINF) == tan(interval(4.3, 4.8)));  // k+=1 branch
    BOOST_CHECK(interval(tan(-2), tan(-1.7)) == tan(interval(-2, -1.7)));  // k+=1 branch
    std::cout << tan(interval(-2, -1.7)) << '\n';
}

/*
  test case 5: sqr(), sqrt(), power()
*/
BOOST_AUTO_TEST_CASE(test_power_root)
{
    using namespace rocs;

    BOOST_CHECK(interval(0.01, 25) == sqr(interval(-5, -0.1))); //sqr
    BOOST_CHECK(interval(0.01, 25) == sqr(interval(0.1, 5)));
    BOOST_CHECK(interval(0, 25) == sqr(interval(-5, 0.1)));
    BOOST_CHECK(interval(0, 0.25) == sqr(interval(0, 0.5)));
    BOOST_CHECK(interval(0, 0.25) == sqr(interval(-0.5, 0)));

    BOOST_CHECK(pow(interval(0, 0.5), 2) == sqr(interval(0, 0.5))); //pow
    BOOST_CHECK(pow(interval(0, 0.5), 2) == sqr(interval(0, 0.5)));
    BOOST_CHECK(interval(-1, 8) == pow(interval(-1, 2), 3));
    BOOST_CHECK(interval(0, 1) == pow(interval(-1, 1), 6));
    BOOST_CHECK(interval(-1, 2) == pow(interval(-1, 2), 1));

    BOOST_CHECK(interval(1, 2) == sqrt(interval(1, 4)));//sqrt
    BOOST_CHECK(interval(0, 2) == sqrt(interval(-1, 4)));
    BOOST_CHECK(interval(0, 0) == sqrt(interval(-1, 0)));
}

/*
  test case 6: intersect, hull
*/
BOOST_AUTO_TEST_CASE(test_inter_hull)
{
    using namespace rocs;

    interval a(-0.7, 0.2);
    interval b(0, 0.5);
    interval c(-0.5, 0);
    interval d(-3, -0.6);
    interval e(NINF, -2);
    interval f(0.3, PINF);
    interval g(NINF, PINF);

    BOOST_CHECK(interval(NAN, NAN) == intersect(b, d)); //intersect
    BOOST_CHECK(c == intersect(a, c));
    BOOST_CHECK(interval(0, 0.2) == intersect(a, b));
    BOOST_CHECK(interval(0, 0) == intersect(b, c));
    BOOST_CHECK(interval(-3, -2) == intersect(e, d));
    BOOST_CHECK(interval(NAN, NAN) == intersect(e, a));
    BOOST_CHECK(interval(0.3, 0.5) == intersect(b, f));
    BOOST_CHECK(a == intersect(a, g));

    BOOST_CHECK(interval(-3, 0.5) == hull(b, d)); //hull
    BOOST_CHECK(interval(-0.7, 0.5) == hull(a, b));
    BOOST_CHECK(interval(-0.5, 0.5) == hull(b, c));
    BOOST_CHECK(interval(NINF, -0.6) == hull(e, d));
    BOOST_CHECK(interval(NINF, 0.2) == hull(e, a));
    BOOST_CHECK(a == hull(a, c));
    BOOST_CHECK(g == hull(a, g));
}

/*
  test case 7: bisections
*/
BOOST_AUTO_TEST_CASE(test_bisection)
{
    rocs::interval a(-0.7, 0.2);

    rocs::interval left = lowerhalf(a);
    rocs::interval right = upperhalf(a);

    BOOST_CHECK(rocs::interval(-0.7, -0.25) == left);
    BOOST_CHECK(rocs::interval(-0.25, 0.2) == right);
}


#include <algorithm>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <iostream>
#include <list>
#include <map>
#include <set>
#include <vector>

using Children = std::vector<int>;
using Map = std::map<int, Children>;

int main()
{
    Map map;

    std::string line;
    while (std::getline(std::cin, line)) {
        std::vector<std::string> words;
        boost::algorithm::split(
                words, line, boost::is_any_of(", "), boost::algorithm::token_compress_on);

        auto it = words.begin();
        int k = std::stoi(&(*it++)[0]);

        if (*it++ != "<->") {
            std::cerr << "ERROR: " << line << "\n";
            return 1;
        }

        for (; it != words.end(); ++it) {
            map[k].push_back(std::stoi(&(*it)[0]));
        }
    }

    std::list<int> queue;
    queue.push_back(0);
    std::set<int> zero_group;
    zero_group.insert(0);

    for(;!queue.empty();) {
        int cur = queue.front();
        queue.pop_front();
        const auto& next = map[cur];
        for (auto i : next) {
            if (zero_group.insert(i).second) {
                queue.push_back(i);
            }
        }
    }

    std::cout << "zero_group: " << zero_group.size() << "\n";
}


//
// Created by michel on 18-08-19.
//

#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>
#include <simple-dsp/core/alignment.h>
#include <simple-dsp/core/bounds.h>
#include <simple-dsp/core/size.h>

#include "test-helper.h"

namespace {

constexpr size_t maximumSize = std::numeric_limits<size_t>::max();

/**
 * Defines a wrapper for power of two-related implementations.
 */
struct PowerOfTwoImplementation {

  sdsp_nodiscard virtual const char *name() const = 0;

  sdsp_nodiscard virtual size_t nextOrSame(size_t size) const = 0;

  sdsp_nodiscard virtual bool is(size_t size) const = 0;

  sdsp_nodiscard virtual bool minusOne(size_t size) const = 0;

  sdsp_nodiscard virtual size_t alignedWith(size_t value,
                                            size_t power) const = 0;

  virtual ~PowerOfTwoImplementation() = default;
};

template <bool USE_CONSTEXPR>
struct SubjectImpl : public PowerOfTwoImplementation {
  using Impl = simpledsp::Power2;

  sdsp_nodiscard const char *name() const override {
    return USE_CONSTEXPR ? "Power2::constant" : "Power2";
  }

  sdsp_nodiscard size_t nextOrSame(size_t size) const override {
    return Impl::same_or_bigger(size);
  }

  sdsp_nodiscard bool is(size_t size) const override { return Impl::is(size); }

  sdsp_nodiscard bool minusOne(size_t size) const override {
    return Impl::is_minus_one(size);
  }

  sdsp_nodiscard size_t alignedWith(size_t value, size_t power) const override {
    return simpledsp::get_aligned_with(value, power);
  }
};

struct ReferenceImpl : public PowerOfTwoImplementation {

  sdsp_nodiscard const char *name() const override { return "Reference"; }

  sdsp_nodiscard size_t nextOrSame(size_t size) const override {
    if (size <= 2) {
      return 2;
    }

    for (size_t test = 2; test > 0; test *= 2) {
      if (test >= size) {
        return test;
      }
    }

    return 0;
  }

  sdsp_nodiscard bool is(size_t size) const override {
    if (size < 2) {
      return false;
    }
    for (size_t test = 2; test > 0; test *= 2) {
      if (test == size) {
        return true;
      };
    }
    return size == 1 + (maximumSize / 2);
  }

  sdsp_nodiscard bool minusOne(size_t size) const override {
    if (size == maximumSize) {
      return true;
    }
    if (size == 0) {
      return false;
    }
    for (size_t test = 2; test > 0; test *= 2) {
      if (test > size) {
        return size == test - 1;
      }
    }
    return false;
  }

  sdsp_nodiscard size_t alignedWith(size_t value, size_t power) const override {
    if (!is(power)) {
      return 0;
    }
    if (value == 0) {
      return 0;
    }
    return power * ((value + power - 1) / power);
  }

} referenceImplementation;

template <typename T, typename A, class P>
using AbstractPower2TestCase =
    simpledsp::testhelper::CompareWithReferenceTestCase<T, A, P>;

template <typename T, typename A>
class Power2TestCase
    : public AbstractPower2TestCase<T, A, PowerOfTwoImplementation> {
  const std::string name;

public:
  Power2TestCase(const PowerOfTwoImplementation &subject, A arg)
      : AbstractPower2TestCase<T, A, PowerOfTwoImplementation>(
            referenceImplementation, subject, arg),
        name(subject.name()) {}

  Power2TestCase(const PowerOfTwoImplementation &subject, A arg1, A arg2)
      : AbstractPower2TestCase<T, A, PowerOfTwoImplementation>(
            referenceImplementation, subject, arg1, arg2),
        name(subject.name()) {}

  sdsp_nodiscard const char *typeOfTestName() const override {
    return name.c_str();
  }
};

struct IsPowerTestCase : public Power2TestCase<bool, size_t> {

  IsPowerTestCase(const size_t value, const PowerOfTwoImplementation &subject)
      : Power2TestCase<bool, size_t>(subject, value) {}

  sdsp_nodiscard const char *methodName() const override { return "is"; }

  sdsp_nodiscard bool
  generateValue(const PowerOfTwoImplementation &impl) const override {
    return impl.is(getArgument(0));
  }
};

struct NextPowerTestCase : public Power2TestCase<size_t, size_t> {

  NextPowerTestCase(const size_t value, const PowerOfTwoImplementation &subject)
      : Power2TestCase<size_t, size_t>(subject, value) {}

  sdsp_nodiscard const char *methodName() const override {
    return "same_or_bigger";
  }

  sdsp_nodiscard size_t
  generateValue(const PowerOfTwoImplementation &impl) const override {
    return impl.nextOrSame(getArgument(0));
  }
};

struct IsMinusOneTestCase : public Power2TestCase<bool, size_t> {

  IsMinusOneTestCase(const size_t value,
                     const PowerOfTwoImplementation &subject)
      : Power2TestCase<bool, size_t>(subject, value) {}

  sdsp_nodiscard const char *methodName() const override {
    return "is_minus_one";
  }

  sdsp_nodiscard bool
  generateValue(const PowerOfTwoImplementation &impl) const override {
    return impl.minusOne(getArgument(0));
  }
};

struct AlignedWithTestCase : public Power2TestCase<size_t, size_t> {

  AlignedWithTestCase(const size_t offset, size_t power,
                      const PowerOfTwoImplementation &subject)
      : Power2TestCase<size_t, size_t>(subject, offset, power) {}

  sdsp_nodiscard const char *methodName() const override {
    return "is_aligned_with";
  }

  sdsp_nodiscard size_t
  generateValue(const PowerOfTwoImplementation &impl) const override {
    return impl.alignedWith(getArgument(0), getArgument(1));
  }

  sdsp_nodiscard const char *getArgumentName(size_t i) const override {
    return i == 0 ? "offset" : i == 1 ? "powerOfTwoValue" : nullptr;
  }
};

SubjectImpl<true> constant;
SubjectImpl<false> runtime;

struct TestSet {
  using TestCase = simpledsp::testhelper::AbstractValueTestCase;

  std::vector<const TestCase *> getTestCases() { return testCases; }

  TestSet() {
    std::vector<size_t> powerTestValues;
    for (size_t i = 2, j = 1; i > j; j = i, i *= 2) {
      addIfAbsent(powerTestValues, j - 1);
      addIfAbsent(powerTestValues, j);
      addIfAbsent(powerTestValues, j + 1);
    }
    addIfAbsent(powerTestValues, std::numeric_limits<size_t>::max() - 1);
    addIfAbsent(powerTestValues, std::numeric_limits<size_t>::max());

    for (size_t value : powerTestValues) {
      testCases.emplace_back(new IsPowerTestCase(value, runtime));
      testCases.emplace_back(new IsPowerTestCase(value, constant));
      testCases.emplace_back(new IsMinusOneTestCase(value, runtime));
      testCases.emplace_back(new IsMinusOneTestCase(value, constant));
      testCases.emplace_back(new NextPowerTestCase(value, runtime));
      testCases.emplace_back(new NextPowerTestCase(value, constant));
    }
    for (size_t offset : powerTestValues) {
      for (size_t powerOfTwo : powerTestValues) {
        if (offset < 10000000 && powerOfTwo < 128 &&
            referenceImplementation.is(powerOfTwo)) {
          testCases.emplace_back(
              new AlignedWithTestCase(offset, powerOfTwo, runtime));
          testCases.emplace_back(
              new AlignedWithTestCase(offset, powerOfTwo, constant));
        }
      }
    }
  }

  ~TestSet() {
    for (const TestCase *testCase : testCases) {
      delete testCase;
    }
  }

private:
  std::vector<const TestCase *> testCases;

  static void addIfAbsent(std::vector<size_t> &values, size_t value) {
    if (!contains(values, value)) {
      values.push_back(value);
    }
  }

  static bool contains(const std::vector<size_t> &haystack, size_t needle) {
    for (size_t value : haystack) {
      if (value == needle) {
        return true;
      }
    }
    return false;
  }
} TEST_SET;

struct MsbTestScenario {
  size_t input;
  int expected;
  int (*function)(size_t);
  const char *name;

  static MsbTestScenario most_significant_bit(size_t value, int expected) {
    return {value, expected, simpledsp::Bits<size_t>::most_significant,
            "Power2For::most_significant_bit"};
  }

  static MsbTestScenario most_significant_single_bit(size_t value,
                                                     int expected) {
    return {value, expected,
            simpledsp::Bits<size_t>::most_significant_single,
            "Power2For::most_significant_single_bit"};
  }

  bool success() const { return expected == function(input); }

  void print(std::ostream &out) const {
    out << name << "(" << input << ")";
    if (success()) {
      out << " = " << expected;
    } else {
      out << " = " << function(input) << ", but expected " << expected;
    }
  }
};

std::ostream &operator<<(std::ostream &stream, const MsbTestScenario &s) {
  s.print(stream);
  return stream;
}

class MsbTestCases {
  std::vector<MsbTestScenario> testCases;

public:
  MsbTestCases() {

    testCases.push_back(MsbTestScenario::most_significant_bit(0, -1));
    testCases.push_back(MsbTestScenario::most_significant_single_bit(0, -1));

    testCases.push_back(MsbTestScenario::most_significant_bit(1, 0));
    testCases.push_back(MsbTestScenario::most_significant_single_bit(1, 0));

    testCases.push_back(MsbTestScenario::most_significant_bit(2, 1));
    testCases.push_back(MsbTestScenario::most_significant_single_bit(2, 1));

    testCases.push_back(MsbTestScenario::most_significant_bit(4, 2));
    testCases.push_back(MsbTestScenario::most_significant_single_bit(4, 2));

    int maxbit = sizeof(size_t) * 8 - 1;
    size_t max = size_t(1) << maxbit;

    testCases.push_back(MsbTestScenario::most_significant_bit(max, maxbit));
    testCases.push_back(
        MsbTestScenario::most_significant_single_bit(max, maxbit));

    testCases.push_back(MsbTestScenario::most_significant_bit(0x10, 4));
    testCases.push_back(MsbTestScenario::most_significant_single_bit(0x10, 4));

    testCases.push_back(MsbTestScenario::most_significant_bit(0x11, 4));
    testCases.push_back(MsbTestScenario::most_significant_single_bit(0x11, -1));

    testCases.push_back(MsbTestScenario::most_significant_bit(0x12, 4));
    testCases.push_back(MsbTestScenario::most_significant_single_bit(0x12, -2));
  }

  const std::vector<MsbTestScenario> getTestCases() { return testCases; }

} MSB_TESTCASES;

} // namespace

BOOST_AUTO_TEST_SUITE(PowerOfTwo)

BOOST_DATA_TEST_CASE(powerTwoScenarios, TEST_SET.getTestCases()) {
  sample->test();
}

BOOST_DATA_TEST_CASE(msbScenarios, MSB_TESTCASES.getTestCases()) {
  BOOST_CHECK(sample.success());
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by abakfja on 3/28/21.
//

#ifndef LA_MATRIX_HPP
#define LA_MATRIX_HPP

#include <boost/numeric/ublas/matrix/matrix/matrix_engine.hpp>
#include <boost/numeric/ublas/matrix/matrix/traits/engine_traits.hpp>
#include <boost/numeric/ublas/matrix/matrix/traits/matrix_traits.hpp>

namespace boost::numeric::ublas::experimental {

template<class engine>
class matrix {
public:
    using scalar_type = typename engine::scalar_type;
    using scalar_reference = scalar_type &;
    using scalar_const_reference = const scalar_type &;

    using storage_traits_type = typename engine::storage_traits_type;

    using storage_type = typename storage_traits_type::storage_type;

    using size_type = typename storage_traits_type::size_type;
    using index_type = std::pair<size_type, size_type>;
    using difference_type = typename storage_traits_type::difference_type;

    using reference = typename storage_traits_type::reference;
    using const_reference = typename storage_traits_type::const_reference;

    using pointer = typename storage_traits_type::pointer;
    using const_pointer = typename storage_traits_type::const_pointer;

    using iterator = typename storage_traits_type::iterator;
    using const_iterator = typename storage_traits_type::const_iterator;

    using reverse_iterator = typename storage_traits_type::reverse_iterator;
    using const_reverse_iterator = typename storage_traits_type::const_reverse_iterator;

    using resizable_tag = typename storage_traits_type::resizable_tag;

    template<typename E = engine,
            typename = detail::enable_if_static<E>>
    matrix(std::initializer_list<scalar_type> l): matrix(resizable_tag{}) {
        std::copy(l.begin(), l.end(), m_data.begin());
    }

    template<typename E = engine,
            typename = detail::enable_if_static<E>>
    explicit matrix(scalar_const_reference v): matrix(resizable_tag{}) {
        fill(v);
    }

    template<typename E = engine,
            typename = detail::enable_if_dynamic<E>>
    explicit matrix(
            size_type r,
            size_type c,
            std::initializer_list<scalar_type> l
    ) : matrix(r, c, resizable_tag{}) {
        std::copy(l.begin(), l.end(), m_data.begin());
    }

    template<typename E = engine,
            typename = detail::enable_if_dynamic<E>>
    constexpr explicit
    matrix(size_type r, size_type c) :
            matrix(r, c, resizable_tag{}) {
    }

    template<typename E = engine,
            typename = detail::enable_if_dynamic<E>>
    constexpr explicit
    matrix(size_type r, size_type c, scalar_type v) :
            matrix(r, c, resizable_tag{}) {
        fill(v);
    }

    [[nodiscard]] index_type size() const {
        return std::make_pair(this->rows, this->cols);
    }

    [[nodiscard]] bool empty() const {
        return m_data.empty();
    }

    constexpr auto begin() noexcept {
        return m_data.begin();
    }

    constexpr auto end() noexcept {
        return m_data.end();
    }

    constexpr auto begin() const noexcept {
        return m_data.begin();
    }

    constexpr auto end() const noexcept {
        return m_data.end();
    }

    constexpr auto at(size_type pos) const {
        return m_data.at(pos);
    }

    constexpr auto at(size_type pos) {
        return m_data.at(pos);
    }

    void fill(const_reference value) {
        std::fill(begin(), end(), value);
    }

    constexpr const_reference operator[](size_type i) const {
        return this->m_data[i];
    }

    constexpr reference operator[](size_type i) {
        return this->m_data[i];
    }

private:

    constexpr explicit matrix(size_type r,
                              size_type c,
                              storage_resizable_container_tag t
    ) : rows{r}, cols{c}, m_data(rows * cols) {
    }

    constexpr explicit matrix(storage_static_container_tag t) :
            rows{engine::rows}, cols{engine::cols} {
    }

    size_type rows;
    size_type cols;
    storage_type m_data;
};

}
#endif //LA_MATRIX_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestSortByTransform
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/algorithm/is_sorted.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/experimental/sort_by_transform.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(sort_int_by_abs)
{
    int data[] = { 1, -2, 4, -3, 0, 5, -8, -9 };
    compute::vector<int> vector(data, data + 8, queue);

    compute::experimental::sort_by_transform(
        vector.begin(),
        vector.end(),
        compute::abs<int>(),
        compute::less<int>(),
        queue
    );

    CHECK_RANGE_EQUAL(int, 8, vector, (0, 1, -2, -3, 4, 5, -8, -9));
}

BOOST_AUTO_TEST_CASE(sort_vectors_by_length)
{
    using compute::float4_;

    float data[] = {
        1.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 1.0f, 0.0f,
        3.0f, 2.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 0.5f, 0.0f
    };

    compute::vector<float4_> vector(4, context);
    compute::copy_n(
        reinterpret_cast<float4_ *>(data), 4, vector.begin(), queue
    );

    compute::experimental::sort_by_transform(
        vector.begin(),
        vector.end(),
        compute::length<float4_>(),
        compute::less<float>(),
        queue
    );

    std::vector<float4_> host_vector(4);
    compute::copy(
        vector.begin(), vector.end(), host_vector.begin(), queue
    );
    BOOST_CHECK_EQUAL(host_vector[0], float4_(0.0f, 0.0f, 0.5f, 0.0f));
    BOOST_CHECK_EQUAL(host_vector[1], float4_(1.0f, 0.0f, 0.0f, 0.0f));
    BOOST_CHECK_EQUAL(host_vector[2], float4_(0.0f, 1.0f, 1.0f, 0.0f));
    BOOST_CHECK_EQUAL(host_vector[3], float4_(3.0f, 2.0f, 1.0f, 0.0f));
}

BOOST_AUTO_TEST_CASE(sort_vectors_by_component)
{
    using compute::float4_;

    float data[] = {
        1.0f, 2.0f, 3.0f, 0.0f,
        9.0f, 8.0f, 7.0f, 0.0f,
        4.0f, 5.0f, 6.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f
    };

    compute::vector<float4_> vector(4, context);
    compute::copy_n(
        reinterpret_cast<float4_ *>(data), 4, vector.begin(), queue
    );

    // sort by y-component
    compute::experimental::sort_by_transform(
        vector.begin(),
        vector.end(),
        compute::get<1>(),
        compute::less<float>(),
        queue
    );

    std::vector<float4_> host_vector(4);
    compute::copy(
        vector.begin(), vector.end(), host_vector.begin(), queue
    );
    BOOST_CHECK_EQUAL(host_vector[0], float4_(0.0f, 0.0f, 0.0f, 0.0f));
    BOOST_CHECK_EQUAL(host_vector[1], float4_(1.0f, 2.0f, 3.0f, 0.0f));
    BOOST_CHECK_EQUAL(host_vector[2], float4_(4.0f, 5.0f, 6.0f, 0.0f));
    BOOST_CHECK_EQUAL(host_vector[3], float4_(9.0f, 8.0f, 7.0f, 0.0f));
}

BOOST_AUTO_TEST_SUITE_END()


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include <Python.h>
#include <boost/python.hpp>
#include <numpy/arrayobject.h>
using namespace boost::python;

#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;

#include <mrpt/config.h>

#include <mrpt/vision/pnp_algos.h>
mrpt::vision::pnp::CPnP pnp_algos;

#if MRPT_HAS_OPENCV
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
using namespace cv;
#endif

#include <iostream>

class PnPAlgos
{
   public:
	PnPAlgos(int new_m);
	~PnPAlgos();
#if MRPT_HAS_OPENCV
	int epnp_solve(
		PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
		PyObject* pose_mat);
	int dls_solve(
		PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
		PyObject* pose_mat);
	int upnp_solve(
		PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
		PyObject* pose_mat);
#endif
	int p3p_solve(
		PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
		PyObject* pose_mat);
	int ppnp_solve(
		PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
		PyObject* pose_mat);
	int rpnp_solve(
		PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
		PyObject* pose_mat);
	int posit_solve(
		PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
		PyObject* pose_mat);
	int lhm_solve(
		PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
		PyObject* pose_mat);

   private:
	int dummy;
};

PnPAlgos::PnPAlgos(int new_m)
{
	dummy = new_m;
#if MRPT_HAS_OPENCV
	std::cout << " Using OpenCV dependency for PnP Algorithms - EPnP, DLS-PnP, "
				 "UPnP(Broken) "
			  << std::endl
			  << std::endl;
#else
	std::cout << " Initializing PnP class " << std::endl << std::endl;
#endif
}
PnPAlgos::~PnPAlgos() = default;
#if MRPT_HAS_OPENCV
int PnPAlgos::epnp_solve(
	PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
	PyObject* pose_mat)
{
	Map<MatrixXd> _obj_pts(
		(double*)PyArray_DATA((PyArrayObject*)obj_pts), 3, n);
	Map<MatrixXd> _img_pts(
		(double*)PyArray_DATA((PyArrayObject*)img_pts), 3, n);
	Map<MatrixXd> _pose_mat(
		(double*)PyArray_DATA((PyArrayObject*)pose_mat), 6, 1);
	Map<MatrixXd> _cam_intrinsic(
		(double*)PyArray_DATA((PyArrayObject*)cam_intrinsic), 3, 3);

	return pnp_algos.epnp(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

int PnPAlgos::dls_solve(
	PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
	PyObject* pose_mat)
{
	Map<MatrixXd> _obj_pts(
		(double*)PyArray_DATA((PyArrayObject*)obj_pts), 3, n);
	Map<MatrixXd> _img_pts(
		(double*)PyArray_DATA((PyArrayObject*)img_pts), 3, n);
	Map<MatrixXd> _pose_mat(
		(double*)PyArray_DATA((PyArrayObject*)pose_mat), 6, 1);
	Map<MatrixXd> _cam_intrinsic(
		(double*)PyArray_DATA((PyArrayObject*)cam_intrinsic), 3, 3);

	return pnp_algos.dls(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

int PnPAlgos::upnp_solve(
	PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
	PyObject* pose_mat)
{
	Map<MatrixXd> _obj_pts(
		(double*)PyArray_DATA((PyArrayObject*)obj_pts), 3, n);
	Map<MatrixXd> _img_pts(
		(double*)PyArray_DATA((PyArrayObject*)img_pts), 3, n);
	Map<MatrixXd> _pose_mat(
		(double*)PyArray_DATA((PyArrayObject*)pose_mat), 6, 1);
	Map<MatrixXd> _cam_intrinsic(
		(double*)PyArray_DATA((PyArrayObject*)cam_intrinsic), 3, 3);

	return pnp_algos.upnp(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}
#endif

int PnPAlgos::p3p_solve(
	PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
	PyObject* pose_mat)
{
	Map<MatrixXd> _obj_pts(
		(double*)PyArray_DATA((PyArrayObject*)obj_pts), 3, n);
	Map<MatrixXd> _img_pts(
		(double*)PyArray_DATA((PyArrayObject*)img_pts), 3, n);
	Map<MatrixXd> _pose_mat(
		(double*)PyArray_DATA((PyArrayObject*)pose_mat), 6, 1);
	Map<MatrixXd> _cam_intrinsic(
		(double*)PyArray_DATA((PyArrayObject*)cam_intrinsic), 3, 3);

	return pnp_algos.p3p(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

int PnPAlgos::ppnp_solve(
	PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
	PyObject* pose_mat)
{
	Map<MatrixXd> _obj_pts(
		(double*)PyArray_DATA((PyArrayObject*)obj_pts), 3, n);
	Map<MatrixXd> _img_pts(
		(double*)PyArray_DATA((PyArrayObject*)img_pts), 3, n);
	Map<MatrixXd> _pose_mat(
		(double*)PyArray_DATA((PyArrayObject*)pose_mat), 6, 1);
	Map<MatrixXd> _cam_intrinsic(
		(double*)PyArray_DATA((PyArrayObject*)cam_intrinsic), 3, 3);

	return pnp_algos.ppnp(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

int PnPAlgos::rpnp_solve(
	PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
	PyObject* pose_mat)
{
	Map<MatrixXd> _obj_pts(
		(double*)PyArray_DATA((PyArrayObject*)obj_pts), 3, n);
	Map<MatrixXd> _img_pts(
		(double*)PyArray_DATA((PyArrayObject*)img_pts), 3, n);
	Map<MatrixXd> _pose_mat(
		(double*)PyArray_DATA((PyArrayObject*)pose_mat), 6, 1);
	Map<MatrixXd> _cam_intrinsic(
		(double*)PyArray_DATA((PyArrayObject*)cam_intrinsic), 3, 3);

	return pnp_algos.rpnp(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

int PnPAlgos::posit_solve(
	PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
	PyObject* pose_mat)
{
	Map<MatrixXd> _obj_pts(
		(double*)PyArray_DATA((PyArrayObject*)obj_pts), 3, n);
	Map<MatrixXd> _img_pts(
		(double*)PyArray_DATA((PyArrayObject*)img_pts), 3, n);
	Map<MatrixXd> _pose_mat(
		(double*)PyArray_DATA((PyArrayObject*)pose_mat), 6, 1);
	Map<MatrixXd> _cam_intrinsic(
		(double*)PyArray_DATA((PyArrayObject*)cam_intrinsic), 3, 3);

	return pnp_algos.posit(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

int PnPAlgos::lhm_solve(
	PyObject* obj_pts, PyObject* img_pts, int n, PyObject* cam_intrinsic,
	PyObject* pose_mat)
{
	Map<MatrixXd> _obj_pts(
		(double*)PyArray_DATA((PyArrayObject*)obj_pts), 3, n);
	Map<MatrixXd> _img_pts(
		(double*)PyArray_DATA((PyArrayObject*)img_pts), 3, n);
	Map<MatrixXd> _pose_mat(
		(double*)PyArray_DATA((PyArrayObject*)pose_mat), 6, 1);
	Map<MatrixXd> _cam_intrinsic(
		(double*)PyArray_DATA((PyArrayObject*)cam_intrinsic), 3, 3);

	return pnp_algos.lhm(_obj_pts, _img_pts, n, _cam_intrinsic, _pose_mat);
}

void export_pnp()
{
	class_<PnPAlgos>("pnp", init<int>(args("m")))
#if MRPT_HAS_OPENCV
		.def("epnp", &PnPAlgos::epnp_solve)
		.def("dls", &PnPAlgos::dls_solve)
		.def("upnp", &PnPAlgos::upnp_solve)
#endif
		.def("p3p", &PnPAlgos::p3p_solve)
		.def("ppnp", &PnPAlgos::ppnp_solve)
		.def("rpnp", &PnPAlgos::rpnp_solve)
		.def("posit", &PnPAlgos::posit_solve)
		.def("lhm", &PnPAlgos::lhm_solve);
}


#include "./read_vrp.h"
#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <string>
#include <cctype>
#include <unordered_map>
#include <boost/algorithm/string.hpp>

using namespace std;

template <typename T>
vector<unsigned int> sort_indexes(const vector<T> &v) {

  // initialize original index locations
  vector<unsigned int> idx(v.size());
  iota(idx.begin(), idx.end(), 0);

  // sort indexes based on comparing values in v
  // using std::stable_sort instead of std::sort
  // to avoid unnecessary index re-orderings
  // when v contains elements of equal values
  stable_sort(idx.begin(), idx.end(),
       [&v](size_t i1, size_t i2) {return v[i1] < v[i2];});

  return idx;
}

//Sort distance matrix for Giant tour
vector<vector<unsigned int>> compute_sorted_distance_matrix(vector<vector<float>>& dist_matrix){
	vector<vector<unsigned int>> sorted_distance_matrix(dist_matrix.size());
	for(int i = 0; i < dist_matrix.size(); i++){
		sorted_distance_matrix[i] = sort_indexes(dist_matrix[i]);
	}

	return sorted_distance_matrix;
}

// This function uses boost library downloaded from: https://www.boost.org/users/history/version_1_73_0.html
vector<string> read_matrix_csv(const string& filename) {
    string line;
    vector<string> elements;
    vector<string> result;
    ifstream f{filename};

	if(!f){
		cout << "File not found" << endl;
	}else{
		while(!f.eof()){
			getline(f, line, ',');
			boost::split(result, line, boost::is_any_of(" \r\n \n"));
			for(int i = 0; i < result.size(); i++){
				if(result[i].size() != 0){
					elements.push_back(result[i]);
				}
			}
		}
	}

	f.close();

    return elements;
}

vector<string> read_elem(const string& filename) {
    string line;
    vector<string> elements;
    vector<string> result;
    ifstream f{filename};

    if(!f){
        cout << "File not found" << endl;
    }else{
        while(!f.eof()){
            getline(f, line);
            boost::split(result, line, [](unsigned char x){return std::isspace(x);});
            for(int i = 0; i < result.size(); i++){
                if(result[i].size() != 0){
                    elements.push_back(result[i]);
                }
            }
        }
    }

    f.close();

    return elements;
}

TSPInstanceValues read_input_tsptw(const string& filename, const int& in_test_phase) {
    cout << filename << endl;
    vector<string> elements = read_elem(filename);
    vector<string>::iterator ptr = elements.begin();

    int n_cust = 0;
    int nb_nodes = 0;
    int depot_pos = 0;
    string token;
    string route_id;
    //string quality_score = "High";

    nb_nodes = stoi(*next(ptr, 1));

    advance(ptr, 1);

    token = *next(ptr, 1);
    advance(ptr, 1);
    if (token != "vehicle_capacity") {
        cout << "Expected token vehicle_capacity but got: " << token << endl;
        exit(1);
    }

    float Q = 0;
    Q = stof(*next(ptr, 1));
    advance(ptr, 1);

    token = *next(ptr, 1);
    advance(ptr, 1);
    if (token != "depot_number") {
        cout << "Expected token depot_number" << endl;
        exit(1);
    }
    depot_pos = stoi(*next(ptr, 1)) - 1; //Check if that 1 really has to be substracted or not
    advance(ptr, 1);

    token = *next(ptr, 1);
    advance(ptr, 1);
    if (token != "route_id") {
        cout << "Expected token route_id" << endl;
        exit(1);
    }
    route_id = *next(ptr, 1);
    advance(ptr, 1);
    /*
    if (in_test_phase == 0) {
        token = *next(ptr, 1);
        advance(ptr, 1);
        if (token != "quality_score") {
            cout << "Expected token quality_score" << endl;
            exit(1);
        }
        quality_score = *next(ptr, 1);
        advance(ptr, 1);
    }
    */

    n_cust = nb_nodes;
    vector<vector<float>> time_matrix(nb_nodes, vector<float>(nb_nodes, 0));
    vector<vector<float>> time_windows(nb_nodes, vector<float>(2, 0));
    vector<vector<float>> location(nb_nodes, vector<float>(2, 0));
    vector<float> service_time(nb_nodes, 0);
    vector<float> demands(nb_nodes, 0);
    vector<string> zone_id(nb_nodes, "0");
    unordered_map<int, string> customers_map;
    unordered_map<string, int> zones_map;
    map<string, int> ordered_zones_map;

    int depot_x = 0;
    int depot_y = 0;
    int node_id;

    token = *next(ptr, 1);
    advance(ptr, 1);
    if(token != "travel_times"){
        cout << "Expected token travel_times" << endl;
        exit(1);
    }

    for(int i = 0; i < nb_nodes; i++){
        if (i == 0){
            for(int k = 0; k < nb_nodes+1; k++){
                token = *next(ptr, 1);
                advance(ptr, 1);
            }
        }
        token = *next(ptr, 1);
        customers_map[i] = token;
        advance(ptr, 1);
        for(int j = 0; j < nb_nodes; j++){
            time_matrix[i][j] = stof(*next(ptr, 1));
            advance(ptr, 1);
        }
    }

    // Compute sorted time matrix
    vector<vector<unsigned int>> sorted_time_matrix = compute_sorted_distance_matrix(time_matrix);

    //Time windows matrix
    token = *next(ptr, 1);

    advance(ptr, 1);
    if(token != "time_windows"){
        cout << "Expected token time_windows" << endl;
        exit(1);
    }
    for(int i = 0; i < nb_nodes; i++){
        for(int j = 0; j < 2; j++){
            time_windows[i][j] = stof(*next(ptr, 1));
            advance(ptr, 1);
        }
    }

    //Service time vector
    token = *next(ptr, 1);
    //cout << token << " ";
    advance(ptr, 1);
    if(token != "service_time"){
        cout << "Expected token service_time" << endl;
        exit(1);
    }
    for(int i = 0; i < nb_nodes; i++){
            service_time[i] = stof(*next(ptr, 1));
            advance(ptr, 1);
    }

    //Demands vector
    token = *next(ptr, 1);
    advance(ptr, 1);
    if(token != "dimensions"){
        cout << "Expected token dimensions" << endl;
        exit(1);
    }
    for(int i = 0; i < nb_nodes; i++){
        demands[i] = stof(*next(ptr, 1));
        advance(ptr, 1);
    }

    //Latitude Longitude
    token = *next(ptr, 1);
    advance(ptr, 1);
    if(token != "latitude_longitude"){
        cout << "Expected token latitude_longitude" << endl;
        exit(1);
    }
    for(int i = 0; i < nb_nodes; i++){
        for(int j = 0; j < 2; j++){
            location[i][j] = stof(*next(ptr, 1));
            advance(ptr, 1);
        }
    }

    token = *next(ptr, 1);
    advance(ptr, 1);
    if(token != "zone_id"){
        cout << "Expected token zone_id" << endl;
        exit(1);
    }
    for(int i = 0; i < nb_nodes; i++){
        token = *next(ptr, 1);
        zone_id[i] = token;
        // check if key 'token' exists in the map or not
        std::unordered_map<string, int>::iterator it = zones_map.find(token);

        // key already present in the map
        if (it != zones_map.end()) {
            it->second++;    // increment map's value for key 'token'
        }
            // key not found
        else {
            zones_map.insert(std::make_pair(token, 1));
        }
        advance(ptr, 1);
    }

    for (auto zone : zones_map ){
        ordered_zones_map.insert(pair<string, int>(zone.first, zone.second));
    }

    //vector<vector<vector<float>>> insertion_tsp_matrix;

    cout << "Parsed file correctly" << endl;

    return {n_cust, depot_pos, Q, time_matrix, time_windows, service_time, demands, location, sorted_time_matrix, customers_map, zone_id, zones_map, ordered_zones_map};
}

TSPHistoricValues read_historic_files(const string& route_fid, const int& number_of_zones, const int& number_of_customers, const int& in_test_phase) {
    string prob_matrix_file( "../prob_matrices_heading/"+route_fid+"_probs.csv" );
    string angles_matrix_file( "../prob_matrices_angle/"+route_fid+"_probs.csv" );
    string historic_zones_file( "../prob_matrices_heading/"+route_fid+"_zroutes.csv" );
    string distance_matrix_file( "../haversine_distance_matrices/"+route_fid+"_dist_matrix.csv" );

    if (in_test_phase == 1){
        prob_matrix_file =  "../data/model_apply_outputs/prob_matrices_heading/"+route_fid+"_probs.csv" ;
        angles_matrix_file =  "../data/model_apply_outputs/prob_matrices_angle/"+route_fid+"_probs.csv" ;
        historic_zones_file = "../data/model_apply_outputs/prob_matrices_heading/"+route_fid+"_zroutes.csv" ;
        distance_matrix_file = "../data/model_apply_outputs/haversine_distance_matrices/"+route_fid+"_dist_matrix.csv" ;
    }
    else {
        prob_matrix_file =  "../data/model_build_outputs/prob_matrices_heading/"+route_fid+"_probs.csv" ;
        angles_matrix_file =  "../data/model_build_outputs/prob_matrices_angle/"+route_fid+"_probs.csv" ;
        historic_zones_file = "../data/model_build_outputs/prob_matrices_heading/"+route_fid+"_zroutes.csv" ;
        distance_matrix_file = "../data/model_build_outputs/haversine_distance_matrices/"+route_fid+"_dist_matrix.csv" ;
    }

    cout << " In read historic files, route_fid: " << route_fid<< endl;

    vector<string> elements = read_matrix_csv(prob_matrix_file);
    vector<string>::iterator ptr = elements.begin();
    vector<string> elements_3 = read_matrix_csv(historic_zones_file);
    vector<string>::iterator ptr_3 = elements_3.begin();
    vector<string> elements_4 = read_matrix_csv(angles_matrix_file); \
    vector<string>::iterator ptr_4 = elements_4.begin();
    vector<string> elements_6 = read_matrix_csv(distance_matrix_file); \
    vector<string>::iterator ptr_6 = elements_6.begin();

    string token;
    string route_id;
    string zone_1;
    string zone_2;
    string zone_3;
    int nb_nodes = number_of_zones -1;
    int max_n_routes = 1;

    vector<vector<float>> distance_matrix(number_of_customers, vector<float>(number_of_customers, 0));
    vector<vector<float>> probabilities_matrix(nb_nodes+1, vector<float>(nb_nodes+1, 0));
    vector<vector<float>> angles_matrix(nb_nodes+1, vector<float>(nb_nodes+1, 0));
    vector<vector<int>> n_routes_matrix(nb_nodes+1, vector<int>(nb_nodes+1, 0));
    vector<string> historic_zones_id(nb_nodes+1, "0");
    vector<string> correct_zones_id(nb_nodes+1, "0");
    cout << "Size matrix: " << nb_nodes << endl;

    unordered_map< string, unordered_map<string, float> > probabilities_df;
    unordered_map< string, unordered_map<string, float> > angles_df;
    unordered_map< string, unordered_map<string, int> > n_routes_df;

    float number_of_missing_zones_data = 0.0;

    //READ HAVERSINE DISTANCE MATRIX

    cout << "number_of_customers: " << number_of_customers << endl;
    for(int i = 0; i < number_of_customers; i++){
        if (i == 0){
            for(int k = 0; k < number_of_customers; k++){
                token = *next(ptr_6, 1);
                advance(ptr_6, 1);
            }
        }
        token = *next(ptr_6, 1);
        advance(ptr_6, 1);
        for(int j = 0; j < number_of_customers; j++){
            distance_matrix[i][j] = stof(*next(ptr_6, 1));
            advance(ptr_6, 1);
        }
    }
    cout << "Parsed DISTANCE MATRIX correctly" << endl;

    for(int i = 0; i < nb_nodes+1; i++){
        token = *next(ptr_3, 0);
        historic_zones_id[i] = token;
        advance(ptr_3, 1);
    }

    //READ PROBABILITY MATRIX FILE
    token = *next(ptr, 1);
    float fill_prob_nan = 0.5;
    string fill_prob_nan_str = "0.5";

    //UNCOMMENT THIS SECTION WHEN PROB_MATRIX HAS A WORD/ELEMENT IN ROW 0 COLUMN 0, OTHERWISE, LEAVE COMMENT
    advance(ptr, 1);
    token = *next(ptr, 1);

    for(int i = 0; i < nb_nodes; i++){
        if (i == 0){
            for(int k = 0; k < nb_nodes-1; k++){
                token = *next(ptr, 1);
                advance(ptr, 1);
            }
        }
        token = *next(ptr, 1);
        advance(ptr, 1);
        zone_1 = historic_zones_id[i];

        for(int j = 0; j < nb_nodes; j++){
            zone_2 = historic_zones_id[j];
            token = *next(ptr, 1);
            if (token == "nan"){
                token = fill_prob_nan_str;
            }
            if (token == "0.5")
            {
                number_of_missing_zones_data++;
            }
            probabilities_matrix[i][j] = stof(token);
            probabilities_df[zone_1][zone_2] = stof(token);
            advance(ptr, 1);
        }
        probabilities_matrix[i][nb_nodes] = fill_prob_nan;
        probabilities_df[zone_1][historic_zones_id[nb_nodes]] = fill_prob_nan;
        zone_3 = historic_zones_id[nb_nodes];
        probabilities_df[zone_3][zone_1] = fill_prob_nan;
    }
    zone_1 = historic_zones_id[nb_nodes];
    probabilities_df[zone_1][zone_1] = fill_prob_nan;

    float percentage_missing_data_matrix = number_of_missing_zones_data/(nb_nodes *nb_nodes);
    cout << "Parsed HEADING MATRIX correctly" << endl;
    //READ ANGLES MATRIX FILE
    token = *next(ptr_4, 1);
    //advance(ptr, 1);

    advance(ptr_4, 1);
    token = *next(ptr_4, 1);

    for(int i = 0; i < nb_nodes; i++){
        if (i == 0){
            for(int k = 0; k < nb_nodes-1; k++){
                token = *next(ptr_4, 1);
                advance(ptr_4, 1);
            }
        }
        token = *next(ptr_4, 1);
        advance(ptr_4, 1);
        zone_1 = historic_zones_id[i];

        for(int j = 0; j < nb_nodes; j++){
            zone_2 = historic_zones_id[j];
            token = *next(ptr_4, 1);
            if (token == "nan"){
                token = fill_prob_nan_str;
            }
            angles_matrix[i][j] = stof(token);
            angles_df[zone_1][zone_2] = stof(token);
            advance(ptr_4, 1);
        }
        angles_matrix[i][nb_nodes] = fill_prob_nan;
        angles_df[zone_1][historic_zones_id[nb_nodes]] = fill_prob_nan;
        zone_3 = historic_zones_id[nb_nodes];
        angles_df[zone_3][zone_1] = fill_prob_nan;
    }
    zone_1 = historic_zones_id[nb_nodes];
    angles_df[zone_1][zone_1] = fill_prob_nan;
    cout << "Parsed ANGLE MATRIX correctly" << endl;

    cout << " Done reading historic files" << endl;

    return {probabilities_matrix, n_routes_matrix, historic_zones_id, probabilities_df, angles_df, n_routes_df, max_n_routes, distance_matrix};
}


/**
 * @file tests/main_tests/radical_test.cpp
 * @author Manish Kumar
 *
 * Test mlpackMain() of radical_main.cpp.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <string>

#define BINDING_TYPE BINDING_TYPE_TEST
static const std::string testName = "Radical";

#include <mlpack/core.hpp>
#include <mlpack/core/util/mlpack_main.hpp>
#include "test_helper.hpp"
#include <mlpack/methods/radical/radical_main.cpp>

#include <boost/test/unit_test.hpp>
#include "../test_tools.hpp"

using namespace mlpack;

struct RadicalTestFixture
{
 public:
  RadicalTestFixture()
  {
    // Cache in the options for this program.
    IO::RestoreSettings(testName);
  }

  ~RadicalTestFixture()
  {
    // Clear the settings.
    bindings::tests::CleanMemory();
    IO::ClearSettings();
  }
};

BOOST_FIXTURE_TEST_SUITE(RadicalMainTest, RadicalTestFixture);

/**
 * Check that output Y and W matrix have valid dimensions.
 */
BOOST_AUTO_TEST_CASE(RadicalOutputDimensionTest)
{
  arma::mat input = arma::randu<arma::mat>(5, 3);

  SetInputParam("input", std::move(input));

  mlpackMain();

  // Check dimension of Y matrix.
  BOOST_REQUIRE_EQUAL(IO::GetParam<arma::mat>("output_ic").n_rows, 5);
  BOOST_REQUIRE_EQUAL(IO::GetParam<arma::mat>("output_ic").n_cols, 3);

  // Check dimension of W matrix.
  BOOST_REQUIRE_EQUAL(IO::GetParam<arma::mat>("output_unmixing").n_rows, 5);
  BOOST_REQUIRE_EQUAL(IO::GetParam<arma::mat>("output_unmixing").n_cols, 5);
}

/**
 * Ensure that replicates & angles are always positive while as noise_std_dev
 * & sweep is always non-negative.
 */
BOOST_AUTO_TEST_CASE(RadicalBoundsTest)
{
  arma::mat input = arma::randu<arma::mat>(5, 3);

  // Test for replicates.

  SetInputParam("input", input);
  SetInputParam("replicates", (int) 0);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;

  bindings::tests::CleanMemory();

  // Test for noise_std_dev.

  SetInputParam("input", input);
  SetInputParam("noise_std_dev", (double) -1.0);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;

  bindings::tests::CleanMemory();

  // Test for angles.

  SetInputParam("input", input);
  SetInputParam("angles", (int) 0);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;

  bindings::tests::CleanMemory();

  // Test for sweeps.

  SetInputParam("input", input);
  SetInputParam("sweeps", (int) -2);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Check the learning process by using different values of noise_std_dev
 * parameter.
 */
BOOST_AUTO_TEST_CASE(RadicalDiffNoiseStdDevTest)
{
  arma::mat input("0.497369 0.891621 0.565789;"
                  "0.33821 0.494571 0.491079;"
                  "0.424898 0.297599 0.475061;"
                  "0.285009 0.152635 0.878107;"
                  "0.321474 0.997979 0.42137");

  SetInputParam("input", input);

  mlpack::math::FixedRandomSeed();
  mlpackMain();

  arma::mat Y = IO::GetParam<arma::mat>("output_ic");

  bindings::tests::CleanMemory();

  SetInputParam("input", std::move(input));
  SetInputParam("noise_std_dev", (double) 0.01);

  mlpack::math::FixedRandomSeed();
  mlpackMain();

  // Check that initial output and final output using two models are different.
  BOOST_REQUIRE_LT(arma::accu(Y ==
      IO::GetParam<arma::mat>("output_ic")), Y.n_elem);
}

/**
 * Check the learning process by using different values of replicates parameter.
 */
BOOST_AUTO_TEST_CASE(RadicalDiffReplicatesTest)
{
  arma::mat input("0.497369 0.891621 0.565789;"
                  "0.33821 0.494571 0.491079;"
                  "0.424898 0.297599 0.475061;"
                  "0.285009 0.152635 0.878107;"
                  "0.321474 0.997979 0.42137");

  SetInputParam("input", input);

  mlpack::math::FixedRandomSeed();
  mlpackMain();

  arma::mat Y = IO::GetParam<arma::mat>("output_ic");

  bindings::tests::CleanMemory();

  SetInputParam("input", std::move(input));
  SetInputParam("replicates", (int) 10);

  mlpack::math::FixedRandomSeed();
  mlpackMain();

  // Check that initial output and final output using two models are different.
  BOOST_REQUIRE_LT(arma::accu(Y ==
      IO::GetParam<arma::mat>("output_ic")), Y.n_elem);
}

/**
 * Check the learning process by using different values of angles parameter.
 */
BOOST_AUTO_TEST_CASE(RadicalDiffAnglesTest)
{
  arma::mat input("0.497369 0.891621 0.565789;"
                  "0.33821 0.494571 0.491079;"
                  "0.424898 0.297599 0.475061;"
                  "0.285009 0.152635 0.878107;"
                  "0.321474 0.997979 0.42137");

  SetInputParam("input", input);

  mlpack::math::FixedRandomSeed();
  mlpackMain();

  arma::mat Y = IO::GetParam<arma::mat>("output_ic");

  bindings::tests::CleanMemory();

  SetInputParam("input", std::move(input));
  SetInputParam("angles", (int) 20);

  mlpack::math::FixedRandomSeed();
  mlpackMain();

  // Check that initial output and final output using two models are different.
  BOOST_REQUIRE_LT(arma::accu(Y ==
      IO::GetParam<arma::mat>("output_ic")), Y.n_elem);
}

/**
 * Check the learning process by using different values of sweeps parameter.
 */
BOOST_AUTO_TEST_CASE(RadicalDiffSweepsTest)
{
  arma::mat input("0.497369 0.891621 0.565789;"
                  "0.33821 0.494571 0.491079;"
                  "0.424898 0.297599 0.475061;"
                  "0.285009 0.152635 0.878107;"
                  "0.321474 0.997979 0.42137");

  SetInputParam("input", input);

  mlpack::math::FixedRandomSeed();
  mlpackMain();

  arma::mat Y = IO::GetParam<arma::mat>("output_ic");

  bindings::tests::CleanMemory();

  SetInputParam("input", std::move(input));
  SetInputParam("sweeps", (int) 2);

  mlpack::math::FixedRandomSeed();
  mlpackMain();

  // Check that initial output and final output using two models are different.
  BOOST_REQUIRE_LT(arma::accu(Y ==
      IO::GetParam<arma::mat>("output_ic")), Y.n_elem);
}

BOOST_AUTO_TEST_SUITE_END();


///States for MCMC
///
///state objects are tied to a stateSpace object which specifies to domain.
///boundary objects allow specification of the domain bounraies in each dimension.
///Written by John G Baker - NASA-GSFC (2013-2014)

#ifndef PTMCMC_PROPOSAL_HH
#define PTMCMC_PROPOSAL_HH
#include "states.hh"
#include "chain.hh"
#include "restart.hh"
#include "probability_function.hh"
#include <Eigen/Eigen>

using namespace std;

//************** PROPOSAL DISTRIBUTION classes ******************************

///A class for defining proposal distributions.
///Useful proposal distributions often reference a chain.
class proposal_distribution: public restartable{
  static int idcount;
protected:
  int id;
  double log_hastings;
  int last_type;
  int accept_count;
  int reject_count;
  chain *ch;
public:
  virtual ~proposal_distribution(){};
  proposal_distribution(){id=idcount;idcount++;log_hastings=0;last_type=0;accept_count=0;reject_count=0;ch=nullptr;};
  virtual double log_hastings_ratio(){return log_hastings;};//(proposal part of Hasting ratio for most recent draw;
  virtual void set_chain(chain *c){ch=c;};//Not always needed.
  //virtual state draw(state &s,Random &rng){return s;};//The base class won't be useful.
  virtual state draw(state &s,chain *caller){return s;};//The base class won't be useful.
  ///Some proposals require some set-up, such as a chain of sufficient length.
  virtual bool is_ready() {return true;};
  virtual proposal_distribution* clone()const{return new proposal_distribution(*this);};
  virtual string show(){return "UnspecifiedProposal()";};
  ///Return type of draw (where that is relevant, otherwise 0)
  virtual int type(){return last_type;}
  virtual bool support_mixing(){return false;}
  virtual void accept(){accept_count++;};//external trigger to indicate that proposal was accepted (available for adaptive proposals)
  virtual void reject(){reject_count++;};//external trigger to indicate that proposal was rejected (available for adaptive proposals)
  virtual void accept( int count){accept_count=count;};//external trigger to indicate that proposal was accepted (available for adaptive proposals)
  virtual void reject(int count){reject_count=count;};//external trigger to indicate that proposal was rejected (available for adaptive proposals)
  virtual void checkpoint(string path)override{};
  virtual void restart(string path)override{};
  virtual string report(int style=0){;//For status reporting on acceptance rate(0) or adaptive shares(1)
    ostringstream ss;
    if(style==0)ss<<accept_count*1.0/(accept_count+reject_count)<<"("<<accept_count<<")";
    return ss.str();
  };
  virtual string test(const vector<state> &samples,Random &rng){return "";};//Perform an intrinsic test and return the output
};

//Apply an stateSpaceInvolution map as a proposal:
class involution_proposal: public proposal_distribution{
  stateSpaceInvolution involution;
public:
  involution_proposal(stateSpaceInvolution &involution):involution(involution){};
  state draw(state &s,chain *caller){
    involution.set_random(*(caller->getPRNG()));
    log_hastings=log(involution.jacobian(s));
    return involution.transformState(s);
  };
  involution_proposal* clone()const{return new involution_proposal(*this);};
  string show(){return "Involution["+involution.get_label()+"]()";};
  string test(const vector<state> &samples,Random &rng){
    ostringstream outs;
    outs<<"Testing involution."<<endl;
    double testsum=0;
    for(auto s:samples){
      involution.set_random(rng);
      testsum+=involution.test_involution(s,1000000);
    }
    outs<<"RMS: "<<sqrt(testsum/samples.size())<<endl;
    return outs.str();
  };
  stateSpaceInvolution get_involution(){return involution;};
};

//Draw from a distribution

//A trivial wrapper
class draw_from_dist: public proposal_distribution{
  const sampleable_probability_function &dist;
public:
  draw_from_dist(const sampleable_probability_function &dist):dist(dist){};
  //state draw(state &s,Random &rng){return dist.drawSample(rng);};
  state draw(state &s,chain *caller){
    state newstate=dist.drawSample(*(caller->getPRNG()));
    //If we are more likely to draw the newstate than the oldstate this goes into the Hastings ratio.
    log_hastings=dist.evaluate_log(s)-dist.evaluate_log(newstate);
    return newstate;
  };
  draw_from_dist* clone()const{return new draw_from_dist(*this);};
  string show(){return "DrawFrom["+dist.show()+"]()";};
};

///A multidimensional gaussian step proposal distribution.
///The constructor argument sigmas specifies the standard deviation width in each dimension.
///The optional constructor argument oneDfrac indicates a fraction of times that the Gaussian
///draw should be restricted to one randomly and uniformly selected dimension.
///
///Added feature: Nontrivial covariance.
///This is built atop the diagonal multidimensional Gaussian
///but we allow that and additional parameter vector transformation is performed.
///The matrix M for this transformation is computed to diagonalize a covariance matrix
///passed in initially.  The old behavior assumed the covariance was already diagonal
///so that M is the identity matrix.
class gaussian_prop: public proposal_distribution{
  bool identity_trans;
  Eigen::MatrixXd diagTransform;
  valarray<double>sigmas;
  gaussian_dist_product *dist;
  double oneDfrac;
  bool scaleWithTemp;
public:
  gaussian_prop(const valarray<double> &sigmas,double oneDfrac=0.0, bool scaleWithTemp=false):sigmas(sigmas),oneDfrac(oneDfrac),scaleWithTemp(scaleWithTemp){
    identity_trans=true;
    valarray<double> zeros(0.0,sigmas.size());
    dist = new gaussian_dist_product(nullptr,zeros, sigmas);
    if(oneDfrac<0||oneDfrac>1){
      cout<<"gaussian_prop(constructor I): We require 0<=oneDfrac<=1. "<<endl;
      exit(1);
    }
  };
  gaussian_prop(vector<double> &sigmas,double oneDfrac=0.0, bool scaleWithTemp=false):
    gaussian_prop(valarray<double>(sigmas.data(),sigmas.size()),oneDfrac,scaleWithTemp){};

  gaussian_prop(Eigen::MatrixXd &covar,double oneDfrac=0.0, bool scaleWithTemp=false):sigmas(sigmas),oneDfrac(oneDfrac),scaleWithTemp(scaleWithTemp){
    identity_trans=false;
    if(covar.rows() != covar.cols()){
      cout<<"gaussian_prop(constructor II): covar must be a square matrix!"<<endl;
      exit(-1);
    }
    int ndim=covar.rows();
    sigmas.resize(ndim);
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
    diagTransform=eigenSolver.eigenvectors();
    Eigen::VectorXd Evalues = eigenSolver.eigenvalues();
    for(int i=0;i<ndim;i++)sigmas[i]=sqrt(Evalues(i));
    valarray<double> zeros(0.0,ndim);
    dist = new gaussian_dist_product(nullptr,zeros, sigmas);
    if(oneDfrac<0||oneDfrac>1){
      cout<<"gaussian_prop(constructor II): We require 0<=oneDfrac<=1. "<<endl;
      exit(1);
    }
    cout<<" constructing gaussian_prop with cov matrix=\n"<<covar<<endl;
    cout<<" Eigenvalues="<<Evalues<<endl;
    cout<<" transform=\n"<<diagTransform<<endl;
    cout<<" test=\n"<<diagTransform*Evalues.asDiagonal()*diagTransform.inverse()<<endl;
  };
  virtual ~gaussian_prop(){if(dist)delete dist;};
  //state draw(state &s,Random &rng){
    //Umm is this Markovian. Do we need to set log_hastings for...
    //state offset=dist->drawSample(rng);
    //return s.add(offset);
  //};
  state draw(state &s,chain *caller){
    state offset=dist->drawSample(*(caller->getPRNG()));;
    double x=1;
    if(oneDfrac>0)x=caller->getPRNG()->Next();
    if(oneDfrac>0&&x<oneDfrac){ //step along one principal axis direction
      int ndim=offset.size();
      int i=ndim*caller->getPRNG()->Next();
      valarray<double>vals(0.0,ndim);
      vals[i]=offset.get_param(i);
      offset=state(offset.getSpace(),vals);
      //cout<<"gaussian_prop:drew: offset="<<offset.get_string()<<endl;
      last_type=1;
    } else last_type=0;
    if(not identity_trans){//if non-trivial, we need to transform to state basis.
      //cout<<"state before transform:"<<offset.get_string()<<endl;
      valarray<double> data;
      offset.get_params_array(data);
      Eigen::Map<Eigen::VectorXd> vec(&data[0],offset.size());
      vec=diagTransform*vec;
      for(int i=0;i<offset.size();i++)offset.set_param(i,vec(i));
      //cout<<"state after transform:"<<offset.get_string()<<endl;
    }
    if(scaleWithTemp)s.scalar_mult(1.0/sqrt(caller->invTemp()));
    return s.add(offset);
  };
  gaussian_prop* clone()const{return new gaussian_prop(*this);};
  string show(){
    ostringstream ss; ss<<"StepBy"<<(identity_trans?"":"Covar")<<"["<<(dist?dist->show():"<null>")<<"](1Dfrac="<<oneDfrac<<",scaleWithTemp="<<scaleWithTemp<<")";return ss.str();};
};

//Typically you want to draw from a set of various proposal distributions.
class proposal_distribution_set: public proposal_distribution{
  int Nsize;
  vector<proposal_distribution*>proposals;
  vector<double>shares;
  vector<double>bin_max;
  //Needed for distribution evolution
  double adapt_rate;
  vector<bool>last_accepted;
  int adapt_count;
  int adapt_every;
  int last_dist;
  bool own_pointers;
  //Needed for thermal scalings
  double Tpow;
  vector<double>hot_shares;
  void reset_bins();

public:
  virtual ~proposal_distribution_set(){
    //cout<<"Deleting: "<<show()<<endl;
    if(own_pointers){
      for(auto prop:proposals){
	//cout<<" -Deleting: "<<prop->show()<<endl;
	if(prop)delete prop;
      }}};//delete proposals
  virtual proposal_distribution_set* clone()const;
  proposal_distribution_set(vector<proposal_distribution*> &props,vector<double> &shares,double adapt_rate=0,double Tpow=0,vector<double> hot_shares=vector<double>(),bool take_pointers=true);
  ///For proposals which draw from a chain, we need to know which chain
  void set_chain(chain *c){ch=c;for(int i=0;i<Nsize;i++)proposals[i]->set_chain(c);};
  ///Randomly select from proposals i in 0..n and draw.
  ///Sets type() value at i+10*proposals[i].type()
  //state draw(state &s,Random &rng);
  state draw(state &s,chain *caller);
  void accept();

  void reject();
  virtual void checkpoint(string path)override;
  virtual void restart(string path)override;
  string show();
  string report(int style=0);//For status reporting on acceptance rate(0) or adaptive shares(1)
  vector<proposal_distribution*>  members()const{return proposals;};
};

///Convenience constructor for a set of involution proposals based on the state
proposal_distribution_set involution_proposal_set(const stateSpace &space,double adapt_rate=0);

///DifferentialEvolution
///Based mainly on (ter Braak and Vrugt 08, Stat Comput (2008) 18: 435–446)
///Also SampsonEA2011 ArXiV:1105.2088
///The basic algorithm involves choosing a random state from the past history and proposing a jump in that
/// direction. terBraakEA also add a "small" random jump
///Comments: SampsonEA comment that "It can be shown that this approach is asymptotically Markovian in the limit as one uses the full past history of the chain."  terBraak don't show this, but state "we conjecture that DE- MCZ is ergodic and converges to a stationary distribution with [the right] pdf."  The basis for this is by general reference to the Adaptive Metropolis-Hastings paper (HaarioEA 2001).
///The basic implementation works with just one chain and draws directly from its past.  This is a little different from the terBraak model which draws exclusively from other chains.
///We well also implement a more sophisticated option which allows working with chain-sets, including parallel tempering chain sets.  Here we will be able to
///provide the option of 1. excluding the self-chain from the
class differential_evolution: public proposal_distribution {
  bool have_chain;
  int dim;
  //The chain will manage the history.
  //int Mcount;
  //int save_every;//to save memory it is not necessary to save every element of the history, nearby samples are not independent anyway.
  //int Scount;
  //vector<state> history;
  ///recommended to sometimes try unit scaling, which promotes mode-hopping.
  double gamma_one_frac;
  double reduce_gamma_fac;
  ///size of small random jumps (terBraak)
  double b_small;
  ///We ignore the first Mcount*ignore_frac entries in the history;
  double ignore_frac;
  ///Discount low-probability history
  double unlikely_alpha;
  //Comment:Citing that work terBraak also comments that (retaining)  the "full [sampled] past is required to guarantee ergodicity of the chain."  In HaaioEA, however, it seems clear that retaining a fixed recent fraction of the past (they give 1/2 as an example) would also be sufficient for their theorem.  I haven't carefully studied it, but this also seems to be supported by Roberts+Rosenthal2007, perhaps satisfying their "diminishing adaptation" condition.
  double snooker;
  //SampsonEA don't use it, but terBraakEA seem to prefer a variant on DE where the scaling is set by a separate drawing of states;  I guess this propotes variety.
  bool do_support_mixing;
  double temperature_mixing_factor;
  /*
  static int icount;
  static vector< vector< int > > mixcounts;
  static vector< int > trycounts;
  */

  //SOME INTERNAL FUNCTIONS
  //state draw_standard(state &s, Random &rng);
  //state draw_snooker(state &s, Random & rng);
  //state  draw_from_chain(Random &rng);
  state draw_standard(state &s, chain *caller);
  state draw_snooker(state &s, chain *caller);
  int  draw_i_from_chain(chain *caller,chain *c);
  state  draw_from_chain(chain *caller);
  int get_min_start_size(){return dim*10;};//Don't run below this chain length
  int get_min_cut_size(){return dim*100;};  //Don't ignore early part below this length
public:
  void reduce_gamma(double factor){reduce_gamma_fac=factor;};
  void mix_temperatures_more(double factor){temperature_mixing_factor=factor;};
  differential_evolution(double snooker=0.0, double gamma_one_frac=0.1,double b_small=0.0001,double ignore_frac=0.3,double unlikely_alpha=0);
  void set_chain(chain *c){ch=c;have_chain=true;dim=ch->getDim();};
  ///Verify that the chain is set and that the chain has some minimum length of 10*dim.
  bool is_ready(){return(have_chain&&ch->size()>=get_min_start_size());};
  //state draw(state &s,Random &rng);
  state draw(state &s,chain *caller);
  virtual differential_evolution* clone()const{return new differential_evolution(*this);};
  string show();
  bool support_mixing(bool do_it){do_support_mixing=do_it;return do_it;};
  bool support_mixing(){return do_support_mixing;};
};
#endif

/*
Idea:
  Intra-specific differential evolution.  It is non-adaptive to have sex with a frog.  Similarly, if the posterior surface includes well-separated peaks of wildly differing sizes and shapes, then the combined information from all of them might not be very good for any of them.
  An option is to use unsupervised learning techniques to select interesting sub-regions of the parameter space which may be more coherently related to each other, then to (some of the time) run DE independently in each.
  A relatively simple implementation would be to use something like K-means clustering to cluster the parameter-space points, and to run DE separately on the cluster points.  (It might be tricky to figure out how to do the detailed balance if the computation suggests a jump out of the cluster region.)  There could be another algorithm for jumping between clusters (say prop to evidence?).
  It might be that we want not to do exactly K-means clustering, but something similar, with posterior weighting in calculating the centers.  Then we might test for the coherence of the clusters, possible splitting or merging them.  Useful statistics might be the cluster variance matrix and the cluster parameter expectation value.  By comparing the posterior at the expectation value with the maximum, we might be able to decide when clusters should be divided or merged.
  The clustering could be managed by arrays of vectors of references to the chain points, with occasional updates of the clustering centers, or just occasional updates to the whole population set overall, keeping it fixed erstwhile... and not referencing the more recent chain history.
*/


#ifndef ASLAM_BACKEND_OPTIMIZER_2_HPP
#define ASLAM_BACKEND_OPTIMIZER_2_HPP

#include <boost/shared_ptr.hpp>
//#include <boost/function.hpp>
#include <sparse_block_matrix/linear_solver.h>
#include <Eigen/Core>
#include <aslam/Exceptions.hpp>
#include <aslam/backend/DogLegTrustRegionPolicy.hpp>
#include <aslam/backend/GaussNewtonTrustRegionPolicy.hpp>
#include <aslam/backend/LevenbergMarquardtTrustRegionPolicy.hpp>
#include <aslam/backend/TrustRegionPolicy.hpp>
#include <boost/thread.hpp>
#include <sm/assert_macros.hpp>
#include <sm/timing/Timer.hpp>
#include "OptimizationProblemBase.hpp"
#include "Optimizer2Options.hpp"
#include "backend.hpp"

namespace sm {

class PropertyTree;
}
namespace aslam {
namespace backend {
class LinearSystemSolver;

/**
 * \class Optimizer2
 *
 * A sparse Gauss-Newton/LM optimizer.
 *
 * The notation in this file follows Harley and Zisserman, Appendix 6.
 *
 * Some Additions to the standard algorithm:
 * Choice of Lambda:
 * H.B. Nielsen. Damping Parameter in Marquardts Method. Technical Report
 * IMM-REP-1999-05, Technical University of Denmark, 1999.
 *
 * Jacobian Normalisation:
 * Jean-Claude Trigeassou, Thierry Poinot & Sandrine Moreau (2003):
 * A Methodology for Estimation of Physical Parameters, Systems Analysis Modelling Simulation, 43:7, 925-943
 *
 * Erik Etien, Damien Halbert, and Thierry Poinot
 * Improved JilesAtherton Model for Least Square Identification Using Sensitivity Function Normalization
 * IEEE TRANSACTIONS ON MAGNETICS, VOL. 44, NO. 7, JULY 2008
 *
 */
class Optimizer2 {
  public:
    //  typedef sm::timing::Timer Timer;
    /// Swapping this to the dummy timer will disable timing
    typedef sm::timing::DummyTimer Timer;
    typedef sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> SparseBlockMatrix;

    SM_DEFINE_EXCEPTION(Exception, aslam::Exception);

    Optimizer2(const Optimizer2Options& options = Optimizer2Options());
    Optimizer2(const sm::PropertyTree& config, boost::shared_ptr<LinearSystemSolver> linearSystemSolver,
               boost::shared_ptr<TrustRegionPolicy> trustRegionPolicy);
    virtual ~Optimizer2();

    /// \brief Set up to work on the optimization problem.
    void setProblem(boost::shared_ptr<OptimizationProblemBase> problem);

    /// \brief initialize the optimizer to run on an optimization problem.
    void initialize();

    /// \brief initialize the linear solver specified in the optimizer options.
    void initializeLinearSolver();

    void initializeTrustRegionPolicy();

    /// \brief Run the optimization
    SolutionReturnValue optimize();

    /// \brief Get the optimizer options.
    Optimizer2Options& options();

    /// \brief return the reduced system dx
    const Eigen::VectorXd& dx() const;

    /// The value of the objective function.
    double J() const;

    /// \brief compute the full covariance matrix. This is expensive.
    void computeCovariances(SparseBlockMatrix& outP, double lambda);

    /// \brief compute only the diagonal covariance blocks.
    void computeDiagonalCovariances(SparseBlockMatrix& outP, double lambda);

    /// \brief compute only the covariance blocks associated with the block indices passed as an argument
    void computeCovarianceBlocks(const std::vector<std::pair<int, int> >& blockIndices, SparseBlockMatrix& outP,
                                 double lambda);

    void computeHessian(SparseBlockMatrix& outH, double lambda);

    /// \brief Evaluate the error at the current state.
    double evaluateError(bool useMEstimator);

    /// \brief Get dense design variable i.
    DesignVariable* designVariable(size_t i);

    /// \brief how many dense design variables are involved in the problem
    size_t numDesignVariables() const;

    /// \brief print the internal timing information.
    void printTiming() const;

    /// \brief Do a bunch of checks to see if the problem is well-defined. This includes checking that every error term
    /// is
    ///        hooked up to design variables and running finite differences on error terms where this is possible.
    void checkProblemSetup();

    /// \brief Build the Gauss-Newton matrices.
    void buildGnMatrices(bool useMEstimator);

    /// Returns the linear solver
    template <class L>
    L* getSolver();

    const Matrix* getJacobian() const;

    const LinearSystemSolver* getBaseSolver() const;

  private:
    /// \brief Zero the Gauss-Newton matrices.
    void zeroMatrices();

    /// \brief Revert the last state update.
    void revertLastStateUpdate();

    /// \brief Apply a state update.
    double applyStateUpdate();

    /// \brief The dense update vector.
    Eigen::VectorXd _dx;

    /// \brief The current value of the cost function.
    double _J;

    /// \brief The previous value of the cost function.
    double _p_J;

    boost::shared_ptr<LinearSystemSolver> _solver;

    boost::shared_ptr<TrustRegionPolicy> _trustRegionPolicy;

    /// \brief The current optimization problem.
    boost::shared_ptr<OptimizationProblemBase> _problem;

    /// \brief all design variables...first the non-marginalized ones (the dense ones), then the marginalized ones.
    std::vector<DesignVariable*> _designVariables;

    /// \brief all of the error terms involved in this problem
    std::vector<ErrorTerm*> _errorTerms;

    /// \brief the current set of options
    Optimizer2Options _options;
};

}  // namespace backend
}  // namespace aslam

#include "aslam/backend/Optimizer2.tpp"

#endif /* ASLAM_BACKEND_OPTIMIZER_HPP */