#ifndef BOOST_ADAPTBX_GRAPH_MAXIMUM_CLIQUE_GREEDY_H
#define BOOST_ADAPTBX_GRAPH_MAXIMUM_CLIQUE_GREEDY_H

#include <boost_adaptbx/graph/vertex_map.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/tuple/tuple.hpp>

#include <vector>
#include <set>
#include <queue>
#include <algorithm>
#include <iterator>
#include <utility>

namespace boost_adaptbx
{
namespace graph
{
namespace greedy
{

class maximum_clique_exception
{};

class bad_vertex_exception : public maximum_clique_exception
{};

template< typename AdjacencyGraph >
class partition
{
public:
  typedef AdjacencyGraph graph_type;
  typedef boost::graph_traits< AdjacencyGraph > graph_traits;
  typedef typename graph_traits::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits::adjacency_iterator adjacency_iterator;

  typedef std::set< vertex_descriptor > vertex_set_type;
  typedef typename vertex_set_type::iterator vertex_set_iterator;
  typedef typename vertex_set_type::const_iterator vertex_set_const_iterator;

private:
  vertex_set_type clique_;
  vertex_set_type neighbours_;

public:
  partition(graph_type const& graph)
    : neighbours_( boost::vertices( graph ).first, boost::vertices( graph ).second )
  {}

  vertex_set_type const& clique() const
  {
    return clique_;
  }

  vertex_set_type const& neighbours() const
  {
    return neighbours_;
  }

  void add(graph_type const& graph, vertex_descriptor vertex)
  {
    if ( neighbours_.find( vertex ) == neighbours_.end() )
    {
      throw bad_vertex_exception();
    }

    clique_.insert( vertex );

    adjacency_iterator v, vend;
    boost::tie( v, vend ) = boost::adjacent_vertices( vertex, graph );
    vertex_set_type adjacents( v, vend );

    vertex_set_iterator it1 = neighbours_.begin();
    vertex_set_iterator it2 = adjacents.begin();

    while ( ( it1 != neighbours_.end() ) && ( it2 != adjacents.end() ) )
    {
      if ( *it1 < *it2 )
      {
        neighbours_.erase( it1++ );
      }
      else if ( *it2 < *it1 )
      {
        ++it2;
      }
      else
      {
        ++it1;
        ++it2;
      }
    }

    neighbours_.erase( it1, neighbours_.end() );
  }
};

template< typename Partition >
struct degree_scorer
{
  typedef Partition partition_type;
  typedef typename partition_type::vertex_set_type vertex_set_type;
  typedef typename vertex_set_type::size_type set_size_type;
  typedef set_size_type result_type;

public:
  degree_scorer()
  {}

  result_type operator ()(partition_type const& partition) const
  {
    return result_type( partition.neighbours().size() );
  }
};

template< typename AdjacencyGraph, typename Partition >
struct exdegree_scorer
{
  typedef AdjacencyGraph graph_type;
  typedef boost::graph_traits< AdjacencyGraph > graph_traits;
  typedef typename graph_traits::adjacency_iterator adjacency_iterator;
  typedef typename graph_traits::vertices_size_type vertices_size_type;
  typedef typename graph_traits::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits::vertex_iterator vertex_iterator;

  typedef Partition partition_type;
  typedef typename partition_type::vertex_set_type vertex_set_type;
  typedef typename vertex_set_type::size_type set_size_type;
  typedef typename vertex_set_type::const_iterator vertices_set_const_iterator;
  typedef std::pair< set_size_type, vertices_size_type > result_type;

private:
  graph_type const& graph_;

public:
  exdegree_scorer(graph_type const& graph)
    : graph_( graph )
  {}

  result_type operator ()(partition_type const& partition) const
  {
    vertices_size_type vertices_count( 0 );
    adjacency_iterator av, avend;
    vertex_set_type const& neighbours = partition.neighbours();

    for (
      vertices_set_const_iterator it = neighbours.begin();
       it != neighbours.end();
       ++it
       )
    {
      boost::tie( av, avend ) = boost::adjacent_vertices( *it, graph_ );
      vertices_count += std::distance( av, avend );
    }

    return result_type( neighbours.size(), vertices_count );
  }
};

template< typename AdjacencyGraph, typename Partition >
struct exdegree_scorer_cached
{
  typedef AdjacencyGraph graph_type;
  typedef boost::graph_traits< AdjacencyGraph > graph_traits;
  typedef typename graph_traits::adjacency_iterator adjacency_iterator;
  typedef typename graph_traits::vertices_size_type vertices_size_type;
  typedef typename graph_traits::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits::vertex_iterator vertex_iterator;

  typedef vertex_map::generic_vertex_map< graph_type, vertices_size_type >
    vertex_exdegree_mapping_type;
  typedef typename vertex_exdegree_mapping_type::property_map_type
    vertex_exdegree_property_map_type;

  typedef Partition partition_type;
  typedef typename partition_type::vertex_set_type vertex_set_type;
  typedef typename vertex_set_type::size_type set_size_type;
  typedef typename vertex_set_type::const_iterator vertices_set_const_iterator;
  typedef std::pair< set_size_type, vertices_size_type > result_type;

private:
  vertex_exdegree_mapping_type exdegree_for_;

public:
  exdegree_scorer_cached(graph_type const& graph) : exdegree_for_( graph )
  {
    vertex_iterator v, vend;
    adjacency_iterator av, avend;
    vertex_exdegree_property_map_type ve_propmap( exdegree_for_.get() );

    for ( boost::tie( v, vend ) = boost::vertices( graph ); v != vend; ++v )
    {
      boost::tie( av, avend ) = boost::adjacent_vertices( *v, graph );
      boost::put( ve_propmap, *v, std::distance( av, avend ) );
    }
  }

  result_type operator ()(partition_type const& partition) const
  {
    vertices_size_type vertices_count( 0 );
    vertex_set_type const& neighbours = partition.neighbours();
    vertex_exdegree_property_map_type ve_propmap( exdegree_for_.get() );

    for (
      vertices_set_const_iterator it = neighbours.begin();
      it != neighbours.end();
      ++it
      )
    {
      vertices_count += boost::get( ve_propmap, *it );
    }

    result_type( neighbours.size(), vertices_count );
  }
};

template< typename Pair >
struct greater_on_second_type
{
  typedef typename Pair::second_type score_type;

  bool operator ()(Pair const& lhs, Pair const& rhs) const
  {
    return lhs.second > rhs.second;
  }
};

template< typename AdjacencyGraph, typename Scorer >
std::vector< partition< AdjacencyGraph > >
maximum_clique(
  AdjacencyGraph const& graph,
  Scorer const& scorer = Scorer(),
  std::size_t maxsol = 0
  )
{
  typedef boost::graph_traits< AdjacencyGraph > graph_traits;
  typedef typename graph_traits::vertex_iterator vertex_iterator;

  typedef partition< AdjacencyGraph > partition_type;
  typedef typename partition_type::vertex_set_type vertex_set_type;
  typedef typename partition_type::vertex_set_const_iterator vertex_set_const_iterator;

  typedef std::vector< partition_type > partition_list;
  typedef typename partition_list::const_iterator partition_const_iterator;

  typedef Scorer scorer_type;
  typedef typename scorer_type::result_type score_type;
  typedef std::pair< partition_type, score_type > partition_score_data;
  typedef greater_on_second_type< partition_score_data > ranker_type;
  typedef std::priority_queue<
    partition_score_data,
    std::vector< partition_score_data >,
    ranker_type >
    partition_data_priority_queue;

  typedef std::set< vertex_set_type > clique_set_type;

  if ( maxsol == 0 )
  {
    vertex_iterator v, vend;
    boost::tie( v, vend ) = boost::vertices( graph );
    maxsol = std::distance( v, vend );
  }
  assert ( 0 < maxsol );

  partition_list solutions;
  solutions.push_back( partition_type( graph ) );
  partition_data_priority_queue solution_queue;
  clique_set_type known_cliques;

  while ( true )
  {
    assert ( solution_queue.empty() );

    for (
      partition_const_iterator pit = solutions.begin();
      pit != solutions.end();
      ++pit
      )
    {
      vertex_set_type const& neighbours = pit->neighbours();

      for (
        vertex_set_const_iterator nit = neighbours.begin();
        nit != neighbours.end();
        ++nit
        )
      {
        partition_type nextpart( *pit );
        nextpart.add( graph, *nit );
        score_type nextscore = scorer( nextpart );

        if ( solution_queue.size() < maxsol )
        {
          if ( known_cliques.insert( nextpart.clique() ).second )
          {
            solution_queue.push( partition_score_data( nextpart, nextscore ) );
          }
        }

        else if ( solution_queue.top().second < nextscore )
        {
          if ( known_cliques.insert( nextpart.clique() ).second )
          {
            known_cliques.erase( solution_queue.top().first.clique() );
            solution_queue.pop();
            solution_queue.push( partition_score_data( nextpart, nextscore ) );
          }
        }
      }
    }

    assert ( solution_queue.size() <= maxsol );

    if ( solution_queue.size() == 0 )
    {
      break;
    }

    solutions.clear();

    while ( !solution_queue.empty() )
    {
      solutions.push_back( solution_queue.top().first );
      solution_queue.pop();
    }

    known_cliques.clear();
  }

  return solutions;
}

} // namespace greedy
} // namespace graph
} // namespace boost_adaptbx

#endif // BOOST_ADAPTBX_GRAPH_MAXIMUM_CLIQUE_GREEDY_H


#include "modprop/compo/AdditiveWrapper.hpp"
#include "modprop/neural/LinearLayer.hpp"
#include "modprop/neural/FullyConnectedNet.hpp"

#include "modprop/neural/HingeActivation.hpp"
#include "modprop/neural/SigmoidActivation.hpp"
#include "modprop/neural/NullActivation.hpp"
#include "modprop/neural/NetworkTypes.h"

#include "modprop/optim/SquaredLoss.hpp"
#include "modprop/optim/StochasticMeanCost.hpp"
#include "modprop/optim/ParameterL2Cost.hpp"

#include "optim/Optimizers.h"

#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/mersenne_twister.hpp>

#include "modprop/utils/Derivatives.hpp"
#include "modprop/utils/Randomization.hpp"

#include <deque>
#include <cstdlib>
#include <iostream>

using namespace percepto;

// Comment the above and uncomment below to use Rectified Linear Units instead
typedef PerceptronNet TestNet;
// typedef ReLUNet TestNet;

unsigned int inputDim = 1;
unsigned int outputDim = 1;
unsigned int numHiddenLayers = 3;
unsigned int layerWidth = 20;
unsigned int batchSize = 20;

struct Regressor
{
	TerminalSource<VectorType> netInput;
	TestNet net;
	SquaredLoss<VectorType> loss;

	Regressor()
	: net( inputDim, outputDim, numHiddenLayers, layerWidth,
	       SigmoidActivation(), TestNet::OUTPUT_UNRECTIFIED )
	{
		net.SetSource( &netInput );
		loss.SetSource( &net.GetOutputSource() );
	}

	Regressor( const Regressor& other )
	: net( other.net )
	{
		net.SetSource( &netInput );
		loss.SetSource( &net.GetOutputSource() );
	}

	void Foreprop()
	{
		netInput.Foreprop();
	}

	double GetOutput()
	{
		return net.GetOutput()(0);
	}

	void Invalidate()
	{
		netInput.Invalidate();
	}
};

