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

#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/BasicAstrodynamics/sphericalStateConversions.h"
#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/Astrodynamics/Ephemerides/keplerEphemeris.h"
#include "Tudat/Astrodynamics/Relativity/metric.h"
#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/numericalAccelerationPartial.h"
#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/gravitationalParameter.h"
#include "Tudat/SimulationSetup/EstimationSetup/createTorquePartials.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/SimulationSetup/PropagationSetup/createTorqueModel.h"
#include "Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"

namespace tudat
{

namespace unit_tests
{

using namespace tudat::relativity;
using namespace tudat::gravitation;
using namespace tudat::aerodynamics;
using namespace tudat::ephemerides;
using namespace tudat::simulation_setup;
using namespace tudat::orbital_element_conversions;
using namespace tudat::unit_conversions;
using namespace tudat::orbit_determination;
using namespace tudat::acceleration_partials;
using namespace tudat::spice_interface;
using namespace tudat::orbit_determination;
using namespace tudat::estimatable_parameters;
using namespace tudat::electro_magnetism;
using namespace tudat::basic_astrodynamics;

BOOST_AUTO_TEST_SUITE( test_torque_partials )

//! Test if partial derivatives of degree 2 torque are correctly implemented
BOOST_AUTO_TEST_CASE( testSecondDegreeGravitationalTorquePartials )
{
    // Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Create bodies
    NamedBodyMap bodyMap;
    bodyMap[ "Mars" ] = std::make_shared< Body >( );
    bodyMap[ "Mars" ]->setEphemeris( std::make_shared< ephemerides::ConstantEphemeris >(
                                         [ = ]( ){ return Eigen::Vector6d::Zero( ); } ) );
    bodyMap[ "Mars" ]->setGravityFieldModel(
                std::make_shared< gravitation::GravityFieldModel >(
                    spice_interface::getBodyGravitationalParameter( "Mars" ) ) );
    bodyMap[ "Phobos" ] = std::make_shared< Body >( );
    std::shared_ptr< Body > phobos = bodyMap.at( "Phobos" );
    std::shared_ptr< Body > mars = bodyMap.at( "Mars" );

    // Set inertia tensor
    Eigen::Matrix3d phobosInertiaTensor = Eigen::Matrix3d::Zero( );
    phobosInertiaTensor( 0, 0 ) = 0.3615;
    phobosInertiaTensor( 1, 1 ) = 0.4265;
    phobosInertiaTensor( 2, 2 ) = 0.5024;

    phobosInertiaTensor *= ( 11.27E3 * 11.27E3 * 1.0659E16 );
    bodyMap[ "Phobos" ]->setBodyInertiaTensor(
                phobosInertiaTensor, ( 0.3615 + 0.4265 + 0.5024 ) / 3.0 );

    // Create gravity field
    double phobosGravitationalParameter = 1.0659E16 * physical_constants::GRAVITATIONAL_CONSTANT;
    double phobosReferenceRadius = 11.27E3;

    Eigen::MatrixXd phobosCosineGravityFieldCoefficients = Eigen::MatrixXd::Zero( 6, 6 ),
            phobosSineGravityFieldCoefficients = Eigen::MatrixXd::Zero( 6, 6 );
    double phobosScaledMeanMomentOfInertia;
    gravitation::getDegreeTwoSphericalHarmonicCoefficients(
                phobosInertiaTensor, phobosGravitationalParameter, phobosReferenceRadius, true,
                phobosCosineGravityFieldCoefficients, phobosSineGravityFieldCoefficients, phobosScaledMeanMomentOfInertia );
    bodyMap[ "Phobos" ]->setGravityFieldModel(
                std::make_shared< gravitation::SphericalHarmonicsGravityField >(
                    phobosGravitationalParameter, phobosReferenceRadius, phobosCosineGravityFieldCoefficients,
                    phobosSineGravityFieldCoefficients, "Phobos_Fixed",
                    std::bind( &Body::setBodyInertiaTensorFromGravityFieldAndExistingMeanMoment, bodyMap.at( "Phobos" ), true ) ) );

    Eigen::Quaterniond noRotationQuaternion = Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) );
    Eigen::Matrix< double, 7, 1 > unitRotationState = Eigen::Matrix< double, 7, 1 >::Zero( );
    unitRotationState( 0 ) = noRotationQuaternion.w( );
    unitRotationState( 1 ) = noRotationQuaternion.x( );
    unitRotationState( 2 ) = noRotationQuaternion.y( );
    unitRotationState( 3 ) = noRotationQuaternion.z( );

    // Set rotation and ephemeris models
    std::map< double, Eigen::Matrix< double, 7, 1 > > dummyRotationMap;
    dummyRotationMap[ -1.0E100 ] = unitRotationState;
    dummyRotationMap[ 1.0E100 ] = unitRotationState;
    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< double, 7, 1 > > > dummyInterpolator =
            std::make_shared< interpolators::LinearInterpolator< double, Eigen::Matrix< double, 7, 1 > > >( dummyRotationMap );
    bodyMap[ "Phobos" ]->setRotationalEphemeris( std::make_shared< TabulatedRotationalEphemeris< double, double > >(
                                                     dummyInterpolator, "ECLIPJ2000", "Phobos_Fixed" ) );

    Eigen::Vector6d phobosKeplerElements = Eigen::Vector6d::Zero( );
    double phobosSemiMajorAxis = 9376.0E3;
    phobosKeplerElements( 0 ) = phobosSemiMajorAxis;
    bodyMap[ "Phobos" ]->setEphemeris( std::make_shared< ephemerides::KeplerEphemeris >(
                                           phobosKeplerElements, 0.0, spice_interface::getBodyGravitationalParameter( "Mars" ),
                                           "Mars", "ECLIPJ2000" ) );

    setGlobalFrameBodyEphemerides( bodyMap, "Mars", "ECLIPJ2000" );

    // Update Phobos and Mars to current state
    double testTime = 1000.0;
    phobos->setStateFromEphemeris( testTime );
    mars->setStateFromEphemeris( testTime );

    // Set new Phobos rotational state
    Eigen::Vector7d phobosRotationalState = Eigen::Vector7d::Zero( );
    phobosRotationalState.segment( 0, 4 ) = tudat::linear_algebra::convertQuaternionToVectorFormat(
                Eigen::Quaterniond( Eigen::AngleAxisd( 0.4343, Eigen::Vector3d::UnitZ( ) ) *
                                    Eigen::AngleAxisd( 2.4354, Eigen::Vector3d::UnitX( ) ) *
                                    Eigen::AngleAxisd( 1.2434, Eigen::Vector3d::UnitY( ) ) ) );
    phobos->setCurrentRotationalStateToLocalFrame( phobosRotationalState );

    // Create torque due to mars on phobos.
    std::shared_ptr< SecondDegreeGravitationalTorqueModel > gravitationalTorque =
            createSecondDegreeGravitationalTorqueModel( bodyMap.at( "Phobos" ), bodyMap.at( "Mars" ), "Phobos", "Mars" );

    // Create torque partial.
    std::shared_ptr< TorquePartial > torquePartial =
            createAnalyticalTorquePartial( gravitationalTorque, std::make_pair( "Phobos", phobos ),
                                           std::make_pair( "Mars", mars ) );

