/**
* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
*
* Copyright (c) 2016 LIBSCAPI (http://crypto.biu.ac.il/SCAPI)
* This file is part of the SCAPI project.
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
*
* 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.
*
* We request that any publication and/or code referring to and/or based on SCAPI contain an appropriate citation to SCAPI, including a reference to
* http://crypto.biu.ac.il/SCAPI.
*
* Libscapi uses several open source libraries. Please see these projects for any further licensing issues.
* For more information , See https://github.com/cryptobiu/libscapi/blob/master/LICENSE.MD
*
* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
*
*/

#pragma once
#include <NTL/GF2X.h>
#include <NTL/GF2E.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/vec_GF2E.h>
#include <NTL/GF2EX.h>
#include <NTL/ZZ.h>

#include "SigmaProtocol.hpp"
#include "../primitives/Prg.hpp"
#include <map>

/**
* Concrete implementation of SigmaProtocol input, used by the SigmaProtocolORMultiple verifier and simulator.

* In SigmaProtocolORMultiple, the common input contains an array of inputs to all of
* its underlying objects and k - number of true statements.
*/
class SigmaOrMultipleCommonInput : public SigmaCommonInput {
public:

	/**
	* Sets the input array and the number of statements that have a witness.
	* @param input contains inputs for all the underlying sigma protocol.
	* @param k number of statements that have a witness.
	*/
	SigmaOrMultipleCommonInput(const vector<shared_ptr<SigmaCommonInput>> & input, int k) {
		sigmaInputs = input;
		this->k = k;
	};

	/**
	* Returns the input array contains inputs for all the underlying sigma protocol.
	*/
	vector<shared_ptr<SigmaCommonInput>> getInputs() { return sigmaInputs; };

	/**
	* Returns the number of statements that have a witness.
	*/
	int getK() { return k; };

	string toString() override;

private:
	vector<shared_ptr<SigmaCommonInput>> sigmaInputs;
	int k; //number of statements that have a witness.
};

/**
* Concrete implementation of SigmaProtocol input, used by the SigmaProtocolORMultipleProver.

* This input contains inputs for the true statements(including witnesses) and input for the false atatements(without witnesses).
*/
class SigmaOrMultipleProverInput  : public SigmaProverInput {
private:
	//hold the prover private input.
	map<int, shared_ptr<SigmaProverInput>> proverInputs;

	//Hold the common parameters of the statement where the prover does not know the witness.
	map<int, shared_ptr<SigmaCommonInput>> simulatorInputs;

public:
	/**
	* Sets the inputs for the underlying provers and simulators.
	* @param proverInputs
	* @param simulatorInputs
	*/
	SigmaOrMultipleProverInput(const map<int, shared_ptr<SigmaProverInput>> & proverInputs, const map<int, shared_ptr<SigmaCommonInput>> & simulatorInputs) {
		this->proverInputs = proverInputs;
		this->simulatorInputs = simulatorInputs;
	}

	/**
	* Returns an array holds the inputs for the underlying provers.
	* @return an array holds the inputs for the underlying provers.
	*/
	map<int, shared_ptr<SigmaProverInput>> getProversInput() { return proverInputs; };

	/**
	* Returns an array holds the inputs for the underlying simulators.
	* @return an array holds the inputs for the underlying simulators.
	*/
	map<int, shared_ptr<SigmaCommonInput>> getSimulatorsInput() { return simulatorInputs; };

	shared_ptr<SigmaCommonInput> getCommonInput() override;
};

/**
* Concrete implementation of SigmaProtocol message.
* This message contains an array the interpolated polynomial, array of SigmaProtocolMsg and challenges.
* The prover used this message to send the first message to the verifier.
*/
class SigmaOrMultipleSecondMsg : public SigmaProtocolMsg {

private:
	vector<vector<byte>> polynomial;
	vector<shared_ptr<SigmaProtocolMsg>> z;
	vector<vector<byte>> challenges;

public:
	SigmaOrMultipleSecondMsg(const vector<vector<byte>> & polynomBytes, const vector<shared_ptr<SigmaProtocolMsg>> & z, const vector<vector<byte>> & challenges) {
		this->polynomial = polynomBytes;
		this->z = z;
		this->challenges = challenges;
	};

	vector<vector<byte>> getPolynomial() { return polynomial; };

	vector<shared_ptr<SigmaProtocolMsg>> getMessages() { return z; };

	vector<vector<byte>> getChallenges() { return challenges; };

	string toString() override;
	void initFromString(const string & raw) override;
};

//Initializes the field GF2E with a random irreducible polynomial with degree t.
void initField(int t, int seed);
//Samples random field elements to be the challenges.
vector<vector<byte>> sampleRandomFieldElements(int numElements, int t, vector<shared_ptr<NTL::GF2E>> & elements, PrgFromOpenSSLAES*  random);
vector<byte> convertElementToBytes(NTL::GF2E & element);
NTL::GF2E convertBytesToGF2E(const vector<byte> & elementByts);
NTL::GF2E generateIndexPolynomial(int i);
//Interpolates the points to get a polynomial.
NTL::GF2EX interpolate(const vector<byte> & challenge, vector<shared_ptr<NTL::GF2E>> & fieldElements, const vector<int> & sampledIndexes);
//Calculates the challenges for the statements with the witnesses.
vector<vector<byte>> getRestChallenges(NTL::GF2EX & polynomial, const vector<int> & indexesInI);
//Returns the byteArray of the polynomial coefficients.
vector<vector<byte>> getPolynomialBytes(NTL::GF2EX & polynomial);

/**
* Concrete implementation of Sigma Simulator.

* This implementation simulates the case that the prover convince a verifier that at least k out of n
* statements is true, where each statement can be proven by an associated Sigma protocol.

*
* The pseudo code of this protocol can be found in Protocol 1.16 of pseudo codes document at {@link http://cryptobiu.github.io/scapi/SDK_Pseudocode.pdf}.

*
*
* @author Cryptography and Computer Security Research Group Department of Computer Science Bar-Ilan University (Moriya Farbstein)
*
*/
class SigmaOrMultipleSimulator : public SigmaSimulator {

	/*
	This class computes the following calculations:
	SAMPLE random points e1,...,en-k in GF[2t].
	COMPUTE the polynomial Q and values en-k+1,...,en like in the protocol.
	RUN the simulator on each statement/challenge pair (xi,ei) for all i=1,...,n to obtain (ai,ei,zi).
	OUTPUT (a1,e1,z1),..., (an,en,zn).
	*/

private:
	vector<shared_ptr<SigmaSimulator>> simulators;	// Underlying simulators.
	int t;								// Soundness parameter.
	int len;							// Number of underlying simulators.
	shared_ptr<PrgFromOpenSSLAES> random;

	/**
	* Checks if the given challenge length is equal to the soundness parameter.
	* @return true if the challenge length is t; false, otherwise.
	*/
	bool checkChallengeLength(int size);

public:
	/**
	* Constructor that gets the underlying simulators.
	* @param simulators array of SigmaSimulator that contains underlying simulators.
	* @param t soundness parameter. t MUST be equal to both t values of the underlying simulators object.
	* @param random
	*/
	SigmaOrMultipleSimulator(const vector<shared_ptr<SigmaSimulator>> & simulators, int t, const shared_ptr<PrgFromOpenSSLAES> & random = get_seeded_prg());

	/**
	* Returns the soundness parameter for this Sigma protocol.
	* @return t soundness parameter
	*/
	int getSoundnessParam() override { return t; }

	/**
	* Computes the simulator computation with the given challenge.
	* @param input MUST be an instance of SigmaORMultipleCommonInput.
	* @param challenge
	* @return the output of the computation - (a, e, z).
	* @throws CheatAttemptException if the received challenge's length is not equal to the soundness parameter.
	* @throws IllegalArgumentException if the given input is not an instance of SigmaORMultipleCommonInput.
	*/
	shared_ptr<SigmaSimulatorOutput> simulate(SigmaCommonInput* input, const vector<byte> & challenge) override;

	/**
	* Computes the simulator computation with a randomly chosen challenge.
	* @param input MUST be an instance of SigmaORMultipleCommonInput.
	* @return the output of the computation - (a, e, z).
	* @throws IllegalArgumentException if the given input is not an instance of SigmaORMultipleCommonInput.
	*/
	shared_ptr<SigmaSimulatorOutput> simulate(SigmaCommonInput* input) override;
};

/**
* Concrete implementation of Sigma Protocol prover computation.

*
* This protocol is used for a prover to convince a verifier that at least k out of n statements are true,
* where each statement can be proven by an associated Sigma protocol.

*
* The pseudo code of this protocol can be found in Protocol 1.16 of pseudo codes document at {@link http://cryptobiu.github.io/scapi/SDK_Pseudocode.pdf}.

*
* @author Cryptography and Computer Security Research Group Department of Computer Science Bar-Ilan University (Moriya Farbstein)
*
*/
class SigmaOrMultipleProverComputation : public SigmaProverComputation {

	/*
	* Let (ai,ei,zi) denote the steps of a Sigma protocol SigmaI for proving that xi is in LRi
	* Let I denote the set of indices for which P has witnesses
	This class computes the following calculations:
	For every j not in I, SAMPLE a random element ej <- GF[2^t]
	For every j not in I, RUN the simulator on statement xj and challenge ej to get transcript (aj,ej,zj)
	For every i in I, RUN the prover P on statement xi to get first message ai
	SET a=(a1,...,an)

	INTERPOLATE the points (0,e) and {(j,ej)} for every j not in I to obtain a degree n-k polynomial Q (s.t. Q(0)=e and Q(j)=ej for every j not in I)
	For every i in I, SET ei = Q(i)
	For every i in I, COMPUTE the response zi to (ai, ei) in SigmaI using input (xi,wi)
	The message is Q,e1,z1,...,en,zn (where by Q we mean its coefficients)
	*/

private:
	map<int, shared_ptr<SigmaProverComputation>> provers;	// Underlying Sigma protocol's provers to the OR calculation.
	map<int, shared_ptr<SigmaSimulator>> simulators;		// Underlying Sigma protocol's simulators to the OR calculation.
	int len;												// Number of underlying provers.
	int t;													// Soundness parameter.
	int k;													//number of witnesses.
	shared_ptr<PrgFromOpenSSLAES> random;											// The indexes of the statements which the prover knows the witnesses.

	shared_ptr<SigmaOrMultipleProverInput> input;			// Used in computeFirstMsg function.

	vector<vector<byte>> challenges;						// hold the challenges to the underlying simulators and provers.
															// Some will be calculate in sampleRandomValues function and some in compueSecondMsg.

	map<int, shared_ptr<SigmaSimulatorOutput>> simulatorsOutput;		// We save this because we calculate it in computeFirstMsg and using
															// it after that, in computeSecondMsg

