#include <stdio.h>

#include <complex>
#include <iostream>
#include <iomanip>
#include <cstdlib>
#include <cmath>
#include <utility>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"

#include "dune/common/stdstreams.hh"
#include "dune/grid/sgrid.hh"
#include "dune/grid/uggrid.hh"
#include "dune/grid/io/file/amirameshreader.hh"
#include "dune/grid/io/file/amirameshwriter.hh"
#include "dune/grid/common/gridinfo.hh"
#include "dune/istl/solvers.hh"

#include "utilities/enums.hh"
#include "fem/assemble.hh"
#include "fem/gridmanager.hh"
#include "fem/lagrangespace.hh"
#include "fem/functional_aux.hh"
#include "fem/coarsening.hh"
#include "fem/norms.hh"

#include "linalg/umfpack_solve.hh"

#include "timestepping/limexWithoutJens.hh"
#include "timestepping/semieuler.hh"
#include "timestepping/extrapolation.hh"

#include "io/vtk.hh"
#include "io/amira.hh"

#include "utilities/kaskopt.hh"
#include "amiramesh.hh"

using namespace Kaskade;

#include "integrate.hh"
#include "aliev.hh"

boost::timer::cpu_timer partTimer;
double assTime, solveTime;

struct InitialValue
{
  typedef double Scalar;
  static int const components = 1;
  typedef Dune::FieldVector<Scalar,components> ValueType;

  InitialValue(int c): component(c) {}

  template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }
  template <class Cell>
  ValueType value(Cell const& cell,
                  Dune::FieldVector<typename Cell::ctype,Cell::dimension> const& localCoordinate) const
  {
	Dune::FieldVector<typename Cell::ctype,Cell::dimensionworld> x = cell.geometry().global(localCoordinate);
	double R=1.0;		// slab: R=0.09		heart: R=1.0
	double beta=1000.0;
	double f=0.0;
	double r;

	//r = sqrt( (x[0]-(0.3))*(x[0]-(0.3)) + (x[1]-(-2.4))*(x[1]-(-2.4)) + (x[2]-182.5)*(x[2]-182.5) );	// heart smooth
	r = sqrt( (x[0]-(-0.9))*(x[0]-(-0.9)) + (x[1]-1.7)*(x[1]-1.7) + (x[2]-1.1)*(x[2]-1.1) );			// heart glenn
	//r = sqrt( (x[0]-1.5)*(x[0]-1.5) + (x[1]-1.5)*(x[1]-1.5) + (x[2]-1.1)*(x[2]-1.1) );				// slab
	beta = -1e4*log(0.001);
	if (r < R) f = 1;
	else f = exp(-beta*(R-r)*(R-r));

    if (component==0)
      return 0.0;		// 0.95*f;
    else if (component==1)
      return 0.0;
    else
      assert("wrong index!\n"==0);
    return 0;
  }

private:
  int component;
};

template <class Eq>
typename Eq::AnsatzVars::VariableSet transformBack(Eq const& eq, typename Eq::AnsatzVars::VariableSet const& vars)
{
  typename Eq::AnsatzVars::VariableSet  x(vars);

  for (int j=0; j<boost::fusion::at_c<0>(x.data).space().degreesOfFreedom(); ++j) {
    (boost::fusion::at_c<0>(x.data)).coefficients()[j] = eq.theta((boost::fusion::at_c<0>(x.data)).coefficients()[j]);
  }

  return x;
}

template <class Eq>
typename Eq::AnsatzVars::VariableSet transformBackDeriv(Eq const& eq, typename Eq::AnsatzVars::VariableSet const& vars)
{
  typename Eq::AnsatzVars::VariableSet  x(vars);

  for (int j=0; j<boost::fusion::at_c<0>(x.data).space().degreesOfFreedom(); ++j) {
    (*boost::fusion::at_c<0>(x.data))[j] = eq.dtheta((*boost::fusion::at_c<0>(x.data))[j]);
  }

  return x;
}

template <class Eq>
typename Eq::AnsatzVars::VariableSet transformBackDerivMul(Eq const& eq, typename Eq::AnsatzVars::VariableSet const& vars1, typename Eq::AnsatzVars::VariableSet const& vars2)
{
  typename Eq::AnsatzVars::VariableSet  x(vars1);

  for (int j=0; j<boost::fusion::at_c<0>(x.data).space().degreesOfFreedom(); ++j) {
    (*boost::fusion::at_c<0>(x.data))[j] = eq.dtheta((*boost::fusion::at_c<0>(x.data))[j]) * (*boost::fusion::at_c<0>(vars2.data))[j];
  }

  return x;
}

template <class Eq>
typename Eq::AnsatzVars::VariableSet transformForward(Eq const& eq, typename Eq::AnsatzVars::VariableSet const& vars)
{
  typename Eq::AnsatzVars::VariableSet  x(vars);

  for (int j=0; j<boost::fusion::at_c<0>(x.data).space().degreesOfFreedom(); ++j) {
    (*boost::fusion::at_c<0>(x.data))[j] = eq.zeta((*boost::fusion::at_c<0>(x.data))[j]);
  }

  return x;
}

template <class Eq>
typename Eq::AnsatzVars::VariableSet transformInitial(Eq const& eq, typename Eq::AnsatzVars::VariableSet const& vars)
{
  typename Eq::AnsatzVars::VariableSet  x(vars);

  for (int j=0; j<boost::fusion::at_c<0>(x.data).space().degreesOfFreedom(); ++j) {
    (*boost::fusion::at_c<0>(x.data))[j] = eq.zetaInitial((*boost::fusion::at_c<0>(x.data))[j]);
  }

  return x;
}

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

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

  std::cout << "Start cardiac simulation (Aliev-Panfilov)" << std::endl;

  fexcept_t flag;
  fegetexceptflag(&flag,FE_DIVBYZERO|FE_INVALID|FE_OVERFLOW|FE_UNDERFLOW);
  fesetexceptflag(&flag,FE_DIVBYZERO|FE_INVALID|FE_OVERFLOW|FE_UNDERFLOW);

  boost::timer::cpu_timer totalTimer;

  int const dim = 3;
  int refinements;
  int  order, extrapolOrder, maxSteps;
  double dt, maxDT, T, rTolT, aTolT, rTolX, aTolX, writeInterval;
  std::string logname = "cardio.log";
  bool refineTimeStep;
  std::string zetaFile, thetaFile;
  double xi, a, gs, ga, mu1, mu2, eps1, kappa, v_rest, v_peak;

  std::string empty, geoFile("slab242.am");
  geoFile = getParameter(pt, "file", geoFile);

  refinements = 0;   // slab: 3,  Herz: 0
  order = 2;
  extrapolOrder = 2;
  T = 1.0;
  dt = 0.01;
  maxDT = 4.0;
  writeInterval = 1.0;
  refineTimeStep = true;
  rTolT = 1e-3;
  aTolT = 1e-3;
  rTolX = 1e-3;
  aTolX = 1e-3;
  maxSteps = 10000;
// 	thetaFile = "zetaLinearInv.gnu";
// 	zetaFile  = "zetaLinear.gnu";
// 	thetaFile = "zetaInv.gnu";     			// original nonlinear scaling
// 	zetaFile  = "zeta.gnu";        			// original nonlinear scaling
// 	thetaFile = "theta-tanh5-100000.gnu";   // tanh nonlinear scaling
// 	zetaFile  = "zeta-tanh5-100000.gnu";    // tanh nonlinear scaling
	thetaFile = "theta-tanh2-100000.gnu";   // tanh nonlinear scaling
	zetaFile  = "zeta-tanh2-100000.gnu";    // tanh nonlinear scaling