    // Create parameter objects
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Mars", gravitational_parameter) );
    parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Phobos", mean_moment_of_inertia ) );
    parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                  1, 0, 3, 3, "Phobos", spherical_harmonics_cosine_coefficient_block ) );
    parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                  1, 1, 3, 3, "Phobos", spherical_harmonics_sine_coefficient_block ) );
    std::shared_ptr< EstimatableParameterSet< double > > parameterSet =
            createParametersToEstimate( parameterNames, bodyMap );

    std::shared_ptr< EstimatableParameter< double > > marsGravitationalParameterParameter =
            parameterSet->getEstimatedDoubleParameters( ).at( 0 );
    std::shared_ptr< EstimatableParameter< double > > phobosMeanMomentOfInertia =
            parameterSet->getEstimatedDoubleParameters( ).at( 1 );
    std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > phobosCosineCoefficientsParameter =
            parameterSet->getEstimatedVectorParameters( ).at( 0 );
    std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > phobosSineCoefficientsParameter =
            parameterSet->getEstimatedVectorParameters( ).at( 1 );

    // Calculate analytical partials.
    torquePartial->update( testTime );

    Eigen::MatrixXd partialWrtPhobosOrientation = Eigen::MatrixXd::Zero( 3, 4 );
    torquePartial->wrtOrientationOfAcceleratedBody( partialWrtPhobosOrientation.block( 0, 0, 3, 4 ) );

    Eigen::MatrixXd partialWrtPhobosRotationalVelocity = Eigen::Matrix3d::Zero( );
    torquePartial->wrtRotationalVelocityOfAcceleratedBody( partialWrtPhobosRotationalVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 );

    Eigen::MatrixXd partialWrtMarsOrientation = Eigen::MatrixXd::Zero( 3, 4  );
    torquePartial->wrtOrientationOfAcceleratingBody( partialWrtMarsOrientation.block( 0, 0, 3, 4 ) );

    Eigen::MatrixXd partialWrtMarsRotationalVelocity = Eigen::Matrix3d::Zero( );
    torquePartial->wrtRotationalVelocityOfAcceleratingBody( partialWrtMarsRotationalVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 );

    Eigen::MatrixXd partialWrtPhobosState = Eigen::MatrixXd::Zero( 3, 6 );
    torquePartial->wrtNonRotationalStateOfAdditionalBody(
                partialWrtPhobosState.block( 0, 0, 3, 6 ), std::make_pair( "Phobos", "" ), propagators::translational_state );
    Eigen::MatrixXd partialWrtMarsState = Eigen::MatrixXd::Zero( 3, 6 );
    torquePartial->wrtNonRotationalStateOfAdditionalBody(
                partialWrtMarsState.block( 0, 0, 3, 6 ), std::make_pair( "Mars", "" ), propagators::translational_state );

    Eigen::Vector3d partialWrtMarsGravitationalParameter = torquePartial->wrtParameter(
                marsGravitationalParameterParameter );
    Eigen::Vector3d partialWrtMeanMomentOfInertia = torquePartial->wrtParameter(
                phobosMeanMomentOfInertia );

    Eigen::MatrixXd partialWrtPhobosCosineCoefficients = torquePartial->wrtParameter(
                phobosCosineCoefficientsParameter );
    Eigen::MatrixXd partialWrtPhobosSineCoefficients = torquePartial->wrtParameter(
                phobosSineCoefficientsParameter );

    // Declare numerical partials.
    Eigen::Matrix3d testPartialWrtPhobosRotationalVelocity = Eigen::Matrix3d::Zero( );
    Eigen::Matrix< double, 3, 4 > testPartialWrtMarsOrientation = Eigen::Matrix< double, 3, 4 >::Zero( );
    Eigen::Matrix3d testPartialWrtMarsRotationalVelocity = Eigen::Matrix3d::Zero( );

    Eigen::Matrix3d testPartialWrtMarsPosition = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtMarsVelocity = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtPhobosPosition = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtPhobosVelocity = Eigen::Matrix3d::Zero( );

    // Define perturbations in orientation for numerical partial/
    Eigen::Vector4d orientationPerturbation;
    orientationPerturbation << 1.0E-9, 1.0E-9, 1.0E-9, 1.0E-9;
    Eigen::Vector3d rotationalVelocityPerturbation;
    rotationalVelocityPerturbation <<  1.0E-6, 1.0E-6, 1.0E-6;

    // Create state access/modification functions for bodies.
    std::function< void( Eigen::Vector7d ) > phobosRotationalStateSetFunction =
            std::bind( &Body::setCurrentRotationalStateToLocalFrame, phobos, std::placeholders::_1 );
    std::function< void( Eigen::Vector7d ) > marsRotationalStateSetFunction =
            std::bind( &Body::setCurrentRotationalStateToLocalFrame, mars, std::placeholders::_1 );

    // Calculate numerical partials wrt rotational state.
    std::vector< Eigen::Vector4d > appliedQuaternionPerturbation;
    Eigen::MatrixXd torqueDeviations = calculateTorqueDeviationDueToOrientationChange(
                phobosRotationalStateSetFunction, gravitationalTorque, phobos->getRotationalStateVector( ), orientationPerturbation,
                appliedQuaternionPerturbation );

    testPartialWrtPhobosRotationalVelocity =  calculateTorqueWrtRotationalStatePartials(
                phobosRotationalStateSetFunction, gravitationalTorque, phobos->getRotationalStateVector( ), rotationalVelocityPerturbation, 4, 3 );
    testPartialWrtMarsOrientation =  calculateTorqueWrtRotationalStatePartials(
                marsRotationalStateSetFunction, gravitationalTorque, mars->getRotationalStateVector( ), orientationPerturbation, 0, 4 );
    testPartialWrtMarsRotationalVelocity =  calculateTorqueWrtRotationalStatePartials(
                marsRotationalStateSetFunction, gravitationalTorque, mars->getRotationalStateVector( ), rotationalVelocityPerturbation, 4, 3 );

    std::function< void( Eigen::Vector6d ) > phobosStateSetFunction =
            std::bind( &Body::setState,  phobos, std::placeholders::_1 );
    std::function< void( Eigen::Vector6d ) > marsStateSetFunction =
            std::bind( &Body::setState, mars, std::placeholders::_1 );

    Eigen::Vector3d positionPerturbation;
    positionPerturbation << 1.0, 1.0, 100.0;
    Eigen::Vector3d velocityPerturbation;
    velocityPerturbation << 1.0E-3, 1.0E-3, 1.0E-3;

    // Calculate numerical partials wrt translational state.
    testPartialWrtMarsPosition = calculateTorqueWrtTranslationalStatePartials(
                marsStateSetFunction, gravitationalTorque, mars->getState( ), positionPerturbation, 0 );
    testPartialWrtMarsVelocity = calculateTorqueWrtTranslationalStatePartials(
                marsStateSetFunction, gravitationalTorque, mars->getState( ), velocityPerturbation, 3 );
    testPartialWrtPhobosPosition = calculateTorqueWrtTranslationalStatePartials(
                phobosStateSetFunction, gravitationalTorque, phobos->getState( ), positionPerturbation, 0 );
    testPartialWrtPhobosVelocity = calculateTorqueWrtTranslationalStatePartials(
                phobosStateSetFunction, gravitationalTorque, phobos->getState( ), velocityPerturbation, 3 );

    // Calculate numerical partials wrt parameters
    Eigen::Vector3d testPartialWrtMarsGravitationalParameter = calculateTorqueWrtParameterPartials(
                marsGravitationalParameterParameter, gravitationalTorque, 1.0E12 );
    Eigen::Vector3d testPartialWrtMeanMomentOfInertia = calculateTorqueWrtParameterPartials(
                phobosMeanMomentOfInertia, gravitationalTorque, 1.0E-1 );
    std::function< void( ) > updateFunction = &emptyFunction;
            //boost::bind( &Body::setBodyInertiaTensorFromGravityFieldAndExistingMeanMoment, bodyMap.at( "Phobos" ), true );
    Eigen::MatrixXd testPartialWrtPhobosCosineCoefficients = calculateTorqueWrtParameterPartials(
                phobosCosineCoefficientsParameter, gravitationalTorque,
                Eigen::VectorXd::Constant( phobosCosineCoefficientsParameter->getParameterSize( ), 1.0E-6 ), updateFunction );
    Eigen::MatrixXd testPartialWrtPhobosSineCoefficients = calculateTorqueWrtParameterPartials(
                phobosSineCoefficientsParameter, gravitationalTorque,
                Eigen::VectorXd::Constant( phobosSineCoefficientsParameter->getParameterSize( ), 1.0E-6 ), updateFunction );

    // Compare numerical and analytical results.
    for( int index = 1; index < 4; index++ )
    {
        Eigen::Vector3d numericalChangeInTorque = torqueDeviations.block( 0, index - 1, 3, 1 );
        Eigen::Vector3d analyticalChangeInTorque =
                partialWrtPhobosOrientation.block( 0, 0, 3, 1 ) * appliedQuaternionPerturbation[ index ]( 0 ) +
                partialWrtPhobosOrientation.block( 0, index, 3, 1 ) * appliedQuaternionPerturbation[ index ]( index );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( numericalChangeInTorque,
                                           analyticalChangeInTorque, 1.0E-6 );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosRotationalVelocity,
                                       partialWrtPhobosRotationalVelocity, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsOrientation,
                                       partialWrtMarsOrientation, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsRotationalVelocity,
                                       partialWrtMarsRotationalVelocity, std::numeric_limits< double >::epsilon( ) );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtPhobosState.block( 0, 0, 3, 3 ),
                                        testPartialWrtPhobosPosition, 1.0E-8 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtMarsState.block( 0, 0, 3, 3 ),
                                        testPartialWrtMarsPosition, 1.0E-8 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtPhobosState.block( 0, 3, 3, 3 ),
                                        testPartialWrtPhobosVelocity, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtMarsState.block( 0, 3, 3, 3 ),
                                        testPartialWrtMarsVelocity, std::numeric_limits< double >::epsilon( ) );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsGravitationalParameter,
                                       partialWrtMarsGravitationalParameter, 1.0E-6 );

    // Check derivative of z-component w.r.t. C20 separately: value is slightly non-zero due to rounding error
    BOOST_CHECK_SMALL( std::fabs( testPartialWrtPhobosCosineCoefficients( 2, 2 ) ), 1.0E6 );
    testPartialWrtPhobosCosineCoefficients( 2, 2 ) = 0.0;

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosCosineCoefficients,
                                       partialWrtPhobosCosineCoefficients, 1.0E-9 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosSineCoefficients,
                                       partialWrtPhobosSineCoefficients, 1.0E-9 );

    for( int i = 0; i < 3; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( partialWrtMeanMomentOfInertia( i, 0 ) - testPartialWrtMeanMomentOfInertia( i, 0 ) ), 1.0E1 );
    }
}

