//
//  io.hpp
//  maks
//
//  Created by Gabriel Moreira on 09/09/2021.
//  Copyright © 2021 Gabriel Moreira. All rights reserved.
//

#ifndef IO_HPP
#define IO_HPP

#include <Eigen/Dense>

/**
 * Reads g2o file.
 *
 * @param path (input) char*  - g2o file name.
 * @param node_i (output) Eigen::VectorXi - node indices.
 * @param node_r (output) Eigen::MatrixXd - SO(3) node rotations.
 * @param node_t (output) Eigen::MatrixXd - node translations.
 * @param edge_i (output) Eigen::VectorXi - edge indices (from).
 * @param edge_j (output) Eigen::VectorXi - edge indices (to).
 * @param edge_r (output) Eigen::MatrixXd - SO(3) edge rotations.
 * @param edge_t (output) Eigen::MatrixXd - edge translations.
 * @param num_nodes (output) int - number of nodes loaded.
 * @param num_edges (output) int - number of edges loaded.
 */
void readG2O(const char*                 path,
             Eigen::Ref<Eigen::VectorXi> node_i,
             Eigen::Ref<Eigen::MatrixXd> node_r,
             Eigen::Ref<Eigen::MatrixXd> node_t,
             Eigen::Ref<Eigen::VectorXi> edge_i,
             Eigen::Ref<Eigen::VectorXi> edge_j,
             Eigen::Ref<Eigen::MatrixXd> edge_r,
             Eigen::Ref<Eigen::MatrixXd> edge_t,
             int&                        num_nodes,
             int&                        num_edges);

/**
 * Reads edge rotations from g2o file.
 *
 * @param path (input) char*  - g2o file name.
 * @param edge_i (output) Eigen::VectorXi - edge id (from).
 * @param edge_j (output) Eigen::VectorXi - edge id (to).
 * @param edge_r (output) Eigen::MatrixXd - Row block matrix containing SO(3) rotations.
 * @param num_edges (output) int - number of edges.
 */
void readG2O(const char*                 path,
             Eigen::Ref<Eigen::VectorXi> edge_i,
             Eigen::Ref<Eigen::VectorXi> edge_j,
             Eigen::Ref<Eigen::MatrixXd> edge_r,
             int&                        num_edges);

#endif /* IO_HPP */


#include "fieldtrack/CovarianceModels.h"
#include <boost/foreach.hpp>
#include <Eigen/Cholesky>

namespace argus
{
CovarianceModel::CovarianceModel() {}
CovarianceModel::~CovarianceModel() {}

VectorType CovarianceModel::GetParameters() const { return VectorType(); }

void CovarianceModel::SetParameters( const VectorType& p )
{
	if( p.size() > 0 )
	{
		throw std::invalid_argument( "Covariance model has no parameters" );
	}
}

MatrixType CovarianceModel::GetBackpropValue() const { return MatrixType(); }

PassCovariance::PassCovariance() {}

void PassCovariance::Foreprop()
{
	BOOST_FOREACH( ConstantModule & mod, _covs )
	{
		mod.Foreprop();
	}
}

void PassCovariance::Invalidate()
{
	BOOST_FOREACH( ConstantModule & mod, _covs )
	{
		mod.Invalidate();
	}
}

void PassCovariance::UnregisterAll()
{
	// TODO Shift unregistration functionality into ModuleBase destructor
	BOOST_FOREACH( ConstantModule & mod, _covs )
	{
		mod.UnregisterAllConsumers();
	}
	_covs.clear();
}

void PassCovariance::BindPredictModule( PredictModule& pred,
                                        const PredictInfo& info )
{
	_covs.emplace_back();
	ConstantModule& cov = _covs.back();
	cov.SetValue( info.trans_noise_cov );
	link_ports( cov.GetOutput(), pred.GetQIn() );
}

void PassCovariance::BindUpdateModule( UpdateModule& upd,
                                       const UpdateInfo& info )
{
	_covs.emplace_back();
	ConstantModule& cov = _covs.back();
	cov.SetValue( info.obs_noise_cov );
	link_ports( cov.GetOutput(), upd.GetRIn() );
}

FixedCovariance::FixedCovariance()
	: _enableL( true ), _enableD( true )
{
	LinkPorts();
}

FixedCovariance::FixedCovariance( const FixedCovariance& other )
	: _enableL( other._enableL ), _enableD( other._enableD )
{
	LinkPorts();

	MatrixType lBase, dBase;
	std::vector<IndPair> lInds, dInds;
	other._lReshape.GetShapeParams( lBase, lInds );
	other._dReshape.GetShapeParams( dBase, dInds );
	_lReshape.SetShapeParams( lBase, lInds );
	_dReshape.SetShapeParams( dBase, dInds );
	_logD.SetValue( other._logD.GetValue() );
	_lVec.SetValue( other._lVec.GetValue() );
}

void FixedCovariance::Foreprop()
{
	_logD.Foreprop();
	_lVec.Foreprop();
}

void FixedCovariance::Invalidate()
{
	_logD.Invalidate();
	_lVec.Invalidate();
}

void FixedCovariance::UnregisterAll()
{
	_ldlt.UnregisterAllConsumers( false );
}

void FixedCovariance::BindPredictModule( PredictModule& pred,
                                         const PredictInfo& info )
{
	link_ports( _ldlt.GetSOut(), pred.GetQIn() );
}

void FixedCovariance::BindUpdateModule( UpdateModule& upd,
                                        const UpdateInfo& info )
{
	link_ports( _ldlt.GetSOut(), upd.GetRIn() );
}

VectorType FixedCovariance::GetParameters() const
{
	VectorType L = GetL();
	VectorType D = GetLogD();
	VectorType out( L.size() + D.size() );
	out.head( L.size() ) = L;
	out.tail( D.size() ) = D;
	return out;
}

void FixedCovariance::SetParameters( const VectorType& p )
{
	VectorType L = GetL();
	VectorType D = GetLogD();
	if( p.size() != (L.size() + D.size() ) )
	{
		throw std::invalid_argument( "Got invalid sized parameter vector" );
	}
	VectorType Lp = p.head( L.size() );
	VectorType Dp = p.tail( D.size() );
	SetL( Lp );
	SetLogD( Dp );
}

MatrixType FixedCovariance::GetBackpropValue() const
{
	const MatrixType& dL = GetLBackpropValue();
	const MatrixType& dD = GetLogDBackpropValue();
	MatrixType out( dL.rows(), dL.cols() + dD.cols() );
	out.leftCols( dL.cols() ) = dL;
	out.rightCols( dD.cols() ) = dD;
	return out;
}

void FixedCovariance::EnableL( bool enable ) { _enableL = enable; }
void FixedCovariance::EnableD( bool enable ) { _enableD = enable; }

void FixedCovariance::Initialize( const MatrixType& cov )
{
	Eigen::LDLT<MatrixType> ldlt( cov );
	if( ldlt.info() != Eigen::ComputationInfo::Success )
	{
		throw std::invalid_argument( "Input matrix does not appear to be PSD" );
	}

	unsigned int dim = cov.rows();

	std::vector<IndPair> trilInds = gen_trilc_inds( dim, 1 );
	VectorType lInit( trilInds.size() );
	MatrixType matL = ldlt.matrixL();
	for( unsigned int i = 0; i < trilInds.size(); ++i )
	{
		lInit( trilInds[i].first ) = matL( trilInds[i].second );
	}
	_lVec.SetValue( lInit );
	_lReshape.SetShapeParams( MatrixType::Identity( dim, dim ), trilInds );

	std::vector<IndPair> diagInds = gen_vec_to_diag_inds( dim );
	VectorType dInit = (ldlt.transpositionsP().transpose() * ldlt.vectorD()).array().log().matrix();
	_logD.SetValue( dInit );
	_dReshape.SetShapeParams( MatrixType::Zero( dim, dim ), diagInds );
}

void FixedCovariance::LinkPorts()
{
	link_ports( _logD.GetOutput(), _expD.GetInput() );
	link_ports( _expD.GetOutput(), _dReshape.GetInput() );
	link_ports( _dReshape.GetOutput(), _ldlt.GetCIn() );

	link_ports( _lVec.GetOutput(), _lReshape.GetInput() );
	link_ports( _lReshape.GetOutput(), _ldlt.GetXIn() );
}

void FixedCovariance::SetL( const VectorType& L )
{
	if( L.size() != _lVec.GetValue().size() )
	{
		throw std::invalid_argument( "L vector incorrect size" );
	}
	_lVec.SetValue( L );
}

VectorType FixedCovariance::GetL() const
{
	const MatrixType& L = _lVec.GetValue();
	Eigen::Map<const VectorType> l( L.data(), L.size(), 1 );
	return l;
}

void FixedCovariance::SetLogD( const VectorType& D )
{
	if( D.size() != _logD.GetValue().size() )
	{
		throw std::invalid_argument( "D vector incorrect size" );
	}
	_logD.SetValue( D );
}

VectorType FixedCovariance::GetLogD() const
{
	const MatrixType& D = _logD.GetValue();
	Eigen::Map<const VectorType> d( D.data(), D.size(), 1 );
	return d;
}

MatrixType FixedCovariance::GetLBackpropValue() const
{
	MatrixType b = _lVec.GetBackpropValue();
	if( !_enableL ) { b.setZero(); }
	return b;
}

MatrixType FixedCovariance::GetLogDBackpropValue() const
{
	MatrixType b = _logD.GetBackpropValue();
	if( !_enableD ) { b.setZero(); }
	return b;
}

MatrixType FixedCovariance::GetValue() const
{
	// Can you say MEGAHACK? Still, doesn't seem a much cleaner way to do this...
	FixedCovariance copy( *this );
	SinkModule cov;
	link_ports( copy.GetCovOut(), cov.GetInput() );
	copy.Foreprop();
	return cov.GetValue();
}

MatrixType FixedCovariance::GetLValue() const
{
	FixedCovariance copy( *this );
	SinkModule L;
	link_ports( copy._lReshape.GetOutput(), L.GetInput() );
	copy.Foreprop();
	return L.GetValue();
}

VectorType FixedCovariance::GetDValue() const
{
	FixedCovariance copy( *this );
	SinkModule d;
	link_ports( copy._expD.GetOutput(), d.GetInput() );
	copy.Foreprop();
	return d.GetValue();
}

OutputPort& FixedCovariance::GetCovOut()
{
	return _ldlt.GetSOut();
}

TimeScaledCovariance::TimeScaledCovariance() {}

void TimeScaledCovariance::UnregisterAll()
{
	_scalers.clear();
	FixedCovariance::UnregisterAll();
}

void TimeScaledCovariance::BindPredictModule( PredictModule& pred,
                                              const PredictInfo& info )
{
	_scalers.emplace_back();
	ScaleModule& scale = _scalers.back();
	scale.SetScale( info.step_dt );
	link_ports( FixedCovariance::GetCovOut(), scale.GetInput() );
	link_ports( scale.GetOutput(), pred.GetQIn() );
}

void TimeScaledCovariance::BindUpdateModule( UpdateModule& upd,
                                             const UpdateInfo& info )
{
	throw std::runtime_error( "Time scaled covariance does not support update modules" );
}

AdaptiveCovariance::AdaptiveCovariance()
	: _lastUpdate( nullptr ), _windowSize( 1 ) {}

void AdaptiveCovariance::SetWindowSize( unsigned int s )
{
	_windowSize = s;
}

void AdaptiveCovariance::SetDefaultValue( const MatrixType& S )
{
	_defaultCov.SetValue( S );
}

void AdaptiveCovariance::Foreprop()
{
	_defaultCov.Foreprop();
}

void AdaptiveCovariance::Invalidate()
{
	_defaultCov.Invalidate();
}

void AdaptiveCovariance::UnregisterAll()
{
	_defaultCov.UnregisterAllConsumers( false );
	_pointRs.clear();
	_estRs.clear();
	_lastUpdate = nullptr;
}

void AdaptiveCovariance::BindPredictModule( PredictModule& pred,
                                            const PredictInfo& info )
{
	// TODO
	throw std::runtime_error( "Adaptive covariance doesn't support predict yet" );
}

void AdaptiveCovariance::BindUpdateModule( UpdateModule& upd,
                                           const UpdateInfo& info )
{
	if( _lastUpdate )
	{
		_pointRs.emplace_back( *_lastUpdate );
		_estRs.emplace_back();

		MeanModule& currR = _estRs.back();
		// NOTE We have to explicitly cast to int since otherwise we will get large
		// positive numbers instead of negative, as desired
		int lagHead = (int) _pointRs.size() - _windowSize;
		unsigned int minInd = std::max( lagHead, 0 );
		for( unsigned int i = minInd; i < _pointRs.size(); ++i )
		{
			currR.RegisterSource( _pointRs[i].GetOutput() );
		}
		_lastUpdate = nullptr;
	}

	if( _estRs.empty() )
	{
		link_ports( _defaultCov.GetOutput(), upd.GetRIn() );
	}
	else
	{
		link_ports( _estRs.back().GetOutput(), upd.GetRIn() );
	}
	_lastUpdate = &upd;
}

AdaptiveCovariance::PointEstimate::PointEstimate( UpdateModule& upd )
{
	const VectorType& y = upd.GetObs();
	unsigned int dim = y.size();

	link_ports( upd.GetUOut(), _uOp.GetInput() );
	link_ports( _uOp.GetOutput(), _denseR.GetLeftIn() );
	link_ports( upd.GetPOut(), _CPCT.GetCIn() );
	_CPCT.SetX( upd.GetObsMatrix().transpose() );
	link_ports( _CPCT.GetSOut(), _denseR.GetRightIn() );
	_diagR.SetShapeParams( MatrixType::Zero( dim, dim ),
	                       gen_dense_to_diag_inds( dim ) );
	link_ports( _denseR.GetOutput(), _diagR.GetInput() );
}

OutputPort& AdaptiveCovariance::PointEstimate::GetOutput()
{
	return _diagR.GetOutput();
}
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2019, LAAS-CNRS, New York University, Max Planck Gesellshaft
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API
#include <boost/test/included/unit_test.hpp>
#include <boost/bind.hpp>
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/states/euclidean.hpp"
#include "crocoddyl/core/states/unicycle.hpp"
#include "crocoddyl/core/numdiff/state.hpp"
#include <Eigen/Dense>

using namespace boost::unit_test;

//____________________________________________________________________________//

void test_state_dimension(crocoddyl::StateAbstract& state, int nx) {
  // Checking the dimension of zero and random states
  BOOST_CHECK(state.zero().size() == nx);
  BOOST_CHECK(state.rand().size() == nx);
}

//____________________________________________________________________________//

void test_integrate_against_difference(crocoddyl::StateAbstract& state) {
  // Generating random states
  Eigen::VectorXd x1 = state.rand();
  Eigen::VectorXd x2 = state.rand();

  // Computing x2 by integrating its difference
  Eigen::VectorXd dx(state.get_ndx());
  state.diff(x1, x2, dx);
  Eigen::VectorXd x2i(state.get_nx());
  state.integrate(x1, dx, x2i);

  Eigen::VectorXd dxi(state.get_ndx());
  state.diff(x2i, x2, dxi);

  // Checking that both states agree
  BOOST_CHECK(dxi.isMuchSmallerThan(1.0, 1e-9));
}

//____________________________________________________________________________//

void test_difference_against_integrate(crocoddyl::StateAbstract& state) {
  // Generating random states
  Eigen::VectorXd x = state.rand();
  Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());