	vector<shared_ptr<NTL::GF2E>> elements;					//Will hold pointers to the sampled field elements,
															//we save the pointers to save the creation of the elements again in computeSecondMsg function.

public:
	/**
	* Constructor that gets the underlying provers.
	* @param provers array of SigmaProverComputation, where each object represent a statement
	* 		  and the prover wants to prove to the verify that the OR of all statements are true.
	* @param t soundness parameter. t MUST be equal to all t values of the underlying provers object.
	* @throws IllegalArgumentException if the given t is not equal to all t values of the underlying provers object.
	*/
	SigmaOrMultipleProverComputation(const map<int, shared_ptr<SigmaProverComputation>> & provers, const map<int, shared_ptr<SigmaSimulator>> & simulators, int t, const shared_ptr<PrgFromOpenSSLAES> & random = get_seeded_prg());

	/**
	* Returns the soundness parameter for this Sigma protocol.
	* @return t soundness parameter
	*/
	int getSoundnessParam() override { return t; }

	/**
	* Computes the first message of the protocol.

	* "For every j not in I, SAMPLE a random element ej <- GF[2^t]

	*  For every j not in I, RUN the simulator on statement xj and challenge ej to get transcript (aj,ej,zj)

	For every i in I, RUN the prover P on statement xi to get first message ai

	SET a=(a1,...,an)".
	* @param input MUST be an instance of SigmaORMultipleInput.
	* @return SigmaMultipleMsg contains a1, ..., am.
	* @throws IllegalArgumentException if input is not an instance of SigmaORMultipleInput.
	* @throws IllegalArgumentException if the number of given inputs is different from the number of underlying provers.
	*/
	shared_ptr<SigmaProtocolMsg> computeFirstMsg(const shared_ptr<SigmaProverInput> & input) override;

	/**
	* Computes the second message of the protocol.

	* "INTERPOLATE the points (0,e) and {(j,ej)} for every j not in I to obtain a degree n-k polynomial Q (s.t. Q(0)=e and Q(j)=ej for every j not in I)

	For every i in I, SET ei = Q(i)

	For every i in I, COMPUTE the response zi to (ai, ei) in Sigmai using input (xi,wi)

	The message is Q,e1,z1,...,en,zn (where by Q we mean its coefficients)".

	* @param challenge
	* @return SigmaMultipleMsg contains z1, ..., zm.
	* @throws CheatAttemptException if the received challenge's length is not equal to the soundness parameter.
	*/
	shared_ptr<SigmaProtocolMsg> computeSecondMsg(const vector<byte> & challenge) override;

	/**
	* Returns the simulator that matches this sigma protocol prover.
	* @return SigmaORMultipleSimulator
	*/
	shared_ptr<SigmaSimulator> getSimulator() override;
};

/**
* Concrete implementation of Sigma Protocol verifier computation.

*
* This protocol is used for a prover to convince a verifier that at least k out of n statements is true,
* where each statement can be proven by an associated Sigma protocol.

*
* The pseudo code of this protocol can be found in Protocol 1.16 of pseudo codes document at {@link http://cryptobiu.github.io/scapi/SDK_Pseudocode.pdf}.

*
*
* @author Cryptography and Computer Security Research Group Department of Computer Science Bar-Ilan University (Moriya Farbstein)
*
*/
class SigmaOrMultipleVerifierComputation : public SigmaVerifierComputation {

	/*
	Let (ai,ei,zi) denote the steps of a Sigma protocol Sigmai for proving that xi is in LRi
	This class computes the following calculations:
	WAIT for messages a1,...,an
	SAMPLE a single random challenge  e <- GF[2^t]

	ACC IFF Q is of degree n-k AND Q(i)=ei for all i=1,...,n AND Q(0)=e, and the verifier output on (ai,ei,zi) for all i=1,...,n is ACC

	*/

private:
	vector<shared_ptr<SigmaVerifierComputation>> verifiers;	// Underlying Sigma protocol verifiers to the OR calculation.
	int len;										// Number of underlying verifiers.
	vector<byte> challengeBytes;										// The challenge.
	int t;											// Soundness parameter.
	NTL::GF2E challengeElement;							// Pointer to the sampled challenge element.
	int k;											// Number of true statements.
	shared_ptr<PrgFromOpenSSLAES> random;

	bool checkPolynomialValidity(const vector<vector<byte>> & polynomial, int k, const NTL::GF2E & challengeElement, const vector<vector<byte>> & challenges);
	NTL::GF2EX createPolynomial(const vector<vector<byte>> & polynomialBytes);

public:
	/**
	* Constructor that gets the underlying verifiers.
	* @param verifiers array of SigmaVerifierComputation, where each object represent a statement
	* 		  and the prover wants to convince a verifier that at least k out of n statements is true.
	* @param t soundness parameter. t MUST be equal to all t values of the underlying verifiers object.
	* @param random source of randomness
	* @throws IllegalArgumentException if the given t is not equal to all t values of the underlying verifiers object.
	*/
	SigmaOrMultipleVerifierComputation(const vector<shared_ptr<SigmaVerifierComputation>> & verifiers, int t, const shared_ptr<PrgFromOpenSSLAES> & random = get_seeded_prg());

	/**
	* Returns the soundness parameter for this Sigma protocol.
	* @return t soundness parameter
	*/
	int getSoundnessParam() override { return t; }

	/**
	* Samples the challenge of the protocol.

	* 	"SAMPLE a single random challenge  e <- GF[2^t]".
	*/
	void sampleChallenge() override;

	/**
	* Sets the given challenge.
	* @param challenge
	*/
	void setChallenge(const vector<byte> & challenge) override;

	/**
	* Returns the sampled challenge.
	* @return the challenge.
	*/
	vector<byte> getChallenge() override { return challengeBytes; }

	/**
	* Computes the verification of the protocol.

	* 	"ACC IFF Q is of degree n-k AND Q(i)=ei for all i=1,...,n AND Q(0)=e, and the verifier output on (ai,ei,zi) for all i=1,...,n is ACC".
	* @param input MUST be an instance of SigmaORMultipleCommonInput.
	* @param a first message from prover
	* @param z second message from prover
	* @return true if the proof has been verified; false, otherwise.
	* @throws IllegalArgumentException if input is not an instance of SigmaORMultipleCommonInput.
	* @throws IllegalArgumentException if the number of given inputs is different from the number of underlying verifier.
	* @throws IllegalArgumentException if the first message of the prover is not an instance of SigmaMultipleMsg
	* @throws IllegalArgumentException if the second message of the prover is not an instance of SigmaORMultipleSecondMsg
	*/
	bool verify(SigmaCommonInput* input, SigmaProtocolMsg* a, SigmaProtocolMsg* z) override;
};


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

#include <map>

#include <functional>
#include <boost/make_shared.hpp>

#include "tudat/simulation/estimation_setup/createObservationViability.h"

namespace tudat
{

namespace observation_models
{

//! Function to filter list of observationViabilitySettings, so that only those relevant for single set of link ends are retained
ObservationViabilitySettingsList filterObservationViabilitySettings(
        const ObservationViabilitySettingsList& observationViabilitySettings,
        const LinkEnds& linkEnds )
{
    ObservationViabilitySettingsList filteredViabilitySettings;

    // Iterate over all viability settings
    for( unsigned int i = 0; i < observationViabilitySettings.size( ); i++ )
    {
        // Iterate over all link ends
        for( LinkEnds::const_iterator linkEndIterator = linkEnds.begin( ); linkEndIterator != linkEnds.end( ); linkEndIterator++ )
        {
            // Check if present viabilitytt setting is relevant
            if( linkEndIterator->second == observationViabilitySettings.at( i )->getAssociatedLinkEnd( ) ||
                    ( ( observationViabilitySettings.at( i )->getAssociatedLinkEnd( ).second == "" ) &&
                      ( observationViabilitySettings.at( i )->getAssociatedLinkEnd( ).first == linkEndIterator->second.first ) ) )
            {
                filteredViabilitySettings.push_back( observationViabilitySettings.at( i ) );
                break;
            }
        }
    }

    return filteredViabilitySettings;
}

//! Function to create an object to check if a minimum elevation angle condition is met for an observation
std::shared_ptr< MinimumElevationAngleCalculator > createMinimumElevationAngleCalculator(
        const simulation_setup::SystemOfBodies& bodies,
        const LinkEnds linkEnds,
        const ObservableType observationType,
        const std::shared_ptr< ObservationViabilitySettings > observationViabilitySettings,
        const std::string& stationName )
{
    if( observationViabilitySettings->observationViabilityType_ != minimum_elevation_angle )
    {
        throw std::runtime_error( "Error when making minimum elevation angle calculator, inconsistent input" );
    }

    // If specific link end is specified
    std::string groundStationNameToUse;
    if( observationViabilitySettings->getAssociatedLinkEnd( ).second != "" )
    {
        groundStationNameToUse = observationViabilitySettings->getAssociatedLinkEnd( ).second;
        if( groundStationNameToUse != stationName )
        {
            throw std::runtime_error( "Error when making minimum elevation angle calculator, inconsistent station input" );
        }
    }
    else
    {
        groundStationNameToUse = stationName;
    }

    if( bodies.count( observationViabilitySettings->getAssociatedLinkEnd( ).first ) == 0 )
    {
        throw std::runtime_error( "Error when making minimum elevation angle calculator, body " +
                                  observationViabilitySettings->getAssociatedLinkEnd( ).first + " not found." );
    }

    // Retrieve pointing angles calculator
    std::shared_ptr< ground_stations::PointingAnglesCalculator > pointingAngleCalculator =
            bodies.at( observationViabilitySettings->getAssociatedLinkEnd( ).first )->
            getGroundStation( groundStationNameToUse )->getPointingAnglesCalculator( );

    // Create check object
    double minimumElevationAngle = observationViabilitySettings->getDoubleParameter( );
    return std::make_shared< MinimumElevationAngleCalculator >(
                getLinkStateAndTimeIndicesForLinkEnd(
                    linkEnds,observationType, observationViabilitySettings->getAssociatedLinkEnd( ) ),
                minimumElevationAngle, pointingAngleCalculator );
}

//! Function to create an object to check if a body avoidance angle condition is met for an observation
std::shared_ptr< BodyAvoidanceAngleCalculator > createBodyAvoidanceAngleCalculator(
        const simulation_setup::SystemOfBodies& bodies,
        const LinkEnds linkEnds,
        const ObservableType observationType,
        const std::shared_ptr< ObservationViabilitySettings > observationViabilitySettings )
{
    if( observationViabilitySettings->observationViabilityType_ != body_avoidance_angle )
    {
        throw std::runtime_error( "Error when making body avoidance angle calculator, inconsistent input" );
    }

    if( bodies.count( observationViabilitySettings->getStringParameter( ) ) == 0 )
    {
        throw std::runtime_error( "Error when making body avoidance angle calculator, body " +
                                  observationViabilitySettings->getStringParameter( ) + " not found." );
    }

    // Create state function of body to be avoided.
    std::function< Eigen::Vector6d( const double ) > stateFunctionOfBodyToAvoid =
            std::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris< double, double >,
                         bodies.at( observationViabilitySettings->getStringParameter( ) ), std::placeholders::_1 );