BOOST_AUTO_TEST_CASE( testInertialTorquePartials )
{
    // Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Create bodies
    NamedBodyMap bodyMap;
    bodyMap[ "Mars" ] = std::make_shared< Body >( );
    bodyMap[ "Mars" ]->setEphemeris( std::make_shared< ephemerides::ConstantEphemeris >(
                                         [ = ]( ){ return Eigen::Vector6d::Zero( ); } ) );
    bodyMap[ "Mars" ]->setGravityFieldModel(
                std::make_shared< gravitation::GravityFieldModel >(
                    spice_interface::getBodyGravitationalParameter( "Mars" ) ) );
    bodyMap[ "Phobos" ] = std::make_shared< Body >( );
    std::shared_ptr< Body > phobos = bodyMap.at( "Phobos" );
    std::shared_ptr< Body > mars = bodyMap.at( "Mars" );

    // Set inertia tensor
    Eigen::Matrix3d phobosInertiaTensor = Eigen::Matrix3d::Zero( );
    phobosInertiaTensor( 0, 0 ) = 0.3615;
    phobosInertiaTensor( 1, 1 ) = 0.4265;
    phobosInertiaTensor( 2, 2 ) = 0.5024;

    phobosInertiaTensor *= ( 11.27E3 * 11.27E3 * 1.0659E16 );
    bodyMap[ "Phobos" ]->setBodyInertiaTensor(
                phobosInertiaTensor, ( 0.3615 + 0.4265 + 0.5024 ) / 3.0 );

    // Create gravity field
    double phobosGravitationalParameter = 1.0659E16 * physical_constants::GRAVITATIONAL_CONSTANT;
    double phobosReferenceRadius = 11.27E3;

    Eigen::MatrixXd phobosCosineGravityFieldCoefficients = Eigen::MatrixXd::Zero( 6, 6 ),
            phobosSineGravityFieldCoefficients = Eigen::MatrixXd::Zero( 6, 6 );
    double phobosScaledMeanMomentOfInertia;
    gravitation::getDegreeTwoSphericalHarmonicCoefficients(
                phobosInertiaTensor, phobosGravitationalParameter, phobosReferenceRadius, true,
                phobosCosineGravityFieldCoefficients, phobosSineGravityFieldCoefficients, phobosScaledMeanMomentOfInertia );

    bodyMap[ "Phobos" ]->setGravityFieldModel(
                std::make_shared< gravitation::SphericalHarmonicsGravityField >(
                    phobosGravitationalParameter, phobosReferenceRadius, phobosCosineGravityFieldCoefficients,
                    phobosSineGravityFieldCoefficients, "Phobos_Fixed" ) );

    Eigen::Quaterniond noRotationQuaternion = Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) );
    Eigen::Matrix< double, 7, 1 > unitRotationState = Eigen::Matrix< double, 7, 1 >::Zero( );
    unitRotationState( 0 ) = noRotationQuaternion.w( );
    unitRotationState( 1 ) = noRotationQuaternion.x( );
    unitRotationState( 2 ) = noRotationQuaternion.y( );
    unitRotationState( 3 ) = noRotationQuaternion.z( );
    unitRotationState( 4 ) = 1.0E-5;
    unitRotationState( 5 ) = 2.0E-5;
    unitRotationState( 6 ) = -3.2E-5;

    // Set rotation and ephemeris models
    std::map< double, Eigen::Matrix< double, 7, 1 > > dummyRotationMap;
    dummyRotationMap[ -1.0E100 ] = unitRotationState;
    dummyRotationMap[ 1.0E100 ] = unitRotationState;
    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< double, 7, 1 > > > dummyInterpolator =
            std::make_shared< interpolators::LinearInterpolator< double, Eigen::Matrix< double, 7, 1 > > >( dummyRotationMap );
    bodyMap[ "Phobos" ]->setRotationalEphemeris( std::make_shared< TabulatedRotationalEphemeris< double, double > >(
                                                     dummyInterpolator, "ECLIPJ2000", "Phobos_Fixed" ) );

    Eigen::Vector6d phobosKeplerElements = Eigen::Vector6d::Zero( );
    double phobosSemiMajorAxis = 9376.0E3;
    phobosKeplerElements( 0 ) = phobosSemiMajorAxis;
    bodyMap[ "Phobos" ]->setEphemeris( std::make_shared< ephemerides::KeplerEphemeris >(
                                           phobosKeplerElements, 0.0, spice_interface::getBodyGravitationalParameter( "Mars" ),
                                           "Mars", "ECLIPJ2000" ) );

    setGlobalFrameBodyEphemerides( bodyMap, "Mars", "ECLIPJ2000" );

    // Update Phobos and Mars to current state
    double testTime = 1000.0;
    phobos->setStateFromEphemeris( testTime );
    mars->setStateFromEphemeris( testTime );

    // Set new Phobos rotational state
    Eigen::Vector7d phobosRotationalState = Eigen::Vector7d::Zero( );
    phobosRotationalState.segment( 0, 4 ) = tudat::linear_algebra::convertQuaternionToVectorFormat(
                Eigen::Quaterniond( Eigen::AngleAxisd( 0.4343, Eigen::Vector3d::UnitZ( ) ) *
                                    Eigen::AngleAxisd( 2.4354, Eigen::Vector3d::UnitX( ) ) *
                                    Eigen::AngleAxisd( 1.2434, Eigen::Vector3d::UnitY( ) ) ) );
    phobosRotationalState( 4 ) = 1.0E-5;
    phobosRotationalState( 5 ) = 2.0E-5;
    phobosRotationalState( 6 ) = -3.2E-5;
    phobos->setCurrentRotationalStateToLocalFrame( phobosRotationalState );

    // Create torque due to mars on phobos.
    std::shared_ptr< InertialTorqueModel > inertialTorqueModel =
            createInertialTorqueModel( bodyMap.at( "Phobos" ), "Phobos" );
    inertialTorqueModel->updateMembers( 0.0 );

    SingleBodyTorqueModelMap torqueList;
    torqueList[ "Phobos" ].push_back( inertialTorqueModel );

    // Create shared_ptr partial.
    std::shared_ptr< TorquePartial > torquePartial =
            createAnalyticalTorquePartial( inertialTorqueModel, std::make_pair( "Phobos", phobos ),
                                           std::make_pair( "Phobos", phobos ) );

    // Create parameter objects.
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Phobos", gravitational_parameter) );
    parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Phobos", mean_moment_of_inertia ) );

    parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                  1, 0, 3, 3, "Phobos", spherical_harmonics_cosine_coefficient_block ) );
    parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                  1, 1, 3, 3, "Phobos", spherical_harmonics_sine_coefficient_block ) );

    std::shared_ptr< EstimatableParameterSet< double > > parameterSet =
            createParametersToEstimate( parameterNames, bodyMap );

    std::shared_ptr< EstimatableParameter< double > > phobosGravitationalParameterParameter =
            parameterSet->getEstimatedDoubleParameters( ).at( 0 );
    std::shared_ptr< EstimatableParameter< double > > meanMomentOfInertiaParameter =
            parameterSet->getEstimatedDoubleParameters( ).at( 1 );
    std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > phobosCosineCoefficientsParameter =
            parameterSet->getEstimatedVectorParameters( ).at( 0 );
    std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > phobosSineCoefficientsParameter =
            parameterSet->getEstimatedVectorParameters( ).at( 1 );

    // Calculate analytical partials.
    torquePartial->update( testTime );

    Eigen::MatrixXd partialWrtPhobosOrientation = Eigen::MatrixXd::Zero( 3, 4 );
    torquePartial->wrtOrientationOfAcceleratedBody( partialWrtPhobosOrientation.block( 0, 0, 3, 4 ) );
    Eigen::MatrixXd partialWrtPhobosRotationalVelocity = Eigen::Matrix3d::Zero( );
    torquePartial->wrtRotationalVelocityOfAcceleratedBody( partialWrtPhobosRotationalVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 );
    Eigen::MatrixXd partialWrtMarsOrientation = Eigen::MatrixXd::Zero( 3, 4  );
    torquePartial->wrtOrientationOfAcceleratingBody( partialWrtMarsOrientation.block( 0, 0, 3, 4 ) );
    Eigen::MatrixXd partialWrtMarsRotationalVelocity = Eigen::Matrix3d::Zero( );
    torquePartial->wrtRotationalVelocityOfAcceleratingBody( partialWrtMarsRotationalVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 );

    Eigen::MatrixXd partialWrtPhobosState = Eigen::MatrixXd::Zero( 3, 6 );
    torquePartial->wrtNonRotationalStateOfAdditionalBody(
                partialWrtPhobosState.block( 0, 0, 3, 6 ), std::make_pair( "Phobos", "" ), propagators::translational_state );
    Eigen::MatrixXd partialWrtMarsState = Eigen::MatrixXd::Zero( 3, 6 );
    torquePartial->wrtNonRotationalStateOfAdditionalBody(
                partialWrtMarsState.block( 0, 0, 3, 6 ), std::make_pair( "Mars", "" ), propagators::translational_state );

    Eigen::Vector3d partialWrtPhobosGravitationalParameter = torquePartial->wrtParameter(
                phobosGravitationalParameterParameter );
    Eigen::Vector3d partialWrtMeanMomentOfInertia = torquePartial->wrtParameter(
                meanMomentOfInertiaParameter );

    Eigen::MatrixXd partialWrtPhobosCosineCoefficients = torquePartial->wrtParameter(
                phobosCosineCoefficientsParameter );
    Eigen::MatrixXd partialWrtPhobosSineCoefficients = torquePartial->wrtParameter(
                phobosSineCoefficientsParameter );

    // Declare numerical partials.
    Eigen::Matrix3d testPartialWrtPhobosRotationalVelocity = Eigen::Matrix3d::Zero( );
    Eigen::Matrix< double, 3, 4 > testPartialWrtMarsOrientation = Eigen::Matrix< double, 3, 4 >::Zero( );
    Eigen::Matrix3d testPartialWrtMarsRotationalVelocity = Eigen::Matrix3d::Zero( );

    Eigen::Matrix3d testPartialWrtMarsPosition = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtMarsVelocity = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtPhobosPosition = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtPhobosVelocity = Eigen::Matrix3d::Zero( );

    // Declare perturbations in orientation for numerical partial
    Eigen::Vector4d orientationPerturbation;
    orientationPerturbation << 1.0E-9, 1.0E-9, 1.0E-9, 1.0E-9;
    Eigen::Vector3d rotationalVelocityPerturbation;
    rotationalVelocityPerturbation <<  1.0E-6, 1.0E-6, 1.0E-6;

    // Create state access/modification functions for bodies.
    std::function< void( Eigen::Vector7d ) > phobosRotationalStateSetFunction =
            std::bind( &Body::setCurrentRotationalStateToLocalFrame, phobos, std::placeholders::_1 );
    std::function< void( Eigen::Vector7d ) > marsRotationalStateSetFunction =
            std::bind( &Body::setCurrentRotationalStateToLocalFrame, mars, std::placeholders::_1 );

    // Calculate numerical partials.
    std::vector< Eigen::Vector4d > appliedQuaternionPerturbation;
    Eigen::MatrixXd torqueDeviations = calculateTorqueDeviationDueToOrientationChange(
                phobosRotationalStateSetFunction, inertialTorqueModel, phobos->getRotationalStateVector( ), orientationPerturbation,
                appliedQuaternionPerturbation );

    testPartialWrtPhobosRotationalVelocity =  calculateTorqueWrtRotationalStatePartials(
                phobosRotationalStateSetFunction, inertialTorqueModel, phobos->getRotationalStateVector( ), rotationalVelocityPerturbation, 4, 3 );
    testPartialWrtMarsOrientation =  calculateTorqueWrtRotationalStatePartials(
                marsRotationalStateSetFunction, inertialTorqueModel, mars->getRotationalStateVector( ), orientationPerturbation, 0, 4 );
    testPartialWrtMarsRotationalVelocity =  calculateTorqueWrtRotationalStatePartials(
                marsRotationalStateSetFunction, inertialTorqueModel, mars->getRotationalStateVector( ), rotationalVelocityPerturbation, 4, 3 );

    std::function< void( Eigen::Vector6d ) > phobosStateSetFunction =
            std::bind( &Body::setState,  phobos, std::placeholders::_1 );
    std::function< void( Eigen::Vector6d ) > marsStateSetFunction =
            std::bind( &Body::setState, mars, std::placeholders::_1 );

    Eigen::Vector3d positionPerturbation;
    positionPerturbation << 1.0, 1.0, 100.0;
    Eigen::Vector3d velocityPerturbation;
    velocityPerturbation << 1.0E-3, 1.0E-3, 1.0E-3;

    // Calculate numerical partials.
    testPartialWrtMarsPosition = calculateTorqueWrtTranslationalStatePartials(
                marsStateSetFunction, inertialTorqueModel, mars->getState( ), positionPerturbation, 0 );
    testPartialWrtMarsVelocity = calculateTorqueWrtTranslationalStatePartials(
                marsStateSetFunction, inertialTorqueModel, mars->getState( ), velocityPerturbation, 3 );
    testPartialWrtPhobosPosition = calculateTorqueWrtTranslationalStatePartials(
                phobosStateSetFunction, inertialTorqueModel, phobos->getState( ), positionPerturbation, 0 );
    testPartialWrtPhobosVelocity = calculateTorqueWrtTranslationalStatePartials(
                phobosStateSetFunction, inertialTorqueModel, phobos->getState( ), velocityPerturbation, 3 );

    std::function< void( ) > updateFunction = //&emptyFunction;
            std::bind( &Body::setBodyInertiaTensorFromGravityFieldAndExistingMeanMoment, bodyMap.at( "Phobos" ), true );
    Eigen::Vector3d testPartialWrtPhobosGravitationalParameter = calculateTorqueWrtParameterPartials(
                phobosGravitationalParameterParameter, inertialTorqueModel, 1.0E8, updateFunction );
    Eigen::MatrixXd testPartialWrtMeanMomentOfInertia = calculateTorqueWrtParameterPartials(
                meanMomentOfInertiaParameter, inertialTorqueModel, 1.0E-1, updateFunction );
    Eigen::MatrixXd testPartialWrtPhobosCosineCoefficients = calculateTorqueWrtParameterPartials(
                phobosCosineCoefficientsParameter, inertialTorqueModel,
                Eigen::VectorXd::Constant( phobosCosineCoefficientsParameter->getParameterSize( ), 1.0E-6 ), updateFunction );
    Eigen::MatrixXd testPartialWrtPhobosSineCoefficients = calculateTorqueWrtParameterPartials(
                phobosSineCoefficientsParameter, inertialTorqueModel,
                Eigen::VectorXd::Constant( phobosSineCoefficientsParameter->getParameterSize( ), 1.0E-6 ), updateFunction );

    //    // Compare numerical and analytical results.
    for( int index = 1; index < 4; index++ )
    {
        Eigen::Vector3d numericalChangeInTorque = torqueDeviations.block( 0, index - 1, 3, 1 );
        Eigen::Vector3d analyticalChangeInTorque =
                partialWrtPhobosOrientation.block( 0, 0, 3, 1 ) * appliedQuaternionPerturbation[ index ]( 0 ) +
                partialWrtPhobosOrientation.block( 0, index, 3, 1 ) * appliedQuaternionPerturbation[ index ]( index );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( numericalChangeInTorque,
                                           analyticalChangeInTorque, std::numeric_limits< double >::epsilon( ) );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosRotationalVelocity,
                                       partialWrtPhobosRotationalVelocity, 1.0E-10 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsOrientation,
                                       partialWrtMarsOrientation, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsRotationalVelocity,
                                       partialWrtMarsRotationalVelocity, std::numeric_limits< double >::epsilon( ) );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtPhobosState.block( 0, 0, 3, 3 ),
                                        testPartialWrtPhobosPosition, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtMarsState.block( 0, 0, 3, 3 ),
                                        testPartialWrtMarsPosition, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtPhobosState.block( 0, 3, 3, 3 ),
                                        testPartialWrtPhobosVelocity, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtMarsState.block( 0, 3, 3, 3 ),
                                        testPartialWrtMarsVelocity, std::numeric_limits< double >::epsilon( ) );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosGravitationalParameter,
                                       partialWrtPhobosGravitationalParameter, 1.0E-6 );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtPhobosCosineCoefficients.block( 0, 0, 3, 2 ),
                                       Eigen::MatrixXd::Zero( 3, 2 ), std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtPhobosCosineCoefficients.block( 0, 5, 3, 4 ),
                                       Eigen::MatrixXd::Zero( 3, 4 ), std::numeric_limits< double >::epsilon( ) );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtPhobosSineCoefficients.block( 0, 0, 3, 1 ),
                                       Eigen::MatrixXd::Zero( 3, 1 ), std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtPhobosSineCoefficients.block( 0, 3, 3, 3 ),
                                       Eigen::MatrixXd::Zero( 3, 3 ), std::numeric_limits< double >::epsilon( ) );

    // Check derivative of z-component w.r.t. C20 separately: value is slightly non-zero due to rounding error
    BOOST_CHECK_SMALL( std::fabs( testPartialWrtPhobosCosineCoefficients( 2, 2 ) ), 1.0E5 );
    testPartialWrtPhobosCosineCoefficients( 2, 2 ) = 0.0;

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosCosineCoefficients,
                                       partialWrtPhobosCosineCoefficients, 1.0E-9 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosSineCoefficients,
                                       partialWrtPhobosSineCoefficients, 1.0E-9 );

    for( int i = 0; i < 3; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( partialWrtMeanMomentOfInertia( i, 0 ) - testPartialWrtMeanMomentOfInertia( i, 0 ) ), 1.0E1 );
    }
}

