/*    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 <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/Aerodynamics/exponentialAtmosphere.h"

#if USE_NRLMSISE00
#include "Tudat/Astrodynamics/Aerodynamics/nrlmsise00Atmosphere.h"
#include "Tudat/Astrodynamics/Aerodynamics/nrlmsise00InputFunctions.h"
#endif
#include "Tudat/Astrodynamics/Aerodynamics/tabulatedAtmosphere.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/geodeticCoordinateConversions.h"
#include "Tudat/Astrodynamics/Ephemerides/approximatePlanetPositions.h"
#include "Tudat/Astrodynamics/Ephemerides/tabulatedEphemeris.h"
#include "Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h"
#include "Tudat/Astrodynamics/Ephemerides/itrsToGcrsRotationModel.h"
//#include "Tudat/Astrodynamics/EarthOrientation/earthOrientationCalculator.h"
#include "Tudat/Astrodynamics/Gravitation/centralGravityModel.h"
#include "Tudat/Astrodynamics/Gravitation/timeDependentSphericalHarmonicsGravityField.h"
#include "Tudat/Astrodynamics/Gravitation/basicSolidBodyTideGravityFieldVariations.h"
#include "Tudat/Basics/testMacros.h"
#include "Tudat/Astrodynamics/Ephemerides/synchronousRotationalEphemeris.h"

#if USE_CSPICE
#include "Tudat/External/SpiceInterface/spiceEphemeris.h"
#endif
#include "Tudat/Astrodynamics/Gravitation/triAxialEllipsoidGravity.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/InputOutput/matrixTextFileReader.h"
#include "Tudat/InputOutput/solarActivityData.h"
#include "Tudat/InputOutput/parseSolarActivityData.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"
#include "Tudat/Mathematics/Interpolators/lagrangeInterpolator.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createAtmosphereModel.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createEphemeris.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createGravityField.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"

namespace tudat
{

namespace unit_tests
{

using namespace simulation_setup;
using namespace input_output;
using namespace reference_frames;
using namespace basic_astrodynamics;

BOOST_AUTO_TEST_SUITE( test_environment_model_setup )

//! Test set up of atmosphere environment models.
BOOST_AUTO_TEST_CASE( test_atmosphereModelSetup )
{

    // Create settings for tabulated atmosphere.
    std::string atmosphereTableFile =
            tudat::input_output::getAtmosphereTablesPath( ) + "USSA1976Until100kmPer100mUntil1000kmPer1000m.dat";
    std::shared_ptr< TabulatedAtmosphereSettings > tabulatedAtmosphereSettings =
            std::make_shared< TabulatedAtmosphereSettings >( atmosphereTableFile );

    // Create settings for exponential atmosphere
    double densityScaleHeight = 8.0E3;
    double constantTemperature = 270.0;
    double densityAtZeroAltitude = 1.225;
    double specificGasConstant = 287.1;
    std::shared_ptr< ExponentialAtmosphereSettings > exponentialAtmosphereSettings =
            std::make_shared< ExponentialAtmosphereSettings >(
                densityScaleHeight, constantTemperature,
                densityAtZeroAltitude, specificGasConstant );

    // Create atmpshere models using setup function
    std::shared_ptr< aerodynamics::AtmosphereModel > exponentialAtmosphere =
            createAtmosphereModel( exponentialAtmosphereSettings, "Earth" );
    std::shared_ptr< aerodynamics::AtmosphereModel > tabulatedAtmosphere =
            createAtmosphereModel( tabulatedAtmosphereSettings, "Earth" );

    // Create atmosphere models manually.
    aerodynamics::TabulatedAtmosphere manualTabulatedAtmosphere( atmosphereTableFile );
    aerodynamics::ExponentialAtmosphere manualExponentialAtmosphere(
                densityScaleHeight, constantTemperature, densityAtZeroAltitude,
                specificGasConstant );

    // Verify equivalence of automatically set up and manual models.
    BOOST_CHECK_EQUAL( manualTabulatedAtmosphere.getDensity( 32.0, 0.0, 0.0, 0.0 ),
                       tabulatedAtmosphere->getDensity( 32.0, 0.0, 0.0, 0.0 ) );
    BOOST_CHECK_EQUAL( manualTabulatedAtmosphere.getPressure( 32.0, 0.0, 0.0, 0.0 ),
                       tabulatedAtmosphere->getPressure( 32.0, 0.0, 0.0, 0.0 ) );
    BOOST_CHECK_EQUAL( manualTabulatedAtmosphere.getTemperature( 32.0, 0.0, 0.0, 0.0 ),
                       tabulatedAtmosphere->getTemperature( 32.0, 0.0, 0.0, 0.0 ) );

    BOOST_CHECK_EQUAL( manualExponentialAtmosphere.getDensity( 32.0, 0.0, 0.0, 0.0 ),
                       exponentialAtmosphere->getDensity( 32.0, 0.0, 0.0, 0.0 ) );
    BOOST_CHECK_EQUAL( manualExponentialAtmosphere.getPressure( 32.0, 0.0, 0.0, 0.0 ),
                       exponentialAtmosphere->getPressure( 32.0, 0.0, 0.0, 0.0 ) );
    BOOST_CHECK_EQUAL( manualExponentialAtmosphere.getTemperature( 32.0, 0.0, 0.0, 0.0 ),
                       exponentialAtmosphere->getTemperature( 32.0, 0.0, 0.0, 0.0 ) );

#if USE_NRLMSISE00
    std::shared_ptr< AtmosphereSettings > nrlmsise00AtmosphereSettings;
    for( int atmosphereTest = 0; atmosphereTest < 2; atmosphereTest++ )
    {
        if( atmosphereTest == 0 )
        {
            nrlmsise00AtmosphereSettings =
                    std::make_shared< AtmosphereSettings >( nrlmsise00 );
        }
        else
        {
            nrlmsise00AtmosphereSettings =
                    std::make_shared< NRLMSISE00AtmosphereSettings >(
                        input_output::getSpaceWeatherDataPath( ) + "sw19571001.txt" );
        }
        std::shared_ptr< aerodynamics::AtmosphereModel > nrlmsiseAtmosphere =
                createAtmosphereModel( nrlmsise00AtmosphereSettings, "Earth" );

        // Compute properties using NRLMSISE00
        double julianDaysSinceJ2000 = convertCalendarDateToJulianDay( 2005, 5, 3, 12, 32, 32.3 ) -
                basic_astrodynamics::JULIAN_DAY_ON_J2000;
        nrlmsiseAtmosphere->getDensity( 150.0E3, 1.0, 0.1, julianDaysSinceJ2000 * physical_constants::JULIAN_DAY );

        // Check if input to NRLMSISE00 is correctly computed (actual density computations tested in dedicated test).
        aerodynamics::NRLMSISE00Input nrlMSISE00Input = std::dynamic_pointer_cast< aerodynamics::NRLMSISE00Atmosphere >(
                    nrlmsiseAtmosphere )->getNRLMSISE00Input( );
        BOOST_CHECK_EQUAL( nrlMSISE00Input.year, 2005 );
        BOOST_CHECK_EQUAL( nrlMSISE00Input.dayOfTheYear, 31 + 28 + 31 + 30 + 3 );
        BOOST_CHECK_SMALL( nrlMSISE00Input.secondOfTheDay - ( 12.0 * 3600.0 + 32.0 * 60.0 + 32.3 ), 1.0E-3 );

        BOOST_CHECK_SMALL( nrlMSISE00Input.f107 - 112.3, 1.0E-14 );
        BOOST_CHECK_SMALL( nrlMSISE00Input.f107a - 93.3, 1.0E-14 );
        BOOST_CHECK_SMALL( nrlMSISE00Input.apDaily - 9.0, 1.0E-14 );
    }
#endif
}

Eigen::Vector6d computeCustomState(
        const double time, const double angularVelocity, const double radius, const double speed )
{
    Eigen::Vector6d currentState = Eigen::Vector6d::Zero( );
    currentState( 0 ) = radius * cos( angularVelocity * time );
    currentState( 1 ) = radius * sin( angularVelocity * time );

    currentState( 3 ) = -speed * sin( angularVelocity * time );
    currentState( 4 ) = speed * cos( angularVelocity * time );

    return currentState;

}

#if USE_CSPICE
//! Test set up of ephemeris environment models.
BOOST_AUTO_TEST_CASE( test_ephemerisSetup )
{
    spice_interface::loadStandardSpiceKernels( );

    {
        // Create settings for approximate planet positions.
        ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData bodyIdentifier =
                ephemerides::ApproximatePlanetPositionsBase::mars;
        bool useCircularCoplanarApproximation = 0;
        std::shared_ptr< ApproximatePlanetPositionSettings > approximateEphemerisSettings =
                std::make_shared< ApproximatePlanetPositionSettings >(
                    bodyIdentifier, useCircularCoplanarApproximation );

        // Create ephemeris using setup function.
        std::shared_ptr< ephemerides::Ephemeris > approximateEphemeris =
                createBodyEphemeris( approximateEphemerisSettings, "Earth" );

        // Create manual ephemeris.
        ephemerides::ApproximatePlanetPositions manualApproximateEphemeris(
                    bodyIdentifier );

        // Verify equivalence of automatically set up and manual models.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    ( manualApproximateEphemeris.getCartesianState( 1.0E7 ) ),
                    ( approximateEphemeris->getCartesianState( 1.0E7 ) ),
                    std::numeric_limits< double >::epsilon( ) );
    }

    {
        // Create spice ephemeris.
        std::shared_ptr< EphemerisSettings > spiceEphemerisSettings =
                std::make_shared< DirectSpiceEphemerisSettings >( "Earth", "J2000" );
        std::shared_ptr< ephemerides::Ephemeris > spiceEphemeris =
                createBodyEphemeris( spiceEphemerisSettings, "Moon" );

        // Compare spice ephemeris against direct spice state.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    ( spice_interface::getBodyCartesianStateAtEpoch(
                          "Moon", "Earth", "J2000", "None", 1.0E7 ) ),
                    ( spiceEphemeris->getCartesianState( 1.0E7 ) ),
                    std::numeric_limits< double >::epsilon( ) );
    }

    {
        // Create custom ephemeris
        double angularVelocity = 2.0 * tudat::mathematical_constants::PI / ( 2.0 * 3600.0 );
        double radius = 8000.0E3;
        double speed = 5000.0;

        std::shared_ptr< EphemerisSettings > customEphemerisSettings =
                std::make_shared< CustomEphemerisSettings >(
                    std::bind( &computeCustomState, std::placeholders::_1,  angularVelocity, radius, speed ),
                    "Earth", "J2000" );
        std::shared_ptr< ephemerides::Ephemeris > customEphemeris =
                createBodyEphemeris( customEphemerisSettings, "Satellite" );

        double testTime = 4.0E8;

        Eigen::Vector6d currentState = customEphemeris->getCartesianState(
                    testTime );
        Eigen::Vector6d currentTestState = computeCustomState(
                    testTime, angularVelocity, radius, speed );

        BOOST_CHECK_CLOSE_FRACTION( currentState( 0 ), currentTestState( 0 ), 2.0 * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( currentState( 1 ), currentTestState( 1 ), 2.0 * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL( currentState( 2 ),  2.0 * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( currentState( 3 ), currentTestState( 3 ), 2.0 * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( currentState( 4 ), currentTestState( 4 ), 2.0 * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL( currentState( 5 ), 2.0 * std::numeric_limits< double >::epsilon( ) );

    }

    {
        // Create tabulated spice ephemeris
        std::shared_ptr< EphemerisSettings > spiceEphemerisSettings =
                std::make_shared< InterpolatedSpiceEphemerisSettings >(
                    1.0E7 - 50.0 * 600.0, 1.0E7 + 50.0 * 600.0, 600.0, "Earth", "J2000" );
        std::shared_ptr< ephemerides::Ephemeris > spiceEphemeris =
                createBodyEphemeris( spiceEphemerisSettings, "Moon" );

        // Compare tabulated spice ephemeris against direct spice state on node point.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    ( spice_interface::getBodyCartesianStateAtEpoch(
                          "Moon", "Earth", "J2000", "None", 1.0E7 ) ),
                    ( spiceEphemeris->getCartesianState( 1.0E7 ) ),
                    std::numeric_limits< double >::epsilon( ) );

        // Manually create table of states from spice
        std::map< double, Eigen::Vector6d > tabulatedStates;
        double currentTime = 1.0E7 - 50.0 * 600.0;
        while( currentTime <= 1.0E7 + 50.0 * 600.0 )
        {
            tabulatedStates[ currentTime ] = spice_interface::getBodyCartesianStateAtEpoch(
                        "Moon", "Earth", "J2000", "None", currentTime );
            currentTime += 600.0;
        }

        // Create tabulated ephemeris.
        std::shared_ptr< EphemerisSettings > tabulatedEphemerisSettings =
                std::make_shared< TabulatedEphemerisSettings >(
                    tabulatedStates, "Earth", "J2000" );
        std::shared_ptr< ephemerides::Ephemeris > tabulatedEphemeris =
                createBodyEphemeris( tabulatedEphemerisSettings, "Moon" );

        // Manually create tabulated ephemeris.
        std::shared_ptr< ephemerides::Ephemeris > manualTabulatedEphemeris =
                std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                    std::make_shared< interpolators::LagrangeInterpolator
                    < double, Eigen::Vector6d > >( tabulatedStates, 6 ),
                    "Earth", "J2000" );

        // Compare ephemerides away from node point.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    ( spiceEphemeris->getCartesianState( 1.0E7 + 110.0 ) ),
                    ( tabulatedEphemeris->getCartesianState( 1.0E7 + 110.0 ) ),
                    std::numeric_limits< double >::epsilon( ) );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    ( spiceEphemeris->getCartesianState( 1.0E7 + 110.0 ) ),
                    ( manualTabulatedEphemeris->getCartesianState( 1.0E7 + 110.0 ) ),
                    std::numeric_limits< double >::epsilon( ) );
    }

}
#endif

#if USE_CSPICE
//! Test set up of gravity field model environment models.
BOOST_AUTO_TEST_CASE( test_gravityFieldSetup )
{
    // Load Spice kernel
    spice_interface::loadStandardSpiceKernels( );

    // Create settings for spice central gravity field model.
    std::shared_ptr< GravityFieldSettings > spiceCentralGravityFieldSettings =
            std::make_shared< GravityFieldSettings >( central_spice );

    // Create spice central gravity field model from setup function.
    std::shared_ptr< gravitation::GravityFieldModel > spiceCentralGravityField =
            createGravityFieldModel( spiceCentralGravityFieldSettings, "Venus" );

    // Check correct creation of gravity field.
    BOOST_CHECK_EQUAL(
                ( spice_interface::getBodyGravitationalParameter( "Venus" ) ),
                ( spiceCentralGravityField->getGravitationalParameter( ) ) );

    // Settings for spherical harmonic acceleration.
    double gravitationalParameter = 398600.4418E9;
    Eigen::Vector3d testPosition( 7.0e6, 8.0e6, 9.0e6 );
    Eigen::MatrixXd cosineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              -4.841651437908150e-4, -2.066155090741760e-10, 2.439383573283130e-6, 0.0, 0.0, 0.0,
              9.571612070934730e-7, 2.030462010478640e-6, 9.047878948095281e-7,
              7.213217571215680e-7, 0.0, 0.0, 5.399658666389910e-7, -5.361573893888670e-7,
              3.505016239626490e-7, 9.908567666723210e-7, -1.885196330230330e-7, 0.0,
              6.867029137366810e-8, -6.292119230425290e-8, 6.520780431761640e-7,
              -4.518471523288430e-7, -2.953287611756290e-7, 1.748117954960020e-7
              ).finished( );
    Eigen::MatrixXd sineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 1.384413891379790e-9, -1.400273703859340e-6, 0.0, 0.0, 0.0,
              0.0, 2.482004158568720e-7, -6.190054751776180e-7, 1.414349261929410e-6, 0.0, 0.0,
              0.0, -4.735673465180860e-7, 6.624800262758290e-7, -2.009567235674520e-7,
              3.088038821491940e-7, 0.0, 0.0, -9.436980733957690e-8, -3.233531925405220e-7,
              -2.149554083060460e-7, 4.980705501023510e-8, -6.693799351801650e-7
              ).finished( );

    // Create settings for central gravity field.
    std::shared_ptr< CentralGravityFieldSettings > centralGravityFieldSettings =
            std::make_shared< CentralGravityFieldSettings >( gravitationalParameter );

    // Create central gravity field with setup function.
    std::shared_ptr< gravitation::GravityFieldModel > centralGravityField =
            createGravityFieldModel( centralGravityFieldSettings, "Earth" );

    // Create central gravity field manually.
    gravitation::GravityFieldModel manualCentralGravityField(
                gravitationalParameter );

    // Verify equivalence of automatically set up and manual models.
    BOOST_CHECK_EQUAL(
                ( manualCentralGravityField.getGravitationalParameter( ) ),
                ( centralGravityField->getGravitationalParameter( ) ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( manualCentralGravityField.getGradientOfPotential( testPosition ) ),
                ( centralGravityField->getGradientOfPotential( testPosition ) ),
                std::numeric_limits< double >::epsilon( ) );

    // Create settings for sh gravity field.
    std::shared_ptr< SphericalHarmonicsGravityFieldSettings > shGravityFieldSettings =
            std::make_shared< SphericalHarmonicsGravityFieldSettings >(
                gravitationalParameter, 6378.0E3, cosineCoefficients, sineCoefficients,
                "Earth_fixed" );

    // Create sh gravity field with setup function.
    std::shared_ptr< gravitation::GravityFieldModel > shGravityField =
            createGravityFieldModel( shGravityFieldSettings, "Earth" );

    // Create sh gravity field manually.
    gravitation::SphericalHarmonicsGravityField manualShGravityField(
                gravitationalParameter, 6378.0E3, cosineCoefficients, sineCoefficients );

    // Verify equivalence of automatically set up and manual models.
    BOOST_CHECK_EQUAL(
                ( manualShGravityField.getGravitationalParameter( ) ),
                ( shGravityField->getGravitationalParameter( ) ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( manualShGravityField.getGradientOfPotential( testPosition ) ),
                ( shGravityField->getGradientOfPotential( testPosition ) ),
                std::numeric_limits< double >::epsilon( ) );

    std::shared_ptr< gravitation::SphericalHarmonicsGravityField > defaultEarthField =
            std::dynamic_pointer_cast< gravitation::SphericalHarmonicsGravityField >(
                createGravityFieldModel( getDefaultGravityFieldSettings(
                                             "Earth", TUDAT_NAN, TUDAT_NAN ), "Earth" ) );
    BOOST_CHECK_EQUAL(
                ( defaultEarthField->getGravitationalParameter( ) ), ( 0.3986004418E15 ) );
    BOOST_CHECK_EQUAL(
                ( defaultEarthField->getReferenceRadius( ) ), ( 6378137.0 ) );
    BOOST_CHECK_EQUAL(
                ( defaultEarthField->getCosineCoefficients( ).rows( ) ), 51 );
    BOOST_CHECK_EQUAL(
                ( defaultEarthField->getCosineCoefficients( ).cols( ) ), 51 );
    BOOST_CHECK_EQUAL(
                ( defaultEarthField->getSineCoefficients( ).rows( ) ), 51 );
    BOOST_CHECK_EQUAL(
                ( defaultEarthField->getSineCoefficients( ).cols( ) ), 51 );
    BOOST_CHECK_EQUAL(
                ( defaultEarthField->getCosineCoefficients( )( 2, 0 ) ), -0.484165371736E-03 );
    BOOST_CHECK_EQUAL(
                ( defaultEarthField->getCosineCoefficients( )( 5, 3 ) ), -0.451955406071E-06 );
    BOOST_CHECK_EQUAL(
                ( defaultEarthField->getSineCoefficients( )( 7, 1 ) ), 0.954336911867E-07 );

    std::shared_ptr< gravitation::SphericalHarmonicsGravityField > defaultMoonField =
            std::dynamic_pointer_cast< gravitation::SphericalHarmonicsGravityField >(
                createGravityFieldModel( getDefaultGravityFieldSettings(
                                             "Moon", TUDAT_NAN, TUDAT_NAN ), "Moon" ) );
    BOOST_CHECK_EQUAL(
                ( defaultMoonField->getGravitationalParameter( ) ), ( 0.4902800238000000E+13 ) );
    BOOST_CHECK_EQUAL(
                ( defaultMoonField->getReferenceRadius( ) ), ( 0.17380E+07 ) );
    BOOST_CHECK_EQUAL(
                ( defaultMoonField->getCosineCoefficients( ).rows( ) ), 51 );
    BOOST_CHECK_EQUAL(
                ( defaultMoonField->getCosineCoefficients( ).cols( ) ), 51 );
    BOOST_CHECK_EQUAL(
                ( defaultMoonField->getSineCoefficients( ).rows( ) ), 51 );
    BOOST_CHECK_EQUAL(
                ( defaultMoonField->getSineCoefficients( ).cols( ) ), 51 );
    BOOST_CHECK_EQUAL(
                ( defaultMoonField->getCosineCoefficients( )( 5, 3 ) ), 0.5493176535439800E-06 );
    BOOST_CHECK_EQUAL(
                ( defaultMoonField->getSineCoefficients( )( 7, 1 ) ), -0.1744763377093700E-06 );

}

//! Test set up of triaxial ellipsoid gravity field model settings
BOOST_AUTO_TEST_CASE( test_triaxialEllipsoidGravityFieldSetup )
{
    double axisA = 26.8E3;
    double axisB = 22.4E3;
    double axisC = 18.4E3;

    double density = 2.7E3;

    std::shared_ptr< SphericalHarmonicsGravityFieldSettings > gravityFieldSettings =
            createHomogeneousTriAxialEllipsoidGravitySettings(
                axisA, axisB, axisC, density, 11, 11, "TestFrame" );

    // Check size of coefficient blocks
    BOOST_CHECK_EQUAL( gravityFieldSettings->getCosineCoefficients( ).rows( ), 12 );
    BOOST_CHECK_EQUAL( gravityFieldSettings->getCosineCoefficients( ).cols( ), 12 );
    BOOST_CHECK_EQUAL( gravityFieldSettings->getSineCoefficients( ).rows( ), 12 );
    BOOST_CHECK_EQUAL( gravityFieldSettings->getSineCoefficients( ).cols( ), 12 );

    // Check frame ID
    BOOST_CHECK_EQUAL( gravityFieldSettings->getAssociatedReferenceFrame( ), "TestFrame"  );

    // Compute expected low-degree unnormalized coefficients
    double referenceRadius = gravitation::calculateTriAxialEllipsoidReferenceRadius( axisA, axisB, axisC );

    double expectedC20 = 1.0 / ( 5.0 * referenceRadius * referenceRadius ) *
            ( axisC * axisC - ( axisA * axisA + axisB * axisB ) / 2.0 );
    double expectedC22 = 1.0 / ( 20.0 * referenceRadius * referenceRadius ) *
            ( axisA * axisA - axisB * axisB );
    double expectedC40 = 15.0 / 7.0 * ( expectedC20 * expectedC20 +
                                        2.0 * expectedC22 * expectedC22 );
    double expectedC42 = 5.0 / 7.0 * ( expectedC20 * expectedC22 );
    double expectedC44 = 5.0 / 28.0 * ( expectedC22 * expectedC22 );

    // Test low-degree coefficients (normalizing them first)
    BOOST_CHECK_CLOSE_FRACTION(
                expectedC20, gravityFieldSettings->getCosineCoefficients( )( 2, 0 ) *
                basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 2, 0 ),
                2.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION(
                expectedC22, gravityFieldSettings->getCosineCoefficients( )( 2, 2 ) *
                basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 2, 2 ),
                2.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION(
                expectedC40, gravityFieldSettings->getCosineCoefficients( )( 4, 0 ) *
                basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 4, 0 ),
                2.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION(
                expectedC42, gravityFieldSettings->getCosineCoefficients( )( 4, 2 ) *
                basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 4, 2 ),
                2.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION(
                expectedC44, gravityFieldSettings->getCosineCoefficients( )( 4, 4 ) *
                basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 4, 4 ),
                2.0 * std::numeric_limits< double >::epsilon( ) );

    // Check whether C-coefficients with odd degree or order, and all S-coefficients, are zero.
    for( unsigned int i = 0; i < 12; i++ )
    {
        for( unsigned int j = 0; j < 12; j++ )
        {
            if( ( i % 2 != 0 ) || ( j % 2 ) != 0 )
            {
                BOOST_CHECK_EQUAL( gravityFieldSettings->getCosineCoefficients( )( i, j ), 0.0 );
            }
            BOOST_CHECK_EQUAL( gravityFieldSettings->getSineCoefficients( )( i, j ), 0.0 );
        }
    }

    double gravitationalParameter = 4.0 / 3.0 * mathematical_constants::PI * axisA * axisB * axisC *
            density * physical_constants::GRAVITATIONAL_CONSTANT;
    BOOST_CHECK_CLOSE_FRACTION(
                gravitationalParameter, gravityFieldSettings->getGravitationalParameter( ),
                2.0 * std::numeric_limits< double >::epsilon( ) );
}

#endif

#if USE_CSPICE
//! Test set up of gravity field model variations environment models.
BOOST_AUTO_TEST_CASE( test_gravityFieldVariationSetup )
{
    // Load Spice kernels
    spice_interface::loadStandardSpiceKernels( );

    // Settings for spherical harmonic acceleration.
    double gravitationalParameter = 398600.4418E9;
    double referenceRadius = 6378.0E3;
    Eigen::MatrixXd cosineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              -4.841651437908150e-4, -2.066155090741760e-10, 2.439383573283130e-6, 0.0, 0.0, 0.0,
              9.571612070934730e-7, 2.030462010478640e-6, 9.047878948095281e-7,
              7.213217571215680e-7, 0.0, 0.0, 5.399658666389910e-7, -5.361573893888670e-7,
              3.505016239626490e-7, 9.908567666723210e-7, -1.885196330230330e-7, 0.0,
              6.867029137366810e-8, -6.292119230425290e-8, 6.520780431761640e-7,
              -4.518471523288430e-7, -2.953287611756290e-7, 1.748117954960020e-7
              ).finished( );
    Eigen::MatrixXd sineCoefficients =
            ( Eigen::MatrixXd( 6, 6 ) <<
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
              0.0, 1.384413891379790e-9, -1.400273703859340e-6, 0.0, 0.0, 0.0,
              0.0, 2.482004158568720e-7, -6.190054751776180e-7, 1.414349261929410e-6, 0.0, 0.0,
              0.0, -4.735673465180860e-7, 6.624800262758290e-7, -2.009567235674520e-7,
              3.088038821491940e-7, 0.0, 0.0, -9.436980733957690e-8, -3.233531925405220e-7,
              -2.149554083060460e-7, 4.980705501023510e-8, -6.693799351801650e-7
              ).finished( );

    // Define body settings.
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Moon" ] = getDefaultSingleBodySettings( "Moon", 0.0, 1.0E7 );
    bodySettings[ "Sun" ] = getDefaultSingleBodySettings( "Sun", 0.0, 1.0E7 );
    bodySettings[ "Earth" ] = getDefaultSingleBodySettings( "Earth", 0.0, 1.0E7 );

    bodySettings[ "Earth" ]->gravityFieldSettings = std::make_shared< SphericalHarmonicsGravityFieldSettings >(
                gravitationalParameter, referenceRadius, cosineCoefficients, sineCoefficients, "IAU_Earth" );

    // Define settings for tidal variations.
    double loveNumber = 0.25;
    std::vector< std::vector< std::complex< double > > > fullLoveNumberVector =
            gravitation::getFullLoveNumbersVector( loveNumber, 3, 2 );
    double testTime = 0.5E7;

    Eigen::MatrixXd cosineCorrections1, cosineCorrections2, cosineCorrections3;
    Eigen::MatrixXd sineCorrections1, sineCorrections2, sineCorrections3;

    // Calculate non-interpolated corrections from two bodies in single object.
    {
        // Define correction settings
        std::vector< std::string > deformingBodies;
        deformingBodies.push_back( "Moon" );
        deformingBodies.push_back( "Sun" );

        bodySettings[ "Earth" ]->gravityFieldVariationSettings.push_back(
                    std::make_shared< BasicSolidBodyGravityFieldVariationSettings >(
                        deformingBodies, fullLoveNumberVector, referenceRadius ) );

        // Create bodies
        NamedBodyMap bodyMap = createBodies( bodySettings );

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

        // Update states.
        bodyMap[ "Earth" ]->setStateFromEphemeris( testTime );
        bodyMap[ "Earth" ]->setCurrentRotationalStateToLocalFrameFromEphemeris( testTime );
        bodyMap[ "Sun" ]->setStateFromEphemeris( testTime );
        bodyMap[ "Moon" ]->setStateFromEphemeris( testTime );

        // Update gravity field
        std::shared_ptr< gravitation::TimeDependentSphericalHarmonicsGravityField > earthGravityField =
                std::dynamic_pointer_cast< gravitation::TimeDependentSphericalHarmonicsGravityField >(
                    bodyMap[ "Earth" ]->getGravityFieldModel( ) );
        earthGravityField->update( testTime );

        // Retrieve corrections.
        cosineCorrections1 = earthGravityField->getCosineCoefficients( ) - cosineCoefficients;
        sineCorrections1 = earthGravityField->getSineCoefficients( ) - sineCoefficients;

    }

    // Calculate non-interpolated corrections from two bodies in separate objects.
    {
        bodySettings[ "Earth" ]->gravityFieldVariationSettings.clear( );

        // Define correction settings
        std::vector< std::string > deformingBodies;
        deformingBodies.push_back( "Moon" );
        bodySettings[ "Earth" ]->gravityFieldVariationSettings.push_back(
                    std::make_shared< BasicSolidBodyGravityFieldVariationSettings >(
                        deformingBodies, fullLoveNumberVector, referenceRadius ) );

        deformingBodies.clear( );
        deformingBodies.push_back( "Sun" );
        bodySettings[ "Earth" ]->gravityFieldVariationSettings.push_back(
                    std::make_shared< BasicSolidBodyGravityFieldVariationSettings >(
                        deformingBodies, fullLoveNumberVector, referenceRadius ) );

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

        // Update states.
        bodyMap[ "Earth" ]->setStateFromEphemeris( testTime );
        bodyMap[ "Earth" ]->setCurrentRotationalStateToLocalFrameFromEphemeris( testTime );
        bodyMap[ "Sun" ]->setStateFromEphemeris( testTime );
        bodyMap[ "Moon" ]->setStateFromEphemeris( testTime );

        // Update gravity field
        std::shared_ptr< gravitation::TimeDependentSphericalHarmonicsGravityField > earthGravityField =
                std::dynamic_pointer_cast< gravitation::TimeDependentSphericalHarmonicsGravityField >(
                    bodyMap[ "Earth" ]->getGravityFieldModel( ) );
        earthGravityField->update( testTime );

        // Retrieve corrections.
        cosineCorrections2 = earthGravityField->getCosineCoefficients( ) - cosineCoefficients;
        sineCorrections2 = earthGravityField->getSineCoefficients( ) - sineCoefficients;
    }

    {
        bodySettings[ "Earth" ]->gravityFieldVariationSettings.clear( );

        // Define correction settings
        std::vector< std::string > deformingBodies;
        deformingBodies.push_back( "Moon" );
        deformingBodies.push_back( "Sun" );
        bodySettings[ "Earth" ]->gravityFieldVariationSettings.push_back(
                    std::make_shared< BasicSolidBodyGravityFieldVariationSettings >(
                        deformingBodies, fullLoveNumberVector, referenceRadius,
                        std::make_shared< ModelInterpolationSettings >(
                            0.25E7, 0.75E7, 600.0,
                            std::make_shared< interpolators::LagrangeInterpolatorSettings >( 8 ) ) ) );

        // Create bodies
        NamedBodyMap bodyMap = createBodies( bodySettings );

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

        // Update gravity field
        std::shared_ptr< gravitation::TimeDependentSphericalHarmonicsGravityField > earthGravityField =
                std::dynamic_pointer_cast< gravitation::TimeDependentSphericalHarmonicsGravityField >(
                    bodyMap[ "Earth" ]->getGravityFieldModel( ) );

        earthGravityField->update( testTime );

        // Retrieve corrections.
        cosineCorrections3 = earthGravityField->getCosineCoefficients( ) - cosineCoefficients;
        sineCorrections3= earthGravityField->getSineCoefficients( ) - sineCoefficients;

    }

    // Mutually compare results of three methods.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( cosineCorrections1.block( 2, 0, 2, 3 ),  cosineCorrections2.block( 2, 0, 2, 3 ), 1.0E-10 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( cosineCorrections1.block( 2, 0, 2, 3 ),  cosineCorrections3.block( 2, 0, 2, 3 ), 1.0E-10 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( sineCorrections1.block( 2, 1, 2, 3 ),  sineCorrections2.block( 2, 1, 2, 3 ), 1.0E-10 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( sineCorrections1.block( 2, 1, 2, 3 ),  sineCorrections2.block( 2, 1, 2, 3 ), 1.0E-10 );

    // Check whether 0 coefficients are actually 0.
    for( unsigned int i = 0; i < 6; i++ )
    {
        for( unsigned int j = 0; j < 6; j++ )
        {
            if( i < 2 || i > 3 || j > 2 )
            {
                BOOST_CHECK_EQUAL( cosineCorrections1( i, j ), 0.0 );
                BOOST_CHECK_EQUAL( cosineCorrections2( i, j ), 0.0 );
                BOOST_CHECK_EQUAL( cosineCorrections3( i, j ), 0.0 );

                BOOST_CHECK_EQUAL( sineCorrections1( i, j ), 0.0 );
                BOOST_CHECK_EQUAL( sineCorrections2( i, j ), 0.0 );
                BOOST_CHECK_EQUAL( sineCorrections3( i, j ), 0.0 );
            }
            else if( j == 0 )
            {
                BOOST_CHECK_EQUAL( sineCorrections1( i, j ), 0.0 );
                BOOST_CHECK_EQUAL( sineCorrections2( i, j ), 0.0 );
                BOOST_CHECK_EQUAL( sineCorrections3( i, j ), 0.0 );
            }

        }
    }

    // Calculate corrections manually and compare against created results.
    std::pair< Eigen::MatrixXd, Eigen::MatrixXd > directMoonTide =
            gravitation::calculateSolidBodyTideSingleCoefficientSetCorrectionFromAmplitude(
                fullLoveNumberVector, 0.4902800238000000E+13 / gravitationalParameter,
                referenceRadius,  spice_interface::getBodyCartesianPositionAtEpoch(
                    "Moon", "Earth", "IAU_Earth", "None", testTime ), 3, 2 );
    std::pair< Eigen::MatrixXd, Eigen::MatrixXd > directSunTide =
            gravitation::calculateSolidBodyTideSingleCoefficientSetCorrectionFromAmplitude(
                fullLoveNumberVector, spice_interface::getBodyGravitationalParameter( "Sun" ) / gravitationalParameter,
                referenceRadius,  spice_interface::getBodyCartesianPositionAtEpoch(
                    "Sun", "Earth", "IAU_Earth", "None", testTime ), 3, 2 );

    for( unsigned int n = 2; n <= 3; n++ )
    {
        for( unsigned m = 0; m <=2; m++ )
        {
            BOOST_CHECK_SMALL( directMoonTide.first( n, m ) + directSunTide.first( n, m ) - cosineCorrections1( n, m ), 1.0E-18 );
            BOOST_CHECK_SMALL( directMoonTide.second( n, m ) + directSunTide.second( n, m ) - sineCorrections1( n, m ), 1.0E-18 );
        }
    }
}
#endif

#if USE_CSPICE
//! Test set up of rotation models.
BOOST_AUTO_TEST_CASE( test_rotationModelSetup )
{

    // Create settings for simple rotation model.
    Eigen::Matrix3d spiceInitialRotationToTargetFrameMatrix;
    spiceInitialRotationToTargetFrameMatrix
            << -0.9548214974296336, 0.2665104385944917, 0.1314841974018291,
            -0.296591573568662, -0.882413772579987, -0.3652114078848295,
            0.01869081416890206, -0.3877088083617987, 0.9215923900425707;
    double venusRotationRate = unit_conversions::convertDegreesToRadians( -1.4813688 ) /
            physical_constants::JULIAN_DAY;
    std::shared_ptr< SimpleRotationModelSettings > simpleRotationSettings =
            std::make_shared< SimpleRotationModelSettings >
            ( "IAU_VENUS", "J2000", Eigen::Quaterniond( spiceInitialRotationToTargetFrameMatrix ),
              1.0E7, venusRotationRate );

    // Create rotation model using setup function
    std::shared_ptr< ephemerides::RotationalEphemeris > approximateEphemeris =
            createRotationModel( simpleRotationSettings, "Earth" );

    // Create rotation model manually.
    ephemerides::SimpleRotationalEphemeris manualApproximateEphemeris(
                Eigen::Quaterniond( spiceInitialRotationToTargetFrameMatrix ),
                venusRotationRate, 1.0E7,
                "J2000", "IAU_VENUS" );

    // Verify equivalence of automatically set up and manual models.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( Eigen::Matrix3d( manualApproximateEphemeris.getRotationToBaseFrame(
                                       4.0E7) ) ),
                ( Eigen::Matrix3d( approximateEphemeris->getRotationToBaseFrame(
                                       4.0E7) ) ),
                std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( Eigen::Matrix3d( manualApproximateEphemeris.getRotationToTargetFrame(
                                       4.0E7) ) ),
                ( Eigen::Matrix3d( approximateEphemeris->getRotationToTargetFrame(
                                       4.0E7) ) ),
                std::numeric_limits< double >::epsilon( ) );

}
#endif

#if USE_CSPICE
//! Test set up of synchronous rotation model.
BOOST_AUTO_TEST_CASE( test_synchronousRotationModelSetup )
{

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

    // Create settings for synchronous rotation model.
    std::vector< std::string > bodiesToCreate;
    bodiesToCreate.push_back( "Europa" );
    bodiesToCreate.push_back( "Jupiter" );

    // Create bodies needed in simulation
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( bodiesToCreate );

    bodySettings[ "Europa" ]->rotationModelSettings = std::make_shared< SynchronousRotationModelSettings >( "Jupiter", "ECLIPJ2000", "IAU_Europa" );
    bodySettings[ "Europa" ]->ephemerisSettings->resetFrameOrigin( "Jupiter" );

    NamedBodyMap bodyMap = createBodies( bodySettings );
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    std::shared_ptr< SynchronousRotationModelSettings > synchronousRotationSettings
            = std::dynamic_pointer_cast< SynchronousRotationModelSettings >( bodySettings[ "Europa" ]->rotationModelSettings );

    // Create rotation model using setup function
    std::shared_ptr< ephemerides::RotationalEphemeris > rotationalEphemerisFromRotationModelSettings =
            createRotationModel( synchronousRotationSettings, "Europa", bodyMap );

    // Create synchronous rotation model directly.
    std::function< Eigen::Vector6d( const double, bool ) > relativeStateFunction = createRelativeStateFunction( bodyMap, "Europa", "Jupiter");

    ephemerides::SynchronousRotationalEphemeris synchronousRotationalEphemeris( relativeStateFunction, "Jupiter", "ECLIPJ2000", "IAU_Europa" );

    // Verify equivalence of automatically set up and manual models.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( Eigen::Matrix3d( synchronousRotationalEphemeris.getRotationToBaseFrame(
                                       4.0E7) ) ),
                ( Eigen::Matrix3d( rotationalEphemerisFromRotationModelSettings->getRotationToBaseFrame(
                                       4.0E7) ) ),
                std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                ( Eigen::Matrix3d( synchronousRotationalEphemeris.getRotationToTargetFrame(
                                       4.0E7) ) ),
                ( Eigen::Matrix3d( rotationalEphemerisFromRotationModelSettings->getRotationToTargetFrame(
                                       4.0E7) ) ),
                std::numeric_limits< double >::epsilon( ) );

}
#endif

#if USE_SOFA
//! Test set up of GCRS<->ITRS rotation model
BOOST_AUTO_TEST_CASE( test_earthRotationModelSetup )
{
    std::shared_ptr< GcrsToItrsRotationModelSettings > rotationSettings =
            std::make_shared< GcrsToItrsRotationModelSettings >( );

    // Create rotation model using setup function
    std::shared_ptr< tudat::ephemerides::RotationalEphemeris > earthRotationModel =
            createRotationModel( rotationSettings, "Earth" );

    double testTime = 5.0E7;

    std::shared_ptr< GcrsToItrsRotationModelSettings > gcrsToItrsRotationSettings2000a =
            std::make_shared< GcrsToItrsRotationModelSettings >( basic_astrodynamics::iau_2000_a );
    std::shared_ptr< GcrsToItrsRotationModelSettings > gcrsToItrsRotationSettings2000b =
            std::make_shared< GcrsToItrsRotationModelSettings >( basic_astrodynamics::iau_2000_b );

    std::shared_ptr< tudat::ephemerides::RotationalEphemeris > earthRotationModel2000a =
            createRotationModel( gcrsToItrsRotationSettings2000a, "Earth" );
    std::shared_ptr< tudat::ephemerides::RotationalEphemeris > earthRotationModel2000b =
            createRotationModel( gcrsToItrsRotationSettings2000b, "Earth" );

    Eigen::Quaterniond rotationMatrix = earthRotationModel->getRotationToBaseFrame( testTime );

    Eigen::Quaterniond rotationMatrix2000a = earthRotationModel2000a->getRotationToBaseFrame( testTime );
    Eigen::Matrix3d matrixDeviation2000a = rotationMatrix2000a.toRotationMatrix( ) - rotationMatrix.toRotationMatrix( );

    Eigen::Quaterniond rotationMatrix2000b = earthRotationModel2000b->getRotationToBaseFrame( testTime );
    Eigen::Matrix3d matrixDeviation2000b = rotationMatrix2000b.toRotationMatrix( ) - rotationMatrix.toRotationMatrix( );

    std::shared_ptr< tudat::ephemerides::GcrsToItrsRotationModel >  defaultEarthModel =
            std::make_shared< ephemerides::GcrsToItrsRotationModel >(
                tudat::earth_orientation::createStandardEarthOrientationCalculator( ) );

    Eigen::Quaterniond defaultRotationMatrix = defaultEarthModel->getRotationToBaseFrame( testTime );
    Eigen::Matrix3d matrixDeviation = defaultRotationMatrix.toRotationMatrix( ) - rotationMatrix.toRotationMatrix( );

    for( unsigned int i = 0; i < 3; i++ )
    {
        for( unsigned int j = 0; j < 3; j++ )
        {
            if( i < 2 && j < 2 )
            {
                BOOST_CHECK_SMALL( std::fabs( matrixDeviation2000a( i, j ) ), 1.0E-13 );
                BOOST_CHECK_SMALL( std::fabs( matrixDeviation2000b( i, j ) ), 1.0E-12 );
                BOOST_CHECK_SMALL( std::fabs( matrixDeviation( i, j ) ), std::numeric_limits< double >::epsilon( ) );
            }
            else
            {
                BOOST_CHECK_SMALL( std::fabs( matrixDeviation2000a( i, j ) ), 1.0E-10 );
                BOOST_CHECK_SMALL( std::fabs( matrixDeviation2000b( i, j ) ), 1.0E-8 );
                BOOST_CHECK_SMALL( std::fabs( matrixDeviation( i, j ) ), std::numeric_limits< double >::epsilon( ) );
            }
        }
    }
}
#endif

#if USE_CSPICE
//! Test set up of radiation pressure interfacel environment models.
BOOST_AUTO_TEST_CASE( test_radiationPressureInterfaceSetup )
{
    // Load Spice kernels
    spice_interface::loadStandardSpiceKernels( );

    // Define body settings.
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = getDefaultSingleBodySettings( "Earth", 0.0, 1.0E7 );
    bodySettings[ "Sun" ] = getDefaultSingleBodySettings( "Sun", 0.0, 1.0E7 );

    // Get settings for vehicle
    double area = 2.34;
    double coefficient = 1.2;
    Eigen::Vector6d initialKeplerElements =
            ( Eigen::Vector6d( ) << 12000.0E3, 0.13, 0.3, 0.0, 0.0, 0.0 ).finished( );
    bodySettings[ "Vehicle" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Vehicle" ]->radiationPressureSettings[ "Sun" ] =
            std::make_shared< CannonBallRadiationPressureInterfaceSettings >( "Sun", area, coefficient );
    bodySettings[ "Vehicle" ]->ephemerisSettings =
            std::make_shared< KeplerEphemerisSettings >(
                initialKeplerElements,
                0.0, spice_interface::getBodyGravitationalParameter( "Earth" ), "Earth", "ECLIPJ2000" );

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

    BOOST_CHECK_EQUAL( bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).size( ), 1 );
    BOOST_CHECK_EQUAL( bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).count( "Sun" ), 1 );

    double testTime = 0.5E7;

    // Update environment to current time.
    bodyMap[ "Sun" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Earth" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Vehicle" ]->setStateFromEphemeris< double, double >( testTime );

    std::shared_ptr< electro_magnetism::RadiationPressureInterface > vehicleRadiationPressureInterface =
            bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).at( "Sun" );

    vehicleRadiationPressureInterface->updateInterface( testTime );
    double sourceDistance = ( ( bodyMap[ "Vehicle" ]->getState( ) -  bodyMap[ "Sun" ]->getState( ) ).
            segment( 0, 3 ) ).norm( );
    double expectedRadiationPressure = electro_magnetism::calculateRadiationPressure(
                defaultRadiatedPowerValues.at( "Sun" ), sourceDistance );

    BOOST_CHECK_CLOSE_FRACTION( expectedRadiationPressure,
                                vehicleRadiationPressureInterface->getCurrentRadiationPressure( ),
                                std::numeric_limits< double >::epsilon( ) );

}
#endif

#if USE_CSPICE
//! Test set up of body shape environment models (see testShapeModels).
BOOST_AUTO_TEST_CASE( test_shapeModelSetup )
{
    // Define test values.
    const Eigen::Vector3d testCartesianPosition( 1917032.190, 6029782.349, -801376.113 );
    const double flattening = 1.0 / 298.257223563;
    const double equatorialRadius = 6378137.0;

    // Test spherical body setup
    {
        std::shared_ptr< BodyShapeSettings > shapeSettings = std::make_shared< SphericalBodyShapeSettings >(
                    equatorialRadius );
        std::shared_ptr< BodyShapeModel > shapeModel = createBodyShapeModel(
                    shapeSettings, "Earth" );

        double calculatedAltitude = shapeModel->getAltitude(
                    testCartesianPosition );
        BOOST_CHECK_CLOSE_FRACTION( calculatedAltitude, testCartesianPosition.norm( ) - equatorialRadius,
                                    std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION( shapeModel->getAverageRadius( ), equatorialRadius,
                                    std::numeric_limits< double >::epsilon( ) );
    }
    // Test oblate spheroid setup
    {
        std::shared_ptr< BodyShapeSettings > shapeSettings = std::make_shared< OblateSphericalBodyShapeSettings >(
                    equatorialRadius, flattening );
        std::shared_ptr< BodyShapeModel > shapeModel = createBodyShapeModel(
                    shapeSettings, "Earth" );

        double calculatedAltitude = shapeModel->getAltitude(
                    testCartesianPosition );
        double manualAltitude = coordinate_conversions::calculateAltitudeOverOblateSpheroid(
                    testCartesianPosition, equatorialRadius, flattening, 1.0E-4 );
        BOOST_CHECK_CLOSE_FRACTION( calculatedAltitude, manualAltitude,
                                    std::numeric_limits< double >::epsilon( ) );
    }

}
#endif

#if USE_CSPICE
//! Test set up of flight conditions object.
BOOST_AUTO_TEST_CASE( test_flightConditionsSetup )
{
    // Load Spice kernels
    spice_interface::loadStandardSpiceKernels( );

    // Define body settings/
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = getDefaultSingleBodySettings(
                "Earth", 0.0, 1.0E7 );
    bodySettings[ "Vehicle" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Vehicle" ] ->aerodynamicCoefficientSettings =
            std::make_shared< ConstantAerodynamicCoefficientSettings >(
                1.0, 2.0, 3.0, Eigen::Vector3d::Zero( ),
                ( Eigen::Vector3d( ) << -1.1, 0.1, 2.3 ).finished( ),
                Eigen::Vector3d::Zero( ), 1, 1 );

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

    // Define expected aerodynamic angles (see testAerodynamicAngleCalculator)
    double testHeadingAngle = 1.229357188236127;
    double testFlightPathAngle = -0.024894033070522;
    double testLatitude = -0.385027359562548;
    double testLongitude = -1.849449608688977;

    double angleOfAttack = 1.232;
    double angleOfSideslip = -0.00322;
    double bankAngle = 2.323432;

    // Create flight conditions object.
    std::shared_ptr< aerodynamics::FlightConditions > vehicleFlightConditions =
            createAtmosphericFlightConditions( bodyMap.at( "Vehicle" ), bodyMap.at( "Earth" ),
                                    "Vehicle", "Earth",
                                    [ & ]( ){ return angleOfAttack; },
                                    [ & ]( ){ return angleOfSideslip; },
                                    [ & ]( ){ return bankAngle; } );

    // Set vehicle body-fixed state (see testAerodynamicAngleCalculator)
    Eigen::Vector6d vehicleBodyFixedState =
            ( Eigen::Vector6d( ) << -1656517.23153109, -5790058.28764025, -2440584.88186829,
              6526.30784888051, -2661.34558272018, 2377.09572383163 ).finished( );

    // Set states in environment.
    double testTime = 0.5E7;
    Eigen::Vector6d vehicleInertialState =
            ephemerides::transformStateToFrameFromRotations(
                vehicleBodyFixedState,
                bodyMap[ "Earth" ]->getRotationalEphemeris( )->getRotationToBaseFrame( testTime ),
            bodyMap[ "Earth" ]->getRotationalEphemeris( )->getDerivativeOfRotationToBaseFrame( testTime ) );
    bodyMap[ "Earth" ]->setState( Eigen::Vector6d::Zero( ) );
    bodyMap[ "Vehicle" ]->setState( vehicleInertialState );
    bodyMap[ "Earth" ]->setCurrentRotationalStateToLocalFrameFromEphemeris( testTime );

    // Update flight conditions
    vehicleFlightConditions->updateConditions( testTime );

    // Check whether calulcate angles and body-fixed state correspond to expected results
    BOOST_CHECK_SMALL(
                std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )
                           ->getAerodynamicAngle( latitude_angle ) - testLatitude),
                10.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL(
                std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )
                           ->getAerodynamicAngle( longitude_angle ) - testLongitude),
                10.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL(
                std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )
                           ->getAerodynamicAngle( heading_angle ) - testHeadingAngle),
                10.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL(
                std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )
                           ->getAerodynamicAngle( flight_path_angle) - testFlightPathAngle),
                10.0 * std::numeric_limits< double >::epsilon( ) );

    BOOST_CHECK_SMALL(
                std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )
                           ->getAerodynamicAngle( angle_of_attack ) - angleOfAttack ),
                10.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL(
                std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )
                           ->getAerodynamicAngle( angle_of_sideslip) - angleOfSideslip),
                10.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL(
                std::fabs( vehicleFlightConditions->getAerodynamicAngleCalculator( )
                           ->getAerodynamicAngle( bank_angle ) - bankAngle),
                10.0 * std::numeric_limits< double >::epsilon( ) );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                vehicleFlightConditions->getCurrentBodyCenteredBodyFixedState( ),vehicleBodyFixedState,
                ( 2.0 * std::numeric_limits< double >::epsilon( ) ) );

}
#endif

//! Test set up of solar sailing radiation pressure interface environment models.
BOOST_AUTO_TEST_CASE( test_solarSailingRadiationPressureInterfaceSetup )
{

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

    // Define body settings.
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = getDefaultSingleBodySettings( "Earth", 0.0, 1.0E7 );
    bodySettings[ "Sun" ] = getDefaultSingleBodySettings( "Sun", 0.0, 1.0E7 );

    // Get settings for vehicle
    Eigen::Vector6d initialKeplerElements =
            ( Eigen::Vector6d( ) << 12000.0E3, 0.13, 0.3, 0.0, 0.0, 0.0 ).finished( );
    bodySettings[ "Vehicle" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Vehicle" ]->ephemerisSettings = std::make_shared< KeplerEphemerisSettings >(
                initialKeplerElements, 0.0, spice_interface::getBodyGravitationalParameter( "Earth" ), "Earth", "ECLIPJ2000" );

    // Define area subject to radiation pressure [m^2].
    double area = 2.0;

    // Set cone angle of the solar sail [rad].
    double coneAngle = 0.25;

    // Get cone angle of the solar sail [rad].
    std::function< double( const double ) > coneAngleFunction
            = [ = ]( const double ){ return coneAngle; };

    // Define clock angle of the solar sail [rad].
    double clockAngle = 0.2;

    // Get clock angle of the solar sail [rad].
    std::function< double( const double ) > clockAngleFunction
            = [ = ]( const double ){ return clockAngle; };

    // Define front emissivity coefficient of the solar sail [-].
    double frontEmissivityCoefficient = 0.4;

    // Define back emissivity coefficient of the solar sail [-].
    double backEmissivityCoefficient = 0.4;

    // Define front Lambertian coefficient of the solar sail [-].
    double frontLambertianCoefficient = 0.4;

    // Define back Lambertian coefficient of the solar sail [-].
    double backLambertianCoefficient = 0.4;

    // Define reflectivity coefficient of the solar sail [-].
    double reflectivityCoefficient = 0.3;

    // Define specular reflection coefficient of the solar sail [-].
    double specularReflectionCoefficient = 1.0;

    // Define central body.
    std::string centralBody = "Earth";

    // Create radiation pressure interface settings.
    std::shared_ptr< SolarSailRadiationInterfaceSettings > radiationPressureInterfaceSettings =
            std::make_shared< SolarSailRadiationInterfaceSettings >(
                "Sun", area, coneAngleFunction, clockAngleFunction, frontEmissivityCoefficient, backEmissivityCoefficient, frontLambertianCoefficient,
                backLambertianCoefficient, reflectivityCoefficient, specularReflectionCoefficient, std::vector< std::string >( ),
                centralBody );

    bodySettings[ "Vehicle" ]->radiationPressureSettings[ "Sun" ] = radiationPressureInterfaceSettings;

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

    BOOST_CHECK_EQUAL( bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).size( ), 1 );
    BOOST_CHECK_EQUAL( bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).count( "Sun" ), 1 );

    double testTime = 0.5E7;

    // Update environment to current time.
    bodyMap[ "Sun" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Earth" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Vehicle" ]->setStateFromEphemeris< double, double >( testTime );

    std::shared_ptr< electro_magnetism::RadiationPressureInterface > vehicleRadiationPressureInterface =
            bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).at( "Sun" );

    // Compute expected radiation pressure.
    vehicleRadiationPressureInterface->updateInterface( testTime );
    double sourceDistance = ( ( bodyMap[ "Vehicle" ]->getState( ) -  bodyMap[ "Sun" ]->getState( ) ).
            segment( 0, 3 ) ).norm( );

    double expectedRadiationPressure = electro_magnetism::calculateRadiationPressure(
                defaultRadiatedPowerValues.at( "Sun" ), sourceDistance );

    BOOST_CHECK_CLOSE_FRACTION( expectedRadiationPressure,
                                vehicleRadiationPressureInterface->getCurrentRadiationPressure( ),
                                std::numeric_limits< double >::epsilon( ) );

    std::shared_ptr< electro_magnetism::SolarSailingRadiationPressureInterface > vehicleSolarSailingRadiationPressureInterface
            = std::dynamic_pointer_cast< electro_magnetism::SolarSailingRadiationPressureInterface > ( vehicleRadiationPressureInterface );

    for ( int i = 0 ; i < 3 ; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( vehicleSolarSailingRadiationPressureInterface->getCentralBodyVelocity( )( )[ i ] -
                           bodyMap[ "Earth" ]->getState()[ i + 3 ] ), 1.0E-15 );

        BOOST_CHECK_SMALL( std::fabs( vehicleSolarSailingRadiationPressureInterface->getCurrentVelocityVector( )[ i ] -
                           ( bodyMap[ "Vehicle" ]->getState( ) - bodyMap[ "Earth" ]->getState( ) ).segment(3,3).normalized()[ i ] ), 1.0E-15 );

        BOOST_CHECK_SMALL( std::fabs( vehicleSolarSailingRadiationPressureInterface->getCurrentSolarVector( )[ i ] -
                           ( - bodyMap[ "Vehicle" ]->getState( ) + bodyMap[ "Sun" ]->getState( ) ).segment(0,3)[ i ] ), 1.0E-15 );

    }

}

BOOST_AUTO_TEST_CASE( test_groundStationCreation )
{
    using namespace unit_conversions;
    using namespace coordinate_conversions;

    const double flattening = 1.0 / 298.257223563;
    const double equatorialRadius = 6378137.0;

    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Earth" ]->shapeModelSettings = std::make_shared< OblateSphericalBodyShapeSettings >(
                equatorialRadius, flattening );

    Eigen::Vector3d testCartesianPosition( 1917032.190, 6029782.349, -801376.113 );
    Eigen::Vector3d testGeodeticPosition(
                -63.667,  convertDegreesToRadians( -7.26654999 ), convertDegreesToRadians( 72.36312094 ) );
    Eigen::Vector3d testSphericalPosition = coordinate_conversions::convertCartesianToSpherical(
                testCartesianPosition );
    testSphericalPosition( 1 ) = mathematical_constants::PI / 2.0 - testSphericalPosition( 1 );

    bodySettings[ "Earth" ]->groundStationSettings.push_back(
                std::make_shared< GroundStationSettings >( "Station1", testCartesianPosition, cartesian_position  ) );
    bodySettings[ "Earth" ]->groundStationSettings.push_back(
                std::make_shared< GroundStationSettings >( "Station2", testSphericalPosition, spherical_position ) );
    bodySettings[ "Earth" ]->groundStationSettings.push_back(
                std::make_shared< GroundStationSettings >( "Station3", testGeodeticPosition, geodetic_position ) );

    // Create bodies
    NamedBodyMap bodyMap = createBodies( bodySettings );

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

    BOOST_CHECK_EQUAL( bodyMap.at( "Earth" )->getGroundStationMap( ).count( "Station1" ), 1 );
    BOOST_CHECK_EQUAL( bodyMap.at( "Earth" )->getGroundStationMap( ).count( "Station2" ), 1 );
    BOOST_CHECK_EQUAL( bodyMap.at( "Earth" )->getGroundStationMap( ).count( "Station3" ), 1 );
    BOOST_CHECK_EQUAL( bodyMap.at( "Earth" )->getGroundStationMap( ).size( ), 3 );

    Eigen::Vector3d testPosition1 = bodyMap.at( "Earth" )->getGroundStationMap( ).at( "Station1" )->
            getNominalStationState( )->getNominalCartesianPosition( );
    Eigen::Vector3d testPosition2= bodyMap.at( "Earth" )->getGroundStationMap( ).at( "Station2" )->
            getNominalStationState( )->getNominalCartesianPosition( );
    Eigen::Vector3d testPosition3 = bodyMap.at( "Earth" )->getGroundStationMap( ).at( "Station3" )->
            getNominalStationState( )->getNominalCartesianPosition( );

    for( unsigned int i = 0; i < 3; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( testPosition1( i ) - testCartesianPosition( i ) ), 1.0E-9 );
        BOOST_CHECK_SMALL( std::fabs( testPosition2( i ) - testCartesianPosition( i ) ), 1.0E-8 );
        BOOST_CHECK_SMALL( std::fabs( testPosition3( i ) - testCartesianPosition( i ) ), 1.0E-3 );
    }
}

#if USE_CSPICE
//! Test set up of panelled radiation pressure interface environment models.
BOOST_AUTO_TEST_CASE( test_panelledRadiationPressureInterfaceSetup )
{

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

    // Define body settings.
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = getDefaultSingleBodySettings( "Earth", 0.0, 1.0E7 );
    bodySettings[ "Sun" ] = getDefaultSingleBodySettings( "Sun", 0.0, 1.0E7 );

    // Get settings for vehicle
    Eigen::Vector6d initialKeplerElements =
            ( Eigen::Vector6d( ) << 12000.0E3, 0.13, 0.3, 0.0, 0.0, 0.0 ).finished( );
    bodySettings[ "Vehicle" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Vehicle" ]->ephemerisSettings = std::make_shared< KeplerEphemerisSettings >(
                initialKeplerElements, 0.0, spice_interface::getBodyGravitationalParameter( "Earth" ), "Earth", "ECLIPJ2000" );

    // Create radiation pressure properties
    std::vector< double > areas;
    areas.push_back( 4.0 );
    areas.push_back( 6.0 );
    areas.push_back( 2.3 );
    areas.push_back( 2.3 );
    areas.push_back( 5.3 );
    areas.push_back( 2.7 );
    areas.push_back( 4.1 );
    areas.push_back( 2.7 );

    std::vector< double > emissivities;
    emissivities.push_back( 0.1 );
    emissivities.push_back( 0.0 );
    emissivities.push_back( 0.1 );
    emissivities.push_back( 0.1 );
    emissivities.push_back( 0.94 );
    emissivities.push_back( 0.1 );
    emissivities.push_back( 0.94 );
    emissivities.push_back( 0.1 );

    std::vector< double > diffuseReflectionCoefficients;
    diffuseReflectionCoefficients.push_back( 0.46 );
    diffuseReflectionCoefficients.push_back( 0.06 );
    diffuseReflectionCoefficients.push_back( 0.46 );
    diffuseReflectionCoefficients.push_back( 0.46 );
    diffuseReflectionCoefficients.push_back( 0.06 );
    diffuseReflectionCoefficients.push_back( 0.46 );
    diffuseReflectionCoefficients.push_back( 0.06 );
    diffuseReflectionCoefficients.push_back( 0.46 );

    std::vector< Eigen::Vector3d > panelSurfaceNormals;
    panelSurfaceNormals.push_back( Eigen::Vector3d::UnitZ( ) );
    panelSurfaceNormals.push_back( - Eigen::Vector3d::UnitZ( ) );
    panelSurfaceNormals.push_back( Eigen::Vector3d::UnitX( ) );
    panelSurfaceNormals.push_back( - Eigen::Vector3d::UnitX( ) );
    panelSurfaceNormals.push_back( Eigen::Vector3d::UnitY( ) );
    panelSurfaceNormals.push_back( Eigen::Vector3d::UnitY( ) );
    panelSurfaceNormals.push_back( - Eigen::Vector3d::UnitY( ) );
    panelSurfaceNormals.push_back( - Eigen::Vector3d::UnitY( ) );

    std::shared_ptr< PanelledRadiationPressureInterfaceSettings > radiationPressureInterfaceSettings =
            std::make_shared< PanelledRadiationPressureInterfaceSettings >(
                "Sun", emissivities, areas, diffuseReflectionCoefficients, panelSurfaceNormals );

    bodySettings[ "Vehicle" ]->radiationPressureSettings[ "Sun" ] = radiationPressureInterfaceSettings;

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

    BOOST_CHECK_EQUAL( bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).size( ), 1 );
    BOOST_CHECK_EQUAL( bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).count( "Sun" ), 1 );

    double testTime = 0.5E7;

    // Update environment to current time.
    bodyMap[ "Sun" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Earth" ]->setStateFromEphemeris< double, double >( testTime );
    bodyMap[ "Vehicle" ]->setStateFromEphemeris< double, double >( testTime );

    std::shared_ptr< electro_magnetism::RadiationPressureInterface > vehicleRadiationPressureInterface =
            bodyMap[ "Vehicle" ]->getRadiationPressureInterfaces( ).at( "Sun" );

    // Compute expected radiation pressure.
    vehicleRadiationPressureInterface->updateInterface( testTime );
    double sourceDistance = ( ( bodyMap[ "Vehicle" ]->getState( ) -  bodyMap[ "Sun" ]->getState( ) ).
            segment( 0, 3 ) ).norm( );

    double expectedRadiationPressure = electro_magnetism::calculateRadiationPressure(
                defaultRadiatedPowerValues.at( "Sun" ), sourceDistance );

    BOOST_CHECK_CLOSE_FRACTION( expectedRadiationPressure,
                                vehicleRadiationPressureInterface->getCurrentRadiationPressure( ),
                                std::numeric_limits< double >::epsilon( ) );

}
#endif

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <cstdio>
#include <cstdlib>
/// ROS
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
/// Opencv
#include <Eigen/Geometry>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
////our include
#include </usr/local/include/config_loader.h>
#include <geometry.h>
#include <klt_tracker.h>
#include <motion_estimator_ransac.h>
#include <reconstruction_visualizer.h>
#include <rgbd_loader.h>

using namespace std;
using namespace cv;

/// rgbd_rtk variables
KLTTracker tracker;
Intrinsics intr(0);
MotionEstimatorRANSAC motion_estimator(intr, 0.008, 0.8);
// others variables
ReconstructionVisualizer visualizer;
Eigen::Affine3f pose = Eigen::Affine3f::Identity();
Eigen::Affine3f trans = Eigen::Affine3f::Identity();
pcl::PointCloud<PointT>::Ptr prev_cloud(new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr curr_cloud(new pcl::PointCloud<PointT>);
string rgb_topic, depth_topic;
int i = 0;

void callback(const sensor_msgs::ImageConstPtr &msgRGB,
              const sensor_msgs::ImageConstPtr &msgD); // subscribing rgb and depth image
void motionEstimator(cv::Mat rgb, cv::Mat depth);      // motion estiomator
void minMaxDebug(Mat depth, Mat rgb, double &min, double &max);
void initRos(int argc, char **argv, string rgb_topic, string depth_topic);

int main(int argc, char **argv) {
    // pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
    string rgb_topic, depth_topic;
    ConfigLoader param_loader(
        "../../../src/ros_autonomous_robot/config_files/ConfigFile.yaml"); // Load config file param
    param_loader.checkAndGetString("rgb_topic", rgb_topic);
    param_loader.checkAndGetString("rgb_topic", depth_topic);

    initRos(argc, argv, rgb_topic, depth_topic);
    return 0;
}
/**
 * Ros Callback, listen to rgb and depth topic
 * @Params rgb and depth messages
 */