  // Computing dx by differentiation of its integrate
  Eigen::VectorXd xidx(state.get_nx());
  state.integrate(x, dx, xidx);
  Eigen::VectorXd dxd(state.get_ndx());
  state.diff(x, xidx, dxd);

  // Checking that both states agree
  BOOST_CHECK((dxd - dx).isMuchSmallerThan(1.0, 1e-9));
}

//____________________________________________________________________________//

void test_Jdiff_firstsecond(crocoddyl::StateAbstract& state) {
  // Generating random values for the initial and terminal states
  Eigen::VectorXd x1 = state.rand();
  Eigen::VectorXd x2 = state.rand();

  // Computing the partial derivatives of the difference function separately
  Eigen::MatrixXd Jdiff_tmp(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jdiff_first(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jdiff_second(state.get_ndx(), state.get_ndx());
  state.Jdiff(x1, x2, Jdiff_first, Jdiff_tmp, crocoddyl::first);
  state.Jdiff(x1, x2, Jdiff_tmp, Jdiff_second, crocoddyl::second);

  // Computing the partial derivatives of the difference function separately
  Eigen::MatrixXd Jdiff_both_first(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jdiff_both_second(state.get_ndx(), state.get_ndx());
  state.Jdiff(x1, x2, Jdiff_both_first, Jdiff_both_second);

  BOOST_CHECK((Jdiff_first - Jdiff_both_first).isMuchSmallerThan(1.0, 1e-9));
  BOOST_CHECK((Jdiff_second - Jdiff_both_second).isMuchSmallerThan(1.0, 1e-9));
}

//____________________________________________________________________________//

void test_Jint_firstsecond(crocoddyl::StateAbstract& state) {
  // Generating random values for the initial and terminal states
  Eigen::VectorXd x = state.rand();
  Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());

  // Computing the partial derivatives of the difference function separately
  Eigen::MatrixXd Jint_tmp(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jint_first(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jint_second(state.get_ndx(), state.get_ndx());
  state.Jintegrate(x, dx, Jint_first, Jint_tmp, crocoddyl::first);
  state.Jintegrate(x, dx, Jint_tmp, Jint_second, crocoddyl::second);

  // Computing the partial derivatives of the interence function separately
  Eigen::MatrixXd Jint_both_first(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jint_both_second(state.get_ndx(), state.get_ndx());
  state.Jintegrate(x, dx, Jint_both_first, Jint_both_second);

  BOOST_CHECK((Jint_first - Jint_both_first).isMuchSmallerThan(1.0, 1e-9));
  BOOST_CHECK((Jint_second - Jint_both_second).isMuchSmallerThan(1.0, 1e-9));
}

//____________________________________________________________________________//

void test_Jdiff_num_diff_firstsecond(crocoddyl::StateAbstract& state) {
  // Generating random values for the initial and terminal states
  Eigen::VectorXd x1 = state.rand();
  Eigen::VectorXd x2 = state.rand();

  // Get the num diff state
  crocoddyl::StateNumDiff state_num_diff(state);

  // Computing the partial derivatives of the difference function separately
  Eigen::MatrixXd Jdiff_num_diff_tmp(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jdiff_num_diff_first(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jdiff_num_diff_second(state.get_ndx(), state.get_ndx());
  state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_first, Jdiff_num_diff_tmp, crocoddyl::first);
  state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_tmp, Jdiff_num_diff_second, crocoddyl::second);

  // Computing the partial derivatives of the difference function separately
  Eigen::MatrixXd Jdiff_num_diff_both_first(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jdiff_num_diff_both_second(state.get_ndx(), state.get_ndx());
  state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_both_first, Jdiff_num_diff_both_second);

  BOOST_CHECK((Jdiff_num_diff_first - Jdiff_num_diff_both_first).isMuchSmallerThan(1.0, 1e-9));
  BOOST_CHECK((Jdiff_num_diff_second - Jdiff_num_diff_both_second).isMuchSmallerThan(1.0, 1e-9));
}

//____________________________________________________________________________//

void test_Jint_num_diff_firstsecond(crocoddyl::StateAbstract& state) {
  // Generating random values for the initial and terminal states
  Eigen::VectorXd x = state.rand();
  Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());

  // Get the num diff state
  crocoddyl::StateNumDiff state_num_diff(state);

  // Computing the partial derivatives of the difference function separately
  Eigen::MatrixXd Jint_num_diff_tmp(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jint_num_diff_first(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jint_num_diff_second(state.get_ndx(), state.get_ndx());
  state_num_diff.Jintegrate(x, dx, Jint_num_diff_first, Jint_num_diff_tmp, crocoddyl::first);
  state_num_diff.Jintegrate(x, dx, Jint_num_diff_tmp, Jint_num_diff_second, crocoddyl::second);

  // Computing the partial derivatives of the interence function separately
  Eigen::MatrixXd Jint_num_diff_both_first(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jint_num_diff_both_second(state.get_ndx(), state.get_ndx());
  state_num_diff.Jintegrate(x, dx, Jint_num_diff_both_first, Jint_num_diff_both_second);

  BOOST_CHECK((Jint_num_diff_first - Jint_num_diff_both_first).isMuchSmallerThan(1.0, 1e-9));
  BOOST_CHECK((Jint_num_diff_second - Jint_num_diff_both_second).isMuchSmallerThan(1.0, 1e-9));
}

//____________________________________________________________________________//

void test_Jdiff_against_numdiff(crocoddyl::StateAbstract& state, double num_diff_modifier) {
  // Generating random values for the initial and terminal states
  Eigen::VectorXd x1 = state.rand();
  Eigen::VectorXd x2 = state.rand();

  // Computing the partial derivatives of the difference function analytically
  Eigen::MatrixXd Jdiff_1(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jdiff_2(state.get_ndx(), state.get_ndx());
  state.Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::first);
  state.Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::second);

  // Computing the partial derivatives of the difference function numerically
  crocoddyl::StateNumDiff state_num_diff(state);
  Eigen::MatrixXd Jdiff_num_1(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jdiff_num_2(state.get_ndx(), state.get_ndx());
  state_num_diff.Jdiff(x1, x2, Jdiff_num_1, Jdiff_num_2);

  // Checking the partial derivatives against NumDiff
  // The previous tolerance was 10*disturbance
  double tol = num_diff_modifier * state_num_diff.get_disturbance();
  BOOST_CHECK((Jdiff_1 - Jdiff_num_1).isMuchSmallerThan(1.0, tol));
  BOOST_CHECK((Jdiff_2 - Jdiff_num_2).isMuchSmallerThan(1.0, tol));
}

//____________________________________________________________________________//

void test_Jintegrate_against_numdiff(crocoddyl::StateAbstract& state, double num_diff_modifier) {
  // Generating random values for the initial state and its rate of change
  Eigen::VectorXd x = state.rand();
  Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());

  // Computing the partial derivatives of the difference function analytically
  Eigen::MatrixXd Jint_1(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jint_2(state.get_ndx(), state.get_ndx());
  state.Jintegrate(x, dx, Jint_1, Jint_2);

  // Computing the partial derivatives of the difference function numerically
  crocoddyl::StateNumDiff state_num_diff(state);
  Eigen::MatrixXd Jint_num_1(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jint_num_2(state.get_ndx(), state.get_ndx());
  state_num_diff.Jintegrate(x, dx, Jint_num_1, Jint_num_2);

  // Checking the partial derivatives against NumDiff
  // The previous tolerance was 10*disturbance
  double tol = num_diff_modifier * state_num_diff.get_disturbance();
  BOOST_CHECK((Jint_1 - Jint_num_1).isMuchSmallerThan(1.0, tol));
  BOOST_CHECK((Jint_2 - Jint_num_2).isMuchSmallerThan(1.0, tol));
}

//____________________________________________________________________________//

void test_Jdiff_and_Jintegrate_are_inverses(crocoddyl::StateAbstract& state) {
  // Generating random states
  Eigen::VectorXd x1 = state.rand();
  Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());
  Eigen::VectorXd x2(state.get_nx());
  state.integrate(x1, dx, x2);

  // Computing the partial derivatives of the integrate and difference function
  Eigen::MatrixXd Jx(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jdx(state.get_ndx(), state.get_ndx());
  state.Jintegrate(x1, dx, Jx, Jdx);
  Eigen::MatrixXd J1(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd J2(state.get_ndx(), state.get_ndx());
  state.Jdiff(x1, x2, J1, J2);

  // Checking that Jdiff and Jintegrate are inverses
  Eigen::MatrixXd dX_dDX = Jdx;
  Eigen::MatrixXd dDX_dX = J2;
  BOOST_CHECK((dX_dDX - dDX_dX.inverse()).isMuchSmallerThan(1.0, 1e-9));
}

//____________________________________________________________________________//

void test_velocity_from_Jintegrate_Jdiff(crocoddyl::StateAbstract& state) {
  // Generating random states
  Eigen::VectorXd x1 = state.rand();
  Eigen::VectorXd dx = Eigen::VectorXd::Random(state.get_ndx());
  Eigen::VectorXd x2(state.get_nx());
  state.integrate(x1, dx, x2);
  Eigen::VectorXd eps = Eigen::VectorXd::Random(state.get_ndx());
  double h = 1e-8;

  // Computing the partial derivatives of the integrate and difference function
  Eigen::MatrixXd Jx(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd Jdx(state.get_ndx(), state.get_ndx());
  state.Jintegrate(x1, dx, Jx, Jdx);
  Eigen::MatrixXd J1(state.get_ndx(), state.get_ndx());
  Eigen::MatrixXd J2(state.get_ndx(), state.get_ndx());
  state.Jdiff(x1, x2, J1, J2);

  // Checking that computed velocity from Jintegrate
  Eigen::MatrixXd dX_dDX = Jdx;
  Eigen::VectorXd x2eps(state.get_nx());
  state.integrate(x1, dx + eps * h, x2eps);
  Eigen::VectorXd x2_eps(state.get_nx());
  state.diff(x2, x2eps, x2_eps);
  BOOST_CHECK((dX_dDX * eps - x2_eps / h).isMuchSmallerThan(1.0, 1e-3));

  // Checking the velocity computed from Jdiff
  Eigen::VectorXd x = state.rand();
  dx.setZero();
  state.diff(x1, x, dx);
  Eigen::VectorXd x2i(state.get_nx());
  state.integrate(x, eps * h, x2i);
  Eigen::VectorXd dxi(state.get_ndx());
  state.diff(x1, x2i, dxi);
  J1.setZero();
  J2.setZero();
  state.Jdiff(x1, x, J1, J2);
  BOOST_CHECK((J2 * eps - (-dx + dxi) / h).isMuchSmallerThan(1.0, 1e-3));
}

//____________________________________________________________________________//

void register_state_vector_unit_tests() {
  int nx = 10;
  double num_diff_modifier = 1e4;

  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_state_dimension, crocoddyl::StateVector(nx), nx)));

  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_integrate_against_difference, crocoddyl::StateVector(nx))));

  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_difference_against_integrate, crocoddyl::StateVector(nx))));

  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, crocoddyl::StateVector(nx))));

  framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_Jint_firstsecond, crocoddyl::StateVector(nx))));

  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_Jdiff_num_diff_firstsecond, crocoddyl::StateVector(nx))));

  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_Jint_num_diff_firstsecond, crocoddyl::StateVector(nx))));

  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_Jdiff_against_numdiff, crocoddyl::StateVector(nx), num_diff_modifier)));

  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_Jintegrate_against_numdiff, crocoddyl::StateVector(nx), num_diff_modifier)));

  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, crocoddyl::StateVector(nx))));

  framework::master_test_suite().add(
      BOOST_TEST_CASE(boost::bind(&test_velocity_from_Jintegrate_Jdiff, crocoddyl::StateVector(nx))));
}