class EffectiveTorqueModel: public basic_astrodynamics::TorqueModel
{
public:
    EffectiveTorqueModel(
            const std::function< Eigen::Matrix3d( ) > inertiaTensorFunction,
            const SingleBodyTorqueModelMap& torqueList ):
        inertiaTensorFunction_( inertiaTensorFunction ), torqueList_( torqueList ){ }

    Eigen::Vector3d getTorque( )
    {
        return effectiveTorque_;
    }

    void updateMembers( const double currentTime )
    {
        Eigen::Vector3d totalTorque = Eigen::Vector3d::Zero( );
        for( auto it = torqueList_.begin( ); it != torqueList_.end( ); it++ )
        {
            for( unsigned int i = 0; i < it->second.size( ); i++ )
            {
                totalTorque += it->second.at( i )->getTorque( );
            }
        }

        effectiveTorque_ = inertiaTensorFunction_( ).inverse( ) * totalTorque;
    }

private:

    std::function< Eigen::Matrix3d( ) > inertiaTensorFunction_;

    SingleBodyTorqueModelMap torqueList_;

    Eigen::Vector3d effectiveTorque_;
};

BOOST_AUTO_TEST_CASE( testConstantTorquePartials )
{
    // Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    NamedBodyMap bodyMap;
    bodyMap[ "Mars" ] = std::make_shared< Body >( );
    bodyMap[ "Mars" ]->setEphemeris( std::make_shared< ephemerides::ConstantEphemeris >(
                                         [ = ]( ){ return Eigen::Vector6d::Zero( ); } ) );
    bodyMap[ "Mars" ]->setGravityFieldModel(
                std::make_shared< gravitation::GravityFieldModel >(
                    spice_interface::getBodyGravitationalParameter( "Mars" ) ) );
    bodyMap[ "Phobos" ] = std::make_shared< Body >( );

    Eigen::Matrix3d phobosInertiaTensor = Eigen::Matrix3d::Zero( );
    phobosInertiaTensor( 0, 0 ) = 0.3615;
    phobosInertiaTensor( 1, 1 ) = 0.4265;
    phobosInertiaTensor( 2, 2 ) = 0.5024;

    phobosInertiaTensor *= ( 11.27E3 * 11.27E3 * 1.0659E16 );
    bodyMap[ "Phobos" ]->setBodyInertiaTensor(
                phobosInertiaTensor, ( 0.3615 + 0.4265 + 0.5024 ) / 3.0 );

    double phobosGravitationalParameter = 1.0659E16 * physical_constants::GRAVITATIONAL_CONSTANT;
    double phobosReferenceRadius = 11.27E3;

    Eigen::MatrixXd phobosCosineGravityFieldCoefficients = Eigen::MatrixXd::Zero( 6, 6 ),
            phobosSineGravityFieldCoefficients = Eigen::MatrixXd::Zero( 6, 6 );
    double phobosScaledMeanMomentOfInertia;
    gravitation::getDegreeTwoSphericalHarmonicCoefficients(
                phobosInertiaTensor, phobosGravitationalParameter, phobosReferenceRadius, true,
                phobosCosineGravityFieldCoefficients, phobosSineGravityFieldCoefficients, phobosScaledMeanMomentOfInertia );

    bodyMap[ "Phobos" ]->setGravityFieldModel(
                std::make_shared< gravitation::SphericalHarmonicsGravityField >(
                    phobosGravitationalParameter, phobosReferenceRadius, phobosCosineGravityFieldCoefficients,
                    phobosSineGravityFieldCoefficients, "Phobos_Fixed",
                    std::bind( &Body::setBodyInertiaTensorFromGravityFieldAndExistingMeanMoment, bodyMap.at( "Phobos" ), true ) ) );

    Eigen::Quaterniond noRotationQuaternion = Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) );
    Eigen::Matrix< double, 7, 1 > unitRotationState = Eigen::Matrix< double, 7, 1 >::Zero( );
    unitRotationState( 0 ) = noRotationQuaternion.w( );
    unitRotationState( 1 ) = noRotationQuaternion.x( );
    unitRotationState( 2 ) = noRotationQuaternion.y( );
    unitRotationState( 3 ) = noRotationQuaternion.z( );
    unitRotationState( 4 ) = 1.0E-5;
    unitRotationState( 5 ) = 2.0E-5;
    unitRotationState( 6 ) = -3.2E-5;

    std::map< double, Eigen::Matrix< double, 7, 1 > > dummyRotationMap;
    dummyRotationMap[ -1.0E100 ] = unitRotationState;
    dummyRotationMap[ 1.0E100 ] = unitRotationState;

    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< double, 7, 1 > > > dummyInterpolator =
            std::make_shared< interpolators::LinearInterpolator< double, Eigen::Matrix< double, 7, 1 > > >( dummyRotationMap );
    bodyMap[ "Phobos" ]->setRotationalEphemeris( std::make_shared< TabulatedRotationalEphemeris< double, double > >(
                                                     dummyInterpolator, "ECLIPJ2000", "Phobos_Fixed" ) );

    Eigen::Vector6d phobosKeplerElements = Eigen::Vector6d::Zero( );
    double phobosSemiMajorAxis = 9376.0E3;
    phobosKeplerElements( 0 ) = phobosSemiMajorAxis;

    bodyMap[ "Phobos" ]->setEphemeris( std::make_shared< ephemerides::KeplerEphemeris >(
                                           phobosKeplerElements, 0.0, spice_interface::getBodyGravitationalParameter( "Mars" ),
                                           "Mars", "ECLIPJ2000" ) );

    // Create empty bodies, phobos and mars.
    std::shared_ptr< Body > phobos = bodyMap.at( "Phobos" );
    std::shared_ptr< Body > mars = bodyMap.at( "Mars" );
    setGlobalFrameBodyEphemerides( bodyMap, "Mars", "ECLIPJ2000" );

    double testTime = 1000.0;
    phobos->setStateFromEphemeris( testTime );

    Eigen::Vector7d phobosRotationalState = Eigen::Vector7d::Zero( );
    phobosRotationalState.segment( 0, 4 ) = tudat::linear_algebra::convertQuaternionToVectorFormat(
                Eigen::Quaterniond( Eigen::AngleAxisd( 0.4343, Eigen::Vector3d::UnitZ( ) ) *
                                    Eigen::AngleAxisd( 2.4354, Eigen::Vector3d::UnitX( ) ) *
                                    Eigen::AngleAxisd( 1.2434, Eigen::Vector3d::UnitY( ) ) ) );
    phobosRotationalState( 4 ) = 1.0E-5;
    phobosRotationalState( 5 ) = 2.0E-5;
    phobosRotationalState( 6 ) = -3.2E-5;

    phobos->setCurrentRotationalStateToLocalFrame( phobosRotationalState );

    mars->setStateFromEphemeris( testTime );
    //    mars->setCurrentRotationalStateToLocalFrameFromEphemeris( 0.0 );

    // Create acceleration due to mars on phobos.
    std::shared_ptr< SecondDegreeGravitationalTorqueModel > gravitationalTorque =
            createSecondDegreeGravitationalTorqueModel( bodyMap.at( "Phobos" ), bodyMap.at( "Mars" ), "Phobos", "Mars" );
    std::shared_ptr< InertialTorqueModel > inertialTorqueModel =
            createInertialTorqueModel( bodyMap.at( "Phobos" ), "Phobos" );
    gravitationalTorque->updateMembers( 0.0 );
    inertialTorqueModel->updateMembers( 0.0 );

    SingleBodyTorqueModelMap torqueList;
    torqueList[ "Mars" ].push_back( gravitationalTorque );
    torqueList[ "Phobos" ].push_back( inertialTorqueModel );

    std::shared_ptr< EffectiveTorqueModel > effectiveTorqueModel =
            std::make_shared< EffectiveTorqueModel >(
                std::bind( &Body::getBodyInertiaTensor, bodyMap.at( "Phobos" ) ), torqueList );
    effectiveTorqueModel->updateMembers( 0.0 );

    // Create central gravity partial.
    std::shared_ptr< TorquePartial > torquePartial =
            createConstantTorqueRotationalDynamicsPartial(
                std::make_pair( "Phobos", bodyMap.at( "Phobos" ) ), torqueList );

    // Create gravitational parameter object.
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Phobos", gravitational_parameter) );
    parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Phobos", mean_moment_of_inertia ) );

    parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                  1, 0, 3, 3, "Phobos", spherical_harmonics_cosine_coefficient_block ) );
    parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                  1, 1, 3, 3, "Phobos", spherical_harmonics_sine_coefficient_block ) );

    std::shared_ptr< EstimatableParameterSet< double > > parameterSet =
            createParametersToEstimate( parameterNames, bodyMap );

    std::shared_ptr< EstimatableParameter< double > > phobosGravitationalParameterParameter =
            parameterSet->getEstimatedDoubleParameters( ).at( 0 );
    std::shared_ptr< EstimatableParameter< double > > meanMomentOfInertiaParameter =
            parameterSet->getEstimatedDoubleParameters( ).at( 1 );
    std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > phobosCosineCoefficientsParameter =
            parameterSet->getEstimatedVectorParameters( ).at( 0 );
    std::shared_ptr< EstimatableParameter< Eigen::VectorXd > > phobosSineCoefficientsParameter =
            parameterSet->getEstimatedVectorParameters( ).at( 1 );

    // Calculate analytical partials.
    torquePartial->update( testTime );

    Eigen::MatrixXd partialWrtPhobosOrientation = Eigen::MatrixXd::Zero( 3, 4 );
    torquePartial->wrtOrientationOfAcceleratedBody( partialWrtPhobosOrientation.block( 0, 0, 3, 4 ) );
    Eigen::MatrixXd partialWrtPhobosRotationalVelocity = Eigen::Matrix3d::Zero( );
    torquePartial->wrtRotationalVelocityOfAcceleratedBody( partialWrtPhobosRotationalVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 );
    Eigen::MatrixXd partialWrtMarsOrientation = Eigen::MatrixXd::Zero( 3, 4  );
    torquePartial->wrtOrientationOfAcceleratingBody( partialWrtMarsOrientation.block( 0, 0, 3, 4 ) );
    Eigen::MatrixXd partialWrtMarsRotationalVelocity = Eigen::Matrix3d::Zero( );
    torquePartial->wrtRotationalVelocityOfAcceleratingBody( partialWrtMarsRotationalVelocity.block( 0, 0, 3, 3 ), 1, 0, 0 );

    Eigen::MatrixXd partialWrtPhobosState = Eigen::MatrixXd::Zero( 3, 6 );
    torquePartial->wrtNonRotationalStateOfAdditionalBody(
                partialWrtPhobosState.block( 0, 0, 3, 6 ), std::make_pair( "Phobos", "" ), propagators::translational_state );
    Eigen::MatrixXd partialWrtMarsState = Eigen::MatrixXd::Zero( 3, 6 );
    torquePartial->wrtNonRotationalStateOfAdditionalBody(
                partialWrtMarsState.block( 0, 0, 3, 6 ), std::make_pair( "Mars", "" ), propagators::translational_state );

    Eigen::Vector3d partialWrtPhobosGravitationalParameter = torquePartial->wrtParameter(
                phobosGravitationalParameterParameter );
    Eigen::Vector3d partialWrtMeanMomentOfInertia = torquePartial->wrtParameter(
                meanMomentOfInertiaParameter );

    Eigen::MatrixXd partialWrtPhobosCosineCoefficients = torquePartial->wrtParameter(
                phobosCosineCoefficientsParameter );
    Eigen::MatrixXd partialWrtPhobosSineCoefficients = torquePartial->wrtParameter(
                phobosSineCoefficientsParameter );

    // Declare numerical partials.
    Eigen::Matrix3d testPartialWrtPhobosRotationalVelocity = Eigen::Matrix3d::Zero( );
    Eigen::Matrix< double, 3, 4 > testPartialWrtMarsOrientation = Eigen::Matrix< double, 3, 4 >::Zero( );
    Eigen::Matrix3d testPartialWrtMarsRotationalVelocity = Eigen::Matrix3d::Zero( );

    Eigen::Matrix3d testPartialWrtMarsPosition = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtMarsVelocity = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtPhobosPosition = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d testPartialWrtPhobosVelocity = Eigen::Matrix3d::Zero( );

    // Declare perturbations in orientation for numerical partial/
    Eigen::Vector4d orientationPerturbation;
    orientationPerturbation << 1.0E-9, 1.0E-9, 1.0E-9, 1.0E-9;
    Eigen::Vector3d rotationalVelocityPerturbation;
    rotationalVelocityPerturbation <<  1.0E-6, 1.0E-6, 1.0E-6;

    // Create state access/modification functions for bodies.
    std::function< void( Eigen::Vector7d ) > phobosRotationalStateSetFunction =
            std::bind( &Body::setCurrentRotationalStateToLocalFrame, phobos, std::placeholders::_1 );
    std::function< void( Eigen::Vector7d ) > marsRotationalStateSetFunction =
            std::bind( &Body::setCurrentRotationalStateToLocalFrame, mars, std::placeholders::_1 );

    //    // Calculate numerical partials.
    std::vector< Eigen::Vector4d > appliedQuaternionPerturbation;
    Eigen::MatrixXd torqueDeviations = calculateTorqueDeviationDueToOrientationChange(
                phobosRotationalStateSetFunction, effectiveTorqueModel, phobos->getRotationalStateVector( ), orientationPerturbation,
                appliedQuaternionPerturbation );

    testPartialWrtPhobosRotationalVelocity =  calculateTorqueWrtRotationalStatePartials(
                phobosRotationalStateSetFunction, effectiveTorqueModel, phobos->getRotationalStateVector( ), rotationalVelocityPerturbation, 4, 3 );
    testPartialWrtMarsOrientation =  calculateTorqueWrtRotationalStatePartials(
                marsRotationalStateSetFunction, effectiveTorqueModel, mars->getRotationalStateVector( ), orientationPerturbation, 0, 4 );
    testPartialWrtMarsRotationalVelocity =  calculateTorqueWrtRotationalStatePartials(
                marsRotationalStateSetFunction, effectiveTorqueModel, mars->getRotationalStateVector( ), rotationalVelocityPerturbation, 4, 3 );

    std::function< void( Eigen::Vector6d ) > phobosStateSetFunction =
            std::bind( &Body::setState,  phobos, std::placeholders::_1 );
    std::function< void( Eigen::Vector6d ) > marsStateSetFunction =
            std::bind( &Body::setState, mars, std::placeholders::_1 );

    Eigen::Vector3d positionPerturbation;
    positionPerturbation << 1.0, 1.0, 100.0;
    Eigen::Vector3d velocityPerturbation;
    velocityPerturbation << 1.0E-3, 1.0E-3, 1.0E-3;

    // Calculate numerical partials.
    testPartialWrtMarsPosition = calculateTorqueWrtTranslationalStatePartials(
                marsStateSetFunction, effectiveTorqueModel, mars->getState( ), positionPerturbation, 0 );
    testPartialWrtMarsVelocity = calculateTorqueWrtTranslationalStatePartials(
                marsStateSetFunction, effectiveTorqueModel, mars->getState( ), velocityPerturbation, 3 );
    testPartialWrtPhobosPosition = calculateTorqueWrtTranslationalStatePartials(
                phobosStateSetFunction, effectiveTorqueModel, phobos->getState( ), positionPerturbation, 0 );
    testPartialWrtPhobosVelocity = calculateTorqueWrtTranslationalStatePartials(
                phobosStateSetFunction, effectiveTorqueModel, phobos->getState( ), velocityPerturbation, 3 );

    std::function< void( ) > updateFunction = &emptyFunction;
           // std::bind( &Body::setBodyInertiaTensorFromGravityFieldAndExistingMeanMoment, bodyMap.at( "Phobos" ), true );
    Eigen::Vector3d testPartialWrtPhobosGravitationalParameter = calculateTorqueWrtParameterPartials(
                phobosGravitationalParameterParameter, effectiveTorqueModel, 1.0E2, updateFunction );
    Eigen::MatrixXd testPartialWrtMeanMomentOfInertia = calculateTorqueWrtParameterPartials(
                meanMomentOfInertiaParameter, effectiveTorqueModel, 1.0E-4, updateFunction );
    Eigen::MatrixXd testPartialWrtPhobosCosineCoefficients = calculateTorqueWrtParameterPartials(
                phobosCosineCoefficientsParameter, effectiveTorqueModel,
                Eigen::VectorXd::Constant( phobosCosineCoefficientsParameter->getParameterSize( ), 1.0E-6 ), updateFunction );
    Eigen::MatrixXd testPartialWrtPhobosSineCoefficients = calculateTorqueWrtParameterPartials(
                phobosSineCoefficientsParameter, effectiveTorqueModel,
                Eigen::VectorXd::Constant( phobosSineCoefficientsParameter->getParameterSize( ), 1.0E-6 ), updateFunction );

    //    // Compare numerical and analytical results.
    for( int index = 1; index < 4; index++ )
    {
        Eigen::Vector3d numericalChangeInTorque = torqueDeviations.block( 0, index - 1, 3, 1 );
        Eigen::Vector3d analyticalChangeInTorque =
                partialWrtPhobosOrientation.block( 0, 0, 3, 1 ) * appliedQuaternionPerturbation[ index ]( 0 ) +
                partialWrtPhobosOrientation.block( 0, index, 3, 1 ) * appliedQuaternionPerturbation[ index ]( index );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( numericalChangeInTorque,
                                           analyticalChangeInTorque, std::numeric_limits< double >::epsilon( ) );
    }

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtPhobosRotationalVelocity,
                                       partialWrtPhobosRotationalVelocity, 1.0E-10 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsOrientation,
                                       partialWrtMarsOrientation, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testPartialWrtMarsRotationalVelocity,
                                       partialWrtMarsRotationalVelocity, std::numeric_limits< double >::epsilon( ) );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtPhobosState.block( 0, 0, 3, 3 ),
                                        testPartialWrtPhobosPosition, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtMarsState.block( 0, 0, 3, 3 ),
                                        testPartialWrtMarsPosition, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtPhobosState.block( 0, 3, 3, 3 ),
                                        testPartialWrtPhobosVelocity, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(  partialWrtMarsState.block( 0, 3, 3, 3 ),
                                        testPartialWrtMarsVelocity, std::numeric_limits< double >::epsilon( ) );

    Eigen::Vector3d sclaedPartialWrtPhobosGravitationalParameter = phobosInertiaTensor * testPartialWrtPhobosGravitationalParameter;
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( sclaedPartialWrtPhobosGravitationalParameter,
                                       partialWrtPhobosGravitationalParameter, 1.0E-6 );

    Eigen::Vector3d scaledPartialWrtMeanMomentOfInertia = phobosInertiaTensor * testPartialWrtMeanMomentOfInertia;
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( scaledPartialWrtMeanMomentOfInertia,
                                       partialWrtMeanMomentOfInertia, 1.0E-6 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtPhobosCosineCoefficients.block( 0, 0, 3, 2 ),
                                       Eigen::MatrixXd::Zero( 3, 2 ), std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtPhobosCosineCoefficients.block( 0, 5, 3, 4 ),
                                       Eigen::MatrixXd::Zero( 3, 4 ), std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtPhobosSineCoefficients.block( 0, 0, 3, 1 ),
                                       Eigen::MatrixXd::Zero( 3, 1 ), std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtPhobosSineCoefficients.block( 0, 3, 3, 3 ),
                                       Eigen::MatrixXd::Zero( 3, 3 ), std::numeric_limits< double >::epsilon( ) );

    // Check derivative of z-component w.r.t. C20 separately: value is slightly non-zero due to rounding error
    Eigen::MatrixXd scaledPartialWrtPhobosCosineCoefficients = phobosInertiaTensor * testPartialWrtPhobosCosineCoefficients;
    BOOST_CHECK_SMALL( std::fabs( scaledPartialWrtPhobosCosineCoefficients( 2, 4 ) ), 1.0E5 );
    scaledPartialWrtPhobosCosineCoefficients( 2, 4 ) = 0.0;
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( scaledPartialWrtPhobosCosineCoefficients, partialWrtPhobosCosineCoefficients, 1.0E-9 );

    Eigen::MatrixXd scaledPartialWrtPhobosSineCoefficients = phobosInertiaTensor * testPartialWrtPhobosSineCoefficients;
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( scaledPartialWrtPhobosSineCoefficients, partialWrtPhobosSineCoefficients, 1.0E-9 );

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-operator
    This function object computes the sum of two parameters of the same type.

    Infix notation can be used with operator '+',

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

    @par Decorators

     - `saturated_` (See @ref group-decorator) computes the saturated sum.

    @see minus, divides, multiplies

    @par Example:

      @snippet plus.cpp plus

    @par Possible output:

      @snippet plus.txt plus

  **/
  Value plus(Value const& x, Value const& y);
} }
#endif

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

