#include <boost/test/unit_test.hpp>

#include <fstream>
#include <iostream>
#include <fc/crypto/aes.hpp>
#include <fc/crypto/city.hpp>
#include <fc/exception/exception.hpp>

#include <fc/variant.hpp>

BOOST_AUTO_TEST_SUITE(fc_crypto)

BOOST_AUTO_TEST_CASE(aes_test)
{
    std::ifstream testfile;
    testfile.open("README.md");

    auto key = fc::sha512::hash( "hello", 5 );
    std::stringstream buffer;
    std::string line;
    std::getline( testfile, line );
    while( testfile.good() )
    {
//       std::cout << line << "\n";
        buffer << line << "\n";
        try {
            std::vector<char> data( line.c_str(),line.c_str()+line.size()+1 );
            std::vector<char> crypt = fc::aes_encrypt( key, data );
            std::vector<char> dcrypt = fc::aes_decrypt( key, crypt );
            BOOST_CHECK( data == dcrypt );

//           memset( crypt.data(), 0, crypt.size() );
//           fc::aes_encoder enc;
//           enc.init( fc::sha256::hash((char*)&key,sizeof(key) ), fc::city_hash_crc_128( (char*)&key, sizeof(key) ) );
//           auto len = enc.encode( dcrypt.data(), dcrypt.size(), crypt.data() );
//           BOOST_CHECK_EQUAL( dcrypt.size(), len );
//
//           fc::aes_decoder dec;
//           dec.init( fc::sha256::hash((char*)&key,sizeof(key) ), fc::city_hash_crc_128( (char*)&key, sizeof(key) ) );
//           len = dec.decode( crypt.data(), len, dcrypt.data() );
//           BOOST_CHECK_EQUAL( dcrypt.size(), len );
//           BOOST_CHECK( !memcmp( dcrypt.data(), data.data(), len) );
        }
        catch ( fc::exception& e )
        {
           std::cout<<e.to_detail_string()<<"\n";
        }
        std::getline( testfile, line );
    }

    line = buffer.str();
    std::vector<char> data( line.c_str(),line.c_str()+line.size()+1 );
    std::vector<char> crypt = fc::aes_encrypt( key, data );
    std::vector<char> dcrypt = fc::aes_decrypt( key, crypt );
    BOOST_CHECK( data == dcrypt );

//   memset( crypt.data(), 0, crypt.size() );
//   fc::aes_encoder enc;
//   enc.init( fc::sha256::hash((char*)&key,sizeof(key) ), fc::city_hash_crc_128( (char*)&key, sizeof(key) ) );
//   auto len = enc.encode( dcrypt.data(), dcrypt.size(), crypt.data() );
//   BOOST_CHECK_EQUAL( dcrypt.size(), len );
//
//   fc::aes_decoder dec;
//   dec.init( fc::sha256::hash((char*)&key,sizeof(key) ), fc::city_hash_crc_128( (char*)&key, sizeof(key) ) );
//   len = dec.decode( crypt.data(), len, dcrypt.data() );
//   BOOST_CHECK_EQUAL( dcrypt.size(), len );
//   BOOST_CHECK( !memcmp( dcrypt.data(), data.data(), len) );
}

BOOST_AUTO_TEST_SUITE_END()


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

#define BOOST_TEST_MAIN

#include <limits>
#include <string>
#include "Tudat/Basics/testMacros.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"

#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>
#include <boost/lambda/lambda.hpp>

#include "Tudat/Astrodynamics/Aerodynamics/exponentialAtmosphere.h"
#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h"
#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/gravitationalParameter.h"
#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationRate.h"
#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationalOrientation.h"
#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicCosineCoefficients.h"
#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/sphericalHarmonicSineCoefficients.h"
#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/mutualSphericalHarmonicGravityPartial.h"
#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.h"

#include "Tudat/Astrodynamics/Gravitation/mutualSphericalHarmonicGravityModel.h"
#include "Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h"
#include "Tudat/SimulationSetup/tudatSimulationHeader.h"

using namespace tudat::ephemerides;
using namespace tudat::simulation_setup;
using namespace tudat::orbit_determination;
using namespace tudat::spice_interface;
using namespace tudat::orbit_determination;
using namespace tudat::estimatable_parameters;
using namespace tudat::gravitation;
using namespace tudat::acceleration_partials;

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_mutual_sh_acceleration_partials )

//! Retrieve Phobos gravity field
std::shared_ptr< GravityFieldSettings > getPhobosGravityFieldSettings( )
{
    Eigen::MatrixXd phobosCosineCoefficients = Eigen::MatrixXd::Zero( 20, 20 );

    phobosCosineCoefficients( 0, 0 ) = 1.0;
    phobosCosineCoefficients( 2, 0 ) = -0.072 / basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 2, 0 );
    phobosCosineCoefficients( 2, 2 ) = -0.048 /basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 2, 2 );

    return std::make_shared< SphericalHarmonicsGravityFieldSettings >(
                7.087546066894452E05, 12.0E3, phobosCosineCoefficients,  Eigen::MatrixXd::Zero( 20, 20 ), "IAU_Phobos" );
}

//! Retrieve Moon gravity field and use as Mars gravity field (makes no difference for test purposes)
std::shared_ptr< GravityFieldSettings > getMarsGravityFieldSettings( )
{
    std::pair< Eigen::MatrixXd, Eigen::MatrixXd > coefficients;
    std::pair< double, double > moonGravityFieldSettings = readGravityFieldFile(
                input_output::getGravityModelsPath( ) + "Moon/lpe200.txt", 50, 50, coefficients, 0, 1 );

    return std::make_shared< SphericalHarmonicsGravityFieldSettings >
            ( moonGravityFieldSettings.first, moonGravityFieldSettings.second,
              coefficients.first, coefficients.second, "IAU_Moon" );
}

