#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h"
#include "Tudat/External/SpiceInterface/spiceInterface.h"

#include "Tudat/Astrodynamics/ElectroMagnetism/panelledRadiationPressure.h"
#include "Tudat/Astrodynamics/Ephemerides/keplerEphemeris.h"
#include "Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h"
#include "Tudat/Astrodynamics/Ephemerides/constantRotationalEphemeris.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/body.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"
#include "Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.h"

namespace tudat
{

using namespace tudat::basic_astrodynamics;
using namespace tudat::simulation_setup;
using namespace tudat::ephemerides;
using namespace tudat::basic_astrodynamics;
using namespace tudat::electro_magnetism;

namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_PanelledRadiationPressure )

//! Test panelled radiation acceleration model with various simple boxes-and-wings models.
BOOST_AUTO_TEST_CASE( testSimpleGeometryPanelledRadiationPressure )
{

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

    // Create bodies needed in simulation
    double initialEphemerisTime = -86400.0;
    double finalEphemerisTime = 1.1 * 365.25 * 86400.0;
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Sun" );
    NamedBodyMap bodyMap = createBodies(
                getDefaultBodySettings( bodyNames,initialEphemerisTime, finalEphemerisTime ) );

    // Create vehicle
    double vehicleMass = 2500.0;
    bodyMap[ "Vehicle" ] = std::make_shared< Body >( );
    bodyMap[ "Vehicle" ]->setConstantBodyMass( vehicleMass );

    // Put vehicle on circular orbit around Sun
    Eigen::Vector6d initialStateInKeplerianElements = Eigen::Vector6d::Zero( );
    initialStateInKeplerianElements[ 0 ] = physical_constants::ASTRONOMICAL_UNIT;
    bodyMap[ "Vehicle" ]->setEphemeris(
                std::make_shared< KeplerEphemeris >(
                    initialStateInKeplerianElements, 0.0, spice_interface::getBodyGravitationalParameter( "Sun" ),
                    "Sun", "ECLIPJ2000", 1 ) );

    // Define constant rotational ephemeris
    Eigen::Vector7d rotationalStateVehicle;
    rotationalStateVehicle.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat( Eigen::Quaterniond( Eigen::Matrix3d::Identity() ));
    rotationalStateVehicle.segment( 4, 3 ) = Eigen::Vector3d::Zero();
    bodyMap[ "Vehicle" ]->setRotationalEphemeris(
                std::make_shared< ConstantRotationalEphemeris >(
                    rotationalStateVehicle, "ECLIPJ2000", "VehicleFixed" ) );

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

    for( unsigned int test = 0; test <= 2; test++ )
    {
        // Create radiation pressure properties
        std::vector< double > areas;
        areas.push_back( 2.532 );
        areas.push_back( 3.254 );
        areas.push_back( 8.654 );
        areas.push_back( 1.346 );
        areas.push_back( 2.454 );
        areas.push_back( 5.345 );

        std::vector< double > emissivities;
        if( test == 0 || test == 2 )
        {
            emissivities.push_back( 0.0 );
            emissivities.push_back( 1.0 );
            emissivities.push_back( 0.0 );
            emissivities.push_back( 1.0 );
        }
        else if( test == 1 )
        {
            emissivities.push_back( 0.2 );
            emissivities.push_back( 0.3 );
            emissivities.push_back( 0.4 );
            emissivities.push_back( 0.5 );
        }
        emissivities.push_back( 1.0 );
        emissivities.push_back( 0.0 );

        std::vector< double > diffuseReflectionCoefficients;
        if( test == 0 || test == 2 )
        {
            diffuseReflectionCoefficients.push_back( 0.0 );
            diffuseReflectionCoefficients.push_back( 0.0 );
            diffuseReflectionCoefficients.push_back( 0.0 );
            diffuseReflectionCoefficients.push_back( 0.0 );
            diffuseReflectionCoefficients.push_back( 0.0 );
            diffuseReflectionCoefficients.push_back( 0.0 );
        }
        else
        {
            diffuseReflectionCoefficients.push_back( 0.6 );
            diffuseReflectionCoefficients.push_back( 0.5 );
            diffuseReflectionCoefficients.push_back( 0.4 );
            diffuseReflectionCoefficients.push_back( 0.3 );
            diffuseReflectionCoefficients.push_back( 0.2 );
            diffuseReflectionCoefficients.push_back( 0.1 );
        }

        if( test == 2 )
        {
            // Define simple rotational ephemeris
            bodyMap[ "Vehicle" ]->setRotationalEphemeris(
                        std::make_shared< tudat::ephemerides::SimpleRotationalEphemeris >(
                            0.2, 0.4, -0.2, 1.0E-5, 0.0, "ECLIPJ2000", "VehicleFixed" ) );
        }

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

        std::shared_ptr< PanelledRadiationPressureInterfaceSettings > radiationPressureInterfaceSettings =
                std::make_shared< PanelledRadiationPressureInterfaceSettings >(
                    "Sun", emissivities, areas, diffuseReflectionCoefficients, panelSurfaceNormals );
        std::shared_ptr< PanelledRadiationPressureInterface > radiationPressureInterface =
                std::dynamic_pointer_cast< PanelledRadiationPressureInterface >(
                    createRadiationPressureInterface( radiationPressureInterfaceSettings, "Vehicle", bodyMap ) );
        bodyMap[ "Vehicle" ]->setRadiationPressureInterface( "Sun", radiationPressureInterface );

        // Define accelerations
        SelectedAccelerationMap accelerationMap;
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOnVehicle;
        accelerationsOnVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                       panelled_radiation_pressure_acceleration ) );
        accelerationMap[ "Vehicle" ] = accelerationsOnVehicle;
        std::map< std::string, std::string > centralBodyMap;
        centralBodyMap[ "Vehicle" ] = "Sun";
        AccelerationMap accelerationModelMap = createAccelerationModelsMap( bodyMap, accelerationMap, centralBodyMap );
        std::shared_ptr< AccelerationModel< Eigen::Vector3d > > accelerationModel =
                accelerationModelMap.at( "Vehicle" ).at( "Sun" ).at( 0 );

        // Compute spacecraft orbital period, and compute test times
        double orbitalPeriod =
                2.0 * mathematical_constants::PI * std::sqrt(
                    std::pow( physical_constants::ASTRONOMICAL_UNIT, 3.0 ) /
                    spice_interface::getBodyGravitationalParameter( "Sun" ) );
        std::vector< double > testTimes = { 0.0, orbitalPeriod / 4.0, orbitalPeriod / 2.0, 3.0 * orbitalPeriod / 4.0 };

        // Compute panelled radiation pressure for various relative Sun positions
        Eigen::Vector3d calculatedAcceleration, expectedAcceleration;
        Eigen::Vector3d sunCenteredVehiclePosition;

        std::shared_ptr< Ephemeris > vehicleEphemeris = bodyMap[ "Vehicle" ]->getEphemeris( );
        for( unsigned int i = 0; i < testTimes.size( ); i++ )
        {
            // Update environment and acceleration
            bodyMap[ "Sun" ]->setStateFromEphemeris( testTimes[ i ] );
            bodyMap[ "Vehicle" ]->setStateFromEphemeris( testTimes[ i ] );
            bodyMap[ "Vehicle" ]->setCurrentRotationToLocalFrameFromEphemeris( testTimes[ i ] );
            radiationPressureInterface->updateInterface( testTimes[ i ] );
            accelerationModel->updateMembers( testTimes[ i ] );

            // Retrieve acceleration
            calculatedAcceleration = accelerationModel->getAcceleration( );

            // Manually compute acceleration
            double radiationPressure = radiationPressureInterface->getCurrentRadiationPressure( );
            sunCenteredVehiclePosition = vehicleEphemeris->getCartesianState( testTimes[ i ] ).segment( 0, 3 );

            // Case with emissivities equal to either 0 or 1 and no diffuse reflection.
            if( test == 0 )
            {
                if( i == 0 || i == 2 )
                {
                    expectedAcceleration = radiationPressure / vehicleMass  *
                            areas.at( i ) * sunCenteredVehiclePosition.normalized( );
                }
                else if( i == 1 || i == 3 )
                {
                    expectedAcceleration = -radiationPressure / vehicleMass * (
                                2.0 * areas.at( i ) * panelSurfaceNormals.at( i ) );
                }
            }

            // Case with more complex panel characteristics.
            else if ( test == 1 )
            {
                expectedAcceleration = radiationPressure / vehicleMass *
                        ( 1.0 + emissivities.at( i ) + 2.0 * diffuseReflectionCoefficients.at( i ) / 3.0 ) *
                        areas.at( i ) * sunCenteredVehiclePosition.normalized( );
            }

            if( test == 0 || test == 1 )
            {
                // Check computed acceleration
                for( unsigned int j = 0; j < 3; j++ )
                {
                    BOOST_CHECK_SMALL( std::fabs( calculatedAcceleration( j ) - expectedAcceleration( j ) ), 2.0E-23 );
                }
            }

            // Case with non constant rotational ephemeris for the spacecraft
            else
            {
                std::shared_ptr< PanelledRadiationPressureAcceleration > panelledRadiationPressureAcceleration =
                        std::dynamic_pointer_cast< PanelledRadiationPressureAcceleration >( accelerationModel );
                Eigen::Quaterniond currentRotationToInertialFrame =
                        bodyMap[ "Vehicle" ]->getRotationalEphemeris( )->getRotationToBaseFrame( testTimes.at( i ) );

                expectedAcceleration = Eigen::Vector3d::Zero();

                for( unsigned int j = 0; j < panelSurfaceNormals.size( ); j++ )
                {
                   Eigen::Vector3d currentPanelNormal =
                           panelledRadiationPressureAcceleration->getCurrentPanelSurfaceNormalInPropagationFrame( j );
                   Eigen::Vector3d expectedPanelNormal =
                           currentRotationToInertialFrame * panelSurfaceNormals.at( j );

                   // Check surface normals orientation.
                   for( unsigned int k = 0; k < 3; k++ )
                   {
                       BOOST_CHECK_SMALL( std::fabs( currentPanelNormal( k ) - expectedPanelNormal( k ) ), 1.0E-15 );
                   }

                   // Compute expected acceleration.
                    double cosinusPanelInclination =  - sunCenteredVehiclePosition.normalized( ).dot( currentPanelNormal );

                    if ( cosinusPanelInclination > 0.0 ){
                        expectedAcceleration += - radiationPressure / vehicleMass  * areas.at( j ) * cosinusPanelInclination *
                                ( ( 1.0 - emissivities.at( j ) ) * - sunCenteredVehiclePosition.normalized(  )
                                    + 2.0 * emissivities.at( j ) * cosinusPanelInclination * currentPanelNormal );
                    }
                }

                // Check computed acceleration
                for( unsigned int j = 0; j < 3; j++ )
                {
                    BOOST_CHECK_SMALL( std::fabs( calculatedAcceleration( j ) - expectedAcceleration( j ) ), 2.0E-23 );
                }

            }
        }
    }

}