    // Create check object
    double bodyAvoidanceAngle = observationViabilitySettings->getDoubleParameter( );
    return std::make_shared< BodyAvoidanceAngleCalculator >(
                getLinkStateAndTimeIndicesForLinkEnd(
                    linkEnds,observationType, observationViabilitySettings->getAssociatedLinkEnd( ) ),
                bodyAvoidanceAngle, stateFunctionOfBodyToAvoid, observationViabilitySettings->getStringParameter( ) );
}

//! Function to create an object to check if a body occultation condition is met for an observation
std::shared_ptr< OccultationCalculator > createOccultationCalculator(
        const simulation_setup::SystemOfBodies& bodies,
        const LinkEnds linkEnds,
        const ObservableType observationType,
        const std::shared_ptr< ObservationViabilitySettings > observationViabilitySettings )
{
    if( observationViabilitySettings->observationViabilityType_ != body_occultation )
    {
        throw std::runtime_error( "Error when making occultation calculator, inconsistent input" );
    }

    if( bodies.count( observationViabilitySettings->getStringParameter( ) ) == 0 )
    {
        throw std::runtime_error( "Error when making occultation calculator, body " +
                                  observationViabilitySettings->getStringParameter( ) + " not found." );
    }

    // Create state function of occulting body.
    std::function< Eigen::Vector6d( const double ) > stateOfOccultingBody =
            std::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris< double, double >,
                         bodies.at( observationViabilitySettings->getStringParameter( ) ), std::placeholders::_1 );

    // Create check object
    if( bodies.at( observationViabilitySettings->getStringParameter( ) )->getShapeModel( ) == nullptr )
    {
        throw std::runtime_error( "Error when makig occultation calculator, no shape model found for " +
                                  observationViabilitySettings->getStringParameter( ) );
    }
    double occultingBodyRadius =
            bodies.at( observationViabilitySettings->getStringParameter( ) )->getShapeModel( )->getAverageRadius( );
    return std::make_shared< OccultationCalculator >(
                getLinkStateAndTimeIndicesForLinkEnd(
                    linkEnds, observationType, observationViabilitySettings->getAssociatedLinkEnd( ) ),
                stateOfOccultingBody, occultingBodyRadius );
}

//! Function to create an list of obervation viability conditions for a single set of link ends
std::vector< std::shared_ptr< ObservationViabilityCalculator > > createObservationViabilityCalculators(
        const simulation_setup::SystemOfBodies& bodies,
        const LinkEnds linkEnds,
        const ObservableType observationType,
        const std::vector< std::shared_ptr< ObservationViabilitySettings > >& observationViabilitySettings )
{
    std::vector< std::shared_ptr< ObservationViabilityCalculator > > linkViabilityCalculators;

    std::vector< std::shared_ptr< ObservationViabilitySettings > > relevantObservationViabilitySettings =
            filterObservationViabilitySettings( observationViabilitySettings, linkEnds );

    for( unsigned int i = 0; i < relevantObservationViabilitySettings.size( ); i++ )
    {

        switch( relevantObservationViabilitySettings.at( i )->observationViabilityType_ )
        {
        case minimum_elevation_angle:
        {
            // Create list of ground stations for which elevation angle check is to be made.
            std::vector< std::string > listOfGroundStations;
            for( LinkEnds::const_iterator linkEndIterator = linkEnds.begin( );
                 linkEndIterator != linkEnds.end( ); linkEndIterator++ )
            {
                if( linkEndIterator->second.first == relevantObservationViabilitySettings.at( i )->getAssociatedLinkEnd( ).first )
                {
                    if( std::find( listOfGroundStations.begin( ), listOfGroundStations.end( ), linkEndIterator->second.second ) ==
                            listOfGroundStations.end( ) )
                    {
                        listOfGroundStations.push_back( linkEndIterator->second.second );
                    }
                }
            }

            // Create elevation angle check separately for eah ground station: check requires different pointing angles calculator
            for( unsigned int j = 0; j < listOfGroundStations.size( ); j++ )
            {
                linkViabilityCalculators.push_back(
                            createMinimumElevationAngleCalculator(
                                bodies, linkEnds, observationType, relevantObservationViabilitySettings.at( i ),
                                listOfGroundStations.at( j ) ) );
            }
            break;
        }
        case body_avoidance_angle:

            linkViabilityCalculators.push_back(
                        createBodyAvoidanceAngleCalculator(
                            bodies, linkEnds, observationType, relevantObservationViabilitySettings.at( i ) ) );
            break;
        case body_occultation:

            linkViabilityCalculators.push_back(
                        createOccultationCalculator(
                            bodies, linkEnds, observationType, relevantObservationViabilitySettings.at( i ) ) );
            break;
        default:
            throw std::runtime_error(
                        "Error when making observation viability calculator, type not recognized " +
                        std::to_string(
                            relevantObservationViabilitySettings.at( i )->observationViabilityType_ ) );
        }

    }

    return linkViabilityCalculators;
}

//! Function to create an list of obervation viability conditions for a number of sets of link ends, for a single observable type
std::map< LinkEnds, std::vector< std::shared_ptr< ObservationViabilityCalculator > > >
createObservationViabilityCalculators(
        const simulation_setup::SystemOfBodies& bodies,
        const std::vector< LinkEnds > linkEnds,
        const ObservableType observationType,
        const std::vector< std::shared_ptr< ObservationViabilitySettings > >& observationViabilitySettings )
{
    std::map< LinkEnds, std::vector< std::shared_ptr< ObservationViabilityCalculator > > > viabilityCalculators;

    for( unsigned int i = 0; i < linkEnds.size( ); i++ )
    {
        viabilityCalculators[ linkEnds.at( i ) ] =
                createObservationViabilityCalculators(
                    bodies, linkEnds.at( i ), observationType, observationViabilitySettings );
    }

    return viabilityCalculators;
}

} // namespace observation_models

} // namespace tudat


#ifndef STAN_MCMC_HMC_NUTS_BASE_NUTS_HPP
#define STAN_MCMC_HMC_NUTS_BASE_NUTS_HPP

#include <stan/callbacks/logger.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <stan/math/prim/scal.hpp>
#include <stan/mcmc/hmc/base_hmc.hpp>
#include <stan/mcmc/hmc/hamiltonians/ps_point.hpp>
#include <algorithm>
#include <cmath>
#include <limits>
#include <string>
#include <vector>

namespace stan {
  namespace mcmc {
    /**
     * The No-U-Turn sampler (NUTS) with multinomial sampling
     */
    template <class Model, template<class, class> class Hamiltonian,
              template<class> class Integrator, class BaseRNG>
    class base_nuts : public base_hmc<Model, Hamiltonian, Integrator, BaseRNG> {
    public:
      base_nuts(const Model& model, BaseRNG& rng)
        : base_hmc<Model, Hamiltonian, Integrator, BaseRNG>(model, rng),
          depth_(0), max_depth_(5), max_deltaH_(1000),
          n_leapfrog_(0), divergent_(false), energy_(0) {
      }

      /**
       * specialized constructor for specified diag mass matrix
       */
      base_nuts(const Model& model, BaseRNG& rng,
                Eigen::VectorXd& inv_e_metric)
        : base_hmc<Model, Hamiltonian, Integrator, BaseRNG>(model, rng,
                                                            inv_e_metric),
          depth_(0), max_depth_(5), max_deltaH_(1000),
          n_leapfrog_(0), divergent_(false), energy_(0) {
      }

      /**
       * specialized constructor for specified dense mass matrix
       */
      base_nuts(const Model& model, BaseRNG& rng,
                Eigen::MatrixXd& inv_e_metric)
        : base_hmc<Model, Hamiltonian, Integrator, BaseRNG>(model, rng,
                                                            inv_e_metric),
        depth_(0), max_depth_(5), max_deltaH_(1000),
        n_leapfrog_(0), divergent_(false), energy_(0) {
      }

      ~base_nuts() {}

      void set_metric(const Eigen::MatrixXd& inv_e_metric) {
        this->z_.set_metric(inv_e_metric);
      }

      void set_metric(const Eigen::VectorXd& inv_e_metric) {
        this->z_.set_metric(inv_e_metric);
      }

      void set_max_depth(int d) {
        if (d > 0)
          max_depth_ = d;
      }

      void set_max_delta(double d) {
        max_deltaH_ = d;
      }

      int get_max_depth() { return this->max_depth_; }
      double get_max_delta() { return this->max_deltaH_; }

      sample
      transition(sample& init_sample, callbacks::logger& logger) {
        // Initialize the algorithm
        this->sample_stepsize();

        this->seed(init_sample.cont_params());

        this->hamiltonian_.sample_p(this->z_, this->rand_int_);
        this->hamiltonian_.init(this->z_, logger);

        ps_point z_plus(this->z_);
        ps_point z_minus(z_plus);

        ps_point z_sample(z_plus);
        ps_point z_propose(z_plus);

        Eigen::VectorXd p_sharp_plus = this->hamiltonian_.dtau_dp(this->z_);
        Eigen::VectorXd p_sharp_dummy = p_sharp_plus;
        Eigen::VectorXd p_sharp_minus = p_sharp_plus;
        Eigen::VectorXd rho = this->z_.p;

        double log_sum_weight = 0;  // log(exp(H0 - H0))
        double H0 = this->hamiltonian_.H(this->z_);
        int n_leapfrog = 0;
        double sum_metro_prob = 0;

        // Build a trajectory until the NUTS criterion is no longer satisfied
        this->depth_ = 0;
        this->divergent_ = false;

        while (this->depth_ < this->max_depth_) {
          // Build a new subtree in a random direction
          Eigen::VectorXd rho_subtree = Eigen::VectorXd::Zero(rho.size());
          bool valid_subtree = false;
          double log_sum_weight_subtree
            = -std::numeric_limits<double>::infinity();

          if (this->rand_uniform_() > 0.5) {
            this->z_.ps_point::operator=(z_plus);
            valid_subtree
              = build_tree(this->depth_, z_propose,
                           p_sharp_dummy, p_sharp_plus, rho_subtree,
                           H0, 1, n_leapfrog,
                           log_sum_weight_subtree, sum_metro_prob,
                           logger);
            z_plus.ps_point::operator=(this->z_);
          } else {
            this->z_.ps_point::operator=(z_minus);
            valid_subtree
              = build_tree(this->depth_, z_propose,
                           p_sharp_dummy, p_sharp_minus, rho_subtree,
                           H0, -1, n_leapfrog,
                           log_sum_weight_subtree, sum_metro_prob,
                           logger);
            z_minus.ps_point::operator=(this->z_);
          }

          if (!valid_subtree) break;

          // Sample from an accepted subtree
          ++(this->depth_);

          if (log_sum_weight_subtree > log_sum_weight) {
            z_sample = z_propose;
          } else {
            double accept_prob
              = std::exp(log_sum_weight_subtree - log_sum_weight);
            if (this->rand_uniform_() < accept_prob)
              z_sample = z_propose;
          }

          log_sum_weight
            = math::log_sum_exp(log_sum_weight, log_sum_weight_subtree);

          // Break when NUTS criterion is no longer satisfied
          rho += rho_subtree;
          if (!compute_criterion(p_sharp_minus, p_sharp_plus, rho))
            break;
        }

        this->n_leapfrog_ = n_leapfrog;

        // Compute average acceptance probabilty across entire trajectory,
        // even over subtrees that may have been rejected
        double accept_prob
          = sum_metro_prob / static_cast<double>(n_leapfrog);

        this->z_.ps_point::operator=(z_sample);
        this->energy_ = this->hamiltonian_.H(this->z_);
        return sample(this->z_.q, -this->z_.V, accept_prob);
      }