struct RegressionProblem
: public NaturalOptimizationProblem
{
	std::deque<Regressor> regressors;
	StochasticMeanCost<double> losses;
	ParameterL2Cost regularizer;
	AdditiveWrapper<double> objective;

	Parameters::Ptr params;

	RegressionProblem( Parameters::Ptr p,
	                   unsigned int batchSize,
	                   double l2Weight )
	{
		params = p;
		losses.SetBatchSize( batchSize );
		regularizer.SetWeight( l2Weight );
		regularizer.SetParameters( params );
		objective.SetSourceA( &losses );
		objective.SetSourceB( &regularizer );
	}

	virtual bool IsMinimization() const { return true; }

	virtual void Resample()
	{
		losses.Resample();
	}

	virtual double ComputeObjective()
	{
		Invalidate();
		Foreprop();
		return objective.GetOutput();
	}

	virtual VectorType ComputeGradient()
	{
		Invalidate();
		Foreprop();
		Backprop();
		return params->GetDerivs();
	}

	virtual VectorType ComputeNaturalGradient()
	{
		Invalidate();
		Foreprop();
		BackpropNatural();
		return params->GetDerivs();
	}

	virtual VectorType GetParameters() const
	{
		return params->GetParamsVec();
	}

	virtual void SetParameters( const VectorType& p )
	{
		params->SetParamsVec( p );
	}

	void Invalidate()
	{
		for( unsigned int i = 0; i < regressors.size(); i++ )
		{
			regressors[i].Invalidate();
		}
		regularizer.Invalidate();
		params->ResetAccumulators();
	}

	void Foreprop()
	{
		const std::vector<unsigned int>& inds = losses.GetActiveInds();
		BOOST_FOREACH( unsigned int i, inds )
		{
			regressors[i].Foreprop();
		}
		regularizer.Foreprop();
	}

	void ForepropAll()
	{
		for( unsigned int i = 0; i < regressors.size(); i++ )
		{
			regressors[i].Foreprop();
		}
		regularizer.Foreprop();
	}

	void Backprop()
	{
		objective.Backprop( MatrixType::Identity(1,1) );
	}

	void BackpropNatural()
	{
		const std::vector<unsigned int>& inds = losses.GetActiveInds();
		MatrixType dodw = MatrixType::Identity( 1, 1 ) / inds.size();
		BOOST_FOREACH( unsigned int i, inds )
		{
			regressors[i].loss.Backprop( dodw );
		}
	}
};

// The highly nonlinear function we will try to fit
double f( double x )
{
	return 8 * std::cos( x ) + 2.5 * x * sin( x ) + 2.8 * x;
}

// Generate a specified number of random xs and corresponding ys
void generate_data( std::vector<VectorType>& xs,
                    std::vector<VectorType>& ys,
                    unsigned int num_data )
{
	boost::random::mt19937 generator;
	boost::random::random_device rng;
	generator.seed( rng );
	boost::random::uniform_real_distribution<> xDist( -5.0, 5.0 );

	xs.clear();
	ys.clear();
	xs.reserve( num_data );
	ys.reserve( num_data );

	VectorType x(1,1);
	VectorType y(1,1);
	for( unsigned int i = 0; i < num_data; i++ )
	{
		x(0) = xDist( generator );
		y(0) = f( x(0) );
		xs.push_back( x );
		ys.push_back( y );
	}
}

int main( int argc, char** argv )
{
	unsigned int numTrain = 150;
	unsigned int numTest = 200;
	double l2Weight = 1E-3;

	std::vector<VectorType> xTest, yTest, xTrain, yTrain;
	generate_data( xTest, yTest, numTest );
	generate_data( xTrain, yTrain, numTrain );

	std::cout << "Initializing net..." << std::endl;
	std::cout << "Creating linear layers..." << std::endl;

	// // ReLU initialization
	Regressor reg;
	Parameters::Ptr params = reg.net.CreateParameters();

	// Randomize parameters
	VectorType p( params->ParamDim() );
	randomize_vector( p, -0.2, 0.2 );
	params->SetParamsVec( p );
	std::cout << "Initial net: " << std::endl << reg.net << std::endl;

	// Create the loss functions
	std::cout << "Generating losses..." << std::endl;
	RegressionProblem trainProblem( params, batchSize, l2Weight );
	RegressionProblem testProblem( params, batchSize, l2Weight );

	// NOTE If we don't reserve, the vector resizing and moving may
	// invalidate the references. Alternatively we can use a deque
	for( unsigned int i = 0; i < numTrain; i++ )
	{
		trainProblem.regressors.emplace_back( reg );
		trainProblem.regressors[i].netInput.SetOutput( xTrain[i] );
		trainProblem.regressors[i].loss.SetTarget( yTrain[i] );
		trainProblem.losses.AddSource( &trainProblem.regressors[i].loss );
	}

	for( unsigned int i = 0; i < numTest; i++ )
	{
		testProblem.regressors.emplace_back( reg );
		testProblem.regressors[i].netInput.SetOutput( xTest[i] );
		testProblem.regressors[i].loss.SetTarget( yTest[i] );
		testProblem.losses.AddSource( &testProblem.regressors[i].loss );
	}

	ModularOptimizer optimizer;

	// AdamSearchDirector::Ptr director = std::make_shared<AdamSearchDirector>();
	// GradientSearchDirector::Ptr director = std::make_shared<GradientSearchDirector>();
	NaturalSearchDirector::Ptr director = std::make_shared<NaturalSearchDirector>();
	optimizer.SetSearchDirector( director );

	BacktrackingSearchStepper::Ptr stepper = std::make_shared<BacktrackingSearchStepper>();
	stepper->SetInitialStep( 1E-1 );
	stepper->SetBacktrackingRatio( 0.5 );
	stepper->SetMaxBacktracks( 20 );
	stepper->SetImprovementRatio( 0.75 );

	// L1ConstrainedSearchStepper::Ptr stepper = std::make_shared<L1ConstrainedSearchStepper>();
	// stepper->SetMaxL1Norm( 1E-1 );
	// stepper->SetStepSize( 1E-2 );
	optimizer.SetSearchStepper( stepper );

	RuntimeTerminationChecker::Ptr runtimeChecker = std::make_shared<RuntimeTerminationChecker>();
	runtimeChecker->SetMaxRuntime( 20 );
	optimizer.AddTerminationChecker( runtimeChecker );

	// GradientTerminationChecker::Ptr gradientChecker = std::make_shared<GradientTerminationChecker>();
	// gradientChecker->SetMinGradientNorm( 1E-9 );
	// optimizer.AddTerminationChecker( gradientChecker );

	// IterationTerminationChecker::Ptr iterationChecker = std::make_shared<IterationTerminationChecker>();
	// iterationChecker->SetMaxIterations( 1e3 );
	// optimizer.AddTerminationChecker( iterationChecker );

	trainProblem.Invalidate();
	testProblem.Invalidate();
	trainProblem.ForepropAll();
	testProblem.ForepropAll();
	trainProblem.losses.ParentCost::Foreprop();
	testProblem.losses.ParentCost::Foreprop();
	std::cout << "initial train avg loss: " << trainProblem.losses.ParentCost::GetOutput() << std::endl;
	std::cout << "initial train max loss: " << trainProblem.losses.ParentCost::ComputeMax() << std::endl;
	std::cout << "initial test avg loss: " << testProblem.losses.ParentCost::GetOutput() << std::endl;
	std::cout << "initial test max loss: " << testProblem.losses.ParentCost::ComputeMax() << std::endl;

	std::cout << "Beginning optimization..." << std::endl;
	optimizer.ResetAll();
	trainProblem.Resample();
	OptimizationResults result = optimizer.Optimize( trainProblem );
	std::cout << "Terminated with condition: " << result.status << std::endl;
	std::cout << "Final net: " << std::endl << reg.net << std::endl;

	trainProblem.Invalidate();
	testProblem.Invalidate();
	trainProblem.ForepropAll();
	testProblem.ForepropAll();
	trainProblem.losses.ParentCost::Foreprop();
	testProblem.losses.ParentCost::Foreprop();
	std::cout << "train avg loss: " << trainProblem.losses.ParentCost::GetOutput() << std::endl;
	std::cout << "train max loss: " << trainProblem.losses.ParentCost::ComputeMax() << std::endl;
	std::cout << "test avg loss: " << testProblem.losses.ParentCost::GetOutput() << std::endl;
	std::cout << "test max loss: " << testProblem.losses.ParentCost::ComputeMax() << std::endl;

	for( unsigned int i = 0; i < numTest; i++ )
	{
		std::cout << "xtest: " << xTest[i] << " ytest: " << yTest[i]
		          << " regout: " << testProblem.regressors[i].GetOutput() << std::endl;
	}

	return 0;
}


#include "src/utils/distributions.h"

#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <stan/math/prim.hpp>
#include <vector>

#include "src/utils/rng.h"

TEST(mix_dist, 1) {
  auto& rng = bayesmix::Rng::Instance().get();

  int nclus = 5;
  Eigen::VectorXd weights1 =
      stan::math::dirichlet_rng(Eigen::VectorXd::Ones(nclus), rng);
  Eigen::VectorXd means1(nclus);
  Eigen::VectorXd sds1(nclus);

  for (int i = 0; i < nclus; i++) {
    means1(i) = stan::math::normal_rng(0, 2, rng);
    sds1(i) = stan::math::uniform_rng(0.1, 2.0, rng);
  }

  int nclus2 = 10;
  Eigen::VectorXd weights2 =
      stan::math::dirichlet_rng(Eigen::VectorXd::Ones(nclus2), rng);
  Eigen::VectorXd means2(nclus2);
  Eigen::VectorXd sds2(nclus2);

  for (int i = 0; i < nclus2; i++) {
    means2(i) = stan::math::normal_rng(0, 2, rng);
    sds2(i) = stan::math::uniform_rng(0.1, 2.0, rng);
  }

  double dist = bayesmix::gaussian_mixture_dist(means1, sds1, weights1, means2,
                                                sds2, weights2);

  ASSERT_GE(dist, 0.0);
}

TEST(mix_dist, 2) {
  int nclus = 5;
  auto& rng = bayesmix::Rng::Instance().get();

  Eigen::VectorXd weights1 =
      stan::math::dirichlet_rng(Eigen::VectorXd::Ones(nclus), rng);
  Eigen::VectorXd means1(nclus);
  Eigen::VectorXd sds1(nclus);

  for (int i = 0; i < nclus; i++) {
    means1(i) = stan::math::normal_rng(0, 2, rng);
    sds1(i) = stan::math::uniform_rng(0.1, 2.0, rng);
  }

  double dist_to_self = bayesmix::gaussian_mixture_dist(
      means1, sds1, weights1, means1, sds1, weights1);

  ASSERT_DOUBLE_EQ(dist_to_self, 0.0);
}


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Mathematics/Interpolators/lagrangeInterpolator.h"
#include "Tudat/Astrodynamics/Ephemerides/tabulatedRotationalEphemeris.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

using namespace ephemerides;

BOOST_AUTO_TEST_SUITE( test_tabulated_rotational_ephemeris )