//____________________________________________________________________________//

bool init_function() {
  // Here we test the state_vector
  register_state_vector_unit_tests();
  return true;
}

//____________________________________________________________________________//

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


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_OCEA_HPP
#define BOOST_GEOMETRY_PROJECTIONS_OCEA_HPP

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct ocea {}; // Oblique Cylindrical Equal Area

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace ocea
    {
            template <typename T>
            struct par_ocea
            {
                T    rok;
                T    rtk;
                T    sinphi;
                T    cosphi;
                T    singam;
                T    cosgam;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_ocea_spheroid
                : public base_t_fi<base_ocea_spheroid<T, Parameters>, T, Parameters>
            {
                par_ocea<T> m_proj_parm;

                inline base_ocea_spheroid(const Parameters& par)
                    : base_t_fi<base_ocea_spheroid<T, Parameters>,
                     T, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T pi = detail::pi<T>();

                    T t;

                    xy_y = sin(lp_lon);
                    t = cos(lp_lon);
                    xy_x = atan((tan(lp_lat) * this->m_proj_parm.cosphi + this->m_proj_parm.sinphi * xy_y) / t);
                    if (t < 0.)
                        xy_x += pi;
                    xy_x *= this->m_proj_parm.rtk;
                    xy_y = this->m_proj_parm.rok * (this->m_proj_parm.sinphi * sin(lp_lat) - this->m_proj_parm.cosphi * cos(lp_lat) * xy_y);
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    T t, s;

                    xy_y /= this->m_proj_parm.rok;
                    xy_x /= this->m_proj_parm.rtk;
                    t = sqrt(1. - xy_y * xy_y);
                    lp_lat = asin(xy_y * this->m_proj_parm.sinphi + t * this->m_proj_parm.cosphi * (s = sin(xy_x)));
                    lp_lon = atan2(t * this->m_proj_parm.sinphi * s - xy_y * this->m_proj_parm.cosphi,
                        t * cos(xy_x));
                }

                static inline std::string get_name()
                {
                    return "ocea_spheroid";
                }

            };

            // Oblique Cylindrical Equal Area
            template <typename Parameters, typename T>
            inline void setup_ocea(Parameters& par, par_ocea<T>& proj_parm)
            {
                static const T half_pi = detail::half_pi<T>();

                T phi_0=0.0, phi_1, phi_2, lam_1, lam_2, lonz, alpha;

                proj_parm.rok = 1. / par.k0;
                proj_parm.rtk = par.k0;
                /*If the keyword "alpha" is found in the sentence then use 1point+1azimuth*/
                if ( pj_param_r(par.params, "alpha", alpha)) {
                    /*Define Pole of oblique transformation from 1 point & 1 azimuth*/
                    //alpha = pj_get_param_r(par.params, "alpha"); // set above
                    lonz = pj_get_param_r(par.params, "lonc");
                    /*Equation 9-8 page 80 (http://pubs.usgs.gov/pp/1395/report.pdf)*/
                    proj_parm.singam = atan(-cos(alpha)/(-sin(phi_0) * sin(alpha))) + lonz;
                    /*Equation 9-7 page 80 (http://pubs.usgs.gov/pp/1395/report.pdf)*/
                    proj_parm.sinphi = asin(cos(phi_0) * sin(alpha));
                /*If the keyword "alpha" is NOT found in the sentence then use 2points*/
                } else {
                    /*Define Pole of oblique transformation from 2 points*/
                    phi_1 = pj_get_param_r(par.params, "lat_1");
                    phi_2 = pj_get_param_r(par.params, "lat_2");
                    lam_1 = pj_get_param_r(par.params, "lon_1");
                    lam_2 = pj_get_param_r(par.params, "lon_2");
                    /*Equation 9-1 page 80 (http://pubs.usgs.gov/pp/1395/report.pdf)*/
                    proj_parm.singam = atan2(cos(phi_1) * sin(phi_2) * cos(lam_1) -
                        sin(phi_1) * cos(phi_2) * cos(lam_2),
                        sin(phi_1) * cos(phi_2) * sin(lam_2) -
                        cos(phi_1) * sin(phi_2) * sin(lam_1) );

                    /* take care of P->lam0 wrap-around when +lam_1=-90*/
                    if (lam_1 == -half_pi)
                        proj_parm.singam = -proj_parm.singam;

                    /*Equation 9-2 page 80 (http://pubs.usgs.gov/pp/1395/report.pdf)*/
                    proj_parm.sinphi = atan(-cos(proj_parm.singam - lam_1) / tan(phi_1));
                }
                par.lam0 = proj_parm.singam + half_pi;
                proj_parm.cosphi = cos(proj_parm.sinphi);
                proj_parm.sinphi = sin(proj_parm.sinphi);
                proj_parm.cosgam = cos(proj_parm.singam);
                proj_parm.singam = sin(proj_parm.singam);
                par.es = 0.;
            }

    }} // namespace detail::ocea
    #endif // doxygen

    /*!
        \brief Oblique Cylindrical Equal Area projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
        \par Projection parameters
         - lonc: Longitude (only used if alpha (or gamma) is specified) (degrees)
         - alpha: Alpha (degrees)
         - lat_1: Latitude of first standard parallel (degrees)
         - lat_2: Latitude of second standard parallel (degrees)
         - lon_1 (degrees)
         - lon_2 (degrees)
        \par Example
        \image html ex_ocea.gif
    */
    template <typename T, typename Parameters>
    struct ocea_spheroid : public detail::ocea::base_ocea_spheroid<T, Parameters>
    {
        inline ocea_spheroid(const Parameters& par) : detail::ocea::base_ocea_spheroid<T, Parameters>(par)
        {
            detail::ocea::setup_ocea(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::ocea, ocea_spheroid, ocea_spheroid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class ocea_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<ocea_spheroid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void ocea_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("ocea", new ocea_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_OCEA_HPP


#include <queue>

#include <boost/multiprecision/integer.hpp>
//#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/functional/hash.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <fc/smart_ref_impl.hpp>
#include <fc/uint128.hpp>

#include <graphene/chain/database.hpp>

namespace graphene { namespace chain {

using transfer_graph_type = std::unordered_multimap<uint64_t, uint64_t>;
using node_type = transfer_graph_type::key_type;

using matrix_type = boost::numeric::ublas::matrix<double>;
using outlink_matrix_type = matrix_type;
//using outlink_matrix_type = boost::numeric::ublas::compressed_matrix<double>;
//using outlink_matrix_type = std::unordered_map<uint64_t, std::unordered_map<uint64_t, double>>;

/*struct get_transfer_accounts_visitor
{
   struct tranfser_accounts_type
   {
      account_id_type from;
      account_id_type to;
      asset amount;
   };

   typedef fc::optional<tranfser_accounts_type> result_type;

   result_type operator()( const transfer_operation& op )
   {

      return result_type{tranfser_accounts_type{op.from, op.to, op.amount}};
   }

   template <typename Operation>
   result_type operator()( const Operation& op)
   {
      return result_type{};
   }
};*/

outlink_matrix_type database::construct_outlink_matrix(fc::time_point_sec last_maintenance, std::unordered_map<uint64_t, uint64_t>& accounts_map)//, std::unordered_map<object_id_type, uint32_t>& account_transfers)
{
   /*auto transaction_expiration = get_global_properties().parameters.maximum_time_until_expiration;
   if (last_maintenance > head_block_time() - transaction_expiration) {
      transfer_graph_type transfer_graph;

      const auto& all_transactions = get_index_type<transaction_index>().indices().get<by_trx_id>();
      for (const auto& tr : all_transactions) {
         for ( const auto& op : tr.trx.operations) {
            get_transfer_accounts_visitor vtor;
            auto account_pair = op.visit(vtor);
            if (account_pair.valid()) {
               account_id_type from {account_pair->from};
               account_id_type to   {account_pair->to};

               //std::cout << from(*this).name << " : " << uint64_t(from) << " -> " << to(*this).name << '\n';

               auto pair = std::pair<const object_id_type, object_id_type>(to, from); //not a bug
               if (std::find(transfer_graph.cbegin(), transfer_graph.cend(), pair) == transfer_graph.cend())
                  transfer_graph.insert(std::move(pair));

               ++account_transfers[object_id_type{from}];
            }
         }
      }
      return transfer_graph;
   }*/

   auto optional_block = fetch_block_by_id(head_block_id());

   std::unordered_map<uint64_t, std::unordered_map<uint64_t, double>> weight_matrix;
   //std::unordered_map<uint64_t, std::unordered_map<uint64_t, double>> outlink_matrix;

   std::unordered_set<uint64_t> accounts_set;

   size_t blocks = 0;
   auto max_blocks = get_global_properties().parameters.importance_score_block_count;
   asset min_transfer_amount = get_global_properties().parameters.min_transfer_for_importance;
   auto min_balance_amount = get_global_properties().parameters.min_balance_for_importance;

    const double log09 = std::log(0.9);
    const uint32_t blocks_per_day = 60 * 60 * 24 / get_global_properties().parameters.block_interval;

    uint32_t head_height = head_block_num();
    uint32_t block_height = head_height;

   while(optional_block.valid() && (blocks++ < max_blocks)) {
      auto& block = *optional_block;

      for (const auto& tr : block.transactions) {
         for ( const auto& op : tr.operations) {
            get_transfer_accounts_visitor vtor;
            auto transfer_object = op.visit(vtor);
            if (transfer_object.valid() && transfer_object->amount > min_transfer_amount) {
               uint64_t from {object_id_type(transfer_object->from).instance()};
               uint64_t to   {object_id_type(transfer_object->to).instance()};

               auto check_balance = [&](uint64_t acc) -> bool {
                  account_id_type acc_id(acc);
                  auto current_balance = get_balance(acc_id, asset_id_type()).amount.value;
                  return current_balance > min_balance_amount;
               };

               if (check_balance(from) && check_balance(to)) {
                  int64_t floor = std::floor((head_height - block_height) * 1. / blocks_per_day);
                  double weight = transfer_object->amount.amount.value * std::exp(log09 * floor);

                  //std::cout << from(*this).name << " : " << uint64_t(from) << " -> " << to(*this).name << '\n';
                  //auto pair = std::pair<const uint64_t, uint64_t>(from.instance.value, to.instance.instance.value);

                  weight_matrix[to][from] += weight;

                  accounts_set.insert(from);
                  accounts_set.insert(to);
               }
            }
         }
      }

     optional_block = fetch_block_by_id(block.previous);
     block_height = graphene::chain::block_header::num_from_id(block.previous);
   }

   std::vector<uint64_t> accounts(accounts_set.begin(), accounts_set.end());
   std::sort(accounts.begin(), accounts.end());
   // map from account number to continuous index
   //std::unordered_map<uint64_t, uint64_t> accounts_map;
   for (size_t i = 0; i < accounts.size(); ++i)
      accounts_map[accounts.at(i)] = i;

   outlink_matrix_type result(accounts.size(), accounts.size(), 0.);
   for (auto it = weight_matrix.begin(), end = weight_matrix.end(); it != end; ++it) {

      size_t row_size = it->second.size();

      for (const auto& tx : it->second) {
            uint64_t to = it->first, from = tx.first;
            double first_weight = tx.second, second_weight = 0;
            auto from_it = weight_matrix.find(from);
            if (from_it != weight_matrix.end()) {
                  auto to_it = from_it->second.find(to);
                  if (to_it != from_it->second.end())
                        second_weight = to_it->second;
            }
      /*for (const auto& tx : it->second) {
            uint64_t from = it->first, to = tx.first;
            double first_weight = tx.second, second_weight = 0;
            auto to_it = weight_matrix.find(to);
            if (to_it != weight_matrix.end()) {
                  auto from_it = to_it->second.find(from);
                  if (from_it != to_it->second.end())
                        second_weight = from_it->second;
            }*/

            double outlink = first_weight - second_weight;
            //double outlink = second_weight - first_weight;
            //outlink_matrix[to][from] = outlink > 0 ? outlink : 0;
            result.insert_element(accounts_map.at(from), accounts_map.at(to), outlink > 0 ? outlink : 0);
      }
   }

   //std::cerr << "Res: " << result << "\n!!!\n";

   /*using boost::numeric::ublas::norm_1;
   for (size_t j = 0; j < result.size2(); ++j) {
         boost::numeric::ublas::matrix_column<outlink_matrix_type> col(result, j);
         double norm = norm_1(col);
         if (norm > 0)
            for (size_t i = 0; i < col.size(); ++i)
                  col(i) = col(i) / norm;
   }*/

   return result;
}

std::set<node_type> get_gamma(const transfer_graph_type& transfer_graph, const node_type& base_node)
{
      std::set<node_type> gamma{base_node};
      size_t bucket_num = transfer_graph.bucket(base_node);
      auto it = transfer_graph.cbegin(bucket_num);
      for (; it != transfer_graph.cend(bucket_num); ++it)
            gamma.insert(it->second);
      return gamma;
};

fc::optional<std::unordered_set<node_type>> get_core(const transfer_graph_type& transfer_graph, node_type node,
                                                      double epsilon, uint32_t mu)
{
      //size_t bucket_num = transfer_graph.bucket(node);
      //auto it = transfer_graph.cbegin(bucket_num);

      //if (transfer_graph.bucket_size(bucket_num) < mu)
      //      return {};

      std::set<node_type> first = get_gamma(transfer_graph, node);
      if (first.size() < mu)
            return {};

      std::unordered_set<node_type> core;
      auto it = first.cbegin();
      //for (; it != transfer_graph.cend(bucket_num); ++it) {
      for (; it != first.cend(); ++it) {
            std::set<node_type> second = get_gamma(transfer_graph, *it);
            std::set<node_type> intersection;
            std::set_intersection(first.cbegin(), first.cend(), second.cbegin(), second.cend(),
                        std::inserter(intersection, intersection.begin()));
            double similarity = intersection.size() / std::sqrt(first.size() * second.size());

            /*std::cerr << node << " <-> " << *it << " : " <<
                  intersection.size() << " / √(" << first.size() << " * " << second.size() << ") = " <<
                  similarity << std::endl;*/

            if (similarity >= epsilon)
                  core.insert(*it);
      }
      if (core.size() >= mu) return core;
      return {};
      //return (sum > mu) ? core : {};
};

#if 0
std::unordered_map<node_type, uint32_t> database::detail::cluster_graph_simple(const transfer_graph_type& transfer_graph, double epsilon, uint32_t mu)
{
      std::unordered_map<node_type, uint32_t> node_ids;
      const uint32_t non_member_id = 0;
      const uint32_t outlier_id = 1;
      const uint32_t hub_id = 2;
      uint32_t last_cluster = 2;

      auto skip_same = [](transfer_graph_type::const_iterator it, transfer_graph_type::const_iterator end) {
            transfer_graph_type::const_iterator last = it++;
            for(; it != end && it->first == last->first; last = it++);
            return it;
      };

      auto it = transfer_graph.begin();
      for(; it != transfer_graph.end(); it = skip_same(it, transfer_graph.end())) {
            //std::cerr << it->first.instance() << " ";
            const auto& node = it->first;
            if (node_ids.find(node) != node_ids.end())
                  continue;

            std::queue<node_type> queue;
            auto core_optional = get_core(transfer_graph, node, epsilon, mu);
            if (core_optional.valid()) {
                  last_cluster++;
                  node_ids[node] = last_cluster;
                  for (const auto& neighbour : *core_optional) {
                        /*auto neighbour_id = node_ids.find(neighbour);
                        if (neighbour_id == node_ids.end()) {
                              node_ids[neighbour] = last_cluster;
                        }*/
                        queue.push(neighbour);
                  }
                  while(!queue.empty()) {
                        auto front = queue.front();
                        queue.pop();
                        auto front_core_optional = get_core(transfer_graph, front, epsilon, mu);
                        if (front_core_optional.valid()) {
                              for (const auto& reachable_node : *front_core_optional) {
                                    auto reachable_id = node_ids.find(reachable_node);
                                    if (reachable_id == node_ids.end()) {
                                          node_ids[reachable_node] = last_cluster;
                                          queue.push(reachable_node);
                                    }
                                    else if (reachable_id->second == non_member_id)
                                          reachable_id->second = last_cluster;
                              }
                        }
                  }
            }
            else {
                  node_ids[node] = non_member_id;
            }
      }
      std::cerr << "!\n";
      for (auto& id : node_ids) {
      //it = transfer_graph.begin();
      //for(; it != transfer_graph.end(); it = skip_same(it, transfer_graph.end())) {
            //std::cerr << id.first.instance() << ' ';
            //auto id = std::make_pair(it->first, node_ids.at(it->first));
            if (id.second == non_member_id) {
                  auto gamma = get_gamma(transfer_graph, id.first);
                  /*bool is_hub = std::inner_product(gamma.begin(), gamma.end(), gamma.begin(), false, std::logical_or<bool>{},
                        [&](const decltype(*gamma.begin())& lhs, const decltype(*gamma.begin())& rhs) {
                              return node_ids.at(lhs) != node_ids.at(rhs);
                  });*/
                  bool is_hub = false;
                  auto g_it1 = gamma.cbegin();
                  auto g_end = gamma.cend();
                  std::cerr << id.first << " -> " << std::flush;
                  for (const auto& gnode : gamma)
                        std::cerr << gnode << ' ' << std::flush;
                  for (; g_it1 != g_end && !is_hub; ++g_it1) {
                        for (auto g_it2 = std::next(g_it1); g_it2 != g_end && !is_hub; ++g_it2) {
                              auto id1 = node_ids.at(*g_it1);
                              auto id2 = node_ids.at(*g_it2);
                              if (id1 > hub_id && id2 > hub_id && id1 != id2)
                                    is_hub = true;
                        }
                  }
                  std::cerr << "!\n";
                  id.second = is_hub ? hub_id : outlier_id;
            }
      }
      std::cerr << "!\n";
      return node_ids;
}
#endif

const uint32_t MIN_CLUSTER = 2;

std::unordered_map<node_type, uint32_t> detail::cluster_graph_simple(const outlink_matrix_type& outlink_matrix, const std::unordered_map<uint64_t, uint64_t>& accounts_map, double epsilon, uint32_t mu, share_type min_clustering_transfer)
{
      std::unordered_map<node_type, uint32_t> node_ids;
      //const uint32_t non_member_id = 0;
      const uint32_t non_member_id = std::numeric_limits<uint32_t>::max();
      const uint32_t outlier_id = 0;
      const uint32_t hub_id = 1;
      uint32_t last_cluster = 1;

      //auto min_clustering_transfer = get_global_properties().parameters.min_transfer_for_clustering;
      transfer_graph_type transfer_graph;
      for (size_t i = 0; i < outlink_matrix.size1(); ++i) {
            for (size_t j = i + 1; j < outlink_matrix.size2(); ++j)
                  if (outlink_matrix(i,j) + outlink_matrix(j,i) > 0) {//min_clustering_transfer) {
                        transfer_graph.emplace(i, j);
                        transfer_graph.emplace(j, i);
                  }
      }

      auto skip_same = [](transfer_graph_type::const_iterator it, transfer_graph_type::const_iterator end) {
            transfer_graph_type::const_iterator last = it++;
            for(; it != end && it->first == last->first; last = it++);
            return it;
      };

      for(const auto& p: accounts_map) {
            const auto node = p.second;
            if (node_ids.find(node) != node_ids.end())
                  continue;

            std::queue<node_type> queue;
            auto core_optional = get_core(transfer_graph, node, epsilon, mu);
            if (core_optional.valid()) {
                  last_cluster++;
                  node_ids[node] = last_cluster;
                  for (const auto& neighbour : *core_optional) {
                        queue.push(neighbour);
                  }
                  while(!queue.empty()) {
                        auto front = queue.front();
                        queue.pop();
                        auto front_core_optional = get_core(transfer_graph, front, epsilon, mu);
                        if (front_core_optional.valid()) {
                              for (const auto& reachable_node : *front_core_optional) {
                                    auto reachable_id = node_ids.find(reachable_node);
                                    if (reachable_id == node_ids.end()) {
                                          node_ids[reachable_node] = last_cluster;
                                          queue.push(reachable_node);
                                    }
                                    else if (reachable_id->second == non_member_id)
                                          reachable_id->second = last_cluster;
                              }
                        }
                  }
            }
            else {
                  node_ids[node] = non_member_id;
            }
      }
      for (auto& id : node_ids) {
            if (id.second == non_member_id) {
                  auto gamma = get_gamma(transfer_graph, id.first);
                  /*bool is_hub = std::inner_product(gamma.begin(), gamma.end(), gamma.begin(), false, std::logical_or<bool>{},
                        [&](const decltype(*gamma.begin())& lhs, const decltype(*gamma.begin())& rhs) {
                              return node_ids.at(lhs) != node_ids.at(rhs);
                  });*/
                  bool is_hub = false;
                  auto g_it1 = gamma.cbegin();
                  auto g_end = gamma.cend();
                  for (; g_it1 != g_end && !is_hub; ++g_it1) {
                        for (auto g_it2 = std::next(g_it1); g_it2 != g_end && !is_hub; ++g_it2) {
                              auto id1 = node_ids.at(*g_it1);
                              auto id2 = node_ids.at(*g_it2);
                              if (id1 > hub_id && id2 > hub_id && id1 != id2)
                                    is_hub = true;
                        }
                  }
                  id.second = is_hub ? hub_id : outlier_id;
            }
      }
      return node_ids;
}

boost::numeric::ublas::vector<double> detail::getNCDAwareRank(const outlink_matrix_type& outlink_matrix, const std::unordered_map<node_type, uint32_t>& clusters, double etha, double mu, double epsilon)
{
      /*double etha = get_global_properties().rank_etha;
      double mu = get_global_properties().rank_mu;
      double epsilon = get_global_properties().rank_epsilon;*/

      size_t N = outlink_matrix.size1();
      size_t min_cluster = std::numeric_limits<size_t>::max();

      //condense clusters
      std::unordered_map<uint32_t, std::unordered_set<node_type>> cluster_sets;
      std::unordered_map<node_type, uint32_t> condensed_clusters;
      {
         std::unordered_map<uint32_t, std::unordered_set<node_type>> cluster_sets_sparse;
         for (const auto &p : clusters) {
            if (p.second < min_cluster) min_cluster = p.second;
            cluster_sets_sparse[p.second].insert(p.first);
         }

         size_t counter = 0;
         for (auto &p : cluster_sets_sparse) {
            auto next_cluster = counter;
            for (auto node_id : p.second)
               condensed_clusters[node_id] = next_cluster;
            cluster_sets[next_cluster] = std::move(p.second);
            ++counter;
         }
      }

      size_t clusters_size = cluster_sets.size();

      matrix_type matrixA(clusters_size, N, 0);
      for (const auto& p : condensed_clusters) {
            matrixA(p.second, p.first) = 1;
      }

      matrix_type teleportation_matrix(N, N, 0);
      double block_multiplier = 1. / clusters_size;
      for (size_t j = 0; j < N; ++j) {
            double val = block_multiplier / cluster_sets.at(condensed_clusters.at(j)).size();//cluster_sets.at(j).size();
            for (size_t i = 0; i < N; ++i)
                  teleportation_matrix(i,j) = val;
      }

      //matrix M = R * A
      matrix_type matrixR(N, clusters_size, 0);
      for (size_t i = 0; i < N; ++i) {
            std::unordered_set<uint32_t> proximal_nodes{static_cast<unsigned int>(i)};
            for (size_t i2 = 0; i2 < N; ++i2) {
                  if (outlink_matrix(i,i2) > 0)
                        proximal_nodes.insert(i2);
            }
            std::set<uint32_t> proximity_clusters;
            for (const uint32_t node : proximal_nodes) {
                  proximity_clusters.insert(condensed_clusters.at(node));
            }
            for (size_t j = 0; j < clusters_size; ++j) {
                  uint32_t cluster = j;
                  if (proximity_clusters.find(cluster) != proximity_clusters.end()) {
                        matrixR(i,j) = 1. / proximity_clusters.size() / cluster_sets.at(cluster).size();
                  }
            }
      }

      /*for (size_t j = 0; j < clusters_size; ++j) {
            const auto& j_set = cluster_sets.at(j);
            double val = 1 / j_set.size();
            uint32_t proximal_blocks_count = 0;
            for (size_t i = 0; i < N; ++i) {

            }

      }*/

      //std::cerr << "R: " << matrixR << std::endl;

      using boost::numeric::ublas::prod;
      using boost::numeric::ublas::norm_1;
      using boost::numeric::ublas::trans;

      boost::numeric::ublas::vector<double> NCDARank(N, 1), vec(N, 0); //init value!
      do{
            vec = NCDARank;
            boost::numeric::ublas::vector<double> rank_r_prod = prod(vec, matrixR);
            NCDARank = prod(etha * vec, outlink_matrix) + prod(mu * rank_r_prod, matrixA) + prod((1 - etha - mu) * vec, teleportation_matrix);
            //std::cerr << "rank: " << NCDARank << std::endl;
      } while (norm_1(NCDARank - vec) > epsilon);

      return NCDARank;
}

void database::calculate_importance_score(fc::time_point_sec last_maintenance)
{
      using boost::numeric::ublas::element_prod;
      using boost::numeric::ublas::norm_1;

      const auto& globs = get_global_properties().parameters;

      auto min_transfer = globs.min_transfer_for_clustering.value;
      double c_mu       = globs.clustering_mu;
      double c_epsilon  = globs.clustering_epsilon;

      double r_mu       = globs.rank_mu;
      double r_etha     = globs.rank_etha;
      double r_epsilon  = globs.rank_epsilon;

      double s_cluster  = globs.structure_cluster_weight;
      double s_outlier  = globs.structure_outlier_weight;

      double i_omega_o  = globs.importance_omega_o;
      double i_omega_i  = globs.importance_omega_i;

      std::unordered_map<uint64_t, uint64_t> accounts_map;
      auto outlink = construct_outlink_matrix(last_maintenance, accounts_map);

      boost::numeric::ublas::vector<double> outlink_vec(outlink.size1());
      for (size_t i = 0; i < outlink.size1(); ++i) {
            boost::numeric::ublas::matrix_row<outlink_matrix_type> row(outlink, i);
            outlink_vec(i) = std::accumulate(row.begin(), row.end(), 0.);
      }
      using boost::numeric::ublas::norm_1;
      for (size_t j = 0; j < outlink.size2(); ++j) {
         boost::numeric::ublas::matrix_column<outlink_matrix_type> col(outlink, j);
         double norm = norm_1(col);
         if (norm > 0)
            for (size_t i = 0; i < col.size(); ++i)
                  col(i) = col(i) / norm;
      }

      outlink = boost::numeric::ublas::trans(outlink);

      auto clusters = detail::cluster_graph_simple(outlink, accounts_map, c_epsilon, c_mu, min_transfer);
      auto rank = detail::getNCDAwareRank(outlink, clusters, r_etha, r_mu, r_epsilon);
      //std::cerr << "R " << rank << '\n';

      boost::numeric::ublas::vector<uint64_t> balances(rank.size());
      boost::numeric::ublas::vector<double> structure_weight(rank.size());
      for (const auto& p : accounts_map) {
            account_id_type acc_id(p.first);
            balances(p.second) = get_balance(acc_id, asset_id_type()).amount.value;

            structure_weight(p.second) = (clusters.at(p.second) > MIN_CLUSTER) ? s_cluster : s_outlier ;
      }

      /*for (size_t i = 0; i < outlink.size1(); ++i) {
            boost::numeric::ublas::matrix_column<outlink_matrix_type> col(outlink, i);
            outlink_vec(i) = std::accumulate(col.begin(), col.end(), 0.);
      }*/

      //std::cerr << "Bs " << balances << "\n";
      //std::cerr << "Ov " << outlink_vec << "\n";

      boost::numeric::ublas::vector<double> inter_vec = balances + i_omega_o * outlink_vec;
      double norm = norm_1(inter_vec);
      //std::cerr << "iv " << inter_vec / norm << "\n";
      boost::numeric::ublas::vector<double> score = element_prod(inter_vec / norm + i_omega_i * rank, structure_weight);

      //std::cerr << "Sc " << score << "\n";

      const auto& all_accounts = get_index_type<account_index>().indices().get<by_id>();
      for (auto& account : all_accounts) {

         //auto& sender_statistics = account.statistics(*this);

         //std::cout << sender_account.name << " : " << uint64_t(sender_account.id) << " = " << acc_transfers << '\n';

         double importance_score = 0;
         auto acc_num = object_id_type(account.get_id()).instance();
         if (accounts_map.find(acc_num) != accounts_map.end())
            importance_score = score(accounts_map.at(acc_num));

         modify(account.statistics(*this), [&](account_statistics_object& s)
         {
            s.importance_score = importance_score;
         });
      }
}

} }


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/StdVector>

#include <map>
#include <limits>
#include <vector>
#include <memory>
#include <iostream>
#include <fstream>

#include <ros/ros.h>

#include "generate_dynamic_scene/obstacle.hpp"
#include "dynamic_motion_planner/chomp_trajectory.hpp"
#include "dynamic_motion_planner/chomp_cost.hpp"
#include "generate_dynamic_scene/obstacle.hpp"
#include "generate_dynamic_scene/human.hpp"
#include "utils/utility_functions.hpp"

namespace hdi_plan {
class Chomp {
public:
	Chomp(double collision_threshold, double planning_time_limit, int max_iterations, int max_iterations_after_collision_free,
		  double learning_rate, double obstacle_cost_weight, double dynamic_obstacle_cost_weight, double dynamic_collision_factor,
		  double smoothness_cost_weight, double smoothness_cost_velocity, double smoothness_cost_acceleration, double smoothness_cost_jerk,
		  double ridge_factor, double min_clearence, double joint_update_limit, double quadrotor_radius,
		  const std::shared_ptr<ChompTrajectory>& trajectory, const std::map<std::string, std::shared_ptr<Obstacle>>& obstacle_map,
		  const std::map<int, std::shared_ptr<Human>>& human_map, int chomp_path_file_num);
	~Chomp();

	std::vector<Eigen::Vector3d> get_optimized_trajectory() const {
		return this->optimized_trajectory_;
	}

private:
	// debug
	std::ofstream chomp_cost_file;
	void check_params();

	std::shared_ptr<ChompTrajectory> full_trajectory_;
	std::vector<Eigen::Vector3d> optimized_trajectory_;
	int num_vars_all_;
	int num_vars_free_;
	int num_vars_origin_;
	int free_vars_start_;
	int free_vars_end_;
	int last_improvement_iteration_;
	bool is_collsion_free_{false};
	int iteration_{0};
	int collision_free_iteration_{0};
	bool filter_mode_{false};
	double collision_threshold_{0.07};

	// joint
	int num_joints_{3};
	std::vector<std::shared_ptr<ChompCost>> joint_costs_;

	// trajectory
	Eigen::MatrixXd best_trajectory_;
	double best_trajectory_cost_;

	// matrix
	Eigen::MatrixXd smoothness_increments_;
	Eigen::MatrixXd collision_increments_;
	Eigen::MatrixXd dynamic_collision_increments_;
	Eigen::MatrixXd final_increments_;

	// temporary variables
	Eigen::VectorXd smoothness_derivative_;
	Eigen::MatrixXd jacobian_;
	Eigen::MatrixXd jacobian_pseudo_inverse_;
	Eigen::Matrix3d jacobian_jacobian_tranpose_;

	// smoothness cost
	//std::shared_ptr<ChompCost> joint_cost_;

	// collision cost
	std::map<std::string, std::shared_ptr<Obstacle>> obstacle_map_;
	std::vector<Eigen::Vector3d> collision_point_pos_;
	std::vector<Eigen::Vector3d> collision_point_vel_;
	std::vector<Eigen::Vector3d> collision_point_acc_;
	std::vector<double> collision_point_potential_;
	std::vector<double> collision_point_vel_mag_;
	std::vector<Eigen::Vector3d> collision_point_potential_gradient_;

	double get_potential(const Eigen::Vector3d& point);
	void get_collision_point_pos();
	Eigen::Vector3d get_distance_gradient(const Eigen::Vector3d& point);
	double get_potential_for_gradient(double x, double y, double z);
	void export_potential_data(bool if_dynamic);
	void export_potential_gradient_data(bool if_dynamic);
	//void get_jacobian(int trajectory_point, const Eigen::Vector3d& collision_point_pos);

	// dynamic collision cost
	std::map<int, std::shared_ptr<Human>> human_map_;
	std::vector<double> dynamic_collision_point_potential_;
	std::vector<Eigen::Vector3d> dynamic_collision_point_potential_gradient_;
	double get_dynamic_potential(const Eigen::Vector3d& point, int index);
	Eigen::Vector3d get_dynamic_distance_gradient(const Eigen::Vector3d& point, int index);
	double get_dynamic_potential_for_gradient(double x, double y, double z, int index);

	// optimize process
	void initialize();
	bool optimize();
	void convert_matrix_to_trajectory_points_vector();

	void perform_forward_kinematics();
	double get_collision_cost();
	double get_smoothness_cost();
	double get_dynamic_collision_cost();
	void calculate_smoothness_increments();
	void calculate_collision_increments();
	void calculate_dynamic_collision_increments();
	void calculate_total_increments();
	void add_increments_to_trajectory();

	int worst_collision_cost_state_{-1};
	int worst_dynamic_collision_cost_state_{-1};

	// parameters
	double planning_time_limit_{60.0};
	int max_iterations_{50};
	int max_iterations_after_collision_free_{15};
	double learning_rate_{0.1};
	double obstacle_cost_weight_{0.1};
	double dynamic_obstacle_cost_weight_{0.1};
	double dynamic_collision_factor_{0.1};
	double smoothness_cost_weight_{0.5};
	double smoothness_cost_velocity_{0.0};
	double smoothness_cost_acceleration_{1.0};
	double smoothness_cost_jerk_{0.0};
	double ridge_factor_{0.0};
	double min_clearence_{0.5};
	double joint_update_limit_{0.1};
	double quadrotor_radius_{0.5};
	bool use_stochastic_descent_{false};
	// export data
	int chomp_path_file_num_{0};

};

}


#include "testsuite.h"
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    Array<int,1> A(16384);

    Array<int,1> B(A);
    A.free();
    BZTEST(A.data() == 0);
    BZTEST(A.extent(0) == 0);
    B = 0;   // If B's data was accidentally freed, then will hopefully
             // cause a segment violation

    B.free();
    BZTEST(B.data() == 0);
    BZTEST(B.extent(0) == 0);

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_CONSTANTS_EXPX2C2_HPP_INCLUDED
#define NT2_EXPONENTIAL_CONSTANTS_EXPX2C2_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
    /*!
      @brief Expx2c2 generic tag

      Represents the Expx2c2 constant in generic contexts.

      @par Models:
      Hierarchy
    **/
    BOOST_SIMD_CONSTANT_REGISTER( Expx2c2, double
                                , 0, 0x3d000000         // 1.f/32
                                , 0x3f80000000000000ll  // 1.0/128
                                )
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::Expx2c2, Site> dispatching_Expx2c2(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::Expx2c2, Site>();
    }
    template<class... Args>
    struct impl_Expx2c2;
  }
  /*!
    Generates a costant used in expx2.

    @par Semantic:

    @code
    T r = Expx2c2<T>();
    @endcode

    is similar to:

    @code
    if T is integral
      r = T(0)
    else if T is double
      r =  1/T(128);
    else if T is float
      r =  1/T(32);
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Expx2c2, Expx2c2);
}

#endif


//==================================================================================================
/*!
  @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_TOUINT_S_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_TOUINT_S_HPP_INCLUDED

#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/is_ngez.hpp>
#include <boost/simd/function/saturate.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( touint_
                          , (typename A0)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bd::scalar_< bd::int_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0, unsigned>;

    BOOST_FORCEINLINE result_t operator() (const saturated_tag &,  A0 a0
                                    ) const BOOST_NOEXCEPT
    {
      return result_t(saturate<result_t>(a0));
    }
  };
  BOOST_DISPATCH_OVERLOAD ( touint_
                          , (typename A0)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bd::scalar_< bd::uint_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const saturated_tag &,  A0 a0
                                    ) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( touint_
                          , (typename A0)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0, unsigned>;
    BOOST_FORCEINLINE result_t operator() (const saturated_tag &,  A0 a0
                                    ) const BOOST_NOEXCEPT
    {
      if (is_ngez(a0)) return Zero<result_t>();
      if (a0 >= Valmax<result_t>()) return Valmax<result_t>();
      return result_t(a0);
    }
  };
} } }

#endif


//==================================================================================================
/*!
  @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_FUNCTION_CLZ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CLZ_HPP_INCLUDED

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

  /*!
    @ingroup group-bitwise
    This function object returns  the bit count of leading zeros.

    @par Header <boost/simd/function/clz.hpp>

    @see  ctz, popcnt

    @par Example:

      @snippet clz.cpp clz

    @par Possible output:

      @snippet clz.txt clz

  **/
  as_integer_t<Value> clz(Value const& x);
} }
#endif

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

#endif


#include <iostream>
#include <string>
#include <vector>
#include <map>
#include <iterator>
#include <bits/stdc++.h>
#include <locale>
#include <codecvt>
#include <boost/algorithm/string.hpp>
#include "../stem/stemming/english_stem.h"

using namespace std;
using namespace boost;
using convert_t = std::codecvt_utf8<wchar_t>;
std::wstring_convert<convert_t, wchar_t> strconverter;

string stopWords[] = {
    "ourselves", "hers", "between", "yourself", "but", "again", "there", "about", "once", "during", "out", "very", "having",
    "with", "they", "own", "an", "be", "some", "for", "do", "its", "yours", "such", "into", "of", "most", "itself", "other", "off", "is", "s",
    "am", "or", "who", "as", "from", "him", "each", "the", "themselves", "until", "below", "are", "we", "these", "your", "his", "through", "don",
    "nor", "me", "were", "her", "more", "himself", "this", "down", "should", "our", "their", "while", "above", "both", "up", "to", "ours", "had",
    "she", "all", "no", "when", "at", "any", "before", "them", "same", "and", "been", "have", "in", "will", "on", "does", "yourselves", "then",
    "that", "because", "what", "over", "why", "so", "can", "did", "not", "now", "under", "he", "you", "herself", "has", "just", "where", "too",
    "only", "myself", "which", "those", "i", "after", "few", "whom", "t", "being", "if", "theirs", "my", "against", "a", "by", "doing", "it",
    "how", "further", "was", "here", "than"
};

int sizeStop = sizeof(stopWords)/sizeof(stopWords[0]);
bool queryProcessed = false;

bool isStopWord(string s){
    for(int i=0;i<sizeStop;i++){
        if(s==stopWords[i])
            return true;
    }
    return false;
}

string my_to_string(wstring wstr){
    return strconverter.to_bytes(wstr);
}

wstring my_to_wstring(string str){
    return strconverter.from_bytes(str);
}

void pageRank(){
    int n = 5, nIter = 50;               // n, nIter, a and d are the inputs
    float a[n][n] = {{0, 0, 0, 0, 1},
        {0.5, 0, 0, 0, 0},
        {0.5, 0, 0, 0, 0},
        {0, 1, 0.5, 0, 0},
        {0, 0, 0.5, 1, 0}};
    float d = 0.85, sum = 0;
    float b[n];
    float c[n];
    for(int i=0;i<n;i++){
        b[i] = ((float) rand()+1)/((float) RAND_MAX+1);
        sum += b[i];
    }
    for(int i=0;i<n;i++){
        b[i]/=sum;
    }
    while(nIter--){
        for(int i=0;i<n;i++){
            sum=0;
            for(int j=0;j<n;j++){
                sum+=a[i][j]*b[j];
            }
            c[i]=d*sum+(1-d)/n;
        }
        for(int i=0;i<n;i++)
            b[i]=c[i];
    }
    cout<<"Final scores: ";
    for(int i=0;i<n;i++){
        cout<<b[i]<<" ";
    }
    cout<<endl;
}

vector<int> andQuery(vector<vector<int>> vocab, int wordIndex, vector<int> currentResult){
    queryProcessed = true;
    for(int i=currentResult.size()-1;i>-1;i--){
        if(vocab[wordIndex][currentResult[i]]==0){
            currentResult.erase(currentResult.begin()+i);
        }
    }
    return currentResult;
}

vector<int> notQuery(vector<vector<int>> vocab, int wordIndex, vector<int> currentResult){
    queryProcessed = true;
    for(int i=currentResult.size();i>-1;i--){
        if(vocab[wordIndex][currentResult[i]]==1){
            currentResult.erase(currentResult.begin()+i);
        }
    }
    return currentResult;
}

vector<int> orQuery(vector<vector<int>> vocab, int wordIndex, vector<int> currentResult, int m){
    if(queryProcessed==false){
        currentResult.clear();
        for(int i=0;i<m;i++){
            if(vocab[wordIndex][currentResult[i]]==1){
                currentResult.push_back(i);
            }
        }
    }
    else{
        for(int i=0;i<m;i++){
            if(vocab[wordIndex][i]==1){
                currentResult.push_back(i);
            }
        }
        sort(currentResult.begin(), currentResult.end());
        for(int i=1;i<currentResult.size();i++){
            if(currentResult[i]==currentResult[i-1]){
                currentResult.erase(currentResult.begin()+i);
                i--;
            }
        }
    }
    queryProcessed = true;
    return currentResult;
}

int main(){
    // pageRank();
    const char * fileName;
    stemming::english_stem<> StemEnglish;
    string line;
    vector<string> splitVec;
    vector<string> files;
    map<string, int> wordMap;
    map<string, int>::iterator it;
    vector<vector<int>> vocab;

    // reading index in memory
    try {
        ifstream file("index");
        if(file.is_open()){
            while(getline(file,line)){
                trim(line);
                split(splitVec, line, is_any_of("@"), token_compress_on);
                for(int i=1;i<splitVec.size();i++){
                    trim(splitVec[i]);
                    if(splitVec[i]=="")
                        continue;
                    files.push_back(splitVec[i]);
                    // cout<<splitVec[i]<<endl;
                }
                break;
            }
            while(getline(file, line)) {
                trim(line);
                if(line=="")
                    continue;
                split(splitVec, line, is_any_of(" "), token_compress_on);
                if(splitVec.size()==0)
                    continue;
                wordMap.insert(make_pair(splitVec[0], wordMap.size()));
                vector<int> tempVec;
                for(int z=1;z<splitVec.size();z++){
                    tempVec.push_back(stoi(splitVec[z]));
                }
                vocab.push_back(tempVec);
            }
            file.close();
        }
        else{
            throw 0;
        }
    }
    catch(...) {
        cout<<"Index file not present.\n";
        exit(0);
    }

    // query processing
    string query, input, stemmed, queryType;
    int wordIndex;
    cout<<"Syntax: [and/or/not] [queryTerm1] [and/or/not] [queryTerm2]..."<<endl;
    do{
        queryProcessed = false;
        vector<int> result;
        for(int i=0;i<files.size();i++){
            result.push_back(i);
        }
        cout<<"Type query: ";
        getline(cin, query);
        to_lower(query);
        trim(query);
        split(splitVec, query, is_any_of(".,-:'\"?/_()*!#@;[]{}<>&"), token_compress_on);
        if(splitVec.size()>1){
            cout<<"Syntax error in input query"<<endl<<"None of "<<".,-:'\"?/_()*!#@;[]{}<>&"<<" should be present in query."<<endl;
            goto nextQuery;
        }
        split(splitVec, query, is_any_of(" "), token_compress_on);
        if(splitVec.size()%2){
            cout<<"Syntax error in input query. Length not even."<<endl;
            goto nextQuery;
        }
        for(int i=0;i<splitVec.size();i++){
            queryType = splitVec[i];
            i++;
            if(!isStopWord(splitVec[i])){
                wstring temp = my_to_wstring(splitVec[i]);
                StemEnglish(temp);
                stemmed = my_to_string(temp);
            }
            else{
                continue;
            }
            it = wordMap.find(stemmed);
            if(it==wordMap.end()){
                if(queryType=="and"){
                    result.clear();
                    break;
                }
                else if(queryType=="or"||queryType=="not")
                    continue;
                else{
                    cout<<"Syntax error in input query. "+queryType+" present instead of and/or/not."<<endl;
                    goto nextQuery;
                }
            }
            else{
                wordIndex = it->second;
            }
            if(queryType=="and"){
                result = andQuery(vocab, wordIndex, result);
            }
            else if(queryType=="or"){
                result = orQuery(vocab, wordIndex, result, files.size());
            }
            else if(queryType=="not"){
                result = notQuery(vocab, wordIndex, result);
            }
            else{
                cout<<"Syntax error in input query. "+queryType+" present instead of and/or/not."<<endl;
                goto nextQuery;
            }
        }
        if(result.size()==0||queryProcessed==false){
            cout<<"No results found!"<<endl;
        }
        else{
            for(int i=0;i<result.size();i++){
                cout<<files[result[i]]<<endl;
            }
            cout<<endl;
        }
        nextQuery: cout<<"Test more queries? If yes, type yes. If no, type anything else.\n";
        getline(cin, input);
    }while(input=="yes");

    return 1;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_FAST_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_FAST_HYPOT_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  fast_hypot generic tag

      Represents the fast_hypot function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct fast_hypot_ : ext::elementwise_<fast_hypot_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<fast_hypot_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_fast_hypot_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::fast_hypot_, Site> dispatching_fast_hypot_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::fast_hypot_, Site>();
    }
    template<class... Args>
    struct impl_fast_hypot_;
  }
  /*!
    Computes \f$(x^2 + y^2)^{1/2}\f$

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = fast_hypot(x, y);
    @endcode

    The code is equivalent to:

    @code
    T r =sqrt(sqr(x)+sqr(y));
    @endcode

    Fast means that nothing is done to avoid overflow or inaccuracies
    for large values. See @funcref{hypot} if that matters.

    @param  a0
    @param  a1

    @return      a value of the same floating type as the input.
  **/

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::fast_hypot_, fast_hypot, 2)
} }

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Olga Diamanti <olga.diam@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "LinSpaced.h"
#include "n_polyvector.h"
#include "edge_topology.h"
#include "local_basis.h"
#include "nchoosek.h"
#include "slice.h"
#include "polyroots.h"
#include "igl_inline.h"
#include <Eigen/Sparse>

