/*    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_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>
#include <string>

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

#include "tudat/basics/testMacros.h"

#include "tudat/astro/ground_stations/pointingAnglesCalculator.h"
#include "tudat/simulation/estimation.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::observation_models;
using namespace tudat::spice_interface;
using namespace tudat::ephemerides;
using namespace tudat::simulation_setup;
using namespace tudat::ground_stations;
using namespace tudat::unit_conversions;

BOOST_AUTO_TEST_SUITE( test_observation_viability_calculators )

BOOST_AUTO_TEST_CASE( testSeparateObservationViabilityCalculators )
{

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

    // Create default Earth
    std::vector< std::string > bodyNames = { "Earth" };
    BodyListSettings bodySettings = getDefaultBodySettings(
                bodyNames, "Earth", "J2000" );
    SystemOfBodies bodies = createSystemOfBodies( bodySettings );

    // Define ground station
    double stationRadius = spice_interface::getAverageRadius( "Earth" );
    double stationLatitude = convertDegreesToRadians( 20.0 );
    double stationLongitude = convertDegreesToRadians( 50.0 );
    createGroundStation( bodies.at( "Earth" ), "Station",
                         ( Eigen::Vector3d( ) << stationRadius, stationLatitude, stationLongitude ).finished( ),
                         coordinate_conversions::spherical_position );

    // Get inertial ground station state function
    std::function< Eigen::Vector6d( const double ) > groundStationStateFunction =
            getLinkEndCompleteEphemerisFunction( std::make_pair( "Earth", "Station" ), bodies );

    // Get Earth-fixed ground station position
    Eigen::Vector3d earthFixedGroundStationState =
            bodies.at( "Earth" )->getGroundStation( "Station" )->getNominalStationState( )->getCartesianStateInTime( 0.0 ).segment( 0, 3 );

    // Define limiting elevation angle for test
    double testAngle = 20.0 * mathematical_constants::PI / 180.0;

    // Retrieve pointing angle calculator
    std::shared_ptr< PointingAnglesCalculator > pointingAngleCalculator =
            bodies.at( "Earth" )->getGroundStation( "Station" )->getPointingAnglesCalculator( );

    // Define test body for occultation/avoidance tests
    Eigen::Vector6d testBodyState = Eigen::Vector6d::Zero( );
    testBodyState( 0 ) = 8000.0E3;
    double testBodyRadius = 1000.0E3;

    // Declare test variables
    Eigen::Vector3d manualGroundStationInertialPosition;
    Eigen::Vector3d vectorToTarget;
    Eigen::Vector3d vectorToTestBody;
    Eigen::Vector6d targetState = Eigen::Vector6d::Zero( );
    Eigen::Vector6d groundStationState = Eigen::Vector6d::Zero( );
    Eigen::Vector3d localVerticalVector = Eigen::Vector3d::UnitZ( );

    // Run different test cases:
    // 0: one-way uplink
    // 1: one-way downlink
    for( unsigned int i = 0; i < 2; i++ )
    {
        int targetIndex = -1;
        int groundStationIndex = -1;
        std::vector< std::pair< int, int > > linkEndIndices;
        if( i == 0 )
        {
            targetIndex = 1;
            groundStationIndex = 0;
            linkEndIndices = { { 0, 1 } };
        }
        else if( i == 1 )
        {
            targetIndex = 0;
            groundStationIndex = 1;
            linkEndIndices = { { 1, 0 } };
        }
        std::shared_ptr< ObservationViabilityCalculator > minimumElevationCalculator =
                std::make_shared< MinimumElevationAngleCalculator >(
                    linkEndIndices, testAngle, pointingAngleCalculator );
        std::shared_ptr< ObservationViabilityCalculator > bodyAvoidanceCalculator =
                std::make_shared< BodyAvoidanceAngleCalculator >(
                    linkEndIndices, testAngle, [=](const double){return testBodyState;}, "TestBody" );
        std::shared_ptr< ObservationViabilityCalculator > bodyOccultationCalculator =
                std::make_shared< OccultationCalculator >(
                    linkEndIndices, [=](const double){return testBodyState;}, testBodyRadius );

        std::vector< double > linkEndTimes;
        linkEndTimes.resize( 2 );

        std::vector< Eigen::Vector6d > linkEndStates;
        linkEndStates.resize( 2 );

        // Test viability settings for ~100 orbits
        for( int j = 0; j < 20000; j++ )
        {
            // Set spacraft state in circular orbit (velocity not set)
            targetState( 0 ) = std::sin( ( static_cast< double >( j ) / 200.0 ) * 2.0 * mathematical_constants::PI ) * 10000.0E3;
            targetState( 2 ) = std::cos( ( static_cast< double >( j ) / 200.0 ) * 2.0 * mathematical_constants::PI ) * 10000.0E3;
            // Set link end times (exagerated light time for testing purposes)
            linkEndTimes[ 0 ] = 2.0 * 3600.0 * ( static_cast< double >( j ) / 200.0 );
            linkEndTimes[ 1 ] = 2.0 * 3600.0 * ( static_cast< double >( j ) / 200.0 ) + 100.0;

            linkEndStates[ groundStationIndex ] = groundStationStateFunction( linkEndTimes[ groundStationIndex ] );
            linkEndStates[ targetIndex ] = targetState;

            // Manually calculate elevation angle
            manualGroundStationInertialPosition = spice_interface::computeRotationMatrixBetweenFrames(
                        "IAU_Earth", "J2000", linkEndTimes[ groundStationIndex ] ) * earthFixedGroundStationState;
            vectorToTarget = ( targetState.segment( 0, 3 ) - manualGroundStationInertialPosition );
            vectorToTestBody = ( testBodyState.segment( 0, 3 ) - manualGroundStationInertialPosition );

            // Test elevation angle
            {
                double manualElevationAngle = mathematical_constants::PI / 2.0 -
                        std::acos( manualGroundStationInertialPosition.normalized( ).dot( vectorToTarget.normalized( ) ) );

                // Compute viability and check against manual calculation
                bool isObservationViable = minimumElevationCalculator->isObservationViable(
                            linkEndStates, linkEndTimes );
                if( manualElevationAngle > testAngle )
                {
                    BOOST_CHECK_EQUAL( isObservationViable, 1 );
                }
                else
                {
                    BOOST_CHECK_EQUAL( isObservationViable, 0 );
                }
            }

            // Test avoidance angle
            {
                double manualAvoidanceAngle =
                        std::acos( ( vectorToTestBody.normalized( ) ).dot( vectorToTarget.normalized( ) ) );

                // Compute viability and check against manual calculation
                bool isObservationViable = bodyAvoidanceCalculator->isObservationViable(
                            linkEndStates, linkEndTimes );
                if( manualAvoidanceAngle > testAngle )
                {
                    BOOST_CHECK_EQUAL( isObservationViable, 1 );
                }
                else
                {
                    BOOST_CHECK_EQUAL( isObservationViable, 0 );
                }
            }

            // Test occultation
            {

                bool manualIsObservationViable = true;
                if( mission_geometry::computeShadowFunction(
                            manualGroundStationInertialPosition, 0.0,
                            testBodyState.segment( 0, 3 ),
                            testBodyRadius,
                            targetState.segment( 0, 3 ) ) < 1.0E-10 )
                {
                    manualIsObservationViable = false;
                }

                // Compute viability and check against manual calculation
                bool isObservationViable = bodyOccultationCalculator->isObservationViable(
                            linkEndStates, linkEndTimes );
                BOOST_CHECK_EQUAL( isObservationViable, manualIsObservationViable );
            }
        }
    }
}

////! Test function to manually compute elevation angle(s) of observation link at given body
//std::vector< double > getBodyLinkElevationAngles(
//        const LinkEnds linkEnds,
//        const ObservableType observableType,
//        const std::string referenceBody,
//        const std::vector< Eigen::Vector6d > linkEndStates,
//        const std::vector< double > linkEndTimes,
//        const SystemOfBodies& bodies )
//{
//    std::shared_ptr< ground_stations::PointingAnglesCalculator > currentPointingAnglesCalculator;
//    std::vector< double > elevationAngles;
//    switch( observableType )
//    {
//    case one_way_range:
//    {
//        if( linkEnds.at( transmitter ).first == referenceBody )
//        {
//            currentPointingAnglesCalculator = bodies.at( referenceBody )->getGroundStation(
//                        linkEnds.at( transmitter ).second )->getPointingAnglesCalculator( );
//            elevationAngles.push_back( currentPointingAnglesCalculator->calculateElevationAngle(
//                                           ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ), linkEndTimes.at( 0 ) ) );

//        }
//        else if( linkEnds.at( receiver ).first == referenceBody )
//        {
//            currentPointingAnglesCalculator = bodies.at( referenceBody )->getGroundStation(
//                        linkEnds.at( receiver ).second )->getPointingAnglesCalculator( );
//            elevationAngles.push_back( currentPointingAnglesCalculator->calculateElevationAngle(
//                                           ( linkEndStates.at( 0 ) - linkEndStates.at( 1 ) ).segment( 0, 3 ), linkEndTimes.at( 1 ) ) );
//        }
//        break;
//    }
//    case one_way_doppler:
//    {
//        if( linkEnds.at( transmitter ).first == referenceBody )
//        {
//            currentPointingAnglesCalculator = bodies.at( referenceBody )->getGroundStation(
//                        linkEnds.at( transmitter ).second )->getPointingAnglesCalculator( );
//            elevationAngles.push_back( currentPointingAnglesCalculator->calculateElevationAngle(
//                                           ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ), linkEndTimes.at( 0 ) ) );

//        }
//        else if( linkEnds.at( receiver ).first == referenceBody )
//        {
//            currentPointingAnglesCalculator = bodies.at( referenceBody )->getGroundStation(
//                        linkEnds.at( receiver ).second )->getPointingAnglesCalculator( );
//            elevationAngles.push_back( currentPointingAnglesCalculator->calculateElevationAngle(
//                                           ( linkEndStates.at( 0 ) - linkEndStates.at( 1 ) ).segment( 0, 3 ), linkEndTimes.at( 1 ) ) );
//        }
//        break;
//    }
//    case angular_position:
//    {
//        if( linkEnds.at( transmitter ).first == referenceBody )
//        {
//            currentPointingAnglesCalculator = bodies.at( referenceBody )->getGroundStation(
//                        linkEnds.at( transmitter ).second )->getPointingAnglesCalculator( );
//            elevationAngles.push_back( currentPointingAnglesCalculator->calculateElevationAngle(
//                                           ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ), linkEndTimes.at( 0 ) ) );

//        }
//        else if( linkEnds.at( receiver ).first == referenceBody )
//        {
//            currentPointingAnglesCalculator = bodies.at( referenceBody )->getGroundStation(
//                        linkEnds.at( receiver ).second )->getPointingAnglesCalculator( );
//            elevationAngles.push_back( currentPointingAnglesCalculator->calculateElevationAngle(
//                                           ( linkEndStates.at( 0 ) - linkEndStates.at( 1 ) ).segment( 0, 3 ), linkEndTimes.at( 1 ) ) );
//        }
//        break;
//    }
//    case one_way_differenced_range:
//    {
//        if( linkEnds.at( transmitter ).first == referenceBody )
//        {
//            currentPointingAnglesCalculator = bodies.at( referenceBody )->getGroundStation(
//                        linkEnds.at( transmitter ).second )->getPointingAnglesCalculator( );
//            elevationAngles.push_back( currentPointingAnglesCalculator->calculateElevationAngle(
//                                           ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ), linkEndTimes.at( 0 ) ) );
//            elevationAngles.push_back( currentPointingAnglesCalculator->calculateElevationAngle(
//                                           ( linkEndStates.at( 3 ) - linkEndStates.at( 2 ) ).segment( 0, 3 ), linkEndTimes.at( 2 ) ) );

//        }
//        else if( linkEnds.at( receiver ).first == referenceBody )
//        {
//            currentPointingAnglesCalculator = bodies.at( referenceBody )->getGroundStation(
//                        linkEnds.at( receiver ).second )->getPointingAnglesCalculator( );
//            elevationAngles.push_back( currentPointingAnglesCalculator->calculateElevationAngle(
//                                           ( linkEndStates.at( 0 ) - linkEndStates.at( 1 ) ).segment( 0, 3 ), linkEndTimes.at( 1 ) ) );
//            elevationAngles.push_back( currentPointingAnglesCalculator->calculateElevationAngle(
//                                           ( linkEndStates.at( 2 ) - linkEndStates.at( 3 ) ).segment( 0, 3 ), linkEndTimes.at( 3 ) ) );
//        }
//        break;
//    }
//    case n_way_range:
//    {
//        int linkEndIndex = 0;
//        for( LinkEnds::const_iterator linkEndIterator = linkEnds.begin( ); linkEndIterator != linkEnds.end( );
//             linkEndIterator++ )
//        {
//            if( linkEndIterator->second.first == referenceBody )
//            {
//                currentPointingAnglesCalculator = bodies.at( referenceBody )->getGroundStation(
//                            linkEndIterator->second.second )->getPointingAnglesCalculator( );
//                if( linkEndIndex != 0 )
//                {
//                    elevationAngles.push_back(
//                                currentPointingAnglesCalculator->calculateElevationAngle(
//                                    ( linkEndStates.at( 2 * ( linkEndIndex - 1 ) ) -
//                                      linkEndStates.at( 2 * ( linkEndIndex - 1 ) + 1 ) ).segment( 0, 3 ),
//                                    linkEndTimes.at( 2 * ( linkEndIndex - 1 ) + 1 ) ) );
//                }

//                if( linkEndIndex != static_cast< int >( linkEnds.size( ) ) - 1 )
//                {
//                    elevationAngles.push_back(
//                                currentPointingAnglesCalculator->calculateElevationAngle(
//                                    ( linkEndStates.at( 2 * linkEndIndex + 1) -
//                                      linkEndStates.at( 2 * linkEndIndex ) ).segment( 0, 3 ),
//                                    linkEndTimes.at( 2 * linkEndIndex ) ) );
//                }

//            }
//            linkEndIndex++;
//        }
//        break;
//    }
//    default:
//        throw std::runtime_error( "Error when testing elevation angle viability, observable not recognized" );

//    }

//    return elevationAngles;
//}

////! Test function to manually compute body avoidance angle(s) of observation link at given body
//std::vector< double > getBodyCosineAvoidanceAngles(
//        const LinkEnds linkEnds,
//        const ObservableType observableType,
//        const std::string referenceBody,
//        const std::string bodyToAvoid,
//        const std::vector< Eigen::Vector6d > linkEndStates,
//        const std::vector< double > linkEndTimes,
//        const SystemOfBodies& bodies )
//{
//    std::vector< double > cosineAvoidanceAngles;
//    switch( observableType )
//    {
//    case one_way_range:
//    {
//        if( linkEnds.at( transmitter ).first == referenceBody )
//        {
//            cosineAvoidanceAngles.push_back(
//                        linear_algebra::computeCosineOfmanualElevationAngle(
//                            ( ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ) ), (
//                                bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                    ( linkEndTimes.at( 1 ) + linkEndTimes.at( 0 ) ) / 2.0 ).segment( 0, 3 ) -
//                                linkEndStates.at( 0 ).segment( 0, 3 ) ) ) );
//        }
//        else if( linkEnds.at( receiver ).first == referenceBody )
//        {
//            cosineAvoidanceAngles.push_back(
//                        linear_algebra::computeCosineOfmanualElevationAngle(
//                            ( ( linkEndStates.at( 0 ) - linkEndStates.at( 1 ) ).segment( 0, 3 ) ), (
//                                bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                    ( linkEndTimes.at( 1 ) + linkEndTimes.at( 0 ) ) / 2.0 ).segment( 0, 3 ) -
//                                linkEndStates.at( 1 ).segment( 0, 3 ) ) ) );
//        }
//        break;
//    }
//    case one_way_doppler:
//    {
//        if( linkEnds.at( transmitter ).first == referenceBody )
//        {
//            cosineAvoidanceAngles.push_back(
//                        linear_algebra::computeCosineOfmanualElevationAngle(
//                            ( ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ) ), (
//                                bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                    ( linkEndTimes.at( 1 ) + linkEndTimes.at( 0 ) ) / 2.0 ).segment( 0, 3 ) -
//                                linkEndStates.at( 0 ).segment( 0, 3 ) ) ) );
//        }
//        else if( linkEnds.at( receiver ).first == referenceBody )
//        {
//            cosineAvoidanceAngles.push_back(
//                        linear_algebra::computeCosineOfmanualElevationAngle(
//                            ( ( linkEndStates.at( 0 ) - linkEndStates.at( 1 ) ).segment( 0, 3 ) ), (
//                                bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                    ( linkEndTimes.at( 1 ) + linkEndTimes.at( 0 ) ) / 2.0 ).segment( 0, 3 ) -
//                                linkEndStates.at( 1 ).segment( 0, 3 ) ) ) );
//        }
//        break;
//    }
//    case angular_position:
//    {
//        if( linkEnds.at( transmitter ).first == referenceBody )
//        {
//            cosineAvoidanceAngles.push_back(
//                        linear_algebra::computeCosineOfmanualElevationAngle(
//                            ( ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ) ), (
//                                bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                    ( linkEndTimes.at( 1 ) + linkEndTimes.at( 0 ) ) / 2.0 ).segment( 0, 3 ) -
//                                linkEndStates.at( 0 ).segment( 0, 3 ) ) ) );
//        }
//        else if( linkEnds.at( receiver ).first == referenceBody )
//        {
//            cosineAvoidanceAngles.push_back(
//                        linear_algebra::computeCosineOfmanualElevationAngle(
//                            ( ( linkEndStates.at( 0 ) - linkEndStates.at( 1 ) ).segment( 0, 3 ) ), (
//                                bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                    ( linkEndTimes.at( 1 ) + linkEndTimes.at( 0 ) ) / 2.0 ).segment( 0, 3 ) -
//                                linkEndStates.at( 1 ).segment( 0, 3 ) ) ) );
//        }
//        break;
//    }
//    case one_way_differenced_range:
//    {
//        if( linkEnds.at( transmitter ).first == referenceBody )
//        {
//            cosineAvoidanceAngles.push_back(
//                        linear_algebra::computeCosineOfmanualElevationAngle(
//                            ( ( linkEndStates.at( 1 ) - linkEndStates.at( 0 ) ).segment( 0, 3 ) ), (
//                                bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                    ( linkEndTimes.at( 1 ) + linkEndTimes.at( 0 ) ) / 2.0 ).segment( 0, 3 ) -
//                                linkEndStates.at( 0 ).segment( 0, 3 ) ) ) );
//            cosineAvoidanceAngles.push_back(
//                        linear_algebra::computeCosineOfmanualElevationAngle(
//                            ( ( linkEndStates.at( 3 ) - linkEndStates.at( 2 ) ).segment( 0, 3 ) ), (
//                                bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                    ( linkEndTimes.at( 3 ) + linkEndTimes.at( 2 ) ) / 2.0 ).segment( 0, 3 ) -
//                                linkEndStates.at( 2 ).segment( 0, 3 ) ) ) );
//        }
//        else if( linkEnds.at( receiver ).first == referenceBody )
//        {
//            cosineAvoidanceAngles.push_back(
//                        linear_algebra::computeCosineOfmanualElevationAngle(
//                            ( ( linkEndStates.at( 0 ) - linkEndStates.at( 1 ) ).segment( 0, 3 ) ), (
//                                bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                    ( linkEndTimes.at( 1 ) + linkEndTimes.at( 0 ) ) / 2.0 ).segment( 0, 3 ) -
//                                linkEndStates.at( 1 ).segment( 0, 3 ) ) ) );
//            cosineAvoidanceAngles.push_back(
//                        linear_algebra::computeCosineOfmanualElevationAngle(
//                            ( ( linkEndStates.at( 2 ) - linkEndStates.at( 3 ) ).segment( 0, 3 ) ), (
//                                bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                    ( linkEndTimes.at( 3 ) + linkEndTimes.at( 2 ) ) / 2.0 ).segment( 0, 3 ) -
//                                linkEndStates.at( 3 ).segment( 0, 3 ) ) ) );
//        }
//        break;
//    }
//    case n_way_range:
//    {
//        int linkEndIndex = 0;
//        for( LinkEnds::const_iterator linkEndIterator = linkEnds.begin( ); linkEndIterator != linkEnds.end( );
//             linkEndIterator++ )
//        {
//            if( linkEndIterator->second.first == referenceBody )
//            {
//                if( linkEndIndex != 0 )
//                {
//                    cosineAvoidanceAngles.push_back(
//                                linear_algebra::computeCosineOfmanualElevationAngle(
//                                    ( ( linkEndStates.at( 2 * ( linkEndIndex - 1 ) ) -
//                                        linkEndStates.at( 2 * ( linkEndIndex - 1 ) + 1 ) ).segment( 0, 3 ) ), (
//                                        bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                            ( linkEndTimes.at( 2 * ( linkEndIndex - 1 ) + 1  ) +
//                                              linkEndTimes.at( 2 * ( linkEndIndex - 1 ) ) ) / 2.0 ).segment( 0, 3 ) -
//                                        linkEndStates.at( 2 * ( linkEndIndex - 1 ) + 1 ).segment( 0, 3 ) ) ) );
//                }

//                if( linkEndIndex != static_cast< int >( linkEnds.size( ) ) - 1 )
//                {
//                    cosineAvoidanceAngles.push_back(
//                                linear_algebra::computeCosineOfmanualElevationAngle(
//                                    ( linkEndStates.at( 2 * linkEndIndex + 1 ) -
//                                      linkEndStates.at( 2 * linkEndIndex ) ).segment( 0, 3 ), (
//                                        bodies.at( bodyToAvoid )->getStateInBaseFrameFromEphemeris< double, double >(
//                                            ( linkEndTimes.at( 2 * linkEndIndex + 1  ) +
//                                              linkEndTimes.at( 2 * linkEndIndex  ) ) / 2.0 ).segment( 0, 3 ) -
//                                        linkEndStates.at( 2 * linkEndIndex ).segment( 0, 3 ) ) ) );
//                }

//            }
//            linkEndIndex++;
//        }
//        break;
//    }
//    default:
//        throw std::runtime_error( "Error when testing avoidance angle viability, observable not recognized" );

//    }

//    return cosineAvoidanceAngles;
//}

////! Test function to manually compute body distance between body center of mass and observation link vector
//std::vector< double > getDistanceBetweenLineOfSightVectorAndPoint(
//        const std::string bodyToAnalyze,
//        const std::vector< Eigen::Vector6d > linkEndStates,
//        const std::vector< double > linkEndTimes,
//        const SystemOfBodies& bodies )
//{
//    std::vector< double > pointDistances;
//    for( unsigned int i = 0; i < linkEndStates.size( ); i += 2 )
//    {
//        Eigen::Vector3d linkEnd1Position = linkEndStates.at( i ).segment( 0, 3 );
//        Eigen::Vector3d linkEnd2Position = linkEndStates.at( i + 1 ).segment( 0, 3 );

//        Eigen::Vector3d pointPosition = bodies.at( bodyToAnalyze )->getStateInBaseFrameFromEphemeris< double, double >(
//                    ( linkEndTimes.at( i ) + linkEndTimes.at( i ) ) / 2.0 ).segment( 0, 3 );

//        pointDistances.push_back( ( ( linkEnd2Position - pointPosition ).cross( linkEnd1Position - pointPosition ) ).norm( ) /
//                                  ( linkEnd2Position - linkEnd1Position ).norm( ) );

//    }
//    return pointDistances;
//}

////! Test if observation viability calculators correctly constrain observations
///*!
// *  Test if observation viability calculators correctly constrain observations. Test is performed for a list of observables,
// *  and a list of link ends per observable. The elevation angle, occultation and avoidance angle conditions are all checked.
// */
//BOOST_AUTO_TEST_CASE( testObservationViabilityCalculators )
//{
//    //Load spice kernels.
//    spice_interface::loadStandardSpiceKernels( );