#endif


// Copyright 2019 ETH Zürich, Thomas Schöps
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/Geometry>
#include <gtest/gtest.h>
#include <libvis/eigen.h>
#include <libvis/image_display.h>
#include <libvis/libvis.h>
#include <libvis/logging.h>

#include "badslam/direct_ba.h"
#include "badslam/kernels.h"
#include "badslam/util.cuh"
#include "badslam/render_window.h"
#include "badslam/cuda_depth_processing.cuh"

using namespace vis;

static void ComputeError(
    int width,
    int height,
    cudaStream_t stream,
    DirectBA& direct_ba,
    const SE3f& global_tr_frame_1,
    float expected_z) {
  vector<float> surfel_x(direct_ba.surfel_count());
  vector<float> surfel_y(direct_ba.surfel_count());
  vector<float> surfel_z(direct_ba.surfel_count());

  direct_ba.surfels()->DownloadPartAsync(kSurfelX * direct_ba.surfels()->ToCUDA().pitch(), direct_ba.surfel_count() * sizeof(float), stream, surfel_x.data());
  direct_ba.surfels()->DownloadPartAsync(kSurfelY * direct_ba.surfels()->ToCUDA().pitch(), direct_ba.surfel_count() * sizeof(float), stream, surfel_y.data());
  direct_ba.surfels()->DownloadPartAsync(kSurfelZ * direct_ba.surfels()->ToCUDA().pitch(), direct_ba.surfel_count() * sizeof(float), stream, surfel_z.data());

  cudaStreamSynchronize(stream);

  constexpr bool kShowErrorVisualization = false;
  Image<Vec3u8> error_visualization(width, height);
  if (kShowErrorVisualization) {
    for (int y = 0; y < height; ++ y) {
      for (int x = 0; x < width; ++ x) {
        error_visualization(x, y) = Vec3u8(0, 0, 255);
      }
    }
  }

  SE3f frame_tr_global = global_tr_frame_1.inverse();
  Mat3f frame_r_global = frame_tr_global.rotationMatrix();
  const Vec3f& frame_t_global = frame_tr_global.translation();

  int num_fails = 0;
  int num_correct = 0;
  for (usize i = 0; i < direct_ba.surfel_count(); ++ i) {
    Vec3f cam_space_surfel = frame_r_global * Vec3f(surfel_x[i], surfel_y[i], surfel_z[i]) + frame_t_global;
    Vec2f pxy;
    if (direct_ba.depth_camera().ProjectToPixelCornerConvIfVisible(cam_space_surfel, 0, &pxy)) {
      float actual_z = cam_space_surfel.z();
      float error = fabs(expected_z - actual_z);

      constexpr float kEpsilon = 1e-3f;
      if (error > kEpsilon) {
        ++ num_fails;
        if (kShowErrorVisualization) {
          error_visualization(pxy.x(), pxy.y()) = Vec3u8(255, 0, 0);
        }
      } else {
        ++ num_correct;
        if (kShowErrorVisualization) {
          u8 intensity = std::min<float>(255, (255.99f / kEpsilon) * error);
          error_visualization(pxy.x(), pxy.y()) = Vec3u8::Constant(intensity);
        }
      }
    }
  }

  EXPECT_GE(num_correct, 100000);
  EXPECT_LE(num_fails, 75000);
  if (num_fails > 0) {
    LOG(INFO) << "num_fails: " << num_fails << " (" << ((100.f * num_fails) / direct_ba.surfel_count()) << "% of surfels; surfel_count: " << direct_ba.surfel_count() << ")";
  }

  if (kShowErrorVisualization) {
    ImageDisplay debug_display;
    debug_display.Update(error_visualization, "Surfel depth error visualization");
    std::getchar();
  }
}