//! Test to see if rotational ephemeris provides correct output, by comparing output from class functions to direct Spice calls.
BOOST_AUTO_TEST_CASE( testTabulatedRotationalEphemeris )
{
    // Define frames between which rotation will be computed
    const std::string baseFrame = "J2000";
    const std::string targetFrame = "IAU_Mars";

    // Load Spice kernel
    spice_interface::loadStandardSpiceKernels( );

    // Define time interval over which rotations are to be generated
    double startTime = 0.0;
    double finalTime = 2.0 * physical_constants::JULIAN_DAY;
    double timeStep = 10.0;

    // Create a list of concatenated quaternions and angular velocity vectors for rotation.
    std::map< double, Eigen::Matrix< double, 7, 1 > > rotationMap;
    double currentTime = startTime;
    while( currentTime < finalTime )
    {
        Eigen::Matrix< double, 7, 1 > currentState;
        Eigen::Quaterniond currentRotationToBaseFrame = spice_interface::computeRotationQuaternionBetweenFrames(
                    targetFrame, baseFrame, currentTime );
        currentState.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat(
                    currentRotationToBaseFrame );
        currentState.segment( 4, 3 ) = currentRotationToBaseFrame.inverse( ) *
                spice_interface::getAngularVelocityVectorOfFrameInOriginalFrame(
                    baseFrame, targetFrame, currentTime );
        rotationMap[ currentTime ] = currentState;
        currentTime += timeStep;
    }

    // Create interpolator for rotation
    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< double, 7, 1 > > > rotationInterpolator =
            std::make_shared< interpolators::LagrangeInterpolator< double, Eigen::Matrix< double, 7, 1 > > >( rotationMap, 6 );

    // Create tabulated rotational model
    std::shared_ptr< TabulatedRotationalEphemeris< double, double > > tabulatedEphemeris =
            std::make_shared< TabulatedRotationalEphemeris<  double, double > >( rotationInterpolator );

    //  Declare variables for rotation properties retrieved from class/spice
    Eigen::Matrix3d currentRotationMatrixToTargetFrame, currentRotationMatrixToBaseFrame;
    Eigen::Matrix3d expectedRotationMatrixToTargetFrame, expectedRotationMatrixToBaseFrame;

    Eigen::Matrix3d currentRotationMatrixDerivativeToTargetFrame, currentRotationMatrixDerivativeToBaseFrame;
    Eigen::Matrix3d expectedRotationMatrixDerivativeToTargetFrame, expectedRotationMatrixDerivativeToBaseFrame;

    Eigen::Vector3d bodyAngularVelocityVectorInBaseFrame, bodyAngularVelocityVectorInTargetFrame;
    Eigen::Vector3d expectedBodyAngularVelocityVectorInBaseFrame, expectedBodyAngularVelocityVectorInTargetFrame;

    // Set initial time slightly away from start of interpolator
    currentTime = startTime + 3600.0;

    // Set interpolation time step such that interpolation is not on the nodes
    double interpolationTimeStep = 100.0 * mathematical_constants::PI;

    // Compare rotational properties from interpolator/Spice
    while( currentTime < finalTime - 3600.0 )
    {

        // Compare rotation matrices
        currentRotationMatrixToTargetFrame = tabulatedEphemeris->getRotationToTargetFrame( currentTime );
        expectedRotationMatrixToTargetFrame = spice_interface::computeRotationQuaternionBetweenFrames(
                    baseFrame, targetFrame, currentTime );

        currentRotationMatrixToBaseFrame = tabulatedEphemeris->getRotationToBaseFrame( currentTime );
        expectedRotationMatrixToBaseFrame = spice_interface::computeRotationQuaternionBetweenFrames(
                    targetFrame, baseFrame, currentTime );

        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL(
                            std::fabs( currentRotationMatrixToTargetFrame( i, j ) -
                                       expectedRotationMatrixToTargetFrame( i, j ) ), 1.0E-10 );
                BOOST_CHECK_SMALL(
                            std::fabs( currentRotationMatrixToBaseFrame( i, j ) -
                                       expectedRotationMatrixToBaseFrame( i, j ) ), 1.0E-10 );
            }
        }

        // Compare derivatives of rotation matrices
        currentRotationMatrixDerivativeToTargetFrame = tabulatedEphemeris->getDerivativeOfRotationToTargetFrame( currentTime );
        expectedRotationMatrixDerivativeToTargetFrame = spice_interface::computeRotationMatrixDerivativeBetweenFrames(
                    baseFrame, targetFrame, currentTime );

        currentRotationMatrixDerivativeToBaseFrame = tabulatedEphemeris->getDerivativeOfRotationToBaseFrame( currentTime );
        expectedRotationMatrixDerivativeToBaseFrame = spice_interface::computeRotationMatrixDerivativeBetweenFrames(
                    targetFrame, baseFrame, currentTime );

        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL(
                            std::fabs( currentRotationMatrixDerivativeToTargetFrame( i, j ) -
                                       expectedRotationMatrixDerivativeToTargetFrame( i, j ) ), 1.0E-15 );
                BOOST_CHECK_SMALL(
                            std::fabs( currentRotationMatrixDerivativeToBaseFrame( i, j ) -
                                       expectedRotationMatrixDerivativeToBaseFrame( i, j ) ), 1.0E-15 );
            }
        }

        // Compare angular velocity vectors
        bodyAngularVelocityVectorInBaseFrame = tabulatedEphemeris->getRotationalVelocityVectorInBaseFrame( currentTime );
        bodyAngularVelocityVectorInTargetFrame = tabulatedEphemeris->getRotationalVelocityVectorInTargetFrame( currentTime );

        expectedBodyAngularVelocityVectorInBaseFrame = spice_interface::getAngularVelocityVectorOfFrameInOriginalFrame(
                    baseFrame, targetFrame, currentTime );
        expectedBodyAngularVelocityVectorInTargetFrame =
                expectedRotationMatrixToTargetFrame * expectedBodyAngularVelocityVectorInBaseFrame;

        for( unsigned int i = 0; i < 3; i++ )
        {

            BOOST_CHECK_SMALL(
                        std::fabs( bodyAngularVelocityVectorInBaseFrame( i ) -
                                   expectedBodyAngularVelocityVectorInBaseFrame( i ) ), 1.0E-10 );
            BOOST_CHECK_SMALL(
                        std::fabs( bodyAngularVelocityVectorInTargetFrame( i ) -
                                   expectedBodyAngularVelocityVectorInTargetFrame( i ) ), 1.0E-10 );
        }

        currentTime += interpolationTimeStep;
    }

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#include "../../planning/PlanZMPList/ZMPPlanner.h"
#include "../../patten/PreviewController/PreviewControl.h"

#include <iostream>

#include <Eigen/Core>

using namespace std;
using namespace Eigen;

const int ONE_STEP_THRE = 2;
const int MAX_STEP = 1;

class testGaitGenerator{
public:
	testGaitGenerator(const double dt, const double gait_period, const double zc)
		: dt(dt), gait_period(gait_period)
	{
		preview_node = new preview_control(dt, 1.6, zc);
		plan_node = new ZMPPlanner(gait_period, 0.05f);
	}
	void run(int max_step, const double torso_x, const double torso_y, const double torso_th)
	{
		Vector2d tP_B(Vector2d::Zero());
		Vector2d prev_refzmp(Vector2d(0.0f,0.0f));
		Vector2d next_torso_pos = prev_refzmp;
		Vector2d com_pos, com_vel, com_acc;

		double tth_B=0.0;
		double preview_timer=0.0f;
		int total_step=1;

		// Main Loop
		while(1){
			// Generate Reference ZMP
			int zmp_gen=0;
			plan_node->refzmp_list.push_back(Vector3d(0.0f,prev_refzmp[0], prev_refzmp[1]));
			tP_B = next_torso_pos; //tth_B += torso_th;
			while(1){
				plan_node->getNextZMP(tP_B, tth_B);
				if(plan_node->getWalkState() == STOP) break;
				zmp_gen++;
				if(ONE_STEP_THRE <= zmp_gen) plan_node->setStopFlag();
				tP_B += Vector2d(torso_x, torso_y); tth_B += torso_th;
			}

			// Preview Controller
			preview_node->interpolation_zmp_trajectory(plan_node->refzmp_list);
			while(preview_node->update(com_pos, com_vel, com_acc)){
				Vector2d temp_refzmp; RowVector2d temp_outputzmp;
				preview_node->get_ref_zmp(temp_refzmp);
				preview_node->output_zmp(temp_outputzmp);

				com_pos_list.push_back(com_pos);
				refzmp.push_back(temp_refzmp);
				outputzmp.push_back(temp_outputzmp.transpose());

				preview_timer += dt;
				if(total_step < max_step){
					if(gait_period <= preview_timer){
						preview_timer = 0.0f;
						break;
					}
				}
			}

			prev_refzmp[0] = ((plan_node->refzmp_list[2][1]-plan_node->refzmp_list[1][1])/2.0)+plan_node->refzmp_list[1][1];//plan_node->refzmp_list[1][1] + (torso_x/2.0f);
			prev_refzmp[1] = ((plan_node->refzmp_list[2][2]-plan_node->refzmp_list[1][2])/2.0)+plan_node->refzmp_list[1][2];
			next_torso_pos[0] = torso_x * total_step;
			next_torso_pos[1] = torso_y * total_step;
			plan_node->buffer_clear();
			preview_node->buffer_clear();

			total_step++;
			if(max_step < total_step) break;
		}
	}
	void plot_gait_pattern()
	{
		FILE *gp = popen("gnuplot -persist\n", "w");
		fprintf(gp, "set xlabel \"x [m]\"\n");
		fprintf(gp, "set ylabel \"y [m]\"\n");

		fprintf(gp, "plot '-' with lines lw 2 title \"Center of Mass\", '-' with lines lw 2 title \"Reference ZMP\", '-' with lines lw 2 title \"Out Put ZMP\"\n");
		for(size_t i=0;i<com_pos_list.size();i++) fprintf(gp, "%f\t%f\n", com_pos_list[i](0), com_pos_list[i](1)); fprintf(gp, "e\n");
		for(size_t i=0;i<refzmp.size();i++) fprintf(gp, "%f\t%f\n", refzmp[i](0), refzmp[i](1)); fprintf(gp, "e\n");
		//for(size_t i=0;i<outputzmp.size();i++) fprintf(gp, "%f\t%f\n", outputzmp[i](0), outputzmp[i](1)); fprintf(gp, "e\n");
		fprintf(gp, "exit\n");
		pclose(gp);
	}
private:
	ZMPPlanner *plan_node;
	preview_control *preview_node;

	vector<Vector2d> com_pos_list;
	vector<Vector2d> refzmp;
	vector<Vector2d> outputzmp;

	double dt, gait_period;
};

void print_usage()
{
	std::cerr << "Usage: testGaitPatternGenerator [param] <step> <torso x> <torso y> <torso th>" << std::endl;
	std::cerr << "[param]:" << std::endl;
	std::cerr << "	step: max number of foot steps." << std::endl;
	std::cerr << "	torso x: max torso x." << std::endl;
	std::cerr << "	torso y: max torso y."	<< std::endl;
	std::cerr << "	torso th:	max torso th." << std::endl;
}