//! Test whether partials of mutual spherical harmonic acceleration are computed correctly
BOOST_AUTO_TEST_CASE( testMutualSphericalHarmonicGravityPartials )
{
    // Load spice kernel.
    spice_interface::loadStandardSpiceKernels( { input_output::getSpiceKernelPath( ) + "de430_mar097_small.bsp" } );

    std::vector< std::string > bodyList;
    bodyList.push_back( "Sun" );
    bodyList.push_back( "Mars" );

    // Run test for inertial and Mars-centered acceleration.
    for( int testCase = 0; testCase < 2; testCase++ )
    {

        double initialTime = 1.0E6 - 1.0E5;

        // Retrieve body settings
        std::map< std::string, std::shared_ptr< BodySettings > > bodySettings = getDefaultBodySettings(
                    bodyList, 1.0E6 - 1.0E5, 1.0E6 + 1.0E5 );
        bodySettings[ "Phobos" ] = std::make_shared< BodySettings >( );
        bodySettings[ "Phobos" ]->ephemerisSettings = getDefaultEphemerisSettings(
                    "Phobos" );

        // Update rotation models
        bodySettings[ "Mars" ]->rotationModelSettings = std::make_shared< SimpleRotationModelSettings >(
                    "ECLIPJ2000", "IAU_Mars",
                    spice_interface::computeRotationQuaternionBetweenFrames(
                        "ECLIPJ2000", "IAU_Mars", initialTime ),
                    initialTime, 2.0 * mathematical_constants::PI /
                    ( physical_constants::JULIAN_DAY + 40.0 * 60.0 ) );
        bodySettings[ "Phobos" ]->rotationModelSettings = std::make_shared< SimpleRotationModelSettings >(
                    "ECLIPJ2000", "IAU_Phobos",
                    spice_interface::computeRotationQuaternionBetweenFrames(
                        "ECLIPJ2000", "IAU_Phobos", initialTime ),
                    initialTime, 2.0 * mathematical_constants::PI /
                    ( physical_constants::JULIAN_DAY / 4.0 ) );

        // Update gravity field settings.
        bodySettings[ "Mars" ]->gravityFieldSettings = getMarsGravityFieldSettings( );
        bodySettings[ "Phobos" ]->gravityFieldSettings = getPhobosGravityFieldSettings( );

        // Create body objects
        NamedBodyMap bodyMap = createBodies( bodySettings );
        setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

        // Create links to set and get state functions of bodies.
        std::shared_ptr< Body > mars = bodyMap.at( "Mars" );
        std::function< void( Eigen::Vector6d ) > marsStateSetFunction = std::bind( &Body::setState, mars, std::placeholders::_1  );
        std::function< Eigen::Vector6d( ) > marsStateGetFunction = std::bind( &Body::getState, mars );
        mars->setStateFromEphemeris( 1.0E6 );

        std::shared_ptr< Body > phobos = std::dynamic_pointer_cast< Body >( bodyMap.at( "Phobos" ) );
        std::function< void( Eigen::Vector6d ) > phobosStateSetFunction = std::bind( &Body::setState, phobos, std::placeholders::_1  );
        std::function< Eigen::Vector6d( ) > phobosStateGetFunction = std::bind( &Body::getState, phobos );
        phobos->setStateFromEphemeris( 1.0E6 );

        // Calculate and set mars orientation at epoch.
        mars->setCurrentRotationToLocalFrameFromEphemeris( 1.0E6 );
        mars->setStateFromEphemeris( 1.0E6 );
        phobos->setCurrentRotationToLocalFrameFromEphemeris( 1.0E6 );
        phobos->setStateFromEphemeris( 1.0E6 );

        // Define central body
        std::string centralBody = "";
        std::shared_ptr< Body > centralBodyObject;
        if( testCase == 1 )
        {
            centralBody = "Mars";
            centralBodyObject = mars;
        }

        // Create acceleration model.
        std::shared_ptr< AccelerationSettings > accelerationSettings =
                std::make_shared< MutualSphericalHarmonicAccelerationSettings >( 10, 10, 6, 6, 0, 0 );
        std::shared_ptr< MutualSphericalHarmonicsGravitationalAccelerationModel > accelerationModel =
                std::dynamic_pointer_cast< MutualSphericalHarmonicsGravitationalAccelerationModel >(
                    createAccelerationModel( phobos, mars, accelerationSettings, "Phobos", "Mars",
                                             centralBodyObject, centralBody ) );

        // Retrieve gravity fields
        std::shared_ptr< SphericalHarmonicsGravityField > marsGravityField =
                std::dynamic_pointer_cast< SphericalHarmonicsGravityField  >(
                    bodyMap.at( "Mars" )->getGravityFieldModel( ) );

        std::shared_ptr< SphericalHarmonicsGravityField > phobosGravityField =
                std::dynamic_pointer_cast< SphericalHarmonicsGravityField  >(
                    bodyMap.at( "Phobos" )->getGravityFieldModel( ) );

        // Create gravitational parameter estimation settings.
        std::shared_ptr< EstimatableParameter< double > > marsGravitationalParameterObject = std::make_shared<
                GravitationalParameter >( marsGravityField, "Mars" );
        std::shared_ptr< EstimatableParameter< double > > phobosGravitationalParameterObject = std::make_shared<
                GravitationalParameter >( phobosGravityField, "Phobos" );

        // Create rotation parameter estimation settings.
        std::shared_ptr< EstimatableParameter< double > > marsRotationRate = std::make_shared<
                RotationRate >( std::dynamic_pointer_cast< SimpleRotationalEphemeris >(
                                    mars->getRotationalEphemeris( ) ), "Mars" );
        std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > marsPolePosition = std::make_shared<
                ConstantRotationalOrientation >( std::dynamic_pointer_cast< SimpleRotationalEphemeris >(
                                                     mars->getRotationalEphemeris( ) ), "Mars" );
        std::shared_ptr< EstimatableParameter< double > > phobosRotationRate = std::make_shared<
                RotationRate >( std::dynamic_pointer_cast< SimpleRotationalEphemeris >(
                                    phobos->getRotationalEphemeris( ) ), "Phobos" );
        std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > phobosPolePosition = std::make_shared<
                ConstantRotationalOrientation >( std::dynamic_pointer_cast< SimpleRotationalEphemeris >(
                                                     phobos->getRotationalEphemeris( ) ), "Phobos" );

        // Create Mars gravity field coefficient estimation settings.
        std::function< Eigen::MatrixXd( ) > getSineCoefficientsFunction =
                std::bind( &SphericalHarmonicsGravityField::getSineCoefficients, marsGravityField );
        std::function< void( Eigen::MatrixXd ) > setSineCoefficientsFunction =
                std::bind( &SphericalHarmonicsGravityField::setSineCoefficients, marsGravityField, std::placeholders::_1 );
        std::function< Eigen::MatrixXd( ) > getCosineCoefficientsFunction =
                std::bind( &SphericalHarmonicsGravityField::getCosineCoefficients, marsGravityField );
        std::function< void( Eigen::MatrixXd ) > setCosineCoefficientsFunction =
                std::bind( &SphericalHarmonicsGravityField::setCosineCoefficients, marsGravityField, std::placeholders::_1 );

        std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > marsSineCoefficients =
                std::make_shared< SphericalHarmonicsSineCoefficients >(
                    getSineCoefficientsFunction, setSineCoefficientsFunction,
                    getSphericalHarmonicBlockIndices( 3, 1, 5, 5 ), "Mars" );
        std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > marsCosineCoefficients =
                std::make_shared< SphericalHarmonicsCosineCoefficients >(
                    getCosineCoefficientsFunction, setCosineCoefficientsFunction,
                    getSphericalHarmonicBlockIndices( 3, 0, 5, 5 ), "Mars" );

        // Create Phobos gravity field coefficient estimation settings.
        getSineCoefficientsFunction =
                std::bind( &SphericalHarmonicsGravityField::getSineCoefficients, phobosGravityField );
        setSineCoefficientsFunction =
                std::bind( &SphericalHarmonicsGravityField::setSineCoefficients, phobosGravityField, std::placeholders::_1 );
        getCosineCoefficientsFunction =
                std::bind( &SphericalHarmonicsGravityField::getCosineCoefficients, phobosGravityField );
        setCosineCoefficientsFunction =
                std::bind( &SphericalHarmonicsGravityField::setCosineCoefficients, phobosGravityField, std::placeholders::_1 );

        std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > phobosSineCoefficients =
                std::make_shared< SphericalHarmonicsSineCoefficients >(
                    getSineCoefficientsFunction, setSineCoefficientsFunction,
                    getSphericalHarmonicBlockIndices( 2, 1, 2, 2 ), "Phobos" );
        std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > phobosCosineCoefficients =
                std::make_shared< SphericalHarmonicsCosineCoefficients >(
                    getCosineCoefficientsFunction, setCosineCoefficientsFunction,
                    getSphericalHarmonicBlockIndices( 2, 0, 2, 2 ), "Phobos" );

        // Create parameter objects
        std::vector< std::shared_ptr< EstimatableParameter< double > > > doubleParameters;
        doubleParameters.push_back( marsGravitationalParameterObject );
        doubleParameters.push_back( phobosGravitationalParameterObject );
        doubleParameters.push_back( marsRotationRate );
        doubleParameters.push_back( phobosRotationRate );

        std::vector< std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > > vectorParameters;
        vectorParameters.push_back( marsSineCoefficients );
        vectorParameters.push_back( marsCosineCoefficients );
        vectorParameters.push_back( phobosSineCoefficients );
        vectorParameters.push_back( phobosCosineCoefficients );
        vectorParameters.push_back( marsPolePosition );
        vectorParameters.push_back( phobosPolePosition );

        std::shared_ptr< EstimatableParameterSet< double > > parameterSet =
                std::make_shared< EstimatableParameterSet< double > >( doubleParameters, vectorParameters );

        // Create acceleration partial object.
        std::shared_ptr< MutualSphericalHarmonicsGravityPartial > accelerationPartial =
                std::dynamic_pointer_cast< MutualSphericalHarmonicsGravityPartial > (
                    createAnalyticalAccelerationPartial(
                        accelerationModel, std::make_pair( "Phobos", phobos ),
                        std::make_pair( "Mars", mars ), bodyMap, parameterSet ) );

        // Calculate analytical partials.
        accelerationModel->updateMembers( 1.0E6 );
        accelerationPartial->update( 1.0E6 );

        // Get analytical partials w.r.t. positions and velocities
        Eigen::MatrixXd partialWrtMarsPosition = Eigen::Matrix3d::Zero( );
        accelerationPartial->wrtPositionOfAcceleratingBody( partialWrtMarsPosition.block( 0, 0, 3, 3 ) );
        Eigen::MatrixXd partialWrtMarsVelocity = Eigen::Matrix3d::Zero( );
        accelerationPartial->wrtVelocityOfAcceleratingBody( partialWrtMarsVelocity.block( 0, 0, 3, 3 ) );
        Eigen::MatrixXd partialWrtPhobosPosition = Eigen::Matrix3d::Zero( );
        accelerationPartial->wrtPositionOfAcceleratedBody( partialWrtPhobosPosition.block( 0, 0, 3, 3 ) );
        Eigen::MatrixXd partialWrtPhobosVelocity = Eigen::Matrix3d::Zero( );
        accelerationPartial->wrtVelocityOfAcceleratedBody( partialWrtPhobosVelocity.block( 0, 0, 3, 3 ) );

        // Get analytical partials w.r.t. parameters
        Eigen::Vector3d partialWrtMarsGravitationalParameter = accelerationPartial->wrtParameter(
                    marsGravitationalParameterObject );
        Eigen::Vector3d partialWrtPhobosGravitationalParameter = accelerationPartial->wrtParameter(
                    phobosGravitationalParameterObject );
        Eigen::Vector3d partialWrtMarsRotationRate = accelerationPartial->wrtParameter(
                    marsRotationRate );
        Eigen::MatrixXd partialWrtMarsPolePosition = accelerationPartial->wrtParameter(
                    marsPolePosition );
        Eigen::Vector3d partialWrtPhobosRotationRate = accelerationPartial->wrtParameter(
                    phobosRotationRate );
        Eigen::MatrixXd partialWrtPhobosPolePosition = accelerationPartial->wrtParameter(
                    phobosPolePosition );
        Eigen::MatrixXd partialWrtMarsCosineCoefficients = accelerationPartial->wrtParameter(
                    marsCosineCoefficients );
        Eigen::MatrixXd partialWrtMarsSineCoefficients = accelerationPartial->wrtParameter(
                    marsSineCoefficients );
        Eigen::MatrixXd partialWrtPhobosCosineCoefficients = accelerationPartial->wrtParameter(
                    phobosCosineCoefficients );
        Eigen::MatrixXd partialWrtPhobosSineCoefficients = accelerationPartial->wrtParameter(
                    phobosSineCoefficients );

        // Set perturbations in position and velocity for numerical partial
        Eigen::Vector3d positionPerturbation;
        positionPerturbation << 100.0, 100.0, 100.0;
        Eigen::Vector3d velocityPerturbation;
        velocityPerturbation << 1.0, 1.0, 1.0;

        // Calculate numerical partials.
        Eigen::Matrix3d testPartialWrtPhobosPosition = calculateAccelerationWrtStatePartials(
                    phobosStateSetFunction, accelerationModel, phobos->getState( ), positionPerturbation, 0 );
        Eigen::Matrix3d testPartialWrtPhobosVelocity = calculateAccelerationWrtStatePartials(
                    phobosStateSetFunction, accelerationModel, phobos->getState( ), velocityPerturbation, 3 );
        Eigen::Matrix3d testPartialWrtMarsPosition = calculateAccelerationWrtStatePartials(
                    marsStateSetFunction, accelerationModel, mars->getState( ), positionPerturbation, 0 );
        Eigen::Matrix3d testPartialWrtMarsVelocity = calculateAccelerationWrtStatePartials(
                    marsStateSetFunction, accelerationModel, mars->getState( ), velocityPerturbation, 3 );

        Eigen::Vector3d testPartialWrtMarsGravitationalParameter = calculateAccelerationWrtParameterPartials(
                    marsGravitationalParameterObject, accelerationModel, 1.0E14 );
        Eigen::Vector3d testPartialWrtPhobosGravitationalParameter = calculateAccelerationWrtParameterPartials(
                    phobosGravitationalParameterObject, accelerationModel, 1.0E14 );

        Eigen::Vector3d testPartialWrtMarsRotationRate = calculateAccelerationWrtParameterPartials(
                    marsRotationRate, accelerationModel, 1.0E-9, &emptyFunction, 1.0E6, std::bind(
                        &Body::setCurrentRotationToLocalFrameFromEphemeris, mars, std::placeholders::_1 ) );
        Eigen::MatrixXd testPartialWrtPolePosition = calculateAccelerationWrtParameterPartials(
                    marsPolePosition, accelerationModel, ( Eigen::VectorXd( 2 ) << 1.0E-4, 1.0E-4 ).finished( ),
                    &emptyFunction, 1.0E6, std::bind(
                        &Body::setCurrentRotationToLocalFrameFromEphemeris, mars, std::placeholders::_1 ) );

        Eigen::Vector3d testPartialWrtPhobosRotationRate = calculateAccelerationWrtParameterPartials(
                    phobosRotationRate, accelerationModel, 1.0E-8, &emptyFunction, 1.0E6, std::bind(
                        &Body::setCurrentRotationToLocalFrameFromEphemeris, phobos, std::placeholders::_1 ) );
        Eigen::MatrixXd testPartialWrtPhobosPolePosition = calculateAccelerationWrtParameterPartials(
                    phobosPolePosition, accelerationModel, ( Eigen::VectorXd( 2 ) << 1.0E-3, 1.0E-3 ).finished( ),
                    &emptyFunction, 1.0E6, std::bind(
                        &Body::setCurrentRotationToLocalFrameFromEphemeris, phobos, std::placeholders::_1 ) );

        Eigen::MatrixXd testPartialWrtMarsCosineCoefficients = calculateAccelerationWrtParameterPartials(
                    marsCosineCoefficients, accelerationModel, Eigen::VectorXd::Constant(
                        marsCosineCoefficients->getParameterValue( ).size( ), 1, 1.0 ) );
        Eigen::MatrixXd testPartialWrtMarsSineCoefficients = calculateAccelerationWrtParameterPartials(
                    marsSineCoefficients, accelerationModel, Eigen::VectorXd::Constant(
                        marsSineCoefficients->getParameterValue( ).size( ), 1, 1.0 ) );
        Eigen::MatrixXd testPartialWrtPhobosCosineCoefficients = calculateAccelerationWrtParameterPartials(
                    phobosCosineCoefficients, accelerationModel, Eigen::VectorXd::Constant(
                        phobosCosineCoefficients->getParameterValue( ).size( ), 1, 1.0 ) );
        Eigen::MatrixXd testPartialWrtPhobosSineCoefficients = calculateAccelerationWrtParameterPartials(
                    phobosSineCoefficients, accelerationModel, Eigen::VectorXd::Constant(
                        phobosSineCoefficients->getParameterValue( ).size( ), 1, 1.0 ) );

        // Compare numerical and analytical partials of position and velocity partials.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsPosition, partialWrtMarsPosition, 1.0e-9 );
        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( std::fabs( testPartialWrtMarsVelocity( i, j ) - partialWrtMarsVelocity( i, j ) ),
                                   std::numeric_limits< double >::epsilon( ) );
                BOOST_CHECK_SMALL( std::fabs( partialWrtMarsVelocity( i, j ) ),
                                   std::numeric_limits< double >::epsilon( ) );
            }

        }

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosPosition, partialWrtPhobosPosition, 1.0e-9 );
        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( std::fabs( testPartialWrtPhobosVelocity( i, j ) - partialWrtPhobosVelocity( i, j ) ),
                                   std::numeric_limits< double >::epsilon( ) );
                BOOST_CHECK_SMALL( std::fabs( partialWrtPhobosVelocity( i, j ) ),
                                   std::numeric_limits< double >::epsilon( ) );
            }
        }

        // Compare numerical and analytical partials of gravitational parameters
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsGravitationalParameter,
                                           partialWrtMarsGravitationalParameter, 1.0e-14 );

        if( testCase == 0 )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( std::fabs( testPartialWrtPhobosGravitationalParameter( j ) -
                                              partialWrtPhobosGravitationalParameter( j ) ),
                                   std::numeric_limits< double >::epsilon( ) );
                BOOST_CHECK_SMALL( std::fabs( partialWrtPhobosGravitationalParameter( j ) ),
                                   std::numeric_limits< double >::epsilon( ) );
            }
        }
        else
        {
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosGravitationalParameter,
                                               partialWrtPhobosGravitationalParameter, 1.0e-14 );

        }

        // Compare numerical and analytical partials of rotation parameters
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsRotationRate, partialWrtMarsRotationRate, 1.0e-5 );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPolePosition, partialWrtMarsPolePosition, 1.0e-5 );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosRotationRate, partialWrtPhobosRotationRate, 1.0e-5 );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosPolePosition, partialWrtPhobosPolePosition, 1.0e-5 );

        // Compare numerical and analytical partials of gravity field coefficients
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsSineCoefficients, partialWrtMarsSineCoefficients, 1.0E-10 );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsCosineCoefficients, partialWrtMarsCosineCoefficients, 1.0E-10 );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosSineCoefficients, partialWrtPhobosSineCoefficients, 1.0E-8 );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosCosineCoefficients, partialWrtPhobosCosineCoefficients, 1.0E-10 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


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

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2013-2017 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2014-2020.
// Modifications copyright (c) 2014-2020, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP

#include <algorithm>

#include <boost/geometry/core/exception.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/geometries/concepts/segment_concept.hpp>
#include <boost/geometry/geometries/segment.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/algorithms/detail/assign_values.hpp>
#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/algorithms/detail/equals/point_point.hpp>
#include <boost/geometry/algorithms/detail/recalculate.hpp>

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

#include <boost/geometry/strategy/cartesian/area.hpp>
#include <boost/geometry/strategy/cartesian/envelope.hpp>
#include <boost/geometry/strategy/cartesian/expand_box.hpp>
#include <boost/geometry/strategy/cartesian/expand_segment.hpp>

#include <boost/geometry/strategies/cartesian/disjoint_box_box.hpp>
#include <boost/geometry/strategies/cartesian/disjoint_segment_box.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/cartesian/point_in_point.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_winding.hpp>
#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>
#include <boost/geometry/strategies/covered_by.hpp>
#include <boost/geometry/strategies/intersection.hpp>
#include <boost/geometry/strategies/intersection_result.hpp>
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/strategies/side_info.hpp>
#include <boost/geometry/strategies/within.hpp>

#include <boost/geometry/policies/robustness/rescale_policy_tags.hpp>
#include <boost/geometry/policies/robustness/robust_point_type.hpp>

#if defined(BOOST_GEOMETRY_DEBUG_ROBUSTNESS)
#  include <boost/geometry/io/wkt/write.hpp>
#endif

namespace boost { namespace geometry
{

namespace strategy { namespace intersection
{

/*!
    \see http://mathworld.wolfram.com/Line-LineIntersection.html
 */
template
<
    typename CalculationType = void
>
struct cartesian_segments
{
    typedef cartesian_tag cs_tag;

    typedef side::side_by_triangle<CalculationType> side_strategy_type;

    static inline side_strategy_type get_side_strategy()
    {
        return side_strategy_type();
    }

    template <typename Geometry1, typename Geometry2>
    struct point_in_geometry_strategy
    {
        typedef strategy::within::cartesian_winding
            <
                typename point_type<Geometry1>::type,
                typename point_type<Geometry2>::type,
                CalculationType
            > type;
    };

    template <typename Geometry1, typename Geometry2>
    static inline typename point_in_geometry_strategy<Geometry1, Geometry2>::type
        get_point_in_geometry_strategy()
    {
        typedef typename point_in_geometry_strategy
            <
                Geometry1, Geometry2
            >::type strategy_type;
        return strategy_type();
    }

    template <typename Geometry>
    struct area_strategy
    {
        typedef area::cartesian
            <
                CalculationType
            > type;
    };

    template <typename Geometry>
    static inline typename area_strategy<Geometry>::type get_area_strategy()
    {
        typedef typename area_strategy<Geometry>::type strategy_type;
        return strategy_type();
    }

    template <typename Geometry>
    struct distance_strategy
    {
        typedef distance::pythagoras
            <
                CalculationType
            > type;
    };

    template <typename Geometry>
    static inline typename distance_strategy<Geometry>::type get_distance_strategy()
    {
        typedef typename distance_strategy<Geometry>::type strategy_type;
        return strategy_type();
    }

    typedef envelope::cartesian<CalculationType> envelope_strategy_type;

    static inline envelope_strategy_type get_envelope_strategy()
    {
        return envelope_strategy_type();
    }

    typedef expand::cartesian_segment expand_strategy_type;

    static inline expand_strategy_type get_expand_strategy()
    {
        return expand_strategy_type();
    }

    typedef within::cartesian_point_point point_in_point_strategy_type;

    static inline point_in_point_strategy_type get_point_in_point_strategy()
    {
        return point_in_point_strategy_type();
    }

    typedef within::cartesian_point_point equals_point_point_strategy_type;

    static inline equals_point_point_strategy_type get_equals_point_point_strategy()
    {
        return equals_point_point_strategy_type();
    }

    typedef disjoint::cartesian_box_box disjoint_box_box_strategy_type;

    static inline disjoint_box_box_strategy_type get_disjoint_box_box_strategy()
    {
        return disjoint_box_box_strategy_type();
    }

    typedef disjoint::segment_box disjoint_segment_box_strategy_type;

    static inline disjoint_segment_box_strategy_type get_disjoint_segment_box_strategy()
    {
        return disjoint_segment_box_strategy_type();
    }

    typedef covered_by::cartesian_point_box disjoint_point_box_strategy_type;
    typedef covered_by::cartesian_point_box covered_by_point_box_strategy_type;
    typedef within::cartesian_point_box within_point_box_strategy_type;
    typedef envelope::cartesian_box envelope_box_strategy_type;
    typedef expand::cartesian_box expand_box_strategy_type;

    template <typename CoordinateType, typename SegmentRatio>
    struct segment_intersection_info
    {
    private :
        typedef typename select_most_precise
            <
                CoordinateType, double
            >::type promoted_type;

        promoted_type comparable_length_a() const
        {
            return dx_a * dx_a + dy_a * dy_a;
        }

        promoted_type comparable_length_b() const
        {
            return dx_b * dx_b + dy_b * dy_b;
        }

        template <typename Point, typename Segment1, typename Segment2>
        void assign_a(Point& point, Segment1 const& a, Segment2 const& ) const
        {
            assign(point, a, dx_a, dy_a, robust_ra);
        }
        template <typename Point, typename Segment1, typename Segment2>
        void assign_b(Point& point, Segment1 const& , Segment2 const& b) const
        {
            assign(point, b, dx_b, dy_b, robust_rb);
        }

        template <typename Point, typename Segment>
        void assign(Point& point, Segment const& segment,
                    CoordinateType const& dx, CoordinateType const& dy,
                    SegmentRatio const& ratio) const
        {
            // Calculate the intersection point based on segment_ratio
            // Up to now, division was postponed. Here we divide using numerator/
            // denominator. In case of integer this results in an integer
            // division.
            BOOST_GEOMETRY_ASSERT(ratio.denominator() != 0);

            typedef typename promote_integral<CoordinateType>::type calc_type;

            calc_type const numerator
                = boost::numeric_cast<calc_type>(ratio.numerator());
            calc_type const denominator
                = boost::numeric_cast<calc_type>(ratio.denominator());
            calc_type const dx_calc = boost::numeric_cast<calc_type>(dx);
            calc_type const dy_calc = boost::numeric_cast<calc_type>(dy);

            set<0>(point, get<0, 0>(segment)
                   + boost::numeric_cast<CoordinateType>(numerator * dx_calc
                                                         / denominator));
            set<1>(point, get<0, 1>(segment)
                   + boost::numeric_cast<CoordinateType>(numerator * dy_calc
                                                         / denominator));
        }

        template <int Index, int Dim, typename Point, typename Segment>
        static bool exceeds_side_in_dimension(Point& p, Segment const& s)
        {
            // Situation a (positive)
            //     0>-------------->1     segment
            // *                          point left of segment<I> in D x or y
            // Situation b (negative)
            //     1<--------------<0     segment
            // *                          point right of segment<I>
            // Situation c (degenerate), return false (check other dimension)
            auto const& c = get<Dim>(p);
            auto const& c0 = get<Index, Dim>(s);
            auto const& c1 = get<1 - Index, Dim>(s);
            return c0 < c1 ? math::smaller(c, c0)
                 : c0 > c1 ? math::larger(c, c0)
                 : false;
        }

        template <int Index, typename Point, typename Segment>
        static bool exceeds_side_of_segment(Point& p, Segment const& s)
        {
            return exceeds_side_in_dimension<Index, 0>(p, s)
                || exceeds_side_in_dimension<Index, 1>(p, s);
        }

        template <typename Point, typename Segment>
        static void assign_if_exceeds(Point& point, Segment const& s)
        {
            if (exceeds_side_of_segment<0>(point, s))
            {
                detail::assign_point_from_index<0>(s, point);
            }
            else if (exceeds_side_of_segment<1>(point, s))
            {
                detail::assign_point_from_index<1>(s, point);
            }
        }

    public :
        template <typename Point, typename Segment1, typename Segment2>
        void calculate(Point& point, Segment1 const& a, Segment2 const& b) const
        {
            bool use_a = true;

            // Prefer one segment if one is on or near an endpoint
            bool const a_near_end = robust_ra.near_end();
            bool const b_near_end = robust_rb.near_end();
            if (a_near_end && ! b_near_end)
            {
                use_a = true;
            }
            else if (b_near_end && ! a_near_end)
            {
                use_a = false;
            }
            else
            {
                // Prefer shorter segment
                promoted_type const len_a = comparable_length_a();
                promoted_type const len_b = comparable_length_b();
                if (len_b < len_a)
                {
                    use_a = false;
                }
                // else use_a is true but was already assigned like that
            }

            if (use_a)
            {
                assign_a(point, a, b);
            }
            else
            {
                assign_b(point, a, b);
            }

#if defined(BOOST_GEOMETRY_USE_RESCALING)
            return;
#endif

            // Verify nearly collinear cases (the threshold is arbitrary
            // but influences performance). If the intersection is located
            // outside the segments, then it should be moved.
            if (robust_ra.possibly_collinear(1.0e-3)
                && robust_rb.possibly_collinear(1.0e-3))
            {
                // The segments are nearly collinear and because of the calculation
                // method with very small denominator, the IP appears outside the
                // segment(s). Correct it to the end point.
                // Because they are nearly collinear, it doesn't really matter to
                // to which endpoint (or it is corrected twice).
                assign_if_exceeds(point, a);
                assign_if_exceeds(point, b);
            }
        }