// 	thetaFile = "zetaInv-MW.gnu";     		// MW nonlinear scaling
// 	zetaFile  = "zeta-MW.gnu";        		// MW nonlinear scaling
// 	thetaFile = "theta-2d-1e-5.gnu";		// MW nonlinear scaling
// 	zetaFile  = "zeta-2d-1e-5.gnu";			// MW nonlinear scaling
//	thetaFile = "theta-2d-1e-7.gnu";		// MW nonlinear scaling
//	zetaFile  = "zeta-2d-1e-7.gnu";			// MW nonlinear scaling
// 	thetaFile = "zetaInvQuadratic.gnu";     // Quadratic nonlinear scaling
// 	zetaFile  = "zetaQuadratic.gnu";        // Quadratic nonlinear scaling
// 	thetaFile = "zetaIdentityInv.gnu";
// 	zetaFile  = "zetaIdentity.gnu";
// 	thetaFile = "";
// 	zetaFile  = "";

  xi   = 1.0;
  kappa = 0.001;
  a = 0.1;
  gs = 8.0;
  ga = 8.0;
  mu1 = 0.07;
  mu2 = 0.3;
  eps1 = 0.01;

  v_rest = 0.0;   //-85.0;
  v_peak = 1.0;   //35.0;

  int heapSize=16384;
  typedef Dune::UGGrid<dim> Grid;
  Grid::setDefaultHeapSize(heapSize);
  Dune::GridFactory<Grid> factory;

  DuneAmiraMesh<dim,dim,Grid> mesh(geoFile.c_str());
  mesh.InsertUGGrid(factory);

  std::unique_ptr<Grid> grid( factory.createGrid() );
  for (int k=0; k<refinements; k++)
  {
    grid->globalRefine(1);
  }

  // some information on the refined mesh
  std::cout << std::endl << "Grid: " << grid->size(0) << " tetrahedra, " << std::endl;
  std::cout << "      " << grid->size(1) << " triangles, " << std::endl;
  std::cout << "      " << grid->size(dim-1) << " edges, " << std::endl;
  std::cout << "      " << grid->size(dim) << " points" << std::endl;
  // a gridmanager is constructed
  // as connector between geometric and algebraic information
  GridManager<Grid> gridManager(std::move(grid));
  gridManager.enforceConcurrentReads(true);

  typedef Grid::LeafGridView LeafView;

  	// construct involved spaces.
    //typedef ContinuousHierarchicMapper<double,Grid>  LMapper;
  typedef ContinuousLagrangeMapper<double,LeafView> LMapper;
  typedef FEFunctionSpace<LMapper> H1Space;

  H1Space h1Space(gridManager,gridManager.grid().leafView(),order);

  	typedef boost::fusion::vector<H1Space const*> Spaces;
  	Spaces spaces(&h1Space);

  	// construct variable list.
  	typedef boost::fusion::vector<VariableDescription<0,1,0>,
    							  VariableDescription<0,1,1> > VariableDescriptions;
  	std::string varNames[2] = { "u", "v" };

  	typedef VariableSetDescription<Spaces,VariableDescriptions> VariableSet;
  	VariableSet variableSet(spaces,varNames);

  	typedef AlievPanfilovEquation<double,VariableSet> Equation;
  	Equation Eq(xi,kappa,a,gs,ga,mu1,mu2,eps1,v_rest,v_peak,zetaFile,thetaFile);
  	// Eq.check();

  std::vector<VariableSet::VariableSet> solutions;
  std::cout << "solutions.size = " << solutions.size() << std::endl;
  // std::vector<VariableSet::VariableSet> devnull;

  Eq.time(0);
  VariableSet::VariableSet x(variableSet);
  Eq.scaleInitialValue<0>(InitialValue(0),x);
  Eq.scaleInitialValue<1>(InitialValue(1),x);

  T     = 1.0;
  dt    = 1e-3;			//0.001;
  maxDT = 1.0;
  rTolT = 5e-3;		// slab: 1e-2; herz(Glenn): 5e-3;		//1e-5/8.0;
  aTolT = 5e-3;		// slab: 1e-2; herz(Glenn): 5e-3;		//1e-5/8.0;
  rTolX = 1e-2;		// slab: 1e-2; herz(Glenn): 5e-3;		//1e-5*2.0;
  aTolX = 1e-2;		// slab: 1e-2; herz(Glenn): 5e-3;		//1e-5*2.0;
  maxSteps = 10000;
  x = integrate(gridManager,Eq,variableSet,spaces,gridManager.grid(),
                dt,maxDT,T,maxSteps,rTolT,aTolT,rTolX,aTolX,extrapolOrder,
                std::back_inserter(solutions),writeInterval,x,DirectType::MUMPS);

  T     = 225.0;
  dt    = 5e-3;		//0.001;
  rTolT = 2e-3;		//1e-5/8.0;
  aTolT = 2e-3;		//1e-5/8.0;
  rTolX = 1.0e-2;   //1.5e-2;		//1e-5*2.0;
  aTolX = 1.0e-2;   //1.5e-2;		//1e-5*2.0;
  maxSteps = 10000;
  x = integrate(gridManager,Eq,variableSet,spaces,gridManager.grid(),
                dt,maxDT,T,maxSteps,rTolT,aTolT,rTolX,aTolX,extrapolOrder,
                std::back_inserter(solutions),writeInterval,x,DirectType::MUMPS);

  T     = 226.0;
  dt    = 1e-3;			//0.001;
  rTolT = 2e-3;		//1e-5/8.0;
  aTolT = 2e-3;		//1e-5/8.0;
  rTolX = 1.5e-2;		//1e-5*2.0;
  aTolX = 1.5e-2;		//1e-5*2.0;
  maxSteps = 10000;
  x = integrate(gridManager,Eq,variableSet,spaces,gridManager.grid(),
                dt,maxDT,T,maxSteps,rTolT,aTolT,rTolX,aTolX,extrapolOrder,
                std::back_inserter(solutions),writeInterval,x,DirectType::MUMPS);

  T     = 1000.0;
  dt    = 1e-3;		//0.001;
  rTolT = 2e-3;		//1e-5/8.0;
  aTolT = 2e-3;		//1e-5/8.0;
  rTolX = 1.5e-2;		//1e-5*2.0;
  aTolX = 1.5e-2;		//1e-5*2.0;
  maxSteps = 10000;
  x = integrate(gridManager,Eq,variableSet,spaces,gridManager.grid(),
                dt,maxDT,T,maxSteps,rTolT,aTolT,rTolX,aTolX,extrapolOrder,
                std::back_inserter(solutions),writeInterval,x,DirectType::MUMPS);

  std::cout << " End of integrate" << std::endl;

//   if (order>1)
//     gridManager.globalRefine(order);

//   for (int i=0; i<solutions.size(); ++i) {
//     //VariableSet::VariableSet x(transformBack(Eq,solutions[i]));
//     VariableSet::VariableSet xT(transformBack(Eq,x));
//
//     std::ostringstream fn;
//     fn << "graph/out-rescaled";
//     fn.width(3);
//     fn.fill('0');
//     fn.setf(std::ios_base::right,std::ios_base::adjustfield);
//     fn << i;
//     fn.flush();
//     writeVTKFile(variableSet,xT,fn.str());
//   }

//
//   for (int i=0; i<solutions.size(); ++i) {
//     std::ostringstream fn;
//     fn << "graph/outScaled";
//     fn.width(3);
//     fn.fill('0');
//     fn.setf(std::ios_base::right,std::ios_base::adjustfield);
//     fn << i;
//     fn.flush();
//     writeVTKFile(variableSet,solutions[i],fn.str());
//   }

  std::cout << "End Aliev-Panfilov integration" << std::endl;

  return 0;
}


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include "hoa_allrad_gain_calculator.hpp"

#include <libefl/basic_matrix.hpp>
#include <libefl/vector_functions.hpp>

#include <libobjectmodel/object_vector.hpp>
#include <libobjectmodel/hoa_source.hpp>

#include <boost/filesystem.hpp>

#include <cassert>
#include <ciso646>
#include <cmath>
#include <cstdio>