//! Test panelled radiation acceleration model for a spacecraft in various orbits with respect to the Sun.
BOOST_AUTO_TEST_CASE( testPanelledRadiationPressureMontenbruckModel )
{

    // Box-and-wings model is partially obtained from Montenbruck, O., 2017.
    // Semi-analytical solar radiation pressure modeling for QZS-1 orbit-normal and yaw-steering attitude.
    // Advances in Space Research, 59(8), pp.2088-2100..

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

    // Create bodies needed in simulation
    double initialEphemerisTime = 0.0;
    double finalEphemerisTime = 1.1 * 365.25 * 86400.0;
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Sun" );
    NamedBodyMap bodyMap = createBodies(
                getDefaultBodySettings( bodyNames,initialEphemerisTime, finalEphemerisTime ) );

    // Create vehicle
    double vehicleMass = 2000.0;
    bodyMap[ "Vehicle" ] = std::make_shared< Body >( );
    bodyMap[ "Vehicle" ]->setConstantBodyMass( vehicleMass );

    for ( int testCase = 0 ; testCase < 4 ; testCase++){

        // Put vehicle on circular orbit around the Sun with i = 0 deg
        if ( testCase == 0 || testCase == 3 ){
            Eigen::Vector6d initialStateInKeplerianElements = Eigen::Vector6d::Zero( );
            initialStateInKeplerianElements[ 0 ] = physical_constants::ASTRONOMICAL_UNIT;
            bodyMap[ "Vehicle" ]->setEphemeris( std::make_shared< KeplerEphemeris >( initialStateInKeplerianElements, 0.0,
                                                         spice_interface::getBodyGravitationalParameter( "Sun" ), "Sun", "ECLIPJ2000", 1 ) );
        }

        // Put vehicle in circular orbit around the Sun with i = 90 deg
        else if ( testCase == 1 ){
            Eigen::Vector6d initialStateInKeplerianElements = Eigen::Vector6d::Zero( );
            initialStateInKeplerianElements[ 0 ] = physical_constants::ASTRONOMICAL_UNIT;
            initialStateInKeplerianElements[ orbital_element_conversions::inclinationIndex ] = unit_conversions::convertDegreesToRadians( 90.0 );
            bodyMap[ "Vehicle" ]->setEphemeris( std::make_shared< KeplerEphemeris >( initialStateInKeplerianElements, 0.0,
                                                         spice_interface::getBodyGravitationalParameter( "Sun" ), "Sun", "ECLIPJ2000", 1 ) );
        }

        // Put vehicle in circular orbit around the Sun with arbitrary chosen inclination
        else if ( testCase == 2 ){
            Eigen::Vector6d initialStateInKeplerianElements = Eigen::Vector6d::Zero( );
            initialStateInKeplerianElements[ 0 ] = physical_constants::ASTRONOMICAL_UNIT;
            initialStateInKeplerianElements[ orbital_element_conversions::inclinationIndex ] = unit_conversions::convertDegreesToRadians( 20.0 );
            bodyMap[ "Vehicle" ]->setEphemeris( std::make_shared< KeplerEphemeris >( initialStateInKeplerianElements, 0.0,
                                                         spice_interface::getBodyGravitationalParameter( "Sun" ), "Sun", "ECLIPJ2000", 1 ) );
        }

        // Set-up rotational ephemeris for vehicle.
        if ( testCase < 3 ){
            // Define constant rotational model.
            Eigen::Vector7d rotationalStateVehicle;
            rotationalStateVehicle.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat( Eigen::Quaterniond( Eigen::Matrix3d::Identity() ));
            rotationalStateVehicle.segment( 4, 3 ) = Eigen::Vector3d::Zero();
            bodyMap[ "Vehicle" ]->setRotationalEphemeris( std::make_shared< ConstantRotationalEphemeris >( rotationalStateVehicle, "ECLIPJ2000",
                                                                                "VehicleFixed" ) );
        }
        else if ( testCase == 3 ){
            // Define simple rotational model.
            bodyMap[ "Vehicle" ]->setRotationalEphemeris( std::make_shared< tudat::ephemerides::SimpleRotationalEphemeris >(
                            0.2, 0.4, -0.2, 1.0E-5, 0.0, "ECLIPJ2000", "VehicleFixed" ) );
        }

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

        std::vector< double > areas;
        std::vector< double > emissivities;
        std::vector< double > diffuseReflectionCoefficients;
        std::vector< Eigen::Vector3d > panelSurfaceNormals;

        // Define panels properties for test cases with constant rotational model.
        if ( testCase < 3 ){

            // Panels areas
            areas.push_back( 2.0 );
            areas.push_back( 4.0 );
            areas.push_back( 6.0 );
            areas.push_back( 9.9 );
            areas.push_back( 2.3 );
            areas.push_back( 9.9 );
            areas.push_back( 2.3 );
            areas.push_back( 4.6 );
            areas.push_back( 2.7 );
            areas.push_back( 5.8 );
            areas.push_back( 2.7 );

            // Emissivities
            emissivities.push_back( 0.0 );
            emissivities.push_back( 0.1 );
            emissivities.push_back( 0.0 );
            emissivities.push_back( 0.0 );
            emissivities.push_back( 0.1 );
            emissivities.push_back( 0.0 );
            emissivities.push_back( 0.1 );
            emissivities.push_back( 0.0 );
            emissivities.push_back( 0.1 );
            emissivities.push_back( 0.0 );
            emissivities.push_back( 0.1 );

            //Diffuse reflection coefficients
            diffuseReflectionCoefficients.push_back( 0.06 );
            diffuseReflectionCoefficients.push_back( 0.46 );
            diffuseReflectionCoefficients.push_back( 0.06 );
            diffuseReflectionCoefficients.push_back( 0.06 );
            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 );
            diffuseReflectionCoefficients.push_back( 0.06 );
            diffuseReflectionCoefficients.push_back( 0.46 );

            // Panels surface normals
            panelSurfaceNormals.push_back( Eigen::Vector3d::UnitZ( ) );
            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::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( ) );
        }

        // Define panels properties for test cases with constant rotational model (simpler boxes-and-wings model).
        else if ( testCase == 3 ){

            // Panels areas
            areas.push_back( 9.9 );
            areas.push_back( 2.3 );
            areas.push_back( 9.9 );
            areas.push_back( 2.3 );

            // Emissivities
            emissivities.push_back( 0.0 );
            emissivities.push_back( 0.1 );
            emissivities.push_back( 0.0 );
            emissivities.push_back( 0.1 );

            //Diffuse reflection coefficients
            diffuseReflectionCoefficients.push_back( 0.06 );
            diffuseReflectionCoefficients.push_back( 0.46 );
            diffuseReflectionCoefficients.push_back( 0.06 );
            diffuseReflectionCoefficients.push_back( 0.46 );

            // Panels surface normals
            panelSurfaceNormals.push_back( Eigen::Vector3d::UnitX( ) );
            panelSurfaceNormals.push_back( Eigen::Vector3d::UnitX( ) );
            panelSurfaceNormals.push_back( - Eigen::Vector3d::UnitX( ) );
            panelSurfaceNormals.push_back( - Eigen::Vector3d::UnitX( ) );
        }

        // Create panelled radiation pressure interface.
        std::shared_ptr< PanelledRadiationPressureInterfaceSettings > radiationPressureInterfaceSettings =
                std::make_shared< PanelledRadiationPressureInterfaceSettings >(
                    "Sun", emissivities, areas, diffuseReflectionCoefficients, panelSurfaceNormals );

        std::shared_ptr< PanelledRadiationPressureInterface > radiationPressureInterface =
                std::dynamic_pointer_cast< PanelledRadiationPressureInterface >(
                    createRadiationPressureInterface( radiationPressureInterfaceSettings, "Vehicle", bodyMap ) );

        bodyMap[ "Vehicle" ]->setRadiationPressureInterface( "Sun", radiationPressureInterface );

        // Define accelerations.
        SelectedAccelerationMap accelerationMap;
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOnVehicle;
        accelerationsOnVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                       panelled_radiation_pressure_acceleration ) );

        accelerationMap[ "Vehicle" ] = accelerationsOnVehicle;

        std::map< std::string, std::string > centralBodyMap; centralBodyMap[ "Vehicle" ] = "Sun";
        AccelerationMap accelerationModelMap = createAccelerationModelsMap( bodyMap, accelerationMap, centralBodyMap );
        std::shared_ptr< AccelerationModel< Eigen::Vector3d > > accelerationModel =
                accelerationModelMap.at( "Vehicle" ).at( "Sun" ).at( 0 );

        // Compute spacecraft orbital period, and compute test times
        double orbitalPeriod = 2.0 * mathematical_constants::PI * std::sqrt( std::pow( physical_constants::ASTRONOMICAL_UNIT, 3.0 ) /
                                                                             spice_interface::getBodyGravitationalParameter( "Sun" ) );
        std::vector< double > testTimes = { 0.0, orbitalPeriod / 4.0, orbitalPeriod / 2.0, 3.0 * orbitalPeriod / 4.0 };

        // Compute panelled radiation pressure for various relative Sun positions.
        Eigen::Vector3d calculatedAcceleration, expectedAcceleration;

        Eigen::Vector3d sunCenteredVehiclePosition;
        std::shared_ptr< Ephemeris > vehicleEphemeris = bodyMap[ "Vehicle" ]->getEphemeris( );

        for( unsigned int i = 0; i < testTimes.size( ) ; i++ )
        {
            // Update environment and acceleration
            bodyMap[ "Sun" ]->setStateFromEphemeris( testTimes[ i ] );
            bodyMap[ "Vehicle" ]->setStateFromEphemeris( testTimes[ i ] );
            bodyMap[ "Vehicle" ]->setCurrentRotationToLocalFrameFromEphemeris( testTimes[ i ] );
            radiationPressureInterface->updateInterface( testTimes[ i ] );
            accelerationModel->updateMembers( testTimes[ i ] );

            // Retrieve acceleration.
            calculatedAcceleration = accelerationModel->getAcceleration( );

            double radiationPressure = radiationPressureInterface->getCurrentRadiationPressure( );
            Eigen::Vector3d expectedVehicleToSunNormalisedVector = ( bodyMap[ "Sun" ]->getState( ) - bodyMap[ "Vehicle" ]->getState( ) ).segment( 0, 3 )
                    .normalized();

            // Calculated accelerations.

            // Calculated acceleration if Sun-Vehicle vector aligned with +X-axis (acceleration generated by panels whose normals are
            // along +X-axis only)
            double cosinusPanelInclinationPositiveXaxis = expectedVehicleToSunNormalisedVector.dot( Eigen::Vector3d::UnitX( ) );
            Eigen::Vector3d accelerationPositiveXaxisOrientedPanels = - cosinusPanelInclinationPositiveXaxis
                    * radiationPressure / bodyMap[ "Vehicle" ]->getBodyMass()
                    * ( areas[5] * ( ( 1.0 - emissivities[5] ) * expectedVehicleToSunNormalisedVector
                    + 2.0 / 3.0 * diffuseReflectionCoefficients[5] * Eigen::Vector3d::UnitX( ) )
                    + areas[6] * ( ( 1.0 - emissivities[6] ) * expectedVehicleToSunNormalisedVector
                    + ( 2.0 / 3.0 * diffuseReflectionCoefficients[6] + 2.0 * cosinusPanelInclinationPositiveXaxis * emissivities[6] )
                    * Eigen::Vector3d::UnitX( ) )  );

            // Calculated acceleration generated by the panels whose normals are along -X-axis.
            double cosinusPanelInclinationNegativeXaxis = expectedVehicleToSunNormalisedVector.dot( - Eigen::Vector3d::UnitX() );
            Eigen::Vector3d accelerationNegativeXaxisOrientedPanels = - cosinusPanelInclinationNegativeXaxis
                    * radiationPressure / bodyMap[ "Vehicle" ]->getBodyMass()
                    * ( areas[3] * ( ( 1.0 - emissivities[3] ) * expectedVehicleToSunNormalisedVector
                    + 2.0 / 3.0 * diffuseReflectionCoefficients[3] * - Eigen::Vector3d::UnitX( ) )
                    + areas[4] * ( ( 1.0 - emissivities[4] ) * expectedVehicleToSunNormalisedVector
                    + ( 2.0 / 3.0 * diffuseReflectionCoefficients[4] + 2.0 * emissivities[4] * cosinusPanelInclinationNegativeXaxis )
                    * - Eigen::Vector3d::UnitX( ) ) );

            // Calculated acceleration generated by the panels whose normals are along +Y-axis.
            double cosinusPanelInclinationPositiveYaxis = expectedVehicleToSunNormalisedVector.dot( Eigen::Vector3d::UnitY() );
            Eigen::Vector3d accelerationPositiveYaxisOrientedPanels = - cosinusPanelInclinationPositiveYaxis
                    * radiationPressure / bodyMap[ "Vehicle" ]->getBodyMass()
                    * ( areas[7] * ( ( 1.0 - emissivities[7] ) * expectedVehicleToSunNormalisedVector
                    + 2.0 / 3.0 * diffuseReflectionCoefficients[7] * Eigen::Vector3d::UnitY( ) )
                    + areas[8] * ( ( 1.0 - emissivities[8] ) * expectedVehicleToSunNormalisedVector
                    + ( 2.0 / 3.0 * diffuseReflectionCoefficients[8] + 2.0 * emissivities[8] * cosinusPanelInclinationPositiveYaxis )
                    * Eigen::Vector3d::UnitY( ) ) );

            // Calculated acceleration generated by the panels whose normals are along -Y-axis.
            double cosinusPanelInclinationNegativeYaxis = expectedVehicleToSunNormalisedVector.dot( - Eigen::Vector3d::UnitY() );
            Eigen::Vector3d accelerationNegativeYaxisOrientedPanels = - cosinusPanelInclinationNegativeYaxis
                    * radiationPressure / bodyMap[ "Vehicle" ]->getBodyMass()
                    * ( areas[9] * ( ( 1.0 - emissivities[9] ) * expectedVehicleToSunNormalisedVector
                    + 2.0 / 3.0 * diffuseReflectionCoefficients[9] * - Eigen::Vector3d::UnitY( ) )
                    + areas[10] * ( ( 1.0 - emissivities[10] ) * expectedVehicleToSunNormalisedVector
                    + ( 2.0 / 3.0 * diffuseReflectionCoefficients[10] + 2.0 * emissivities[10] * cosinusPanelInclinationNegativeYaxis )
                    * - Eigen::Vector3d::UnitY( ) ) );

            // Calculated acceleration generated by the panels whose normals are along +Z-axis.
            double cosinusPanelInclinationPositiveZaxis = expectedVehicleToSunNormalisedVector.dot( Eigen::Vector3d::UnitZ() );
            Eigen::Vector3d accelerationPositiveZaxisOrientedPanels = - cosinusPanelInclinationPositiveZaxis
                    * radiationPressure / bodyMap[ "Vehicle" ]->getBodyMass()
                    * ( areas[0] * ( ( 1.0 - emissivities[0] ) * expectedVehicleToSunNormalisedVector
                    + 2.0 / 3.0 * diffuseReflectionCoefficients[0] * Eigen::Vector3d::UnitZ( ) )
                    + areas[1] * ( ( 1.0 - emissivities[1] ) * expectedVehicleToSunNormalisedVector
                    + ( 2.0 / 3.0 * diffuseReflectionCoefficients[1] + 2.0 * cosinusPanelInclinationPositiveZaxis * emissivities[1] )
                    * Eigen::Vector3d::UnitZ( ) ) );

            // Calculated acceleration generated by the panels whose normals are along -Z-axis.
            double cosinusPanelInclinationNegativeZaxis = expectedVehicleToSunNormalisedVector.dot( - Eigen::Vector3d::UnitZ() );
            Eigen::Vector3d accelerationNegativeZaxisOrientedPanels = - cosinusPanelInclinationNegativeZaxis
                    * radiationPressure / bodyMap[ "Vehicle" ]->getBodyMass()
                    * areas[2] * ( ( 1.0 - emissivities[2] ) * expectedVehicleToSunNormalisedVector
                    + 2.0 / 3.0 * diffuseReflectionCoefficients[2] * - Eigen::Vector3d::UnitZ( ) );

            // Equatorial orbit case: vehicle in circular orbit around the Sun with i = 0 deg.
            if ( testCase == 0 ){

                // At t = 0: Sun-Vehicle vector expected along +X-axis
                // The acceleration is expected to be generated by the panels whose normals are along -X-axis only.
                if ( i == 0 ){
                    expectedAcceleration = accelerationNegativeXaxisOrientedPanels;
                }

                // At t = period/4: Sun-Vehicle vector expected along +Y-axis
                // The acceleration is expected to be generated by the panels whose normals are along -Y-axis only.
                else if ( i == 1 ){
                    expectedAcceleration = accelerationNegativeYaxisOrientedPanels;
                }

                // At t = period/2: Sun-Vehicle vector expected along -X-axis
                // The acceleration is expected to be generated by the panels whose normals are along +X-axis only.
                else if ( i == 2 ){
                    expectedAcceleration = accelerationPositiveXaxisOrientedPanels;
                }

                // At t = 3 * period/4: Sun-Vehicle vector expected along -Y-axis
                // The acceleration is expected to be generated by the panels whose normals are along +Y-axis only.
                else if ( i == 3 ){
                    expectedAcceleration = accelerationPositiveYaxisOrientedPanels;
                }
            }

            // Polar orbit case: vehicle in circular orbit around the Sun with i = 90 deg
            if ( testCase == 1 ){

                // At t = 0: Sun-Vehicle vector expected along +X-axis.
                // The acceleration is expected to be generated by the panels whose normals are along -X-axis only.
                if ( i == 0 ){
                    expectedAcceleration = accelerationNegativeXaxisOrientedPanels;
                }

                // At t = period/4: Sun-Vehicle vector expected along +Z-axis.
                // The acceleration is expected to be generated by the panels whose normals are along -Z-axis only.
                else if ( i == 1 ){
                    expectedAcceleration = accelerationNegativeZaxisOrientedPanels;
                }

                // At t = period/2: Sun-Vehicle vector expected along -X-axis.
                // The acceleration is expected to be generated by the panels whose normals are along +X-axis only.
                else if ( i == 2 ){
                    expectedAcceleration = accelerationPositiveXaxisOrientedPanels;
                }

                // At t = 3*period/4: Sun-Vehicle vector expected along -Z-axis.
                // The acceleration is expected to be generated by the panels whose normals are along +Z-axis only.
                else if ( i == 3 ){
                    expectedAcceleration = accelerationPositiveZaxisOrientedPanels;
                }
            }

            // Circular orbit with arbitrary chosen inclination
            if ( testCase == 2 ){

                // At t = 0: Sun-Vehicle vector expected along +X-axis.
                // The acceleration is expected to be generated by the panels whose normals are along -X-axis only.
                if ( i == 0 ){
                    expectedAcceleration = accelerationNegativeXaxisOrientedPanels;
                }

                // At t = period/4: Sun-vehicle vector expected to have components along +Z and +Y axes.
                // The acceleration is expected to be generated by the panels whose normals are along -Y and -Z axes.
                else if ( i == 1 ){
                    expectedAcceleration = accelerationNegativeZaxisOrientedPanels + accelerationNegativeYaxisOrientedPanels;
                }

                // At t = period/2: Sun-Vehicle vector expected along -X-axis.
                // The acceleration is expected to be generated by the panels whose normals are along +X-axis only.
                else if ( i == 2 ){
                    expectedAcceleration = accelerationPositiveXaxisOrientedPanels;
                }

                // At t = period/4: Sun-vehicle vector expected to have components along -Z and -Y axes.
                // The acceleration is expected to be generated by the panels whose normals are along +Y and +Z axes.
                else if ( i == 3 ){
                    expectedAcceleration = accelerationPositiveZaxisOrientedPanels + accelerationPositiveYaxisOrientedPanels;
                }

            }

            // Case with non constant rotational model for the spacecraft.
            if ( testCase == 3 )
            {
               Eigen::Quaterniond currentRotationToInertialFrame = bodyMap[ "Vehicle" ]->getRotationalEphemeris( )
                        ->getRotationToBaseFrame( testTimes.at( i ) );
               Eigen::Vector3d expectedPanelNormalPositiveXaxis = currentRotationToInertialFrame * Eigen::Vector3d::UnitX();
               Eigen::Vector3d expectedPanelNormalNegativeXaxis = currentRotationToInertialFrame * - Eigen::Vector3d::UnitX();

               double cosinusPanelInclinationPositiveXaxis = expectedVehicleToSunNormalisedVector.dot( expectedPanelNormalPositiveXaxis );

               // Determine the normal of the panels that are actually generating a radiation pressure acceleration,
               // depending on their current inertial orientation.
               Eigen::Vector3d expectedPanelNormal;
               if ( cosinusPanelInclinationPositiveXaxis >= 0.0 ){
                   expectedPanelNormal = expectedPanelNormalPositiveXaxis;
               }
               else{
                   expectedPanelNormal = expectedPanelNormalNegativeXaxis;
               }

               // Calculate expected acceleration (same characteristics for the panels oriented along the -X and +X axes).
               expectedAcceleration = - radiationPressure / bodyMap[ "Vehicle" ]->getBodyMass()
                       * ( areas[0] * fabs( cosinusPanelInclinationPositiveXaxis ) * ( ( 1.0 - emissivities[0] ) * expectedVehicleToSunNormalisedVector
                       + 2.0 / 3.0 * diffuseReflectionCoefficients[0] * expectedPanelNormal )
                       + areas[1] * fabs( cosinusPanelInclinationPositiveXaxis ) * ( ( 1.0 - emissivities[1] ) * expectedVehicleToSunNormalisedVector
                       + ( 2.0 / 3.0 * diffuseReflectionCoefficients[1] + 2.0 * fabs(cosinusPanelInclinationPositiveXaxis) * emissivities[1] )
                       * expectedPanelNormal ) );
            }

            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( std::fabs( calculatedAcceleration( j ) - expectedAcceleration( j ) ), 3.0E-23 );
            }

        }
    }

}