        CoordinateType dx_a, dy_a;
        CoordinateType dx_b, dy_b;
        SegmentRatio robust_ra;
        SegmentRatio robust_rb;
    };

    template <typename D, typename W, typename ResultType>
    static inline void cramers_rule(D const& dx_a, D const& dy_a,
        D const& dx_b, D const& dy_b, W const& wx, W const& wy,
        // out:
        ResultType& nominator, ResultType& denominator)
    {
        // Cramers rule
        nominator = geometry::detail::determinant<ResultType>(dx_b, dy_b, wx, wy);
        denominator = geometry::detail::determinant<ResultType>(dx_a, dy_a, dx_b, dy_b);
        // Ratio r = nominator/denominator
        // Collinear if denominator == 0, intersecting if 0 <= r <= 1
        // IntersectionPoint = (x1 + r * dx_a, y1 + r * dy_a)
    }

    // Version for non-rescaled policies
    template
    <
        typename UniqueSubRange1,
        typename UniqueSubRange2,
        typename Policy
    >
    static inline typename Policy::return_type
        apply(UniqueSubRange1 const& range_p,
              UniqueSubRange2 const& range_q,
              Policy const& policy)
    {
        // Pass the same ranges both as normal ranges and as modelled ranges
        return apply(range_p, range_q, policy, range_p, range_q);
    }

    // Version for non rescaled versions.
    // The "modelled" parameter might be rescaled (will be removed later)
    template
    <
        typename UniqueSubRange1,
        typename UniqueSubRange2,
        typename Policy,
        typename ModelledUniqueSubRange1,
        typename ModelledUniqueSubRange2
    >
    static inline typename Policy::return_type
        apply(UniqueSubRange1 const& range_p,
              UniqueSubRange2 const& range_q,
              Policy const& policy,
              ModelledUniqueSubRange1 const& modelled_range_p,
              ModelledUniqueSubRange2 const& modelled_range_q)
    {
        typedef typename UniqueSubRange1::point_type point1_type;
        typedef typename UniqueSubRange2::point_type point2_type;

        BOOST_CONCEPT_ASSERT( (concepts::ConstPoint<point1_type>) );
        BOOST_CONCEPT_ASSERT( (concepts::ConstPoint<point2_type>) );

        point1_type const& p1 = range_p.at(0);
        point1_type const& p2 = range_p.at(1);
        point2_type const& q1 = range_q.at(0);
        point2_type const& q2 = range_q.at(1);

        // Declare segments, currently necessary for the policies
        // (segment_crosses, segment_colinear, degenerate, one_degenerate, etc)
        model::referring_segment<point1_type const> const p(p1, p2);
        model::referring_segment<point2_type const> const q(q1, q2);

        typedef typename select_most_precise
            <
                typename geometry::coordinate_type<typename ModelledUniqueSubRange1::point_type>::type,
                typename geometry::coordinate_type<typename ModelledUniqueSubRange1::point_type>::type
            >::type modelled_coordinate_type;

        typedef segment_ratio<modelled_coordinate_type> ratio_type;
        segment_intersection_info
            <
                typename select_calculation_type<point1_type, point2_type, CalculationType>::type,
                ratio_type
            > sinfo;

        sinfo.dx_a = get<0>(p2) - get<0>(p1); // distance in x-dir
        sinfo.dx_b = get<0>(q2) - get<0>(q1);
        sinfo.dy_a = get<1>(p2) - get<1>(p1); // distance in y-dir
        sinfo.dy_b = get<1>(q2) - get<1>(q1);

        return unified<ratio_type>(sinfo, p, q, policy, modelled_range_p, modelled_range_q);
    }

    //! Returns true if two segments do not overlap.
    //! If not, then no further calculations need to be done.
    template
    <
        std::size_t Dimension,
        typename PointP,
        typename PointQ
    >
    static inline bool disjoint_by_range(PointP const& p1, PointP const& p2,
                                         PointQ const& q1, PointQ const& q2)
    {
        auto minp = get<Dimension>(p1);
        auto maxp = get<Dimension>(p2);
        auto minq = get<Dimension>(q1);
        auto maxq = get<Dimension>(q2);
        if (minp > maxp)
        {
            std::swap(minp, maxp);
        }
        if (minq > maxq)
        {
            std::swap(minq, maxq);
        }

        // In this case, max(p) < min(q)
        //     P         Q
        // <-------> <------->
        // (and the space in between is not extremely small)
        return math::smaller(maxp, minq) || math::smaller(maxq, minp);
    }

    // Implementation for either rescaled or non rescaled versions.
    template
    <
        typename RatioType,
        typename SegmentInfo,
        typename Segment1,
        typename Segment2,
        typename Policy,
        typename UniqueSubRange1,
        typename UniqueSubRange2
    >
    static inline typename Policy::return_type
        unified(SegmentInfo& sinfo,
                Segment1 const& p, Segment2 const& q, Policy const&,
                UniqueSubRange1 const& range_p,
                UniqueSubRange2 const& range_q)
    {
        typedef typename UniqueSubRange1::point_type point1_type;
        typedef typename UniqueSubRange2::point_type point2_type;
        typedef typename select_most_precise
            <
                typename geometry::coordinate_type<point1_type>::type,
                typename geometry::coordinate_type<point2_type>::type
            >::type coordinate_type;

        point1_type const& p1 = range_p.at(0);
        point1_type const& p2 = range_p.at(1);
        point2_type const& q1 = range_q.at(0);
        point2_type const& q2 = range_q.at(1);

        bool const p_is_point = equals_point_point(p1, p2);
        bool const q_is_point = equals_point_point(q1, q2);

        if (p_is_point && q_is_point)
        {
            return equals_point_point(p1, q2)
                ? Policy::degenerate(p, true)
                : Policy::disjoint()
                ;
        }

        if (disjoint_by_range<0>(p1, p2, q1, q2)
         || disjoint_by_range<1>(p1, p2, q1, q2))
        {
            return Policy::disjoint();
        }

        side_info sides;
        sides.set<0>(side_strategy_type::apply(q1, q2, p1),
                     side_strategy_type::apply(q1, q2, p2));

        if (sides.same<0>())
        {
            // Both points are at same side of other segment, we can leave
            return Policy::disjoint();
        }

        sides.set<1>(side_strategy_type::apply(p1, p2, q1),
                     side_strategy_type::apply(p1, p2, q2));

        if (sides.same<1>())
        {
            // Both points are at same side of other segment, we can leave
            return Policy::disjoint();
        }

        bool collinear = sides.collinear();

        // Calculate the differences again
        // (for rescaled version, this is different from dx_p etc)
        coordinate_type const dx_p = get<0>(p2) - get<0>(p1);
        coordinate_type const dx_q = get<0>(q2) - get<0>(q1);
        coordinate_type const dy_p = get<1>(p2) - get<1>(p1);
        coordinate_type const dy_q = get<1>(q2) - get<1>(q1);

        // r: ratio 0-1 where intersection divides A/B
        // (only calculated for non-collinear segments)
        if (! collinear)
        {
            coordinate_type denominator_a, nominator_a;
            coordinate_type denominator_b, nominator_b;

            cramers_rule(dx_p, dy_p, dx_q, dy_q,
                get<0>(p1) - get<0>(q1),
                get<1>(p1) - get<1>(q1),
                nominator_a, denominator_a);

            cramers_rule(dx_q, dy_q, dx_p, dy_p,
                get<0>(q1) - get<0>(p1),
                get<1>(q1) - get<1>(p1),
                nominator_b, denominator_b);

            math::detail::equals_factor_policy<coordinate_type>
                policy(dx_p, dy_p, dx_q, dy_q);

            coordinate_type const zero = 0;
            if (math::detail::equals_by_policy(denominator_a, zero, policy)
             || math::detail::equals_by_policy(denominator_b, zero, policy))
            {
                // If this is the case, no rescaling is done for FP precision.
                // We set it to collinear, but it indicates a robustness issue.
                sides.set<0>(0, 0);
                sides.set<1>(0, 0);
                collinear = true;
            }
            else
            {
                sinfo.robust_ra.assign(nominator_a, denominator_a);
                sinfo.robust_rb.assign(nominator_b, denominator_b);
            }
        }

        if (collinear)
        {
            std::pair<bool, bool> const collinear_use_first
                    = is_x_more_significant(geometry::math::abs(dx_p),
                                            geometry::math::abs(dy_p),
                                            geometry::math::abs(dx_q),
                                            geometry::math::abs(dy_q),
                                            p_is_point, q_is_point);

            if (collinear_use_first.second)
            {
                // Degenerate cases: segments of single point, lying on other segment, are not disjoint
                // This situation is collinear too

                if (collinear_use_first.first)
                {
                    return relate_collinear<0, Policy, RatioType>(p, q,
                            p1, p2, q1, q2,
                            p_is_point, q_is_point);
                }
                else
                {
                    // Y direction contains larger segments (maybe dx is zero)
                    return relate_collinear<1, Policy, RatioType>(p, q,
                            p1, p2, q1, q2,
                            p_is_point, q_is_point);
                }
            }
        }

        return Policy::segments_crosses(sides, sinfo, p, q);
    }

private:
    // first is true if x is more significant
    // second is true if the more significant difference is not 0
    template <typename CoordinateType>
    static inline std::pair<bool, bool>
        is_x_more_significant(CoordinateType const& abs_dx_a,
                              CoordinateType const& abs_dy_a,
                              CoordinateType const& abs_dx_b,
                              CoordinateType const& abs_dy_b,
                              bool const a_is_point,
                              bool const b_is_point)
    {
        //BOOST_GEOMETRY_ASSERT_MSG(!(a_is_point && b_is_point), "both segments shouldn't be degenerated");

        // for degenerated segments the second is always true because this function
        // shouldn't be called if both segments were degenerated

        if (a_is_point)
        {
            return std::make_pair(abs_dx_b >= abs_dy_b, true);
        }
        else if (b_is_point)
        {
            return std::make_pair(abs_dx_a >= abs_dy_a, true);
        }
        else
        {
            CoordinateType const min_dx = (std::min)(abs_dx_a, abs_dx_b);
            CoordinateType const min_dy = (std::min)(abs_dy_a, abs_dy_b);
            return min_dx == min_dy ?
                    std::make_pair(true, min_dx > CoordinateType(0)) :
                    std::make_pair(min_dx > min_dy, true);
        }
    }

