<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    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 <thread>

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

#include <Eigen/Geometry>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Astrodynamics/Ephemerides/tabulatedEphemeris.h"

#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h"
#include "Tudat/InputOutput/basicInputOutput.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalStateConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/torqueModelTypes.h"
#include "Tudat/External/SpiceInterface/spiceRotationalEphemeris.h"
#include "Tudat/Astrodynamics/Ephemerides/constantEphemeris.h"
#include "Tudat/Astrodynamics/Ephemerides/keplerEphemeris.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/body.h"
#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h"
#include "Tudat/SimulationSetup/PropagationSetup/propagationSettings.h"
#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h"
#include "Tudat/Astrodynamics/Aerodynamics/UnitTests/testApolloCapsuleCoefficients.h"

namespace tudat
{
namespace unit_tests
{

//Using declarations.
using namespace tudat;
using namespace tudat::ephemerides;
using namespace tudat::interpolators;
using namespace tudat::numerical_integrators;
using namespace tudat::spice_interface;
using namespace tudat::simulation_setup;
using namespace tudat::basic_astrodynamics;
using namespace tudat::orbital_element_conversions;
using namespace tudat::propagators;
using namespace tudat::coordinate_conversions;
using namespace tudat::reference_frames;

// Create body map to be used for propagation of rotational motion of Phobos (no coupling to orbit)
NamedBodyMap getTestBodyMap( const double phobosSemiMajorAxis,
                             const bool useSymmetricEquator = 0 )
{
    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;

    if( useSymmetricEquator )
    {
        phobosInertiaTensor( 0, 0 ) = phobosInertiaTensor( 1, 1 );
    }

    double phobosReferenceRadius = 11.27E3;
    double phobosMass = 1.0659E16;

    phobosInertiaTensor *= (phobosReferenceRadius * phobosReferenceRadius * phobosMass );
    bodyMap[ "Phobos" ]->setBodyInertiaTensor( phobosInertiaTensor );

    double phobosGravitationalParameter = phobosMass * physical_constants::GRAVITATIONAL_CONSTANT;

    Eigen::MatrixXd phobosCosineGravityFieldCoefficients = Eigen::Matrix3d::Zero( ),
            phobosSineGravityFieldCoefficients = Eigen::Matrix3d::Zero( );
    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( );

    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( );
    phobosKeplerElements( 0 ) = phobosSemiMajorAxis;

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

    return bodyMap;
}

BOOST_AUTO_TEST_SUITE( test_rotational_dynamics_propagation )

//! Function to test torque-free propagation with initial rotation around one of its principal axes
BOOST_AUTO_TEST_CASE( testSimpleRotationalDynamicsPropagation )
{
    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Test code for different propagation schemes
    for( int propagatorType = 0; propagatorType < 3; propagatorType++ )
    {
        // Perform test for initial rotationa about body-fixed x, y and z axes.
        for( unsigned axisCase = 0; axisCase < 3; axisCase++ )
        {
            // Retrieve list of body objects.
            NamedBodyMap bodyMap = getTestBodyMap( 9376.0E3 );

            // Define time range of test.
            double initialEphemerisTime = 1.0E7;
            double finalEphemerisTime = initialEphemerisTime + 10.0 * 86400.0;

            // Set torques between bodies that are to be taken into account.
            SelectedTorqueMap torqueMap;
            std::vector< std::string > bodiesToIntegrate;
            bodiesToIntegrate.push_back( "Phobos" );

            // Define mean motion (equal to rotation rate).
            double phobosSemiMajorAxis = 9376.0E3;
            double meanMotion = std::sqrt( getBodyGravitationalParameter( "Mars" ) / std::pow( phobosSemiMajorAxis, 3.0 ) );

            // Define initial rotational state
            Eigen::Quaterniond initialRotation =
                    reference_frames::getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion(
                        0.2, 0.7 );
            Eigen::Matrix3d initialRotationMatrixToBaseFrame = initialRotation.toRotationMatrix( );
            Eigen::Matrix3d initialRotationMatrixToTargetFrame = initialRotationMatrixToBaseFrame.transpose( );
            Eigen::VectorXd systemInitialState = Eigen::VectorXd::Zero( 7 );
            systemInitialState.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat(
                        initialRotation );
            systemInitialState( 4 + axisCase ) = meanMotion;

            // Create torque models
            basic_astrodynamics::TorqueModelMap torqueModelMap = createTorqueModelsMap(
                        bodyMap, torqueMap, bodiesToIntegrate );

            // Define propagator settings.
            std::shared_ptr< RotationalStatePropagatorSettings< double > > propagatorSettings =
                    std::make_shared< RotationalStatePropagatorSettings< double > >
                    ( torqueModelMap, bodiesToIntegrate, systemInitialState, std::make_shared< PropagationTimeTerminationSettings >(
                          finalEphemerisTime ) );

            // Define integrator settings.
            std::shared_ptr< numerical_integrators::IntegratorSettings< > > integratorSettings =
                    std::make_shared< RungeKuttaVariableStepSizeSettings< > >
                    ( initialEphemerisTime, 10.0,
                      RungeKuttaCoefficients::rungeKuttaFehlberg78,
                      2.0, 30.0, 1.0E-13, 1.0E-13 );

            // Propagate dynamics
            SingleArcDynamicsSimulator< double > dynamicsSimulator(
                        bodyMap, integratorSettings, propagatorSettings, true, false, true );

            // Retrieve Phobos rotation model with reset rotational state
            std::shared_ptr< RotationalEphemeris > phobosRotationalEphemeris = bodyMap[ "Phobos" ]->getRotationalEphemeris( );

            // Declare rotational velocity vectors to compute/expect
            Eigen::Vector3d currentRotationalVelocityInTargetFrame, currentRotationalVelocityInBaseFrame;
            Eigen::Vector3d expectedRotationalVelocityVectorInBaseFrame;

            // Declare rotation rate in body-fixed frame (constant)
            Eigen::Vector3d expectedRotationalVelocityVectorInTargetFrame = Eigen::Vector3d::Zero( );
            expectedRotationalVelocityVectorInTargetFrame( axisCase ) = meanMotion;

            // Declare rotatio matrices to compute/expect
            Eigen::Matrix3d currentRotationMatrixToTargetFrame, currentRotationMatrixToBaseFrame;
            Eigen::Matrix3d expectedRotationMatrixToTargetFrame, expectedRotationMatrixToBaseFrame;

            // Declare rotatio matrix derivatives to compute/expect
            Eigen::Matrix3d currentRotationMatrixDerivativeToTargetFrame, currentRotationMatrixDerivativeToBaseFrame,
                    currentIndirectRotationMatrixDerivative;
            Eigen::Matrix3d expectedRotationMatrixDerivativeToTargetFrame, expectedRotationMatrixDerivativeToBaseFrame;

            // Declare expected rotation matrices w.r.t. initial rotational state
            Eigen::Matrix3d expectedRotationToTargetFrameFromInitialRotation, expectedRotationToBaseFrameFromInitialRotation;

            // Compare expected and true rotational state for list of times
            double startTime = initialEphemerisTime;
            double endTime = finalEphemerisTime - 3600.0;
            double currentTime = startTime;
            double timeStep = 600.0;
            while ( currentTime < endTime )
            {
                // Define expected rotation angle
                double currentAngle = meanMotion * ( currentTime - initialEphemerisTime );

                // Compute expected rotation matrices and compare to result from ephemerides
                Eigen::Vector3d rotationAxis;
                if( axisCase == 0 )
                {
                    rotationAxis = Eigen::Vector3d::UnitX( );
                }
                else if( axisCase == 1 )
                {
                    rotationAxis = Eigen::Vector3d::UnitY( );
                }
                else if( axisCase == 2 )
                {
                    rotationAxis = Eigen::Vector3d::UnitZ( );
                }

                Eigen::Matrix3d baseRotationToTargetFrame =
                        Eigen::AngleAxisd( -1.0 * currentAngle, rotationAxis ).toRotationMatrix( );

                currentRotationMatrixToTargetFrame = phobosRotationalEphemeris->getRotationToTargetFrame( currentTime );
                expectedRotationToTargetFrameFromInitialRotation =
                        baseRotationToTargetFrame * initialRotationMatrixToTargetFrame;
                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL(
                                    std::fabs( currentRotationMatrixToTargetFrame( i, j ) -
                                               expectedRotationToTargetFrameFromInitialRotation( i, j ) ), 1.0E-10 );
                    }
                }

                currentRotationMatrixToBaseFrame = phobosRotationalEphemeris->getRotationToBaseFrame( currentTime );
                expectedRotationToBaseFrameFromInitialRotation =
                        initialRotationMatrixToBaseFrame * baseRotationToTargetFrame.transpose( );
                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL(
                                    std::fabs( currentRotationMatrixToBaseFrame( i, j ) -
                                               expectedRotationToBaseFrameFromInitialRotation( i, j ) ), 1.0E-10 );
                    }
                }

                // Compute expected rotation matrix derivatives and compare to result from ephemerides
                currentRotationMatrixDerivativeToTargetFrame =
                        phobosRotationalEphemeris->getDerivativeOfRotationToTargetFrame( currentTime );
                Eigen::Matrix3d premultiplierMatrix;