namespace visr
{
namespace rcl
{

HoaAllRadGainCalculator::
HoaAllRadGainCalculator( SignalFlowContext const & context,
                         char const * name,
                         CompositeComponent * parent,
                         std::size_t numberOfObjectChannels,
                         panning::LoudspeakerArray const & regularArrayConfig,
                         panning::LoudspeakerArray const & realArrayConfig,
                         efl::BasicMatrix<Afloat> const & decodeMatrix,
                         pml::ListenerPosition const & listenerPosition /*= pml::ListenerPosition()*/,
                         bool adaptiveListenerPosition /*= false*/ )
  : AtomicComponent( context, name, parent )
  , mObjectInput( "objectInput", *this, pml::EmptyParameterConfig() )
  , mGainMatrixInput( "gainInput", *this, pml::MatrixParameterConfig( realArrayConfig.getNumRegularSpeakers(), numberOfObjectChannels ) )
  , mGainMatrixOutput( "gainOutput", *this, pml::MatrixParameterConfig( realArrayConfig.getNumRegularSpeakers(), numberOfObjectChannels ) )
  , mListenerInput( adaptiveListenerPosition ? new ParameterInput<pml::DoubleBufferingProtocol, pml::ListenerPosition>( "listenerInput", *this ) : nullptr )
  , mRegularDecodeMatrix( decodeMatrix.numberOfRows(), regularArrayConfig.getNumRegularSpeakers(), cVectorAlignmentSamples )
  , mRealDecodeMatrix( regularArrayConfig.getNumSpeakers(), realArrayConfig.getNumRegularSpeakers(), cVectorAlignmentSamples )
{
  std::size_t const numRegularSpeakers = regularArrayConfig.getNumSpeakers();
  std::size_t const numHarmonicSignals = decodeMatrix.numberOfRows();

  // Deduce the Ambisonics order from the number of harmonic signals
  std::size_t const hoaOrder = static_cast<std::size_t>(std::ceil( std::sqrt( numHarmonicSignals ) )) - 1;
  if( ((hoaOrder + 1)*(hoaOrder + 1) != numHarmonicSignals) or (numHarmonicSignals == 0) )
  {
    throw std::invalid_argument( "The number of rows in the decoder matrix does not correspond to a real-valued HOA order." );
  }
  if( decodeMatrix.numberOfColumns() != numRegularSpeakers )
  {
    throw std::invalid_argument( "The number of columns in the decoder matrix does not match the size of the regular array." );
  }

  mRegularDecodeMatrix.copy( decodeMatrix );

  mAllRadCalculator.reset( new panning::AllRAD( regularArrayConfig,
    realArrayConfig,
    mRegularDecodeMatrix,
    static_cast<unsigned int>(hoaOrder) ) );

  // set the default initial listener position.
  // This also initialises the internal data members (e.g., the VBAP calculator and the calculation of the VBAP decode matrix)
  setListenerPosition( listenerPosition );
}

HoaAllRadGainCalculator::~HoaAllRadGainCalculator()
{
}

void HoaAllRadGainCalculator::setListenerPosition( CoefficientType x, CoefficientType y, CoefficientType z )
{
  mAllRadCalculator->setListenerPosition( x, y, z );
}

void HoaAllRadGainCalculator::setListenerPosition( pml::ListenerPosition const & pos )
{
  setListenerPosition( pos.x(), pos.y(), pos.z() );
}

void HoaAllRadGainCalculator::process()
{
  if( mListenerInput and mListenerInput->changed() )
  {
    pml::ListenerPosition const & pos = mListenerInput->data();
    mAllRadCalculator->setListenerPosition( pos.x(), pos.y(), pos.z() );
    mListenerInput->resetChanged();
  }

  // Because of the SharedData protocol of the gain input, we must always copy the input and
  // handle the HOA objects.
  pml::ObjectVector const & objects =  mObjectInput.data();
  pml::MatrixParameter<SampleType> const & inMtx = mGainMatrixInput.data();
  pml::MatrixParameter<SampleType> & outMtx = mGainMatrixOutput.data();
  outMtx.copy( inMtx );

  std::size_t const numChannels = outMtx.numberOfColumns();
  std::size_t const numLoudspeakers = outMtx.numberOfRows();

  efl::BasicMatrix<Afloat> const & decodeGains = mAllRadCalculator->decodingGains();

  // For the moment, we assume that the audio channels of the objects are identical to the final channel numbers.
  // Any potential re-routing will be added later.
  for( objectmodel::Object const & obj : objects )
  {
    // Note: This does not allow object types derived from HoaSource.
    objectmodel::ObjectTypeId const ti = obj.type();
    if( ti != objectmodel::ObjectTypeId::HoaSource )
    {
      continue;
    }
    try
    {
      objectmodel::HoaSource const & hoaSrc = dynamic_cast<objectmodel::HoaSource const &>(obj);
      std::size_t const numHoaSignals = hoaSrc.numberOfChannels();
      if( numHoaSignals != (hoaSrc.order() + 1)*(hoaSrc.order() + 1) )
      {
        throw std::runtime_error( "The number of harmonic signals is inconsistent with the HOA order." );
      }
      for( std::size_t sigIdx( 0 ); sigIdx < numHoaSignals; ++sigIdx )
      {
        std::size_t const chIdx = hoaSrc.channelIndex( sigIdx );
        if( chIdx >= numChannels )
        {
          throw std::runtime_error( "Channel index exceeds maximum admissible value." );
        }
        for( std::size_t spkIdx( 0 ); spkIdx < numLoudspeakers; ++spkIdx )
        {
          outMtx( spkIdx, chIdx ) = decodeGains( sigIdx, spkIdx );
        }
      }
    }
    catch( std::exception const & ex )
    {
      status( StatusMessage::Error, "Error decoding HOA source: ",  ex.what() );
      return;
    }
  } // for( objectmodel::ObjectVector::value_type const & objEntry : objects )
  mObjectInput.resetChanged(); // Always reset.
}

} // namespace rcl
} // namespace visr


#include <chrono>
#include <fstream>
#include <jsoncpp/json/json.h>
#include <NTL/ZZ.h>
#include "innergen.h"
#include "InnergenMapTask.h"

void run_map_task(int rho, int d, int numLines, file secrets, file parameters, file outputPath, file resultsPath){
	auto start = std::chrono::high_resolution_clock::now();

	//Read secrets into string
	std::ifstream secrets_ifs(secrets);
	std::string secrets_str,secrets_line;
	while(getline(secrets_ifs,secrets_line)) secrets_str+=secrets_line;
	secrets_ifs.close();

	//Read parameters into string
	std::ifstream params_ifs(parameters);
	std::string params_str,params_line;
	while(getline(params_ifs,params_line)) params_str+=params_line;
	params_ifs.close();

	InnergenMapTask mapTask(rho, d, numLines, secrets_str, params_str, outputPath);
	NTL::ZZ result = mapTask.run();
	auto finish = std::chrono::high_resolution_clock::now();
	auto totalTaskTime = std::chrono::duration_cast<std::chrono::nanoseconds>(finish-start).count();
	Json::Value root;
	std::stringstream ss;
	ss << result;
	root["result"] = ss.str();
	root["task_execution_time"] = Json::UInt64(totalTaskTime);
	root["number_of_inputs"] = Json::UInt64(mapTask.numberEncrypted);
	root["total_encryption_time"] = Json::UInt64(mapTask.totalEncryptionTime);
	std::ofstream results_ofs(resultsPath);
	if(results_ofs.is_open()){
		results_ofs << root;
		results_ofs.close();
	}
}


/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <mc_rbdyn/ForceSensor.h>
#include <mc_rbdyn/Robot.h>
#include <mc_rbdyn/rpy_utils.h>
#include <mc_rtc/logging.h>

#include <boost/filesystem.hpp>
#include <fstream>
namespace bfs = boost::filesystem;

namespace mc_rbdyn
{

namespace detail
{
/** Structure used to hold the calibration data. This structure is *NOT*
 * intended to be constructed directly, see @ForceSensorsCalibrator for usage.
 * It is composed of 13 parameters (real numbers) that should be loaded
 * from a calibration file. **/
struct ForceSensorCalibData
{
public:
  /** Default constructor, always returns zero contribution */
  ForceSensorCalibData() {}

  /** Load data from a file, using a gravity vector. The file
   * should contain 13 parameters in that order: mass (1),
   * rpy for X_f_ds (3), position for X_p_vb (3), wrench
   * offset (6).
   *
   * If the file does not exist, default calibration parameters that do nothing
   * will be used. If the file exists but its parameters are invalid, an exception will be thrown.
   *
   * @throws std::invalid_argument if the file is ill-formed.
   **/
  void loadData(const std::string & filename, const Eigen::Vector3d & gravity)
  {
    constexpr int nr_params = 13;
    std::ifstream strm(filename);
    if(!strm.is_open())
    {
      LOG_ERROR("Could not open " << filename)
      return;
    }

    // Vector 13d
    Eigen::Matrix<double, nr_params, 1> X;
    double temp;
    for(int i = 0; i < nr_params; ++i)
    {
      strm >> temp;
      if(!strm.good())
      {
        LOG_ERROR_AND_THROW(std::invalid_argument, "[ForceSensorCalibData] Invalid calibration file "
                                                       << filename << ". " << dataRequirements())
      }
      if(strm.eof())
      {
        LOG_ERROR_AND_THROW(std::invalid_argument, "[ForceSensorCalibData] Calibration file "
                                                       << filename << " too short. " << dataRequirements())
      }
      X(i) = temp;
    }

    mass_ = X(0);
    worldForce_ = sva::ForceVecd(Eigen::Vector3d(0., 0., 0.), -X(0) * gravity);
    X_f_ds_ = mc_rbdyn::rpyToPT(X.segment<3>(1));
    X_p_vb_ = sva::PTransformd(Eigen::Matrix3d::Identity(), X.segment<3>(4));
    offset_ = sva::ForceVecd(X.segment<6>(7));
  }

  static std::string dataRequirements()
  {
    return R"(The file should contain 13 parameters in that order:
    - mass (1)
    - rpy of local rotation between the model force sensor and real one (3)
    - translation from the parent body to the virtual link CoM (3)
    - wrench offset (6).)";
  }