      void get_sampler_param_names(std::vector<std::string>& names) {
        names.push_back("stepsize__");
        names.push_back("treedepth__");
        names.push_back("n_leapfrog__");
        names.push_back("divergent__");
        names.push_back("energy__");
      }

      void get_sampler_params(std::vector<double>& values) {
        values.push_back(this->epsilon_);
        values.push_back(this->depth_);
        values.push_back(this->n_leapfrog_);
        values.push_back(this->divergent_);
        values.push_back(this->energy_);
      }

      virtual bool compute_criterion(Eigen::VectorXd& p_sharp_minus,
                                     Eigen::VectorXd& p_sharp_plus,
                                     Eigen::VectorXd& rho) {
        return    p_sharp_plus.dot(rho) > 0
               && p_sharp_minus.dot(rho) > 0;
      }

      /**
       * Recursively build a new subtree to completion or until
       * the subtree becomes invalid.  Returns validity of the
       * resulting subtree.
       *
       * @param depth Depth of the desired subtree
       * @param z_propose State proposed from subtree
       * @param p_sharp_left p_sharp from left boundary of returned tree
       * @param p_sharp_right p_sharp from the right boundary of returned tree
       * @param rho Summed momentum across trajectory
       * @param H0 Hamiltonian of initial state
       * @param sign Direction in time to built subtree
       * @param n_leapfrog Summed number of leapfrog evaluations
       * @param log_sum_weight Log of summed weights across trajectory
       * @param sum_metro_prob Summed Metropolis probabilities across trajectory
       * @param logger Logger for messages
      */
      bool build_tree(int depth, ps_point& z_propose,
                      Eigen::VectorXd& p_sharp_left,
                      Eigen::VectorXd& p_sharp_right,
                      Eigen::VectorXd& rho,
                      double H0, double sign, int& n_leapfrog,
                      double& log_sum_weight, double& sum_metro_prob,
                      callbacks::logger& logger) {
        // Base case
        if (depth == 0) {
          this->integrator_.evolve(this->z_, this->hamiltonian_,
                                   sign * this->epsilon_,
                                   logger);
          ++n_leapfrog;

          double h = this->hamiltonian_.H(this->z_);
          if (boost::math::isnan(h))
            h = std::numeric_limits<double>::infinity();

          if ((h - H0) > this->max_deltaH_) this->divergent_ = true;

          log_sum_weight = math::log_sum_exp(log_sum_weight, H0 - h);

          if (H0 - h > 0)
            sum_metro_prob += 1;
          else
            sum_metro_prob += std::exp(H0 - h);

          z_propose = this->z_;
          rho += this->z_.p;

          p_sharp_left = this->hamiltonian_.dtau_dp(this->z_);
          p_sharp_right = p_sharp_left;

          return !this->divergent_;
        }
        // General recursion
        Eigen::VectorXd p_sharp_dummy(this->z_.p.size());

        // Build the left subtree
        double log_sum_weight_left = -std::numeric_limits<double>::infinity();
        Eigen::VectorXd rho_left = Eigen::VectorXd::Zero(rho.size());

        bool valid_left
          = build_tree(depth - 1, z_propose,
                       p_sharp_left, p_sharp_dummy, rho_left,
                       H0, sign, n_leapfrog,
                       log_sum_weight_left, sum_metro_prob,
                       logger);

        if (!valid_left) return false;

        // Build the right subtree
        ps_point z_propose_right(this->z_);

        double log_sum_weight_right = -std::numeric_limits<double>::infinity();
        Eigen::VectorXd rho_right = Eigen::VectorXd::Zero(rho.size());

        bool valid_right
          = build_tree(depth - 1, z_propose_right,
                       p_sharp_dummy, p_sharp_right, rho_right,
                       H0, sign, n_leapfrog,
                       log_sum_weight_right, sum_metro_prob,
                       logger);

        if (!valid_right) return false;

        // Multinomial sample from right subtree
        double log_sum_weight_subtree
          = math::log_sum_exp(log_sum_weight_left, log_sum_weight_right);
        log_sum_weight
          = math::log_sum_exp(log_sum_weight, log_sum_weight_subtree);

        if (log_sum_weight_right > log_sum_weight_subtree) {
          z_propose = z_propose_right;
        } else {
          double accept_prob
            = std::exp(log_sum_weight_right - log_sum_weight_subtree);
          if (this->rand_uniform_() < accept_prob)
            z_propose = z_propose_right;
        }

        Eigen::VectorXd rho_subtree = rho_left + rho_right;
        rho += rho_subtree;

        return compute_criterion(p_sharp_left, p_sharp_right, rho_subtree);
      }

      int depth_;
      int max_depth_;
      double max_deltaH_;

      int n_leapfrog_;
      bool divergent_;
      double energy_;
    };

  }  // mcmc
}  // stan
#endif


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_PositronionizationSubshellPositronatomicReaction_def.hpp
//! \author Luke Kersting
//! \brief  The positron-ionization subshell positron-atomic reaction class definition
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_POSITRONIONIZATION_SUBSHELL_POSITRONATOMIC_REACTION_DEF_HPP
#define MONTE_CARLO_POSITRONIONIZATION_SUBSHELL_POSITRONATOMIC_REACTION_DEF_HPP

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "MonteCarlo_PositronionizationPositronatomicReaction.hpp"
#include "MonteCarlo_PositronatomicReactionType.hpp"
#include "Utility_DesignByContract.hpp"