                if( axisCase == 0 )
                {
                    premultiplierMatrix = reference_frames::X_AXIS_ROTATION_MATRIX_DERIVATIVE_PREMULTIPLIER;
                }
                else if( axisCase == 1 )
                {
                    premultiplierMatrix = reference_frames::Y_AXIS_ROTATION_MATRIX_DERIVATIVE_PREMULTIPLIER ;
                }
                else if( axisCase == 2 )
                {
                    premultiplierMatrix = reference_frames::Z_AXIS_ROTATION_MATRIX_DERIVATIVE_PREMULTIPLIER ;
                }

                expectedRotationMatrixDerivativeToTargetFrame =
                        meanMotion * premultiplierMatrix * baseRotationToTargetFrame *
                        initialRotationMatrixToTargetFrame;

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

                currentRotationMatrixDerivativeToBaseFrame =
                        phobosRotationalEphemeris->getDerivativeOfRotationToBaseFrame( currentTime );
                expectedRotationMatrixDerivativeToBaseFrame =
                        expectedRotationMatrixDerivativeToTargetFrame.transpose( );

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

                // Compute expected angular velocity vectors and compare to result from ephemerides
                currentRotationalVelocityInTargetFrame =
                        phobosRotationalEphemeris->getRotationalVelocityVectorInTargetFrame( currentTime );
                BOOST_CHECK_SMALL( std::fabs( currentRotationalVelocityInTargetFrame( 0 ) -
                                              expectedRotationalVelocityVectorInTargetFrame( 0 ) ),
                                   meanMotion * 1.0E-15 );
                BOOST_CHECK_SMALL( std::fabs( currentRotationalVelocityInTargetFrame( 1 ) -
                                              expectedRotationalVelocityVectorInTargetFrame( 1 ) ),
                                   meanMotion * 1.0E-15 );
                BOOST_CHECK_SMALL( std::fabs( currentRotationalVelocityInTargetFrame( 2 ) -
                                              expectedRotationalVelocityVectorInTargetFrame( 2 ) ),
                                   meanMotion * 1.0E-15 );

                currentRotationalVelocityInBaseFrame =
                        phobosRotationalEphemeris->getRotationalVelocityVectorInBaseFrame( currentTime );
                expectedRotationalVelocityVectorInBaseFrame =
                        currentRotationMatrixToBaseFrame * expectedRotationalVelocityVectorInTargetFrame;
                BOOST_CHECK_SMALL( std::fabs( currentRotationalVelocityInBaseFrame( 0 ) -
                                              expectedRotationalVelocityVectorInBaseFrame( 0 ) ),
                                   meanMotion * 1.0E-15 );
                BOOST_CHECK_SMALL( std::fabs( currentRotationalVelocityInBaseFrame( 1 ) -
                                              expectedRotationalVelocityVectorInBaseFrame( 1 ) ),
                                   meanMotion * 1.0E-15 );
                BOOST_CHECK_SMALL( std::fabs( currentRotationalVelocityInBaseFrame( 2 ) -
                                              expectedRotationalVelocityVectorInBaseFrame( 2 ) ),
                                   meanMotion * 1.0E-15 );

                currentTime += timeStep;
            }

            Eigen::Matrix3d numericalRotationMatrixDerivativeToBaseFrame, numericalRotationMatrixDerivativeToTargetFrame;
            Eigen::Matrix3d upperturbedMatrix, downperturbedMatrix;

            // Test whether rotation matrix derivatives are consistent with rotation matrices (using central differences)
            double timePerturbation = 0.1;
            currentTime = startTime + timeStep;
            while ( currentTime < endTime )
            {
                // Test rotation matrix derivative to base frame
                currentRotationMatrixDerivativeToBaseFrame =
                        phobosRotationalEphemeris->getDerivativeOfRotationToBaseFrame( currentTime );

                upperturbedMatrix =
                        phobosRotationalEphemeris->getRotationToBaseFrame( currentTime + timePerturbation ).toRotationMatrix( );
                downperturbedMatrix =
                        phobosRotationalEphemeris->getRotationToBaseFrame( currentTime - timePerturbation ).toRotationMatrix( );
                numericalRotationMatrixDerivativeToBaseFrame =
                        ( upperturbedMatrix - downperturbedMatrix ) / ( 2.0 * timePerturbation );

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

                // Test rotation matrix derivative to target frame
                currentRotationMatrixDerivativeToTargetFrame =
                        phobosRotationalEphemeris->getDerivativeOfRotationToTargetFrame( currentTime );

                upperturbedMatrix =
                        phobosRotationalEphemeris->getRotationToTargetFrame( currentTime + timePerturbation ).toRotationMatrix( );
                downperturbedMatrix =
                        phobosRotationalEphemeris->getRotationToTargetFrame( currentTime - timePerturbation ).toRotationMatrix( );
                numericalRotationMatrixDerivativeToTargetFrame =
                        ( upperturbedMatrix - downperturbedMatrix ) / ( 2.0 * timePerturbation );

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

                currentTime += timeStep;

            }
        }
    }
}

//! Function to test torque-free propagation with initial rotation not around one of its principal axes. The compuited results
//! are compared to the expected precession
BOOST_AUTO_TEST_CASE( testSimpleRotationalDynamicsPropagationWithObliquity )
{
    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Test code for different propagation schemes
    for( int propagatorType = 0; propagatorType < 3; propagatorType++ )
    {
        // Retrieve list of body objects.
        NamedBodyMap bodyMap = getTestBodyMap( 9376.0E3, 1 );

        // Define time range of test.
        double initialEphemerisTime = 1.0E7;
        double finalEphemerisTime = initialEphemerisTime + 10.0 * 86400.0;

        // Set torques between bodies that are to be taken into account.
        SelectedTorqueMap torqueMap;
        std::vector< std::string > bodiesToIntegrate;
        bodiesToIntegrate.push_back( "Phobos" );

        // Define mean motion (equal to rotation rate).
        double phobosSemiMajorAxis = 9376.0E3;
        double meanMotion = std::sqrt( getBodyGravitationalParameter( "Mars" ) /
                                       std::pow( phobosSemiMajorAxis, 3.0 ) );

        // Define initial rotational state
        Eigen::Quaterniond nominalInitialRotation = Eigen::Quaterniond( 1.0, 0.0, 0.0, 0.0 );
        double initialObliquity = 20.0 * mathematical_constants::PI / 180.0;
        Eigen::VectorXd systemInitialState = Eigen::VectorXd::Zero( 7 );
        systemInitialState.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat(
                    Eigen::AngleAxisd( -initialObliquity, Eigen::Vector3d::UnitX( ) ) * nominalInitialRotation );
        double initialXAngularVelocity = 0.1 * meanMotion;
        systemInitialState( 4 ) = initialXAngularVelocity;
        systemInitialState( 5 ) = 0.0 * meanMotion;
        systemInitialState( 6 ) = meanMotion;

        // Create torque models
        basic_astrodynamics::TorqueModelMap torqueModelMap = createTorqueModelsMap(
                    bodyMap, torqueMap, bodiesToIntegrate );

        // Define integrator settings.
        std::shared_ptr< numerical_integrators::IntegratorSettings< > > integratorSettings =
                std::make_shared< RungeKuttaVariableStepSizeSettings< > >
                ( initialEphemerisTime, 10.0,
                  RungeKuttaCoefficients::rungeKuttaFehlberg78,
                  30.0, 300.0, 1.0E-14, 1.0E-14 );

        // Define propagator settings.
        std::shared_ptr< RotationalStatePropagatorSettings< double > > propagatorSettings =
                std::make_shared< RotationalStatePropagatorSettings< double > >
                ( torqueModelMap, bodiesToIntegrate, systemInitialState, std::make_shared< PropagationTimeTerminationSettings >(
                      finalEphemerisTime ) );

        // Propagate dynamics
        SingleArcDynamicsSimulator< double > dynamicsSimulator(
                    bodyMap, integratorSettings, propagatorSettings, true, false, true );

        // Retrieve Phobos rotation model with reset rotational state
        std::shared_ptr< RotationalEphemeris > phobosRotationalEphemeris = bodyMap[ "Phobos" ]->getRotationalEphemeris( );

        // Declare vectors/matrices to be used in test
        Eigen::Vector3d currentRotationalVelocityInBaseFrame, currentRotationalVelocityInTargetFrame,
                indirectRotationalVelocityInBaseFrame;
        Eigen::Matrix3d currentRotationMatrixToBaseFrame, currentRotationMatrixToTargetFrame;
        Eigen::Matrix3d currentRotationMatrixDerivativeToBaseFrame, currentRotationMatrixDerivativeToTargetFrame;
        Eigen::Matrix3d numericalRotationMatrixDerivativeToBaseFrame, numericalRotationMatrixDerivativeToTargetFrame;
        Eigen::Matrix3d upperturbedMatrix, downperturbedMatrix;

        //    Eigen::Vector3d eulerAngles, calculatedEulerAngleRates, expectedEulerAngleRates;
        //    double eulerPhase = 0.0;

        // Compare expected and true rotational state for list of times
        double startTime = initialEphemerisTime + 3600.0;
        double endTime = finalEphemerisTime - 3600.0;
        double currentTime = startTime;
        double timeStep = ( endTime - startTime ) / 20.0;
        double timePerturbation = 0.1;

        double eulerFrequency = ( 0.5024 - 0.4265 ) / 0.4265 * meanMotion;

        while ( currentTime < endTime )
        {
            currentRotationalVelocityInTargetFrame =
                    phobosRotationalEphemeris->getRotationalVelocityVectorInTargetFrame( currentTime );

            // Compare propagated and expected angular velocity vecots
            BOOST_CHECK_SMALL( currentRotationalVelocityInTargetFrame( 0 ) -
                               initialXAngularVelocity * std::cos( eulerFrequency * ( currentTime - initialEphemerisTime ) ),
                               1.0E-15 );
            BOOST_CHECK_SMALL( currentRotationalVelocityInTargetFrame( 1 ) -
                               initialXAngularVelocity * std::sin( eulerFrequency * ( currentTime - initialEphemerisTime ) ),
                               1.0E-15 );
            BOOST_CHECK_CLOSE_FRACTION( currentRotationalVelocityInTargetFrame( 2 ), meanMotion, 1.0E-15 );

            // Compare rotation matrix derivative to base frame with finite difference result
            currentRotationMatrixDerivativeToBaseFrame =
                    phobosRotationalEphemeris->getDerivativeOfRotationToBaseFrame( currentTime );

            upperturbedMatrix =
                    phobosRotationalEphemeris->getRotationToBaseFrame( currentTime + timePerturbation ).toRotationMatrix( );
            downperturbedMatrix =
                    phobosRotationalEphemeris->getRotationToBaseFrame( currentTime - timePerturbation ).toRotationMatrix( );
            numericalRotationMatrixDerivativeToBaseFrame =
                    ( upperturbedMatrix - downperturbedMatrix ) / ( 2.0 * timePerturbation );

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

            // Compare rotation matrix derivative to target frame with finite difference result
            currentRotationMatrixDerivativeToTargetFrame =
                    phobosRotationalEphemeris->getDerivativeOfRotationToTargetFrame( currentTime );

            upperturbedMatrix =
                    phobosRotationalEphemeris->getRotationToTargetFrame( currentTime + timePerturbation ).toRotationMatrix( );
            downperturbedMatrix =
                    phobosRotationalEphemeris->getRotationToTargetFrame( currentTime - timePerturbation ).toRotationMatrix( );
            numericalRotationMatrixDerivativeToTargetFrame =
                    ( upperturbedMatrix - downperturbedMatrix ) / ( 2.0 * timePerturbation );

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

            // Test consistency between rotation matrix and derivative with expected angular velocity vector
            indirectRotationalVelocityInBaseFrame =
                    getRotationalVelocityVectorInBaseFrameFromMatrices(
                        phobosRotationalEphemeris->getRotationToTargetFrame( currentTime ).toRotationMatrix( ),
                        phobosRotationalEphemeris->getDerivativeOfRotationToBaseFrame( currentTime ) );

            currentRotationalVelocityInBaseFrame = phobosRotationalEphemeris->getRotationalVelocityVectorInBaseFrame( currentTime );
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL(
                            std::fabs( indirectRotationalVelocityInBaseFrame( j ) -
                                       currentRotationalVelocityInBaseFrame( j ) ), 1.0E-15 );
            }

            currentRotationalVelocityInBaseFrame =
                    phobosRotationalEphemeris->getRotationToBaseFrame( currentTime ) * currentRotationalVelocityInTargetFrame;
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL(
                            std::fabs( indirectRotationalVelocityInBaseFrame( j ) -
                                       currentRotationalVelocityInBaseFrame( j ) ), 1.0E-15 );
            }

            currentTime += timeStep;
        }
    }
}