int main(int argc, char *argv[])
{
	testGaitGenerator tgg(0.01, 0.32, 0.27);

	if(argc == 5){
		int max_step = atoi(argv[1]);
		double torso_x	= static_cast<double>(atof(argv[2]));
		double torso_y	= static_cast<double>(atof(argv[3]));
		double torso_th	= static_cast<double>(atof(argv[4]));

		tgg.run(max_step, torso_x, torso_y, torso_th);
	}else{
		print_usage();
		return -1;
	}

	tgg.plot_gait_pattern();

	return 0;
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/lexical_cast.hpp>
using namespace std;
void solve(vector<long long int> &v, string s, int m, int cnt, int a, int b, int c) {
    if (cnt < m) solve(v, s + '3', m, cnt + 1, a + 1, b, c), solve(v, s + '5', m, cnt + 1, a, b + 1, c), solve(v, s + '7', m, cnt + 1, a, b, c + 1);
    if (cnt >= 3 && a >= 1 && b >= 1 && c >= 1) v.push_back(boost::lexical_cast<int>(s));
}
int main() {
    long long int n; cin >> n;
    vector<long long int> v;
    int c = boost::lexical_cast<string>(n).length(), ans = 0;
    solve(v, "", c, 0, 0, 0, 0);
    for (auto x : v) if (x <= n) ans++;
    cout << ans << endl;
}


#include "SSV.h"
#include <opencv2/opencv.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <boost/timer.hpp>

struct cluster_t
{
    cluster_t(int index, float avg_saliency) : index_(index), avg_saliency_(avg_saliency) {}
    bool operator<(const cluster_t& other) const { return avg_saliency_ < other.avg_saliency_; }
    int index_;
    float avg_saliency_;
};

int main(int argc, char** argv)
{
    SSV::PointCloudT::Ptr cloud = boost::shared_ptr <SSV::PointCloudT> (new SSV::PointCloudT ());
    pcl::console::print_highlight ("Loading point cloud...\n");
    if (pcl::io::loadPCDFile<SSV::PointT> (argv[1], *cloud))
    {
        pcl::console::print_error ("Error loading cloud file!\n");
        return (1);
    }

    char* imageName = argv[2];
    cv::Mat salmap;
    salmap = cv::imread(imageName);
    if(!salmap.data)
    {
        pcl::console::print_error ("Error loading saliency maps!\n");
        return (1);
    }

    float voxel_resolution = 0.008f;
    bool voxel_res_specified = pcl::console::find_switch (argc, argv, "-v");
    if (voxel_res_specified)
      pcl::console::parse(argc, argv, "-v", voxel_resolution);

    float kmeans_num_cluster = 2;
    bool kmeans_num_cluster_specified = pcl::console::find_switch (argc, argv, "-k");
    if (kmeans_num_cluster_specified)
      pcl::console::parse(argc, argv, "-k", kmeans_num_cluster);

    float min_seed_resolution = 0.1f;
    bool min_seed_resolution_specified = pcl::console::find_switch (argc, argv, "-i");
    if (min_seed_resolution_specified)
      pcl::console::parse(argc, argv, "-i", min_seed_resolution);

    float max_seed_resolution = 0.5f;
    bool max_seed_resolution_specified = pcl::console::find_switch (argc, argv, "-a");
    if (max_seed_resolution_specified)
      pcl::console::parse(argc, argv, "-a", max_seed_resolution);

    float spatial_weight = 0.15f;
    if (pcl::console::find_switch (argc, argv, "-z"))
      pcl::console::parse(argc, argv, "-z", spatial_weight);

    float normal_weight = 0.15f;
    if (pcl::console::find_switch (argc, argv, "-n"))
      pcl::console::parse(argc, argv, "-n", normal_weight);

    cv::Mat bgr[3];
    cv::split(salmap, bgr);
    cv::Mat salmap_im = bgr[0];
    salmap_im.convertTo(salmap_im, CV_32F);

    int attempts = 5;
    cv::Mat labels;
    cv::Mat avg_saliency;
    cv::kmeans(salmap_im.reshape(0,1).t(),
               kmeans_num_cluster,
               labels,
               cv::TermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10000, 0.0001),
               attempts,
               cv::KMEANS_PP_CENTERS,
               avg_saliency );

    std::vector<cluster_t> clusterdata(kmeans_num_cluster, cluster_t(0,0.0f));
    for (std::size_t c = 0; c < kmeans_num_cluster; ++c)
        clusterdata[c] = cluster_t(c, avg_saliency.at<float>(c));

    std::sort(clusterdata.begin(), clusterdata.end()); // sorts according to increasing average saliency

    // assign indices to clusters
    std::vector<SSV::Cluster> ssv_clusters(kmeans_num_cluster);
    for (int i = 0; i < cloud->size(); ++i)
    {
        int label = labels.at<int>(i);
        int cluster_idx = clusterdata[label].index_;
        ssv_clusters[cluster_idx].cloud_indices_.push_back(i);
    }

    // assign cluster resolutions
    if (kmeans_num_cluster == 1)
    {
        ssv_clusters[0].resolution_ = min_seed_resolution;
    }
    else
    {
    float step_size = (log10(min_seed_resolution)-log10(max_seed_resolution))/ static_cast<float>(kmeans_num_cluster-1);
        for (std::size_t c = 0; c < kmeans_num_cluster; ++c)
        {
            ssv_clusters[c].resolution_ = pow(10.0f, (log10(max_seed_resolution) + static_cast<float>(c)*step_size) );
        }
    }

    SSVOptions ssv_opt;
    ssv_opt.normal_weight_ = normal_weight;
    ssv_opt.spatial_distance_weight_ = spatial_weight;
    ssv_opt.color_weight_ = 1.0f - (normal_weight + spatial_weight);
    ssv_opt.voxel_resolution_ = voxel_resolution;

    pcl::console::print_highlight ("SSV Generating supervoxels...\n");
    boost::timer timer_ssv;
    SSV ssv(ssv_opt);
    ssv.set_input(cloud, ssv_clusters);

    // add clusters together
    std::vector<SSV::PointLCloudT::Ptr> labeled_clusters = ssv.get_labeled_clusters();
    SSV::PointLCloudT::Ptr result( new SSV::PointLCloudT() );

    std::vector<int> num_sv = ssv.num_supervoxels_per_cluster();
    int num_labels = 0;
    for (std::size_t c = 0; c < labeled_clusters.size(); ++c)
    {
        for (auto& p : *labeled_clusters[c])
            p.label += num_labels;

        *result += *labeled_clusters[c];
        num_labels += num_sv[c];
    }
    float elapsed = timer_ssv.elapsed();
    std::cout << "SSV Time elapsed: " << elapsed << " seconds.\n";

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->initCameraParameters();

    // Create all the viewports
    int v1(0);
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
    viewer->setBackgroundColor (0.0, 0.0, 0.0, v1);

    int v2(0);
    viewer->createViewPort (0.5, 0.0, 1.0, 1.0, v2);
    viewer->setBackgroundColor (0.0, 0.0, 0.0, v2);

    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud(cloud, "point cloud", v1);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY,1.0, "point cloud");

    viewer->addPointCloud(result, "ssv voxel",v2);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY,1.0, "ssv voxel");

    while (!viewer->wasStopped()) {
        viewer->spinOnce(100);
    }

    return 0;
}


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

// Copyright (c) 2007-2013 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_JOIN_ROUND_BY_DIVIDE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_JOIN_ROUND_BY_DIVIDE_HPP

// Buffer strategies

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/strategies/tags.hpp>
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

#include <boost/geometry/extensions/strategies/buffer_side.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

template
<
    typename PointIn,
    typename PointOut
>
struct join_round_by_divide
{
    inline join_round_by_divide(int max_level = 4)
        : m_max_level(max_level)
    {}

    typedef typename strategy::side::services::default_strategy<typename cs_tag<PointIn>::type>::type side;
    typedef typename coordinate_type<PointOut>::type coordinate_type;

    typedef typename geometry::select_most_precise
        <
            typename geometry::select_most_precise
                <
                    typename geometry::coordinate_type<PointIn>::type,
                    typename geometry::coordinate_type<PointOut>::type
                >::type,
            double
        >::type promoted_type;

    int m_max_level;

    template <typename RangeOut>
    inline void mid_points(PointIn const& vertex,
                PointIn const& p1, PointIn const& p2,
                coordinate_type const& buffer_distance,
                RangeOut& range_out,
                int level = 1) const
    {
        // Generate 'vectors'
        coordinate_type vp1_x = get<0>(p1) - get<0>(vertex);
        coordinate_type vp1_y = get<1>(p1) - get<1>(vertex);

        coordinate_type vp2_x = (get<0>(p2) - get<0>(vertex));
        coordinate_type vp2_y = (get<1>(p2) - get<1>(vertex));

        // Average them to generate vector in between
        coordinate_type two = 2;
        coordinate_type v_x = (vp1_x + vp2_x) / two;
        coordinate_type v_y = (vp1_y + vp2_y) / two;

        coordinate_type length2 = sqrt(v_x * v_x + v_y * v_y);

        coordinate_type prop = buffer_distance / length2;

        PointIn mid_point;
        set<0>(mid_point, get<0>(vertex) + v_x * prop);
        set<1>(mid_point, get<1>(vertex) + v_y * prop);

        if (level < m_max_level)
        {
            mid_points(vertex, p1, mid_point, buffer_distance, range_out, level + 1);
        }
        range_out.push_back(mid_point);
        if (level < m_max_level)
        {
            mid_points(vertex, mid_point, p2, buffer_distance, range_out, level + 1);
        }
    }

    template <typename RangeOut>
    inline void apply(PointIn const& ip, PointIn const& vertex,
                PointIn const& perp1, PointIn const& perp2,
                coordinate_type const& buffer_distance,
                RangeOut& range_out) const
    {
        if (geometry::equals(perp1, perp2))
        {
            //std::cout << "Corner for equal points " << geometry::wkt(ip) << " " << geometry::wkt(perp1) << std::endl;
            return;
        }

        coordinate_type zero = 0;
        int signum = buffer_distance > zero ? 1
                   : buffer_distance < zero ? -1
                   : 0;

        if (side::apply(perp1, ip, perp2) == signum)
        {
        }
        else
        {
            // Generate 'vectors'
            coordinate_type vix = (get<0>(ip) - get<0>(vertex));
            coordinate_type viy = (get<1>(ip) - get<1>(vertex));

            coordinate_type length_i = sqrt(vix * vix + viy * viy);

            coordinate_type const bd = geometry::math::abs(buffer_distance);
            coordinate_type prop = bd / length_i;

            PointIn bp;
            set<0>(bp, get<0>(vertex) + vix * prop);
            set<1>(bp, get<1>(vertex) + viy * prop);

            range_out.push_back(perp1);

            if (m_max_level > 1)
            {
                mid_points(vertex, perp1, bp, bd, range_out);
                range_out.push_back(bp);
                mid_points(vertex, bp, perp2, bd, range_out);
            }
            else if (m_max_level == 1)
            {
                range_out.push_back(bp);
            }

            range_out.push_back(perp2);
        }
    }
};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_JOIN_ROUND_BY_DIVIDE_HPP


#include <armadillo>
#include <emissions.hpp>
#include <ForwardBackward.hpp>
#include <cmath>
#include <json.hpp>
#include <iostream>
#include <vector>

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

namespace hsmm {

    double gaussianlogpdf_(double x, double mu, double sigma) {
        double ret = ((x - mu)*(x - mu)) / (-2*sigma*sigma);
        ret = ret - log(sqrt(2 * M_PI) * sigma);
        return ret;
    }

    double gaussianpdf_(double x, double mu, double sigma) {
        return exp(gaussianlogpdf_(x, mu, sigma));
    }

    field<mat> fromMatToField(const mat& obs) {
        field<mat> ret(obs.n_cols);
        for(int i = 0; i < obs.n_cols; i++)
            ret(i) = obs.col(i);
        return ret;
    }