  /** Return the gravity wrench applied on the force sensor in the sensor
   * frame, i.e. the external force $f_{ext}$ is:
   * $f_{ext} = f_{mes} - wfToSensor$.
   * @param X_0_p the transform in the world frame of the parent body of the sensor
   * @param X_p_f the transform from the parent body to the sensor itself
   */
  inline sva::ForceVecd wfToSensor(const sva::PTransformd & X_0_p, const sva::PTransformd & X_p_f) const
  {
    sva::PTransformd X_0_ds = X_f_ds_ * X_p_f * X_0_p;
    sva::PTransformd X_0_vb(X_0_ds.inv().rotation(), (X_p_vb_ * X_0_p * X_0_ds.inv()).translation());
    return offset_ + X_0_vb.transMul(worldForce_);
  }

  /** Return X_f_ds, the pure rotation transform from the position given by
   * the URDF and the one estimated by calibration
   */
  inline const sva::PTransformd & X_f_ds() const
  {
    return X_f_ds_;
  }

  /** Return the mass of the link generating the wrench */
  inline double mass() const
  {
    return mass_;
  }

  inline const sva::ForceVecd & offset() const
  {
    return offset_;
  }

private:
  /** Mass of the link generating the wrench */
  double mass_ = 0.0;
  /** Constant gravity wrench applied on the force sensor
   * in the world frame: $\{0, mg \} */
  sva::ForceVecd worldForce_ = sva::ForceVecd(Eigen::Vector6d::Zero());
  /** Local rotation between the model force sensor and real one */
  sva::PTransformd X_f_ds_ = sva::PTransformd::Identity();
  /** Transform from the parent body to the virtual link CoM */
  sva::PTransformd X_p_vb_ = sva::PTransformd::Identity();
  /** Force/torque offset */
  sva::ForceVecd offset_ = sva::ForceVecd(Eigen::Vector6d::Zero());
};
} // namespace detail

ForceSensor::ForceSensor() : ForceSensor("", "", sva::PTransformd::Identity()) {}

ForceSensor::ForceSensor(const std::string & name, const std::string & parentBodyName, const sva::PTransformd & X_p_f)
: name_(name), parentBody_(parentBodyName), X_p_f_(X_p_f), wrench_(Eigen::Vector6d::Zero()),
  calibration_(std::make_shared<detail::ForceSensorCalibData>())
{
}

ForceSensor::~ForceSensor() {}

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

const std::string & ForceSensor::parentBody() const
{
  return parentBody_;
}

const sva::PTransformd & ForceSensor::X_p_f() const
{
  return X_p_f_;
}

const sva::PTransformd ForceSensor::X_0_f(const mc_rbdyn::Robot & robot) const
{
  return X_p_f() * robot.bodyPosW(parentBody_);
}

const sva::ForceVecd & ForceSensor::wrench() const
{
  return wrench_;
}

void ForceSensor::wrench(const sva::ForceVecd & wrench)
{
  wrench_ = wrench;
}

void ForceSensor::loadCalibrator(const std::string & calib_file, const Eigen::Vector3d & gravity)
{
  if(!bfs::exists(calib_file))
  {
    LOG_WARNING("No calibration file " << calib_file << " found for force sensor " << name());
    return;
  }
  else
  {
    calibration_->loadData(calib_file, gravity);
  }
}

const sva::PTransformd & ForceSensor::X_fsmodel_fsactual() const
{
  return calibration_->X_f_ds();
}

const sva::PTransformd ForceSensor::X_fsactual_parent() const
{
  return (X_fsmodel_fsactual() * X_p_f_).inv();
}

double ForceSensor::mass() const
{
  return calibration_->mass();
}

const sva::ForceVecd & ForceSensor::offset() const
{
  return calibration_->offset();
}

const sva::ForceVecd ForceSensor::wrenchWithoutGravity(const mc_rbdyn::Robot & robot) const
{
  sva::PTransformd X_0_p = robot.mbc().bodyPosW[robot.bodyIndexByName(parentBody_)];
  auto w = wrench_ - calibration_->wfToSensor(X_0_p, X_p_f_);
  return w;
}

sva::ForceVecd ForceSensor::worldWrench(const mc_rbdyn::Robot & robot) const
{
  sva::ForceVecd w_fsactual = wrench();
  sva::PTransformd X_parent_0 = robot.mbc().bodyPosW[robot.bodyIndexByName(parentBody_)].inv();
  sva::PTransformd X_fsactual_0 = X_parent_0 * X_fsactual_parent();
  return X_fsactual_0.dualMul(w_fsactual);
}

sva::ForceVecd ForceSensor::worldWrenchWithoutGravity(const mc_rbdyn::Robot & robot) const
{
  sva::ForceVecd w_fsactual = wrenchWithoutGravity(robot);
  sva::PTransformd X_parent_0 = robot.mbc().bodyPosW[robot.bodyIndexByName(parentBody_)].inv();
  sva::PTransformd X_fsactual_0 = X_parent_0 * X_fsactual_parent();
  return X_fsactual_0.dualMul(w_fsactual);
}

} // namespace mc_rbdyn


#include "problem.h"

#include <algorithm>
#include <iostream>
#include <fstream>
#include <functional>
#include <map>
#include <numeric>
#include <utility>

#include <boost/graph/copy.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include "macros.h"

#define GET_DISTANCE_MAP(graph, source, map) \
	boost::dijkstra_shortest_paths(graph, source, boost::distance_map(boost::make_iterator_property_map(map.begin(), boost::get(boost::vertex_index, graph))))

using namespace std;
using namespace boost;
using json = nlohmann::json;

problem::problem(const graph_type& _graph, const std::vector<commodity>& commodities) :
	graph(_graph), commodities(commodities),
	is_tolled_map(boost::get(edge_tolled, graph)),
	cost_map(boost::get(boost::edge_weight, graph)),
	tollfree_graph(graph, edge_tollfree_predicate<edge_tolled_map_type>(is_tolled_map))
{
	update();
}

problem::problem(const graph_type& _graph, std::vector<commodity>&& commodities) :
	graph(_graph), commodities(std::move(commodities)),
	is_tolled_map(boost::get(edge_tolled, graph)),
	cost_map(boost::get(boost::edge_weight, graph)),
	tollfree_graph(graph, edge_tollfree_predicate<edge_tolled_map_type>(is_tolled_map))
{
	update();
}

problem::problem(const problem& prob) : problem(prob.graph, prob.commodities) { }

problem::problem(problem&& prob) :
	graph(prob.graph), commodities(std::move(prob.commodities)),
	is_tolled_map(boost::get(edge_tolled, graph)),
	cost_map(boost::get(boost::edge_weight, graph)),
	tollfree_graph(graph, edge_tollfree_predicate<edge_tolled_map_type>(is_tolled_map)),
	big_m(std::move(prob.big_m)),
	big_n(std::move(prob.big_n))
{
	update_indices();
}

problem problem::parse_json(const json& json_obj) {
	// Read number of vertices
	int num_vertices = json_obj["V"].get<int>();
	graph_type graph(num_vertices);

	// Read edges
	for (const json& edge : json_obj["A"]) {
		// Data from Json
		int src = edge["src"].get<int>() - 1;
		int dst = edge["dst"].get<int>() - 1;
		cost_type cost = edge["cost"].get<cost_type>();
		bool tolled = edge["toll"].get<bool>();

		// Edge properties (cost, tolled)
		edge_property_type prop;
		get_property_value(prop, edge_weight) = cost;
		get_property_value(prop, edge_tolled) = tolled;

		// Add edge to graph
		add_edge(src, dst, prop, graph);
	}

	// Read commodities
	vector<commodity> commodities(json_obj["K"].size());
	for (const json& comm : json_obj["K"]) {
		// Data from Json
		int index = comm["index"].get<int>() - 1;
		int orig = comm["orig"].get<int>() - 1;
		int dest = comm["dest"].get<int>() - 1;
		demand_type demand = comm["demand"].get<demand_type>();

		commodities[index] = commodity{ orig, dest, demand };
	}

	// Add the problem into the list
	return problem(graph, std::move(commodities));
}

json problem::get_json(const problem& prob) {
	json json_obj;

	// Number of vertices
	json_obj["V"] = num_vertices(prob.graph);

	// Edges
	json edges_obj;

	auto cost_map = get(edge_weight, prob.graph);
	auto tolled_map = get(edge_tolled, prob.graph);
	graph_traits<problem::graph_type>::edge_iterator ei, ei_end;

	for (tie(ei, ei_end) = edges(prob.graph); ei != ei_end; ++ei) {
		// Edge data
		int src = source(*ei, prob.graph);
		int dst = target(*ei, prob.graph);
		cost_type cost = cost_map[*ei];
		bool tolled = tolled_map[*ei];

		// Edge ptree node
		json edge;
		edge["src"] = src + 1;
		edge["dst"] = dst + 1;
		edge["cost"] = cost;
		edge["toll"] = tolled;

		edges_obj.push_back(edge);
	}

	json_obj["A"] = edges_obj;

	// Commodities
	json commodities_obj;

	for (int i = 0; i < prob.commodities.size(); ++i) {
		const commodity& c = prob.commodities[i];

		json commodity_obj;
		commodity_obj["index"] = i + 1;
		commodity_obj["orig"] = c.origin + 1;
		commodity_obj["dest"] = c.destination + 1;
		commodity_obj["demand"] = c.demand;

		commodities_obj.push_back(commodity_obj);
	}

	json_obj["K"] = commodities_obj;

	// Add to root node
	return json_obj;
}