    template
    <
        std::size_t Dimension,
        typename Policy,
        typename RatioType,
        typename Segment1,
        typename Segment2,
        typename RobustPoint1,
        typename RobustPoint2
    >
    static inline typename Policy::return_type
        relate_collinear(Segment1 const& a,
                         Segment2 const& b,
                         RobustPoint1 const& robust_a1, RobustPoint1 const& robust_a2,
                         RobustPoint2 const& robust_b1, RobustPoint2 const& robust_b2,
                         bool a_is_point, bool b_is_point)
    {
        if (a_is_point)
        {
            return relate_one_degenerate<Policy, RatioType>(a,
                get<Dimension>(robust_a1),
                get<Dimension>(robust_b1), get<Dimension>(robust_b2),
                true);
        }
        if (b_is_point)
        {
            return relate_one_degenerate<Policy, RatioType>(b,
                get<Dimension>(robust_b1),
                get<Dimension>(robust_a1), get<Dimension>(robust_a2),
                false);
        }
        return relate_collinear<Policy, RatioType>(a, b,
                                get<Dimension>(robust_a1),
                                get<Dimension>(robust_a2),
                                get<Dimension>(robust_b1),
                                get<Dimension>(robust_b2));
    }

    /// Relate segments known collinear
    template
    <
        typename Policy,
        typename RatioType,
        typename Segment1,
        typename Segment2,
        typename Type1,
        typename Type2
    >
    static inline typename Policy::return_type
        relate_collinear(Segment1 const& a, Segment2 const& b,
                         Type1 oa_1, Type1 oa_2,
                         Type2 ob_1, Type2 ob_2)
    {
        // Calculate the ratios where a starts in b, b starts in a
        //         a1--------->a2         (2..7)
        //                b1----->b2      (5..8)
        // length_a: 7-2=5
        // length_b: 8-5=3
        // b1 is located w.r.t. a at ratio: (5-2)/5=3/5 (on a)
        // b2 is located w.r.t. a at ratio: (8-2)/5=6/5 (right of a)
        // a1 is located w.r.t. b at ratio: (2-5)/3=-3/3 (left of b)
        // a2 is located w.r.t. b at ratio: (7-5)/3=2/3 (on b)
        // A arrives (a2 on b), B departs (b1 on a)

        // If both are reversed:
        //         a2<---------a1         (7..2)
        //                b2<-----b1      (8..5)
        // length_a: 2-7=-5
        // length_b: 5-8=-3
        // b1 is located w.r.t. a at ratio: (8-7)/-5=-1/5 (before a starts)
        // b2 is located w.r.t. a at ratio: (5-7)/-5=2/5 (on a)
        // a1 is located w.r.t. b at ratio: (7-8)/-3=1/3 (on b)
        // a2 is located w.r.t. b at ratio: (2-8)/-3=6/3 (after b ends)

        // If both one is reversed:
        //         a1--------->a2         (2..7)
        //                b2<-----b1      (8..5)
        // length_a: 7-2=+5
        // length_b: 5-8=-3
        // b1 is located w.r.t. a at ratio: (8-2)/5=6/5 (after a ends)
        // b2 is located w.r.t. a at ratio: (5-2)/5=3/5 (on a)
        // a1 is located w.r.t. b at ratio: (2-8)/-3=6/3 (after b ends)
        // a2 is located w.r.t. b at ratio: (7-8)/-3=1/3 (on b)
        Type1 const length_a = oa_2 - oa_1; // no abs, see above
        Type2 const length_b = ob_2 - ob_1;

        RatioType ra_from(oa_1 - ob_1, length_b);
        RatioType ra_to(oa_2 - ob_1, length_b);
        RatioType rb_from(ob_1 - oa_1, length_a);
        RatioType rb_to(ob_2 - oa_1, length_a);

        // use absolute measure to detect endpoints intersection
        // NOTE: it'd be possible to calculate bx_wrt_a using ax_wrt_b values
        int const a1_wrt_b = position_value(oa_1, ob_1, ob_2);
        int const a2_wrt_b = position_value(oa_2, ob_1, ob_2);
        int const b1_wrt_a = position_value(ob_1, oa_1, oa_2);
        int const b2_wrt_a = position_value(ob_2, oa_1, oa_2);

        // fix the ratios if necessary
        // CONSIDER: fixing ratios also in other cases, if they're inconsistent
        // e.g. if ratio == 1 or 0 (so IP at the endpoint)
        // but position value indicates that the IP is in the middle of the segment
        // because one of the segments is very long
        // In such case the ratios could be moved into the middle direction
        // by some small value (e.g. EPS+1ULP)
        if (a1_wrt_b == 1)
        {
            ra_from.assign(0, 1);
            rb_from.assign(0, 1);
        }
        else if (a1_wrt_b == 3)
        {
            ra_from.assign(1, 1);
            rb_to.assign(0, 1);
        }

        if (a2_wrt_b == 1)
        {
            ra_to.assign(0, 1);
            rb_from.assign(1, 1);
        }
        else if (a2_wrt_b == 3)
        {
            ra_to.assign(1, 1);
            rb_to.assign(1, 1);
        }

        if ((a1_wrt_b < 1 && a2_wrt_b < 1) || (a1_wrt_b > 3 && a2_wrt_b > 3))
        //if ((ra_from.left() && ra_to.left()) || (ra_from.right() && ra_to.right()))
        {
            return Policy::disjoint();
        }

        bool const opposite = math::sign(length_a) != math::sign(length_b);

        return Policy::segments_collinear(a, b, opposite,
                                          a1_wrt_b, a2_wrt_b, b1_wrt_a, b2_wrt_a,
                                          ra_from, ra_to, rb_from, rb_to);
    }

    /// Relate segments where one is degenerate
    template
    <
        typename Policy,
        typename RatioType,
        typename DegenerateSegment,
        typename Type1,
        typename Type2
    >
    static inline typename Policy::return_type
        relate_one_degenerate(DegenerateSegment const& degenerate_segment,
                              Type1 d, Type2 s1, Type2 s2,
                              bool a_degenerate)
    {
        // Calculate the ratios where ds starts in s
        //         a1--------->a2         (2..6)
        //              b1/b2      (4..4)
        // Ratio: (4-2)/(6-2)
        RatioType const ratio(d - s1, s2 - s1);

        if (!ratio.on_segment())
        {
            return Policy::disjoint();
        }

        return Policy::one_degenerate(degenerate_segment, ratio, a_degenerate);
    }

    template <typename ProjCoord1, typename ProjCoord2>
    static inline int position_value(ProjCoord1 const& ca1,
                                     ProjCoord2 const& cb1,
                                     ProjCoord2 const& cb2)
    {
        // S1x  0   1    2     3   4
        // S2       |---------->
        return math::equals(ca1, cb1) ? 1
             : math::equals(ca1, cb2) ? 3
             : cb1 < cb2 ?
                ( ca1 < cb1 ? 0
                : ca1 > cb2 ? 4
                : 2 )
              : ( ca1 > cb1 ? 0
                : ca1 < cb2 ? 4
                : 2 );
    }