    mat fromFieldToMat(const field<mat>& obs) {
        int total_cols = 0;
        int total_rows = obs(0).n_rows;
        for(auto& m : obs) {
            total_cols += m.n_cols;
            assert(m.n_rows == total_rows);
        }
        mat ret(total_rows, total_cols);
        int idx = 0;
        for(auto& m: obs) {
            ret.cols(idx, m.n_cols - 1) = m;
            idx += m.n_cols;
        }
        return ret;
    }

    /**
     * Abstract emission implementation.
     */
    AbstractEmission::AbstractEmission(int nstates, int dimension) :
            nstates_(nstates), dimension_(dimension) {}

    int AbstractEmission::getNumberStates() const {
        return nstates_;
    }

    int AbstractEmission::getDimension() const {
        return dimension_;
    }

    cube AbstractEmission::likelihoodCube(int min_duration, int ndurations,
            const field<mat>& obs) const {
        return exp(loglikelihoodCube(min_duration, ndurations, obs));
    }

    // This should return a cube of dimensions (nstates, nobs, ndurations)
    // where the entry (i, j, k) is the log-likelihood of the observations
    // in the interval [j, min_duration + k - 1] being produced by state i.
    cube AbstractEmission::loglikelihoodCube(int min_duration, int ndurations,
            const field<mat>& obs) const {
        int nobs = obs.n_elem;
        cube pdf(getNumberStates(), nobs, ndurations);
        pdf.fill(-datum::inf);
        for(int i = 0; i < getNumberStates(); i++)
            for(int t = 0; t < nobs; t++)
                for(int d = 0; d < ndurations; d++) {
                    if (t + min_duration + d > nobs)
                        break;
                    int end_idx = t + min_duration + d - 1;
                    pdf(i, t, d) = loglikelihood(i, obs.rows(t, end_idx));
                }
        return pdf;
    }

    field<mat> AbstractEmission::sampleFromState(int state, int size) {
        return sampleFromState(state, size, rand_generator_);
    }

    json AbstractEmission::to_stream() const {
        cout << "Warning: serialization of emission parameters not implemented"
                << endl;
        return json::object();  // empty json object by default.
    }

    void AbstractEmission::from_stream(const json& emission_params) {
        cout << "Warning: not updating the emission parameters from the stream"
                << endl;
        return;
    }

    /*
     * AbstractEmissionOnlineSetting implementation
     */
    field<mat> AbstractEmissionOnlineSetting::sampleNextObsGivenPastObs(int state,
            int seg_dur, const field<mat>& past_obs) {
        return sampleNextObsGivenPastObs(state, seg_dur, past_obs,
                rand_generator_);
    }

    // Default implementation. Disregards any information from the last
    // segment.
    field<mat> AbstractEmissionOnlineSetting::sampleFirstSegmentObsGivenLastSegment(
            int curr_state, int curr_seg_dur, const field<mat> &last_segment,
            int last_state, std::mt19937 &rng) const {
        field<mat> empty_segment;
        return sampleNextObsGivenPastObs(curr_state, curr_seg_dur,
                empty_segment, rng);
    }

    field<mat> AbstractEmissionOnlineSetting::sampleFirstSegmentObsGivenLastSegment(
            int curr_state, int curr_seg_dur, const field<mat> &last_segment,
            int last_state) {
        return sampleFirstSegmentObsGivenLastSegment(curr_state, curr_seg_dur,
                last_segment, last_state, rand_generator_);
    }

    /**
     * AbstractEmissionIIDobs implementation
     */
    AbstractEmissionConditionalIIDobs::AbstractEmissionConditionalIIDobs(
            int nstates, int dimension) :
            AbstractEmission(nstates, dimension) {}

    double AbstractEmissionConditionalIIDobs::loglikelihood(int state,
            const field<mat>& obs) const {
        double ret = 0;
        int seg_dur = obs.n_elem;
        for(int i = 0; i < seg_dur; i++) {

            // If there are missing outputs just skip them.
            if (!obs(i).is_empty())
                ret += loglikelihoodIIDobs(state, seg_dur, i, obs(i));
        }
        return ret;
    }

    /**
     * DummyGaussianEmission implementation.
     */
    DummyGaussianEmission::DummyGaussianEmission(vec& means, vec& std_devs) :
            AbstractEmissionOnlineSetting(means.n_elem, 1), means_(means),
            std_devs_(std_devs) {
        assert(means_.n_elem == std_devs_.n_elem);
    }

    DummyGaussianEmission* DummyGaussianEmission::clone() const {
        return new DummyGaussianEmission(*this);
    }

    double DummyGaussianEmission::loglikelihood(int state,
            const field<mat>& obs) const {
        double ret = 0;
        int seg_dur = obs.n_elem;
        for(int i = 0; i < seg_dur; i++) {

            // If there are missing outputs just skip them.
            if (!obs(i).is_empty())
                ret += loglikelihoodIIDobs(state, seg_dur, i, obs(i));
        }
        return ret;
    }

    double DummyGaussianEmission::loglikelihoodIIDobs(int state, int seg_dur,
            int offset, const mat& single_obs) const {
        assert(single_obs.n_rows == 1 && single_obs.n_cols == 1);
        return gaussianlogpdf_(single_obs(0, 0), means_(state),
                std_devs_(state));
    }

    json DummyGaussianEmission::to_stream() const {
        vector<double> means = conv_to<vector<double>>::from(means_);
        vector<double> std_devs = conv_to<vector<double>>::from(std_devs_);
        json ret;
        ret["means"] = means;
        ret["std_devs"] = std_devs;
        return ret;
    }

    void DummyGaussianEmission::reestimate(int min_duration,
            const field<cube>& meta, const field<field<mat>>& mobs) {
        int nseq = mobs.n_elem;
        for(int i = 0; i < getNumberStates(); i++) {

            // Reestimating the mean.
            vector<double> num_mult;
            vector<double> num_obs;
            for(int s = 0; s < nseq; s++) {
                auto& obs = mobs(s);
                int nobs = obs.n_elem;
                const cube& eta = meta(s);
                int ndurations = eta.n_cols;
                for(int t = min_duration - 1; t < nobs; t++) {
                    for(int d = 0; d < ndurations; d++) {
                        int first_idx_seg = t - min_duration - d + 1;
                        if (first_idx_seg < 0)
                            break;

                        // Since the observations factorize given t, d and i.
                        for(int k = first_idx_seg; k <= t; k++) {
                            num_mult.push_back(eta(i, d, t));
                            num_obs.push_back(obs(k)(0,0));
                        }
                    }
                }
            }
            vec num_mult_v(num_mult);
            vec num_obs_v(num_obs);
            num_mult_v = num_mult_v - logsumexp(num_mult_v);
            num_mult_v = exp(num_mult_v);
            double new_mean = dot(num_mult_v, num_obs_v);

            // Reestimating the variance.
            vector<double> num_obs_var;
            for(int s = 0; s < nseq; s++) {
                const auto& obs = mobs(s);
                int nobs = obs.n_elem;
                const cube& eta = meta(s);
                int ndurations = eta.n_cols;
                for(int t = min_duration - 1; t < nobs; t++) {
                    for(int d = 0; d < ndurations; d++) {
                        int first_idx_seg = t - min_duration - d + 1;
                        if (first_idx_seg < 0)
                            break;

                        // Since the observations factorize given t, d and i.
                        for(int k = first_idx_seg; k <= t; k++) {
                            double diff = (obs(k)(0,0) - new_mean);
                            num_obs_var.push_back(diff * diff);
                        }
                    }
                }
            }
            vec num_obs_var_v(num_obs_var);
            double new_variance = dot(num_mult_v, num_obs_var_v);

            means_(i) = new_mean;
            std_devs_(i) = sqrt(new_variance);
        }
    }

    field<mat> DummyGaussianEmission::sampleFromState(int state,
            int size, mt19937 &rng) const {
        mat ret = randn<mat>(1, size) * std_devs_(state) + means_(state);
        return fromMatToField(ret);
    }

    field<mat> DummyGaussianEmission::sampleNextObsGivenPastObs(int state,
            int seg_dur, const field<mat>& past_obs, mt19937 &rng) const {
        assert(past_obs.n_elem < seg_dur);
        field<mat> ret(seg_dur);
        int idx = 0;
        for(int i = 0; i < past_obs.n_elem; i++, idx++)
            ret(i) = past_obs(i);
        for(int i = idx; i < seg_dur; i++)
            ret(i) = randn<mat>(1, 1) * std_devs_(state) + means_(state);
        return ret;
    }

    /**
     * DummyMultivariateGaussianEmission implementation.
     */
    DummyMultivariateGaussianEmission::DummyMultivariateGaussianEmission(
            mat& means, double std_dev_output_noise) :
            AbstractEmission(means.n_rows, means.n_cols), means_(means),
            std_dev_output_noise_(std_dev_output_noise) {}

    DummyMultivariateGaussianEmission* DummyMultivariateGaussianEmission::
            clone() const {
        return new DummyMultivariateGaussianEmission(*this);
    }

    double DummyMultivariateGaussianEmission::loglikelihood(int state,
                const field<mat>& obs) const {
        mat copy_obs = fromFieldToMat(obs);
        assert(copy_obs.n_rows == getDimension());
        int size = copy_obs.n_cols;
        for(int i = 0; i < getDimension(); i++)
            copy_obs.row(i) -= linspace<rowvec>(0.0, 1.0, size) +
                    means_(state, i);
        double ret = 0.0;
        for(int i = 0; i < getDimension(); i++)
            for(int j = 0; j < size; j++)
                ret += gaussianlogpdf_(copy_obs(i, j), 0,
                        std_dev_output_noise_);
        return ret;
    }

    void DummyMultivariateGaussianEmission::reestimate(int min_duration,
            const field<cube>& eta, const field<field<mat>>& mobs) {
        // TODO.
    }

    field<mat> DummyMultivariateGaussianEmission::sampleFromState(
            int state, int size, mt19937 &rng) const {
        mat ret = randn<mat>(getDimension(), size) * std_dev_output_noise_;
        for(int i = 0; i < getDimension(); i++)
            ret.row(i) += linspace<rowvec>(0.0, 1.0, size) + means_(state, i);
        return fromMatToField(ret);
    }

};


#include "catch.hpp"

#include "libirc/transformation.h"

#include "config.h"
#include "libirc/conversion.h"
#include "libirc/io.h"
#include "libirc/irc.h"
#include "libirc/molecule.h"
#include "libirc/wilson.h"

#include <iostream>

#ifdef HAVE_ARMA
#include <armadillo>
using vec3 = arma::vec3;
using vec = arma::vec;
using mat = arma::mat;

template<typename T>
using Mat = arma::Mat<T>;
#elif HAVE_EIGEN3
#include <Eigen/Dense>
using vec3 = Eigen::Vector3d;
using vec = Eigen::VectorXd;
using mat = Eigen::MatrixXd;

template<typename T>
using Mat = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
#else
#error
#endif

using namespace irc;

TEST_CASE("RMS") {
  using namespace transformation;

  vec v{1, 2, 3, 4};

  CHECK(rms(v) == Approx(std::sqrt(30 / 4.)));
}