vector<problem> problem::read_from_json(std::string filename)
{
	vector<problem> all_problems;

	ifstream is(filename);
	json root;
	is >> root;

	if (root.is_array()) {
		for (const json& problem_obj : root) {
			// Add the problem into the list
			all_problems.push_back(problem::parse_json(problem_obj));
		}
	}
	else {
		all_problems.push_back(problem::parse_json(root["problem"]));
	}

	return all_problems;
}

void problem::write_to_json(std::string filename, const std::vector<problem>& problems)
{
	json root;

	if (problems.size() == 1) {
		root["problem"] = problem::get_json(problems[0]);
	}
	else {
		for (const problem& problem : problems) {
			// Add to root node
			root.push_back(problem::get_json(problem));
		}
	}

	ofstream os(filename);
	os << root;
}

json problem::get_json() const {
	return get_json(*this);
}

void problem::write_to_json(std::string filename) const
{
	write_to_json(filename, vector<problem> { *this });
}

void problem::update()
{
	update_indices();
	update_big_mn();
}

void problem::update_indices()
{
	using namespace std;
	using namespace boost;
	using edge_index_rel = edge_index_bimap_type::value_type;

	// Reset
	tolled_index_map.clear();
	tollfree_index_map.clear();
	alledges_index_map.clear();

	// Mapping
	edge_iterator ei, ei_end;
	int tolled_index = 0, tollfree_index = 0;

	for (tie(ei, ei_end) = edges(graph); ei != ei_end; ++ei) {
		bool is_tolled = is_tolled_map[*ei];
		if (is_tolled) {
			tolled_index_map.insert(edge_index_rel(tolled_index++, *ei));
		}
		else {
			tollfree_index_map.insert(edge_index_rel(tollfree_index++, *ei));
		}
	}

	// All edges mapping
	auto alledges_inserter = inserter(alledges_index_map, alledges_index_map.begin());

	copy(tolled_index_map.begin(), tolled_index_map.end(), alledges_inserter);
	transform(tollfree_index_map.begin(), tollfree_index_map.end(), alledges_inserter,
			  [tolled_index](const edge_index_rel& entry) {
				  return edge_index_rel(entry.left + tolled_index, entry.right);
			  });
}

void problem::update_big_mn()
{
	int V = num_vertices(graph);
	int K = commodities.size();
	int A1 = tolled_index_map.size();

	big_m = cost_matrix(K, cost_array(A1));
	big_n = cost_array(A1);

	cost_matrix tollfree_o(K, cost_array(V)), nulltoll_o(K, cost_array(V));
	LOOP(k, K) {
		int o = commodities[k].origin;
		GET_DISTANCE_MAP(graph, o, nulltoll_o[k]);
		GET_DISTANCE_MAP(tollfree_graph, o, tollfree_o[k]);
	}

	LOOP(a, A1) {
		auto edge = tolled_index_map.left.at(a);
		int i = source(edge, graph);
		int j = target(edge, graph);
		cost_type c = cost_map[edge];

		cost_array tollfree_i(V), nulltoll_j(V);
		GET_DISTANCE_MAP(graph, j, nulltoll_j);
		GET_DISTANCE_MAP(tollfree_graph, i, tollfree_i);

		big_n[a] = -numeric_limits<cost_type>::infinity();
		LOOP(k, K) {
			int o = commodities[k].origin;
			int d = commodities[k].destination;

			cost_type m1, m2, m3, m4;
			m1 = tollfree_i[j] - c;
			m2 = tollfree_o[k][j] - nulltoll_o[k][i] - c;
			m3 = tollfree_i[d] - c - nulltoll_j[d];
			m4 = tollfree_o[k][d] - nulltoll_o[k][i] - c - nulltoll_j[d];

			// M has 0.001f tolerance because exact M may cause infeasibility of subproblem
			big_m[k][a] = max((cost_type)0, min({ m1, m2, m3, m4 }));
			big_n[a] = max(big_n[a], big_m[k][a]);
		}
	}
}

cost_type problem::get_obj_upper_bound() const
{
	int V = num_vertices(graph);
	int K = commodities.size();

	cost_type sum_obj = 0;
	LOOP(k, K) {
		int o = commodities[k].origin;
		int d = commodities[k].destination;

		cost_array tollfree(V), nulltoll(V);
		GET_DISTANCE_MAP(graph, o, nulltoll);
		GET_DISTANCE_MAP(tollfree_graph, o, tollfree);

		sum_obj += (tollfree[d] - nulltoll[d]) * commodities[k].demand;
	}

	return sum_obj;
}


#include <fstream>
#include <string>
#include <cmath>

// BOOST GRAPH LIBRARY (BGL)
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

// BOOST GEOMETRY
#include <boost/geometry.hpp>

// BOOST POLYGON
#include <boost/polygon/polygon.hpp>

// BOOST PROGRAM OPTIONS
#include<boost/program_options.hpp>

// FLAGS TO SET BEHAVIOUR
bool is_log;
double simplify_quantity;
bool fill_inside;
double travel_speed;
std::vector<double> radii;
double buffer_distance;
double additional_buffer_distance;
const double default_buffer_distance{ 0.0002 };
int points_per_circle;
bool subtract_smaller;
std::string in_file;
std::string out_file;
std::string centres;
bool predecessor;

// OTHER CONSTANTS
const double MULTIPLIER{ 100000000.0 };
// radius of Earth in km
const double R{ 6371.0 };
// factor to change degrees to radians
const double TO_RAD{ 3.1415926536 / 180.0 };

/*
ATTRIBUTES AT EACH VERTEX
We could choose to include osmid and highway, but for the sake of
saving memory we do not include them as they are not needed in the
scope of the project. We leave them in comment.
*/
struct VertexProperty
{
    // longitude at the vertex
    double x;
    // latitude at the vertex
    double y;
    // std::string osmid;
    // std::string highway;

    // predecessor vertex id of the vertex after shortest distance calculation
    int pred;
    // distance from the closest source vertex
    double dist;
};

/*
ATTRIBUTES OF EACH EDGE
Similarly we ignore osmid, name, highway, maxspeed, oneway.
*/
struct EdgeProperty
{
    // std::string osmid;
    // std::string name;
    // std::string highway;
    // std::string maxspeed;
    // std::string oneway;

    // length of the edge in meters
    double length;
    // Linestring that describes all points an edge composed of
    std::string geometry;
    // the weight in our calculations, the time it takes to travel the edge with a given speed
    double time;
    // true if a the edge is within <radius> distance from the closest cource
    std::vector<bool> included;
};

// the graph type we use with directed edges
using Graph_t = boost::adjacency_list<boost::vecS,
                                    boost::vecS,
                                    boost::directedS,
                                    VertexProperty,
                                    EdgeProperty>;

using Vertex_t = Graph_t::vertex_descriptor;
using Edge_t = Graph_t::edge_descriptor;

namespace bg = boost::geometry;
namespace gtl = boost::polygon;
namespace po = boost::program_options;

// types we define for the Boost Geometry Library
typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
typedef bg::model::polygon<point_t, false> polygon_t;
typedef bg::model::multi_polygon<polygon_t> mpolygon_t;
typedef bg::model::multi_point<point_t> mpoint_t;
typedef bg::model::linestring<point_t> linestring_t;
typedef bg::model::multi_linestring<linestring_t> mlinestring_t;
typedef bg::ring_type<polygon_t>::type ring_t;

// types we define for the Boost Polygon Library
typedef gtl::polygon_with_holes_data<long int> Polygon_Holes;
typedef gtl::polygon_data<long int> Polygon_NoHoles;
typedef gtl::point_data<long int> Point;
typedef gtl::polygon_set_data<long int> PolygonSet;
template <typename T>
struct lookup_polygon_set_type { typedef PolygonSet type; };
typedef typename lookup_polygon_set_type<gtl::property_merge<long int, long int>>::type polygon_set_type;
typedef std::map<std::set<long int>, polygon_set_type> property_merge_result_type;

// my_visitor ensures that we stop when up to radius we covered the graph,
// and do not compute the distance and predicate to all vertices
struct my_visitor : boost::default_dijkstra_visitor
{
    using base = boost::default_dijkstra_visitor;
    struct done{};