void callback(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD) {

    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    } catch (cv_bridge::Exception &e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrD;
    try {
        cv_ptrD = cv_bridge::toCvShare(msgD);
    } catch (cv_bridge::Exception &e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    // convert the depth image CV_32 to CV_16UC1
    Mat depth_unsigned_short;
    cv_ptrD->image.convertTo(depth_unsigned_short, CV_16UC1, 1000, 0.0);
    // Call motion estimator
    motionEstimator(cv_ptrRGB->image, depth_unsigned_short);
}
/**
 * Motion estimator using keypoints
 * @Params rgb and depth images as cv::Mat
 */
void motionEstimator(cv::Mat rgb, cv::Mat depth) {

    *curr_cloud = getPointCloud(rgb, depth, intr);
    // track feature points in current frame
    tracker.track(rgb);

    if (i > 0) { // Estimate motion between the current and the previous frame/point clouds
        trans =
            motion_estimator.estimate(tracker.prev_pts_, prev_cloud, tracker.curr_pts_, curr_cloud);
        pose = pose * trans;
    }

    for (size_t k = 0; k < tracker.curr_pts_.size(); k++) { // View tracked points
        cv::Point2i pt1 = tracker.prev_pts_[k];
        cv::Point2i pt2 = tracker.curr_pts_[k];
        cv::circle(rgb, pt1, 1, CV_RGB(255, 0, 0), -1);
        cv::circle(rgb, pt2, 3, CV_RGB(0, 0, 255), -1);
        cv::line(rgb, pt1, pt2, CV_RGB(0, 0, 255));
    }

    // 3D vizualization
    if (i == 0) visualizer.addReferenceFrame(pose, "origin");
    visualizer.addQuantizedPointCloud(curr_cloud, 0.3, pose);
    visualizer.viewReferenceFrame(pose);
    visualizer.viewPointCloud(curr_cloud, pose);
    // visualizer.viewQuantizedPointCloud(curr_cloud, 0.02, pose);
    visualizer.spinOnce();

    depth /= 5;
    cv::imshow("OPENCV_WINDOW_RGB", rgb);
    cv::imshow("OPENCV_WINDOW_DEPTH", depth);
    cv::waitKey(1);
    // Let the prev. cloud in the next frame be the current cloud
    *prev_cloud = *curr_cloud;
    i++;
}

/**
 * Get the min and max distance in a depth image
 *
 * @Param Depth image and rgb image as cv::Mat; min(insert as "infinite") and max(insert as 0)  as
 * reference
 * @Return min and max values as reference
 */
void minMaxDebug(Mat depth, Mat rgb, double &min, double &max) {

    max = depth.at<unsigned short>(0, 0);
    for (int i = 0; i < depth.rows; i++) {
        for (int j = 0; j < depth.cols; j++) {
            // if(depth.at<unsigned short>(i,j) >=25000 ){
            //  cv::circle(rgb, Point(j,i), 1, CV_RGB(255,0,0), -1);
            //}
            if (depth.at<unsigned short>(i, j) != depth.at<unsigned short>(i, j)) {
                continue;
            }
            if (depth.at<unsigned short>(i, j) == 0.0) {
                continue;
            }
            if (depth.at<unsigned short>(i, j) < min) {
                min = depth.at<unsigned short>(i, j);
            }

            if (depth.at<unsigned short>(i, j) > max) {
                max = depth.at<unsigned short>(i, j);
            }
        }
    }
}
void initRos(int argc, char **argv, string rgb_topic, string depth_topic) {
    ros::init(argc, argv, "motion_estimator"); // initializing ros
    ros::start();
    ros::NodeHandle nh;

    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, rgb_topic,
                                                            1); // subscribing to rgb topic
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, depth_topic,
                                                              1); // subscribing to depth topic
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image>
        MySyncPolicy; // defining which topics will be sync
    message_filters::Synchronizer<MySyncPolicy> sync(
        MySyncPolicy(20), rgb_sub,
        depth_sub); // sync rgb and depth topic in "MySyncPolicy(x)" miliseconds
    sync.registerCallback(boost::bind(&callback, _1, _2));

    ros::spin();
}