#include <Eigen/Geometry>
#include <iostream>
#include <complex>

namespace igl {
  template <typename DerivedV, typename DerivedF>
  class PolyVectorFieldFinder
  {
  private:
    const Eigen::PlainObjectBase<DerivedV> &V;
    const Eigen::PlainObjectBase<DerivedF> &F; int numF;
    const int n;

    Eigen::MatrixXi EV; int numE;
    Eigen::MatrixXi F2E;
    Eigen::MatrixXi E2F;
    Eigen::VectorXd K;

    Eigen::VectorXi isBorderEdge;
    int numInteriorEdges;
    Eigen::Matrix<int,Eigen::Dynamic,2> E2F_int;
    Eigen::VectorXi indInteriorToFull;
    Eigen::VectorXi indFullToInterior;

    DerivedV B1, B2, FN;

    IGL_INLINE void computek();
    IGL_INLINE void setFieldFromGeneralCoefficients(const  std::vector<Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> > &coeffs,
                                                    std::vector<Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 2> > &pv);
    IGL_INLINE void computeCoefficientLaplacian(int n, Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &D);
    IGL_INLINE void getGeneralCoeffConstraints(const Eigen::VectorXi &isConstrained,
                                    const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &cfW,
                                    int k,
                                    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> &Ck);
    IGL_INLINE void precomputeInteriorEdges();