    my_visitor(double dist) : distance(dist) {}

    void finish_vertex(Vertex_t v, Graph_t const& g)
    {
        if (g[v].dist > distance)
            throw done{};

        base::finish_vertex(v, g);
    }

  private:
    double distance;
};

// spherical distance between two vertices given their azimuth angle (longitude)
// and zenith angle (latitude)
double haversine_dist(double ph1, double th1, double ph2, double th2)
{
	double dx, dy, dz;
	ph1 -= ph2;
	ph1 *= TO_RAD, th1 *= TO_RAD, th2 *= TO_RAD;

	dz = sin(th1) - sin(th2);
	dx = cos(ph1) * cos(th1) - cos(th2);
	dy = sin(ph1) * cos(th1);
	return asin(sqrt(dx * dx + dy * dy + dz * dz) / 2) * 2 * R;
}

// the haversine formula computes the id of the closest node in the graph
int closest_node(double x, double y, Graph_t g)
{
    double min_dist = 1000000000.0;
    double dist = 0.0;
    int min_id;
    for (auto vd : boost::make_iterator_range(vertices(g))) {
        dist = haversine_dist(x, y, g[vd].x, g[vd].y);
        if (dist < min_dist){
            min_dist = dist;
            min_id = vd;
        }
    }
    return min_id;
}

// converts BGL type polygon into GTL type polygon
// the credit goes to
// https://github.com/mmccoo/nerd_mmccoo/blob/master/boost_polygon_geometry/poly_utils.cpp
void geom_poly2gtl_poly(const polygon_t& boost_poly, Polygon_Holes& gtlpoly)
{
    std::vector<Point> pts;
    for (point_t pt : boost_poly.outer()) {
        pts.push_back(gtl::construct<Point>((long int) (pt.get<0>()*MULTIPLIER),
                                            (long int) (pt.get<1>()*MULTIPLIER)));
    }
    gtl::set_points(gtlpoly, pts.begin(), pts.end());

    std::vector<Polygon_NoHoles> holes;
    for (ring_t r: boost_poly.inners()) {
        std::vector<Point> pts;
        for (point_t pt : r) {
            pts.push_back(gtl::construct<Point>((long int) (pt.get<0>()*MULTIPLIER),
                                                (long int) (pt.get<1>()*MULTIPLIER)));
        }
        Polygon_NoHoles hole;
        gtl::set_points(hole, pts.begin(), pts.end());
        holes.push_back(hole);
    }
    gtl::set_holes(gtlpoly, holes.begin(), holes.end());
}

// converts GTL type polygon into BGL type polygon
// the credit goes to
// https://github.com/mmccoo/nerd_mmccoo/blob/master/boost_polygon_geometry/poly_utils.cpp
void gtl_poly2geom_poly(const Polygon_Holes& gtlpoly, polygon_t& boost_poly)
{
    boost_poly.clear();
    for(Point pt : gtlpoly) {
        boost_poly.outer().push_back(
            point_t((double)gtl::x(pt)/MULTIPLIER,(double)gtl::y(pt)/MULTIPLIER));
    }

    int num_holes{ 0 };
    for(auto iter = gtlpoly.begin_holes(); iter != gtlpoly.end_holes(); ++iter) {
        num_holes++;
        const gtl::polygon_data<long int> h = *iter;

        ring_t r;
        for(Point pt : h) {
            r.push_back(point_t((double)gtl::x(pt)/MULTIPLIER,(double)gtl::y(pt)/MULTIPLIER));
        }
        boost_poly.inners().push_back(r);
    }

    // if gtl goes in the wrong direction. clockwise vs counter-clockwise.
    bg::correct(boost_poly);
}

// rescales Linestring
void scale_geom(mlinestring_t mls, mlinestring_t &mls_scaled, double scale_x, double scale_y)
{
    mls_scaled.clear();

    for (linestring_t ls : mls) {
        std::vector<point_t> pts;
        linestring_t ls_out;

        for (point_t pt : ls){
            pts.push_back(point_t(pt.get<0>()*scale_x, pt.get<1>()*scale_y));
        }
        bg::assign_points(ls_out, pts);
        mls_scaled.push_back(ls_out);
    }
}

// rescales Polygon
void scale_geom(polygon_t  poly, polygon_t &poly_scaled, double scale_x, double scale_y)
{
    poly_scaled.clear();

    for (point_t pt : poly.outer()) {
        poly_scaled.outer().push_back(point_t(pt.get<0>()*scale_x, pt.get<1>()*scale_y));
    }

    std::vector<Polygon_NoHoles> holes;
    for (ring_t r: poly.inners()) {
        std::vector<point_t> pts;
        for (point_t pt : r) {
            pts.push_back(point_t(pt.get<0>()*scale_x, pt.get<1>()*scale_y));
        }
        ring_t ring;
        bg::assign_points(ring, pts);
        poly_scaled.inners().push_back(ring);
    }
}

// rescales Multipolygon
void scale_geom(mpolygon_t  mpoly, mpolygon_t &mpoly_scaled, double scale_x, double scale_y)
{
    mpoly_scaled.clear();

    for (polygon_t poly : mpoly) {
        polygon_t poly_scaled;
        scale_geom(poly, poly_scaled, scale_x, scale_y);
        mpoly_scaled.push_back(poly_scaled);
    }
}

// reads graphml file into Graph_t
// Again we omit certain OSM properties to save memory. These are left in comment.
Graph_t ReadGraph(std::string fn)
{
    std::ifstream is(fn.c_str());
    if (!is.is_open()) {
        std::cout << "loading file '" << fn << "'failed." << std::endl;
        throw "Could not load file.";
    }
    Graph_t graph;
    boost::dynamic_properties dp(boost::ignore_other_properties);
    dp.property("x", boost::get(&VertexProperty::x, graph));
    dp.property("y", boost::get(&VertexProperty::y, graph));
    // dp.property("osmid", boost::get(&VertexProperty::osmid, graph));
    // dp.property("highway", boost::get(&VertexProperty::highway, graph));

    // dp.property("osmid", boost::get(&EdgeProperty::osmid, graph));
    // dp.property("name", boost::get(&EdgeProperty::name, graph));
    // dp.property("highwaye", boost::get(&EdgeProperty::highway, graph));
    // dp.property("maxspeed", boost::get(&EdgeProperty::maxspeed, graph));
    // dp.property("oneway", boost::get(&EdgeProperty::oneway, graph));
    dp.property("length", boost::get(&EdgeProperty::length, graph));
    dp.property("geometry", boost::get(&EdgeProperty::geometry, graph));

    boost::read_graphml(is, graph, dp);

    return graph;
};

// read the source points from text file which contains a WKT Linestring
linestring_t read_src_points(std::string centres)
{
    std::ifstream input(centres);
    std::string input_line;
    for(std::string line; getline(input, line); ) {
        input_line += line;
    }
    linestring_t src_points;
    bg::read_wkt(input_line, src_points);
    return src_points;
}

// writes radii and Multipolygons into files
void write_isochrones(std::string out_file, std::vector<mpolygon_t> isochrone_polys)
{
    std::ofstream poly_file(out_file);
    for (int r : radii) {
        poly_file << r << " ";
    }
    poly_file << std::endl;
    for (int i=0; i<radii.size(); i++) {
        poly_file << std::setprecision(12) << bg::wkt(isochrone_polys[i]) << std::endl;
    }
      poly_file.flush();
      poly_file.close();
}

// only changes the "included" property if the edge
// is reached within a certain radius with, this is done with Dijkstra's algorithm
void get_isochrones(int source, std::vector<double> radii, Graph_t &g) {
    double maxi = -1;
    for (auto radius : radii) {
        if (maxi < radius) maxi = radius;
    }
    my_visitor vis { maxi };

    try {
    boost::dijkstra_shortest_paths(g, source,
                          boost::visitor(vis)
                          .predecessor_map(get(&VertexProperty::pred, g))
                          .distance_map(get(&VertexProperty::dist, g))
                          .weight_map(get(&EdgeProperty::time, g)));
    } catch(my_visitor::done const&) {
    }

    for (int i=0; i<radii.size(); i++) {
        Vertex_t src, tar;
        auto es = boost::edges(g);
        for (auto eit = es.first; eit != es.second; ++eit) {
            src = boost::source(*eit, g);
            tar = boost::target(*eit, g);
            if (g[tar].dist < radii[i] && g[src].dist < radii[i]) {
                if(predecessor == true && g[tar].pred != src) {
                    continue;
                }
                g[*eit].included[i] = true;
            }
        }
    }
}