namespace MonteCarlo{

// Basic Constructor
template<typename InterpPolicy, bool processed_cross_section>
PositronionizationSubshellPositronatomicReaction<InterpPolicy,processed_cross_section>::PositronionizationSubshellPositronatomicReaction(
    const std::shared_ptr<const std::vector<double> >& incoming_energy_grid,
    const std::shared_ptr<const std::vector<double> >& cross_section,
    const size_t threshold_energy_index,
    const Data::SubshellType interaction_subshell,
    const std::shared_ptr<const ElectroionizationSubshellElectronScatteringDistribution>&
            electroionization_subshell_distribution )
  : BaseType( incoming_energy_grid,
              cross_section,
              threshold_energy_index ),
    d_interaction_subshell( interaction_subshell ),
    d_electroionization_subshell_distribution( electroionization_subshell_distribution ),
    d_reaction_type( convertSubshellEnumToPositronionizationPositronatomicReactionEnum(
                                                        interaction_subshell ) )
{
  // Make sure the interaction subshell is valid
  testPrecondition( interaction_subshell != Data::INVALID_SUBSHELL );
  testPrecondition( interaction_subshell !=Data::UNKNOWN_SUBSHELL );

  // Make sure the distribution data is valid
  testPrecondition( electroionization_subshell_distribution.use_count() > 0 );

  // Make sure the threshold energy isn't less than the binding energy
  testPrecondition( (*incoming_energy_grid)[threshold_energy_index] >=
                    d_electroionization_subshell_distribution->getBindingEnergy() );
}

// Constructor
template<typename InterpPolicy, bool processed_cross_section>
PositronionizationSubshellPositronatomicReaction<InterpPolicy,processed_cross_section>::PositronionizationSubshellPositronatomicReaction(
    const std::shared_ptr<const std::vector<double> >& incoming_energy_grid,
    const std::shared_ptr<const std::vector<double> >& cross_section,
    const size_t threshold_energy_index,
    const std::shared_ptr<const Utility::HashBasedGridSearcher<double>>& grid_searcher,
    const Data::SubshellType interaction_subshell,
    const std::shared_ptr<const ElectroionizationSubshellElectronScatteringDistribution>&
            electroionization_subshell_distribution )
  : BaseType( incoming_energy_grid,
              cross_section,
              threshold_energy_index,
              grid_searcher ),
    d_interaction_subshell( interaction_subshell ),
    d_electroionization_subshell_distribution(
            electroionization_subshell_distribution ),
    d_reaction_type( convertSubshellEnumToPositronionizationPositronatomicReactionEnum(
            interaction_subshell ) )
{
  // Make sure the interaction subshell is valid
  testPrecondition( interaction_subshell != Data::INVALID_SUBSHELL );
  testPrecondition( interaction_subshell !=Data::UNKNOWN_SUBSHELL );

  // Make sure the distribution data is valid
  testPrecondition( electroionization_subshell_distribution.use_count() > 0 );

  // Make sure the threshold energy isn't less than the binding energy
  testPrecondition( (*incoming_energy_grid)[threshold_energy_index] >=
                    d_electroionization_subshell_distribution->getBindingEnergy() );
}

// Return the number of electrons emitted from the rxn at the given energy
/*! \details A knock-on electron from this subshell will be emitted.
 */
template<typename InterpPolicy, bool processed_cross_section>
unsigned PositronionizationSubshellPositronatomicReaction<InterpPolicy,processed_cross_section>::getNumberOfEmittedElectrons( const double energy ) const
{
  if( energy >= this->getThresholdEnergy() )
    return 1u;
  else
    return 0u;
}

// Return the differential cross section
template<typename InterpPolicy, bool processed_cross_section>
double PositronionizationSubshellPositronatomicReaction<InterpPolicy,processed_cross_section>::getDifferentialCrossSection(
    const double incoming_energy,
    const double outgoing_energy ) const
{
  // Make sure the energies are valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( outgoing_energy >= 0.0 );
  testPrecondition( outgoing_energy <= incoming_energy );

  if ( !this->isEnergyWithinEnergyGrid( incoming_energy ) )
    return 0.0;

  // Evaluate the forward cross section at the incoming energy
  double forward_cs = this->getCrossSection( incoming_energy );

  // Sample the pdf using the energy of the knock-on electron
  double pdf = d_electroionization_subshell_distribution->evaluatePDF(
          incoming_energy,
          outgoing_energy );

  return forward_cs*pdf;
}

// Simulate the reaction
template<typename InterpPolicy, bool processed_cross_section>
void PositronionizationSubshellPositronatomicReaction<InterpPolicy,processed_cross_section>::react(
     PositronState& positron,
     ParticleBank& bank,
     Data::SubshellType& shell_of_interaction ) const
{
  // Make sure the positron energy isn't less than the binding energy
  testPrecondition( positron.getEnergy() >=
                    d_electroionization_subshell_distribution->getBindingEnergy() );

  d_electroionization_subshell_distribution->scatterPositron(
                                               positron,
                                               bank,
                                               shell_of_interaction);

  positron.incrementCollisionNumber();

  shell_of_interaction = d_interaction_subshell;
}

// Return the reaction type
template<typename InterpPolicy, bool processed_cross_section>
PositronatomicReactionType PositronionizationSubshellPositronatomicReaction<InterpPolicy,processed_cross_section>::getReactionType() const
{
  return d_reaction_type;
}

// Get the interaction subshell (non-standard interface)
template<typename InterpPolicy, bool processed_cross_section>
unsigned PositronionizationSubshellPositronatomicReaction<InterpPolicy,processed_cross_section>::getSubshell() const
{
  return d_interaction_subshell;
}

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_POSITRONIONIZATION_SUBSHELL_POSITRONATOMIC_REACTION_DEF_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_PositronionizationSubshellPositronatomicReaction_def.hpp
//---------------------------------------------------------------------------//


/*
 * graph_solver.cpp
 *
 *  Created on: Aug 30, 2019
 *      Author: Jorge Nicho
 */

#include <numeric>

#include <fstream>

#include <memory>

#include <console_bridge/console.h>

#include <boost/format.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>

#include "descartes_planner/bdsp_graph_planner.h"

static const int VIRTUAL_VERTEX_INDEX = -1;

namespace descartes_planner
{

template<typename FloatT>
descartes_planner::BDSPGraphPlanner<FloatT>::BDSPGraphPlanner(typename std::shared_ptr< SamplesContainer<FloatT> > container,
                                                    bool report_all_failures):
  container_(container),
  report_all_failures_(report_all_failures)
{
  if(container_ == nullptr)
  {
    // if no container is provided then use default implementation
    container_ = std::make_shared< DefaultSamplesContainer<FloatT> >();
  }
}

template<typename FloatT>
descartes_planner::BDSPGraphPlanner<FloatT>::~BDSPGraphPlanner()
{

}

template<typename FloatT>
typename EdgeEvaluator<FloatT>::ConstPtr descartes_planner::BDSPGraphPlanner<FloatT>::getEdgeEvaluator(std::uint32_t idx)
{
  if(edge_evaluators_.size() == 1)
  {
    return edge_evaluators_.front();
  }
  else
  {
    return edge_evaluators_.at(idx);
  }
}

template<typename FloatT>
void descartes_planner::BDSPGraphPlanner<FloatT>::setup(std::vector< typename PointSampler<FloatT>::Ptr >& points,
                                                   std::vector<typename EdgeEvaluator<FloatT>::ConstPtr>& edge_evaluators)
{

  failed_points_.clear();
  failed_edges_.clear();

  // setting up point sampler container
  points_.clear();
  std::copy(points.begin(),points.end(),std::back_inserter(points_));
  container_->clear();
  container_->allocate(points_.size());

  // setting up edge evaluators
  edge_evaluators_.clear();
  if(edge_evaluators.size() == 1)
  {
    edge_evaluators_ = edge_evaluators;
  }
  else if(edge_evaluators.size() == points_.size() - 1)
  {
    std::copy(edge_evaluators.begin(),edge_evaluators.end(),std::back_inserter(edge_evaluators_));
  }
  else if(edge_evaluators.empty())
  {
    throw std::runtime_error("Edge evaluators vector is empty");
  }
  else if(edge_evaluators.size() > 1 && edge_evaluators.size() != points.size() - 1)
  {
    throw std::runtime_error(boost::str(
        boost::format("Edge evaluators vector's size (%lu) must be one less than that of the points vector (%lu)") %
        edge_evaluators.size() % points.size() ) );
  }
}

template<typename FloatT>
bool descartes_planner::BDSPGraphPlanner<FloatT>::build(std::vector< typename PointSampler<FloatT>::Ptr >& points,
           typename EdgeEvaluator<FloatT>::ConstPtr edge_evaluator)
{
  // setting up edge evaluators
  std::vector<typename EdgeEvaluator<FloatT>::ConstPtr> edge_evaluators = {edge_evaluator};
  return build(points, edge_evaluators);
}

template<typename FloatT>
std::vector< EdgeProperties<FloatT> > descartes_planner::BDSPGraphPlanner<FloatT>::filterDisconnectedEdges(
    const std::vector< EdgeProperties<FloatT> >& edges,const std::map<std::size_t, VertexProperties>& connected_src_vertices,
    std::uint32_t current_vertex_count) const
{
  auto new_edges = edges;
  auto start_loc = new_edges.begin();
  auto end_loc = new_edges.end();
  auto new_end_loc = std::remove_if(new_edges.begin(), new_edges.end(), [&](const EdgeProperties<FloatT>& edge){
    int src_idx = edge.src_vtx.sample_index + current_vertex_count;
    return connected_src_vertices.count(src_idx) == 0;
  });

  new_edges.erase(new_end_loc, end_loc);
  return new_edges;
}

template<typename FloatT>
bool descartes_planner::BDSPGraphPlanner<FloatT>::build(std::vector<typename PointSampler<FloatT>::Ptr>& points,
                                                   std::vector<typename EdgeEvaluator<FloatT>::ConstPtr>& edge_evaluators)
{
  setup(points, edge_evaluators);

  //// adding virtual vertex
  graph_.clear();

  // generating samples now
  std::size_t max_num_samples = 0;
  for(std::size_t  i = 0; i < points_.size(); i++)
  {
    typename PointSampleGroup<FloatT>::Ptr samples = points_[i]->generate();
    if(!samples || samples->values.empty())
    {
      CONSOLE_BRIDGE_logError("Failed to generate samples for point %lu",i);
      failed_points_.push_back(i);
      if(report_all_failures_)
      {
        continue;
      }
      return false;
    }
    container_->at(i) = samples;
    max_num_samples = max_num_samples < samples->num_samples ? samples->num_samples : max_num_samples;
  }

  // no need to proceed if sample generation failed
  if(!failed_points_.empty())
  {
    CONSOLE_BRIDGE_logError("Failed to generate one or more point samples, use getFailedPoints to get the failed points");
    return false;
  }

  // build the graph now
  typename PointSampleGroup<FloatT>::Ptr samples1 = nullptr;
  typename PointSampleGroup<FloatT>::Ptr samples2 = nullptr;

  std::uint32_t vertex_count = 1;
  bool add_virtual_vertex = true;
  std::map<std::size_t, VertexProperties> src_vertices_added;
  std::map<std::size_t, VertexProperties> dst_vertices_added;
  std::vector<bool> dst_sample_indices_added(max_num_samples, false);
  std::vector<std::size_t> src_vertices_disconnected;
  src_vertices_disconnected.reserve(max_num_samples);

  // use samples to populate edges in order to build the search graph
  for(std::size_t i = 1; i < points_.size(); i++)
  {

    // geting samples for both points
    std::size_t p1_idx = i -1;
    std::size_t p2_idx = i;

    samples1 = (*container_)[p1_idx];
    samples2 = (*container_)[p2_idx];
    samples1->point_id = p1_idx; // TODO: setting ids may not be necessary
    samples2->point_id = p2_idx;

    // validating vertex samples
    using SampleMap = std::map< std::size_t, typename PointSampleGroup<FloatT>::Ptr >;
    SampleMap sample_groups = {{p1_idx, samples1}, {p2_idx, samples2}};
    if(!std::all_of(sample_groups.begin(), sample_groups.end(),[](typename SampleMap::value_type& kv){
      if(kv.second == nullptr)
      {
        CONSOLE_BRIDGE_logError("Invalid samples received for point with index %lu",kv.first);
        return false;
      }
      if(kv.second->values.empty())
      {
        CONSOLE_BRIDGE_logError("No valid samples were found in point %lu",kv.first);
        return false;
      }
      return true;
    }))
    {
      return false;
    }

    // updating vector of disconnected vertices in source point
    src_vertices_disconnected.clear();
    if(!dst_vertices_added.empty())
    {
      for(std::size_t ii = 0; ii < samples1->num_samples; ii++)
      {
        if(!dst_sample_indices_added[ii])
        {
          src_vertices_disconnected.push_back(ii);
        }
      }
      CONSOLE_BRIDGE_logDebug("Found %lu disconnected samples of %lu in source point %lu",
                             src_vertices_disconnected.size(), samples1->num_samples, p1_idx);
    }

    // reseting array
    std::fill(dst_sample_indices_added.begin(), dst_sample_indices_added.end(), false);

    // evaluate edges
    using EdgeProp = EdgeProperties<FloatT>;
    auto edge_evaluator = getEdgeEvaluator(p1_idx);
    std::vector< EdgeProperties<FloatT> > edges = edge_evaluator->evaluate(samples1, samples2, src_vertices_disconnected, {});

    if(edges.empty())
    {
      CONSOLE_BRIDGE_logError("Edge evaluation between points %lu and %lu failed", samples1->point_id,
                              samples2->point_id);
      failed_edges_.push_back(p1_idx);
      if(report_all_failures_)
      {
        continue;
      }
      return false;
    }

    // no need to proceed if an edge has already failed
    if(!failed_edges_.empty())
    {
      continue;
    }

    CONSOLE_BRIDGE_logDebug("Found %lu edges between nodes (%i, %i)",edges.size(),samples1->point_id ,samples2->point_id );

    // check that at least one is valid
    std::size_t num_valid_edges = std::accumulate(edges.begin(), edges.end(),0,[](std::size_t c, const EdgeProperties<FloatT>& edge){
      return c + (edge.valid ? 1 : 0);
    });
    if(num_valid_edges == 0)
    {
      CONSOLE_BRIDGE_logError("Not a single valid edge was found between points (%lu, %lu)",p1_idx,p2_idx);
      return false;
    }
    else
    {
      CONSOLE_BRIDGE_logDebug("Point (%lu, %lu) has %lu valid edges out of %lu = %i x %i",p1_idx,p2_idx,num_valid_edges,edges.size(),
                               samples1->num_samples, samples2->num_samples);
    }

    // filtering disconnected edges
    if(!dst_vertices_added.empty())
    {
      edges = filterDisconnectedEdges(edges, dst_vertices_added, vertex_count);
      if(edges.empty())
      {
        CONSOLE_BRIDGE_logError("Edge between points %lu and %lu has no continuous path", samples1->point_id,
                                samples2->point_id);
        failed_edges_.push_back(p1_idx);
        if(report_all_failures_)
        {
          continue;
        }
        return false;
      }
    }

    src_vertices_added.clear();
    dst_vertices_added.clear();
    for(EdgeProp& edge: edges)
    {
      if(!edge.valid)
      {
        continue;
      }

      if(edge.weight >= std::numeric_limits<FloatT>::max())
      {
        CONSOLE_BRIDGE_logError("Found edge with very high weight value between points (%lu, %lu)", p1_idx, p2_idx);
        continue;
      }

      bool added;

      std::uint32_t src_vtx_index = edge.src_vtx.sample_index + vertex_count;
      std::uint32_t dst_vtx_index =  edge.dst_vtx.sample_index + vertex_count + samples1->num_samples;

      if(src_vtx_index >= dst_vtx_index)
      {
        CONSOLE_BRIDGE_logError("Found equal source and destination vertices values at iteration", i);
        return false;
      }

      if(src_vtx_index <= 0)
      {
        CONSOLE_BRIDGE_logError("Source vertex is negative at iteration %i", i);
        return false;
      }

      // adding edge to virtual vertex first
      if(add_virtual_vertex && (src_vertices_added.count(src_vtx_index) == 0))
      {
        typename GraphT::edge_descriptor e;

        CONSOLE_BRIDGE_logDebug("Adding edge (0, %lu) to virtual vertex",src_vtx_index);
        VertexProperties virtual_vertex_props;
        virtual_vertex_props.point_id = VIRTUAL_VERTEX_INDEX;
        virtual_vertex_props.sample_index = 0;
        EdgeProperties<FloatT> virtual_edge= { .weight = 0, .valid = edge.valid,
                                               .src_vtx = virtual_vertex_props, .dst_vtx = edge.src_vtx};
        boost::tie(e,added) = boost::add_edge(0, src_vtx_index, graph_);
        if(!added)
        {
          CONSOLE_BRIDGE_logWarn("Edge (%lu, %lu) has already been added to the graphs",0,
                                 src_vtx_index);
          return false;
        }
        graph_[e] = virtual_edge;

      }

      typename GraphT::edge_descriptor e;
      boost::tie(e,added) = boost::add_edge(src_vtx_index, dst_vtx_index, graph_);
      CONSOLE_BRIDGE_logDebug("Added edge (%lu, %lu)",src_vtx_index, dst_vtx_index);
      if(!added)
      {
        CONSOLE_BRIDGE_logError("Edge (%lu, %lu) has already been added to the graphs",src_vtx_index,
                                dst_vtx_index);
        return false;
      }
      else
      {
        // setting edge properties
        graph_[e]= edge;
      }

      src_vertices_added[src_vtx_index] = edge.src_vtx;
      dst_vertices_added[dst_vtx_index] = edge.dst_vtx;
      dst_sample_indices_added[edge.dst_vtx.sample_index] = true;
    }

    if(src_vertices_added.empty() || dst_vertices_added.empty())
    {
      CONSOLE_BRIDGE_logError("No continuous path could be found between points %i and %i",p1_idx, p2_idx);
      failed_edges_.push_back(p1_idx);
      if(report_all_failures_)
      {
        continue;
      }
      return false;
    }

    vertex_count += samples1->num_samples;
    add_virtual_vertex = false; // do not add edges for the virtual vertex anymore

  }

  if(!failed_edges_.empty())
  {
    CONSOLE_BRIDGE_logError("Failed to generate one or more point samples, use getFailedEdges to get the failed edges");
    return false;
  }

  end_vertices_ = dst_vertices_added;
  return true;
}

template<typename FloatT>
void descartes_planner::BDSPGraphPlanner<FloatT>::getFailedEdges(std::vector<std::size_t>& failed_edges)
{
  failed_edges = failed_edges_;
}

template<typename FloatT>
void descartes_planner::BDSPGraphPlanner<FloatT>::getFailedPoints(std::vector<std::size_t>& failed_points)
{
  failed_points = failed_points_;
}

template<typename FloatT>
std::shared_ptr< const SamplesContainer<FloatT> > descartes_planner::BDSPGraphPlanner<FloatT>::getContainer() const
{
  return container_;
}

template<typename FloatT>
void descartes_planner::BDSPGraphPlanner<FloatT>::writeGraphLogs(const std::vector<FloatT>& weights,
                                                               const std::vector<typename GraphT::vertex_descriptor>& predecessors)
{
  typedef boost::graph_traits<GraphT> GraphTraits;

  // declare file names
  const std::string dot_file_name = "dijkstra-no-color-map-eg.dot";
  const std::string txt_file_name = "dijkstra_shortest_path_dump.txt";

  // writing dot file
  std::ofstream dot_file(dot_file_name);

  dot_file << "digraph D {\n"
    << "  rankdir=LR\n"
    << "  size=\"4,3\"\n"
    << "  ratio=\"fill\"\n"
    << "  edge[style=\"bold\"]\n" << "  node[shape=\"circle\"]\n";

  typename GraphTraits::edge_iterator  ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(graph_); ei != ei_end; ++ei) {
    typename GraphTraits::edge_descriptor e = *ei;
    typename GraphTraits::vertex_descriptor u = source(e, graph_), v = target(e, graph_);
    EdgeProperties<FloatT> edge_props = graph_[e];
    VertexProperties u_vertex = edge_props.src_vtx;
    VertexProperties v_vertex = edge_props.dst_vtx;

    if(u_vertex.point_id < 0 || v_vertex.point_id < 0)
    {
      continue;
    }

    dot_file << "\tP" << u_vertex.point_id << "_S" << u_vertex.sample_index <<"_G" << u << " -> ";
    dot_file << "P" << v_vertex.point_id << "_S" << v_vertex.sample_index <<"_G" << v;

    //dot_file << u << " -> " << v;
    dot_file << " [label=\"" << int(1e3 * edge_props.weight)  << "\"";
    if (predecessors[v] == u)
      dot_file << ", color=\"black\"";
    else
      dot_file << ", color=\"red\"";
    dot_file << "];\n";
  }
  dot_file << "}";