    template <typename Point1, typename Point2>
    static inline bool equals_point_point(Point1 const& point1, Point2 const& point2)
    {
        return strategy::within::cartesian_point_point::apply(point1, point2);
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType>
struct default_strategy<cartesian_tag, CalculationType>
{
    typedef cartesian_segments<CalculationType> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::intersection

namespace strategy
{

namespace within { namespace services
{

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

}} // within::services

namespace covered_by { namespace services
{

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, linear_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

template <typename Geometry1, typename Geometry2, typename AnyTag1, typename AnyTag2>
struct default_strategy<Geometry1, Geometry2, AnyTag1, AnyTag2, polygonal_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef strategy::intersection::cartesian_segments<> type;
};

}} // within::services

} // strategy

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP


#ifndef SDL_GUI_THREAD_HPP
#define SDL_GUI_THREAD_HPP

#include <boost/fiber/channel_op_status.hpp>
#include <scluk/language_extension.hpp>
#include <thread>
#include <complex>
#include "sdl/form.hpp"
#include "audio_params.hpp"

using namespace scluk::language_extension;

class sdl_gui_thread : std::jthread {

    public:
    struct gui_info_t {
        bool do_output_audio = true, do_apply_effect = false, do_print = false, do_exit = false;
        i32 pitch = 12;
        gui_info_t(){}
    } data;

    audio::gui_simplex_chan channel;
    sdl_gui_thread(gui_info_t gui_data = gui_info_t())
        : std::jthread(&sdl_gui_thread::run, this), data(gui_data), channel(16){}

    private:
    audio::dft_array hrm_arr;

    void run() {
        using namespace std::chrono_literals;

        sdl::window win("phase vocoder", { 1000, 540 });
        win.set_icon("music_note.png");
        const auto bg = sdl::color(0xa0, 0xe0, 0xa0, 0xff);
        const sdl::font font("/usr/share/fonts/TTF/FiraCode-Light.ttf", 12);
        win.set_text_color(sdl::color::black, bg);

        win.set_draw_cb([this, &font, bg](sdl::window& w) {
            if(channel.try_pop(hrm_arr) == boost::fibers::channel_op_status::success) {
                w.clear(bg);

                //find amplitude and freq of main harmonic
                const u64 N = hrm_arr.size()/2;

                u64 max_index = 0;
                f32 max_norm = 0.f;
                for(u64 i : range(N))
                    if(f32 norm = std::norm(hrm_arr[i]); norm > max_norm) {
                        max_norm = norm;
                        max_index = i;
                    }
                f32 max_ampl = std::abs(hrm_arr[max_index]);
                //draw the spectrum
                i32 min_y = 60;
                i32 max_y = w.res.y-10;
                f32 max_h = std::max(f32(max_y - min_y), 0.f);
                sdl::rect r { .x = 0, .y = max_y, .w = std::max(1, (w.res.x-20) / i32(N)), .h = 0 };

                for(u64 i : range(hrm_arr.size()/2)) {
                    r.h = -std::min(i32(max_h), i32(max_h * std::abs(hrm_arr[i]) / max_ampl));
                    r.x = 10 + i32(i) * r.w;
                    if(r.x > w.res.x - 10) break;
                    w.set_draw_color(i%2 ? sdl::color(0x7a, 0x9e, 0xb7, 0xff) : sdl::color(0x7a, 0x76, 0xb7, 0xff));
                    w.draw_rect(r);
                }
            }
            //draw text
            auto yn = [](bool b) -> const char* { return b ? "yes" : "no"; };
            w.draw_text(font,
                sout("[P] Printing: %\n[A] Audio output: %\n[F] Effect: %; %% semitones",
                yn(data.do_print), yn(data.do_output_audio), yn(data.do_apply_effect), data.pitch < 0 ? "-" : "+", abs(data.pitch)),
                { 20, 10 });
        });

        win.set_key_cb([this](sdl::window&, SDL_KeyboardEvent e) {
            switch(e.keysym.sym) {
                case SDLK_ESCAPE:
                    quit();
                    break;
                case SDLK_a:
                    if(e.type == SDL_KEYDOWN)
                        data.do_output_audio = !data.do_output_audio;
                    break;
                case SDLK_p:
                    if(e.type == SDL_KEYDOWN)
                        data.do_print = !data.do_print;
                    break;
                case SDLK_SPACE:
                    {
                        static bool spacebar_state = false;
                        bool last_spacebar_state = spacebar_state;
                        spacebar_state = e.type == SDL_KEYDOWN;
                        if(spacebar_state != last_spacebar_state)
                            data.do_apply_effect = !data.do_apply_effect;
                    }
                    break;
                case SDLK_f:
                    if(e.type == SDL_KEYDOWN)
                        data.do_apply_effect = !data.do_apply_effect;
                    break;
                case SDLK_RIGHT:
                case SDLK_KP_6:
                    if(e.type == SDL_KEYDOWN)
                        data.pitch++;
                    break;
                case SDLK_LEFT:
                case SDLK_KP_4:
                    if(e.type == SDL_KEYDOWN)
                        data.pitch--;
                    break;
            }
        });

        win.set_quit_cb([this](sdl::window&) { quit(); });

        while(!data.do_exit) {
            std::this_thread::sleep_for(25ms);
            win.poll_events();
            win.render_frame();
        }
    }

    void quit() {
        data.do_exit = true;
        scluk::out("GUI thread exiting");
    }
};

#endif //SDL_GUI_THREAD_HPP


#ifndef TREE_NODE_INL
#define TREE_NODE_INL

#include "TreeNode.h"

#include <boost/spirit/include/qi.hpp>

#include <limits>

namespace nuisken {
namespace randomforests {

template <class Type>
bool TreeNode<Type>::train(const std::vector<FeatureRawPtr>& features,
                           const TreeParameters& treeParameters,
                           std::vector<FeatureRawPtr>& leftFeatures,
                           std::vector<FeatureRawPtr>& rightFeatures) {
    //葉ノードであれば学習は行わない
    if (isLeaf() || depth >= treeParameters.getMaxDepth() ||
        features.size() <= treeParameters.getMinNumberOfData()) {
        leaf = true;
        return true;
    }

    //最適な結果の値を保持
    auto bestValue = -std::numeric_limits<double>::max();

    for (int i = 0; i < treeParameters.getNumberOfTrainIteration(); ++i) {
        //ランダムにパラメータを選択
        SplitParameters tempParameter = type.generateRandomParameter();

        //選択したパラメータで2点の特徴の差を計算
        std::vector<Container> splitValues;
        splitValues.reserve(features.size());
        for (const auto& feature : features) {
            splitValues.emplace_back(type.calculateSplitValue(feature, tempParameter), feature);
        }
        std::sort(std::begin(splitValues), std::end(splitValues),
                  [](const Container& x, const Container& y) { return x.first < y.first; });

        //τの範囲を決定（evaluateで計算した値の最小値～最大値の範囲）
        auto minValue = splitValues.front().first;
        auto maxValue = splitValues.back().first;

        if (0 == (maxValue - minValue)) {
            continue;
        }

        for (int j = 0; j < treeParameters.getNumberOfTauIteration(); ++j) {
            auto tempTau = type.generateTau(minValue, maxValue);
            std::vector<FeatureRawPtr> tempLeftFeatures;
            std::vector<FeatureRawPtr> tempRightFeatures;
            split(splitValues, tempTau, tempLeftFeatures, tempRightFeatures);

            //分割した結果を評価

            auto tempValue = type.evaluateSplit(tempLeftFeatures, tempRightFeatures);

            //よりよい結果なら結果を更新
            if (tempValue > bestValue) {
                bestValue = tempValue;
                leftFeatures = tempLeftFeatures;
                rightFeatures = tempRightFeatures;
                splitParameter = tempParameter;
                tau = tempTau;
            }
        }
    }

    if (0 == leftFeatures.size() || 0 == rightFeatures.size()) {
        leaf = true;
        return true;
    } else {
        return false;
    }
}

template <class Type>
void TreeNode<Type>::split(const std::vector<Container>& splitValues, double tau,
                           std::vector<FeatureRawPtr>& leftFeatures,
                           std::vector<FeatureRawPtr>& rightFeatures) const {
    leftFeatures.reserve(splitValues.size());
    rightFeatures.reserve(splitValues.size());

    auto splitItr = std::begin(splitValues);
    while ((*splitItr).first < tau) {
        leftFeatures.push_back((*splitItr).second);
        ++splitItr;
    }

    auto end = std::end(splitValues);
    // std::transform(splitItr, end, std::back_inserter(rightFeatures),
    //               [](const Container& a) {return a.second; });
    for (auto itr = splitItr; itr != end; ++itr) {
        rightFeatures.push_back((*itr).second);
    }
}

template <class Type>
typename TreeNode<Type>::LeafPtr TreeNode<Type>::match(const FeatureRawPtr& feature) const {
    if (isLeaf()) {
        return leafData;
    } else {
        if (type.decision(feature, splitParameter, tau)) {
            return leftChild->match(feature);
        } else {
            return rightChild->match(feature);
        }
    }
}

template <class Type>
void TreeNode<Type>::save(std::ofstream& treeStream) const {
    saveNode(treeStream);
    treeStream << std::endl;

    if (leftChild != 0) {
        leftChild->save(treeStream);
    }
    if (rightChild != 0) {
        rightChild->save(treeStream);
    }
}

template <class Type>
void TreeNode<Type>::saveNode(std::ofstream& treeStream) const {
    treeStream << depth << "," << leaf << "," << tau << ",";
    splitParameter.save(treeStream);

    if (leaf) {
        leafData->save(treeStream);
    }
}

template <class Type>
void TreeNode<Type>::load(std::ifstream& treeStream) {
    std::string line;
    std::getline(treeStream, line);
    boost::tokenizer<boost::escaped_list_separator<char>> tokenizer(line);
    std::queue<std::string> nodeElements;
    for (auto it = std::begin(tokenizer); it != std::end(tokenizer); ++it) {
        nodeElements.push(*it);
    }

    loadNode(nodeElements);

    if (leaf) {
        leafData = type.loadLeafData(nodeElements);
    } else {
        leftChild = std::make_unique<TreeNode<Type>>();
        leftChild->setType(type);
        leftChild->load(treeStream);

        rightChild = std::make_unique<TreeNode<Type>>();
        rightChild->setType(type);
        rightChild->load(treeStream);
    }
}

template <class Type>
void TreeNode<Type>::loadNode(std::queue<std::string>& nodeElements) {
    boost::spirit::qi::parse(std::begin(nodeElements.front()), std::end(nodeElements.front()),
                             boost::spirit::qi::int_, depth);
    nodeElements.pop();

    int leafInt;
    boost::spirit::qi::parse(std::begin(nodeElements.front()), std::end(nodeElements.front()),
                             boost::spirit::qi::int_, leafInt);
    leaf = static_cast<bool>(leafInt);
    nodeElements.pop();

    boost::spirit::qi::parse(std::begin(nodeElements.front()), std::end(nodeElements.front()),
                             boost::spirit::qi::double_, tau);
    nodeElements.pop();

    splitParameter.load(nodeElements);
}
}
}

#endif


// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <Eigen/Eigenvalues>

#include "SurgSim/Collision/CollisionPair.h"
#include "SurgSim/Collision/Representation.h"
#include "SurgSim/DataStructures/DataGroupBuilder.h"
#include "SurgSim/DataStructures/DataStructuresConvert.h"
#include "SurgSim/Framework/FrameworkConvert.h"
#include "SurgSim/Input/InputComponent.h"
#include "SurgSim/Input/OutputComponent.h"
#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Math/Matrix.h"
#include "SurgSim/Math/Quaternion.h"
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Vector.h"
#include "SurgSim/Physics/RigidRepresentation.h"
#include "SurgSim/Physics/VirtualToolCoupler.h"

using SurgSim::Math::makeSkewSymmetricMatrix;
using SurgSim::Math::Vector3d;
using SurgSim::Math::Vector6d;
using SurgSim::Math::Matrix33d;
using SurgSim::Math::Matrix66d;
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Quaterniond;
using SurgSim::Framework::checkAndConvert;

namespace SurgSim
{

namespace Physics
{
SURGSIM_REGISTER(SurgSim::Framework::Component, SurgSim::Physics::VirtualToolCoupler, VirtualToolCoupler);

VirtualToolCoupler::VirtualToolCoupler(const std::string& name) :
	SurgSim::Framework::Behavior(name),
	m_poseName(SurgSim::DataStructures::Names::POSE),
	m_linearStiffness(std::numeric_limits<double>::quiet_NaN()),
	m_linearDamping(std::numeric_limits<double>::quiet_NaN()),
	m_angularStiffness(std::numeric_limits<double>::quiet_NaN()),
	m_angularDamping(std::numeric_limits<double>::quiet_NaN()),
	m_localAttachmentPoint(Vector3d(std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
									std::numeric_limits<double>::quiet_NaN())),
	m_calculateInertialTorques(false),
	m_initializeRigidWithInputPose(true),
	m_logger(SurgSim::Framework::Logger::getLogger("Physics/VirtualToolCoupler")),
	m_hapticOutputOnlyWhenColliding(false),
	m_previousInputPose(Math::RigidTransform3d::Identity()),
	m_poseIndex(-1),
	m_linearVelocityIndex(-1),
	m_angularVelocityIndex(-1),
	m_forceIndex(-1),
	m_torqueIndex(-1),
	m_inputLinearVelocityIndex(-1),
	m_inputAngularVelocityIndex(-1),
	m_inputPoseIndex(-1),
	m_springJacobianIndex(-1),
	m_damperJacobianIndex(-1)
{
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
									  LinearStiffness, getOptionalLinearStiffness, setOptionalLinearStiffness);
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
									  LinearDamping, getOptionalLinearDamping, setOptionalLinearDamping);
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
									  AngularStiffness, getOptionalAngularStiffness, setOptionalAngularStiffness);
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, SurgSim::DataStructures::OptionalValue<double>,
									  AngularDamping, getOptionalAngularDamping, setOptionalAngularDamping);
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler,
									  SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>, AttachmentPoint,
									  getOptionalAttachmentPoint, setOptionalAttachmentPoint);
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, bool, CalculateInertialTorques,
									  getCalculateInertialTorques, setCalculateInertialTorques);
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, bool, HapticOutputOnlyWhenColliding,
									  isHapticOutputOnlyWhenColliding, setHapticOutputOnlyWhenColliding);

	SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, std::shared_ptr<SurgSim::Framework::Component>,
									  Input, getInput, setInput);
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, std::shared_ptr<SurgSim::Framework::Component>,
									  Output, getOutput, setOutput);
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, std::shared_ptr<SurgSim::Framework::Component>,
									  Representation, getRepresentation, setRepresentation);

	SURGSIM_ADD_SERIALIZABLE_PROPERTY(VirtualToolCoupler, bool, IntializeRigidWithInputPose,
									  isInitializingRigidWithInputPose, setInitializeRigidWithInputPose);
}

VirtualToolCoupler::~VirtualToolCoupler()
{
}

const std::shared_ptr<SurgSim::Framework::Component> VirtualToolCoupler::getInput()
{
	return m_input;
}

void VirtualToolCoupler::setInput(const std::shared_ptr<SurgSim::Framework::Component> input)
{
	m_input = checkAndConvert<SurgSim::Input::InputComponent>(input, "SurgSim::Input::InputComponent");
}

const std::shared_ptr<SurgSim::Framework::Component> VirtualToolCoupler::getOutput()
{
	return m_output;
}

void VirtualToolCoupler::setOutput(const std::shared_ptr<SurgSim::Framework::Component> output)
{
	m_output = checkAndConvert<SurgSim::Input::OutputComponent>(output, "SurgSim::Input::OutputComponent");
}

const std::shared_ptr<SurgSim::Framework::Component> VirtualToolCoupler::getRepresentation()
{
	return m_rigid;
}

void VirtualToolCoupler::setRepresentation(const std::shared_ptr<SurgSim::Framework::Component> rigid)
{
	m_rigid = std::dynamic_pointer_cast<SurgSim::Physics::RigidRepresentation>(rigid);
}

const std::string& VirtualToolCoupler::getPoseName()
{
	return m_poseName;
}

void VirtualToolCoupler::setPoseName(const std::string& poseName)
{
	m_poseName = poseName;
}