// unary union from a property_merge into mpolygon_t
void polygon_merge(gtl::property_merge<long int, long int> pm, mpolygon_t &multipolygon)
{
    property_merge_result_type merge_result;
    pm.merge(merge_result);
    PolygonSet result = merge_result.begin()->second;
    std::vector<Polygon_Holes> polys;
    result.get(polys);
    for (int k=0; k<polys.size(); k++) {
        polygon_t poly;
        gtl_poly2geom_poly(polys[k], poly);
        multipolygon.push_back(poly);
    }
}

// computes the Multilinestring within a given radius
void get_multilinestring_within(int i, Graph_t g, mlinestring_t &mls)
{
    auto es = boost::edges(g);

    // for a directed edge src is the source and tar is the target vertex
    Vertex_t src, tar;

    linestring_t ls;

    // we loop through all the edges in the graph
    for (auto eit = es.first; eit != es.second; ++eit) {
        // if the i-th smallest radius is in reach
        if (g[*eit].included[i] == true) {
            // if i > 0 and we already processed the edge in the previous run
            // of the outmost loop then we continue in the loop
            if (i > 0 && g[*eit].included[i-1] == true) {
                continue;
            }

            src = boost::source(*eit, g);
            tar = boost::target(*eit, g);
            std::string str(g[*eit].geometry);

            // if the geometry of the edge is not empty then we read
            // the geometry into Linestring
            if (str.length() > 1) {
                bg::read_wkt(str, ls);
            }
            // else we consider the endpoints of of the edge as Linestring
            else {
                std::vector<point_t> pts;
                pts.push_back(point_t(g[src].x, g[src].y));
                pts.push_back(point_t(g[tar].x, g[tar].y));
                bg::assign_points(ls, pts);
            }
            mls.push_back(ls);
        }
    }
}

// applies transformations on the geometry
void build_isochrone_geometry(std::vector<mpolygon_t> &isochrone_polys,
                            std::vector<Polygon_Holes> polys, double lat_lon_ratio)
{
    bg::strategy::buffer::distance_symmetric<double> distance_strategy(buffer_distance);
    bg::strategy::buffer::join_round join_strategy(points_per_circle);
    bg::strategy::buffer::end_round end_strategy(points_per_circle);
    bg::strategy::buffer::point_circle circle_strategy(points_per_circle);
    bg::strategy::buffer::side_straight side_strategy;

    // auxiliary variables for further computations with polygons
    polygon_t    poly, poly_aux;
    ring_t       ring;
    mpolygon_t   mpoly, mpoly_scaled, mpoly_collect;

    // we loop over radii starting with the smallest
    for (int j=0; j<polys.size(); j++) {
        gtl_poly2geom_poly(polys[j], poly);
            if (fill_inside == true && predecessor == false) {
                ring = poly.outer();
                bg::convert(ring, poly_aux);
                bg::simplify(poly_aux, poly, simplify_quantity);
                bg::strategy::buffer::distance_symmetric<double>
                    distance_strategy(additional_buffer_distance);
                bg::buffer(poly, mpoly, distance_strategy, side_strategy,
                           join_strategy, end_strategy, circle_strategy);
                scale_geom(mpoly, mpoly_scaled, 1/lat_lon_ratio, 1);
                ring.clear();
                ring = mpoly_scaled[0].outer();
                mpoly_scaled.clear();
                mpoly.clear();
                poly.clear();
                bg::convert(ring, poly);
            } else {
                bg::simplify(poly, poly_aux, simplify_quantity);
                scale_geom(poly_aux, poly, 1/lat_lon_ratio, 1);
            }
            mpoly_collect.push_back(poly);
    }
    isochrone_polys.push_back(mpoly_collect);
}

void union_of_fill_inside_true_predecessor_false(std::vector<mpolygon_t> &isochrone_polys)
{
    std::vector<mpolygon_t> isochrone_polys_union;
    gtl::property_merge<long int, long int> pm;
    for (mpolygon_t mpoly : isochrone_polys) {
        for (polygon_t poly : mpoly){
            Polygon_Holes pl;
            geom_poly2gtl_poly(poly, pl);
            pm.insert(pl, 0);
        }
        mpolygon_t multipolygon, multipolygon_outer;
        polygon_merge(pm, multipolygon);
        for (polygon_t poly: multipolygon) {
            ring_t ring = poly.outer();
            poly.clear();
            bg::convert(ring, poly);
            multipolygon_outer.push_back(poly);
        }
        isochrone_polys_union.push_back(multipolygon_outer);
    }
    isochrone_polys.clear();
    isochrone_polys.insert(isochrone_polys.end(),
                           isochrone_polys_union.begin(), isochrone_polys_union.end());
}

void subtract_smaller_polygon(std::vector<mpolygon_t> &isochrone_polys)
{
    std::vector<mpolygon_t> isochrone_polys_difference;
    for (int j=0; j<isochrone_polys.size()-1; j++) {
        if (j == 0) {
            isochrone_polys_difference.push_back(isochrone_polys[0]);
        }
        gtl::property_merge<long int, long int> pm;
        Polygon_Holes pl;
        for (polygon_t poly : isochrone_polys[j+1]) {
            geom_poly2gtl_poly(poly, pl);
            pm.insert(pl, 0);
        }
        for (polygon_t poly : isochrone_polys[j]) {
            geom_poly2gtl_poly(poly, pl);
            pm.insert(pl, 1);
        }
        mpolygon_t multipolygon;
        polygon_merge(pm, multipolygon);
        isochrone_polys_difference.push_back(multipolygon);
    }
    isochrone_polys.clear();
    isochrone_polys.insert(isochrone_polys.end(),
                           isochrone_polys_difference.begin(), isochrone_polys_difference.end());
}

// obtaining command line options
int cmd_input(int ac, char * av[],
            bool &is_log,
            double &simplify_quantity,
            bool &fill_inside,
            double &travel_speed,
            std::vector<double> &radii,
            double &buffer_distance,
            double &additional_buffer_distance,
            int &points_per_circle,
            bool &subtract_smaller,
            std::string &in_file,
            std::string &out_file,
            std::string &centres,
            bool &predecessor)
{
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("verbose", po::bool_switch(&is_log)->default_value(false),
        "prints log details")
        ("simplify", po::value<double>(&simplify_quantity),
        "the epsilon in Ramer-Douglas-Peucker algorithm, the bigger the number is, the less points shall be present in the output")
        ("fill", po::bool_switch(&fill_inside)->default_value(false),
        "if the polygon contains holes it removes them")
        ("travel-speed", po::value<double>(&travel_speed)->default_value(20.0),
        "the travel speed in km/h")
        ("radii", po::value<std::vector<double>>(&radii)
                ->multitoken()->default_value(std::vector<double>{10,20,30,40,50,60},
        "10,20,30,40,50,60"), "the radii of the isochrone curves in minutes")
        ("circle-points", po::value<int>(&points_per_circle)->default_value(1),
        "the radii of the isochrone curves in minutes")
        ("subtract", po::bool_switch(&subtract_smaller)->default_value(false),
        "subtracts the smaller radius polygon from the larger thus creating a hole in that")
        ("input-file", po::value<std::string>(&in_file)->required(),
        "the input .graphml file")
        ("output-file,o", po::value<std::string>(&out_file)->default_value("out.txt"),
        "the output text file")
        ("sources", po::value<std::string>(&centres)->default_value("sources.txt"),
        "a file containing a WKT Linestring whose vertices are the sources")
        ("predecessor", po::bool_switch(&predecessor)->default_value(false),
        "computes the isochrone curves with a shortest path tree, fill does not affect it, it is not adviced to have multiple sources in this case")
        ("buffer-distance", po::value<double>(&buffer_distance)->default_value(default_buffer_distance),
        "thickness added to the roads and polygons")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(ac, av, desc), vm);

    if (vm.count("help")) {
        std::cout << desc << "\n";
        return 1;
    }

    po::notify(vm);

    if (buffer_distance <= 0) {
        std::cout << "the buffer distance must be positive\n";
        return 1;
    }

    if (fill_inside == true and predecessor == false) {
        if (buffer_distance > default_buffer_distance*2) {
            additional_buffer_distance = buffer_distance - default_buffer_distance;
            buffer_distance = default_buffer_distance;
        }
        else {
            buffer_distance /= 2;
            additional_buffer_distance = buffer_distance;
        }
    }
    else {
        if (buffer_distance > default_buffer_distance*2) {
            std::cout << "WARNING: too large buffer distance may result in the program never complete.\n";
        }
    }
    return 0;
}