// This checks that the surfel depth optimization in bundle adjustment works
// correctly. It sets up a keyframe and a scene with surfels that match its
// depth image. The it perturbs the keyframe's image and tests whether
// optimizing the surfel positions moves them accordingly.
static void TestGeometryOptimizationWithPhotometricResidual(bool use_pcg) {
  srand(0);

  // Initialize camera
  constexpr int width = 640;
  constexpr int height = 480;
  const float camera_parameters[4] = {0.5f * height, 0.5f * height, 0.5f * width - 0.5f, 0.5f * height - 0.5f};
  PinholeCamera4f camera(width, height, camera_parameters);

  // Set keyframe pose
  Matrix<float, 6, 1> global_tr_frame_log;
  global_tr_frame_log << 0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f;
  SE3f global_tr_frame_0 = SE3f::exp(global_tr_frame_log);

  // Initialize CUDA stream
  cudaStream_t stream;
  cudaStreamCreate(&stream);

  // Initialize DirectBA
  constexpr float raw_to_float_depth = 1.f / 5000;
  DirectBA direct_ba(
      /*max_surfel_count*/ 1000 * 1000,
      raw_to_float_depth,
      /*baseline_fx*/ 40,
      /*sparse_surfel_cell_size*/ 1.f,
      /*surfel_merge_dist_factor*/ 0.8f,
      /*min_observation_count_while_bootstrapping_1*/ 1,
      /*min_observation_count_while_bootstrapping_2*/ 1,
      /*min_observation_count*/ 1,
      /*color_camera_initial_estimate*/ camera,
      /*depth_camera_initial_estimate*/ camera,
      /*pyramid_level_for_color*/ 0,
      /*use_depth_residuals*/ false,
      /*use_descriptor_residuals*/ true,
      nullptr,
      /*global_T_anchor_frame*/ SE3f());

  constexpr int kFrameOffsetPx = 100;
  constexpr float kDepth = 2.f;

  // Create a known depth map
  Image<u16> depth_image(width, height);
  for (int y = 0; y < height; ++ y) {
    for (int x = 0; x < width; ++ x) {
      if (x < kFrameOffsetPx || y == 0 || x == width - 1 || y == height - 1) {
        depth_image(x, y) = numeric_limits<u16>::max();
      } else {
        depth_image(x, y) = (kDepth / raw_to_float_depth) + 0.5f;
      }
    }
  }

  Image<Vec3u8> color_image(width, height);
  for (int y = 0; y < height; ++ y) {
    for (int x = 0; x < width; ++ x) {
      u8 i = (255 / 2.f) * (1.f + sin(0.15f * x + 0.5f * sin(0.25f * y))) + rand() % 1;
      color_image(x, y) = Vec3u8(i, i, i);
    }
  }

//   ImageDisplay debug_display;
//   debug_display.Update(color_image, "debug: color");
//   std::getchar();

//   ImageDisplay debug_display;
//   debug_display.Update(inv_depth_image, "debug: inv depth map", 0.f, 0.5f);
//   std::getchar();

  RGBDVideo<Vec3u8, u16> rgbd_video;
  shared_ptr<Keyframe> keyframe_0(new Keyframe(
      stream,
      /*frame_index*/ 0,
      direct_ba.depth_params(),
      direct_ba.depth_camera(),
      depth_image,
      color_image,
      global_tr_frame_0));

  // Force normals to fronto-parallel.
  Image<u16> normals_image(width, height);
  for (int y = 0; y < height; ++ y) {
    for (int x = 0; x < width; ++ x) {
      normals_image(x, y) = ImageSpaceNormalToU16(0, 0);
    }
  }
  CUDABuffer<u16>* kf_normals_buffer = const_cast<CUDABuffer<u16>*>(&keyframe_0->normals_buffer());
  kf_normals_buffer->UploadAsync(/*stream*/ 0, normals_image);

  direct_ba.AddKeyframe(keyframe_0);

  // Reproject the first keyframe into a second one
  for (int y = 0; y < height; ++ y) {
    for (int x = 0; x < width; ++ x) {
      if (x == 0 || y == 0 || x >= width - kFrameOffsetPx || y == height - 1) {
        depth_image(x, y) = numeric_limits<u16>::max();
      } else {
        // NOTE: adding depth noise
        depth_image(x, y) = depth_image(x + kFrameOffsetPx, y) + 0.0001f * (rand() % 100) / raw_to_float_depth;
        color_image(x, y) = color_image(x + kFrameOffsetPx, y);
      }
    }
  }

  // kFrameOffsetPx * fx_inv / 1 == offset_x / kDepth
  float offset_x = kDepth * kFrameOffsetPx / camera.parameters()[0];
  SE3f frame_0_T_frame_1;
  frame_0_T_frame_1.translation().x() = offset_x;
  SE3f global_tr_frame_1 = global_tr_frame_0 * frame_0_T_frame_1;

  shared_ptr<Keyframe> keyframe_1(new Keyframe(
      stream,
      /*frame_index*/ 1,
      direct_ba.depth_params(),
      direct_ba.depth_camera(),
      depth_image,
      color_image,
      global_tr_frame_1));

  // Force normals to fronto-parallel.
  kf_normals_buffer = const_cast<CUDABuffer<u16>*>(&keyframe_1->normals_buffer());
  kf_normals_buffer->UploadAsync(/*stream*/ 0, normals_image);

  direct_ba.AddKeyframe(keyframe_1);

  // Create surfels from keyframe 1 (having noisy depth values)
  direct_ba.CreateSurfelsForKeyframe(stream, /*filter_new_surfels*/ false, keyframe_1);

//   // Remove depth noise in the keyframe depth map again
//   for (int y = 0; y < height; ++ y) {
//     for (int x = 0; x < width; ++ x) {
//       if (x == 0 || y == 0 || x >= width - kFrameOffsetPx || y == height - 1) {
//         depth_image(x, y) = numeric_limits<u16>::max();
//       } else {
//         depth_image(x, y) = (kDepth / raw_to_float_depth) + 0.5f;
//       }
//     }
//   }
//   const_cast<CUDABuffer<u16>&>(keyframe_1->depth_buffer()).UploadAsync(0, depth_image);

  // Optimize surfel positions
  direct_ba.BundleAdjustment(
      stream,
      /*optimize_depth_intrinsics*/ false,
      /*optimize_color_intrinsics*/ false,
      /*do_surfel_updates*/ false,
      /*optimize_poses*/ false,
      /*optimize_geometry*/ true,
      /*min_iterations*/ 60,
      /*max_iterations*/ 60,
      use_pcg,
      /*active_keyframe_window_start*/ 0,
      /*active_keyframe_window_end*/ direct_ba.keyframes().size() - 1,
      /*increase_ba_iteration_count*/ true);

  ComputeError(width, height, stream, direct_ba, global_tr_frame_1, kDepth);

  cudaStreamDestroy(stream);
}

TEST(Optimization, AlternatingGeometryOptimizationWithPhotometricResidual) {
  TestGeometryOptimizationWithPhotometricResidual(/*use_pcg*/ false);
}

TEST(Optimization, PCGGeometryOptimizationWithPhotometricResidual) {
  TestGeometryOptimizationWithPhotometricResidual(/*use_pcg*/ true);
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testSymbolicFactorGraphB.cpp
 * @brief   Unit tests for a symbolic Factor Graph
 * @author  Frank Dellaert
 */

#include <tests/smallExample.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;

using namespace std;
using namespace gtsam;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
TEST( SymbolicFactorGraph, symbolicFactorGraph )
{
  Ordering o; o += X(1),L(1),X(2);
  // construct expected symbolic graph
  SymbolicFactorGraph expected;
  expected.push_factor(o[X(1)]);
  expected.push_factor(o[X(1)],o[X(2)]);
  expected.push_factor(o[X(1)],o[L(1)]);
  expected.push_factor(o[X(2)],o[L(1)]);

  // construct it from the factor graph
  GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o);
  SymbolicFactorGraph actual(factorGraph);

  CHECK(assert_equal(expected, actual));
}

///* ************************************************************************* */
//TEST( SymbolicFactorGraph, findAndRemoveFactors )
//{
//  // construct it from the factor graph graph
//  GaussianFactorGraph factorGraph = example::createGaussianFactorGraph();
//  SymbolicFactorGraph actual(factorGraph);
//  SymbolicFactor::shared_ptr f1 = actual[0];
//  SymbolicFactor::shared_ptr f3 = actual[2];
//  actual.findAndRemoveFactors(X(2));
//
//  // construct expected graph after find_factors_and_remove
//  SymbolicFactorGraph expected;
//  SymbolicFactor::shared_ptr null;
//  expected.push_back(f1);
//  expected.push_back(null);
//  expected.push_back(f3);
//  expected.push_back(null);
//
//  CHECK(assert_equal(expected, actual));
//}
///* ************************************************************************* */
//TEST( SymbolicFactorGraph, factors)
//{
//  // create a test graph
//  GaussianFactorGraph factorGraph = example::createGaussianFactorGraph();
//  SymbolicFactorGraph fg(factorGraph);
//
//  // ask for all factor indices connected to x1
//  list<size_t> x1_factors = fg.factors(X(1));
//  int x1_indices[] = { 0, 1, 2 };
//  list<size_t> x1_expected(x1_indices, x1_indices + 3);
//  CHECK(x1_factors==x1_expected);
//
//  // ask for all factor indices connected to x2
//  list<size_t> x2_factors = fg.factors(X(2));
//  int x2_indices[] = { 1, 3 };
//  list<size_t> x2_expected(x2_indices, x2_indices + 2);
//  CHECK(x2_factors==x2_expected);
//}

///* ************************************************************************* */
//TEST( SymbolicFactorGraph, removeAndCombineFactors )
//{
//  // create a test graph
//  GaussianFactorGraph factorGraph = example::createGaussianFactorGraph();
//  SymbolicFactorGraph fg(factorGraph);
//
//  // combine all factors connected to x1
//  SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1));
//
//  // check result
//  SymbolicFactor expected(L(1),X(1),X(2));
//  CHECK(assert_equal(expected,*actual));
//}