    IGL_INLINE void minQuadWithKnownMini(const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &Q,
                                         const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &f,
                                         const Eigen::VectorXi isConstrained,
                                         const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &xknown,
                                         Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &x);

  public:
    IGL_INLINE PolyVectorFieldFinder(const Eigen::PlainObjectBase<DerivedV> &_V,
                                     const Eigen::PlainObjectBase<DerivedF> &_F,
                                     const int &_n);
    IGL_INLINE bool solve(const Eigen::VectorXi &isConstrained,
               const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &cfW,
               Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &output);

  };
}

template<typename DerivedV, typename DerivedF>
IGL_INLINE igl::PolyVectorFieldFinder<DerivedV, DerivedF>::
          PolyVectorFieldFinder(const Eigen::PlainObjectBase<DerivedV> &_V,
                                const Eigen::PlainObjectBase<DerivedF> &_F,
                                const int &_n):
V(_V),
F(_F),
numF(_F.rows()),
n(_n)
{

  igl::edge_topology(V,F,EV,F2E,E2F);
  numE = EV.rows();

  precomputeInteriorEdges();

  igl::local_basis(V,F,B1,B2,FN);

  computek();

};

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::PolyVectorFieldFinder<DerivedV, DerivedF>::
precomputeInteriorEdges()
{
  // Flag border edges
  numInteriorEdges = 0;
  isBorderEdge.setZero(numE,1);
  indFullToInterior = -1*Eigen::VectorXi::Ones(numE,1);

  for(unsigned i=0; i<numE; ++i)
  {
    if ((E2F(i,0) == -1) || ((E2F(i,1) == -1)))
      isBorderEdge[i] = 1;
      else
      {
        indFullToInterior[i] = numInteriorEdges;
        numInteriorEdges++;
      }
  }

  E2F_int.resize(numInteriorEdges, 2);
  indInteriorToFull.setZero(numInteriorEdges,1);
  int ii = 0;
  for (int k=0; k<numE; ++k)
  {
    if (isBorderEdge[k])
      continue;
    E2F_int.row(ii) = E2F.row(k);
    indInteriorToFull[ii] = k;
    ii++;
  }

}

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::PolyVectorFieldFinder<DerivedV, DerivedF>::
minQuadWithKnownMini(const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &Q,
                          const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &f,
                     const Eigen::VectorXi isConstrained,
                          const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &xknown,
                          Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &x)
{
  int N = Q.rows();

  int nc = xknown.rows();
  Eigen::VectorXi known; known.setZero(nc,1);
  Eigen::VectorXi unknown; unknown.setZero(N-nc,1);

  int indk = 0, indu = 0;
  for (int i = 0; i<N; ++i)
    if (isConstrained[i])
    {
      known[indk] = i;
      indk++;
    }
    else
    {
      unknown[indu] = i;
      indu++;
    }

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > Quu, Quk;

  igl::slice(Q,unknown, unknown, Quu);
  igl::slice(Q,unknown, known, Quk);

  std::vector<typename Eigen::Triplet<std::complex<typename DerivedV::Scalar> > > tripletList;

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fu(N-nc,1);

  igl::slice(f,unknown, Eigen::VectorXi::Zero(1,1), fu);

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > rhs = (Quk*xknown).sparseView()+.5*fu;

  Eigen::SparseLU< Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > > solver;
  solver.compute(-Quu);
  if(solver.info()!=Eigen::Success)
  {
    std::cerr<<"Decomposition failed!"<<std::endl;
    return;
  }
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> >  b  = solver.solve(rhs);
  if(solver.info()!=Eigen::Success)
  {
    std::cerr<<"Solving failed!"<<std::endl;
    return;
  }

  indk = 0, indu = 0;
  x.setZero(N,1);
  for (int i = 0; i<N; ++i)
    if (isConstrained[i])
      x[i] = xknown[indk++];
    else
      x[i] = b.coeff(indu++,0);

}