//    // Define environment settings
//    std::vector< std::string > bodyNames;
//    bodyNames.push_back( "Earth" );
//    bodyNames.push_back( "Mars" );
//    bodyNames.push_back( "Sun" );
//    bodyNames.push_back( "Moon" );

//    BodyListSettings bodySettings =
//            getDefaultBodySettings( bodyNames );

//    // Set simplified rotation models for Earth/Mars (spice rotation retrieval is slow)
//    bodySettings.at( "Earth" )->rotationModelSettings = std::make_shared< SimpleRotationModelSettings >(
//                "ECLIPJ2000", "IAU_Earth",
//                spice_interface::computeRotationQuaternionBetweenFrames(
//                    "ECLIPJ2000", "IAU_Earth", 0.0 ),
//                0.0, 2.0 * mathematical_constants::PI /
//                ( physical_constants::JULIAN_DAY ) );
//    bodySettings.at( "Mars" )->rotationModelSettings = std::make_shared< SimpleRotationModelSettings >(
//                "ECLIPJ2000", "IAU_Mars",
//                spice_interface::computeRotationQuaternionBetweenFrames(
//                    "ECLIPJ2000", "IAU_Mars", 0.0 ),
//                0.0, 2.0 * mathematical_constants::PI /
//                ( physical_constants::JULIAN_DAY + 40.0 * 60.0 ) );