///* ************************************************************************* */
//TEST( SymbolicFactorGraph, eliminateOne )
//{
//  Ordering o; o += X(1),L(1),X(2);
//  // create a test graph
//  GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o);
//  SymbolicFactorGraph fg(factorGraph);
//
//  // eliminate
//  IndexConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o[X(1)]+1);
//
//  // create expected symbolic IndexConditional
//  IndexConditional expected(o[X(1)],o[L(1)],o[X(2)]);
//
//  CHECK(assert_equal(expected,*actual));
//}

/* ************************************************************************* */
TEST( SymbolicFactorGraph, eliminate )
{
  Ordering o; o += X(2),L(1),X(1);

  // create expected Chordal bayes Net
  IndexConditional::shared_ptr x2(new IndexConditional(o[X(2)], o[L(1)], o[X(1)]));
  IndexConditional::shared_ptr l1(new IndexConditional(o[L(1)], o[X(1)]));
  IndexConditional::shared_ptr x1(new IndexConditional(o[X(1)]));

  SymbolicBayesNet expected;
  expected.push_back(x2);
  expected.push_back(l1);
  expected.push_back(x1);

  // create a test graph
  GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o);
  SymbolicFactorGraph fg(factorGraph);

  // eliminate it
  SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();

  CHECK(assert_equal(expected,actual));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#include <json11.hpp>
#include <src/routesfetcher.h>
#include <src/utils.hpp>
#include <aws/lambda-runtime/runtime.h>
#include <boost/graph/adjacency_list.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/graph/adj_list_serialize.hpp>
#include <chrono>
#include <iostream>
#include <string>
#include <vector>

using namespace arlib;
using namespace aws::lambda_runtime;
using namespace json11;
using namespace std;

using Graph = boost::adjacency_list<boost::vecS, boost::vecS,
        boost::bidirectionalS, Location,
        boost::property<boost::edge_weight_t, float>>;
using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
using Edge = typename boost::graph_traits<Graph>::edge_descriptor;

Graph g;
 /*
  * https://docs.aws.amazon.com/apigateway/latest/developerguide/set-up-lambda-proxy-integrations.html#api-gateway-simple-proxy-for-lambda-output-format
  * API Gateway Input format
  * {
     "resource": "Resource path",
     "path": "Path parameter",
     "httpMethod": "Incoming request's method name"
     "headers": {String containing incoming request headers}
     "multiValueHeaders": {List of strings containing incoming request headers}
     "queryStringParameters": {query string parameters }
     "multiValueQueryStringParameters": {List of query string parameters}
     "pathParameters":  {path parameters}
     "stageVariables": {Applicable stage variables}
     "requestContext": {Request context, including authorizer-returned key-value pairs}
     "body": "A JSON string of the request payload."
     "isBase64Encoded": "A boolean flag to indicate if the applicable request payload is Base64-encode"

  * }
  * Only "queryStringParameters" will be used here
  *
  * API Gateway Output format
  * {
    "isBase64Encoded": true|false,
    "statusCode": httpStatusCode,
    "headers": { "headerName": "headerValue", ... },
    "body": "..."
  * }
  */
invocation_response my_handler(invocation_request const& request) {
    auto start_read_files = chrono::steady_clock::now();
    if (boost::num_vertices(g) == 0) {
        std::ifstream in("data.btl");
        {
            boost::archive::text_iarchive ia(in);
            ia >> g;
        }
    }
    auto end_read_files = chrono::steady_clock::now();
    logElapsedMillis("Loaded coordinates and weights", start_read_files, end_read_files);
    string err;
    auto js_obj = json11::Json::parse(request.payload, err);
    auto parameters = js_obj["queryStringParameters"];
    float s_lat, s_lon, e_lat, e_lon;
    bool reroute;
    int num_routes;
    try {
        s_lat = stof(parameters["s_lat"].string_value());
        s_lon = stof(parameters["s_lon"].string_value());
        e_lat = stof(parameters["e_lat"].string_value());
        e_lon = stof(parameters["e_lon"].string_value());
    }
    catch (invalid_argument &) {
        return invocation_response::failure("Start and end coordinates must be provided", "InvalidRequest");
    }
    reroute = parseBoolean(parameters["reroute"].string_value());
    try {
        auto num_routes_str = parameters["n_routes"].string_value();
        num_routes = stoi(num_routes_str);
    }
    catch (invalid_argument &) {
        num_routes = 1;
    }
    Vertex start;
    auto start_get_vertices = chrono::steady_clock::now();
    get_vertex(s_lat, s_lon, g, start);
    Vertex end;
    get_vertex(e_lat, e_lon, g, end);
    auto end_get_vertices = chrono::steady_clock::now();
    logElapsedMillis("Found vertices", start_get_vertices, end_get_vertices);
    auto paths = get_alternative_routes(g, start, end, num_routes, 0.9, reroute);
    auto headers = Json::object {{"Access-Control-Allow-Origin", "*"}};
    Json response = Json::object{
        {"isBase64Encoded", false},
        {"statusCode", 200},
        {"headers", headers},
        {"body", paths.dump()}
    };
    cout << response.dump() << std::endl;
    return invocation_response::success(response.dump(), "application/json");
}

int main() {
    run_handler(my_handler);
    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_FREXP_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_FREXP_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/frexp.hpp>
#include <boost/simd/include/functions/scalar/is_invalid.hpp>
#include <boost/simd/include/functions/scalar/bitwise_notand.hpp>
#include <boost/simd/include/functions/scalar/bitwise_and.hpp>
#include <boost/simd/include/functions/scalar/shri.hpp>
#include <boost/simd/include/functions/scalar/minus.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/dispatch/meta/adapted_traits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/at.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::frexp_, tag::cpu_, (A0)(A2)
                            , (scalar_< double_<A0> >)
                              (scalar_< double_<A0> >)
                              (scalar_< int64_<A2> >)
    )
  {
    typedef int result_type;
    inline int operator()(A0 const& a0,A0 & r0,A2 & r1) const
    {
      typedef typename dispatch::meta::as_integer<A0, signed>::type      int_type;
      if (a0 == 0 || is_invalid(a0))
      {
        r0 = a0;
        r1 = Zero<A2>();
        return 0;
      }
      else
      {
        const int_type me = Maxexponent<A0>()-1;
        const int_type nmb= Nbmantissabits<A0>();
        const int_type n1 = ((2*me+3)<<nmb);
        const int_type n2 = me<<nmb;
        A0 ci_exp=simd::bitwise_cast<A0>(n1);
        r1 = simd::bitwise_cast<int_type>(b_and(ci_exp, a0));// extract exponent
        A0 x = b_notand(ci_exp, a0);                         // clear exponent in a0
        r1 = sub(shri(r1,nmb), me);                          // compute exponent
        r0 = b_or(x,n2);                                     // insert exponent+1 in x
        if (r1 > me) {r1 = 0; r0+= a0;}
        return 0;
      }
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::frexp_, tag::cpu_, (A0)(A2)
                                     , (scalar_< floating_<A0> >)
                                     (scalar_< integer_<A2> >)
    )
  {
    typedef A0 result_type;
    inline A0 operator()(A0 const& a0,A2 & a2) const
    {
      A0 a1;
      frexp(a0, a1, a2);
      return a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::frexp_, tag::cpu_, (A0)
                                     , (scalar_< floating_<A0> >)
    )
  {
    typedef A0                                                      mantissa;
    typedef typename dispatch::meta::as_integer<A0,signed>::type    exponent;
    typedef boost::fusion::vector<mantissa,exponent>             result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type res;
      frexp(a0, boost::fusion::at_c<0>(res), boost::fusion::at_c<1>(res));
      return res;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::frexp_, tag::cpu_, (A0)(A2),
                                     (scalar_< single_<A0> >)
                                     (scalar_< single_<A0> >)
                                     (scalar_< int32_<A2> >)
    )
  {
    typedef int result_type;
    inline int operator()(A0 const& a0,A0 & r0,A2 & r1) const
    {
      typedef typename dispatch::meta::as_integer<A0, signed>::type      int_type;
      if (a0 == 0 || is_invalid(a0))
      {
        r0 = a0;
        r1 = Zero<A2>();
        return 0;
      }
      else
      {
        const int_type me = Maxexponent<A0>()-1;
        const int_type nmb= Nbmantissabits<A0>();
        const int_type n1 = ((2*me+3)<<nmb);
        const int_type n2 = me<<nmb;
        A0 ci_exp=simd::bitwise_cast<A0>(n1);
        r1 = simd::bitwise_cast<int_type>(b_and(ci_exp, a0));// extract exponent
        A0 x = b_notand(ci_exp, a0);                         // clear exponent in a0
        r1 = sub(shri(r1,nmb), me);                          // compute exponent
        r0 = b_or(x,n2);                                     // insert exponent+1 in x
        if (r1 > me) {r1 = 0; r0+= a0;}
        return 0;
      }
    }
  };
} } }

#endif