//! Perform concurrent rotational and translational dynamics, with aerodynamic force and torque-free rotational motion, and
//! check if aerodynamic angles and force coefficients are indeed taken from propagated rotation.
BOOST_AUTO_TEST_CASE( testRotationalAndTranslationalDynamicsPropagation )
{

    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Set simulation start epoch.
    const double simulationStartEpoch = 0.0;

    // Set simulation end epoch.
    const double simulationEndEpoch = 3100.0;

    // Set numerical integration fixed step size.
    const double fixedStepSize = 1.0;

    // Define simulation body settings.
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( { "Earth" }, simulationStartEpoch - 10.0 * fixedStepSize,
                                    simulationEndEpoch + 10.0 * fixedStepSize );
    bodySettings[ "Earth" ]->ephemerisSettings = std::make_shared< simulation_setup::ConstantEphemerisSettings >(
                Eigen::Vector6d::Zero( ), "SSB", "J2000" );
    bodySettings[ "Earth" ]->rotationModelSettings->resetOriginalFrame( "J2000" );

    // Create Earth object
    simulation_setup::NamedBodyMap bodyMap = simulation_setup::createBodies( bodySettings );

    // Create vehicle objects.
    bodyMap[ "Apollo" ] = std::make_shared< simulation_setup::Body >( );
    bodyMap[ "Apollo" ]->setConstantBodyMass( 5.0E3 );

    // Create vehicle aerodynamic coefficients
    bodyMap[ "Apollo" ]->setAerodynamicCoefficientInterface(
                unit_tests::getApolloCoefficientInterface( ) );

    // Set inertia tensor (dummy values)
    Eigen::Matrix3d inertiaTensor = Eigen::Matrix3d::Zero( );
    inertiaTensor( 0, 0 ) = 0.3615;
    inertiaTensor( 1, 1 ) = 0.4265;
    inertiaTensor( 2, 2 ) = 0.5024;
    inertiaTensor *= ( 0.1 * 25.0 * 5.0E3 );
    bodyMap[ "Apollo" ]->setBodyInertiaTensor( inertiaTensor );

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

    // Set tabulated ephemerides for orbit and rotation
    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< double, 7, 1 > > >
            dummyRotationInterpolator =
            std::make_shared< interpolators::LinearInterpolator< double, Eigen::Matrix< double, 7, 1 > > >( dummyRotationMap );
    bodyMap[ "Apollo" ]->setRotationalEphemeris( std::make_shared< TabulatedRotationalEphemeris< double, double > >(
                                                     dummyRotationInterpolator, "J2000", "Apollo_Fixed" ) );

    std::map< double, Eigen::Matrix< double, 6, 1 > > dummyStateMap;
    dummyStateMap[ -1.0E100 ] = Eigen::Matrix< double, 6, 1 >::Zero( );
    dummyStateMap[ 1.0E100 ] = Eigen::Matrix< double, 6, 1 >::Zero( );
    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< double, 6, 1 > > >
            dummyStateInterpolator =
            std::make_shared< interpolators::LinearInterpolator< double, Eigen::Matrix< double, 6, 1 > > >( dummyStateMap );
    bodyMap[ "Apollo" ]->setEphemeris( std::make_shared< TabulatedCartesianEphemeris< double, double > >(
                                           dummyStateInterpolator, "SSB", "J2000" ) );

    // Finalize body creation.
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "J2000" );

    // Test code for different propagation schemes
    for( int propagatorType = 0; propagatorType < 3; propagatorType++ )
    {
        // Test simulation with and without torques.
        for( int simulationCase = 0; simulationCase < 2; simulationCase++ )
        {
            // Define propagator settings variables.
            SelectedAccelerationMap accelerationMap;
            std::vector< std::string > bodiesToPropagate;
            std::vector< std::string > centralBodies;

            // Define acceleration model settings.
            std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfApollo;
            accelerationsOfApollo[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( 4, 0 ) );
            accelerationsOfApollo[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( aerodynamic ) );
            accelerationMap[ "Apollo" ] = accelerationsOfApollo;

            bodiesToPropagate.push_back( "Apollo" );
            centralBodies.push_back( "Earth" );

            // Create acceleration models
            basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                        bodyMap, accelerationMap, bodiesToPropagate, centralBodies );

            // Set spherical elements for Apollo.
            Eigen::Vector6d apolloSphericalEntryState;
            apolloSphericalEntryState( SphericalOrbitalStateElementIndices::radiusIndex ) =
                    spice_interface::getAverageRadius( "Earth" ) + 120.0E3;
            apolloSphericalEntryState( SphericalOrbitalStateElementIndices::latitudeIndex ) = 0.0;
            apolloSphericalEntryState( SphericalOrbitalStateElementIndices::longitudeIndex ) = 1.2;
            apolloSphericalEntryState( SphericalOrbitalStateElementIndices::speedIndex ) = 7.4E3;
            apolloSphericalEntryState( SphericalOrbitalStateElementIndices::flightPathIndex ) =
                    -1.2 * mathematical_constants::PI / 180.0;
            apolloSphericalEntryState( SphericalOrbitalStateElementIndices::headingAngleIndex ) = 0.6;

            // Convert apollo state from spherical elements to Cartesian elements.
            Eigen::Vector6d systemInitialState = convertSphericalOrbitalToCartesianState(
                        apolloSphericalEntryState );
            std::shared_ptr< ephemerides::RotationalEphemeris > earthRotationalEphemeris =
                    bodyMap.at( "Earth" )->getRotationalEphemeris( );
            systemInitialState = transformStateToGlobalFrame( systemInitialState, simulationStartEpoch, earthRotationalEphemeris );

            // Define initial rotational state
            Eigen::Quaterniond initialRotation = Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) );
            Eigen::VectorXd systemInitialRotationalState = Eigen::VectorXd::Zero( 7 );
            systemInitialRotationalState.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat(
                        initialRotation );
            systemInitialRotationalState( 4 ) = 1.0E-4;

            // Create torque models
            SelectedTorqueMap selectedTorqueModelMap;
            if( simulationCase > 0 )
            {
                selectedTorqueModelMap[ "Apollo" ][ "Earth" ].push_back(
                            std::make_shared< TorqueSettings >( aerodynamic_torque ) );
            }

            basic_astrodynamics::TorqueModelMap torqueModelMap = createTorqueModelsMap(
                        bodyMap, selectedTorqueModelMap, bodiesToPropagate );

            // Define list of dependent variables to save.
            std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesList;
            dependentVariablesList.push_back(
                        std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                            "Apollo", reference_frames::latitude_angle ) );
            dependentVariablesList.push_back(
                        std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                            "Apollo", reference_frames::longitude_angle ) );
            dependentVariablesList.push_back(
                        std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                            "Apollo", reference_frames::heading_angle ) );
            dependentVariablesList.push_back(
                        std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                            "Apollo", reference_frames::flight_path_angle ) );
            dependentVariablesList.push_back(
                        std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                            "Apollo", reference_frames::angle_of_attack ) );
            dependentVariablesList.push_back(
                        std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                            "Apollo", reference_frames::angle_of_sideslip ) );
            dependentVariablesList.push_back(
                        std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                            "Apollo", reference_frames::bank_angle   ) );
            if( simulationCase == 1 )
            {
                dependentVariablesList.push_back(
                            std::make_shared< SingleDependentVariableSaveSettings >(
                                aerodynamic_force_coefficients_dependent_variable, "Apollo" ) );
                dependentVariablesList.push_back(
                            std::make_shared< SingleTorqueDependentVariableSaveSettings >(
                                aerodynamic_torque, "Apollo", "Earth" ) );
                dependentVariablesList.push_back(
                            std::make_shared< SingleDependentVariableSaveSettings >(
                                total_torque_dependent_variable, "Apollo" ) );
            }

            // Create object with list of dependent variables
            std::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave =
                    std::make_shared< DependentVariableSaveSettings >( dependentVariablesList );

            // Define termination conditions
            std::shared_ptr< PropagationTerminationSettings > terminationSettings =
                    std::make_shared< PropagationTimeTerminationSettings >( 250.0 );

            // Create propagator settings for rotation.
            std::shared_ptr< RotationalStatePropagatorSettings< double > > rotationalPropagatorSettings =
                    std::make_shared< RotationalStatePropagatorSettings< double > >
                    ( torqueModelMap, bodiesToPropagate, systemInitialRotationalState, terminationSettings );

            // Create propagation settings for translational dynamics.
            std::shared_ptr< TranslationalStatePropagatorSettings< double > > translationalPropagatorSettings =
                    std::make_shared< TranslationalStatePropagatorSettings< double > >
                    ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState,
                      terminationSettings, cowell );

            // Create full propagator settings for rotation.
            std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > >  propagatorSettingsList;
            propagatorSettingsList.push_back( translationalPropagatorSettings );
            propagatorSettingsList.push_back( rotationalPropagatorSettings );

            std::shared_ptr< PropagatorSettings< double > > propagatorSettings =
                    std::make_shared< MultiTypePropagatorSettings< double > >(
                        propagatorSettingsList, terminationSettings, dependentVariablesToSave );

            // Create integrator settings for rotation.
            std::shared_ptr< IntegratorSettings< > > integratorSettings =
                    std::make_shared< RungeKuttaVariableStepSizeSettings< > >
                    ( 0.0, 0.02,
                      RungeKuttaCoefficients::rungeKuttaFehlberg78, 1.0E-4, 0.02, 1.0E-12, 1.0E-12 );

            // Create simulation object and propagate dynamics.
            SingleArcDynamicsSimulator< > dynamicsSimulator(
                        bodyMap, integratorSettings, propagatorSettings, true, false, true);
            std::map< double, Eigen::VectorXd > dependentVariableHistory = dynamicsSimulator.getDependentVariableHistory( );
            std::map< double, Eigen::VectorXd > propagationHistory = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );

            // Define test variables
            double currentLatitude, currentLongitude, currentFlightPathAngle, currentHeadingAngle,
                    currentAngleOfAttack, currentSideslipAngle, currentBankAngle, currentRotationAngle;
            Eigen::Matrix3d currentInertialToBodyFixedFrameRotation, currentEarthFixedToLVLHFrameRotation,
                    currentLVLHToTrajectoryFrameRotation,
                    currentTrajectoryToAerodynamicFrameRotation, currentAerodynamicToBodyFixesFrameRotation,
                    currentInertialToBodyFixedFrame, expectedInertialToBodyFixedFrame;
            Eigen::Matrix3d expectedInertialToBodyFixedFrameRotation;
            std::shared_ptr< RotationalEphemeris > earthRotationModel = bodyMap.at( "Earth" )->getRotationalEphemeris( );
            std::shared_ptr< RotationalEphemeris > apolloRotationModel = bodyMap.at( "Apollo" )->getRotationalEphemeris( );

            // Iterate over saved data, manually compute inertial to body-fixed rotation, and compare to expected matrix for
            // simulationCase=0; test inertial time-derivative of angular momentum to check consistency with magnitude of torque
            std::map< double, Eigen::Vector3d > inertialAngularMomentumMap;
            std::map< double, Eigen::Vector3d > inertialTorqueMap;
            for( std::map< double, Eigen::VectorXd >::const_iterator variableIterator = dependentVariableHistory.begin( );
                 variableIterator != dependentVariableHistory.end( ); variableIterator++ )
            {
                if( simulationCase == 0 )
                {
                    // Retrieve saved angles
                    currentLatitude = variableIterator->second( 0 );
                    currentLongitude = variableIterator->second( 1 );
                    currentHeadingAngle = variableIterator->second( 2 );
                    currentFlightPathAngle = variableIterator->second( 3 );
                    currentAngleOfAttack = variableIterator->second( 4 );
                    currentSideslipAngle = variableIterator->second( 5 );
                    currentBankAngle = variableIterator->second( 6 );

                    // Compute matrices from angles
                    currentInertialToBodyFixedFrameRotation = earthRotationModel->getRotationToTargetFrame( variableIterator->first );
                    currentEarthFixedToLVLHFrameRotation = getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion(
                                currentLongitude, currentLatitude );
                    currentLVLHToTrajectoryFrameRotation = getLocalVerticalFrameToTrajectoryTransformationQuaternion(
                                currentFlightPathAngle, currentHeadingAngle );
                    currentTrajectoryToAerodynamicFrameRotation = getTrajectoryToAerodynamicFrameTransformationQuaternion(
                                currentBankAngle );
                    currentAerodynamicToBodyFixesFrameRotation = getAirspeedBasedAerodynamicToBodyFrameTransformationQuaternion(
                                currentAngleOfAttack, currentSideslipAngle );
                    currentInertialToBodyFixedFrame = currentAerodynamicToBodyFixesFrameRotation *
                            currentTrajectoryToAerodynamicFrameRotation *
                            currentLVLHToTrajectoryFrameRotation * currentEarthFixedToLVLHFrameRotation *
                            currentInertialToBodyFixedFrameRotation;

                    // Compure expected rotation angle and rotation matrix
                    currentRotationAngle = systemInitialRotationalState( 4 ) * variableIterator->first;
                    expectedInertialToBodyFixedFrame =
                            Eigen::AngleAxisd( -1.0 * currentRotationAngle, Eigen::Vector3d::UnitX( ) ).toRotationMatrix( );

                    // Compare expected and actual rotation matrices
                    for( unsigned int i = 0; i < 3; i++ )
                    {
                        for( unsigned int j = 0; j < 3; j++ )
                        {
                            BOOST_CHECK_SMALL(
                                        std::fabs( expectedInertialToBodyFixedFrame( i, j ) -
                                                   currentInertialToBodyFixedFrame( i, j ) ), 1.0E-13 );
                        }
                    }
                }

                // Retrieve torque and angular momentum in inertial frame.
                else if( simulationCase == 1 )
                {
                    Eigen::Matrix3d currentRotationFromApolloFixedToInertialFrame = apolloRotationModel->getRotationToBaseFrame(
                                variableIterator->first ).toRotationMatrix( );
                    Eigen::Matrix3d currentRotationFromInertialToBodyFixedFrame =
                            currentRotationFromApolloFixedToInertialFrame.transpose( );
                    Eigen::Matrix3d apolloInertiaTensorInInertialFrame =
                            currentRotationFromApolloFixedToInertialFrame * inertiaTensor *
                            currentRotationFromInertialToBodyFixedFrame;
                    Eigen::Vector3d apolloAngularVelocityVectorInInertialFrame =
                            apolloRotationModel->getRotationalVelocityVectorInBaseFrame( variableIterator->first );
                    inertialAngularMomentumMap[ variableIterator->first ] =
                            apolloInertiaTensorInInertialFrame * apolloAngularVelocityVectorInInertialFrame;
                    inertialTorqueMap[ variableIterator->first ] =
                            currentRotationFromApolloFixedToInertialFrame * variableIterator->second.segment( 10, 3 );
                }
            }

            // Compare time rate of angular momentum (using finite differences) with torque magnitudes
            if( simulationCase == 1 )
            {
                // Create and set interpolator for angular momentum.
                std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Vector3d > >
                        angularMomentumInterpolator =
                        std::make_shared< interpolators::LagrangeInterpolator< double, Eigen::Vector3d > >(
                            inertialAngularMomentumMap, 6 );
                typedef interpolators::OneDimensionalInterpolator< double, Eigen::Vector3d > LocalInterpolator;
                std::function< Eigen::Vector3d( const double ) > angularMomentumFunction = std::bind(
                            static_cast< Eigen::Vector3d( LocalInterpolator::* )( const double ) >
                            ( &LocalInterpolator::interpolate ), angularMomentumInterpolator, std::placeholders::_1 );

                double timeStep = 0.001;

                // Iterate over saved data, and check consistency of torque and angular momentum
                for( std::map< double, Eigen::Vector3d >::const_iterator variableIterator = ( inertialTorqueMap.begin( )++ );
                     variableIterator != inertialTorqueMap.end( ); variableIterator++ )
                {
                    if( variableIterator->first < ( --inertialTorqueMap.end( ) )->first - 10.0
                            && variableIterator->first > inertialTorqueMap.begin( )->first + 10.0 )
                    {
                        Eigen::Vector3d angularMomentumDerivative = numerical_derivatives::computeCentralDifferenceFromFunction(
                                    angularMomentumFunction, variableIterator->first, timeStep, numerical_derivatives::order4 );

                        for( unsigned int i = 0; i < 3; i++ )
                        {
                            BOOST_CHECK_SMALL(
                                        std::fabs( inertialTorqueMap.at( variableIterator->first )( i ) -
                                                   angularMomentumDerivative( i ) ), 0.25 );
                        }
                    }
                }
            }
        }
    }
}