//    // Set unrealistically large radius of Moon to make iss occultation mode significant
//    double moonRadius = 1.0E6;
//    bodySettings.at( "Moon" )->shapeModelSettings = std::make_shared< SphericalBodyShapeSettings >( moonRadius );

//    // Create list of body objects
//    SystemOfBodies bodies = createSystemOfBodies( bodySettings );

//    // Create ground stations
//    std::pair< std::string, std::string > earthStation1 = std::pair< std::string, std::string >( "Earth", "EarthStation1" );
//    std::pair< std::string, std::string > earthStation2 = std::pair< std::string, std::string >( "Earth", "EarthStation2" );
//    std::pair< std::string, std::string > mslStation1 = std::pair< std::string, std::string >( "Mars", "MarsStation1" );
//    std::pair< std::string, std::string > mslStation2 = std::pair< std::string, std::string >( "Mars", "MarsStation2" );
//    createGroundStation( bodies.at( "Mars" ), "MarsStation1", ( Eigen::Vector3d( ) << 100.0, 0.2, 2.1 ).finished( ),
//                         coordinate_conversions::geodetic_position );
//    createGroundStation( bodies.at( "Mars" ), "MarsStation2", ( Eigen::Vector3d( ) << -2000.0, -0.4, 0.1 ).finished( ),
//                         coordinate_conversions::geodetic_position );
//    createGroundStation( bodies.at( "Earth" ), "EarthStation1", ( Eigen::Vector3d( ) << 800.0, 0.12, 5.3 ).finished( ),
//                         coordinate_conversions::geodetic_position );
//    createGroundStation( bodies.at( "Earth" ), "EarthStation2", ( Eigen::Vector3d( ) << 100.0, 0.15, 0.0 ).finished( ),
//                         coordinate_conversions::geodetic_position );

//    // Define one-way range/one-way Doppler/angular position/one-way differenced range link ends
//    LinkEnds oneWayLinkEnds1;
//    oneWayLinkEnds1[ transmitter ] = std::make_pair( "Mars", "MarsStation1" );
//    oneWayLinkEnds1[ receiver ] = std::make_pair( "Earth", "EarthStation1" );

//    LinkEnds oneWayLinkEnds2;
//    oneWayLinkEnds2[ transmitter ] = std::make_pair( "Earth", "EarthStation2" );
//    oneWayLinkEnds2[ receiver ] = std::make_pair( "Mars", "MarsStation2" );

//    LinkEnds oneWayLinkEnds3;
//    oneWayLinkEnds3[ transmitter ] = std::make_pair( "Mars", "MarsStation1" );
//    oneWayLinkEnds3[ receiver ] = std::make_pair( "Earth", "EarthStation2" );

//    std::vector< LinkEnds > oneWayRangeLinkEnds;
//    oneWayRangeLinkEnds.push_back( oneWayLinkEnds1 );
//    oneWayRangeLinkEnds.push_back( oneWayLinkEnds2 );
//    oneWayRangeLinkEnds.push_back( oneWayLinkEnds3 );

//    // Define two-way range link ends
//    LinkEnds twoWayLinkEnds1;
//    twoWayLinkEnds1[ transmitter ] = std::make_pair( "Mars", "MarsStation1" );
//    twoWayLinkEnds1[ reflector1 ] = std::make_pair( "Earth", "EarthStation1" );
//    twoWayLinkEnds1[ receiver ] = std::make_pair( "Mars", "MarsStation1" );

//    LinkEnds twoWayLinkEnds2;
//    twoWayLinkEnds2[ transmitter ] = std::make_pair( "Earth", "EarthStation2" );
//    twoWayLinkEnds2[ reflector1 ] = std::make_pair( "Mars", "MarsStation2" );
//    twoWayLinkEnds2[ receiver ] = std::make_pair( "Earth", "EarthStation1" );

//    LinkEnds twoWayLinkEnds3;
//    twoWayLinkEnds3[ transmitter ] = std::make_pair( "Mars", "MarsStation2" );
//    twoWayLinkEnds3[ reflector1 ] = std::make_pair( "Earth", "EarthStation2" );
//    twoWayLinkEnds3[ receiver ] = std::make_pair( "Mars", "MarsStation1" );
//    std::vector< LinkEnds > twoWayRangeLinkEnds;
//    twoWayRangeLinkEnds.push_back( twoWayLinkEnds1 );
//    twoWayRangeLinkEnds.push_back( twoWayLinkEnds2 );
//    twoWayRangeLinkEnds.push_back( twoWayLinkEnds3 );

//    // Create list of link ends per obsevables
//    std::map< ObservableType, std::vector< LinkEnds > > testLinkEndsList;
//    testLinkEndsList[ one_way_range ] = oneWayRangeLinkEnds;
//    testLinkEndsList[ one_way_differenced_range ] = oneWayRangeLinkEnds;
////    testLinkEndsList[ angular_position ] = oneWayRangeLinkEnds;
//    testLinkEndsList[ n_way_range ] = twoWayRangeLinkEnds;

//    // Define list of observation times that are to be checked: one observation every 2.5 days, over a period of 10 years.
//    std::vector< double > unconstrainedObservationTimes;
//    LinkEndType referenceLinkEnd = transmitter;
//    double initialTime = 0.0, finalTime = 10.0 * physical_constants::JULIAN_YEAR, timeStep = physical_constants::JULIAN_DAY * 2.5;
//    double currentTime = initialTime;
//    while( currentTime <= finalTime )
//    {
//        unconstrainedObservationTimes.push_back( currentTime );
//        currentTime += timeStep;
//    }
//    int unconstrainedNumberOfObservations = unconstrainedObservationTimes.size( );

//    // Define minimum elevation angles for Earth/Mars stations
//    double earthtestAngle = 4.0 * mathematical_constants::PI / 180.0;
//    double marstestAngle = 10.0 * mathematical_constants::PI / 180.0;

//    // Define minimum Sun avoidance angles for Earth/Mars stations
//    double earthSunAvoidanceAngle = 30.0 * mathematical_constants::PI / 180.0;
//    double marsSunAvoidanceAngle = 21.0 * mathematical_constants::PI / 180.0;