int main(int ac, char * av[])
{
    // command line arguments
    int error = cmd_input(
            ac, av,
            is_log,
            simplify_quantity,
            fill_inside,
            travel_speed,
            radii,
            buffer_distance,
            additional_buffer_distance,
            points_per_circle,
            subtract_smaller,
            in_file,
            out_file,
            centres,
            predecessor
    );

    if (error == 1){
        return 1;
    }

    if (is_log == true) {
        std::cout << "File \"" << in_file << "\" starts being read.\n";
    }

    Graph_t g = ReadGraph(in_file);

    if (is_log == true) {
        std::cout << "File \"" << in_file << "\" is read.\n";
    }

    double meters_per_minute{ travel_speed * 1000.0 / 60.0 };
    auto es = boost::edges(g);

    // we initialise "included" as a vector of falses
    for (auto eit = es.first; eit != es.second; ++eit) {
        g[*eit].time =  (g[*eit].length / meters_per_minute);
        for (int i=0; i<radii.size(); i++) {
            g[*eit].included.push_back(false);
        }
    }

    // 40075==circumference of Earth in km;
    // 111.32==1 degree of latitude in km
    // lat_lon_ratio expresses the ratio of one meter of latitude and longitude
    // at given latitude
    double lat_lon_ratio{ 40075.0 * cos(g[0].y * TO_RAD) / 360.0 / 111.32 };

    // loads the source points
    linestring_t src_points = read_src_points(centres);

    int idx = 0;
    auto size = src_points.size();
    for (point_t pt : src_points) {
        idx++;
        int source = closest_node(pt.get<0>(), pt.get<1>(), g);
        if (is_log == true) {
            std::cout << "#" <<  idx << "/" << size << ", " << "x="
                      << pt.get<0>() << ", y=" << pt.get<1>() << "\n";
        }
        get_isochrones(source, radii, g);
    }
    if (is_log == true) {
        std::cout << "Dijkstra is done.\n";
        std::cout << "Geometry is being assembled.\n";
    }

    //buffer strategy, for reference visit the website of BGL
    bg::strategy::buffer::distance_symmetric<double> distance_strategy(buffer_distance);
    bg::strategy::buffer::join_round join_strategy(points_per_circle);
    bg::strategy::buffer::end_round end_strategy(points_per_circle);
    bg::strategy::buffer::point_circle circle_strategy(points_per_circle);
    bg::strategy::buffer::side_straight side_strategy;

    // we collect the computed isochronic polygons in "isochrone_polys"
    std::vector<mpolygon_t> isochrone_polys;

    // the result of GTL property merge (union in our case) is converted into
    // std::vector<Polygon_Holes> which is stored in "polys,"
    // futhermore the previous (smaller) isochronic curve is inserted into the
    // next one thus speeding up computation, which is done with "polys"
    std::vector<Polygon_Holes> polys;

    // we loop over radii starting with the smallest
    for (int i=0; i<radii.size(); i++) {
        // auxiliary variables for polygon manipulations
        mlinestring_t mls, mls_scaled;
        mpolygon_t   mpoly_aux;
        Polygon_Holes pl;

        get_multilinestring_within(i, g, mls);

        // at this point if collected in a vector, the isochronic Multilinestrings
        // could be saved

        // we collect the polygons to be unioned in "pm"
        gtl::property_merge<long int, long int> pm;

        // we insert the previous isochronic curve here thus gaining speed
        for (Polygon_Holes polygon : polys) {
            pm.insert(polygon, 0);
        }
        // polys emptied for further computations after insertion
        polys.clear();

        // in buffer we scale the geometries thus buffer affects x and y directions equally
        scale_geom(mls, mls_scaled, lat_lon_ratio, 1);

        // memory saving for the rest of the loop
        mls.clear();

        // buffer applied to the scaled Multilinestring
        for (linestring_t ls : mls_scaled) {
            bg::buffer(ls, mpoly_aux, distance_strategy, side_strategy,
                       join_strategy, end_strategy, circle_strategy);
            geom_poly2gtl_poly(mpoly_aux.at(0), pl);
            pm.insert(pl, 0);
        }

        // "merge_result" holds the result for the unary union
        property_merge_result_type merge_result;
        pm.merge(merge_result);
        PolygonSet result = merge_result.begin()->second;
        result.get(polys);

        build_isochrone_geometry(isochrone_polys, polys, lat_lon_ratio);

    }

    if (fill_inside == true && predecessor == false) {
        union_of_fill_inside_true_predecessor_false(isochrone_polys);
    }

    if (subtract_smaller == true) {
        subtract_smaller_polygon(isochrone_polys);
    }

    // writing result into file
    write_isochrones(out_file, isochrone_polys);

      if (is_log == true) {
          std::cout << "Polygons are written into \"" << out_file << "\"\n";
      }
      return 0;
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_REM_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_REM_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/constant/nan.hpp>
#include <boost/simd/function/div.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_finite.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return a1 ? a0%a1 : a0;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::pedantic_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (pedantic_tag const &
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if (is_nez(a1)&&is_eqz(a0)) return a0;
      return fnms(div(fix, a0, a1), a1, a0);
    }
  };

   BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename T)
                           , bd::cpu_
                           , bs::pedantic_tag
                           , bs::tag::fix_
                          , bd::scalar_<bd::arithmetic_<T>>
                          , bd::scalar_<bd::arithmetic_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::fix_> const&
                                  , T const& a, T const& b ) const BOOST_NOEXCEPT
    {
      return pedantic_(rem)(a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return fnms(a1, div(fix, a0, a1), a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bs::tag::fix_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, bd::functor<bs::tag::fix_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return std::fmod(a0, a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::fix_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (bd::functor<bs::tag::fix_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return fnms(a1, div(fix, a0, a1), a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &,  A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
     return std::fmod(a0, a1);
    }
  };
} } }

// Other flavors of rem
#include <boost/simd/arch/common/scalar/function/remround.hpp>
#include <boost/simd/arch/common/scalar/function/remnearbyint.hpp>
#include <boost/simd/arch/common/scalar/function/remfloor.hpp>
#include <boost/simd/arch/common/scalar/function/remceil.hpp>

#endif


#ifndef plane_calibration_SRC_CALIBRATION_PARAMETERS_HPP_
#define plane_calibration_SRC_CALIBRATION_PARAMETERS_HPP_

#include <mutex>
#include <memory>
#include <Eigen/Dense>

namespace plane_calibration
{

class CalibrationParameters
{
public:
  class Parameters
  {
  public:
    Parameters()
    {
      ground_plane_offset_ = Eigen::Vector3d(0.0, 0.0, 0.0);
      rotation_ = Eigen::AngleAxisd::Identity();
      max_deviation_ = 0.0;
      deviation_ = 0.0;
      precompute_planes_ = true;
      precomputed_plane_pairs_count_ = 0;
    }

    Parameters(const double& max_deviation, const Eigen::Vector3d& ground_plane_offset,
               const Eigen::AngleAxisd& rotation, bool precompute_planes, int precomputed_plane_pairs_count)
    {
      ground_plane_offset_ = ground_plane_offset;
      rotation_ = rotation;
      max_deviation_ = max_deviation;
      deviation_ = max_deviation_;
      precompute_planes_ = precompute_planes;
      precomputed_plane_pairs_count_ = precomputed_plane_pairs_count;
    }

    Eigen::Affine3d getTransform() const
    {
      return Eigen::Translation3d(ground_plane_offset_) * rotation_;
    }

    Eigen::Vector3d ground_plane_offset_;
    Eigen::AngleAxisd rotation_;

    double max_deviation_;
    double deviation_;

    bool precompute_planes_;
    int precomputed_plane_pairs_count_;
  };

  CalibrationParameters(bool precompute_planes, int precomputed_plane_pairs_count);
  CalibrationParameters(const double& max_deviation, const Eigen::Vector3d& ground_plane_offset,
                        const Eigen::AngleAxisd& rotation, bool precompute_planes, int precomputed_plane_pairs_count);
  CalibrationParameters(const CalibrationParameters &object);

  bool getUpdatedParameters(Parameters& updated_parameters);
  Parameters getParameters();
  bool parametersUpdated();
  Eigen::Affine3d getTransform() const;

  void update(const Eigen::Vector3d& ground_plane_offset, const double& max_deviation,
              const Eigen::AngleAxisd& rotation);
  void update(const Eigen::Vector3d& ground_plane_offset, const Eigen::AngleAxisd& rotation);
  void update(const Parameters& parameters);

  void update(const Eigen::Vector3d& ground_plane_offset);
  void update(const Eigen::AngleAxisd& rotation);
  void update(const double& deviation);
  void updateDeviations(const double& value);
  void updatePrecomputation(const bool& enable, const int& plane_pair_count);

  void updateDeviation(const double& deviation);

protected:
  mutable std::mutex mutex_;
  bool updated_;
  Parameters parameters_;
};
typedef std::shared_ptr<CalibrationParameters> CalibrationParametersPtr;

} /* end namespace */

#endif