/*
Copyright (c) 2020 Naomasa Matsubayashi

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

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

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

#include <cstdint>
#include <cmath>
#include <array>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <iterator>
#include <charconv>
#include <random>
#include <boost/container/flat_map.hpp>
#include <boost/program_options.hpp>
#include <omp.h>
#include <nlohmann/json.hpp>
#include "ifm/setter.h"
#include "ifm/spectrum_image.h"
#include "ifm/load_monoral.h"
#include "ifm/bessel.h"
#include "ifm/fm.h"
#include "ifm/adam.h"
#include "ifm/2op.h"
#include "ifm/exp_match.h"
int main( int argc, char *argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("bessel,b", boost::program_options::value<std::string>()->default_value( "bessel.mp" ), "ベッセル関数近似係数")
    ("input,i", boost::program_options::value<std::string>(), "入力ファイル")
    ("resolution,r", boost::program_options::value<int>()->default_value(13),  "分解能")
    ("damped,d", boost::program_options::value<bool>()->default_value(false),  "減衰振動")
    ("note,n", boost::program_options::value<int>()->default_value(60), "音階")
    ("verbose,v", boost::program_options::value<bool>()->default_value(false), "詳細を表示");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") || !params.count("input") ) {
    std::cout << options << std::endl;
    return 0;
  }

  const auto [audio,sample_rate] = ifm::load_monoral( params["input"].as<std::string>(), true );
  ifm::spectrum_image conv( params["note"].as<int>(), sample_rate, 1 << params["resolution"].as<int>() );
  auto [image,delay] = conv( audio );
  unsigned int width = conv.get_width();
  unsigned int height = image.size() / width;
  unsigned int harms = width / 24;
  std::vector< float > harm( harms * height );
  std::vector< float > ec( height );
  unsigned int highest = 0;
  float highest_sum = 0.f;
  for( unsigned int y = 0; y != height; ++y ) {
    float sum = 0;
    for( unsigned int x = 24; x < width; x += 24 )
      sum += image[ y * width + x ];
    ec[ y ] = sum;
    if( sum > highest_sum ) {
      highest_sum = sum;
      highest = y;
    }
    if( ec[ y ] == 0 ) {
      ec.resize( y );
      harm.resize( harms * y );
      break;
    }
  }
  for( unsigned int y = 0; y != ec.size(); ++y ) {
    for( unsigned int x = 1; x != harms; ++x )
      harm[ y * harms + ( x - 1 ) ] = image[ y * width + x * 24 ]/ec[ y ];
  }
  std::vector< std::pair< float, float > > pre;
  {
    std::ifstream bessel_file( params["bessel"].as<std::string>(), std::ofstream::binary );
    nlohmann::json bessel = nlohmann::json::from_msgpack( bessel_file );
    for( const auto &v: bessel )
      pre.emplace_back( v.at( 0 ), v.at( 1 ) );
  }
  std::vector< float > loss( ec.size() );
  std::vector< float > em( ec.size() );
  const auto [l,freq,b] = ifm::find_b_2op( harm.data() + highest * harms, pre );
  em[ highest ] = b;
  loss[ highest ] = l;
  std::cout << "modulator freq: " << freq << std::endl;
  std::cout << "modulator scale: " << b << std::endl;
  std::cout << "loss: " << l << std::endl;
  const auto n = ifm::generate_n( freq );
  float current_b = b;
  for( unsigned int y = highest + 1; y < em.size(); ++y ) {
    auto [l,b_] = ifm::find_b_2op( harm.data() + y * harms, pre, freq, current_b, n );
    em[ y ] = b_;
    current_b = std::min( b_, current_b );
    loss[ y ] = l;
  }
  current_b = b;
  for( unsigned int y = highest; y > 0; --y ) {
    auto [l,b_] = ifm::find_b_2op( harm.data() + ( y - 1 ) * harms, pre, freq, current_b, n );
    em[ y - 1 ] = b_;
    current_b = std::min( b_, current_b );
    loss[ y ] = l;
  }
  if(  params[ "verbose" ].as< bool >() ) {
    for( unsigned int y = 0; y != ec.size(); ++y )
      std::cout << y * 0.01f << " " << ec[ y ]/ec[ highest ] << " " << em[ y ] << " " << loss[ y ] << std::endl;
  }
  bool damped = params[ "damped" ].as< bool >();
  const auto ece = ifm::get_attack_and_decay_exp( ec.data(), ec.size(), 0.01f, damped );
  const auto eme = ifm::get_attack_and_decay_exp( em.data(), ec.size(), 0.01f, damped );
  std::cout << ece.attack_a << " " << ece.attack_b << " " << ece.decay_a << " " << ece.decay_b <<std::endl;
  std::cout << eme.attack_a << " " << eme.attack_b << " " << eme.decay_a << " " << eme.decay_b <<std::endl;
  const auto ecf = ifm::get_attack_and_decay( ec.data(), ec.size(), 0.01f, damped, damped );
  const auto emf = ifm::get_attack_and_decay( em.data(), em.size(), 0.01f, damped, damped );
  std::cout << ifm::store_envelope_param_keyframe( ecf.first ).dump() << " " << ecf.second << std::endl;
  std::cout << ifm::store_envelope_param_keyframe( emf.first ).dump() << " " << emf.second << std::endl;
}


#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "utility.h"

#include <vector>
#include <string>

#include <iostream>

// 快捷键
#include <QShortcut>

#include "dataprocess_utils.h"
#include <Eigen/Core>

#include "instance.h"

// 可视化插件
#include "visualize.h"

#include "autodataassociation.h"
#include <opencv2/opencv.hpp>

using namespace Eigen;
using namespace std;

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    // 初始化图像显示
    frame = new QFrame(ui->label);
//    frame->resize(640,480);
    frame->setGeometry(0,0,640,480);
    pixmap = new QPixmap();
    palette = new QPalette();

    // 注册快捷键
    QShortcut *shortcut_left = new QShortcut(QKeySequence(Qt::Key_Left), this);
    QObject::connect(shortcut_left, SIGNAL(activated()), this, SLOT(on_pushButton_left_clicked()));

    QShortcut *shortcut_right = new QShortcut(QKeySequence(Qt::Key_Right), this);
    QObject::connect(shortcut_right, SIGNAL(activated()), this, SLOT(on_pushButton_right_clicked()));

    // 初始化table性能
    ui->tableWidget->setSelectionBehavior(QAbstractItemView::SelectRows);
    ui->tableWidget_instance->setSelectionBehavior(QAbstractItemView::SelectRows);

    mbInstanceTableInitiated = false;
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::refreshText(){

    QString strShow = QString::number(miCurrentID) + QString(" / ") + QString::number(miTotalNum);
    ui->label_num->setText( strShow );
}

void MainWindow::on_pushButton_clicked()
{
    QString strPath = ui->lineEdit_detection_dir->text();
    GetFileNames(strPath.toStdString(), mDetectionFiles );
    sortFileNames(mDetectionFiles, mDetectionFiles);

    miTotalNum = mDetectionFiles.size();

    // 更新图像位置
    msImagePath = ui->lineEdit_imagePath->text().toStdString();
    // 更新检测位置
    msDetectPath = ui->lineEdit_detection_dir->text().toStdString();
    // 更新标签.
    msLabelConfigPath = ui->lineEdit_labelConfig->text().toStdString();
    readLabelConfig(msLabelConfigPath);
    // 更新实例.
    msInstancesPath = ui->lineEdit_instancePath->text().toStdString();
    readInstances(msInstancesPath);
    refreshInstanceTable();

    // 读取第0张图.
    miCurrentID = 0;
    dealWithPic(miCurrentID);

    // 加载自动数据关联.
    Matrix3d calib;
//    calib << 517.3, 0, 318.6,
//                0, 516.5, 255.3,
//                0, 0, 1;
    // For TUM fr1

    calib << 520.9, 0, 325.1,
        0, 521.0, 249.7,
        0, 0, 1;
    // For TUM fr2

    mAssociater.initialize(mvInstances, calib);

    // 初始化数据集工具
    msDatasetDir = ui->lineEdit_datasetDir->text().toStdString();
    mLoader.loadDataset(msDatasetDir);

}

void MainWindow::dealWithPic(int id){
    cout << "Deal with " << id << endl;

    // 读取图像切换.
    string detFullPath = mDetectionFiles[id];
    string bareFileName = splitFileName(detFullPath, true);
    string imageFullPath = msImagePath + bareFileName + ".png";

    // 处理opencv相关图像.
    std::cout << "bareFileName: " << bareFileName << std::endl;
    std::cout << "image path: " << imageFullPath << std::endl;
    (*pixmap) = QPixmap(imageFullPath.c_str());

    palette->setBrush(frame->backgroundRole(),QBrush(*pixmap));
    frame->setPalette(*palette);
    frame->setAutoFillBackground(true);
    //frame->setMask(pixmap->mask());  //可以将图片中透明部分显示为透明的
    frame->show();

    refreshText();

    // 读取检测
    readDetection(id);

    // 进行自动数据关联
    if(ui->checkBox->isChecked())
        automaticAssociation();

}

// 上一个
void MainWindow::on_pushButton_left_clicked()
{
    if(miCurrentID>0){
        saveCurrentDetection();        // 保存当前情况

        miCurrentID--;
        dealWithPic(miCurrentID);
    }
}

// 下一个
void MainWindow::on_pushButton_right_clicked()
{
    if(miCurrentID<miTotalNum-1){
        saveCurrentDetection();        // 保存当前情况

        miCurrentID++;
        dealWithPic(miCurrentID);
    }
}

void MainWindow::readDetection(int id){
    string detFileName = mDetectionFiles[id];

    // 读取det数据.
    MatrixXd detMat = readDataFromFile(detFileName.c_str());
    std::cout << detMat << std::endl;

    // 绘制det结果到控件中. 表格?
    refreshTable(detMat);

    // 保存当前detMat
    mmDetMat = detMat;

}

void MainWindow::refreshTable(MatrixXd &mat){
    int rows = mat.rows();

    QTableWidget* pTable = ui->tableWidget;
    pTable->clearContents();
    pTable->setRowCount(rows);
    for( int i=0;i<rows;i++){
        VectorXd v = mat.row(i);

        for( int p=0;p<mat.cols();p++)
        {
            double data = v(p);
            //string strData = to_string(data);
            //std::cout << i << ", " << p << ": " << strData << std::endl;
            pTable->setItem(i, p, new QTableWidgetItem(QString::number(data)));

            // 处理 label 栏
            if (p==5)
            {
                QString str;
                str = QString::number(data) + QString(" / ") + QString::fromStdString(mvLabelIDToString[int(data)]);
                pTable->setItem(i, p, new QTableWidgetItem(str));
            }
        }

    }

}

void MainWindow::readLabelConfig(string &path){
    mvLabelIDToString = readStringFromFile(path.c_str());
}

void MainWindow::readInstances(string &path){
    MatrixXd instanceMat = readDataFromFile(path.c_str(), false);        // 剔除首行
    int num = instanceMat.rows();

    mvInstances.clear();
    mvInstances.resize(num);
    for( int i=0; i<num; i++)
    {
        VectorXd v = instanceMat.row(i);

        Instance obj;
        obj.id = int(v(0));
        for(int n=0;n<9;n++)
            obj.param[n] = double(v(n+1));

        obj.label = int(v(10));

        mvInstances[i] = obj;
    }

}

void MainWindow::refreshInstanceTable(){
    int num = mvInstances.size();
    QTableWidget* pTable = ui->tableWidget_instance3D;
    pTable->clearContents();
    pTable->setRowCount(num);
    for( int i=0; i<num;i++ )
    {
        Instance* pObj = &mvInstances[i];
        pTable->setItem(i, INS_TABLE_ID, new QTableWidgetItem(QString::number(pObj->id)));

        for( int n=0; n<9; n++)
            pTable->setItem(i, n+1, new QTableWidgetItem(QString::number(pObj->param[n])));

        int label = pObj->label;
        QString str;
        str = QString::number(label) + QString(" / ") + QString::fromStdString(mvLabelIDToString[label]);
        pTable->setItem(i, INS_TABLE_LABEL, new QTableWidgetItem(str));

    }

    mbInstanceTableInitiated = true;
}

void MainWindow::on_tableWidget_itemSelectionChanged()
{
    int currentRow = ui->tableWidget->currentRow();

    QTableWidgetItem* pItem = ui->tableWidget->item(currentRow, DET_TABLE_INSTANCE);

    if(pItem!=NULL){
        QString text = pItem->text();
        std::cout << "text: " << text.toStdString() << std::endl;

        int instance = text.toInt();
        std::cout << "instance: " << instance << std::endl;
        if( instance > 0)
            ui->tableWidget_instance->setCurrentCell(instance, 0);
    }
    else
    {
        // 尚未安排Instance
        //ui->tableWidget_instance->
    }
}

void MainWindow::saveCurrentDetection(){
    string currentPath = mDetectionFiles[miCurrentID];

    // 主要处理detMat的最后一列.
    int rows = ui->tableWidget->rowCount();
    int cols = ui->tableWidget->columnCount();

    // 第一种情况，尚未做标注， detMat的size比表格小.
    if( cols > mmDetMat.cols() )
        mmDetMat.conservativeResize(mmDetMat.rows(), cols);

    // 修改最后一列
    for( int i =0; i<rows; i++)
    {
        QTableWidgetItem* pItem = ui->tableWidget->item(i, DET_TABLE_INSTANCE);

        if(pItem!=NULL)
        {
            mmDetMat(i,DET_TABLE_INSTANCE) = pItem->text().toInt();
        }
        else
        {
            mmDetMat(i,DET_TABLE_INSTANCE) = -1;
        }

    }

    // 保存mmDetMat
    saveMatToFile(mmDetMat, currentPath.c_str());
}

void MainWindow::on_pushButton_3_clicked()
{
    saveCurrentDetection();
}

void MainWindow::on_pushButton_4_clicked()
{
    saveCurrentDetection();

    // Save all files.
    saveCurrentInstances();

    this->destroy();
}

void MainWindow::on_tableWidget_instance_itemSelectionChanged()
{
    // 若此时 det table 被选中，则更换其instance
    int currentRow = ui->tableWidget->currentRow();

    int currentInstance = ui->tableWidget_instance->currentRow();
    if( currentRow > -1 )
    {
        QTableWidgetItem* item = ui->tableWidget->item(currentRow, DET_TABLE_INSTANCE);
        if(item == NULL)
        {
            ui->tableWidget->setItem(currentRow, DET_TABLE_INSTANCE, new QTableWidgetItem(QString::number(currentInstance)));
        }
        else
            item->setText(QString::number(currentInstance));
    }
}

void MainWindow::on_pushButton_5_clicked()
{
    std::cout << "Initializing Viewer..." << std::endl;

    string path_setting = ui->lineEdit_settingPath->text().toStdString();
    string path_pcd = ui->lineEdit_pcdPath->text().toStdString();

    mVisualizer.startVisualizationThread(path_setting);
    mVisualizer.addBackgroundPoint(path_pcd);
    mVisualizer.refreshInstances(mvInstances);
}

void MainWindow::on_pushButton_2_clicked()
{

}

void MainWindow::on_pushButton_6_clicked()
{
    // 入口: txt 文档 -> 内存

    // 添加: 改变内存, 重新渲染显示
    // 修改过程 -> 同步改变内存

    // 出口: 关闭程序, 内存存储到txt

    Instance ins;
    ins.id = mvInstances.size();
    ins.label = 0;

    mvInstances.push_back(ins);

    refreshInstanceTable(); // 刷新实例table

}

void MainWindow::on_tableWidget_instance3D_cellChanged(int row, int column)
{
    if( !mbInstanceTableInitiated ) return;
    if(mvInstances.size() == 0) return;
    // 更新内存中的Instance存储情况.
    if( column == INS_TABLE_LABEL ){
        QTableWidgetItem* item = ui->tableWidget_instance3D->item(row, column);

        // 先拆分  id / txt.
        QString str = item->text();
        QString num_str = str.section(" / ", 0,0);

        mvInstances[row].label = num_str.toInt();
    }
    else if( column > 0 ){
        QTableWidgetItem* item = ui->tableWidget_instance3D->item(row, column);
        mvInstances[row].param[column-1] = item->text().toDouble();
    }

    // 更新
    if(mVisualizer.isInitialized())
        mVisualizer.refreshInstances(mvInstances);

}

void MainWindow::on_horizontalSlider_valueChanged(int value)
{
    double percentage = 1.0 + (double)(value-50)/100.0;
    double changed_value = miCurrentSliderCenter * percentage;

    QTableWidgetItem* item = ui->tableWidget_instance3D->item(miCurrentInstanceTableRow, miCurrentInstanceTableCol);
    item->setText(QString::number(changed_value));
}

void MainWindow::on_tableWidget_instance3D_currentCellChanged(int currentRow, int currentColumn, int previousRow, int previousColumn)
{
    int row = currentRow;
    int column = currentColumn;
    if( column < INS_TABLE_X || column > INS_TABLE_C) return;
    // 获取当前值.
    QTableWidgetItem* item = ui->tableWidget_instance3D->item(row, column);
    double value = item->text().toDouble();

    miCurrentSliderCenter = value;

    miCurrentInstanceTableRow = row;
    miCurrentInstanceTableCol = column;

    std::cout << "miCurrentInstanceTableRow: " << miCurrentInstanceTableRow << std::endl;
    std::cout << "miCurrentInstanceTableCol: " << miCurrentInstanceTableCol << std::endl;
    // 刷新滚动条上下限
    ui->horizontalSlider->setValue(50);
}

void MainWindow::on_tableWidget_instance3D_cellActivated(int row, int column)
{

}

void MainWindow::on_pushButton_7_clicked()
{
    on_pushButton_4_clicked(); // 与2d按钮一致.
}

void MainWindow::saveCurrentInstances()
{
    QTableWidget* pTable = ui->tableWidget_instance3D;

    int rows = pTable->rowCount();
    int cols = pTable->columnCount();

    int num = mvInstances.size();

    // 保存
    MatrixXd insMat = instancesToMat(mvInstances);

    // 保存mmDetMat
    saveMatToFile(insMat, msInstancesPath.c_str());

}

MatrixXd MainWindow::instancesToMat(vector<Instance> &instances){
    int rows = instances.size();
    int cols = 11;
    Eigen::MatrixXd mat;
    mat.resize(rows, cols);
    for(int i=0;i<rows;i++)
    {
        Eigen::Matrix<double, 11, 1> vec;
        for(int n=0;n<9;n++)
            vec(n+1) = instances[i].param[n];
        vec(0) = instances[i].id;
        vec(10) = instances[i].label;

        mat.row(i) = vec;
    }

    return mat;

}

void MainWindow::automaticAssociation(){
    // 步骤1： 找到该帧对应的groundtruth;
    // 没有gt的帧可以直接舍去.

    // 临时参数： CALIB INFORMATION

    // 当前 timestamp

    // 感觉需要加入cv::Mat了. 然后将mLoader也交给 mAssociater去完成.
    VectorXd pose;
    cv::Mat rgb, depth;
    bool find_gt = mLoader.findFrameUsingID(miCurrentID, rgb, depth, pose); // 给TUM的loader添加一个函数接口.

    if(find_gt)
    {
        vector<Association> associations = mAssociater.process(pose, mmDetMat);

        // 获得了，如何显示出来?

        // 1） 直接将对应表格切换ID.
        int assoNum = associations.size();
        for(int i=0; i < assoNum; i++)
        {
            std::cout << "Asso " << i << " : " << associations[i].detID
                      << " -> " << associations[i].instanceID << std::endl;
            QString str;
            str = QString::number(associations[i].instanceID);
            ui->tableWidget->setItem(associations[i].detID, DET_TABLE_INSTANCE, new QTableWidgetItem(str));
        }

        // 2） 在图像中可视化投影结果，对应InstanceID， LabelID
        // cv 读取当前帧.
        // 读取图像切换.
        string detFullPath = mDetectionFiles[miCurrentID];
        string bareFileName = splitFileName(detFullPath, true);
        string imageFullPath = msImagePath + bareFileName + ".png";
        cv::Mat oriMat = cv::imread(imageFullPath, IMREAD_COLOR);
        cv::Mat mat = mAssociater.drawProjection(oriMat);
        // cv->

        (*pixmap) = QPixmap::fromImage(QImage((unsigned char*) mat.data, mat.cols, mat.rows, QImage::Format_RGB888));;

        palette->setBrush(frame->backgroundRole(),QBrush(*pixmap));
        frame->setPalette(*palette);
        frame->setAutoFillBackground(true);
        //frame->setMask(pixmap->mask());  //可以将图片中透明部分显示为透明的
        frame->show();

    }
    else
        std::cout << "No ground truth found for frame" << miCurrentID << std::endl;

}

void MainWindow::on_checkBox_showtraj_stateChanged(int arg1)
{
    if(ui->checkBox_showtraj->isChecked()){
        MatrixXd poseMat;
        poseMat.resize(0, 7);

        int jump = miTotalNum / 100;
        for( int i=0; i<miTotalNum ;i=i+jump)
        {
            // 开启
            VectorXd pose;
            cv::Mat rgb, depth;
            bool find_gt = mLoader.findFrameUsingID(i, rgb, depth, pose); // 给TUM的loader添加一个函数接口.

            if(find_gt)
            {
                poseMat.conservativeResize(poseMat.rows()+1, poseMat.cols());
                poseMat.row(poseMat.rows()-1) = pose;
            }

        }

        mVisualizer.addTrajectory(poseMat);
    }
    else
        mVisualizer.clearTrajectory();
        // 关闭
}