//    // Create observation viability settings
//    std::vector< std::shared_ptr< ObservationViabilitySettings > > observationViabilitySettings;
//    observationViabilitySettings.push_back( std::make_shared< ObservationViabilitySettings >(
//                                                minimum_elevation_angle, std::make_pair( "Earth", "" ), "",
//                                                earthtestAngle ) );
//    observationViabilitySettings.push_back( std::make_shared< ObservationViabilitySettings >(
//                                                minimum_elevation_angle, std::make_pair( "Mars", "" ), "",
//                                                marstestAngle ) );
//    observationViabilitySettings.push_back( std::make_shared< ObservationViabilitySettings >(
//                                                body_avoidance_angle, std::make_pair( "Earth", "" ), "Sun",
//                                                earthSunAvoidanceAngle ) );
//    observationViabilitySettings.push_back( std::make_shared< ObservationViabilitySettings >(
//                                                body_avoidance_angle, std::make_pair( "Mars", "" ), "Sun",
//                                                marsSunAvoidanceAngle ) );
//    observationViabilitySettings.push_back( std::make_shared< ObservationViabilitySettings >(
//                                                body_occultation, std::make_pair( "Earth", "" ), "Moon" ) );

//    // Create observation model and observation time settings for all observables
//    std::vector< std::shared_ptr< ObservationSimulationSettings< double > > > observationTimeSettings;
//    std::vector< std::shared_ptr< ObservationSimulationSettings< double > > > observationTimeSettingsConstrained;

//    std::vector< std::shared_ptr< ObservationModelSettings > >  observationSettingsList;
//    for( std::map< ObservableType, std::vector< LinkEnds > >::const_iterator observableIterator = testLinkEndsList.begin( );
//         observableIterator != testLinkEndsList.end( ); observableIterator++ )
//    {
//        for( unsigned int i = 0; i < observableIterator->second.size( ); i++ )
//        {
//            LinkEnds linkEnds = observableIterator->second.at( i );
//            if( observableIterator->first == one_way_differenced_range )
//            {
//                observationSettingsList.push_back(
//                            std::make_shared< OneWayDifferencedRangeRateObservationSettings >( linkEnds,
//                                                                                [ ]( const double ){ return 60.0; }, std::shared_ptr< LightTimeCorrectionSettings > ( ) ) );
//            }
//            else if( observableIterator->first == n_way_range )
//            {
//                observationSettingsList.push_back(
//                            std::make_shared< NWayRangeObservationSettings >( linkEnds,
//                                                                              std::shared_ptr< LightTimeCorrectionSettings >( ), observableIterator->second.at( i ).size( ) ) );
//            }
//            else
//            {
//                observationSettingsList.push_back(
//                            std::make_shared< ObservationModelSettings >(
//                                observableIterator->first, linkEnds, std::shared_ptr< LightTimeCorrectionSettings >( ) ) );

//            }
//            observationTimeSettings.push_back(
//                        std::make_shared< TabulatedObservationSimulationSettings< double > >(
//                            observableIterator->first, observableIterator->second.at( i ), unconstrainedObservationTimes,
//                            referenceLinkEnd ) );
//            observationTimeSettingsConstrained.push_back(
//                        std::make_shared< TabulatedObservationSimulationSettings< double > >(
//                            observableIterator->first, observableIterator->second.at( i ), unconstrainedObservationTimes,
//                            referenceLinkEnd, observationViabilitySettings ) );
//        }
//    }

//    // Create osbervation simulatos
//    std::vector< std::shared_ptr< ObservationSimulatorBase< double, double > > > observationSimulators =
//            createObservationSimulators( observationSettingsList, bodies );
//    std::map< ObservableType, std::shared_ptr< ObservationSimulatorBase< double, double > > > observationSimulatorsMap;
//    for( unsigned int i = 0; i < observationSimulators.size( ); i++ )
//    {
//        observationSimulatorsMap[ observationSimulators.at( i )->getObservableType( ) ] = observationSimulators.at( i );
//    }

//    // Simulate observations without constraints directly from simulateObservations function
//    std::shared_ptr< observation_models::ObservationCollection< > > unconstrainedSimulatedObservables =
//            simulateObservations( observationTimeSettings, observationSimulators, bodies );

//    // Simulate observations with viability constraints directly from simulateObservations function
//    std::shared_ptr< observation_models::ObservationCollection< > > constrainedSimulatedObservables =
//            simulateObservations( observationTimeSettingsConstrained, observationSimulators, bodies );

//    int numberOfObservables = testLinkEndsList.size( );

//    // Check consistency of simulated observations from ObservationSimulator objects/simulateObservations function
//    BOOST_CHECK_EQUAL( numberOfObservables, unconstrainedSimulatedObservables->getObservationTypeStartAndSize( ).size( ) );
//    BOOST_CHECK_EQUAL( numberOfObservables, constrainedSimulatedObservables->getObservationTypeStartAndSize( ).size( ) );

//    // Create iterators over all simulated observations
//    auto unconstrainedSortedObservations = unconstrainedSimulatedObservables->getObservationSetStartAndSize( );
//    auto constrainedSortedObservations = constrainedSimulatedObservables->getObservationSetStartAndSize( );

//    std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > >::iterator unconstrainedIterator =
//            unconstrainedSortedObservations.begin( );
//    std::map< ObservableType, std::map< LinkEnds, std::vector< std::pair< int, int > > > >::iterator constrainedIterator =
//            constrainedSortedObservations.begin( );

//    std::vector< double > linkEndTimes;
//    std::vector< Eigen::Vector6d > linkEndStates;

//    std::vector< double > unconstrainedConcatenatedTimes = unconstrainedSimulatedObservables->getConcatenatedTimeVector( );
//    std::vector< double > constrainedConcatenatedTimes = constrainedSimulatedObservables->getConcatenatedTimeVector( );

//    // Iterate over all observations and check viability constraints
//    for( int i = 0; i < numberOfObservables; i++ )
//    {
//        int numberOfLinkEnds = testLinkEndsList.at( unconstrainedIterator->first ).size( );
//        int currentObservableSize = getObservableSize( unconstrainedIterator->first );

//        // Check consistency of simulated observations from ObservationSimulator objects/simulateObservations function
//        BOOST_CHECK_EQUAL( numberOfLinkEnds, unconstrainedIterator->second.size( ) );
//        BOOST_CHECK_EQUAL( numberOfLinkEnds, constrainedIterator->second.size( ) );

//        // Create iterators over all simulated observations of current observable
//        auto unconstrainedLinkIterator = unconstrainedIterator->second.begin( );
//        auto constrainedLinkIterator = constrainedIterator->second.begin( );

//        ObservableType currentObservable = unconstrainedIterator->first;

//        std::shared_ptr< ObservationSimulatorBase< double, double > > currentObservationSimulator =
//                observationSimulatorsMap.at( currentObservable );

//        // Iterate over all link ends of current observables.
//        for( int j = 0; j < numberOfLinkEnds; j++ )
//        {
//            LinkEnds currentLinkEnds = unconstrainedLinkIterator->first;

//            std::vector< std::shared_ptr< ObservationViabilityCalculator > > currentViabilityCalculators =
//                    tudat::observation_models::createObservationViabilityCalculators(
//                        bodies, currentLinkEnds, currentObservable, observationViabilitySettings );

//            int unconstrainedIndex = 0;
//            int constrainedIndex = 0;

//            bool currentObservationWasViable = 0;
//            bool currentObservationIsViable = 0;
//            bool isSingleViabilityConditionMet = 0;
//            Eigen::VectorXd currentObservation;

//            // Retrieve observations for current link ends/observable type
//            std::vector< std::pair< int, int > > constrainedIndices = constrainedLinkIterator->second;
//            std::vector< std::pair< int, int > > unconstrainedIndices = unconstrainedLinkIterator->second;

//            BOOST_CHECK_EQUAL( constrainedIndices.size( ), unconstrainedIndices.size( ) );

//            for( unsigned int k = 0; k < constrainedIndices.size( ); k++ )
//            {

//                int currentUnconstrainedStartIndex = unconstrainedIndices.at( k ).first;
//                int currentUnconstrainedBlockSize = unconstrainedIndices.at( k ).second;

//                int currentConstrainedStartIndex = constrainedIndices.at( k ).first;
//                int currentConstrainedBlockSize = constrainedIndices.at( k ).second;
//                std::cout<<currentObservable<<" "<<j<<std::endl;

//                std::cout<<currentUnconstrainedStartIndex<<" "<<currentUnconstrainedBlockSize<<" "<<unconstrainedConcatenatedTimes.size( )<<std::endl;
////                Eigen::VectorXd unconstrainedObservationSegment =
////                        unconstrainedSimulatedObservables->getObservationVector( ).segment(
////                            currentUnconstrainedStartIndex, currentUnconstrainedBlockSize );
////                Eigen::VectorXd constrainedObservationSegment =
////                        constrainedSimulatedObservables->currentConstrainedBlockSize( ).segment(
////                            currentConstrainedStartIndex, currentBlockSize );

//                std::vector< double > unconstrainedTimesSegment(
//                            unconstrainedConcatenatedTimes.begin( ) + currentUnconstrainedStartIndex,
//                            unconstrainedConcatenatedTimes.begin( ) + currentUnconstrainedStartIndex + currentUnconstrainedBlockSize );
//                std::cout<<currentConstrainedStartIndex<<" "<<currentConstrainedBlockSize<<" "<<constrainedConcatenatedTimes.size( )<<std::endl;

//                std::vector< double > constrainedTimesSegment(
//                            constrainedConcatenatedTimes.begin( ) + currentConstrainedStartIndex,
//                            constrainedConcatenatedTimes.begin( ) + currentConstrainedStartIndex + currentConstrainedBlockSize );

////                std::pair< Eigen::VectorXd, std::vector< double > > unconstrainedSingleLinkObservations;
////                    unconstrainedSimulatedObservables->
////                std::pair< Eigen::VectorXd, std::vector< double > > constrainedSingleLinkObservations;

//                // Iterate over all unconstrained observations for current observable/link ends
//                while( unconstrainedIndex < unconstrainedNumberOfObservations )
//                {
//                    // Check if current observation was rejected by viability calculators
//                    if( constrainedIndex >= static_cast< int >( constrainedTimesSegment.size( ) ) )
//                    {
//                        currentObservationWasViable = false;
//                    }
//                    else if( unconstrainedTimesSegment.at( unconstrainedIndex ) ==
//                             constrainedTimesSegment.at( constrainedIndex ) )
//                    {
//                        currentObservationWasViable = true;
//                    }
//                    else
//                    {
//                        currentObservationWasViable = false;
//                    }

//                    // Re-simulate current observation
//                    if( currentObservable == angular_position )
//                    {
//                        currentObservation = std::dynamic_pointer_cast< ObservationSimulator< 2 > >(
//                                    currentObservationSimulator )->getObservationModel( currentLinkEnds )->
//                                computeObservationsWithLinkEndData(
//                                    unconstrainedObservationTimes.at( unconstrainedIndex ), referenceLinkEnd,
//                                    linkEndTimes, linkEndStates );
//                    }
//                    else
//                    {
//                        currentObservation = std::dynamic_pointer_cast< ObservationSimulator< 1 > >(
//                                    currentObservationSimulator )->getObservationModel( currentLinkEnds )->
//                                computeObservationsWithLinkEndData(
//                                    unconstrainedObservationTimes.at( unconstrainedIndex ), referenceLinkEnd,
//                                    linkEndTimes, linkEndStates );
//                    }