//! Test panelled radiation acceleration model with time-varying solar panels orientation.
BOOST_AUTO_TEST_CASE( testPanelledRadiationPressureTimeVaryingPanelOrientation )
{

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

    // Create bodies needed in simulation
    double initialEphemerisTime = 0.0;
    double finalEphemerisTime = 1.1 * 365.25 * 86400.0;
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Sun" );
    NamedBodyMap bodyMap = createBodies(
                getDefaultBodySettings( bodyNames,initialEphemerisTime, finalEphemerisTime ) );

    // Create vehicle
    double vehicleMass = 2000.0;
    bodyMap[ "Vehicle" ] = std::make_shared< Body >( );
    bodyMap[ "Vehicle" ]->setConstantBodyMass( vehicleMass );

    // Compute spacecraft orbital period, and compute test times
    double orbitalPeriod = 2.0 * mathematical_constants::PI * std::sqrt( std::pow( physical_constants::ASTRONOMICAL_UNIT, 3.0 ) /
                                                                         spice_interface::getBodyGravitationalParameter( "Sun" ) );
    std::vector< double > testTimes = { 0.0, orbitalPeriod / 4.0, orbitalPeriod / 2.0, 3.0 * orbitalPeriod / 4.0 };

    // Put vehicle on circular orbit around Sun.
    Eigen::Vector6d initialStateInKeplerianElements = Eigen::Vector6d::Zero( );
    initialStateInKeplerianElements[ 0 ] = physical_constants::ASTRONOMICAL_UNIT;
    bodyMap[ "Vehicle" ]->setEphemeris( std::make_shared< KeplerEphemeris >( initialStateInKeplerianElements, 0.0,
                                                 spice_interface::getBodyGravitationalParameter( "Sun" ), "Sun", "ECLIPJ2000", 1 ) );

    // Create radiation pressure properties.
    std::vector< double > areas;
    areas.push_back( 2.0 );
    areas.push_back( 4.0 );

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

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

    // Define (constant) panel surface orientations.
    std::vector< Eigen::Vector3d > panelSurfaceNormals;
    panelSurfaceNormals.push_back( Eigen::Vector3d::UnitX( ) );
    panelSurfaceNormals.push_back( - Eigen::Vector3d::UnitX( ) );

    // Define rotational model parameters.
    std::vector< double > rightAscensionPole;
    rightAscensionPole.push_back( 0.0 );
    rightAscensionPole.push_back( 0.2 );

    std::vector< double > declinationPole;
    declinationPole.push_back( mathematical_constants::PI / 2.0 );
    declinationPole.push_back( 0.4 );

    std::vector< double > primeMeridianLongitude;
    primeMeridianLongitude.push_back( - mathematical_constants::PI / 2.0 );
    primeMeridianLongitude.push_back( - 0.2 );

    std::vector< double > rotationalRate;
    rotationalRate.push_back( 1.0E-5 );
    rotationalRate.push_back( 1.0E-5 );

    std::vector< double > numberSecondsSinceEpoch;
    numberSecondsSinceEpoch.push_back( 0.0 );
    numberSecondsSinceEpoch.push_back( 0.0 );

    // Case 0: vehicle-fixed axes aligned with inertial ones at time t = 0.
    // Case 1: arbitrary chosen rotational model for the vehicle.

    for ( unsigned int testCase = 0 ; testCase < 2 ; testCase++){

        /// First calculation with simple rotational ephemeris and constant panel orientation.

        // Define simple rotational ephemeris.
        bodyMap[ "Vehicle" ]->setRotationalEphemeris( std::make_shared< tudat::ephemerides::SimpleRotationalEphemeris >(
                        rightAscensionPole[ testCase ], declinationPole[ testCase ],  primeMeridianLongitude[ testCase ],
                        rotationalRate[ testCase ], numberSecondsSinceEpoch[ testCase ], "ECLIPJ2000", "VehicleFixed" ) );

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

        // Create panelled radiation pressure interface.
        std::shared_ptr< PanelledRadiationPressureInterfaceSettings > radiationPressureInterfaceSettings =
                std::make_shared< PanelledRadiationPressureInterfaceSettings >(
                    "Sun", emissivities, areas, diffuseReflectionCoefficients, panelSurfaceNormals );

        std::shared_ptr< PanelledRadiationPressureInterface > radiationPressureInterface =
                std::dynamic_pointer_cast< PanelledRadiationPressureInterface >(
                    createRadiationPressureInterface( radiationPressureInterfaceSettings, "Vehicle", bodyMap ) );

        bodyMap[ "Vehicle" ]->setRadiationPressureInterface( "Sun", radiationPressureInterface );

        // Define accelerations.
        SelectedAccelerationMap accelerationMap;
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOnVehicle;
        accelerationsOnVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                       panelled_radiation_pressure_acceleration ) );

        accelerationMap[ "Vehicle" ] = accelerationsOnVehicle;

        std::map< std::string, std::string > centralBodyMap; centralBodyMap[ "Vehicle" ] = "Sun";
        AccelerationMap accelerationModelMap = createAccelerationModelsMap( bodyMap, accelerationMap, centralBodyMap );
        std::shared_ptr< AccelerationModel< Eigen::Vector3d > > accelerationModel =
                accelerationModelMap.at( "Vehicle" ).at( "Sun" ).at( 0 );

        // Compute radiation pressure acceleration for different Sun positions.
        std::vector< Eigen::Vector3d > calculatedAcceleration;

        Eigen::Vector3d sunCenteredVehiclePosition;
        std::shared_ptr< Ephemeris > vehicleEphemeris = bodyMap[ "Vehicle" ]->getEphemeris( );

        for( unsigned int i = 0; i < testTimes.size( ) ; i++ )
        {
            // Update environment and acceleration
            bodyMap[ "Sun" ]->setStateFromEphemeris( testTimes[ i ] );
            bodyMap[ "Vehicle" ]->setStateFromEphemeris( testTimes[ i ] );
            bodyMap[ "Vehicle" ]->setCurrentRotationToLocalFrameFromEphemeris( testTimes[ i ] );
            radiationPressureInterface->updateInterface( testTimes[ i ] );
            accelerationModel->updateMembers( testTimes[ i ] );

            // Retrieve acceleration.
            calculatedAcceleration.push_back( accelerationModel->getAcceleration( ) );

        }

        /// Second calculation with constant rotational ephemeris and time-varying panel orientation

        // Define constant rotational ephemeris
        Eigen::Vector7d rotationalStateVehicle;
        rotationalStateVehicle.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat( Eigen::Quaterniond( Eigen::Matrix3d::Identity() ));
        rotationalStateVehicle.segment( 4, 3 ) = Eigen::Vector3d::Zero();
        bodyMap[ "Vehicle" ]->setRotationalEphemeris( std::make_shared< ConstantRotationalEphemeris >( rotationalStateVehicle, "ECLIPJ2000",
                                                                                "VehicleFixed" ) );

        // Define time-varying panel orientation.
        std::vector< std::function< Eigen::Vector3d ( const double ) > > timeVaryingPanelSurfaceNormals;
        for ( unsigned int j = 0 ; j < panelSurfaceNormals.size() ; j++ )
        {
            timeVaryingPanelSurfaceNormals.push_back( [ = ]( const double currentTime ){

                Eigen::Vector3d currentPanelSurfaceOrientation = (
                            reference_frames::getInertialToPlanetocentricFrameTransformationQuaternion(
                                basic_mathematics::computeModulo( ( currentTime - numberSecondsSinceEpoch[ testCase ] )
                                                                  * rotationalRate[ testCase ], 2.0 * mathematical_constants::PI ) )
                          * reference_frames::getInertialToPlanetocentricFrameTransformationQuaternion( declinationPole[ testCase ],
                                                                                rightAscensionPole[ testCase ],
                                                                                primeMeridianLongitude[ testCase ] ) )
                          .toRotationMatrix().inverse() * panelSurfaceNormals[ j ];

                return currentPanelSurfaceOrientation; } );
        }

        // Create panelled radiation pressure interface.
        std::shared_ptr< PanelledRadiationPressureInterfaceSettings > radiationPressureInterfaceSettingsWithTimeVaryingPanelSurfaceNormal =
                std::make_shared< PanelledRadiationPressureInterfaceSettings >(
                    "Sun", emissivities, areas, diffuseReflectionCoefficients, timeVaryingPanelSurfaceNormals );

        std::shared_ptr< PanelledRadiationPressureInterface > radiationPressureInterfaceTimeVaryingSurfaceNormal =
                std::dynamic_pointer_cast< PanelledRadiationPressureInterface >(
                    createRadiationPressureInterface( radiationPressureInterfaceSettingsWithTimeVaryingPanelSurfaceNormal, "Vehicle", bodyMap ) );

        bodyMap[ "Vehicle" ]->setRadiationPressureInterface( "Sun", radiationPressureInterfaceTimeVaryingSurfaceNormal );

        accelerationModelMap = createAccelerationModelsMap( bodyMap, accelerationMap, centralBodyMap );

        std::shared_ptr< AccelerationModel< Eigen::Vector3d > > accelerationModelTimeVaryingPanelSurfaceNormal =
                accelerationModelMap.at( "Vehicle" ).at( "Sun" ).at( 0 );

        // Compute radiation pressure acceleration for different Sun positions.
        std::vector< Eigen::Vector3d > calculatedAccelerationTimeVaryingPanelOrientation;

        for( unsigned int i = 0; i < testTimes.size( ) ; i++ )
        {
            // Update environment and acceleration
            bodyMap[ "Sun" ]->setStateFromEphemeris( testTimes[ i ] );
            bodyMap[ "Vehicle" ]->setStateFromEphemeris( testTimes[ i ] );
            bodyMap[ "Vehicle" ]->setCurrentRotationToLocalFrameFromEphemeris( testTimes[ i ] );
            radiationPressureInterfaceTimeVaryingSurfaceNormal->updateInterface( testTimes[ i ] );
            accelerationModelTimeVaryingPanelSurfaceNormal->updateMembers( testTimes[ i ] );

            // Retrieve acceleration.
            calculatedAccelerationTimeVaryingPanelOrientation.push_back( accelerationModelTimeVaryingPanelSurfaceNormal->getAcceleration( ) );
        }

        for( unsigned int j = 0; j < testTimes.size() ; j++ )
        {
            for ( unsigned int i = 0 ; i < 3 ; i++ ){
                BOOST_CHECK_SMALL( std::fabs(
                                       calculatedAcceleration[ j ][ i ] - calculatedAccelerationTimeVaryingPanelOrientation[ j ][ i ] ), 3.0E-23 );
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


/*

Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder(s) nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <string>

#include <math.h>
#include <stdio.h>
#include <ctime>
#include <time.h>

#include "quality.h"
#include "PGR_5.h"
#include "normal_component_box_surface.h"

using namespace std;
using namespace Eigen;
using namespace KDL;
const double PPI_  = 3.141592653589793238463;

const double PI_  = 3.141592653589793238463;

//////////////////////////////////  BOX

std::vector<double> box;
//////////////////////////////////////////////

/////////////////////////////////// CONTACT POINTS

std::vector<double> contact_points;
///////////////////////////////////////

////////////////////////////////////  CONTACT FORCE

std::vector<double> contact_forces;
/////////////////////////////////////////////

////////////////////////////////////// JOINTS

std::vector<double> joints;
////////////////////////////////////////////

int n_c = 0;
int n_q = 0;
double quality_i = 0;
double contact_stiffness = 100;
double joint_stiffness = 1;
double mu = 1.5;
double f_i_max = 200;

//////////////////////////////////////////// ROBOT SCTRUCTURE

KDL::Tree hand_tree("RootName");

  KDL::Chain chain_palm;

  KDL::Chain chain_finger_right_0;
  KDL::Chain chain_finger_left_0;
  KDL::Chain chain_finger_right_1;
  KDL::Chain chain_finger_left_1;
  KDL::Chain chain_finger_right_2;
  KDL::Chain chain_finger_left_2;

  KDL::Chain chain_palm_finger_left_0; // return the result
  KDL::Chain chain_palm_finger_left_1;
  KDL::Chain chain_palm_finger_left_2;
  KDL::Chain chain_palm_finger_right_0;
  KDL::Chain chain_palm_finger_right_1;
  KDL::Chain chain_palm_finger_right_2;

  KDL::JntArray jointpositions_right_0 = JntArray(8);
  KDL::JntArray jointpositions_right_1 = JntArray(8);
  KDL::JntArray jointpositions_right_2 = JntArray(8);
  KDL::JntArray jointpositions_left_0 = JntArray(8);
  KDL::JntArray jointpositions_left_1 = JntArray(8);
  KDL::JntArray jointpositions_left_2 = JntArray(8);

  KDL::Jacobian jacobian_right_0;
  KDL::Jacobian jacobian_left_0;
  KDL::Jacobian jacobian_right_1;
  KDL::Jacobian jacobian_left_1;
  KDL::Jacobian jacobian_right_2;
  KDL::Jacobian jacobian_left_2;

  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_right_0;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_left_0;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_right_1;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_left_1;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_right_2;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_left_2;

///////////////////////////////////////////////////////////

//------------------------------------------------------------------------------------------
void createStructureKDL()
{

  chain_palm.addSegment(Segment("palm_trasl_x_link",Joint(Joint::TransX),Frame(Vector(0.0,0.0,0.0))));
  chain_palm.addSegment(Segment("palm_trasl_y_link",Joint(Joint::TransY),Frame(Vector(0.0,0.0,0.0))));
  chain_palm.addSegment(Segment("palm_trasl_z_link",Joint(Joint::TransZ),Frame(Vector(0.0,0.0,0.0))));
  chain_palm.addSegment(Segment("palm_rot_x_link",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.0))));
  chain_palm.addSegment(Segment("palm_rot_y_link",Joint(Joint::RotY),Frame(Vector(0.0,0.0,0.0))));
  chain_palm.addSegment(Segment("palm_rot_z_link",Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.0))));

  chain_finger_right_0.addSegment(Segment("finger_right_proximal_link_0",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));
  chain_finger_right_0.addSegment(Segment("finger_right_distal_link_0",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  chain_finger_left_0.addSegment(Segment("finger_left_proximal_link_0",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.3))));
  chain_finger_left_0.addSegment(Segment("finger_left_distal_link_0",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  chain_finger_right_1.addSegment(Segment("finger_right_proximal_link_1",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));
  chain_finger_right_1.addSegment(Segment("finger_right_distal_link_1",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  chain_finger_left_1.addSegment(Segment("finger_left_proximal_link_1",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.3))));
  chain_finger_left_1.addSegment(Segment("finger_left_distal_link_1",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  chain_finger_right_2.addSegment(Segment("finger_right_proximal_link_2",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));
  chain_finger_right_2.addSegment(Segment("finger_right_distal_link_2",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  chain_finger_left_2.addSegment(Segment("finger_left_proximal_link_2",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.3))));
  chain_finger_left_2.addSegment(Segment("finger_left_distal_link_2",Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.03))));

  hand_tree.addChain(chain_palm,"RootName");
  hand_tree.addChain(chain_finger_right_0,"palm_rot_z_link");
  hand_tree.addChain(chain_finger_left_0, "palm_rot_z_link");
  hand_tree.addChain(chain_finger_right_1,"palm_rot_z_link");
  hand_tree.addChain(chain_finger_left_1, "palm_rot_z_link");
  hand_tree.addChain(chain_finger_right_2,"palm_rot_z_link");
  hand_tree.addChain(chain_finger_left_2, "palm_rot_z_link");

  hand_tree.getChain("RootName", "finger_right_distal_link_0", chain_palm_finger_right_0);
  hand_tree.getChain("RootName", "finger_left_distal_link_0", chain_palm_finger_left_0);
  hand_tree.getChain("RootName", "finger_right_distal_link_1", chain_palm_finger_right_1);
  hand_tree.getChain("RootName", "finger_left_distal_link_1", chain_palm_finger_left_1);
  hand_tree.getChain("RootName", "finger_right_distal_link_2", chain_palm_finger_right_2);
  hand_tree.getChain("RootName", "finger_left_distal_link_2", chain_palm_finger_left_2);

}
//-------------------------------------------------------------------------------------------

//-------------------------------------------------------------------------------------------
void setJoints(std::vector<double>  joints_in)
{
  for(int i=0 ; i < 6 ; i++)
      jointpositions_right_0(i) = jointpositions_left_0(i) = jointpositions_right_1(i) = jointpositions_left_1(i) = jointpositions_right_2(i) = jointpositions_left_2(i) = joints[i];

    jointpositions_right_0(6) = joints_in[6];
    jointpositions_right_0(7) = joints_in[7];

    jointpositions_left_0(6) = joints_in[8];
    jointpositions_left_0(7) = joints_in[9];

    jointpositions_right_1(6) = joints_in[10];
    jointpositions_right_1(7) = joints_in[11];

    jointpositions_left_1(6) = joints_in[12];
    jointpositions_left_1(7) = joints_in[13];

    jointpositions_right_2(6) = joints_in[14];
    jointpositions_right_2(7) = joints_in[15];

    jointpositions_left_2(6) = joints_in[16];
    jointpositions_left_2(7) = joints_in[17];

  jointpositions_right_0.resize(chain_palm_finger_right_0.getNrOfJoints());
  jointpositions_left_0.resize(chain_palm_finger_left_0.getNrOfJoints());

  jointpositions_right_1.resize(chain_palm_finger_right_1.getNrOfJoints());
  jointpositions_left_1.resize(chain_palm_finger_left_1.getNrOfJoints());

  jointpositions_right_2.resize(chain_palm_finger_right_2.getNrOfJoints());
  jointpositions_left_2.resize(chain_palm_finger_left_2.getNrOfJoints());
}

void initJacobian()
{
    jacobian_right_0.resize(chain_palm_finger_right_0.getNrOfJoints());
    jacobian_left_0.resize(chain_palm_finger_left_0.getNrOfJoints());

    jnt_to_jac_right_0.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_right_0));
    jnt_to_jac_left_0.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_left_0));

    jacobian_right_1.resize(chain_palm_finger_right_1.getNrOfJoints());
    jacobian_left_1.resize(chain_palm_finger_left_1.getNrOfJoints());

    jnt_to_jac_right_1.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_right_1));
    jnt_to_jac_left_1.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_left_1));

    jacobian_right_2.resize(chain_palm_finger_right_2.getNrOfJoints());
    jacobian_left_2.resize(chain_palm_finger_left_2.getNrOfJoints());

    jnt_to_jac_right_2.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_right_2));
    jnt_to_jac_left_2.reset(new KDL::ChainJntToJacSolver(chain_palm_finger_left_2));
}

int main (int argc, char **argv)
{

  ros::init(argc, argv, "Test_file_4_finger");
  ros::NodeHandle nh;

  nh.param<std::vector<double>>("box", box, std::vector<double>{1, 1, 1});
  nh.param<std::vector<double>>("contact_points", contact_points, std::vector<double>{1,1,1,1,1,1});
  nh.param<std::vector<double>>("contact_forces", contact_forces, std::vector<double>{1,1,1,1,1,1});
  nh.param<std::vector<double>>("joints", joints, std::vector<double>{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
  nh.param<double>("mu", mu, 0.5);
  nh.param<double>("f_i_max", f_i_max, 1);
  nh.param<int>("n_c", n_c, 0);
  nh.param<int>("n_q", n_q, 0);

  if(n_c <= 0)
    return 0;

  createStructureKDL();
  setJoints(joints);
  initJacobian();

  Eigen::MatrixXd Skew_Matrix1(3,3);
  Eigen::MatrixXd Skew_Matrix2(3,3);
  Eigen::MatrixXd Skew_Matrix3(3,3);
  Eigen::MatrixXd Skew_Matrix4(3,3);
  Eigen::MatrixXd Skew_Matrix5(3,3);
  Eigen::MatrixXd Skew_Matrix6(3,3);

  Skew_Matrix1(0,0) = Skew_Matrix1(1,1) = Skew_Matrix1(2,2) = 0;
      Skew_Matrix1(0,1) = - contact_points[2]; // -rz
      Skew_Matrix1(0,2) = contact_points[1];   // ry
      Skew_Matrix1(1,0) = contact_points[2];   // rz
      Skew_Matrix1(2,0) = - contact_points[1]; // -ry
      Skew_Matrix1(1,2) = - contact_points[0]; // -rx
      Skew_Matrix1(2,1) = contact_points[0];   // rx
  // left

 Skew_Matrix2(0,0) = Skew_Matrix2(1,1) = Skew_Matrix2(2,2) = 0;
      Skew_Matrix2(0,1) = - contact_points[5]; // -rz
      Skew_Matrix2(0,2) = contact_points[4];   // ry
      Skew_Matrix2(1,0) = contact_points[5];   // rz
      Skew_Matrix2(2,0) = - contact_points[4]; // -ry
      Skew_Matrix2(1,2) = - contact_points[3]; // -rx
      Skew_Matrix2(2,1) = contact_points[3];   // rx
  // right

  Skew_Matrix3(0,0) = Skew_Matrix3(1,1) = Skew_Matrix3(2,2) = 0;
      Skew_Matrix3(0,1) = - contact_points[8]; // -rz
      Skew_Matrix3(0,2) = contact_points[7];   // ry
      Skew_Matrix3(1,0) = contact_points[8];   // rz
      Skew_Matrix3(2,0) = - contact_points[7]; // -ry
      Skew_Matrix3(1,2) = - contact_points[6]; // -rx
      Skew_Matrix3(2,1) = contact_points[6];   // rx
  // up

 Skew_Matrix4(0,0) = Skew_Matrix4(1,1) = Skew_Matrix4(2,2) = 0;
      Skew_Matrix4(0,1) = - contact_points[11]; // -rz
      Skew_Matrix4(0,2) = contact_points[10];   // ry
      Skew_Matrix4(1,0) = contact_points[11];   // rz
      Skew_Matrix4(2,0) = - contact_points[10]; // -ry
      Skew_Matrix4(1,2) = - contact_points[9]; // -rx
      Skew_Matrix4(2,1) = contact_points[9];   // rx
  // down

  Skew_Matrix5(0,0) = Skew_Matrix5(1,1) = Skew_Matrix5(2,2) = 0;
      Skew_Matrix5(0,1) = - contact_points[14]; // -rz
      Skew_Matrix5(0,2) = contact_points[13];   // ry
      Skew_Matrix5(1,0) = contact_points[14];   // rz
      Skew_Matrix5(2,0) = - contact_points[13]; // -ry
      Skew_Matrix5(1,2) = - contact_points[12]; // -rx
      Skew_Matrix5(2,1) = contact_points[12];   // rx
  // front of

 Skew_Matrix6(0,0) = Skew_Matrix6(1,1) = Skew_Matrix6(2,2) = 0;
      Skew_Matrix6(0,1) = - contact_points[17]; // -rz
      Skew_Matrix6(0,2) = contact_points[16];   // ry
      Skew_Matrix6(1,0) = contact_points[17];   // rz
      Skew_Matrix6(2,0) = - contact_points[16]; // -ry
      Skew_Matrix6(1,2) = - contact_points[15]; // -rx
      Skew_Matrix6(2,1) = contact_points[15];   // rx
  // behind

Eigen::MatrixXd G_c_t = MatrixXd::Zero(6, 6*n_c);
Eigen::MatrixXd G1 = MatrixXd::Zero(6,6);
Eigen::MatrixXd G2 = MatrixXd::Zero(6,6);
Eigen::MatrixXd G3 = MatrixXd::Zero(6,6);
Eigen::MatrixXd G4 = MatrixXd::Zero(6,6);
Eigen::MatrixXd G5 = MatrixXd::Zero(6,6);
Eigen::MatrixXd G6 = MatrixXd::Zero(6,6);

Eigen::MatrixXd R1(3,3);
Eigen::MatrixXd R2(3,3);
Eigen::MatrixXd R3(3,3);
Eigen::MatrixXd R4(3,3);
Eigen::MatrixXd R5(3,3);
Eigen::MatrixXd R6(3,3);

R1 << 1, 0, 0,
       0, cos(-90*PPI_/180), -sin(-90*PPI_/180),
       0, sin(-90*PPI_/180), cos(-90*PPI_/180);
// left

R2 << 1, 0, 0,
      0, cos(90*PPI_/180), -sin(90*PPI_/180),
      0, sin(90*PPI_/180), cos(90*PPI_/180);
// right

R3 << 1, 0, 0,
      0, cos(180*PPI_/180), -sin(180*PPI_/180),
      0, sin(180*PPI_/180), cos(180*PPI_/180);
// up

R4 << 1, 0, 0,
      0, 1, 0,
      0, 0, 1;
// down

R5 <<  cos(-90*PPI/180),  0, sin(-90*PPI/180),
                   0   ,  1,    0 ,
      -sin(-90*PPI/180),  0, cos(-90*PPI/180);
//front of

R6 << cos(90*PPI/180),  0, sin(90*PPI/180),
                 0   ,  1,    0 ,
      -sin(90*PPI/180), 0, cos(90*PPI/180);
//behind

G1.block<3,3>(0,0) = R1.transpose();
G1.block<3,3>(3,3) = R1.transpose();
G1.block<3,3>(3,0) = Skew_Matrix1 * R1.transpose();

G2.block<3,3>(0,0) = R2.transpose();
G2.block<3,3>(3,3) = R2.transpose();
G2.block<3,3>(3,0) = Skew_Matrix2 * R2.transpose();

G3.block<3,3>(0,0) = R3.transpose();
G3.block<3,3>(3,3) = R3.transpose();
G3.block<3,3>(3,0) = Skew_Matrix3 * R3.transpose();

G4.block<3,3>(0,0) = R4.transpose();
G4.block<3,3>(3,3) = R4.transpose();
G4.block<3,3>(3,0) = Skew_Matrix4 * R4.transpose();

G5.block<3,3>(0,0) = R5.transpose();
G5.block<3,3>(3,3) = R5.transpose();
G5.block<3,3>(3,0) = Skew_Matrix5 * R5.transpose();

G6.block<3,3>(0,0) = R6.transpose();
G6.block<3,3>(3,3) = R6.transpose();
G6.block<3,3>(3,0) = Skew_Matrix6 * R6.transpose();

G_c_t.block<6,6>(0,0) = G1;
G_c_t.block<6,6>(0,6) = G2;
G_c_t.block<6,6>(0,12) = G3;
G_c_t.block<6,6>(0,18) = G4;
G_c_t.block<6,6>(0,24) = G5;
G_c_t.block<6,6>(0,30) = G6;

Eigen::MatrixXd Jacobian = MatrixXd::Zero(6*n_c,n_q);

jnt_to_jac_right_0->JntToJac(jointpositions_right_0, jacobian_right_0);
jnt_to_jac_left_0->JntToJac(jointpositions_left_0, jacobian_left_0);

jnt_to_jac_right_1->JntToJac(jointpositions_right_1, jacobian_right_1);
jnt_to_jac_left_1->JntToJac(jointpositions_left_1, jacobian_left_1);

jnt_to_jac_right_2->JntToJac(jointpositions_right_2, jacobian_right_2);
jnt_to_jac_left_2->JntToJac(jointpositions_left_2, jacobian_left_2);

Jacobian.block<6,8>(0,0) = jacobian_right_0.data;                  // first contact - first finger

Jacobian.block<6,6>(6,0) = jacobian_left_0.data.block<6,6>(0,0);   // second contact - second finger
Jacobian.block<6,2>(6,8) = jacobian_left_0.data.block<6,2>(0,6);

Jacobian.block<6,6>(12,0) = jacobian_right_1.data.block<6,6>(0,0); // third contact - third finger
Jacobian.block<6,2>(12,10) = jacobian_right_1.data.block<6,2>(0,6);

Jacobian.block<6,6>(18,0) = jacobian_left_1.data.block<6,6>(0,0);  // four-th contact - four-th finger
Jacobian.block<6,2>(18,12) = jacobian_left_1.data.block<6,2>(0,6);

Jacobian.block<6,6>(24,0) = jacobian_right_2.data.block<6,6>(0,0);  // five-th contact - five-th finger
Jacobian.block<6,2>(24,14) = jacobian_right_2.data.block<6,2>(0,6);

Jacobian.block<6,6>(30,0) = jacobian_left_2.data.block<6,6>(0,0);  // six-th contact - six-th finger
Jacobian.block<6,2>(30,16) = jacobian_left_2.data.block<6,2>(0,6);

    Eigen::MatrixXd R_c = MatrixXd::Zero(3*n_c,3*n_c);
    int k = 0;
    for(int i = 0 ; i < n_c ; i++)
    {
      R_c.block<3,3>(k,k) = MatrixXd::Identity(3,3);
      k += 3;
    }

    Eigen::VectorXd f_c_(contact_forces.size());
    for(int i = 0 ; i < contact_forces.size() ; i++ )
      f_c_(i) = contact_forces[i];

    Eigen::MatrixXd Contact_Stiffness_Matrix = MatrixXd::Zero(3,3);   // Kis
      for(int i = 0 ; i < Contact_Stiffness_Matrix.rows() ; i++) //Kis
          Contact_Stiffness_Matrix(i,i) = contact_stiffness;

    Eigen::MatrixXd Joint_Stiffness_Matrix = MatrixXd::Zero(n_q,n_q);    // Kp
      for(int j = 0 ; j < Joint_Stiffness_Matrix.rows() ; j++) //Kp
          Joint_Stiffness_Matrix(j,j) = joint_stiffness;

  //quality_i = quality_pcr_pgr_5(f_c_, G_c, Jacobian, R_c, Contact_Stiffness_Matrix, Joint_Stiffness_Matrix, mu, f_i_max);
 // quality_i = quality_pcr_pgr_5(f_c_, G_b, Jacobian, R_c, Contact_Stiffness_Matrix, Joint_Stiffness_Matrix, mu, f_i_max);

  quality_i = quality_pcr_pgr_5(f_c_, G_c_t, Jacobian, R_c, Contact_Stiffness_Matrix, Joint_Stiffness_Matrix, mu, f_i_max);

  cout << " quality : " << quality_i << endl;

  ros::spinOnce();
  return 0;
}


/*
 * @copyright Copyright (c) 2017 CERN and the Allpix Squared authors.
 * This software is distributed under the terms of the MIT License, copied verbatim in the file "LICENSE.md".
 * In applying this license, CERN does not waive the privileges and immunities granted to it by virtue of its status as an
 * Intergovernmental Organization or submit itself to any jurisdiction.
 */

#include "CapacitiveTransferModule.hpp"

#include <fstream>
#include <limits>
#include <memory>
#include <new>
#include <stdexcept>
#include <string>
#include <utility>

#include "core/config/exceptions.h"
#include "core/geometry/HybridPixelDetectorModel.hpp"
#include "core/utils/log.h"
#include "core/utils/unit.h"
#include "tools/ROOT.h"

#include <Eigen/Core>

#include "objects/PixelCharge.hpp"

using namespace allpix;

CapacitiveTransferModule::CapacitiveTransferModule(Configuration config,
                                                   Messenger* messenger,
                                                   std::shared_ptr<Detector> detector)
    : Module(std::move(config), detector), messenger_(messenger), detector_(std::move(detector)) {
    // Enable parallelization of this module if multithreading is enabled
    enable_parallelization();

    model_ = detector_->getModel();
    config_.setDefault("output_plots", 0);
    config_.setDefault("cross_coupling", 1);
    config_.setDefault("nominal_gap", 0.0);
    config_.setDefault("minimum_gap", config_.get<double>("nominal_gap"));

    // Require propagated deposits for single detector
    messenger->bindSingle(this, &CapacitiveTransferModule::propagated_message_, MsgFlags::REQUIRED);
}

void CapacitiveTransferModule::init() {

    if(config_.count({"coupling_matrix", "coupling_file", "coupling_scan_file"}) > 1) {
        throw InvalidCombinationError(
            config_, {"coupling_matrix", "coupling_file", "coupling_scan_file"}, "More than one coupling input defined");
    } else if(config_.has("coupling_matrix")) {
        relative_coupling = config_.getMatrix<double>("coupling_matrix");
        max_row = static_cast<unsigned int>(relative_coupling.size());
        max_col = static_cast<unsigned int>(relative_coupling[0].size());

        if(config_.get<bool>("output_plots")) {
            LOG(TRACE) << "Creating output plots";

            // Create histograms if needed
            coupling_map = new TH2D("coupling_map",
                                    "Coupling;pixel x;pixel y",
                                    static_cast<int>(max_col),
                                    -0.5,
                                    static_cast<int>(max_col) - 0.5,
                                    static_cast<int>(max_row),
                                    -0.5,
                                    static_cast<int>(max_row) - 0.5);

            for(size_t col = 0; col < max_col; col++) {
                for(size_t row = 0; row < max_row; row++) {
                    coupling_map->SetBinContent(
                        static_cast<int>(col + 1), static_cast<int>(row + 1), relative_coupling[max_row - row - 1][col]);
                }
            }
        }

        LOG(STATUS) << max_col << "x" << max_row << " coupling matrix imported from config file";

    } else if(config_.has("coupling_file")) {
        LOG(TRACE) << "Reading cross-coupling matrix file " << config_.get<std::string>("coupling_file");
        std::ifstream input_file(config_.getPath("coupling_file", true), std::ifstream::in);
        if(!input_file.good()) {
            throw InvalidValueError(config_, "coupling_file", "Matrix file not found");
        }

        double dummy = -1;
        std::string file_line;
        matrix_rows = 0;
        while(getline(input_file, file_line)) {
            std::stringstream mystream(file_line);
            matrix_cols = 0;
            while(mystream >> dummy) {
                matrix_cols++;
            }
            matrix_rows++;
        }
        input_file.clear();
        input_file.seekg(0, std::ios::beg);

        relative_coupling.resize(matrix_rows);
        for(auto& el : relative_coupling) {
            el.resize(matrix_cols);
        }

        max_col = matrix_cols;
        max_row = matrix_rows;

        size_t row = matrix_rows - 1, col = 0;
        while(getline(input_file, file_line)) {
            std::stringstream mystream(file_line);
            col = 0;
            while(mystream >> dummy) {
                relative_coupling[col][row] = dummy;
                col++;
            }
            row--;
        }
        input_file.close();

        if(config_.get<bool>("output_plots")) {
            LOG(TRACE) << "Creating output plots";

            // Create histograms if needed
            coupling_map = new TH2D("coupling_map",
                                    "Coupling;pixel x;pixel y",
                                    static_cast<int>(max_col),
                                    -0.5,
                                    static_cast<int>(max_col) - 0.5,
                                    static_cast<int>(max_row),
                                    -0.5,
                                    static_cast<int>(max_row) - 0.5);

            for(col = 0; col < max_col; col++) {
                for(row = 0; row < max_row; row++) {
                    coupling_map->SetBinContent(
                        static_cast<int>(col + 1), static_cast<int>(row + 1), relative_coupling[col][row]);
                }
            }
        }

        LOG(STATUS) << matrix_cols << "x" << matrix_rows << " capacitance matrix imported from file "
                    << config_.get<std::string>("coupling_file");

    } else if(config_.has("coupling_scan_file")) {
        TFile* root_file = new TFile(config_.getPath("coupling_scan_file", true).c_str());
        if(root_file->IsZombie()) {
            throw InvalidValueError(config_, "coupling_scan_file", "ROOT file is corrupted. Please, check it");
        }
        for(int i = 1; i < 10; i++) {
            capacitances[i - 1] = static_cast<TGraph*>(root_file->Get(Form("Pixel_%i", i)));
            if(capacitances[i - 1]->IsZombie()) {
                throw InvalidValueError(
                    config_, "coupling_scan_file", "ROOT TGraphs couldn't be imported. Please, check it");
            }
            capacitances[i - 1]->SetBit(TGraph::kIsSortedX);
        }
        matrix_cols = 3;
        matrix_rows = 3;
        if(config_.get<int>("cross_coupling") == 1) {
            max_col = 3;
            max_row = 3;
        } else {
            LOG(STATUS) << "Cross-coupling (neighbour charge transfer) disabled";
            max_col = 1;
            max_row = 1;
        }

        root_file->Delete();

        if(config_.has("minimum_gap")) {
            minimum_gap = config_.get<double>("minimum_gap");
        }
        if(config_.has("nominal_gap")) {
            nominal_gap = config_.get<double>("nominal_gap");
        }
        Eigen::Vector3d origin(0, 0, minimum_gap);

        if(config_.has("tilt_center")) {
            center[0] = config_.get<ROOT::Math::XYPoint>("tilt_center").x() * model_->getPixelSize().x();
            center[1] = config_.get<ROOT::Math::XYPoint>("tilt_center").y() * model_->getPixelSize().y();
            origin = Eigen::Vector3d(center[0], center[1], minimum_gap);
        }

        Eigen::Vector3d rotated_normal(0, 0, 1);
        if(config_.has("chip_angle")) {
            angles[0] = config_.get<ROOT::Math::XYPoint>("chip_angle").x();
            angles[1] = config_.get<ROOT::Math::XYPoint>("chip_angle").y();

            if(angles[0] != 0.0) {
                auto rotation_x = Eigen::AngleAxisd(angles[0], Eigen::Vector3d::UnitX()).toRotationMatrix();
                rotated_normal = rotation_x * rotated_normal;
            }
            if(angles[1] != 0.0) {
                auto rotation_y = Eigen::AngleAxisd(angles[1], Eigen::Vector3d::UnitY()).toRotationMatrix();
                rotated_normal = rotation_y * rotated_normal;
            }
        }

        normalization = 1 / (capacitances[4]->Eval(static_cast<double>(Units::convert(nominal_gap, "um")), nullptr, "S"));
        plane = Eigen::Hyperplane<double, 3>(rotated_normal, origin);

        if(config_.get<bool>("output_plots")) {
            LOG(TRACE) << "Creating output plots";

            // Create histograms if needed
            auto pixel_grid = model_->getNPixels();
            gap_map = new TH2D("gap_map",
                               "Gap;pixel x;pixel y",
                               pixel_grid.x(),
                               -0.5,
                               pixel_grid.x() - 0.5,
                               pixel_grid.y(),
                               -0.5,
                               pixel_grid.y() - 0.5);

            capacitance_map = new TH2D("capacitance_map",
                                       "Capacitance;pixel x;pixel y",
                                       pixel_grid.x(),
                                       -0.5,
                                       pixel_grid.x() - 0.5,
                                       pixel_grid.y(),
                                       -0.5,
                                       pixel_grid.y() - 0.5);

            relative_capacitance_map = new TH2D("relative_capacitance_map",
                                                "Relative Capacitance;pixel x;pixel y",
                                                pixel_grid.x(),
                                                -0.5,
                                                pixel_grid.x() - 0.5,
                                                pixel_grid.y(),
                                                -0.5,
                                                pixel_grid.y() - 0.5);

            for(int col = 0; col < pixel_grid.x(); col++) {
                for(int row = 0; row < pixel_grid.y(); row++) {
                    auto local_x = col * model_->getPixelSize().x();
                    auto local_y = row * model_->getPixelSize().y();

                    Eigen::Vector3d pixel_point(local_x, local_y, 0);
                    Eigen::Vector3d pixel_projection = plane.projection(pixel_point);
                    pixel_gap = pixel_projection[2];

                    gap_map->Fill(col, row, static_cast<double>(Units::convert(pixel_gap, "um")));
                    capacitance_map->Fill(
                        col, row, capacitances[4]->Eval(static_cast<double>(Units::convert(pixel_gap, "um")), nullptr, "S"));
                    relative_capacitance_map->Fill(
                        col,
                        row,
                        capacitances[4]->Eval(static_cast<double>(Units::convert(pixel_gap, "um")), nullptr, "S") /
                            capacitances[4]->Eval(static_cast<double>(Units::convert(nominal_gap, "um")), nullptr, "S"));
                }
            }

            LOG(STATUS) << "Using " << config_.getPath("coupling_scan_file", true).c_str()
                        << " ROOT file as input for the capacitance vs pixel gaps";
        }
    } else {
        throw ModuleError(
            "Capacitive coupling was not defined. Please, check the README file for configuration options or use "
            "the SimpleTransfer module.");
    }
}

void CapacitiveTransferModule::run(unsigned int) {

    // Find corresponding pixels for all propagated charges
    LOG(TRACE) << "Transferring charges to pixels";
    unsigned int transferred_charges_count = 0;
    std::map<Pixel::Index, std::pair<double, std::vector<const PropagatedCharge*>>, pixel_cmp> pixel_map;
    for(auto& propagated_charge : propagated_message_->getData()) {
        auto position = propagated_charge.getLocalPosition();
        // Ignore if outside depth range of implant
        if(std::fabs(position.z() - (model_->getSensorCenter().z() + model_->getSensorSize().z() / 2.0)) >
           config_.get<double>("max_depth_distance")) {
            LOG(DEBUG) << "Skipping set of " << propagated_charge.getCharge() << " propagated charges at "
                       << propagated_charge.getLocalPosition() << " because their local position is not in implant range";
            continue;
        }

        // Find the nearest pixel
        auto xpixel = static_cast<int>(std::round(position.x() / model_->getPixelSize().x()));
        auto ypixel = static_cast<int>(std::round(position.y() / model_->getPixelSize().y()));
        LOG(DEBUG) << "Hit at pixel " << xpixel << ", " << ypixel;

        Pixel::Index pixel_index;
        double neighbour_charge = 0;
        double ccpd_factor = 0;

        Eigen::Vector3d pixel_point;
        Eigen::Vector3d pixel_projection;

        for(size_t row = 0; row < max_row; row++) {
            for(size_t col = 0; col < max_col; col++) {
                if(config_.get<int>("cross_coupling") == 0) {
                    col = static_cast<size_t>(std::floor(matrix_cols / 2));
                    row = static_cast<size_t>(std::floor(matrix_rows / 2));
                }

                // Ignore if out of pixel grid
                if((xpixel + static_cast<int>(col - static_cast<size_t>(std::floor(matrix_cols / 2)))) < 0 ||
                   (xpixel + static_cast<int>(col - static_cast<size_t>(std::floor(matrix_cols / 2)))) >=
                       model_->getNPixels().x() ||
                   (ypixel + static_cast<int>(row - static_cast<size_t>(std::floor(matrix_rows / 2)))) < 0 ||
                   (ypixel + static_cast<int>(row - static_cast<size_t>(std::floor(matrix_rows / 2)))) >=
                       model_->getNPixels().y()) {
                    LOG(DEBUG) << "Skipping set of propagated charges at " << propagated_charge.getLocalPosition()
                               << " because their nearest pixel (" << xpixel << "," << ypixel
                               << ") is outside the pixel matrix";
                    continue;
                }

                pixel_index =
                    Pixel::Index(static_cast<unsigned int>(xpixel + static_cast<int>(col) - std::floor(matrix_cols / 2)),
                                 static_cast<unsigned int>(ypixel + static_cast<int>(row) - std::floor(matrix_rows / 2)));

                if(config_.has("coupling_scan_file")) {
                    double local_x = pixel_index.x() * model_->getPixelSize().x();
                    double local_y = pixel_index.y() * model_->getPixelSize().y();
                    pixel_point = Eigen::Vector3d(local_x, local_y, 0);
                    pixel_projection = plane.projection(pixel_point);
                    pixel_gap = pixel_projection[2];

                    ccpd_factor = capacitances[row * 3 + col]->Eval(
                                      static_cast<double>(Units::convert(pixel_gap, "um")), nullptr, "S") *
                                  normalization;
                } else if(config_.has("coupling_file")) {
                    ccpd_factor = relative_coupling[col][row];
                } else if(config_.has("coupling_matrix")) {
                    ccpd_factor = relative_coupling[max_row - row - 1][col];
                } else {
                    ccpd_factor = 1;
                    LOG(ERROR) << "This shouldn't happen. Transferring 100% of detected charge";
                }

                // Update statistics
                unique_pixels_.insert(pixel_index);

                transferred_charges_count += static_cast<unsigned int>(propagated_charge.getCharge() * ccpd_factor);
                neighbour_charge = propagated_charge.getCharge() * ccpd_factor;

                LOG(DEBUG) << "Set of " << propagated_charge.getCharge() * ccpd_factor << " charges brought to neighbour "
                           << col << "," << row << " pixel " << pixel_index << "with cross-coupling of " << ccpd_factor * 100
                           << "%";

                // Add the pixel the list of hit pixels
                pixel_map[pixel_index].first += neighbour_charge;
                pixel_map[pixel_index].second.emplace_back(&propagated_charge);
            }
        }
    }

    // Create pixel charges
    LOG(TRACE) << "Combining charges at same pixel";
    std::vector<PixelCharge> pixel_charges;
    for(auto& pixel_index_charge : pixel_map) {
        double charge = pixel_index_charge.second.first;

        // Get pixel object from detector
        auto pixel = detector_->getPixel(pixel_index_charge.first.x(), pixel_index_charge.first.y());
        pixel_charges.emplace_back(pixel, charge, pixel_index_charge.second.second);
        LOG(DEBUG) << "Set of " << charge << " charges combined at " << pixel.getIndex();
    }

    // Writing summary and update statistics
    LOG(INFO) << "Transferred " << transferred_charges_count << " charges to " << pixel_map.size() << " pixels";
    total_transferred_charges_ += transferred_charges_count;

    // Dispatch message of pixel charges
    auto pixel_message = std::make_shared<PixelChargeMessage>(pixel_charges, detector_);
    messenger_->dispatchMessage(this, pixel_message);
}

void CapacitiveTransferModule::finalize() {
    // Print statistics
    LOG(INFO) << "Transferred total of " << total_transferred_charges_ << " charges to " << unique_pixels_.size()
              << " different pixels";

    if(config_.get<bool>("output_plots")) {
        if(config_.has("coupling_scan_file")) {
            gap_map->Write();
            capacitance_map->Write();
            relative_capacitance_map->Write();
            for(int i = 1; i < 10; i++) {
                capacitances[i - 1]->Write(Form("Pixel_%i", i));
            }
        } else {
            coupling_map->Write();
        }
    }
}


#include "VelocityFilter.h"
#include <iostream>
#include <fstream>
#include <vector>
#include "Distance.h"
#include <Eigen/Core>
#include "CTCD.h"

using namespace std;
using namespace Eigen;

void loadMesh(const char *filename, VectorXd &verts, Matrix3Xi &faces)
{
	ifstream ifs(filename);
	if(!ifs)
		return;

	vector<double> pos;
	vector<int> faceidx;

	while(true)
	{
		char c;
		ifs >> c;
		if(!ifs)
			break;
		if(c == 'v')
		{
			for(int i=0; i<3; i++)
			{
				double p;
				ifs >> p;
				pos.push_back(p);
			}
		}
		if(c == 'f')
		{
			for(int i=0; i<3; i++)
			{
				int vert;
				ifs >> vert;
				faceidx.push_back(vert);
			}
		}
	}

	int nverts = pos.size()/3;
	int nfaces = faceidx.size()/3;

	verts.resize(3*nverts);
	for(int i=0; i<3*nverts; i++)
		verts[i] = pos[i];

	faces.resize(3, nfaces);
	for(int i=0; i<nfaces; i++)
		for(int j=0; j<3; j++)
			faces.coeffRef(j, i) = faceidx[3*i+j]-1;
}

int main(int argc, char *argv[])
{
	if(argc != 3)
	{
		std::cerr << "Usage: computeGap (coarseMesh) (fineMesh)" << std::endl;
		return -1;
	}
	VectorXd qcoarse;
	Matrix3Xi fcoarse;
	loadMesh(argv[1], qcoarse, fcoarse);
	int numcoarseverts = qcoarse.size()/3;
	std::cout << "Loaded coarse mesh with " << numcoarseverts << " vertices and " << fcoarse.cols() << " faces" << std::endl;

	VectorXd qfine;
	Matrix3Xi ffine;
	loadMesh(argv[2], qfine, ffine);
	std::cout << "Loaded fine mesh with " << qfine.size()/3 << " vertices and " << ffine.cols() << " faces" << std::endl;

	for(int i=0; i<fcoarse.cols(); i++)
	{
		for(int j=0; j<ffine.cols(); j++)
		{
			double t;
			for(int k=0; k<3; k++)
			{
				if(CTCD::vertexFaceCTCD(
qcoarse.segment<3>(3*fcoarse.col(i)[k]),
qfine.segment<3>(3*ffine.col(j)[0]),
qfine.segment<3>(3*ffine.col(j)[1]),
qfine.segment<3>(3*ffine.col(j)[2]),
qcoarse.segment<3>(3*fcoarse.col(i)[(k+1)%3]),
qfine.segment<3>(3*ffine.col(j)[0]),
qfine.segment<3>(3*ffine.col(j)[1]),
qfine.segment<3>(3*ffine.col(j)[2]), 1e-8, t))
				{
					std::cout << "Meshes intersect!" <<
std::endl;
					return 0;
				}
			}
		}
	}

	VectorXd q(qcoarse.size() + qfine.size());
	Matrix3Xi f;
	f.resize(3, fcoarse.cols() + ffine.cols());
	for(int i=0; i<(int)qcoarse.size(); i++)
		q[i] = qcoarse[i];
	for(int i=0; i<qfine.size(); i++)
		q[qcoarse.size()+i] = qfine[i];
	for(int i=0; i<(int)fcoarse.cols(); i++)
		f.col(i) = fcoarse.col(i);
	for(int i=0; i<(int)ffine.cols(); i++)
		for(int j=0; j<3; j++)
			f.coeffRef(j, fcoarse.cols()+i) = ffine.coeff(j,i) + numcoarseverts;

	set<int> fixedVerts;
	for(int i=0; i<(int)qfine.size()/3; i++)
		fixedVerts.insert(qcoarse.size()/3 + i);

	double distance = Distance::meshSelfDistance(q, f, fixedVerts);
	std::cout << "Min distance between coarse mesh and itself/fine mesh: " << distance << std::endl;
}