void VirtualToolCoupler::update(double dt)
{
	DataStructures::DataGroup inputData;
	m_input->getData(&inputData);
	inputData.poses().cacheIndex(m_poseName, &m_poseIndex);
	inputData.vectors().cacheIndex(DataStructures::Names::LINEAR_VELOCITY, &m_linearVelocityIndex);
	if (m_linearVelocityIndex < 0)
	{
		SURGSIM_LOG_ONCE(m_logger, WARNING) << getFullName() << " is receiving an input DataGroup that does not "
			<< "contain a linear velocity vector named " << DataStructures::Names::LINEAR_VELOCITY
			<< ", so it will be estimated.";
	}
	inputData.vectors().cacheIndex(DataStructures::Names::ANGULAR_VELOCITY, &m_angularVelocityIndex);
	if (m_angularVelocityIndex < 0)
	{
		SURGSIM_LOG_ONCE(m_logger, WARNING) << getFullName() << " is receiving an input DataGroup that does not "
			<< "contain an angular velocity vector named " << DataStructures::Names::ANGULAR_VELOCITY
			<< ") so it will be estimated.";
	}

	RigidTransform3d inputPose;
	if (inputData.poses().get(m_poseIndex, &inputPose))
	{
		Vector3d inputLinearVelocity(Vector3d::Zero());
		const bool gotInputLinearVelocity = inputData.vectors().get(m_linearVelocityIndex, &inputLinearVelocity);
		if (!gotInputLinearVelocity)
		{
			inputLinearVelocity = (inputPose.translation() - m_previousInputPose.translation()) / dt;
		}

		Vector3d inputAngularVelocity(Vector3d::Zero());
		const bool gotInputAngularVelocity = inputData.vectors().get(m_angularVelocityIndex, &inputAngularVelocity);
		if (!gotInputAngularVelocity)
		{
			Math::computeRotationVector(inputPose, m_previousInputPose, &inputAngularVelocity);
			inputAngularVelocity /= dt;
		}
		m_previousInputPose = inputPose;

		RigidTransform3d inputAlignment = m_input->getLocalPose();
		inputPose = inputAlignment * inputPose;
		if (m_initializeRigidWithInputPose)
		{
			m_initializeRigidWithInputPose = false;
			setInitialInputPose(inputPose);
		}
		inputLinearVelocity = inputAlignment.linear() * inputLinearVelocity;
		inputAngularVelocity = inputAlignment.rotation() * inputAngularVelocity;

		RigidState objectState(m_rigid->getCurrentState());
		RigidTransform3d objectPose(objectState.getPose());
		Vector3d objectPosition = objectPose * m_rigid->getMassCenter();
		Vector3d attachmentPoint = objectPose * m_localAttachmentPoint;
		Vector3d leverArm = attachmentPoint - objectPosition;
		Vector3d attachmentPointVelocity = objectState.getLinearVelocity();
		attachmentPointVelocity += objectState.getAngularVelocity().cross(leverArm);

		Vector3d force = m_linearStiffness * (inputPose.translation() - attachmentPoint);
		force += m_linearDamping * (inputLinearVelocity - attachmentPointVelocity);

		Vector3d rotationVector;
		SurgSim::Math::computeRotationVector(inputPose, objectPose, &rotationVector);
		Vector3d torque = m_angularStiffness * rotationVector;
		torque += m_angularDamping * (inputAngularVelocity - objectState.getAngularVelocity());

		Vector6d generalizedForce;
		generalizedForce << force, torque;

		const Matrix33d identity3x3 = Matrix33d::Identity();
		const Matrix33d zero3x3 = Matrix33d::Zero();
		Matrix66d generalizedStiffness;
		generalizedStiffness << m_linearStiffness* identity3x3, zero3x3,
							 zero3x3, m_angularStiffness* identity3x3;
		Matrix66d generalizedDamping;
		generalizedDamping << m_linearDamping* identity3x3, zero3x3,
						   zero3x3, m_angularDamping* identity3x3;

		if (m_calculateInertialTorques)
		{
			SurgSim::DataStructures::Location location;
			location.rigidLocalPosition.setValue(m_localAttachmentPoint);
			m_rigid->addExternalGeneralizedForce(location, generalizedForce, generalizedStiffness, generalizedDamping);
		}
		else
		{
			m_rigid->addExternalGeneralizedForce(generalizedForce, generalizedStiffness, generalizedDamping);
		}

		if (m_output != nullptr)
		{
			RigidTransform3d outputAlignment = m_output->getLocalPose().inverse();
			Matrix33d outputAlignmentUnScaled = outputAlignment.rotation();

			bool output = true;
			if (m_hapticOutputOnlyWhenColliding)
			{
				auto collision = m_rigid->getCollisionRepresentation();
				output = (collision == nullptr) || (collision->getCollisions().unsafeGet().size() > 0);
			}

			if (output)
			{
				m_outputData.vectors().set(m_forceIndex, outputAlignmentUnScaled * (-force));
				m_outputData.vectors().set(m_torqueIndex, outputAlignmentUnScaled * (-torque));
				if (gotInputLinearVelocity && gotInputAngularVelocity)
				{
					m_outputData.matrices().set(m_springJacobianIndex, -generalizedStiffness);
					m_outputData.matrices().set(m_damperJacobianIndex, -generalizedDamping);
				}
				else
				{
					m_outputData.matrices().reset(m_springJacobianIndex);
					m_outputData.matrices().reset(m_damperJacobianIndex);
				}
			}
			else
			{
				m_outputData.vectors().set(m_forceIndex, Vector3d::Zero());
				m_outputData.vectors().set(m_torqueIndex, Vector3d::Zero());
				m_outputData.matrices().reset(m_springJacobianIndex);
				m_outputData.matrices().reset(m_damperJacobianIndex);
			}

			if (gotInputLinearVelocity)
			{
				m_outputData.vectors().set(m_inputLinearVelocityIndex, outputAlignment.linear() * inputLinearVelocity);
			}
			else
			{
				m_outputData.vectors().reset(m_inputLinearVelocityIndex);
			}

			if (gotInputAngularVelocity)
			{
				m_outputData.vectors().set(m_inputAngularVelocityIndex, outputAlignmentUnScaled * inputAngularVelocity);
			}
			else
			{
				m_outputData.vectors().reset(m_inputAngularVelocityIndex);
			}
			m_outputData.poses().set(m_inputPoseIndex, outputAlignment * inputPose);
			m_output->setData(m_outputData);
		}
	}
}

bool VirtualToolCoupler::doInitialize()
{
	m_outputData = buildOutputData();

	m_forceIndex = m_outputData.vectors().getIndex(SurgSim::DataStructures::Names::FORCE);
	m_torqueIndex = m_outputData.vectors().getIndex(SurgSim::DataStructures::Names::TORQUE);
	m_inputLinearVelocityIndex = m_outputData.vectors().getIndex(SurgSim::DataStructures::Names::INPUT_LINEAR_VELOCITY);
	m_inputAngularVelocityIndex =
		m_outputData.vectors().getIndex(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY);
	m_inputPoseIndex = m_outputData.poses().getIndex(SurgSim::DataStructures::Names::INPUT_POSE);
	m_springJacobianIndex = m_outputData.matrices().getIndex(SurgSim::DataStructures::Names::SPRING_JACOBIAN);
	m_damperJacobianIndex = m_outputData.matrices().getIndex(SurgSim::DataStructures::Names::DAMPER_JACOBIAN);

	return true;
}

SurgSim::DataStructures::DataGroup VirtualToolCoupler::buildOutputData()
{
	SurgSim::DataStructures::DataGroupBuilder builder;
	builder.addVector(SurgSim::DataStructures::Names::FORCE);
	builder.addVector(SurgSim::DataStructures::Names::TORQUE);
	builder.addMatrix(SurgSim::DataStructures::Names::SPRING_JACOBIAN);
	builder.addPose(SurgSim::DataStructures::Names::INPUT_POSE);
	builder.addMatrix(SurgSim::DataStructures::Names::DAMPER_JACOBIAN);
	builder.addVector(SurgSim::DataStructures::Names::INPUT_LINEAR_VELOCITY);
	builder.addVector(SurgSim::DataStructures::Names::INPUT_ANGULAR_VELOCITY);
	return builder.createData();
}

bool VirtualToolCoupler::doWakeUp()
{
	if (m_input == nullptr)
	{
		SURGSIM_LOG_SEVERE(m_logger) <<
									 "VirtualToolCoupler named " << getName() << " does not have an Input Component.";
		return false;
	}
	if (m_rigid == nullptr)
	{
		SURGSIM_LOG_SEVERE(m_logger) << "VirtualToolCoupler named " << getName() << " does not have a Representation.";
		return false;
	}

	if (m_optionalAttachmentPoint.hasValue())
	{
		m_localAttachmentPoint = m_optionalAttachmentPoint.getValue();
	}
	else
	{
		m_localAttachmentPoint = m_rigid->getMassCenter();
	}

	// Provide sensible defaults based on the rigid representation.
	// If one or both of the stiffness and damping are not provided, they are
	// calculated to provide a critically damped system (dampingRatio-1.0).
	// For a mass-spring system, the damping ratio is defined as:
	//
	//     dampingRatio = (damping) / (2 * sqrt(mass * stiffness))
	//
	double dampingRatio = 1.0;
	double mass = m_rigid->getMass();
	if (!m_optionalLinearDamping.hasValue())
	{
		if (m_optionalLinearStiffness.hasValue())
		{
			m_linearStiffness = m_optionalLinearStiffness.getValue();
		}
		else
		{
			m_linearStiffness = mass * 100.0;
		}
		m_linearDamping = 2.0 * dampingRatio * sqrt(mass * m_linearStiffness);
	}
	else
	{
		m_linearDamping = m_optionalLinearDamping.getValue();
		if (m_optionalLinearStiffness.hasValue())
		{
			m_linearStiffness = m_optionalLinearStiffness.getValue();
		}
		else
		{
			m_linearStiffness = pow(m_linearDamping / dampingRatio, 2) / (4.0 * mass);
		}
	}

	Matrix33d leverArmMatrix = makeSkewSymmetricMatrix((m_localAttachmentPoint - m_rigid->getMassCenter()).eval());
	const Matrix33d& inertia = m_rigid->getLocalInertia() + mass * leverArmMatrix * leverArmMatrix;
	double maxInertia = inertia.eigenvalues().real().maxCoeff();
	if (!m_optionalAngularDamping.hasValue())
	{
		if (m_optionalAngularStiffness.hasValue())
		{
			m_angularStiffness = m_optionalAngularStiffness.getValue();
		}
		else
		{
			m_angularStiffness = maxInertia * 1000.0;
		}
		m_angularDamping = 2.0 * dampingRatio * sqrt(maxInertia * m_angularStiffness);
	}
	else
	{
		m_angularDamping = m_optionalAngularDamping.getValue();
		if (m_optionalAngularStiffness.hasValue())
		{
			m_angularStiffness = m_optionalAngularStiffness.getValue();
		}
		else
		{
			m_angularStiffness = pow(m_angularDamping / dampingRatio, 2) / (4.0 * maxInertia);
		}
	}

	SURGSIM_LOG_INFO(m_logger) << "VirtualToolCoupler named '" << getName() <<
		"' has linear stiffness " << m_linearStiffness << ", linear damping " << m_linearDamping
		<< ", angular stiffness " << m_angularStiffness << ", and angular damping " << m_angularDamping
		<< ". Its Physics Representation has mass " << m_rigid->getMass();
	return true;
}

int VirtualToolCoupler::getTargetManagerType() const
{
	return SurgSim::Framework::MANAGER_TYPE_PHYSICS;
}