//                    // Re-compute viability according to viability calculator objects.
//                    currentObservationIsViable = true;
//                    for( unsigned int k = 0; k < currentViabilityCalculators.size( ); k++ )
//                    {
//                        isSingleViabilityConditionMet = currentViabilityCalculators.at( k )->isObservationViable(
//                                    linkEndStates, linkEndTimes );
//                        if( !isSingleViabilityConditionMet )
//                        {
//                            currentObservationIsViable = false;
//                        }
//                    }

//                    // Manually recompute mars elevation angle condition
//                    std::vector< double > marsElevationAngles = getBodyLinkElevationAngles(
//                                currentLinkEnds, currentObservable, "Mars", linkEndStates, linkEndTimes, bodies );
//                    bool computedViability = true;
//                    for( unsigned int l = 0; l < marsElevationAngles.size( ); l++ )
//                    {
//                        if( marsElevationAngles.at( l ) < marstestAngle )
//                        {
//                            computedViability =  false;
//                        }
//                    }

////                    std::cout<<computedViability<<" ";
//                    // Manually recompute earth elevation angle condition
//                    std::vector< double > earthElevationAngles = getBodyLinkElevationAngles(
//                                currentLinkEnds, currentObservable, "Earth", linkEndStates, linkEndTimes, bodies );
//                    for( unsigned int l = 0; l < earthElevationAngles.size( ); l++ )
//                    {
//                        if( earthElevationAngles.at( l ) < earthtestAngle )
//                        {
//                            computedViability =  false;
//                        }
//                    }
////                    std::cout<<computedViability<<" ";

//                    // Manually recompute earth-Sun avoidance angle condition
//                    std::vector< double > earthSunCosineAvoidanceAngles = getBodyCosineAvoidanceAngles(
//                                currentLinkEnds, currentObservable, "Earth", "Sun", linkEndStates, linkEndTimes, bodies );
//                    for( unsigned int l = 0; l < earthSunCosineAvoidanceAngles.size( ); l++ )
//                    {
//                        if( earthSunCosineAvoidanceAngles.at( l ) > std::cos( earthSunAvoidanceAngle ) )
//                        {
//                            computedViability =  false;
//                        }
//                    }
////                    std::cout<<computedViability<<" ";

//                    // Manually recompute mars-Sun avoidance angle condition
//                    std::vector< double > marsSunCosineAvoidanceAngles = getBodyCosineAvoidanceAngles(
//                                currentLinkEnds, currentObservable, "Mars", "Sun", linkEndStates, linkEndTimes, bodies );
//                    for( unsigned int l = 0; l < marsSunCosineAvoidanceAngles.size( ); l++ )
//                    {
//                        if( marsSunCosineAvoidanceAngles.at( l ) > std::cos( marsSunAvoidanceAngle ) )
//                        {
//                            computedViability =  false;
//                        }
//                    }
////                    std::cout<<computedViability<<" ";

//                    // Manually recompute occultataion consition
//                    std::vector< double > moonLineOfSightDistances = getDistanceBetweenLineOfSightVectorAndPoint(
//                                "Moon", linkEndStates, linkEndTimes, bodies );
//                    for( unsigned int l = 0; l < moonLineOfSightDistances.size( ); l++ )
//                    {
////                        std::cout<<moonLineOfSightDistances.at( l )<<" "<<moonRadius<<std::endl;
//                        if( moonLineOfSightDistances.at( l ) < moonRadius )
//                        {
//                            computedViability =  false;
//                        }
//                    }

////                    std::cout<<currentObservationIsViable<<std::endl<<std::endl;

//                    // Check manual/automatic viability check
//                    BOOST_CHECK_EQUAL( currentObservationIsViable, currentObservationWasViable );
//                    BOOST_CHECK_EQUAL( computedViability, currentObservationWasViable );

//                    if( currentObservationWasViable )
//                    {
//                        constrainedIndex++;
//                    }
//                    unconstrainedIndex++;
//                }
////                BOOST_CHECK_EQUAL( constrainedIndex, constrainedLinkIterator->second.second.size( ) );

//            }
//            unconstrainedLinkIterator++;
//            constrainedLinkIterator++;
//        }

//        unconstrainedIterator++;
//        constrainedIterator++;
//    }
//}

BOOST_AUTO_TEST_SUITE_END( )

}

}