TEST_CASE("Transformation") {

  bool verbose{true};

  SECTION("Cartesian to internal for ethanol") {
    using namespace molecule;
    using namespace connectivity;
    using namespace transformation;
    using namespace tools::conversion;
    using namespace io;
    using namespace std;

    // Load molecule from file
    const auto molecule = load_xyz<vec3>(config::molecules_dir + "ethanol.xyz");

    // Compute interatomic distance for formaldehyde molecule
    mat dd{distances<vec3, mat>(molecule)};

    // Build graph based on the adjacency matrix
    UGraph adj{adjacency_matrix(dd, molecule)};

    // Compute distance matrix and predecessor matrix
    mat dist{distance_matrix<mat>(adj)};

    // Compute bonds
    std::vector<Bond> B{bonds(dist, molecule)};

    // Print bonds
    if (verbose) {
      print_bonds<vec3, vec>(to_cartesian<vec3, vec>(molecule), B);
    }

    // Compute angles
    std::vector<Angle> A{angles(dist, molecule)};

    // Print angles
    if (verbose) {
      print_angles<vec3, vec>(to_cartesian<vec3, vec>(molecule), A);
    }

    // Compute dihedral angles
    std::vector<Dihedral> D{dihedrals(dist, molecule)};

    // Print dihedral angles
    if (verbose) {
      print_dihedrals<vec3, vec>(to_cartesian<vec3, vec>(molecule), D);
    }

    // Compute linear angles
    std::vector<LinearAngle<vec3>> LA{linear_angles(dist, molecule)};

    // Print linear angles
    if (verbose) {
      print_linear_angles<vec3, vec>(to_cartesian<vec3, vec>(molecule), LA);
    }

    // Compute linear angles
    std::vector<OutOfPlaneBend> OOPB{out_of_plane_bends(dist, molecule)};

    // Print linear angles
    if (verbose) {
      print_out_of_plane_bends<vec3, vec>(to_cartesian<vec3, vec>(molecule),
                                          OOPB);
    }

    // Compute number of cartesian coordinates
    std::size_t n_c{3 * molecule.size()};

    // Allocate vector for cartesian positions
    vec x_c{linalg::zeros<vec>(n_c)};

    // Fill vector with cartesian positions
    for (std::size_t i{0}; i < molecule.size(); i++) {
      x_c(3 * i + 0) = molecule[i].position(0);
      x_c(3 * i + 1) = molecule[i].position(1);
      x_c(3 * i + 2) = molecule[i].position(2);
    }

    if (verbose) {
      // Print cartesian coordinates
      cout << "\nCartesian coordinates (a.u.):\n " << x_c << endl;
    }

    if (verbose) {
      // Compute and print internal redundant coordinates
      cout << "Internal redundant coordinates (a.u.):\n"
           << cartesian_to_irc<vec3, vec>(x_c, B, A, D, LA, OOPB) << endl;
    }
  }

  SECTION("Internal to cartesian for H2") {
    using namespace molecule;
    using namespace connectivity;
    using namespace transformation;
    using namespace tools::conversion;
    using namespace io;
    using namespace std;
    using namespace wilson;

    // Load molecule from file
    Molecule<vec3> molecule{{{"H", {0., 0., 0.}}, {"H", {1., 0., 0.}}}};

    // Connectivity
    mat dd{distances<vec3, mat>(molecule)};
    UGraph adj{adjacency_matrix(dd, molecule)};
    mat dist{distance_matrix<mat>(adj)};

    // Compute bonds
    std::vector<Bond> B{bonds(dist, molecule)};

    // Print bonds
    if (verbose) {
      print_bonds<vec3, vec>(to_cartesian<vec3, vec>(molecule), B);
    }

    // Check number of bonds
    REQUIRE(B.size() == 1);

    // Wilson B matrix
    mat W = wilson_matrix<vec3, vec, mat>(
        molecule::to_cartesian<vec3, vec>(molecule), B);

    // Allocate vector for internal reaction coordinates
    vec q_irc{connectivity::cartesian_to_irc<vec3, vec>(
        molecule::to_cartesian<vec3, vec>(molecule), B, {}, {}, {}, {})};

    // Displacement in internal coordinates
    vec dq_irc{0.1};

    // Print displacement in internal coordinates
    if (verbose) {
      cout << "\nDisplacement in internal coordinates (a.u.):\n " << dq_irc
           << endl;
    }

    // Compute number of cartesian coordinates
    std::size_t n_c{3 * molecule.size()};

    // Allocate vector for cartesian positions
    vec x_c_old{linalg::zeros<vec>(n_c)};

    // Fill vector with cartesian positions
    for (std::size_t i{0}; i < molecule.size(); i++) {
      x_c_old(3 * i + 0) = molecule[i].position(0);
      x_c_old(3 * i + 1) = molecule[i].position(1);
      x_c_old(3 * i + 2) = molecule[i].position(2);
    }

    // Compute new cartesian coordinates
    const auto itc_result = irc_to_cartesian<vec3, vec, mat>(
        q_irc, dq_irc, x_c_old, B, {}, {}, {}, {});
    const auto x_c = itc_result.x_c;

    // Print cartesian coordinates
    if (verbose) {
      cout << "\nNew cartesian coordinates (a.u.):\n " << x_c << endl;
    }

    vec3 p1{x_c(0), x_c(1), x_c(2)};
    vec3 p2{x_c(3), x_c(4), x_c(5)};

    Approx target{q_irc(0) + dq_irc(0)};
    target.margin(1e-6);

    REQUIRE(distance(p1, p2) == target);
  }

  SECTION("Internal to cartesian for H2O") {
    using namespace molecule;
    using namespace connectivity;
    using namespace transformation;
    using namespace tools::conversion;
    using namespace io;
    using namespace std;
    using namespace wilson;

    // Load molecule from file
    const auto molecule = load_xyz<vec3>(config::molecules_dir + "water.xyz");

    // Compute interatomic distance for formaldehyde molecule
    mat dd{distances<vec3, mat>(molecule)};

    // Build graph based on the adjacency matrix
    UGraph adj{adjacency_matrix(dd, molecule)};

    // Compute distance matrix and predecessor matrix
    mat dist{distance_matrix<mat>(adj)};

    // Compute bonds
    std::vector<Bond> B{bonds(dist, molecule)};

    // Print bonds
    if (verbose) {
      print_bonds<vec3, vec>(to_cartesian<vec3, vec>(molecule), B);
    }

    // Check number of bonds
    REQUIRE(B.size() == 2);

    // Compute angle
    std::vector<Angle> A{angles(dist, molecule)};

    // Print angles
    if (verbose) {
      print_angles<vec3, vec>(to_cartesian<vec3, vec>(molecule), A);
    }

    // Compute Wilson B matrix
    mat W = wilson_matrix<vec3, vec, mat>(
        molecule::to_cartesian<vec3, vec>(molecule), B, A);

    // Allocate vector for internal reaction coordinates
    vec q_irc_old{connectivity::cartesian_to_irc<vec3, vec>(
        molecule::to_cartesian<vec3, vec>(molecule), B, A, {}, {}, {})};

    // Displacement in internal coordinates
    vec dq_irc{0.0, 0.0, 1. / 180. * tools::constants::pi};

    // Compute new internal coordinates
    vec q_irc_new{q_irc_old + dq_irc};

    // Print new internal coordinates
    if (verbose) {
      cout << "\nNew internal coordinates:\n " << q_irc_new << endl;
    }

    // Allocate vector for cartesian positions
    vec x_c_old{to_cartesian<vec3, vec>(molecule)};

    // Compute new cartesian coordinates
    const auto itc_result = irc_to_cartesian<vec3, vec, mat>(
        q_irc_old, dq_irc, x_c_old, B, A, {}, {}, {});
    const auto x_c = itc_result.x_c;

    // Print cartesian coordinates
    if (verbose) {
      cout << "\nNew cartesian coordinates (a.u.):\n " << x_c << endl;
    }

    // Reconstruct atomic positions (points in 3D space)
    vec3 p1{x_c(0), x_c(1), x_c(2)};
    vec3 p2{x_c(3), x_c(4), x_c(5)};
    vec3 p3{x_c(6), x_c(7), x_c(8)};

    SECTION("Bond 1") {
      Approx target{q_irc_new(0)};
      target.margin(1e-4);
      REQUIRE(distance(p1, p2) == target);
    }

    SECTION("Bond 2") {
      Approx target{q_irc_new(1)};
      target.margin(1e-4);
      REQUIRE(distance(p2, p3) == target);
    }

    SECTION("Angle") {
      Approx target{q_irc_new(2)};
      target.margin(1e-4);
      REQUIRE(angle(p1, p2, p3) == target);
    }
  }

  SECTION("Internal to cartesian for H2O2") {
    using namespace molecule;
    using namespace connectivity;
    using namespace transformation;
    using namespace tools::conversion;
    using namespace io;
    using namespace std;
    using namespace wilson;

    // Load molecule from file
    const auto molecule =
        load_xyz<vec3>(config::molecules_dir + "hydrogen_peroxide.xyz");

    // Compute interatomic distance for formaldehyde molecule
    mat dd{distances<vec3, mat>(molecule)};

    // Build graph based on the adjacency matrix
    UGraph adj{adjacency_matrix(dd, molecule)};

    // Compute distance matrix and predecessor matrix
    mat dist{distance_matrix<mat>(adj)};

    // Compute bonds
    std::vector<Bond> B{bonds(dist, molecule)};

    // Print bonds
    if (verbose) {
      print_bonds<vec3, vec>(to_cartesian<vec3, vec>(molecule), B);
    }

    // Check number of bonds
    REQUIRE(B.size() == 3);

    // Compute angles
    std::vector<Angle> A{angles(dist, molecule)};

    // Print angles
    if (verbose) {
      print_angles<vec3, vec>(to_cartesian<vec3, vec>(molecule), A);
    }

    // Check number of angles
    REQUIRE(A.size() == 2);

    // Compute dihedral angles
    std::vector<Dihedral> D{dihedrals(dist, molecule)};

    // Print dihedrals
    if (verbose) {
      print_dihedrals<vec3, vec>(to_cartesian<vec3, vec>(molecule), D);
    }

    // Check number of dihedrals
    REQUIRE(D.size() == 1);

    // Compute Wilson B matrix
    mat W = wilson_matrix<vec3, vec, mat>(
        molecule::to_cartesian<vec3, vec>(molecule), B, A, D);

    // Allocate vector for internal reaction coordinates
    vec q_irc_old{connectivity::cartesian_to_irc<vec3, vec>(
        molecule::to_cartesian<vec3, vec>(molecule), B, A, D, {}, {})};

    // Displacement in internal coordinates
    vec dq_irc{0.0, 0.0, 0.0, 0.0, 0.0, 1. / 180 * tools::constants::pi};

    // Compute new internal coordinates
    vec q_irc_new{q_irc_old + dq_irc};

    // Print new internal coordinates
    if (verbose) {
      cout << "\nNew internal coordinates:\n " << q_irc_new << endl;
    }

    // Allocate vector for cartesian positions
    vec x_c_old{to_cartesian<vec3, vec>(molecule)};

    // Compute new cartesian coordinates
    const auto itc_result = irc_to_cartesian<vec3, vec, mat>(
        q_irc_old, dq_irc, x_c_old, B, A, D, {}, {});
    const auto x_c = itc_result.x_c;

    // Print cartesian coordinates
    if (verbose) {
      cout << "\nNew cartesian coordinates (a.u.):\n " << x_c << endl;
    }

    // Reconstruct atomic positions (points in 3D space)
    vec3 p1{x_c(0), x_c(1), x_c(2)};
    vec3 p2{x_c(3), x_c(4), x_c(5)};
    vec3 p3{x_c(6), x_c(7), x_c(8)};
    vec3 p4{x_c(9), x_c(10), x_c(11)};

    SECTION("Bond 1") {
      Approx target{q_irc_new(0)};
      target.margin(1e-4);
      REQUIRE(distance(p1, p2) == target);
    }

    SECTION("Bond 2") {
      Approx target{q_irc_new(1)};
      target.margin(1e-4);
      REQUIRE(distance(p1, p3) == target);
    }

    SECTION("Bond 3") {
      Approx target{q_irc_new(2)};
      target.margin(1e-4);
      REQUIRE(distance(p2, p4) == target);
    }

    SECTION("Angle 1") {
      Approx target{q_irc_new(3)};
      target.margin(1e-4);
      REQUIRE(angle(p2, p1, p3) == target);
    }

    SECTION("Angle 2") {
      Approx target{q_irc_new(4)};
      target.margin(1e-4);
      REQUIRE(angle(p1, p2, p4) == target);
    }

    SECTION("Dihedral") {
      Approx target{q_irc_new(5)};
      target.margin(1e-4);
      REQUIRE(dihedral(p4, p2, p1, p3) == target);
    }
  }

  SECTION("Internal to cartesian for CO2") {
    using namespace molecule;
    using namespace connectivity;
    using namespace transformation;
    using namespace tools::conversion;
    using namespace io;
    using namespace std;
    using namespace wilson;

    // Load molecule from file
    const auto molecule =
        load_xyz<vec3>(config::molecules_dir + "carbon_dioxide.xyz");

    // Compute interatomic distance for molecule
    mat dd{distances<vec3, mat>(molecule)};

    // Build graph based on the adjacency matrix
    UGraph adj{adjacency_matrix(dd, molecule)};

    // Compute distance matrix and predecessor matrix
    mat dist{distance_matrix<mat>(adj)};

    // Compute bonds
    std::vector<Bond> B{bonds(dist, molecule)};

    // Print bonds
    if (verbose) {
      print_bonds<vec3, vec>(to_cartesian<vec3, vec>(molecule), B);
    }

    // Check number of bonds
    REQUIRE(B.size() == 2);

    // Compute angles
    std::vector<Angle> A{angles(dist, molecule)};

    // Print angles
    if (verbose) {
      print_angles<vec3, vec>(to_cartesian<vec3, vec>(molecule), A);
    }

    // Check number of angles
    REQUIRE(A.size() == 0);

    // Compute dihedral angles
    std::vector<Dihedral> D{dihedrals(dist, molecule)};

    // Print dihedrals
    if (verbose) {
      print_dihedrals<vec3, vec>(to_cartesian<vec3, vec>(molecule), D);
    }

    // Check number of dihedrals
    REQUIRE(D.size() == 0);

    // Compute linear angles
    std::vector<LinearAngle<vec3>> LA{linear_angles(dist, molecule)};

    // Print linear angles
    if (verbose) {
      print_linear_angles<vec3, vec>(to_cartesian<vec3, vec>(molecule), LA);
    }

    // Check number of linear angles
    REQUIRE(LA.size() == 2);

    // Compute Wilson B matrix
    mat W = wilson_matrix<vec3, vec, mat>(
        molecule::to_cartesian<vec3, vec>(molecule), B, A, D, LA, {});

    // Allocate vector for internal reaction coordinates
    vec q_irc_old{connectivity::cartesian_to_irc<vec3, vec>(
        molecule::to_cartesian<vec3, vec>(molecule), B, A, D, LA, {})};

    // Displacement in internal coordinates
    vec dq_irc{0.0, 0.0, 0.0, 1. / 180 * tools::constants::pi};

    // Compute new internal coordinates
    vec q_irc_new{q_irc_old + dq_irc};

    // Print new internal coordinates
    if (verbose) {
      cout << "\nNew internal coordinates:\n " << q_irc_new << endl;
    }

    // Allocate vector for cartesian positions
    vec x_c_old{to_cartesian<vec3, vec>(molecule)};

    // Compute new cartesian coordinates
    const auto itc_result = irc_to_cartesian<vec3, vec, mat>(
        q_irc_old, dq_irc, x_c_old, B, A, D, LA, {});
    const auto x_c = itc_result.x_c;

    // Print cartesian coordinates
    if (verbose) {
      cout << "\nNew cartesian coordinates (a.u.):\n " << x_c << endl;
    }

    // Reconstruct atomic positions (points in 3D space)
    vec3 p1{x_c(0), x_c(1), x_c(2)};
    vec3 p2{x_c(3), x_c(4), x_c(5)};
    vec3 p3{x_c(6), x_c(7), x_c(8)};

    SECTION("Bond CO{1}") {
      Approx target{q_irc_new(0)};
      target.margin(1e-4);
      REQUIRE(distance(p1, p2) == target);
    }

    SECTION("Bond CO{2}") {
      Approx target{q_irc_new(1)};
      target.margin(1e-4);
      REQUIRE(distance(p1, p3) == target);
    }

    SECTION("Angle OCO") {
      Approx target{q_irc_new(3)};
      target.margin(1e-4);
      REQUIRE(angle(LA[1], x_c) == target);
    }
  }

  SECTION("Big change in water") {
    using namespace molecule;
    using namespace connectivity;
    using namespace transformation;
    using namespace tools::conversion;
    using namespace io;
    using namespace std;
    using namespace wilson;

    auto molecule = molecule::Molecule<vec3>{
        {{"O", {0., 0., 0.}}, {"H", {1., 0., 0.}}, {"H", {0., 1., 0.}}}};
    multiply_positions(molecule, tools::conversion::angstrom_to_bohr);

    // Compute interatomic distance for formaldehyde molecule
    mat dd{distances<vec3, mat>(molecule)};

    // Build graph based on the adjacency matrix
    UGraph adj{adjacency_matrix(dd, molecule)};

    // Compute distance matrix and predecessor matrix
    mat dist{distance_matrix<mat>(adj)};

    // Compute bonds
    std::vector<Bond> B{bonds(dist, molecule)};

    // Check number of bonds
    REQUIRE(B.size() == 2);

    // Compute angle
    std::vector<Angle> A{angles(dist, molecule)};

    // Compute Wilson B matrix
    mat W = wilson_matrix<vec3, vec, mat>(
        molecule::to_cartesian<vec3, vec>(molecule), B, A);

    // Allocate vector for internal reaction coordinates
    vec q_irc_old{connectivity::cartesian_to_irc<vec3, vec>(
        molecule::to_cartesian<vec3, vec>(molecule), B, A, {}, {}, {})};

    // Displacement in internal coordinates
    vec dq_irc_75{0.5, 0.5, 75. / 180. * tools::constants::pi};
    vec dq_irc_89{0.5, 0.5, 89. / 180. * tools::constants::pi};

    // Compute new internal coordinates
    vec q_irc_new{q_irc_old + dq_irc_75};
    CAPTURE(q_irc_new);

    // Allocate vector for cartesian positions
    vec x_c_old{to_cartesian<vec3, vec>(molecule)};

    // Compute new cartesian coordinates
    const auto itc_result_single = irc_to_cartesian_single<vec3, vec, mat>(
        q_irc_old, dq_irc_75, x_c_old, B, A, {}, {}, {});
    CHECK(!itc_result_single.converged);

    const auto itc_result = irc_to_cartesian<vec3, vec, mat>(
        q_irc_old, dq_irc_75, x_c_old, B, A, {}, {}, {});
    CHECK(itc_result.converged);
    const auto x_c = itc_result.x_c;

    const auto itc_result89 = irc_to_cartesian<vec3, vec, mat>(
        q_irc_old, dq_irc_89, x_c_old, B, A, {}, {}, {});
    CHECK(itc_result89.converged);

    // Print cartesian coordinates
    CAPTURE(x_c);

    // Reconstruct atomic positions (points in 3D space)
    vec3 p1{x_c(0), x_c(1), x_c(2)};
    vec3 p2{x_c(3), x_c(4), x_c(5)};
    vec3 p3{x_c(6), x_c(7), x_c(8)};

    SECTION("Bond 1") {
      Approx target{q_irc_new(0)};
      target.margin(1e-4);
      REQUIRE(distance(p1, p2) == target);
    }

    SECTION("Bond 2") {
      Approx target{q_irc_new(1)};
      target.margin(1e-4);
      REQUIRE(distance(p1, p3) == target);
    }

    SECTION("Angle") {
      Approx target{q_irc_new(2)};
      target.margin(1e-4);
      REQUIRE(angle(p2, p1, p3) == target);
    }
  }
}