  // writing text file
  std::ofstream graph_file(txt_file_name);
  std::ostream& ref = graph_file;
  boost::print_graph(graph_, ref);
  graph_file.close();

  // print log names
  const auto log_file_names = {dot_file_name, txt_file_name};
  for(const auto& f : log_file_names)
  {
    CONSOLE_BRIDGE_logInform("wrote graph log file %s", f.c_str());
  }
}

template<typename FloatT>
bool descartes_planner::BDSPGraphPlanner<FloatT>::solve(
    std::vector<typename PointData<FloatT>::ConstPtr>& solution_points)
{
  typename GraphT::vertex_descriptor virtual_vertex = vertex(0, graph_), current_vertex;
  std::size_t num_vert = boost::num_vertices(graph_);
  std::vector<typename GraphT::vertex_descriptor> predecessors(num_vert);
  std::vector<FloatT> weights(num_vert, 0.0);

/*  boost::dijkstra_shortest_paths(graph_, virtual_vertex,
    weight_map(get(&EdgeProperties<FloatT>::weight, graph_))
    .distance_map(boost::make_iterator_property_map(weights.begin(),get(boost::vertex_index, graph_)))
    .predecessor_map(&predecessors[0]));*/

  CONSOLE_BRIDGE_logDebug("Descartes Searching through graph now ...");
  boost::dijkstra_shortest_paths_no_color_map(graph_, virtual_vertex,
   weight_map(get(&EdgeProperties<FloatT>::weight, graph_))
   .distance_map(boost::make_iterator_property_map(weights.begin(),get(boost::vertex_index, graph_)))
   .predecessor_map(boost::make_iterator_property_map(predecessors.begin(),get(boost::vertex_index, graph_))));
  CONSOLE_BRIDGE_logDebug("Descartes graph search completed");

  CONSOLE_BRIDGE_logDebug("Num vertices %i", num_vert);
  CONSOLE_BRIDGE_logDebug("Predecessor array size %lu",predecessors.size());
  CONSOLE_BRIDGE_logDebug("Weights array size %lu",weights.size());
  CONSOLE_BRIDGE_logDebug("End vertices size %lu", end_vertices_.size());

  // iterating through out edges while inspecting the predecessor
  typedef boost::graph_traits<GraphT> GraphTraits;
  typename GraphT::vertex_descriptor cheapest_end_vertex = -1;
  double cost = std::numeric_limits<FloatT>::infinity();

  for(const auto& kv: end_vertices_)
  {
    typename GraphT::vertex_descriptor candidate_vertex = kv.first;
    CONSOLE_BRIDGE_logDebug("Searching end vertex %i with cost %f", candidate_vertex,
                           weights[candidate_vertex]);
    if(weights[candidate_vertex] > cost)
    {
      CONSOLE_BRIDGE_logDebug("cost too high, skipping to next end vertex");
      continue;
    }
    cost = weights[candidate_vertex];

    // check if it is connected
    typename GraphT::vertex_descriptor prev_vertex = predecessors[candidate_vertex];
    typename GraphTraits::out_edge_iterator out_i, out_end;
    for(boost::tie(out_i, out_end)= boost::out_edges(prev_vertex, graph_); out_i != out_end; out_i++)
    {
      typename GraphTraits::edge_descriptor e = *out_i;
      typename GraphT::vertex_descriptor targ = boost::target(e, graph_);
      if(targ == candidate_vertex)
      {
        cheapest_end_vertex = candidate_vertex;
        break;
      }
    }
  }
  current_vertex = cheapest_end_vertex;
  if(static_cast<int>(current_vertex) < 0 )
  {
    CONSOLE_BRIDGE_logError("Found no continuous solution path through graph");
    writeGraphLogs(weights, predecessors);
    return false;
  }

  CONSOLE_BRIDGE_logInform("Found valid shortest path with end vertex: %i and cost %f", current_vertex, cost);

  solution_points.resize(container_->size(), nullptr);
  auto add_solution = [&](VertexProperties& vp) -> bool{

    if(vp.point_id  == VIRTUAL_VERTEX_INDEX)
    {
      // found virtual index, just return
      return true;
    }

    if(vp.point_id >= points_.size())
    {
      CONSOLE_BRIDGE_logError("Source vertex index %i exceeds point buffer of size %lu",vp.point_id, points_.size());
      return false;
    }

    typename PointSampleGroup<FloatT>::Ptr  sample_group = container_->at(vp.point_id);

    // recompute or retrieve the sample and storing it
    if(solution_points[vp.point_id] != nullptr) // can not have more than one solutions
    {
      CONSOLE_BRIDGE_logDebug("Sample for point %i has already been assigned", vp.point_id);
      return true;
    }

    typename PointData<FloatT>::ConstPtr point_data = sample_group->at(vp.sample_index);
    if(!point_data)
    {
      CONSOLE_BRIDGE_logError("SampleGroup %i has no sample %lu", vp.point_id, vp.sample_index);
      return false;
    }
    solution_points[vp.point_id] = point_data;
    CONSOLE_BRIDGE_logDebug("Added %s solution point %i of %lu points",(point_data != nullptr ? "valid" : "null"),
                             vp.point_id, solution_points.size());
    return true;
  };

  int vertex_counter = 0;
  while(current_vertex != virtual_vertex)
  {
    typename GraphT::vertex_descriptor prev_vertex = predecessors[current_vertex];
    typename GraphTraits::out_edge_iterator out_i, out_end;
    bool found_next = false;
    for(boost::tie(out_i, out_end)= boost::out_edges(prev_vertex, graph_); out_i != out_end; out_i++)
    {
      typename GraphTraits::edge_descriptor e = *out_i;
      typename GraphT::vertex_descriptor targ = boost::target(e, graph_);
      if(targ == current_vertex)
      {
        found_next = true;
        current_vertex = prev_vertex;

        // grab sampler
        EdgeProperties<FloatT> edge_props = graph_[e];
        CONSOLE_BRIDGE_logDebug("Points %lu and %lu connected by edge (%lu, %lu)",
                                 edge_props.src_vtx.point_id, edge_props.dst_vtx.point_id,
                                 prev_vertex, targ);

        if( !(add_solution(edge_props.dst_vtx) && add_solution(edge_props.src_vtx)))
        {
          break;
        }
      }
    }

    if(!found_next )
    {
      break;
    }

    vertex_counter++;
  }

  CONSOLE_BRIDGE_logDebug("Exiting vertex traversing loop with vertex count at %i", vertex_counter);

  for(std::size_t i = 0; i < solution_points.size(); i++)
  {
    typename PointData<FloatT>::ConstPtr point_data = solution_points[i];
    if(point_data == nullptr)
    {
      CONSOLE_BRIDGE_logError("Invalid solution for point %lu was found",i);
      return false;
    }
  }
  return true;
}

// explicit specializations
template class BDSPGraphPlanner<float>;
template class BDSPGraphPlanner<double>;

} /* namespace descartes_planner */