void VirtualToolCoupler::setInitialInputPose(const SurgSim::Math::RigidTransform3d& pose)
{
	RigidState state = m_rigid->getCurrentState();
	state.setPose(pose);
	m_rigid->setInitialState(state);
}

void VirtualToolCoupler::overrideLinearStiffness(double linearStiffness)
{
	SURGSIM_ASSERT(!isInitialized()) << "Cannot override vtc parameter after it has initialized";

	m_optionalLinearStiffness.setValue(linearStiffness);
	m_linearStiffness = linearStiffness;
}

double VirtualToolCoupler::getLinearStiffness()
{
	SURGSIM_ASSERT(isAwake() || m_optionalLinearStiffness.hasValue())
			<< "Vtc parameter has not been initialized";

	return m_linearStiffness;
}

void VirtualToolCoupler::overrideLinearDamping(double linearDamping)
{
	SURGSIM_ASSERT(!isInitialized()) << "Cannot override vtc parameter after it has initialized";

	m_optionalLinearDamping.setValue(linearDamping);
	m_linearDamping = linearDamping;
}

double VirtualToolCoupler::getLinearDamping()
{
	SURGSIM_ASSERT(isAwake() || m_optionalLinearDamping.hasValue())
			<< "Vtc parameter has not been initialized";

	return m_linearDamping;
}

void VirtualToolCoupler::overrideAngularStiffness(double angularStiffness)
{
	SURGSIM_ASSERT(!isInitialized()) << "Cannot override vtc parameter after it has initialized";

	m_optionalAngularStiffness.setValue(angularStiffness);
	m_angularStiffness = angularStiffness;
}

double VirtualToolCoupler::getAngularStiffness()
{
	SURGSIM_ASSERT(isAwake() || m_optionalAngularStiffness.hasValue())
			<< "Vtc parameter has not been initialized";

	return m_angularStiffness;
}

void VirtualToolCoupler::overrideAngularDamping(double angularDamping)
{
	SURGSIM_ASSERT(!isInitialized()) << "Cannot override vtc parameter after it has initialized";

	m_optionalAngularDamping.setValue(angularDamping);
	m_angularDamping = angularDamping;
}

double VirtualToolCoupler::getAngularDamping()
{
	SURGSIM_ASSERT(isAwake() || m_optionalAngularDamping.hasValue())
			<< "Vtc parameter has not been initialized";

	return m_angularDamping;
}

void VirtualToolCoupler::overrideAttachmentPoint(const SurgSim::Math::Vector3d& attachment)
{
	SURGSIM_ASSERT(!isInitialized()) << "Cannot override vtc parameter after it has initialized";

	m_optionalAttachmentPoint.setValue(attachment);
	m_localAttachmentPoint = attachment;
}

const SurgSim::Math::Vector3d& VirtualToolCoupler::getAttachmentPoint()
{
	SURGSIM_ASSERT(isAwake() || m_optionalAttachmentPoint.hasValue())
			<< "Vtc parameter has not been initialized";
	return m_localAttachmentPoint;
}

void VirtualToolCoupler::setOptionalLinearStiffness(
	const SurgSim::DataStructures::OptionalValue<double>& linearStiffness)
{
	m_optionalLinearStiffness = linearStiffness;
}

const SurgSim::DataStructures::OptionalValue<double>& VirtualToolCoupler::getOptionalLinearStiffness() const
{
	return m_optionalLinearStiffness;
}

void VirtualToolCoupler::setOptionalLinearDamping(const SurgSim::DataStructures::OptionalValue<double>& linearDamping)
{
	m_optionalLinearDamping = linearDamping;
}

const SurgSim::DataStructures::OptionalValue<double>& VirtualToolCoupler::getOptionalLinearDamping() const
{
	return m_optionalLinearDamping;
}

void VirtualToolCoupler::setOptionalAngularStiffness(
	const SurgSim::DataStructures::OptionalValue<double>& angularStiffness)
{
	m_optionalAngularStiffness = angularStiffness;
}

const SurgSim::DataStructures::OptionalValue<double>& VirtualToolCoupler::getOptionalAngularStiffness() const
{
	return m_optionalAngularStiffness;
}

void VirtualToolCoupler::setOptionalAngularDamping(
	const SurgSim::DataStructures::OptionalValue<double>& angularDamping)
{
	m_optionalAngularDamping = angularDamping;
}

const SurgSim::DataStructures::OptionalValue<double>& VirtualToolCoupler::getOptionalAngularDamping() const
{
	return m_optionalAngularDamping;
}

void VirtualToolCoupler::setOptionalAttachmentPoint(
	const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>& attachmentPoint)
{
	m_optionalAttachmentPoint = attachmentPoint;
}

const SurgSim::DataStructures::OptionalValue<SurgSim::Math::Vector3d>&
VirtualToolCoupler::getOptionalAttachmentPoint() const
{
	return m_optionalAttachmentPoint;
}

void VirtualToolCoupler::setCalculateInertialTorques(bool calculateInertialTorques)
{
	m_calculateInertialTorques = calculateInertialTorques;
}

bool VirtualToolCoupler::getCalculateInertialTorques() const
{
	return m_calculateInertialTorques;
}

void VirtualToolCoupler::setInitializeRigidWithInputPose(bool val)
{
	m_initializeRigidWithInputPose = val;
}

void VirtualToolCoupler::doRetire()
{
	m_outputData.resetAll();
	Behavior::doRetire();
}

void VirtualToolCoupler::setLocalActive(bool val)
{
	m_outputData.resetAll();
	Behavior::setLocalActive(val);
}

bool VirtualToolCoupler::isHapticOutputOnlyWhenColliding() const
{
	return m_hapticOutputOnlyWhenColliding;
}

void VirtualToolCoupler::setHapticOutputOnlyWhenColliding(bool hapticOutputOnlyWhenColliding)
{
	m_hapticOutputOnlyWhenColliding = hapticOutputOnlyWhenColliding;
}

bool VirtualToolCoupler::isInitializingRigidWithInputPose() const
{
	return m_initializeRigidWithInputPose;
}

}; /// Physics
}; /// SurgSim


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 arithmetic toolbox - toints/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////

#include <nt2/arithmetic/include/functions/toints.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( toints_real,  BOOST_SIMD_REAL_TYPES)
{

  using nt2::toints;
  using nt2::tag::toints_;
  typedef typename boost::dispatch::meta::call<toints_(T)>::type              r_t;
  typedef typename boost::dispatch::meta::as_integer<T, signed>::type wished_r_t;

  NT2_TEST_TYPE_IS( r_t, wished_r_t );
  // specific values tests
  NT2_TEST_EQUAL(toints(nt2::Inf<T>()) , nt2::Inf<r_t>());
  NT2_TEST_EQUAL(toints(nt2::Minf<T>()), nt2::Minf<r_t>());
  NT2_TEST_EQUAL(toints(nt2::Mone<T>()), nt2::Mone<r_t>());
  NT2_TEST_EQUAL(toints(nt2::Nan<T>()) , nt2::Zero<r_t>());
  NT2_TEST_EQUAL(toints(nt2::One<T>()) , nt2::One<r_t>());
  NT2_TEST_EQUAL(toints(nt2::Zero<T>()), nt2::Zero<r_t>());
 } // end of test for floating_

NT2_TEST_CASE_TPL ( toints_unsigned_int,  BOOST_SIMD_UNSIGNED_TYPES)
{

  using nt2::toints;
  using nt2::tag::toints_;
  typedef typename boost::dispatch::meta::call<toints_(T)>::type               r_t;
  typedef typename boost::dispatch::meta::as_integer<T, signed>::type wished_r_t;

  NT2_TEST_TYPE_IS( r_t, wished_r_t );
  // specific values tests
  NT2_TEST_EQUAL(toints(nt2::One<T>()), nt2::One<r_t>());
  NT2_TEST_EQUAL(toints(nt2::Zero<T>()), nt2::Zero<r_t>());
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( toints_signed_int,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{

  using nt2::toints;
  using nt2::tag::toints_;
  typedef typename boost::dispatch::meta::call<toints_(T)>::type               r_t;
  typedef typename boost::dispatch::meta::as_integer<T, signed>::type wished_r_t;

  NT2_TEST_TYPE_IS( r_t, wished_r_t );
//  NT2_TEST_EQUAL(toints(nt2::Mone<T>()), nt2::Valmax<r_t>());
  NT2_TEST_EQUAL(toints(nt2::Mone<T>()), nt2::Mone<r_t>());
  NT2_TEST_EQUAL(toints(nt2::One<T>()), nt2::One<r_t>());
  NT2_TEST_EQUAL(toints(nt2::Zero<T>()), nt2::Zero<r_t>());
} // end of test for signed_int_


/*
 * Copyright 2020 INRIA
 */

#ifndef __eigenpy_utils_scalar_is_approx_hpp__
#define __eigenpy_utils_scalar_is_approx_hpp__

#include <Eigen/Core>

namespace eigenpy
{
  template<typename MatrixType1, typename MatrixType2>
  inline EIGEN_DONT_INLINE bool is_approx(const Eigen::MatrixBase<MatrixType1> & mat1,
                                          const Eigen::MatrixBase<MatrixType2> & mat2,
                                          const typename MatrixType1::Scalar & prec)
  {
    return mat1.isApprox(mat2,prec);
  }

  template<typename MatrixType1, typename MatrixType2>
  inline EIGEN_DONT_INLINE bool is_approx(const Eigen::MatrixBase<MatrixType1> & mat1,
                                          const Eigen::MatrixBase<MatrixType2> & mat2)
  {
    return is_approx(mat1,mat2,Eigen::NumTraits<typename MatrixType1::Scalar>::dummy_precision());
  }
}

#endif // ifndef __eigenpy_utils_scalar_is_approx_hpp__


//          Copyright Oliver Kowalke 2009.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <cstdlib>
#include <stdexcept>
#include <vector>

#include <boost/assert.hpp>
#include <boost/bind.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include "boost/task.hpp"

namespace pt = boost::posix_time;
namespace tsk = boost::tasks;

typedef tsk::static_pool< tsk::unbounded_fifo > pool_type;

int serial_fib( int n)
{
	if( n < 2)
		return n;
	else
		return serial_fib( n - 1) + serial_fib( n - 2);
}

int parallel_fib( int n, int cutof)
{
	if ( n < cutof)
	{
		return serial_fib( n);
	}
	else
	{
		BOOST_ASSERT( boost::this_task::runs_in_pool() );

		tsk::handle< int > h1(
			tsk::async(
				tsk::make_task(
					parallel_fib,
					n - 1,
					cutof),
				tsk::as_sub_task() ) );

		tsk::handle< int > h2(
			tsk::async(
				tsk::make_task(
					parallel_fib,
					n - 2,
					cutof),
				tsk::as_sub_task() ) );

		return h1.get() + h2.get();
	}
}

int main( int argc, char *argv[])
{
	try
	{
		pool_type pool( tsk::poolsize( 5) );

		int n = 10;
		tsk::handle< int > h(
			tsk::async(
				tsk::make_task(
					parallel_fib,
					n,
					5),
				pool) );

		std::cout << "fibonacci(" << n << ") == " << h.get() << std::endl;

		return EXIT_SUCCESS;
	}
	catch ( std::exception const& e)
	{ std::cerr << "exception: " << e.what() << std::endl; }
	catch ( ... )
	{ std::cerr << "unhandled" << std::endl; }

	return EXIT_FAILURE;
}