#ifndef BOOST_GEOMETRY_PROJECTIONS_TMERC_HPP
#define BOOST_GEOMETRY_PROJECTIONS_TMERC_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// 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 <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/function_overloads.hpp>
#include <boost/geometry/srs/projections/impl/pj_mlfn.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct tmerc {};
    struct utm {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace tmerc
    {

            static const double EPS10 = 1.e-10;
            //static const double FC1 = 1.;
            //static const double FC2 = .5;
            //static const double FC3 = .16666666666666666666;
            //static const double FC4 = .08333333333333333333;
            //static const double FC5 = .05;
            //static const double FC6 = .03333333333333333333;
            //static const double FC7 = .02380952380952380952;
            //static const double FC8 = .01785714285714285714;

            template <typename T>
            inline T FC1() { return 1.; }
            template <typename T>
            inline T FC2() { return .5; }
            template <typename T>
            inline T FC3() { return .16666666666666666666666666666666666666; }
            template <typename T>
            inline T FC4() { return .08333333333333333333333333333333333333; }
            template <typename T>
            inline T FC5() { return .05; }
            template <typename T>
            inline T FC6() { return .03333333333333333333333333333333333333; }
            template <typename T>
            inline T FC7() { return .02380952380952380952380952380952380952; }
            template <typename T>
            inline T FC8() { return .01785714285714285714285714285714285714; }

            template <typename T>
            struct par_tmerc
            {
                T    esp;
                T    ml0;
                T    en[EN_SIZE];
            };

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_tmerc_ellipsoid : public base_t_fi<base_tmerc_ellipsoid<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_tmerc<CalculationType> m_proj_parm;

                inline base_tmerc_ellipsoid(const Parameters& par)
                    : base_t_fi<base_tmerc_ellipsoid<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(e_forward)  ellipse
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    static const CalculationType HALFPI = detail::HALFPI<CalculationType>();
                    static const CalculationType FC1 = tmerc::FC1<CalculationType>();
                    static const CalculationType FC2 = tmerc::FC2<CalculationType>();
                    static const CalculationType FC3 = tmerc::FC3<CalculationType>();
                    static const CalculationType FC4 = tmerc::FC4<CalculationType>();
                    static const CalculationType FC5 = tmerc::FC5<CalculationType>();
                    static const CalculationType FC6 = tmerc::FC6<CalculationType>();
                    static const CalculationType FC7 = tmerc::FC7<CalculationType>();
                    static const CalculationType FC8 = tmerc::FC8<CalculationType>();

                    CalculationType al, als, n, cosphi, sinphi, t;

                    /*
                     * Fail if our longitude is more than 90 degrees from the
                     * central meridian since the results are essentially garbage.
                     * Is error -20 really an appropriate return value?
                     *
                     *  http://trac.osgeo.org/proj/ticket/5
                     */
                    if( lp_lon < -HALFPI || lp_lon > HALFPI )
                    {
                        xy_x = HUGE_VAL;
                        xy_y = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(-14) );
                        return;
                    }

                    sinphi = sin(lp_lat);
                    cosphi = cos(lp_lat);
                    t = fabs(cosphi) > 1e-10 ? sinphi/cosphi : 0.;
                    t *= t;
                    al = cosphi * lp_lon;
                    als = al * al;
                    al /= sqrt(1. - this->m_par.es * sinphi * sinphi);
                    n = this->m_proj_parm.esp * cosphi * cosphi;
                    xy_x = this->m_par.k0 * al * (FC1 +
                        FC3 * als * (1. - t + n +
                        FC5 * als * (5. + t * (t - 18.) + n * (14. - 58. * t)
                        + FC7 * als * (61. + t * ( t * (179. - t) - 479. ) )
                        )));
                    xy_y = this->m_par.k0 * (pj_mlfn(lp_lat, sinphi, cosphi, this->m_proj_parm.en) - this->m_proj_parm.ml0 +
                        sinphi * al * lp_lon * FC2 * ( 1. +
                        FC4 * als * (5. - t + n * (9. + 4. * n) +
                        FC6 * als * (61. + t * (t - 58.) + n * (270. - 330 * t)
                        + FC8 * als * (1385. + t * ( t * (543. - t) - 3111.) )
                        ))));
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    static const CalculationType HALFPI = detail::HALFPI<CalculationType>();
                    static const CalculationType FC1 = tmerc::FC1<CalculationType>();
                    static const CalculationType FC2 = tmerc::FC2<CalculationType>();
                    static const CalculationType FC3 = tmerc::FC3<CalculationType>();
                    static const CalculationType FC4 = tmerc::FC4<CalculationType>();
                    static const CalculationType FC5 = tmerc::FC5<CalculationType>();
                    static const CalculationType FC6 = tmerc::FC6<CalculationType>();
                    static const CalculationType FC7 = tmerc::FC7<CalculationType>();
                    static const CalculationType FC8 = tmerc::FC8<CalculationType>();

                    CalculationType n, con, cosphi, d, ds, sinphi, t;

                    lp_lat = pj_inv_mlfn(this->m_proj_parm.ml0 + xy_y / this->m_par.k0, this->m_par.es, this->m_proj_parm.en);
                    if (fabs(lp_lat) >= HALFPI) {
                        lp_lat = xy_y < 0. ? -HALFPI : HALFPI;
                        lp_lon = 0.;
                    } else {
                        sinphi = sin(lp_lat);
                        cosphi = cos(lp_lat);
                        t = fabs(cosphi) > 1e-10 ? sinphi/cosphi : 0.;
                        n = this->m_proj_parm.esp * cosphi * cosphi;
                        d = xy_x * sqrt(con = 1. - this->m_par.es * sinphi * sinphi) / this->m_par.k0;
                        con *= t;
                        t *= t;
                        ds = d * d;
                        lp_lat -= (con * ds / (1.-this->m_par.es)) * FC2 * (1. -
                            ds * FC4 * (5. + t * (3. - 9. *  n) + n * (1. - 4 * n) -
                            ds * FC6 * (61. + t * (90. - 252. * n +
                                45. * t) + 46. * n
                           - ds * FC8 * (1385. + t * (3633. + t * (4095. + 1574. * t)) )
                            )));
                        lp_lon = d*(FC1 -
                            ds*FC3*( 1. + 2.*t + n -
                            ds*FC5*(5. + t*(28. + 24.*t + 8.*n) + 6.*n
                           - ds * FC7 * (61. + t * (662. + t * (1320. + 720. * t)) )
                        ))) / cosphi;
                    }
                }

                static inline std::string get_name()
                {
                    return "tmerc_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_tmerc_spheroid : public base_t_fi<base_tmerc_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_tmerc<CalculationType> m_proj_parm;

                inline base_tmerc_spheroid(const Parameters& par)
                    : base_t_fi<base_tmerc_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(s_forward)  sphere
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    static const CalculationType HALFPI = detail::HALFPI<CalculationType>();

                    CalculationType b, cosphi;

                    /*
                     * Fail if our longitude is more than 90 degrees from the
                     * central meridian since the results are essentially garbage.
                     * Is error -20 really an appropriate return value?
                     *
                     *  http://trac.osgeo.org/proj/ticket/5
                     */
                    if( lp_lon < -HALFPI || lp_lon > HALFPI )
                    {
                        xy_x = HUGE_VAL;
                        xy_y = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(-14) );
                        return;
                    }

                    cosphi = cos(lp_lat);
                    b = cosphi * sin(lp_lon);
                    if (fabs(fabs(b) - 1.) <= EPS10)
                        BOOST_THROW_EXCEPTION( projection_exception(-20) );

                    xy_x = this->m_proj_parm.ml0 * log((1. + b) / (1. - b));
                    xy_y = cosphi * cos(lp_lon) / sqrt(1. - b * b);

                    b = fabs( xy_y );
                    if (b >= 1.) {
                        if ((b - 1.) > EPS10)
                            BOOST_THROW_EXCEPTION( projection_exception(-20) );
                        else xy_y = 0.;
                    } else
                        xy_y = acos(xy_y);

                    if (lp_lat < 0.)
                        xy_y = -xy_y;
                    xy_y = this->m_proj_parm.esp * (xy_y - this->m_par.phi0);
                }

                // INVERSE(s_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    CalculationType h, g;

                    h = exp(xy_x / this->m_proj_parm.esp);
                    g = .5 * (h - 1. / h);
                    h = cos(this->m_par.phi0 + xy_y / this->m_proj_parm.esp);
                    lp_lat = asin(sqrt((1. - h * h) / (1. + g * g)));
                    if (xy_y < 0.) lp_lat = -lp_lat;
                    lp_lon = (g || h) ? atan2(g, h) : 0.;
                }

                static inline std::string get_name()
                {
                    return "tmerc_spheroid";
                }

            };

            template <typename Parameters, typename T>
            inline void setup(Parameters& par, par_tmerc<T>& proj_parm)  /* general initialization */
            {
                if (par.es) {
                    if (!pj_enfn(par.es, proj_parm.en))
                        BOOST_THROW_EXCEPTION( projection_exception(0) );
                    proj_parm.ml0 = pj_mlfn(par.phi0, sin(par.phi0), cos(par.phi0), proj_parm.en);
                    proj_parm.esp = par.es / (1. - par.es);
                } else {
                    proj_parm.esp = par.k0;
                    proj_parm.ml0 = .5 * proj_parm.esp;
                }
            }

            // Transverse Mercator
            template <typename Parameters, typename T>
            inline void setup_tmerc(Parameters& par, par_tmerc<T>& proj_parm)
            {
                setup(par, proj_parm);
            }

            // Universal Transverse Mercator (UTM)
            template <typename Parameters, typename T>
            inline void setup_utm(Parameters& par, par_tmerc<T>& proj_parm)
            {
                static const T ONEPI = detail::ONEPI<T>();

                int zone;

                par.y0 = pj_param(par.params, "bsouth").i ? 10000000. : 0.;
                par.x0 = 500000.;
                if (pj_param(par.params, "tzone").i) /* zone input ? */
                    if ((zone = pj_param(par.params, "izone").i) > 0 && zone <= 60)
                        --zone;
                    else
                        BOOST_THROW_EXCEPTION( projection_exception(-35) );
                else /* nearest central meridian input */
                    if ((zone = int_floor((adjlon(par.lam0) + ONEPI) * 30. / ONEPI)) < 0)
                        zone = 0;
                    else if (zone >= 60)
                        zone = 59;
                par.lam0 = (zone + .5) * ONEPI / 30. - ONEPI;
                par.k0 = 0.9996;
                par.phi0 = 0.;
                setup(par, proj_parm);
            }

    }} // namespace detail::tmerc
    #endif // doxygen

    /*!
        \brief Transverse Mercator projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_tmerc.gif
    */
    template <typename CalculationType, typename Parameters>
    struct tmerc_ellipsoid : public detail::tmerc::base_tmerc_ellipsoid<CalculationType, Parameters>
    {
        inline tmerc_ellipsoid(const Parameters& par) : detail::tmerc::base_tmerc_ellipsoid<CalculationType, Parameters>(par)
        {
            detail::tmerc::setup_tmerc(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Transverse Mercator projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_tmerc.gif
    */
    template <typename CalculationType, typename Parameters>
    struct tmerc_spheroid : public detail::tmerc::base_tmerc_spheroid<CalculationType, Parameters>
    {
        inline tmerc_spheroid(const Parameters& par) : detail::tmerc::base_tmerc_spheroid<CalculationType, Parameters>(par)
        {
            detail::tmerc::setup_tmerc(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Universal Transverse Mercator (UTM) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
        \par Projection parameters
         - zone: UTM Zone (integer)
         - south: Denotes southern hemisphere UTM zone (boolean)
        \par Example
        \image html ex_utm.gif
    */
    template <typename CalculationType, typename Parameters>
    struct utm_ellipsoid : public detail::tmerc::base_tmerc_ellipsoid<CalculationType, Parameters>
    {
        inline utm_ellipsoid(const Parameters& par) : detail::tmerc::base_tmerc_ellipsoid<CalculationType, Parameters>(par)
        {
            detail::tmerc::setup_utm(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Universal Transverse Mercator (UTM) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
        \par Projection parameters
         - zone: UTM Zone (integer)
         - south: Denotes southern hemisphere UTM zone (boolean)
        \par Example
        \image html ex_utm.gif
    */
    template <typename CalculationType, typename Parameters>
    struct utm_spheroid : public detail::tmerc::base_tmerc_spheroid<CalculationType, Parameters>
    {
        inline utm_spheroid(const Parameters& par) : detail::tmerc::base_tmerc_spheroid<CalculationType, Parameters>(par)
        {
            detail::tmerc::setup_utm(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::tmerc, tmerc_spheroid, tmerc_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::utm, utm_spheroid, utm_ellipsoid)

        // Factory entry(s) - dynamic projection
        template <typename CalculationType, typename Parameters>
        class tmerc_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    if (par.es)
                        return new base_v_fi<tmerc_ellipsoid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                    else
                        return new base_v_fi<tmerc_spheroid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        class utm_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    if (par.es)
                        return new base_v_fi<utm_ellipsoid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                    else
                        return new base_v_fi<utm_spheroid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        inline void tmerc_init(detail::base_factory<CalculationType, Parameters>& factory)
        {
            factory.add_to_factory("tmerc", new tmerc_entry<CalculationType, Parameters>);
            factory.add_to_factory("utm", new utm_entry<CalculationType, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_TMERC_HPP


#include <boost/program_options.hpp>

#include <bts/blockchain/address.hpp>
#include <bts/blockchain/pts_address.hpp>
#include <bts/blockchain/types.hpp>
#include <bts/utilities/deterministic_openssl_rand.hpp>
#include <bts/utilities/key_conversion.hpp>

#include <fc/crypto/elliptic.hpp>
#include <fc/io/json.hpp>
#include <fc/reflect/variant.hpp>
#include <fc/filesystem.hpp>
#include <fc/variant_object.hpp>

#include <iostream>

using namespace bts::blockchain;

boost::program_options::variables_map parse_option_variables(int argc, char** argv)
{
    boost::program_options::positional_options_description p_option_config;
    p_option_config.add("json-outfile", 1);

    boost::program_options::options_description option_config("Usage");
    option_config.add_options()
        ("help", "Display this help message and exit")

        ("json-outfile", boost::program_options::value<string>()->default_value(""), "Output file for private key data")

        ("seed", boost::program_options::value<string>(), "Set seed for deterministic key generation")
        ("count", boost::program_options::value<int64_t>()->default_value(-1), "Set number of keys to generate")
        ;

    boost::program_options::variables_map option_variables;
    try
    {
        boost::program_options::store(
                                      boost::program_options::command_line_parser(argc, argv)
                                     .options(option_config)
                                     .positional(p_option_config)
                                     .run(),
                                     option_variables
                                     );
        boost::program_options::notify(option_variables);
    }
    catch (boost::program_options::error& cmdline_error)
    {
        std::cerr << "Error: " << cmdline_error.what() << "\n";
        std::cerr << option_config << "\n";
        exit(1);
    }

    if (option_variables.count("help"))
    {
        std::cout << option_config << "\n";
        exit(0);
    }

    return option_variables;
}

int run( int64_t index, fc::optional<string> seed, fc::optional<fc::path> json_outfile )
{
     fc::ecc::private_key key;

     if( seed.valid() )
     {
         string effective_seed;
         if( index >= 0 )
             effective_seed = (*seed)+std::to_string(index);
         else
             effective_seed = (*seed);
         fc::sha256 hash_effective_seed = fc::sha256::hash(
             effective_seed.c_str(),
             effective_seed.length() );
         key = fc::ecc::private_key::regenerate( hash_effective_seed );
    }
    else
        key = fc::ecc::private_key::generate();

    if( !json_outfile.valid() )
    {
        auto obj = fc::mutable_variant_object();

        obj["public_key"] = public_key_type(key.get_public_key());
        obj["private_key"] = key.get_secret();
        obj["wif_private_key"] = bts::utilities::key_to_wif(key);
        obj["native_address"] = bts::blockchain::address(key.get_public_key());
        obj["pts_address"] = bts::blockchain::pts_address(key.get_public_key());

        std::cout << fc::json::to_pretty_string(obj) << '\n';
        return 0;
    }
    else
    {
        std::cout << "writing new private key to JSON file " << (*json_outfile).string() << "\n";
        fc::json::save_to_file(key, (*json_outfile));

        std::cout << "bts address: "
        << std::string(bts::blockchain::address(key.get_public_key())) << "\n";

        return 0;
    }
}

int main( int argc, char** argv )
{
    boost::program_options::variables_map option_variables =
        parse_option_variables(argc, argv);

    fc::optional<string> seed;
    fc::optional<fc::path> json_outfile;

    if( option_variables.count("seed") )
        seed = option_variables["seed"].as<string>();
    if( option_variables.count("json-outfile") )
    {
        string s_json_outfile = option_variables["json-outfile"].as<string>();
        if( s_json_outfile != "" )
            json_outfile = fc::path(s_json_outfile);
    }

    if( ! option_variables.count("count") )
    {
        std::cerr << "count undefined, should never happen!\n";
        return 1;
    }

    int n = option_variables["count"].as<int64_t>();
    if( n != -1 )
    {
        if( json_outfile.valid() )
        {
            std::cerr << "Combining --count and --json-outfile is currently unsupported\n";
            return 1;
        }
        std::cout << "[\n";
        for( int64_t i=0; i<n; i++ )
        {
            if (i != 0)
                std::cout << ",\n";
            int result = run(i, seed, json_outfile);
            if( result != 0 )
                return result;
        }
        std::cout << "]\n";
    }
    else
        return run(-1, seed, json_outfile);

    return 0;
}


#include <stdio.h>
#include <cfloat>
#include <math.h>
#include <vector>
#include <ctime>
#include <cstdlib>
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <string>
#include <cstddef>
#include <nlopt.hpp>
#include <pangolin/pangolin.h>
#include "opencv2/opencv.hpp"

#include <Eigen/Core>

#include <df/camera/camera.h>
#include <df/camera/linear.h>
#include <df/camera/poly3.h>
#include <df/camera/rig.h>
#include <df/image/backprojection.h>
#include <df/prediction/glRender.h>
#include <df/prediction/glRenderTypes.h>
#include <df/util/args.h>
#include <df/util/glHelpers.h>
#include <df/util/pangolinHelpers.h>
#include <df/util/tensor.h>

#include <assimp/cimport.h>
#include <assimp/scene.h>

template <typename Derived>
inline void operator >>(std::istream & stream, Eigen::MatrixBase<Derived> & M)
{

    for (int r = 0; r < M.rows(); ++r) {
        for (int c = 0; c < M.cols(); ++c) {
            stream >> M(r,c);
        }
    }

}

struct ForegroundRenderType {
    static std::string vertShaderName() {
        static const char name[] = "foreground.vert";
        return std::string(name);
    }
    static std::string fragShaderName() {
        static const char name[] = "foreground.frag";
        return std::string(name);
    }
    static constexpr int numTextures = 1;
    static const GLenum * textureFormats() {
        static const GLenum formats[numTextures] = { GL_RGBA32F};
        return formats;
    }
    static constexpr int numVertexAttributes = 1;
    static const int * vertexAttributeSizes() {
        static const int sizes[numVertexAttributes] = { 3 };
        return sizes;
    }
    static const GLenum * vertexAttributeTypes() {
        static const GLenum types[numVertexAttributes] = { GL_FLOAT };
        return types;
    }
};

unsigned char class_colors[22][3] = {{255, 255, 255}, {255, 0, 0}, {0, 255, 0}, {0, 0, 255}, {255, 255, 0}, {255, 0, 255}, {0, 255, 255},
                              {128, 0, 0}, {0, 128, 0}, {0, 0, 128}, {128, 128, 0}, {128, 0, 128}, {0, 128, 128},
                              {64, 0, 0}, {0, 64, 0}, {0, 0, 64}, {64, 64, 0}, {64, 0, 64}, {0, 64, 64},
                              {192, 0, 0}, {0, 192, 0}, {0, 0, 192}};

struct DataForOpt
{
  int width, height;
  cv::Rect bb2D;
  std::vector<cv::Point3f> bb3D;
  cv::Mat_<float> camMat;
  pangolin::OpenGlMatrixSpec projectionMatrix;
  pangolin::GlBuffer* texturedVertices;
  pangolin::GlBuffer* texturedIndices;
  cv::Mat* gt_mask;
  pangolin::View* view;
  df::GLRenderer<ForegroundRenderType>* renderer;
};

static double optEnergy(const std::vector<double> &pose, std::vector<double> &grad, void *data);
double poseWithOpt(std::vector<double> & vec, DataForOpt data, int iterations);
inline float getIoU(const cv::Rect& bb1, const cv::Rect bb2);
int clamp(int val, int min_val, int max_val);
inline cv::Rect getBB2D(int imageWidth, int imageHeight, const std::vector<cv::Point3f>& bb3D, const cv::Mat& camMat, const cv::Mat& RT);

class Refiner
{
 public:

  Refiner() {};
  Refiner(std::string model_file);
  ~Refiner();

  void setup(std::string model_file);
  void create_window(int width, int height);
  void destroy_window();
  void render(unsigned char* data, unsigned char* labels, float* rois, int num_rois, int num_gt, int width, int height, int num_classes,
                    float* poses_gt, float* poses_pred, float fx, float fy, float px, float py, float* extents, float* poses_new, int is_save);
  void loadModels(std::string filename);
  aiMesh* loadTexturedMesh(const std::string filename, std::string & texture_name);
  void initializeBuffers(aiMesh* assimpMesh, std::string textureName,
    pangolin::GlBuffer & vertices, pangolin::GlBuffer & indices, pangolin::GlBuffer & texCoords, pangolin::GlTexture & texture, bool is_textured);
  void feed_data(int width, int height, unsigned char* data, unsigned char* labels, pangolin::GlTexture & colorTex, pangolin::GlTexture & labelTex);

  void refine(unsigned char* labels, float* rois, int num_rois, int width, int height, int num_classes,
                    float* poses_pred, float fx, float fy, float px, float py, float* extents, float* poses_new);

  void getBb3Ds(float* extents, std::vector<std::vector<cv::Point3f>>& bb3Ds, int num_classes);
  inline std::vector<cv::Point3f> getBB3D(const cv::Vec<float, 3>& extent);

 private:
  int counter_;

  // 3D models
  std::vector<aiMesh*> assimpMeshes_;
  std::vector<std::string> texture_names_;

  // pangoline views
  pangolin::View* gtView_;
  pangolin::View* poseView_;
  pangolin::View* colorView_;
  pangolin::View* labelView_;
  pangolin::View* maskView_;
  pangolin::View* multiView_;

  // buffers
  std::vector<pangolin::GlBuffer> texturedVertices_;
  std::vector<pangolin::GlBuffer> texturedIndices_;
  std::vector<pangolin::GlBuffer> texturedCoords_;
  std::vector<pangolin::GlTexture> texturedTextures_;

  df::GLRenderer<ForegroundRenderType>* renderer_;
};


#include "main.hpp"
#include <Eigen/Core>
#include <chrono>
#include <cstring>
#include <iostream>
#include <string>
#include <vector>
#include "HardwareIO/Joint/LinearAxis/LinearAxis.hpp"
#include "HardwareIO/Joint/OneAxis/OneAxis.hpp"
#include "HardwareIO/Joint/TwoAxis/TwoAxis.hpp"
#include "HardwareIO/Muscle/Muscle.hpp"
#include "HardwareIO/Valve/Valve.hpp"
#include "Input/Controller/Controller.hpp"
#include "MPC/AdaptiveMPC/AdaptiveMPC.hpp"
#include "MPC/AdaptiveMPC/Optimizer.hpp"
#include "PID/PID.hpp"
#include "SMC/SMC.hpp"
#include "Sensors/LinearPotentiometer/LinearPotentiometer.hpp"
#include "Sensors/PressureSensor/PressureSensor.hpp"
#include "adc.h"
#include "dma.h"
#include "gpio.h"
#include "main.h"
#include "nuclear/src/clock.hpp"
#include "stm32f7xx_hal_conf.h"
#include "stm32f7xx_it.h"
#include "tim.h"
#include "usart.h"
#include "utility/io/adc.hpp"
#include "utility/io/gpio.hpp"
#include "utility/io/uart.hpp"

extern "C" {
void SystemClock_Config(void) {
    RCC_OscInitTypeDef RCC_OscInitStruct;
    RCC_ClkInitTypeDef RCC_ClkInitStruct;
    RCC_PeriphCLKInitTypeDef PeriphClkInitStruct;

    // Configure LSE Drive Capability
    HAL_PWR_EnableBkUpAccess();

    // Configure the main internal regulator output voltage
    __HAL_RCC_PWR_CLK_ENABLE();
    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

    // Initializes the CPU, AHB and APB busses clocks
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    RCC_OscInitStruct.HSEState       = RCC_HSE_ON;
    RCC_OscInitStruct.PLL.PLLState   = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource  = RCC_PLLSOURCE_HSE;
    RCC_OscInitStruct.PLL.PLLM       = 8;
    RCC_OscInitStruct.PLL.PLLN       = 432;
    RCC_OscInitStruct.PLL.PLLP       = RCC_PLLP_DIV2;
    RCC_OscInitStruct.PLL.PLLQ       = 9;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
        Error_Handler();
    }

    // Activate the Over-Drive mode
    if (HAL_PWREx_EnableOverDrive() != HAL_OK) {
        Error_Handler();
    }

    // Initializes the CPU, AHB and APB busses clocks
    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource   = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider  = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_7) != HAL_OK) {
        Error_Handler();
    }
    PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART6;
    PeriphClkInitStruct.Usart6ClockSelection = RCC_USART6CLKSOURCE_PCLK2;
    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) {
        Error_Handler();
    }
}

void Error_Handler(void) {
    utility::io::debug.error("\n");
    utility::io::gpio::led3 = true;
    while (1) {
    }
}
}

int main() {
    /*******************************************************************************************************************
    ********************************************** System Initialisation ***********************************************
    *******************************************************************************************************************/
    // Initialise the HAL driver
    HAL_Init();

    // Configure the system clock
    SystemClock_Config();
    MX_GPIO_Init();
    MX_DMA_Init();
    MX_USART6_UART_Init();
    utility::clock::initialise();
    utility::io::adc_io.initialise();

    /*******************************************************************************************************************
    ************************************************* Muscles & Joints *************************************************
    *******************************************************************************************************************/
    auto time_start = NUClear::clock::now();

    utility::io::debug.out("\033[3J");
    utility::io::debug.out("\014");
    utility::io::debug.out("\033[3J");
    utility::io::debug.out("\014");
    utility::io::debug.out("\e[0m");

    utility::io::debug.out("Welcome to PNEUbot\n");

    // We need our setpoint potentiometer
    // auto Setpoint = module::Input::controller.GetPosition();

    // Declare our muscle parameters and populate a vector of muscles
    // float nom_length;
    // float contraction_percent;
    // float critical_ratio;
    // float sonic_conductance;
    // float T_0;
    // float T_1;
    // float damping_coefficient;
    // float muscle_coefficients[4];
    // float F_ce[6][6];

    constexpr float p00 = (76.27) * 10e-35;       // (76.25, 76.29)
    constexpr float p10 = (1.515e+12) * 10e-35;   // (-1.512e+13, 1.815e+13)
    constexpr float p01 = (-1.515e+12) * 10e-35;  // (-1.815e+13, 1.512e+13)
    constexpr float p20 = (7.645e+12) * 10e-35;   // (-3.98e+12, 1.927e+13)
    constexpr float p11 = (-1.984e+13) * 10e-35;  // (-3.816e+13, -1.518e+12)
    constexpr float p02 = (1.219e+13) * 10e-35;   // (-2.637e+12, 2.702e+13)
    constexpr float p30 = (-1.535e+13) * 10e-35;  // (-3.118e+13, 4.877e+11)
    constexpr float p21 = (3.063e+13) * 10e-35;   // (9.519e+12, 5.174e+13)
    constexpr float p12 = (-1.087e+13) * 10e-35;  // (-3.451e+13, 1.277e+13)
    constexpr float p03 = (-4.412e+12) * 10e-35;  // (-2.166e+13, 1.284e+13)
    constexpr float p40 = (2.531e+13) * 10e-35;   // (1.529e+13, 3.532e+13)
    constexpr float p31 = (-2.712e+13) * 10e-35;  // (-3.913e+13, -1.511e+13)
    constexpr float p22 = (-1.771e+13) * 10e-35;  // (-2.676e+13, -8.651e+12)
    constexpr float p13 = (1.455e+13) * 10e-35;   // (9.587e+11, 2.814e+13)
    constexpr float p04 = (4.974e+12) * 10e-35;   // (-5.777e+12, 1.572e+13)
    constexpr float p50 = (-1.836e+12) * 10e-35;  // (-4.701e+12, 1.03e+12)
    constexpr float p41 = (3.682e+12) * 10e-35;   // (1.3e+12, 6.063e+12)
    constexpr float p32 = (2.06e+12) * 10e-35;    // (-9.788e+11, 5.098e+12)
    constexpr float p23 = (-6.712e+12) * 10e-35;  // (-9.919e+12, -3.506e+12)
    constexpr float p14 = (1.642e+12) * 10e-35;   // (-9.098e+11, 4.195e+12)
    constexpr float p05 = (1.164e+12) * 10e-35;   // (-1.678e+12, 4.006e+12)

    // clang-format off

    Eigen::Matrix<float, 6, 6> F_ce;
    F_ce <<  p00, p01, p02, p03, p04, p05,
             p10, p11, p12, p13, p14, 0,
             p20, p21, p22, p23, 0,   0,
             p30, p31, p32, 0,   0,   0,
             p40, p41, 0,   0,   0,   0,
             p50, 0,   0,   0,   0,   0;
    const module::HardwareIO::muscle_properties_t pm_280 = {0.25, 0.108, 0.433, 2.6167e-9, 298.15, 298.15, 0.0007, {1.6e-4, -6.4e-4, 5.5e-4, 0.8e-4}, F_ce};
    // clang-format on

    std::vector<module::HardwareIO::muscle_t> muscles;

    module::HardwareIO::muscle_t muscle1 = {
        module::HardwareIO::valve1, module::Sensors::pressuresensor1, module::Sensors::linearpot1, pm_280};

    module::HardwareIO::muscle_t muscle2 = {
        module::HardwareIO::valve2, module::Sensors::pressuresensor2, module::Sensors::linearpot2, pm_280};

    muscles.push_back(muscle1);
    muscles.push_back(muscle2);

    // Make our joints with the previously declared muscles
    // MPC Controller
    module::HardwareIO::joint::OneAxis<module::MPC::AdaptiveMPC::AdaptiveMPC> one_axis_joint(
        muscles, 1, 0.47, module::MPC::AdaptiveMPC::mpc, module::MPC::AdaptiveMPC::optimizer1);
    // SMC Controller
    // module::HardwareIO::joint::OneAxis<module::SMC::SMC> one_axis_joint(muscles, 1, 0.47, module::SMC::smc);

    // Start our ADC DMA
    utility::io::adc_io.Start();
    utility::io::debug.out("Initialisation Finished\n");

    /*******************************************************************************************************************
    **************************************************** Controller ****************************************************
    *******************************************************************************************************************/
    // This is where our 'controller' starts
    float Sampling_time = 50;  // 0.01 T_s
    int i               = 0;
    time_start          = NUClear::clock::now();  // TODO Remove
    auto prev_now       = NUClear::clock::now();

    while (1) {

        // auto now   = NUClear::clock::now();
        float time = std::chrono::duration_cast<std::chrono::milliseconds>(NUClear::clock::now() - prev_now).count();

        // utility::io::debug.out("PNEUBot is running %lf\n", time / 1000);

        one_axis_joint.UpdateVelocity();

        // Sampling time controller to handle the periodicity of the code
        if (time >= Sampling_time) {  // && time < Sampling_time * 2) {
            utility::io::gpio::led2 = !utility::io::gpio::led2;
            prev_now                = NUClear::clock::now();
            // Time to run our controller again
            // utility::io::debug.out("SP %.2f | ", module::Input::controller.GetPosition());

            one_axis_joint.Compute(module::Input::controller.GetPosition());

            utility::io::adc_io.Start();
            Error_Handler();

            // utility::io::debug.out("%lf ->\t", time);

            // utility::io::adc_io.PrintSensors();
            i++;
        }
        // else if (time >= Sampling_time) {
        //     utility::io::debug.out("Too slow %lf\n", time);
        //     Error_Handler();
        // }
        if (i >= 500) {
            time = std::chrono::duration_cast<std::chrono::milliseconds>(NUClear::clock::now() - time_start).count();
            utility::io::debug.out("TIME %lf, Avg %lf\n", time, time / 500.0);
            Error_Handler();
        }
    }
}

// TODO I think this is how the system needs to work
// Joints know about the muscles, a joint contains all of the muscles it needs to perform. Each muscle has knowledge of
// its respective sensors, pressure, position. A muscle is responsible for reading it's sensors. The joint compute
// function then calls the appropriate MPC function and arranges the inputs how the MPC expects. Something like this


//          Copyright Rein Halbersma 2010-2020.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <core/board/sequence.hpp>              // nano, micro, checkers, Roman, spantsiretti, international, frisian, ktar<10, 11>,
                                                // ktar<10, 12>, Compact_10_12, Compact_12_10, basic_board<12, 10>, canadian, srilankan, dumm
#include <core/board/transform.hpp>             // is_involution, is_idempotent
#include <dctl/core/board/basic_board.hpp>      // board
#include <dctl/core/board/mask.hpp>             // basic_mask
#include <dctl/core/rules/type_traits.hpp>      // is_empty, is_pushable, is_jumpable, invert
#include <dctl/core/state/color.hpp>
#include <boost/range/irange.hpp>               // irange
#include <boost/test/unit_test.hpp>             // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE, BOOST_AUTO_TEST_CASE_TEMPLATE
#include <algorithm>                            // all_of
#include <numeric>                              // accumulate
#include <type_traits>                          // is_same

BOOST_AUTO_TEST_SUITE(BoardTraits)

using namespace dctl::core;

BOOST_AUTO_TEST_CASE(IsEmpty)
{
        static_assert( is_placeable_v<basic_board<rectangular<1, 1>>>);
        static_assert( is_placeable_v<basic_board<rectangular<2, 1, 0>>>);
        static_assert( is_placeable_v<basic_board<rectangular<1, 2, 0>>>);
}

BOOST_AUTO_TEST_CASE(IsPushable)
{
        static_assert(!is_pushable_v<basic_board<rectangular<1, 1>>>);
        static_assert(!is_pushable_v<basic_board<rectangular<2, 1>>>);
        static_assert(!is_pushable_v<basic_board<rectangular<1, 2>>>);
        static_assert(    is_pushable_v<basic_board<rectangular<2, 2>>>);
}

BOOST_AUTO_TEST_CASE(IsJumpable)
{
        static_assert(!is_jumpable_v<basic_board<rectangular<2, 2>>>);
        static_assert(!is_jumpable_v<basic_board<rectangular<3, 2>>>);
        static_assert(!is_jumpable_v<basic_board<rectangular<2, 3>>>);
        static_assert(!is_jumpable_v<basic_board<rectangular<3, 3, 0>>>);
        static_assert(    is_jumpable_v<basic_board<rectangular<3, 3>>>);
        static_assert(    is_jumpable_v<basic_board<rectangular<4, 3, 0>>>);
        static_assert(    is_jumpable_v<basic_board<rectangular<3, 4, 0>>>);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(IsRegular, T, BoardSequence)
{
        static_assert(is_placeable_v<T>);
        static_assert(is_pushable_v<T>);
        static_assert(is_jumpable_v<T>);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(SquaresCountEqualsBoardSize, T, BoardSequence)
{
        using mask_type = basic_mask<T>;
        BOOST_CHECK_EQUAL(mask_type::squares.ssize(), T::size());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(ColumnsEquivalencePartitionSquares, T, BoardSequence)
{
        auto const files = boost::irange(0, T::width);
        using mask_type = basic_mask<T>;

        BOOST_CHECK(
                std::all_of(files.begin(), files.end(), [=](auto i) {
                        return mask_type::file(black_c, i) == mask_type::file(white_c, T::width - 1 - i);
                })
        );

        BOOST_CHECK(
                std::all_of(files.begin(), files.end(), [=](auto i) {
                        return std::all_of(files.begin(), files.end(), [=](auto j) {
                                return i == j ? true : is_disjoint(mask_type::file(white_c, i), mask_type::file(white_c, j));
                        });
                })
        );

        BOOST_CHECK(
                std::accumulate(files.begin(), files.end(), set_t<mask_type>{}, [](auto result, auto i) {
                        return result ^ mask_type::file(white_c, i);
                }) == mask_type::squares
        );
}

BOOST_AUTO_TEST_CASE_TEMPLATE(RowsEquivalencePartitionSquares, T, BoardSequence)
{
        auto const rows = boost::irange(0, T::height);
        using mask_type = basic_mask<T>;

        BOOST_CHECK(
                std::all_of(rows.begin(), rows.end(), [=](auto i) {
                        return mask_type::rank(black_c, i) == mask_type::rank(white_c, T::height - 1 - i);
                })
        );

        BOOST_CHECK(
                std::all_of(rows.begin(), rows.end(), [=](auto i) {
                        return std::all_of(rows.begin(), rows.end(), [=](auto j) {
                                return i == j ? true : is_disjoint(mask_type::rank(white_c, i), mask_type::rank(white_c, j));
                        });
                })
        );

        BOOST_CHECK(
                std::accumulate(rows.begin(), rows.end(), set_t<mask_type>{}, [](auto result, auto i) {
                        return result ^ mask_type::rank(white_c, i);
                }) == mask_type::squares
        );
}

BOOST_AUTO_TEST_SUITE_END()