#ifndef BOOST_GEOMETRY_PROJECTIONS_IGH_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IGH_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

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

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

// 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: 4.9.1

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

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

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/proj/gn_sinu.hpp>
#include <boost/geometry/extensions/gis/projections/proj/moll.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace igh
    {

            template <typename Geographic, typename Cartesian>
            struct par_igh
            {
                boost::shared_ptr<projection<Geographic, Cartesian> > pj[12];
                double dy0;
            };

            static const double d4044118 = (40 + 44/60. + 11.8/3600.) * geometry::math::d2r<double>(); // 40d 44' 11.8" [degrees]

            static const double d10  =  10 * geometry::math::d2r<double>();
            static const double d20  =  20 * geometry::math::d2r<double>();
            static const double d30  =  30 * geometry::math::d2r<double>();
            static const double d40  =  40 * geometry::math::d2r<double>();
            static const double d50  =  50 * geometry::math::d2r<double>();
            static const double d60  =  60 * geometry::math::d2r<double>();
            static const double d80  =  80 * geometry::math::d2r<double>();
            static const double d90  =  90 * geometry::math::d2r<double>();
            static const double d100 = 100 * geometry::math::d2r<double>();
            static const double d140 = 140 * geometry::math::d2r<double>();
            static const double d160 = 160 * geometry::math::d2r<double>();
            static const double d180 = 180 * geometry::math::d2r<double>();

            static const double EPSLN = 1.e-10; // allow a little 'slack' on zone edge positions

            // Converted from #define SETUP(n, proj, x_0, y_0, lon_0)
            template <template <typename, typename, typename> class Entry, typename Parameters, typename Geographic, typename Cartesian>
            inline void do_setup(int n, Parameters const& par, par_igh<Geographic, Cartesian>& proj_parm, double x_0, double y_0, double lon_0)
            {
                Entry<Geographic, Cartesian, Parameters> entry;
                proj_parm.pj[n-1].reset(entry.create_new(par));
                proj_parm.pj[n-1]->mutable_params().x0 = x_0;
                proj_parm.pj[n-1]->mutable_params().y0 = y_0;
                proj_parm.pj[n-1]->mutable_params().lam0 = lon_0;
            }

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_igh_spheroid : public base_t_fi<base_igh_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_igh<Geographic, Cartesian> m_proj_parm;

                inline base_igh_spheroid(const Parameters& par)
                    : base_t_fi<base_igh_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                        int z;
                        if (lp_lat >=  d4044118) {          // 1|2
                          z = (lp_lon <= -d40 ? 1: 2);
                        }
                        else if (lp_lat >=  0) {            // 3|4
                          z = (lp_lon <= -d40 ? 3: 4);
                        }
                        else if (lp_lat >= -d4044118) {     // 5|6|7|8
                               if (lp_lon <= -d100) z =  5; // 5
                          else if (lp_lon <=  -d20) z =  6; // 6
                          else if (lp_lon <=   d80) z =  7; // 7
                          else z = 8;                       // 8
                        }
                        else {                              // 9|10|11|12
                               if (lp_lon <= -d100) z =  9; // 9
                          else if (lp_lon <=  -d20) z = 10; // 10
                          else if (lp_lon <=   d80) z = 11; // 11
                          else z = 12;                      // 12
                        }

                        lp_lon -= this->m_proj_parm.pj[z-1]->params().lam0;
                        this->m_proj_parm.pj[z-1]->fwd(lp_lon, lp_lat, xy_x, xy_y);
                        xy_x += this->m_proj_parm.pj[z-1]->params().x0;
                        xy_y += this->m_proj_parm.pj[z-1]->params().y0;
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                        const double y90 = this->m_proj_parm.dy0 + sqrt(2.0); // lt=90 corresponds to y=y0+sqrt(2.0)

                        int z = 0;
                        if (xy_y > y90+EPSLN || xy_y < -y90+EPSLN) // 0
                          z = 0;
                        else if (xy_y >=  d4044118)       // 1|2
                          z = (xy_x <= -d40? 1: 2);
                        else if (xy_y >=  0)              // 3|4
                          z = (xy_x <= -d40? 3: 4);
                        else if (xy_y >= -d4044118) {     // 5|6|7|8
                               if (xy_x <= -d100) z =  5; // 5
                          else if (xy_x <=  -d20) z =  6; // 6
                          else if (xy_x <=   d80) z =  7; // 7
                          else z = 8;                     // 8
                        }
                        else {                            // 9|10|11|12
                               if (xy_x <= -d100) z =  9; // 9
                          else if (xy_x <=  -d20) z = 10; // 10
                          else if (xy_x <=   d80) z = 11; // 11
                          else z = 12;                    // 12
                        }

                        if (z)
                        {
                          int ok = 0;

                          xy_x -= this->m_proj_parm.pj[z-1]->params().x0;
                          xy_y -= this->m_proj_parm.pj[z-1]->params().y0;
                          this->m_proj_parm.pj[z-1]->inv(xy_x, xy_y, lp_lon, lp_lat);
                          lp_lon += this->m_proj_parm.pj[z-1]->params().lam0;

                          switch (z) {
                            case  1: ok = (lp_lon >= -d180-EPSLN && lp_lon <=  -d40+EPSLN) ||
                                         ((lp_lon >=  -d40-EPSLN && lp_lon <=  -d10+EPSLN) &&
                                          (lp_lat >=   d60-EPSLN && lp_lat <=   d90+EPSLN)); break;
                            case  2: ok = (lp_lon >=  -d40-EPSLN && lp_lon <=  d180+EPSLN) ||
                                         ((lp_lon >= -d180-EPSLN && lp_lon <= -d160+EPSLN) &&
                                          (lp_lat >=   d50-EPSLN && lp_lat <=   d90+EPSLN)) ||
                                         ((lp_lon >=  -d50-EPSLN && lp_lon <=  -d40+EPSLN) &&
                                          (lp_lat >=   d60-EPSLN && lp_lat <=   d90+EPSLN)); break;
                            case  3: ok = (lp_lon >= -d180-EPSLN && lp_lon <=  -d40+EPSLN); break;
                            case  4: ok = (lp_lon >=  -d40-EPSLN && lp_lon <=  d180+EPSLN); break;
                            case  5: ok = (lp_lon >= -d180-EPSLN && lp_lon <= -d100+EPSLN); break;
                            case  6: ok = (lp_lon >= -d100-EPSLN && lp_lon <=  -d20+EPSLN); break;
                            case  7: ok = (lp_lon >=  -d20-EPSLN && lp_lon <=   d80+EPSLN); break;
                            case  8: ok = (lp_lon >=   d80-EPSLN && lp_lon <=  d180+EPSLN); break;
                            case  9: ok = (lp_lon >= -d180-EPSLN && lp_lon <= -d100+EPSLN); break;
                            case 10: ok = (lp_lon >= -d100-EPSLN && lp_lon <=  -d20+EPSLN); break;
                            case 11: ok = (lp_lon >=  -d20-EPSLN && lp_lon <=   d80+EPSLN); break;
                            case 12: ok = (lp_lon >=   d80-EPSLN && lp_lon <=  d180+EPSLN); break;
                          }

                          z = (!ok? 0: z); // projectable?
                        }
                     // if (!z) pj_errno = -15; // invalid x or y
                        if (!z) lp_lon = HUGE_VAL;
                        if (!z) lp_lat = HUGE_VAL;
                }

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

            };

            // Interrupted Goode Homolosine
            template <typename Geographic, typename Cartesian, typename Parameters>
            void setup_igh(Parameters& par, par_igh<Geographic, Cartesian>& proj_parm)
            {
            /*
              Zones:

                -180            -40                       180
                  +--------------+-------------------------+    Zones 1,2,9,10,11 & 12:
                  |1             |2                        |      Mollweide projection
                  |              |                         |
                  +--------------+-------------------------+    Zones 3,4,5,6,7 & 8:
                  |3             |4                        |      Sinusoidal projection
                  |              |                         |
                0 +-------+------+-+-----------+-----------+
                  |5      |6       |7          |8          |
                  |       |        |           |           |
                  +-------+--------+-----------+-----------+
                  |9      |10      |11         |12         |
                  |       |        |           |           |
                  +-------+--------+-----------+-----------+
                -180    -100      -20         80          180
            */

                    double lp_lam = 0, lp_phi = d4044118;
                    double xy1_x, xy1_y;
                    double xy3_x, xy3_y;

                    // sinusoidal zones
                    do_setup<sinu_entry>(3, par, proj_parm, -d100, 0, -d100);
                    do_setup<sinu_entry>(4, par, proj_parm,   d30, 0,   d30);
                    do_setup<sinu_entry>(5, par, proj_parm, -d160, 0, -d160);
                    do_setup<sinu_entry>(6, par, proj_parm,  -d60, 0,  -d60);
                    do_setup<sinu_entry>(7, par, proj_parm,   d20, 0,   d20);
                    do_setup<sinu_entry>(8, par, proj_parm,  d140, 0,  d140);

                    // mollweide zones
                    do_setup<moll_entry>(1, par, proj_parm, -d100, 0, -d100);

                    // y0 ?
                     proj_parm.pj[0]->fwd(lp_lam, lp_phi, xy1_x, xy1_y); // zone 1
                     proj_parm.pj[2]->fwd(lp_lam, lp_phi, xy3_x, xy3_y); // zone 3
                    // y0 + xy1_y = xy3_y for lt = 40d44'11.8"
                    proj_parm.dy0 = xy3_y - xy1_y;

                    proj_parm.pj[0]->mutable_params().y0 = proj_parm.dy0;

                    // mollweide zones (cont'd)
                    do_setup<moll_entry>( 2, par, proj_parm,   d30,  proj_parm.dy0,   d30);
                    do_setup<moll_entry>( 9, par, proj_parm, -d160, -proj_parm.dy0, -d160);
                    do_setup<moll_entry>(10, par, proj_parm,  -d60, -proj_parm.dy0,  -d60);
                    do_setup<moll_entry>(11, par, proj_parm,   d20, -proj_parm.dy0,   d20);
                    do_setup<moll_entry>(12, par, proj_parm,  d140, -proj_parm.dy0,  d140);

                    par.es = 0.;
            }

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

    /*!
        \brief Interrupted Goode Homolosine projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Example
        \image html ex_igh.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct igh_spheroid : public detail::igh::base_igh_spheroid<Geographic, Cartesian, Parameters>
    {
        inline igh_spheroid(const Parameters& par) : detail::igh::base_igh_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::igh::setup_igh(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class igh_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<igh_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void igh_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("igh", new igh_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_IGH_HPP


//| This file is a part of the sferes2 framework.
//| Copyright 2009, ISIR / Universite Pierre et Marie Curie (UPMC)
//| Main contributor(s): Jean-Baptiste Mouret, mouret@isir.fr
//|
//| This software is a computer program whose purpose is to facilitate
//| experiments in evolutionary computation and evolutionary robotics.
//|
//| This software is governed by the CeCILL license under French law
//| and abiding by the rules of distribution of free software.  You
//| can use, modify and/ or redistribute the software under the terms
//| of the CeCILL license as circulated by CEA, CNRS and INRIA at the
//| following URL "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and rights to
//| copy, modify and redistribute granted by the license, users are
//| provided only with a limited warranty and the software's author,
//| the holder of the economic rights, and the successive licensors
//| have only limited liability.
//|
//| In this respect, the user's attention is drawn to the risks
//| associated with loading, using, modifying and/or developing or
//| reproducing the software by the user in light of its specific
//| status of free software, that may mean that it is complicated to
//| manipulate, and that also therefore means that it is reserved for
//| developers and experienced professionals having in-depth computer
//| knowledge. Users are therefore encouraged to load and test the
//| software's suitability as regards their requirements in conditions
//| enabling the security of their systems and/or data to be ensured
//| and, more generally, to use and operate it in the same conditions
//| as regards security.
//|
//| The fact that you are presently reading this means that you have
//| had knowledge of the CeCILL license and that you accept its terms.

#ifndef EVO_FLOAT_IMAGE_HPP_
#define EVO_FLOAT_IMAGE_HPP_

#include <vector>
#include <limits>
#include <boost/foreach.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/nvp.hpp>
#include <sferes/stc.hpp>
#include <sferes/misc.hpp>
#include <sferes/dbg/dbg.hpp>
#include <sferes/gen/float.hpp>
#include <iostream>
#include <cmath>
namespace sferes {
  namespace gen {
    namespace evo_float_image {
      enum mutation_t { polynomial = 0, gaussian, uniform };
      enum cross_over_t { recombination = 0, sbx, no_cross_over };

      template<typename Ev, unsigned long int T>
      struct Mutation_f {
        void operator()(Ev& ev, size_t i) {
          assert(0);
        }
      };
      template<typename Ev, unsigned long int T>
      struct CrossOver_f {
        void operator()(const Ev& f1, const Ev& f2, Ev &c1, Ev &c2) {
          assert(0);
        }
      };
    }

    /// in range [0;1]
    template<unsigned long int Size, typename Params, typename Exact = stc::Itself>
    class EvoFloatImage :
      public Float<Size, Params,
      typename stc::FindExact<Float<Size, Params, Exact>, Exact>::ret> {
     public:
      typedef Params params_t;
      typedef EvoFloatImage<Size, Params, Exact> this_t;

      EvoFloatImage() {}

      //@{
      void mutate() {
        for (size_t i = 0; i < Size; i++)
          if (misc::rand<float>() < Params::evo_float_image::mutation_rate)
            _mutation_op(*this, i);
        _check_invariant();
      }
      void cross(const EvoFloatImage& o, EvoFloatImage& c1, EvoFloatImage& c2) {
        if (Params::evo_float_image::cross_over_type != evo_float_image::no_cross_over &&
            misc::rand<float>() < Params::evo_float_image::cross_rate)
          _cross_over_op(*this, o, c1, c2);
        else if (misc::flip_coin()) {
          c1 = *this;
          c2 = o;
        } else {
          c1 = o;
          c2 = *this;
        }
        _check_invariant();
      }
      void random() {
        BOOST_FOREACH(float &v, this->_data) v = misc::rand<float>();
        _check_invariant();
      }
      //@}

     protected:
      evo_float_image::Mutation_f<this_t, Params::evo_float_image::mutation_type> _mutation_op;
      evo_float_image::CrossOver_f<this_t, Params::evo_float_image::cross_over_type> _cross_over_op;
      void _check_invariant() const {
#ifdef DBG_ENABLED
        BOOST_FOREACH(float p, this->_data) {
          assert(!std::isnan(p));
          assert(!std::isinf(p));
          assert(p >= 0 && p <= 1);
        }
#endif
      }
    };

    // partial specialization for operators
    namespace evo_float_image {
      // polynomial mutation. Cf Deb 2001, p 124 ; param: eta_m
      // perturbation of the order O(1/eta_m)
      template<typename Ev>
      struct Mutation_f<Ev, polynomial> {
        void operator()(Ev& ev, size_t i) {
          SFERES_CONST float eta_m = Ev::params_t::evo_float_image::eta_m;
          assert(eta_m != -1.0f);
          float ri = misc::rand<float>();
          float delta_i = ri < 0.5 ?
                          pow(2.0 * ri, 1.0 / (eta_m + 1.0)) - 1.0 :
                          1 - pow(2.0 * (1.0 - ri), 1.0 / (eta_m + 1.0));
          assert(!std::isnan(delta_i));
          assert(!std::isinf(delta_i));
          float f = ev.data(i) + delta_i;
          ev.data(i, misc::put_in_range(f, 0.0f, 1.0f));
        }
      };

      // gaussian mutation
      template<typename Ev>
      struct Mutation_f<Ev, gaussian> {
        void operator()(Ev& ev, size_t i) {
          SFERES_CONST float sigma = Ev::params_t::evo_float_image::sigma;
          float f = ev.data(i)
                    + misc::gaussian_rand<float>(0, sigma * sigma);
          ev.data(i, misc::put_in_range(f, 0.0f, 1.0f));
        }
      };
      // uniform mutation
      template<typename Ev>
      struct Mutation_f<Ev, uniform> {
        void operator()(Ev& ev, size_t i) {
          SFERES_CONST float max = Ev::params_t::evo_float_image::max;
          float f = ev.data(i)
                    + misc::rand<float>(max) - max / 2.0f;
          ev.data(i, misc::put_in_range(f, 0.0f, 1.0f));
        }
      };

      // recombination
      template<typename Ev>
      struct CrossOver_f<Ev, recombination> {
        void operator()(const Ev& f1, const Ev& f2, Ev &c1, Ev &c2) {
          size_t k = misc::rand<unsigned long int>(f1.size());
          for (size_t i = 0; i < k; ++i) {
            c1.data(i, f1.data(i));
            c2.data(i, f2.data(i));
          }
          for (size_t i = k; i < f1.size(); ++i) {
            c1.data(i, f2.data(i));
            c2.data(i, f1.data(i));
          }
        }
      };

      // no cross-over
      template<typename Ev>
      struct CrossOver_f<Ev, no_cross_over> {
        void operator()(const Ev& f1, const Ev& f2, Ev &c1, Ev &c2) {
        }
      };

      // SBX (cf Deb 2001, p 113) Simulated Binary Crossover
      // suggested eta : 15
      /// WARNING : this code is from deb's code (different from the
      // article ...)
      // A large value ef eta gives a higher probablitity for
      // creating a `near-parent' solutions and a small value allows
      // distant solutions to be selected as offspring.
      template<typename Ev>
      struct CrossOver_f<Ev, sbx> {
        void operator()(const Ev& f1, const Ev& f2, Ev &child1, Ev &child2) {
          SFERES_CONST float eta_c = Ev::params_t::evo_float_image::eta_c;
          assert(eta_c != -1);
          for (unsigned long int i = 0; i < f1.size(); i++) {
            float y1 = std::min(f1.data(i), f2.data(i));
            float y2 = std::max(f1.data(i), f2.data(i));
            SFERES_CONST float yl = 0.0;
            SFERES_CONST float yu = 1.0;
            if (fabs(y1 - y2) > std::numeric_limits<float>::epsilon()) {
              float rand = misc::rand<float>();
              float beta = 1.0 + (2.0 * (y1 - yl) / (y2 - y1));
              float alpha = 2.0 - pow(beta, -(eta_c + 1.0));
              float betaq = 0;
              if (rand <= (1.0 / alpha))
                betaq = pow((rand * alpha), (1.0 / (eta_c + 1.0)));
              else
                betaq = pow ((1.0 / (2.0 - rand * alpha)) , (1.0 / (eta_c + 1.0)));
              float c1 = 0.5 * ((y1 + y2) - betaq * (y2 - y1));
              beta = 1.0 + (2.0 * (yu - y2) / (y2 - y1));
              alpha = 2.0 - pow(beta, -(eta_c + 1.0));
              if (rand <= (1.0 / alpha))
                betaq = pow ((rand * alpha), (1.0 / (eta_c + 1.0)));
              else
                betaq = pow ((1.0/(2.0 - rand * alpha)), (1.0 / (eta_c + 1.0)));
              float c2 = 0.5 * ((y1 + y2) + betaq * (y2 - y1));

              c1 = misc::put_in_range(c1, yl, yu);
              c2 = misc::put_in_range(c2, yl, yu);

              assert(!std::isnan(c1));
              assert(!std::isnan(c2));

              if (misc::flip_coin()) {
                child1.data(i, c1);
                child2.data(i, c2);
              } else {
                child1.data(i, c2);
                child2.data(i, c1);
              }
            }
          }
        }
      };

    } //evo_float_image
  } // gen
} // sferes

#endif