template<typename DerivedV, typename DerivedF>
IGL_INLINE bool igl::PolyVectorFieldFinder<DerivedV, DerivedF>::
                     solve(const Eigen::VectorXi &isConstrained,
                           const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &cfW,
                           Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &output)
{

  // polynomial is of the form:
  // (-1)^0 z^(2n) +
  // (-1)^1 c[0]z^(2n-2) +
  // (-1)^2 c[1]z^(2n-4) +
  // (-1)^3 c[2]z^(2n-6) +
  // ... +
  // (-1)^n c[n-1]

  std::vector<Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> > coeffs(n,Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1>::Zero(numF, 1));

  for (int i =0; i<n; ++i)
  {
    int degree = 2*(i+1);

    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> Ck;
    getGeneralCoeffConstraints(isConstrained,
                               cfW,
                               i,
                               Ck);

    Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > DD;
    computeCoefficientLaplacian(degree, DD);
    Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > f; f.resize(numF,1);

    minQuadWithKnownMini(DD, f, isConstrained, Ck, coeffs[i]);
  }

  std::vector<Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 2> > pv;
  setFieldFromGeneralCoefficients(coeffs, pv);

  output.setZero(numF,3*n);
  for (int fi=0; fi<numF; ++fi)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b1 = B1.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b2 = B2.row(fi);
    for (int i=0; i<n; ++i)
      output.block(fi,3*i, 1, 3) = pv[i](fi,0)*b1 + pv[i](fi,1)*b2;
  }
  return true;
}

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::PolyVectorFieldFinder<DerivedV, DerivedF>::setFieldFromGeneralCoefficients(const  std::vector<Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> > &coeffs,
                                                            std::vector<Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 2> > &pv)
{
  pv.assign(n, Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 2>::Zero(numF, 2));
  for (int i = 0; i <numF; ++i)
  {

    //    poly coefficients: 1, 0, -Acoeff, 0, Bcoeff
    //    matlab code from roots (given there are no trailing zeros in the polynomial coefficients)
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> polyCoeff;
    polyCoeff.setZero(2*n+1,1);
    polyCoeff[0] = 1.;
    int sign = 1;
    for (int k =0; k<n; ++k)
    {
      sign = -sign;
      int degree = 2*(k+1);
      polyCoeff[degree] = (1.*sign)*coeffs[k](i);
    }

    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> roots;
    igl::polyRoots<std::complex<typename DerivedV::Scalar>, typename DerivedV::Scalar >(polyCoeff,roots);

    Eigen::VectorXi done; done.setZero(2*n,1);

    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> u(n,1);
    int ind =0;
    for (int k=0; k<2*n; ++k)
    {
      if (done[k])
        continue;
      u[ind] = roots[k];
      done[k] = 1;

      int mini = -1;
      double mind = 1e10;
      for (int l =k+1; l<2*n; ++l)
      {
        double dist = abs(roots[l]+u[ind]);
        if (dist<mind)
        {
          mind = dist;
          mini = l;
        }
      }
      done[mini] = 1;
      ind ++;
    }
    for (int k=0; k<n; ++k)
    {
      pv[k](i,0) = real(u[k]);
      pv[k](i,1) = imag(u[k]);
    }
  }

}

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::PolyVectorFieldFinder<DerivedV, DerivedF>::computeCoefficientLaplacian(int n, Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &D)
{
  std::vector<Eigen::Triplet<std::complex<typename DerivedV::Scalar> > > tripletList;

  // For every non-border edge
  for (unsigned eid=0; eid<numE; ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = E2F(eid,0);
      int fid1 = E2F(eid,1);

      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid0,
                                           fid0,
                                           std::complex<typename DerivedV::Scalar>(1.)));
      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid1,
                                           fid1,
                                           std::complex<typename DerivedV::Scalar>(1.)));
      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid0,
                                           fid1,
                                                                                -1.*std::polar(1.,-1.*n*K[eid])));
      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid1,
                                           fid0,
                                                                                -1.*std::polar(1.,1.*n*K[eid])));

    }
  }
  D.resize(numF,numF);
  D.setFromTriplets(tripletList.begin(), tripletList.end());

}

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::PolyVectorFieldFinder<DerivedV, DerivedF>::getGeneralCoeffConstraints(const Eigen::VectorXi &isConstrained,
                                                       const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &cfW,
                                                       int k,
                                                       Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> &Ck)
{
  int numConstrained = isConstrained.sum();
  Ck.resize(numConstrained,1);
  int n = cfW.cols()/3;

  Eigen::MatrixXi allCombs;
  {
    Eigen::VectorXi V = igl::LinSpaced<Eigen::VectorXi >(n,0,n-1);
    igl::nchoosek(V,k+1,allCombs);
  }

  int ind = 0;
  for (int fi = 0; fi <numF; ++fi)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b1 = B1.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b2 = B2.row(fi);
    if(isConstrained[fi])
    {
      std::complex<typename DerivedV::Scalar> ck(0);

      for (int j = 0; j < allCombs.rows(); ++j)
      {
        std::complex<typename DerivedV::Scalar> tk(1.);
        //collect products
        for (int i = 0; i < allCombs.cols(); ++i)
        {
          int index = allCombs(j,i);

          const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &w = cfW.block(fi,3*index,1,3);
          typename DerivedV::Scalar w0 = w.dot(b1);
          typename DerivedV::Scalar w1 = w.dot(b2);
          std::complex<typename DerivedV::Scalar> u(w0,w1);
          tk*= u*u;
        }
        //collect sum
        ck += tk;
      }
      Ck(ind) = ck;
      ind ++;
    }
  }

}

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::PolyVectorFieldFinder<DerivedV, DerivedF>::computek()
{
  K.setZero(numE);
  // For every non-border edge
  for (unsigned eid=0; eid<numE; ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = E2F(eid,0);
      int fid1 = E2F(eid,1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N0 = FN.row(fid0);
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N1 = FN.row(fid1);

      // find common edge on triangle 0 and 1
      int fid0_vc = -1;
      int fid1_vc = -1;
      for (unsigned i=0;i<3;++i)
      {
        if (F2E(fid0,i) == eid)
          fid0_vc = i;
        if (F2E(fid1,i) == eid)
          fid1_vc = i;
      }
      assert(fid0_vc != -1);
      assert(fid1_vc != -1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> common_edge = V.row(F(fid0,(fid0_vc+1)%3)) - V.row(F(fid0,fid0_vc));
      common_edge.normalize();

      // Map the two triangles in a new space where the common edge is the x axis and the N0 the z axis
      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> P;
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> o = V.row(F(fid0,fid0_vc));
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> tmp = -N0.cross(common_edge);
      P << common_edge, tmp, N0;
//      P.transposeInPlace();

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V0;
      V0.row(0) = V.row(F(fid0,0)) -o;
      V0.row(1) = V.row(F(fid0,1)) -o;
      V0.row(2) = V.row(F(fid0,2)) -o;

      V0 = (P*V0.transpose()).transpose();

//      assert(V0(0,2) < 1e-10);
//      assert(V0(1,2) < 1e-10);
//      assert(V0(2,2) < 1e-10);

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V1;
      V1.row(0) = V.row(F(fid1,0)) -o;
      V1.row(1) = V.row(F(fid1,1)) -o;
      V1.row(2) = V.row(F(fid1,2)) -o;
      V1 = (P*V1.transpose()).transpose();

//      assert(V1(fid1_vc,2) < 10e-10);
//      assert(V1((fid1_vc+1)%3,2) < 10e-10);

      // compute rotation R such that R * N1 = N0
      // i.e. map both triangles to the same plane
      double alpha = -atan2(V1((fid1_vc+2)%3,2),V1((fid1_vc+2)%3,1));

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> R;
      R << 1,          0,            0,
      0, cos(alpha), -sin(alpha) ,
      0, sin(alpha),  cos(alpha);
      V1 = (R*V1.transpose()).transpose();

//      assert(V1(0,2) < 1e-10);
//      assert(V1(1,2) < 1e-10);
//      assert(V1(2,2) < 1e-10);

      // measure the angle between the reference frames
      // k_ij is the angle between the triangle on the left and the one on the right
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref0 = V0.row(1) - V0.row(0);
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref1 = V1.row(1) - V1.row(0);

      ref0.normalize();
      ref1.normalize();

      double ktemp = atan2(ref1(1),ref1(0)) - atan2(ref0(1),ref0(0));

      // just to be sure, rotate ref0 using angle ktemp...
      Eigen::Matrix<typename DerivedV::Scalar, 2, 2> R2;
      R2 << cos(ktemp), -sin(ktemp), sin(ktemp), cos(ktemp);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 2> tmp1 = R2*(ref0.head(2)).transpose();

//      assert(tmp1(0) - ref1(0) < 1e-10);
//      assert(tmp1(1) - ref1(1) < 1e-10);

      K[eid] = ktemp;
    }
  }

}

IGL_INLINE void igl::n_polyvector(const Eigen::MatrixXd &V,
                             const Eigen::MatrixXi &F,
                             const Eigen::VectorXi& b,
                             const Eigen::MatrixXd& bc,
                             Eigen::MatrixXd &output)
{
  Eigen::VectorXi isConstrained = Eigen::VectorXi::Constant(F.rows(),0);
  Eigen::MatrixXd cfW = Eigen::MatrixXd::Constant(F.rows(),bc.cols(),0);

  for(unsigned i=0; i<b.size();++i)
  {
    isConstrained(b(i)) = 1;
    cfW.row(b(i)) << bc.row(i);
  }
  if (b.size() == F.rows())
  {
    output = cfW;
    return;
  }

  int n = cfW.cols()/3;
  igl::PolyVectorFieldFinder<Eigen::MatrixXd, Eigen::MatrixXi> pvff(V,F,n);
  pvff.solve(isConstrained, cfW, output);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
#endif