//! Test if rotational dynamics propagation correctly produces in-plane libration
BOOST_AUTO_TEST_CASE( testSimpleRotationalDynamicsPropagationWithLibration )
{
    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Retrieve list of body objects.
    NamedBodyMap bodyMap = getTestBodyMap( 9376.0E3, 0 );
    setGlobalFrameBodyEphemerides( bodyMap, "Mars", "ECLIPJ2000" );

    // Define time range of test.
    double initialEphemerisTime = 0.0;
    double finalEphemerisTime = initialEphemerisTime + 86400.0;

    // Set torques between bodies that are to be taken into account.
    std::vector< std::string > bodiesToIntegrate;
    bodiesToIntegrate.push_back( "Phobos" );

    // Define mean motion (equal to rotation rate).
    double phobosSemiMajorAxis = 9376.0E3;
    double marsGravitationalParameter = bodyMap.at( "Mars" )->getGravityFieldModel( )->getGravitationalParameter( );
    double meanMotion = std::sqrt( marsGravitationalParameter /
                                   std::pow( phobosSemiMajorAxis, 3.0 ) );

    // Define initial rotational state
    double initialAnglePerturbation = 1.0E-6;
    double initialRotationRatePerturbation = 1.0E-6;

    Eigen::Quaterniond nominalInitialRotation =
            Eigen::Quaterniond( Eigen::AngleAxisd( -initialAnglePerturbation, Eigen::Vector3d::UnitZ( ) ) );
    Eigen::VectorXd systemInitialState = Eigen::VectorXd::Zero( 7 );
    systemInitialState.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat( nominalInitialRotation );
    systemInitialState( 6 ) = meanMotion * ( 1.0 + initialRotationRatePerturbation );

    Eigen::Matrix3d phobosInertiaTensor = bodyMap.at( "Phobos" )->getBodyInertiaTensor( );

    double frequencySquared = 3.0 * marsGravitationalParameter / std::pow(
                phobosSemiMajorAxis, 3.0 ) * ( phobosInertiaTensor( 1, 1 ) - phobosInertiaTensor( 0, 0 ) ) /
            phobosInertiaTensor( 2, 2 );

    double beta = std::atan2( -meanMotion * initialRotationRatePerturbation,
                              -std::sqrt( frequencySquared ) * initialAnglePerturbation );
    double alpha = -initialAnglePerturbation / std::cos( beta );

    // Test code for different propagation schemes
    for( int propagatorType = 0; propagatorType < 3; propagatorType++ )
    {
        // Create torque models
        for( int torqueType = 0; torqueType < 2; torqueType++ )
        {
            SelectedTorqueMap torqueMap;
            if( torqueType ==  0 )
            {
                torqueMap[ "Phobos" ][ "Mars" ].push_back(
                            std::make_shared< TorqueSettings >( second_order_gravitational_torque ) );
            }
            else
            {
                torqueMap[ "Phobos" ][ "Mars" ].push_back(
                            std::make_shared< SphericalHarmonicTorqueSettings >( 2, 2 ) );
            }

            // Create torque models
            basic_astrodynamics::TorqueModelMap torqueModelMap = createTorqueModelsMap(
                        bodyMap, torqueMap, bodiesToIntegrate );

            // Define integrator settings.
            std::shared_ptr< IntegratorSettings< > > integratorSettings =
                    std::make_shared< IntegratorSettings< > >
                    ( rungeKutta4, initialEphemerisTime, 10.0 );

            // Define propagator settings.
            std::shared_ptr< RotationalStatePropagatorSettings< double > > propagatorSettings =
                    std::make_shared< RotationalStatePropagatorSettings< double > >
                    ( torqueModelMap, bodiesToIntegrate, systemInitialState, std::make_shared< PropagationTimeTerminationSettings >(
                          finalEphemerisTime ) );

            // Propagate dynamics
            SingleArcDynamicsSimulator< double > dynamicsSimulator(
                        bodyMap, integratorSettings, propagatorSettings, true, false, true );

            // Retrieve Phobos rotation model with reset rotational state
            std::shared_ptr< RotationalEphemeris > phobosRotationalEphemeris = bodyMap[ "Phobos" ]->getRotationalEphemeris( );

            // Declare vectors/matrices to be used in test
            Eigen::Vector3d currentRotationalVelocityInBaseFrame, currentRotationalVelocityInTargetFrame;

            //    Eigen::Vector3d eulerAngles, calculatedEulerAngleRates, expectedEulerAngleRates;
            //    double eulerPhase = 0.0;

            // Compare expected and true rotational state for list of times
            double startTime = initialEphemerisTime;
            double endTime = finalEphemerisTime - 3600.0;
            double currentTime = startTime;
            double timeStep = ( endTime - startTime ) / 1000.0;

            currentTime += timeStep;
            while ( currentTime < endTime )
            {
                currentRotationalVelocityInTargetFrame =
                        phobosRotationalEphemeris->getRotationalVelocityVectorInTargetFrame( currentTime );
                currentRotationalVelocityInBaseFrame = phobosRotationalEphemeris->getRotationalVelocityVectorInBaseFrame( currentTime );

                double expectedRotationRateDeviation = -alpha * std::sqrt( frequencySquared ) * std::sin(
                            std::sqrt( frequencySquared ) * currentTime + beta );

                BOOST_CHECK_SMALL( std::fabs( currentRotationalVelocityInTargetFrame( 2 ) - meanMotion - expectedRotationRateDeviation ),
                                   1.0E-16 );

                currentTime += timeStep;
            }

        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


//------------------------------------------------------------------------------
/// \file FoldExpression_tests.cpp
/// \ref C++17 - The Complete Guide: First Edition Paperback – September 6, 2019
/// by Nicolai M. Josuttis
//------------------------------------------------------------------------------
#include "Cpp/Templates/FoldExpressions.h"

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

using Cpp::Templates::FoldExpressions::fold_sum;
using std::string;

BOOST_AUTO_TEST_SUITE(Cpp)
BOOST_AUTO_TEST_SUITE(Templates)

BOOST_AUTO_TEST_SUITE(FoldExpressions_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TemplateParameterPackAcceptsZeroTemplateArguments)
{
  BOOST_TEST(fold_sum<>(11, -42.5) == -31.5);
  BOOST_TEST(fold_sum<>('a', string{"gtc"}) == "agtc");
}

//------------------------------------------------------------------------------
/// cf. Ch. 11 Fold Expressions, pp. 103, Josuttis (2019)
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(FoldSumReturnsSumOfAllPassedArguments)
{
  BOOST_TEST(fold_sum(47, 11, -1) == 57);

  const double x {4269.9624};

  BOOST_TEST(fold_sum(47, 11, x, -1) == 4326.9624);

}

BOOST_AUTO_TEST_SUITE_END() // FoldExpressions_tests

BOOST_AUTO_TEST_SUITE_END() // Templates
BOOST_AUTO_TEST_SUITE_END() // Cpp


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_BLOCK_SHACAL1_HPP
#define CRYPTO3_BLOCK_SHACAL1_HPP

#include <boost/crypto3/block/basic_shacal.hpp>

namespace boost {
    namespace crypto3 {
        namespace block {
            /*!
             * @brief Shacal1. Merkle-Damgård construction foundation for
             * @ref boost::crypto3::hashes::sha1 "SHA1" hashes.
             *
             * @ingroup block
             *
             * Implemented directly from the SHA standard as found at
             * http://csrc.nist.gov/publications/fips/fips180-2/fips180-2.pdf
             *
             * In SHA terminology:
             * - plaintext = H^(i-1)
             * - ciphertext = H^(i)
             * - key = M^(i)
             * - schedule = W
             */
            class shacal1 : public basic_shacal {
            public:
                shacal1(const key_type &k) : basic_shacal(build_schedule(k)) {
                }

                shacal1(schedule_type s) : basic_shacal((prepare_schedule(s), s)) {
                }

            private:
                static schedule_type build_schedule(key_type const &key) {
                    // Copy key into beginning of round_constants_words
                    schedule_type schedule;
                    for (unsigned t = 0; t < key_words; ++t) {
                        schedule[t] = key[t];
                    }
                    prepare_schedule(schedule);
                    return schedule;
                }

                static void prepare_schedule(schedule_type &schedule) {
                    for (unsigned t = key_words; t < rounds; ++t) {
                        schedule[t] = schedule[t - 3] ^ schedule[t - 8] ^ schedule[t - 14] ^ schedule[t - 16];
                        schedule[t] = policy_type::rotl<1>(schedule[t]);
                    }
                }
            };
        }    // namespace block
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_BLOCK_CIPHERS_SHACAL1_HPP


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include "Molassembler/Shapes/Properties.h"

#include "boost/functional/hash.hpp"
#include <Eigen/Dense>
#include "Molassembler/Temple/Adaptors/Transform.h"
#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/GroupBy.h"
#include "Molassembler/Temple/Optionals.h"
#include "Molassembler/Temple/Permutations.h"
#include "Molassembler/Temple/Variadic.h"
#include "Molassembler/Temple/constexpr/Numeric.h"

#include "Molassembler/Shapes/Data.h"

#include <unordered_set>

namespace Scine {
namespace Molassembler {
namespace Shapes {
namespace Detail {

/*!
 * Generates a vector containing strictly monotonically increasing natural
 * numbers representing the range [start, end). If start == end, an empty
 * range is returned.
 */
template<typename NumericType>
std::vector<NumericType> range(
  const NumericType start,
  const NumericType end
) {
  if(start == end) {
    return {};
  }

  std::vector<NumericType> values (end - start);
  std::iota(
    values.begin(),
    values.end(),
    start
  );
  return values;
}

} // namespace Detail

namespace Properties {

std::vector<Vertex> applyPermutation(
  const Permutation& occupation,
  const Permutation& permutation
) {
  assert(occupation.size() == permutation.size());

  return Temple::map(
    permutation,
    Temple::Functor::at(occupation)
  );
}

std::vector<Vertex> applyRotation(
  const std::vector<Vertex>& occupation,
  const Shape shape,
  unsigned rotationFunctionIndex
) {
  return applyPermutation(
    occupation,
    Shapes::rotations(shape).at(rotationFunctionIndex)
  );
}

unsigned rotationPeriodicity(
  const Shapes::Shape shape,
  const std::vector<Vertex>& rotation
) {
  assert(rotation.size() == Shapes::size(shape));

  const auto initialIndices = Temple::iota<Vertex>(Vertex(Shapes::size(shape)));

  std::vector<Vertex> modified = applyPermutation(initialIndices, rotation);

  unsigned i = 1;
  for(/* */; modified != initialIndices && i < 20; ++i) {
    modified = applyPermutation(modified, rotation);
  }

  // No rotation should have a periodicity of 20.
  assert(i != 20);

  return i;
}

bool isRotation(const Permutation& rotation) {
  return Temple::sorted(rotation) == Temple::iota<Vertex>(rotation.size());
}

std::vector<std::vector<Vertex>> positionGroups(const Shapes::Shape shape) {
  /* The idea behind this algorithm is that an individual rotation (which is
   * really just an index permutation) could be interpreted as a directed graph
   * whose connected components represent sets of indices that can be
   * interchanged through multiple applications of the rotation.
   *
   * Then all that is necessary is to somehow combine multiple rotations into
   * another rotation in which overlapping connected components are merged.
   */
  const unsigned S = Shapes::size(shape);

  using IndexGroups = std::vector<Permutation>;

  // Helper function to find an index in an IndexGroup
  auto nestedFind = [](const IndexGroups& a, Vertex i) {
    return Temple::find_if(
      a,
      [&](const auto& group) -> bool {
        return Temple::find(group, i) != std::end(group);
      }
    );
  };

  // Fetches all indices that are part of a permutation's index loop starting at i
  auto loopVertices = [](
    const Permutation& rotation,
    const Vertex i
  ) -> std::vector<Vertex> {
    assert(isRotation(rotation));
    std::vector<Vertex> vertices(1, i);
    unsigned j = rotation.at(i);
    while(j != i) {
      vertices.emplace_back(j);
      j = rotation.at(j);
    }
    return vertices;
  };

  // Combine two rotations into one where overlapping connected components are merged
  auto merge = [&loopVertices](
    Permutation a,
    const Permutation& b
  ) -> Permutation {
    assert(isRotation(a) && isRotation(b));
    const unsigned R = a.size();
    assert(b.size() == R);
    for(Vertex i {0}; i < R; ++i) {
      // Self-reference is uninteresting
      if(b.at(i) == i) {
        continue;
      }

      // So now i is definitely part of a loop of more than one vertex.
      // If the permutation matches, we can already continue
      if(b.at(i) == a.at(i)) {
        continue;
      }

      // Now we have to establish which vertices are in the loop in a
      auto aLoopVertices = loopVertices(a, i);

      // If it's already a full loop, we cannot possibly merge anything else
      if(aLoopVertices.size() == R) {
        return a;
      }

      // If b.at(i) is already in that loop, we can continue.
      if(Temple::find(aLoopVertices, b.at(i)) != std::end(aLoopVertices)) {
        continue;
      }

      // Figure out which vertices from b's loop aren't yet in a
      auto bLoopVertices = Temple::sorted(loopVertices(b, i));
      Temple::sort(aLoopVertices);
      std::vector<Vertex> bVerticesNotInA;
      std::set_difference(
        std::begin(bLoopVertices),
        std::end(bLoopVertices),
        std::begin(aLoopVertices),
        std::end(aLoopVertices),
        std::back_inserter(bVerticesNotInA)
      );

      // Add in any vertices from loops in a that bVerticesNotInA have
      const unsigned t = bVerticesNotInA.size();
      for(Vertex j {0}; j < t; ++j) {
        for(Vertex k : loopVertices(a, bVerticesNotInA.at(j))) {
          if(Temple::find(bVerticesNotInA, k) == std::end(bVerticesNotInA)) {
            bVerticesNotInA.push_back(k);
          }
        }
      }

      // Found full set
      if(bVerticesNotInA.size() == R - 1) {
        a = {Vertex(R - 1)};
        for(Vertex k {0}; k < R - 1; ++k) {
          a.push_back(k);
        }
        return a;
      }

      // And insert them between i -> a.at(i)
      const unsigned hook = a.at(i);
      unsigned previous = i;
      for(unsigned j : bVerticesNotInA) {
        a.at(previous) = j;
        previous = j;
      }
      a.at(previous) = hook;

      assert(isRotation(a));
    }

    return a;
  };

  // Extract all loop groups from a rotation
  auto connectedComponents = [&](const Permutation& rotation) -> IndexGroups {
    const unsigned R = rotation.size();

    IndexGroups groups;
    for(Vertex i {0}; i < R; ++i) {
      if(nestedFind(groups, i) != std::end(groups)) {
        // Already found i
        continue;
      }

      groups.push_back(loopVertices(rotation, i));
    }

    return groups;
  };

  // Merge all rotations and then calculate the connected components
  const auto identity = Temple::iota<Vertex>(S);
  return connectedComponents(
    Temple::accumulate(rotations(shape), identity, merge)
  );
}

std::vector<char> positionGroupCharacters(const Shape shape) {
  const unsigned S = size(shape);
  // Transpose to a character-based symbolic representation
  std::vector<char> characterRepresentation (S);
  char currentChar = 'A';
  for(const auto& equalSet : positionGroups(shape)) {
    for(const auto& equalIndex : equalSet) {
      characterRepresentation.at(equalIndex) = currentChar;
    }

    ++currentChar;
  }

  return characterRepresentation;
}

std::vector<Vertex> inverseRotation(const std::vector<Vertex>& rotation) {
  const unsigned N = rotation.size();

  std::vector<Vertex> permutation (N);

  for(Vertex i {0}; i < N; ++i) {
    permutation.at(rotation.at(i)) = i;
  }

  return permutation;
}

Eigen::Vector3d getCoordinates(
  const Shapes::Shape shape,
  const boost::optional<Vertex>& vertexOption
) {
  if(vertexOption) {
    assert(vertexOption.value() < Shapes::size(shape));

    return coordinates(shape).col(
      vertexOption.value()
    );
  }

  return {0, 0, 0};
}

double getTetrahedronVolume(
  const Eigen::Vector3d& i,
  const Eigen::Vector3d& j,
  const Eigen::Vector3d& k,
  const Eigen::Vector3d& l
) {
  return (i - l).dot(
    (j - l).cross(k - l)
  );
}

double calculateAngleDistortion(
  const Shapes::Shape from,
  const Shapes::Shape to,
  const std::vector<Vertex>& indexMapping
) {
  const unsigned mappingIndexLimit = std::min(
    Shapes::size(from),
    Shapes::size(to)
  );

  assert(indexMapping.size() >= mappingIndexLimit);
  assert(
    std::abs(
      static_cast<int>(Shapes::size(from)) - static_cast<int>(Shapes::size(to))
    ) <= 1
  );

  double angularDistortion = 0;

  for(Vertex i(0); i < mappingIndexLimit; ++i) {
    for(Vertex j(i + 1); j < mappingIndexLimit; ++j) {
      angularDistortion += std::fabs(
        Shapes::angleFunction(from)(i, j)
        - Shapes::angleFunction(to)(
          indexMapping.at(i),
          indexMapping.at(j)
        )
      );
    }
  }

  return angularDistortion;
}

boost::optional<Vertex> propagateIndexOptionalThroughMapping(
  const boost::optional<Vertex>& indexOptional,
  const std::vector<Vertex>& indexMapping
) {
  return Temple::Optionals::map(
    indexOptional,
    [&](const Vertex v) -> Vertex {
      return indexMapping.at(v);
    }
  );
}

double calculateChiralDistortion(
  const Shapes::Shape from,
  const Shapes::Shape to,
  const std::vector<Vertex>& indexMapping
) {

  assert(indexMapping.size() >= std::min(Shapes::size(from), Shapes::size(to)));

  double chiralDistortion = 0;

  for(const auto& tetrahedron : Shapes::tetrahedra(from)) {
    chiralDistortion += std::fabs(
      getTetrahedronVolume(
        getCoordinates(from, tetrahedron.at(0)),
        getCoordinates(from, tetrahedron.at(1)),
        getCoordinates(from, tetrahedron.at(2)),
        getCoordinates(from, tetrahedron.at(3))
      ) - getTetrahedronVolume(
        getCoordinates(
          to,
          propagateIndexOptionalThroughMapping(tetrahedron.at(0), indexMapping)
        ),
        getCoordinates(
          to,
          propagateIndexOptionalThroughMapping(tetrahedron.at(1), indexMapping)
        ),
        getCoordinates(
          to,
          propagateIndexOptionalThroughMapping(tetrahedron.at(2), indexMapping)
        ),
        getCoordinates(
          to,
          propagateIndexOptionalThroughMapping(tetrahedron.at(3), indexMapping)
        )
      )
    );
  }

  return chiralDistortion;
}

std::set<
  std::vector<Vertex>
> generateAllRotations(
  const Shapes::Shape shape,
  const std::vector<Vertex>& indices
) {
  /* Replacing set here with unordered_set does not yield any speed
   * improvements. The hash function is comparatively too expensive for the
   * small number of elements in the set to set it apart from the tree.
   */
  assert(Shapes::size(shape) == indices.size());

  // Idea: Tree-like expansion of all possible combinations of rotations.
  using IndicesList = std::vector<Vertex>;

  std::set<IndicesList> allRotations = {indices};

  /* We keep a chain of all applied rotations that led to a specific index
   * sequence.  The upper limit for every link in the chain is the number of
   * rotations in the shape
   */
  unsigned linkLimit = Shapes::rotations(shape).size();

  std::vector<unsigned> chain = {0};
  /* It's also necessary to keep the structures themselves since we may
   * backtrack and apply a new rotation to a previous structure. Keeping track
   * is cheaper than applying all rotations in the chain to the initial indices
   */
  std::vector<IndicesList> chainStructures = {indices};

  /* Loop is broken when the very first link in the chain has been incremented
   * to the link limit
   */
  while(chain.front() < linkLimit) {
    // perform rotation
    // copy the last element in chainStructures
    auto generated = applyRotation(
      chainStructures.back(),
      shape,
      chain.back()
    );

    // is it something new?
    if(allRotations.count(generated) == 0) {
      // then add it to the set
      allRotations.insert(generated);

      // add it to the chain
      chainStructures.push_back(generated);
      chain.emplace_back(0);
    } else {
      // collapse the chain until we are at an incrementable position (if need be)
      while(
        chain.size() > 1 // retain at least first link in chain
        && chain.back() == linkLimit - 1 // remove link only if just below limit
      ) {
        chain.pop_back();
        chainStructures.pop_back();
      }

      // increment last position in chain
      ++chain.back();
    }
  }

  return allRotations;
}

std::vector<Vertex> applyIndexMapping(
  const Shapes::Shape to,
  const std::vector<Vertex>& mapping
) {
  /* Creates the list of indices in the target shape. Why is this necessary?
   *
   * E.g. An index mapping from linear to T-shaped. The individual
   * shape-internal numbering schemes are shown for the shape positions.
   *
   *  1  –▶  0
   *  |      |
   * (_)    (_) – 1 (new)
   *  |      |                Line pos. 0 to
   *  0  –▶  2                pos. 2 in Tshaped
   *                                 |  ┌– Line pos. 1 to pos. 0 in Tshaped
   *                                 |  |  ┌– This position is new
   * This mapping is represented as {2, 0, 1}.
   *
   * This function writes the indices of original mapping into the target
   * shape's indexing scheme.
   *
   * For this example, this returns {1, 2, 0}:
   *
   *  1 (at pos 0 in internal indexing scheme)
   *  |
   * (_) – 2 (etc.)
   *  |
   *  0
   *
   * For this example, this returns {1, 2, 0}.
   *
   * The closely related mapping {0, 2, 1} yields target indices {0, 2, 1}.
   *
   * Which of these properties are related by target shape rotations?
   *
   *
   *     mapping       target indices
   * -----------------------------------
   *    {2, 0, 1}   =>   {1, 2, 0}
   *        ▲                ▲
   *        |                |
   *        X                | C2 rotation in T-shape shape
   *        |                |
   *        ▼                ▼
   *    {0, 2, 1}   =>   {0, 2, 1}
   *
   */
  std::vector<Vertex> shapeVertices (Shapes::size(to));

  for(Vertex i {0}; i < Shapes::size(to); ++i) {
    shapeVertices.at(
      mapping.at(i)
    ) = i;
  }

  return shapeVertices;
}

DistortionInfo::DistortionInfo(
  std::vector<Vertex> passIndexMapping,
  const double passAngularDistortion,
  const double passChiralDistortion
) : indexMapping(std::move(passIndexMapping)),
    angularDistortion(passAngularDistortion),
    chiralDistortion(passChiralDistortion)
{}

std::size_t hash_value(const std::vector<unsigned>& permutation) {
  return Temple::permutationIndex(permutation);
}

std::vector<DistortionInfo> shapeTransitionMappings(
  const Shape from,
  const Shape to
) {

  /* Symmetries must be adjacent in size (0 = rearrangement,
   * +1 = ligand gain. Ligand loss is a special case where a specific position
   * in the shape group is removed, and is not covered here!
   */
  assert(
    (std::set<int> {0, 1}).count(
      static_cast<int>(Shapes::size(to))
      - static_cast<int>(Shapes::size(from))
    ) == 1
  );

  /* Base idea: We need to go through all possible mappings. In situations where
   * the target shape has one more or one fewer ligand, the last index in
   * the current sequence is either the added or removed ligand, and merely the
   * others are used to calculate the angular and chiral distortions involved
   * in the transition.
   *
   * For instance, from linear to T-shaped
   *
   *   0 - ( ) - 1     ->   0 - ( ) - 2
   *                             |
   *                             1
   *
   *   A mapping of {0 1 2} means that the new ligand is inserted at the 2
   *   position of T-shaped, which would involve distorting the 0-1 angle by
   *   90°. The optimal mapping would be {0 2 1} (or its equivalent rotation
   *   {2 0 1}), which does not have any angular distortion.
   */

  const unsigned largerSize = std::max(
    Shapes::size(from),
    Shapes::size(to)
  );

  auto indexMapping = Temple::iota<Vertex>(largerSize);

  // Store all distortions calculated
  std::vector<DistortionInfo> distortions;

  // Need to keep track of rotations of mappings to avoid repetition
  using PermutationType = std::vector<Vertex>;
  std::unordered_set<
    PermutationType,
    boost::hash<PermutationType>
  > encounteredShapeMappings;

  /* Using std::next_permutation generates all possible mappings!
   * do-while is required since the very first mapping must be considered before
   * calling next_permutation for the first time
   */
  do {
    if( // is the mapping new?
      encounteredShapeMappings.count(
        applyIndexMapping(to, indexMapping)
      ) == 0
    ) {
      /* Add it to the set of possible distortions, calculate angular and
       * chiral distortion involved in the mapping
       */
      distortions.emplace_back(
        indexMapping,
        calculateAngleDistortion(from, to, indexMapping),
        calculateChiralDistortion(from, to, indexMapping)
      );

      /* Any rotations of the mapping in the target shape are equivalent, we
       * do not want to count these as an additional multiplicity, so we
       * generate them and add them to the encountered mappings
       */
      auto allRotations = generateAllRotations(
        to,
        applyIndexMapping(
          to,
          indexMapping
        )
      );

      encounteredShapeMappings.insert(
        allRotations.begin(),
        allRotations.end()
      );
    }
  } while (std::next_permutation(indexMapping.begin(), indexMapping.end()));

  return distortions;
}

std::vector<DistortionInfo> ligandLossTransitionMappings(
  const Shapes::Shape from,
  const Shapes::Shape to,
  const Vertex positionInSourceShape
) {
  // Ensure we are dealing with ligand loss
  assert(Shapes::size(to) + 1 == Shapes::size(from));
  assert(positionInSourceShape < Shapes::size(from));

  /* Generate the index mapping specific to this position loss in the target
   * shape.
   *
   * The possible distortions are determined from the equivalent case of adding
   * the ligand (which was to be deleted) to the smaller shape.
   *
   * The deleted index is added to the end. The lowest permutation of the mapping
   * from the smaller to the larger shape is then:
   *
   * 1, 2, ..., (pos - 1), (pos + 1), ..., (from_size - 1), pos
   */
  std::vector<Vertex> indexMapping = Temple::variadic::concatenate(
    Temple::iota<Vertex>(positionInSourceShape),
    Detail::range(Vertex(positionInSourceShape + 1), Vertex(Shapes::size(from)))
  );

  /* NOTE: From here the algorithm is identical to shapeTransitionMappings
   * save that to and from are swapped in all occasions
   * and that std::next_permutation is only called on the subset excluding the
   * last position (the one that is added / deleted).
   */

  std::vector<DistortionInfo> distortions;

  std::set<
    std::vector<Vertex>
  > encounteredShapeMappings;

  do {
    if(encounteredShapeMappings.count(indexMapping) == 0) {
      distortions.emplace_back(
        indexMapping,
        calculateAngleDistortion(to, from, indexMapping),
        calculateChiralDistortion(to, from, indexMapping)
      );

      auto allRotations = generateAllRotations(to, indexMapping);

      encounteredShapeMappings.insert(
        allRotations.begin(),
        allRotations.end()
      );
    }
  } while (std::next_permutation(indexMapping.begin(), indexMapping.end()));

  return distortions;
}

ShapeTransitionGroup::ShapeTransitionGroup(
  std::vector<
    std::vector<Vertex>
  > passIndexMappings,
  const double passAngleDistortion,
  const double passChiralDistortion
) : indexMappings(std::move(passIndexMappings)),
    angularDistortion(passAngleDistortion),
    chiralDistortion(passChiralDistortion)
{}

ShapeTransitionGroup selectBestTransitionMappings(
  const std::vector<DistortionInfo>& distortions
) {
  /* We are interested only in the those transitions that have the very lowest
   * angular distortion, and within that set only the lowest chiral distortion,
   * so we sub-select within the generated set
   */

  const double lowestAngularDistortion = std::min_element(
    std::begin(distortions),
    std::end(distortions),
    [](const auto& a, const auto& b) -> bool {
      return a.angularDistortion < b.angularDistortion;
    }
  )->angularDistortion;

  std::vector<unsigned> viableDistortions;
  for(unsigned i = 0; i < distortions.size(); ++i) {
    if(distortions.at(i).angularDistortion < lowestAngularDistortion + floatingPointEqualityThreshold) {
      viableDistortions.push_back(i);
    }
  }

  // And now sub-set further on the lowest chiral distortion
  const double lowestChiralDistortion = distortions.at(
    *std::min_element(
      std::begin(viableDistortions),
      std::end(viableDistortions),
      [&](const unsigned a, const unsigned b) -> bool {
        return distortions.at(a).chiralDistortion < distortions.at(b).chiralDistortion;
      }
    )
  ).chiralDistortion;

  Temple::remove_if(
    viableDistortions,
    [&](const unsigned a) -> bool {
      return distortions.at(a).chiralDistortion > lowestChiralDistortion + floatingPointEqualityThreshold;
    }
  );

  return ShapeTransitionGroup(
    Temple::map(viableDistortions, [&](auto i) { return distortions.at(i).indexMapping; }),
    lowestAngularDistortion,
    lowestChiralDistortion
  );
}

unsigned numUnlinkedStereopermutations(
  const Shapes::Shape shape,
  const unsigned nIdenticalLigands
) {
  unsigned count = 1;

  auto indices = Temple::iota<Vertex>(Shapes::size(shape));

  for(unsigned i = 0; i < nIdenticalLigands; ++i) {
    indices.at(i) = 0;
  }

  auto initialRotations = generateAllRotations(shape, indices);

  std::set<decltype(indices)> rotations {
    std::make_move_iterator(std::begin(initialRotations)),
    std::make_move_iterator(std::end(initialRotations))
  };

  while(std::next_permutation(indices.begin(), indices.end())) {
    if(rotations.count(indices) == 0) {
      auto allRotations = generateAllRotations(shape, indices);
      for(const auto& rotation : allRotations) {
        rotations.insert(rotation);
      }

      ++count;
    }
  }

  return count;
}

bool hasMultipleUnlinkedStereopermutations(
  const Shapes::Shape shape,
  const unsigned nIdenticalLigands
) {
  if(nIdenticalLigands == Shapes::size(shape)) {
    return false;
  }

  auto indices = Temple::iota<Vertex>(Shapes::size(shape));

  for(unsigned i = 0; i < nIdenticalLigands; ++i) {
    indices.at(i) = 0;
  }

  auto initialRotations = generateAllRotations(shape, indices);

  std::set<decltype(indices)> rotations {
    std::make_move_iterator(std::begin(initialRotations)),
    std::make_move_iterator(std::end(initialRotations))
  };

  while(Temple::next_permutation(indices)) {
    if(rotations.count(indices) == 0) {
      return true;
    }
  }

  return false;
}

Shape mostSymmetric(std::vector<Shape> selection) {
  std::sort(
    std::begin(selection),
    std::end(selection),
    [](const Shape a, const Shape b) -> bool {
      return std::make_tuple(
        rotations(a).size(),
        nameIndex(b)
      ) < std::make_tuple(
        rotations(b).size(),
        nameIndex(a)
      );
    }
  );

  return selection.back();
}

Shape mostSymmetric(const unsigned shapeSize) {
  std::vector<Shape> propositions;
  propositions.reserve(8);

  for(const Shape proposition : allShapes) {
    if(size(proposition) == shapeSize) {
      propositions.push_back(proposition);
    }
  }

  return mostSymmetric(std::move(propositions));
}

} // namespace Properties
} // namespace Shapes
} // namespace Molassembler
} // namespace Scine


#include <boost/test/unit_test.hpp>

#include <boost/filesystem.hpp>

#include "fhmdot/include/yofi.hpp"

using namespace fhmdot::yofi;

BOOST_AUTO_TEST_CASE(unittest_yofi_tuple) {

  boost::filesystem::path temp = boost::filesystem::temp_directory_path() /
                                 boost::filesystem::unique_path();
  const std::string tempstr = temp.native(); // optional
  // std::cout << "file path : " << tempstr << std::endl;

  {
    std::tuple<char, float, int> tout{'A', 1.5f, 6};
    std::tuple<char, float, int> tin;

    std::ofstream out;
    out.open(tempstr);
    write_tuple(out, tout); // write tuple to file
    out.close();
    std::ifstream in;
    in.open(tempstr);
    read_tuple(in, tin); // read the tuple from file
    std::cout << "  - Test1, tuple: (";
    print_tuple(tin); // print it out
    std::cout << ")" << std::endl;
    in.close();

    BOOST_CHECK(tout == tin);
    boost::filesystem::remove(temp);
  }
  {
    std::tuple<uint16_t, float, uint64_t, int> tout{5, 1.5f, 87, -5};
    std::tuple<uint16_t, float, uint64_t, int> tin;

    std::ofstream out(tempstr);
    write_tuple(out, tout); // write tuple to file
    out.flush();            // flush the file so we can read it immediately

    std::ifstream in(tempstr);
    read_tuple(in, tin); // read the tuple from file
    std::cout << "  - Test2, tuple: (";
    print_tuple(tin); // print it out
    std::cout << ")" << std::endl;

    BOOST_CHECK(tout == tin);
    boost::filesystem::remove(temp);
  }
}

#include <boost/mpl/list.hpp>
#include <complex>

typedef boost::mpl::list<float,
                         double> //, typename std::complex<float>> //,
                                 // std::complex<double>>
    values;

BOOST_AUTO_TEST_CASE_TEMPLATE(unittest_yofi_array, T, values) {

  boost::filesystem::path temp = boost::filesystem::temp_directory_path() /
                                 boost::filesystem::unique_path();
  const std::string tempstr = temp.native();
  // std::cout << "file path : " << tempstr << std::endl;

  {

    std::vector<T> dout{1, 3, 2, 4, 5};
    std::vector<T> din;

    std::ofstream out;
    out.open(tempstr);
    write_array(out, dout); // write tuple to file
    out.close();
    std::ifstream in(tempstr);
    read_array(in, din); // read the tuple from file
    std::cout << "  - Test*, data: (";
    print_array(din); // print it out
    std::cout << ")" << std::endl;
    in.close();

    BOOST_CHECK(dout == din);
    boost::filesystem::remove(temp);
  }
}