// SPDX-License-Identifier: Apache-2.0
/*
Copyright 2019 Blue Cheetah Analog Design 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 <unordered_map>
#include <utility>

#include <boost/geometry.hpp>

#include <cbag/common/typedefs.h>
#include <cbag/polygon/boost_adapt.h>

#include <pybind11_generics/iterator.h>

#include <pybag/rtree.h>

namespace pybag {
namespace util {

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

class rtree {
  public:
    using coordinate_type = cbag::coord_t;
    using point_type = bg::model::point<coordinate_type, 2, bg::cs::cartesian>;
    using box_type = cbag::polygon::rectangle_data<coordinate_type>;
    using tree_value_type = std::pair<box_type, std::size_t>;
    using tree_type = bgi::rtree<tree_value_type, bgi::quadratic<32, 16>>;
    using const_iterator = typename tree_type::const_iterator;
    using const_query_iterator = typename tree_type::const_query_iterator;

  private:
    tree_type index_;
    std::unordered_map<std::size_t, std::pair<box_type, py::object>> val_map_;
    std::size_t cnt_ = 0;

  public:
    rtree() = default;

    bool empty() const noexcept { return val_map_.empty(); }

    box_type get_bbox() const {
        auto tmp = index_.bounds();
        return {tmp.min_corner().template get<0>(), tmp.min_corner().template get<1>(),
                tmp.max_corner().template get<0>(), tmp.max_corner().template get<1>()};
    }

    py::object get_value(std::size_t id) const {
        auto iter = val_map_.find(id);
        if (iter == val_map_.end()) {
            throw std::out_of_range("Cannot find id in rtree.");
        }
        return iter->second.second;
    }

    const_iterator begin() const { return index_.begin(); }
    const_iterator end() const { return index_.end(); }

    const_query_iterator intersect(const box_type &box) const {
        return index_.qbegin(bgi::intersects(box));
    }
    const_query_iterator overlap(const box_type &box) const {
        return index_.qbegin(bgi::overlaps(box));
    }
    const_query_iterator qend() const { return index_.qend(); }

    std::size_t insert(py::object &&obj, box_type &&box) {
        auto ans = cnt_;
        index_.insert(tree_value_type(box, ans));
        val_map_.emplace(ans, std::make_pair(std::move(box), std::move(obj)));
        ++cnt_;
        return ans;
    }

    py::object pop(std::size_t id) {
        auto iter = val_map_.find(id);
        if (iter == val_map_.end()) {
            throw std::out_of_range("Cannot find id in rtree.");
        }
        auto ans = iter->second.second;
        auto val = tree_value_type(iter->second.first, id);
        index_.remove(val);
        val_map_.erase(iter);
        return ans;
    }
};

} // namespace util
} // namespace pybag

namespace pu = pybag::util;
namespace pyg = pybind11_generics;

void bind_rtree(py::module &m) {
    pyg::declare_iterator<pu::rtree::const_iterator>();
    pyg::declare_iterator<pu::rtree::const_query_iterator>();

    auto py_cls = py::class_<pu::rtree>(m, "RTree");
    py_cls.def(py::init<>(), "Create an empty RTree.");
    py_cls.def("__bool__", [](const pu::rtree &self) { return !self.empty(); },
               "True if this object is not empty.");
    py_cls.def("__iter__",
               [](const pu::rtree &self) { return pyg::make_iterator(self.begin(), self.end()); },
               "Iterates over all items in this Rtree.");
    py_cls.def("__getitem__",
               [](const pu::rtree &self, std::size_t id) { return self.get_value(id); },
               "Returns the value corresponding to the given ID.", py::arg("obj_id"));
    py_cls.def("pop", [](pu::rtree &self, std::size_t id) { return self.pop(id); },
               "Removes the given ID from this RTree and returns the associated value.",
               py::arg("obj_id"));
    py_cls.def_property_readonly("bound_box", &pu::rtree::get_bbox, "The overall bounding box.");
    py_cls.def(
        "intersect_iter",
        [](const pu::rtree &self, const pu::rtree::box_type &box) {
            return pyg::make_iterator(self.intersect(box), self.qend());
        },
        "Returns an iterator over all objects that intersects the given box (includes touches).",
        py::arg("box"));
    py_cls.def(
        "overlap_iter",
        [](const pu::rtree &self, const pu::rtree::box_type &box) {
            return pyg::make_iterator(self.overlap(box), self.qend());
        },
        "Returns an iterator over all objects that overlaps the given box (excludes touches).",
        py::arg("box"));
    py_cls.def("insert",
               [](pu::rtree &self, py::object obj, pu::rtree::box_type box) {
                   return self.insert(std::move(obj), std::move(box));
               },
               "Insert given object into RTree.", py::arg("obj"), py::arg("box"));
}


// All content Copyright (C) 2018 Genomics plc
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>

#include "utils/NeedlemanWunsch.hpp"

using wecall::utils::NWPenalties;

BOOST_AUTO_TEST_CASE( shouldMakeSameObjectWithBothInitialisers )
{
    const int32_t baseMatch = 1;
    const int32_t baseMismatch = 2;
    const int32_t indelConstant = 3;
    const int32_t indelLinear = 4;
    const int32_t indelPosition = 5;
    const auto nWPenalties1 =
        NWPenalties( baseMatch, baseMismatch, indelConstant, indelLinear, indelConstant, indelLinear, indelPosition );
    const auto nWPenalties2 = NWPenalties( baseMatch, baseMismatch, indelConstant, indelLinear, indelPosition );

    BOOST_CHECK_EQUAL( nWPenalties1, nWPenalties2 );
}

BOOST_AUTO_TEST_CASE( shouldCalculateMatchingBasesPenalties )
{
    const int32_t baseMatch = 1;
    const int32_t baseMismatch = 10;
    const int32_t insertionConstant = 10;
    const int32_t insertionLinear = 10;
    const int32_t deletionConstant = 10;
    const int32_t deletionLinear = 10;
    const int32_t indelPosition = 10;
    const auto nWPenalties = NWPenalties( baseMatch, baseMismatch, insertionConstant, insertionLinear, deletionConstant,
                                          deletionLinear, indelPosition );

    const char bases[4] = {'A', 'C', 'G', 'T'};

    for ( auto index = 0; index < 4; ++index )
    {
        BOOST_CHECK_EQUAL( baseMatch, nWPenalties.matchFunction( bases[index], bases[index] ) );
    }
}

BOOST_AUTO_TEST_CASE( shouldCalculateMismatchingBasesPenalties )
{
    const int32_t baseMatch = 10;
    const int32_t baseMismatch = 1;
    const int32_t insertionConstant = 10;
    const int32_t insertionLinear = 10;
    const int32_t deletionConstant = 10;
    const int32_t deletionLinear = 10;
    const int32_t indelPosition = 10;
    const auto nWPenalties = NWPenalties( baseMatch, baseMismatch, insertionConstant, insertionLinear, deletionConstant,
                                          deletionLinear, indelPosition );

    const char bases[4] = {'A', 'C', 'G', 'T'};

    for ( auto index1 = 0; index1 < 4; ++index1 )
    {
        for ( auto index2 = 0; index2 < 4; ++index2 )
        {
            if ( index1 != index2 )
            {
                BOOST_CHECK_EQUAL( baseMismatch, nWPenalties.matchFunction( bases[index1], bases[index2] ) );
            }
        }
    }
}

BOOST_AUTO_TEST_CASE( shouldCalculateInsertionOpenPenalty )
{
    const int32_t baseMatch = 10;
    const int32_t baseMismatch = 10;
    const int32_t insertionConstant = 1;
    const int32_t insertionLinear = 2;
    const int32_t deletionConstant = 10;
    const int32_t deletionLinear = 10;
    const int32_t indelPosition = 10;
    const auto nWPenalties = NWPenalties( baseMatch, baseMismatch, insertionConstant, insertionLinear, deletionConstant,
                                          deletionLinear, indelPosition );

    BOOST_CHECK_EQUAL( insertionConstant + insertionLinear, nWPenalties.insertionOpen() );
}

BOOST_AUTO_TEST_CASE( shouldCalculateDeletionOpenPenalty )
{
    const int32_t baseMatch = 10;
    const int32_t baseMismatch = 10;
    const int32_t insertionConstant = 10;
    const int32_t insertionLinear = 10;
    const int32_t deletionConstant = 1;
    const int32_t deletionLinear = 2;
    const int32_t indelPosition = 10;
    const auto nWPenalties = NWPenalties( baseMatch, baseMismatch, insertionConstant, insertionLinear, deletionConstant,
                                          deletionLinear, indelPosition );

    BOOST_CHECK_EQUAL( deletionConstant + deletionLinear, nWPenalties.deletionOpen() );
}

BOOST_AUTO_TEST_CASE( shouldCalculateInsertionFunction )
{
    const int32_t baseMatch = 10;
    const int32_t baseMismatch = 10;
    const int32_t insertionConstant = 1;
    const int32_t insertionLinear = 2;
    const int32_t deletionConstant = 10;
    const int32_t deletionLinear = 10;
    const int32_t indelPosition = 10;
    const auto nWPenalties = NWPenalties( baseMatch, baseMismatch, insertionConstant, insertionLinear, deletionConstant,
                                          deletionLinear, indelPosition );

    for ( auto position = 0; position < 10; ++position )
    {
        BOOST_CHECK_EQUAL( 0, nWPenalties.insertionFunction( position, 0 ) );

        for ( auto insertionLength = 1; insertionLength < 10; ++insertionLength )
        {
            BOOST_CHECK_EQUAL( insertionConstant + insertionLength * insertionLinear + position * indelPosition,
                               nWPenalties.insertionFunction( position, insertionLength ) );
        }
    }
}

BOOST_AUTO_TEST_CASE( shouldCalculateDeletionFunction )
{
    const int32_t baseMatch = 10;
    const int32_t baseMismatch = 10;
    const int32_t insertionConstant = 10;
    const int32_t insertionLinear = 10;
    const int32_t deletionConstant = 1;
    const int32_t deletionLinear = 2;
    const int32_t indelPosition = 10;
    const auto nWPenalties = NWPenalties( baseMatch, baseMismatch, insertionConstant, insertionLinear, deletionConstant,
                                          deletionLinear, indelPosition );

    for ( auto position = 0; position < 10; ++position )
    {
        BOOST_CHECK_EQUAL( 0, nWPenalties.insertionFunction( position, 0 ) );

        for ( auto deletionLength = 1; deletionLength < 10; ++deletionLength )
        {
            BOOST_CHECK_EQUAL( deletionConstant + deletionLength * deletionLinear + position * indelPosition,
                               nWPenalties.deletionFunction( position, deletionLength ) );
        }
    }
}

BOOST_AUTO_TEST_CASE( shouldCalculateExtendInsertion )
{
    const int32_t baseMatch = 10;
    const int32_t baseMismatch = 10;
    const int32_t insertionConstant = 1;
    const int32_t insertionLinear = 2;
    const int32_t deletionConstant = 10;
    const int32_t deletionLinear = 10;
    const int32_t indelPosition = 10;
    const auto nWPenalties = NWPenalties( baseMatch, baseMismatch, insertionConstant, insertionLinear, deletionConstant,
                                          deletionLinear, indelPosition );

    for ( auto position = 0; position < 10; ++position )
    {
        for ( auto insertionLength = 1; insertionLength < 10; ++insertionLength )
        {
            BOOST_CHECK_EQUAL(
                nWPenalties.insertionFunction( position, insertionLength + 1 ),
                nWPenalties.insertionFunction( position, insertionLength ) + nWPenalties.extendInsertion() );
        }
    }
}

BOOST_AUTO_TEST_CASE( shouldCalculateExtendDeletion )
{
    const int32_t baseMatch = 10;
    const int32_t baseMismatch = 10;
    const int32_t insertionConstant = 10;
    const int32_t insertionLinear = 10;
    const int32_t deletionConstant = 1;
    const int32_t deletionLinear = 2;
    const int32_t indelPosition = 10;
    const auto nWPenalties = NWPenalties( baseMatch, baseMismatch, insertionConstant, insertionLinear, deletionConstant,
                                          deletionLinear, indelPosition );

    for ( auto position = 0; position < 10; ++position )
    {
        for ( auto deletionLength = 1; deletionLength < 10; ++deletionLength )
        {
            BOOST_CHECK_EQUAL(
                nWPenalties.deletionFunction( position, deletionLength + 1 ),
                nWPenalties.deletionFunction( position, deletionLength ) + nWPenalties.extendDeletion() );
        }
    }
}


#include "Algorithms/Sorting/SelectionSort.h"

#include <boost/test/unit_test.hpp>
#include <vector>

using Algorithms::Sorting::SelectionSort::SelectionSortIterative;
using Algorithms::Sorting::SelectionSort::minimum_value_index;
using Algorithms::Sorting::SelectionSort::selection_sort_recursive;

BOOST_AUTO_TEST_SUITE(Algorithms)
BOOST_AUTO_TEST_SUITE(SelectionSort_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SelectionSortIterativeSortsCorrectly)
{
  std::vector<int> a {12, 11, 13, 5, 6};

  SelectionSortIterative::selection_sort(a);

  BOOST_TEST(a[0] == 5);
  BOOST_TEST(a[1] == 6);
  BOOST_TEST(a[2] == 11);
  BOOST_TEST(a[3] == 12);
  BOOST_TEST(a[4] == 13);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SelectionSortRecursivelySortsCorrectly)
{
  std::vector<int> a {12, 11, 13, 5, 6};

  selection_sort_recursive<std::vector<int>, int>(
    a,
    static_cast<std::size_t>(0),
    a.size());

  BOOST_TEST(a[0] == 5);
  BOOST_TEST(a[1] == 6);
  BOOST_TEST(a[2] == 11);
  BOOST_TEST(a[3] == 12);
  BOOST_TEST(a[4] == 13);
}

BOOST_AUTO_TEST_SUITE_END() // SelectionSort_tests
BOOST_AUTO_TEST_SUITE_END() // Algorithms


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

#include <boost/simd/reduction/functions/sum.hpp>
#include <boost/simd/include/constants/one.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( sum_, tag::cpu_
                                    , (A0)
                                    , ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef typename A0::value_type result_type;

    BOOST_FORCEINLINE result_type operator()(__m256d a0) const
    {
      __m256d half_sum  = _mm256_hadd_pd( a0, a0 );
      __m128d sum       = _mm_add_pd( _mm256_extractf128_pd( half_sum, 0 )
                                    , _mm256_extractf128_pd( half_sum, 1 )
                                    );
      return _mm_cvtsd_f64(sum);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( sum_, tag::cpu_
                                    , (A0)
                                    , ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;
    BOOST_FORCEINLINE result_type operator()(__m256 a0) const
    {
      A0 r = _mm256_dp_ps(a0, One<A0>(), 0xFF);
      return r[0]+r[4];
    }
  };

} } }

#endif
#endif