#pragma once

#define _USE_MATH_DEFINES
#include <pmp/SurfaceMesh.h>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <array>
#include <memory>

namespace algorithm {

	class Deformation
	{
	public:
		// Construct a deformation operator on the given mesh.
		Deformation(pmp::SurfaceMesh& mesh);
		~Deformation();

		// Set the regions for deformations and computes the operator.
		// @param supportVertices The vertices that should be adjusted to minimize some energy functional.
		// @param handleVertices The vertices which are manually moved.
		void set_regions(const std::vector<pmp::Vertex>& supportVertices,
			const std::vector<pmp::Vertex>& handleVertices);

		// Clears the selected regions, making the operator invalid.
		// Does not change the mesh.
		void reset_regions();

		// Reverts the mesh to the state of the last set_regions() call,
		// canceling changes to the handle.
		void reset_handle();

		// Set the energy form to be minimized as order of the operator L^k.
		void set_order(int k);
		int get_order() const { return laplaceOrder_; }

		// Enable area scaling of the Laplace operator to improve results for bad triangulations.
		void set_area_scaling(bool active);
		bool get_area_scaling() const { return useAreaScaling_; }

		// Factor which allows to locally interpolate between the different operators.
		// Only values between [0,k-1] where k is the order of the operator have an effect.
		// Requires that the regions are set.
		void set_smoothness_handle(pmp::Scalar smoothness);
		void set_smoothness_boundary(pmp::Scalar smoothness);

		// The operator is ready for modifications to the handle vertices.
		bool is_set() const;

		// Applies the given transformation to the handle vertices
		// and updates the vertices in the support region.
		void translate(const pmp::Normal& translation);
		void scale(pmp::Scalar scale);
		void rotate(const pmp::Normal& axis, pmp::Scalar angle);

		// recalculates the origin of the scale operator
		void reset_scale_origin();

		// Smoothing for detail preservation.
		// Use implicit smoothing instead of the displacement from the regular operator.
		void use_implict_smoothing(bool use);
		bool is_using_implict_smoothing() const { return useImplicitSmoothing_; }
		// Should details be added to the vertices in the support region.
		void show_details(bool show);
		bool is_showing_details() const { return showDetails_; }
		// Store details in the local frame or search in the neighborhood for the shortest displacement.
		void set_frame_search_depth(int nRing);
		int get_frame_search_depth() const { return searchNearestRing_; }

		// Parameters for implict smoothing.
		void set_smoothing_strength(pmp::Scalar timeStep);
		pmp::Scalar get_smoothing_strength() const { return smoothingTimeStep_; }
		// Same as set_order() but used in the implicit smoothing.
		void set_smoothing_order(int _order);
		int get_smoothing_order() const { return smoothingOrder_; }
	private:
		// matrix types in use
		using MatScalar = double; // scalar type for equation system solving
		using SparseMatrix = Eigen::SparseMatrix<MatScalar>;
		using SparseMatrixR = Eigen::SparseMatrix<MatScalar, Eigen::RowMajor>;
		using DenseMatrix = Eigen::Matrix<MatScalar, Eigen::Dynamic, Eigen::Dynamic>;
		using DiagonalMatrix = Eigen::DiagonalMatrix<MatScalar, Eigen::Dynamic>;
		using Triplet = Eigen::Triplet<MatScalar>;

		// Updates the lowResPositions of the support vertices with the current operator.
		// @param Apply changes to the actual points. Also adds details if showDetails_ == true.
		void update_support_region(bool apply = true);
		void compute_laplace();
		void compute_higher_order();
		// Recompute the operator with the current params and apply resulting changes to the mesh.
		void update_operator();

		// Decomposes the matrix into parts associated with the free and fixed vertices.
		// @param l1 Output target for the free vertices.
		// @param l2 Output target for the fixed vertices.
		void decompose_operator(const SparseMatrixR& lOperator, SparseMatrix& l1, SparseMatrix& l2) const;
		void compute_boundary_set(int ringSize);
		// @return success
		bool compute_affine_frame();

		// Recomputes the current mesh based on the low resolution version and detail offsets.
		void update_details();
		// Takes the current representation in lowResPositions_ and stores differences to the
		// initial points in detailVectors_.
		void store_details();
		// Performs smoothing to the initial support region and updates encoded details.
		void compute_details();
		// Applies implicit smoothing to the support region storing the results in lowResPositions_.
		void implicit_smoothing(pmp::Scalar timeStep);
		// Computes a local frame based on the vertex normal and one edge.
		// @return The normalized orthogonal basis vectors.
		using Basis = std::tuple<pmp::Normal, pmp::Normal, pmp::Normal>;
		Basis local_frame(pmp::Vertex v) const;

		// Functor that wraps an Eigen solver to allow for different implementations
		// in the background.
		class BasicSolver
		{
		public:
			virtual ~BasicSolver() {}

			// Solve Ax = b.
			// @param what Additional context info that is displayed if the solver fails.
			virtual DenseMatrix solve(const DenseMatrix& b, const char* what) = 0;
		};

		template<typename SolverT>
		class Solver : public BasicSolver
		{
		public:
			// Constructs the lhs.
			Solver(const SparseMatrix& A) : solver_(A) {}

			DenseMatrix solve(const DenseMatrix& b, const char* what) override
			{
				const DenseMatrix x = solver_.solve(b);
				if (solver_.info() != Eigen::Success)
					std::cerr << "Deformation: Could not solve linear system for " << what << ".\n";

				return x;
			}
		private:
			SolverT solver_;
		};

		// mesh and modifier regions
		pmp::SurfaceMesh& mesh_;
		std::vector<pmp::Vertex> supportVertices_;
		std::vector<pmp::Vertex> handleVertices_;
		std::vector<pmp::Vertex> boundaryVertices_;

		// additional vertex properties
		enum struct VertexType { None, Support, Handle, Boundary };
		pmp::VertexProperty<VertexType> typeMarks_;
		pmp::VertexProperty<int> idx_;		//< indices for support region
		pmp::VertexProperty<int> meshIdx_;	//< indices for all vertices
		pmp::VertexProperty<pmp::Scalar> smoothness_;
		pmp::VertexProperty<pmp::Normal> detailVectors_; //< details in a local frame of the high resolution mesh
		pmp::VertexProperty<pmp::Point> lowResPositions_; //< positions in the low resolution representation
		pmp::VertexProperty<pmp::Point> initialPositions_; //< original positions at the time of set_regions()
		pmp::VertexProperty<pmp::Point> points_; //< standard point property "v:point" for convenience
		pmp::VertexProperty<Basis> localFrames_;
		pmp::VertexProperty<pmp::Vertex> localFrameIdx_;

		// Laplace operator
		SparseMatrixR laplacian_; //< symmetric laplacian
		SparseMatrix laplace1_; //< free vertices
		SparseMatrix laplace2_; //< fixed vertices
		DiagonalMatrix areaScale_; //< inverse Voronoi area
		DiagonalMatrix areaScale1Inv_; //< scale for only the free vertices
		DiagonalMatrix smoothnessScale_;
		std::unique_ptr<BasicSolver> solver_;
		int laplaceOrder_ = 2;
		bool useAreaScaling_ = true;

		pmp::Scalar smoothnessHandle_ = 2.0;
		pmp::Scalar smoothnessBoundary_ = 2.0;

		// precomputed basis functions
		DenseMatrix localHandle_;
		bool useBasisFunctions_ = true;
		DenseMatrix boundarySolution_;
		DenseMatrix handleBasis_;
		std::array<pmp::Point,4> affineFrame_;

		// Scale handling
		std::vector<pmp::Point> originScaleVertices_;
		std::vector<pmp::Point> originScaleFrame_;
		pmp::Point centerScale_;

		// smoothing related
		bool useImplicitSmoothing_ = false;
		bool showDetails_ = true;
		int searchNearestRing_ = 1;
		int smoothingOrder_ = 2;
		pmp::Scalar smoothingTimeStep_ = 0.001;
	};
}


#include <iostream>
//#include <boost/config.hpp>
template <int N, int M>
struct meta_add_func
{
	//same way
	//BOOST_STATIC_CONSTANT(int, value = N+M);
	//enum {value = N+M};
	static int const value = N+M;
};
int main()
{
	std::cout<<meta_add_func<10,11>::value <<std::endl;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Johannes Goettker-Schnetmann
 Copyright (C) 2015 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/math/functional.hpp>
#include <ql/time/calendars/nullcalendar.hpp>
#include <ql/processes/hestonprocess.hpp>
#include <ql/processes/blackscholesprocess.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/termstructures/volatility/equityfx/blackconstantvol.hpp>
#include <ql/experimental/finitedifferences/bsmrndcalculator.hpp>
#include <ql/experimental/finitedifferences/hestonrndcalculator.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/bind.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

#include <complex>

namespace QuantLib {

namespace {
        struct HestonParams {
            Real v0, kappa, theta, sigma, rho;
        };

        HestonParams getHestonParams(
            const ext::shared_ptr<HestonProcess>& process) {
            const HestonParams p = { process->v0(),    process->kappa(),
                                     process->theta(), process->sigma(),
                                     process->rho() };
            return p;
        }

        std::complex<Real> gamma(const HestonParams& p, Real p_x) {
            return std::complex<Real>(p.kappa, p.rho*p.sigma*p_x);
        }

        std::complex<Real> omega(const HestonParams& p, Real p_x) {
           const std::complex<Real> g = gamma(p, p_x);
           return std::sqrt(g*g
                  + p.sigma*p.sigma*std::complex<Real>(p_x*p_x, -p_x));
        }

        class CpxPv_Helper
            : public std::unary_function<Real, Real > {
          public:
            CpxPv_Helper(const HestonParams& p, Real x, Time t)
              : p_(p), t_(t), x_(x),
                c_inf_(std::min(10.0, std::max(0.0001,
                      std::sqrt(1.0-square<Real>()(p_.rho))/p_.sigma))
                      *(p_.v0 + p_.kappa*p_.theta*t))  {}

            Real operator()(Real x) const {
                return std::real(transformPhi(x));
            }

            Real p0(Real p_x) const {
                if (p_x < QL_EPSILON) {
                    return 0.0;
                }

                const Real u_x = std::max(QL_EPSILON, -std::log(p_x)/c_inf_);
                return std::real(phi(u_x)
                        /((p_x*c_inf_)*std::complex<Real>(0.0, u_x)));
            }

          private:
            std::complex<Real> transformPhi(Real x) const {
                if (x < QL_EPSILON) {
                    return std::complex<Real>(0.0, 0.0);
                }

                const Real u_x = -std::log(x)/c_inf_;
                return phi(u_x)/(x*c_inf_);
            }

            std::complex<Real> phi(Real p_x) const {
                const Real sigma2 = p_.sigma*p_.sigma;
                const std::complex<Real> g = gamma(p_, p_x);
                const std::complex<Real> o = omega(p_, p_x);
                const std::complex<Real> gamma = (g-o)/(g+o);

                return 2.0*std::exp(std::complex<Real>(0.0, p_x*x_)
                        - p_.v0*std::complex<Real>(p_x*p_x, -p_x)
                          /(g+o*(1.0+std::exp(-o*t_))/(1.0-std::exp(-o*t_)))
                         +p_.kappa*p_.theta/sigma2*(
                           (g-o)*t_ - 2.0*std::log((1.0-gamma*std::exp(-o*t_))
                                                               /(1.0-gamma))));
            }

            const HestonParams& p_;
            const Time t_;
            const Real x_, c_inf_;
        };
    }

    HestonRNDCalculator::HestonRNDCalculator(
        const ext::shared_ptr<HestonProcess>& hestonProcess,
        Real integrationEps, Size maxIntegrationIterations)
    : hestonProcess_(hestonProcess),
      x0_(std::log(hestonProcess_->s0()->value())),
      integrationEps_(integrationEps),
      maxIntegrationIterations_(maxIntegrationIterations) { }

    Real HestonRNDCalculator::x_t(Real x, Time t) const {
        const DiscountFactor dr = hestonProcess_->riskFreeRate()->discount(t);
        const DiscountFactor dq = hestonProcess_->dividendYield()->discount(t);

        return x - x0_ + std::log(dr/dq);
    }

    Real HestonRNDCalculator::pdf(Real x, Time t) const {
        return GaussLobattoIntegral(
            maxIntegrationIterations_, 0.1*integrationEps_)(
            CpxPv_Helper(getHestonParams(hestonProcess_), x_t(x, t), t),
            0.0, 1.0)/M_TWOPI;
    }

    Real HestonRNDCalculator::cdf(Real x, Time t) const {
        return GaussLobattoIntegral(
            maxIntegrationIterations_, 0.1*integrationEps_)(
            boost::bind(&CpxPv_Helper::p0,
                CpxPv_Helper(getHestonParams(hestonProcess_), x_t(x,t),t),_1),
            0.0, 1.0)/M_TWOPI + 0.5;

    }
    Real HestonRNDCalculator::invcdf(Real p, Time t) const {
        const Real v0    = hestonProcess_->v0();
        const Real kappa = hestonProcess_->kappa();
        const Real theta = hestonProcess_->theta();

        const Volatility expVol
            = std::sqrt(theta + (v0-theta)*(1-std::exp(-kappa*t))/(t*kappa));

        const ext::shared_ptr<BlackScholesMertonProcess> bsmProcess(
            ext::make_shared<BlackScholesMertonProcess>(
                hestonProcess_->s0(),
                hestonProcess_->dividendYield(),
                hestonProcess_->riskFreeRate(),
                Handle<BlackVolTermStructure>(
                    ext::make_shared<BlackConstantVol>(
                            hestonProcess_->riskFreeRate()->referenceDate(),
                            NullCalendar(),
                            expVol,
                            hestonProcess_->riskFreeRate()->dayCounter()))));

        const Real guess = BSMRNDCalculator(bsmProcess).invcdf(p, t);

        return RiskNeutralDensityCalculator::InvCDFHelper(
            this, guess, 0.1*integrationEps_, maxIntegrationIterations_)
            .inverseCDF(p, t);
    }
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing atan capabilities

    inverse tangent.

    @par Semantic:

    For every parameter of floating type T

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

    Returns the arc @c r in the interval
    \f$[-\pi/2, \pi/2[\f$ such that <tt>tan(r) == x</tt>.
    If @c x is outside \f$[-1, 1[\f$ the result is Nan.

    @par Decorators

    std_, fast_ for floating entries

    @see atan2, atand, atanpi

  **/
  const boost::dispatch::functor<tag::atan_> atan = {};
} }
#endif

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

#endif