/*    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.
 *
 *    References
 *      Wakker, K. F. (2007), Lecture Notes astro II (Chapter 18), TU Delft course AE4-874,
 *          Delft University of technology, Delft, The Netherlands.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <Eigen/Dense>
#include <cmath>
#include <iostream>

#include "tudat/astro/basic_astro/physicalConstants.h"
#include "tudat/math/basic/mathematicalConstants.h"
#include "tudat/math/basic/basicMathematicsFunctions.h"
#include "tudat/math/basic/coordinateConversions.h"
#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/simulation/simulation.h"
#include "tudat/simulation/propagation_setup/propagationLowThrustProblem.h"
#include "tudat/astro/basic_astro/celestialBodyConstants.h"
#include "tudat/astro/low_thrust/shape_based/baseFunctionsSphericalShaping.h"
#include "tudat/astro/low_thrust/shape_based/compositeFunctionSphericalShaping.h"
#include "tudat/astro/low_thrust/shape_based/sphericalShaping.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat;
using namespace tudat::simulation_setup;
using namespace tudat::ephemerides;
using namespace tudat::shape_based_methods;
using namespace tudat::root_finders;
using namespace tudat::propagators;

//! Test spherical shaping implementation.
BOOST_AUTO_TEST_SUITE( test_spherical_shaping )

//! Test.
BOOST_AUTO_TEST_CASE( test_spherical_shaping_earth_mars_transfer )
{
    spice_interface::loadStandardSpiceKernels( );

    int numberOfRevolutions = 1;
    double julianDate = 8174.5 * physical_constants::JULIAN_DAY;
    double timeOfFlight = 580.0 * physical_constants::JULIAN_DAY;

    // Ephemeris departure body.
    EphemerisPointer pointerToDepartureBodyEphemeris = std::make_shared< ApproximateJplEphemeris>(
                "Earth"  );
    EphemerisPointer pointerToArrivalBodyEphemeris = std::make_shared< ApproximateJplEphemeris >(
                "Mars"  );
    Eigen::Vector6d initialState = pointerToDepartureBodyEphemeris->getCartesianState( julianDate );
    Eigen::Vector6d finalState = pointerToArrivalBodyEphemeris->getCartesianState(
                julianDate + timeOfFlight );

    // Define root finder settings (used to update the updated value of the free coefficient, so that it
    // matches the required time of flight).
    std::shared_ptr< RootFinderSettings > rootFinderSettings =
            tudat::root_finders::bisectionRootFinderSettings( 1.0E-6, TUDAT_NAN, TUDAT_NAN, 30 );

    // Compute shaped trajectory.
    SphericalShaping sphericalShaping = SphericalShaping(
                initialState, finalState, timeOfFlight,
                spice_interface::getBodyGravitationalParameter( "Sun" ),
                numberOfRevolutions, 0.000703, rootFinderSettings, 1.0e-6, 1.0e-1 );

    // Initialise peak acceleration.
    double peakThrustAcceleration = 0.0;

    double stepSize = ( sphericalShaping.getFinalValueInpendentVariable( ) -
                        sphericalShaping.getInitialValueInpendentVariable( ) ) / 5000.0;
    for ( int i = 0 ; i <= 5000 ; i++ )
    {
        double currentThetaAngle = sphericalShaping.getInitialValueInpendentVariable() + i * stepSize;
        if ( sphericalShaping.computeCurrentThrustAcceleration( currentThetaAngle ).norm() >  peakThrustAcceleration )
        {
            peakThrustAcceleration = sphericalShaping.computeCurrentThrustAcceleration( currentThetaAngle ).norm();
        }
    }

    // Check results consistency w.r.t. thesis from T. Roegiers (ADD PROPER REFERENCE)
    double expectedDeltaV = 5700.0;
    double expectedPeakAcceleration = 2.4e-4;

    // DeltaV provided with a precision of 5 m/s
    BOOST_CHECK_SMALL( std::fabs(  sphericalShaping.computeDeltaV() - expectedDeltaV ), 5.0 );
    // Peak acceleration provided with a precision 2.0e-6 m/s^2
    BOOST_CHECK_SMALL( std::fabs(  peakThrustAcceleration - expectedPeakAcceleration ), 2.0e-6 );

}

//! Test.
BOOST_AUTO_TEST_CASE( test_spherical_shaping_earth_1989ML_transfer )
{
    spice_interface::loadStandardSpiceKernels( );

    int numberOfRevolutions = 1;
    double julianDate = 7799.5 * physical_constants::JULIAN_DAY;
    double timeOfFlight = 600.0 * physical_constants::JULIAN_DAY;

    // Ephemeris departure body.
    EphemerisPointer pointerToDepartureBodyEphemeris = std::make_shared< ApproximateJplEphemeris>(
                "Earth"  );
    Eigen::Vector6d initialState = pointerToDepartureBodyEphemeris->getCartesianState( julianDate );

    // Final state derived from ML1989 ephemeris (from Spice).
    Eigen::Vector6d finalState = (
                Eigen::Vector6d() <<
                1.197701029846094E+00 * physical_constants::ASTRONOMICAL_UNIT,
                1.653518856610793E-01 * physical_constants::ASTRONOMICAL_UNIT,
                - 9.230177854743750E-02 * physical_constants::ASTRONOMICAL_UNIT,
                - 4.891080912584867E-05 * physical_constants::ASTRONOMICAL_UNIT / physical_constants::JULIAN_DAY,
                1.588950249593135E-02 * physical_constants::ASTRONOMICAL_UNIT / physical_constants::JULIAN_DAY,
                - 2.980245580772588E-04 * physical_constants::ASTRONOMICAL_UNIT / physical_constants::JULIAN_DAY ).finished();

    // Define root finder settings (used to update the updated value of the free coefficient, so that it matches the required time of flight).
    std::shared_ptr< RootFinderSettings > rootFinderSettings =
            tudat::root_finders::bisectionRootFinderSettings( 1.0E-6, TUDAT_NAN, TUDAT_NAN, 30 );

    // Compute shaped trajectory.
    SphericalShaping sphericalShaping = SphericalShaping(
                initialState, finalState, timeOfFlight,
                spice_interface::getBodyGravitationalParameter( "Sun" ),
                numberOfRevolutions, -0.0000703, rootFinderSettings, -1.0e-2, 1.0e-2 );

    // Compute step size.
    double numberOfSteps = 5000.0;
    double stepSize = ( sphericalShaping.getFinalValueInpendentVariable( ) -
                        sphericalShaping.getInitialValueInpendentVariable( ) ) / numberOfSteps;

    // Initialise peak acceleration.
    double peakThrustAcceleration = 0.0;

    // Compute peak acceleration.
    for ( int i = 0 ; i <= numberOfSteps ; i++ )
    {
        double currentThetaAngle = sphericalShaping.getInitialValueInpendentVariable() + i * stepSize;

        if ( sphericalShaping.computeCurrentThrustAcceleration( currentThetaAngle ).norm() >  peakThrustAcceleration )
        {
            peakThrustAcceleration = sphericalShaping.computeCurrentThrustAcceleration( currentThetaAngle ).norm();
        }
    }

    // Check results consistency w.r.t. thesis from T. Roegiers (ADD PROPER REFERENCE)
    // The expected differences are a bit larger than for Earth-Mars transfer due to the higher uncertainty in 1989ML's ephemeris.
    double expectedDeltaV = 4530.0;
    double expectedPeakAcceleration = 1.8e-4;

    // DeltaV provided with a precision of 0.1 km/s
    BOOST_CHECK_SMALL( std::fabs(  sphericalShaping.computeDeltaV() - expectedDeltaV ), 100.0 );
    // Peak acceleration provided with a precision 1.0e-5 m/s^2
    BOOST_CHECK_SMALL( std::fabs(  peakThrustAcceleration - expectedPeakAcceleration ), 1e-5 );

}

//! Test spherical shaping method with various number of revolutions.
BOOST_AUTO_TEST_CASE( test_spherical_shaping_earth_mars_transfer_multi_revolutions )
{
    spice_interface::loadStandardSpiceKernels( );

    double julianDate = 8174.5 * physical_constants::JULIAN_DAY;

    std::vector< int > numberOfRevolutionsVector = { 0, 1, 2 };
    std::vector< double > timeOfFlightVector = { 300.0, 580.0, 750.0 };

    // Ephemeris departure body.
    EphemerisPointer pointerToDepartureBodyEphemeris = std::make_shared< ApproximateJplEphemeris>(
                "Earth"  );
    EphemerisPointer pointerToArrivalBodyEphemeris = std::make_shared< ApproximateJplEphemeris >(
                "Mars"  );

    // Define root finder settings (used to update the updated value of the free coefficient, so that it matches the required time of flight).
    std::shared_ptr< RootFinderSettings > rootFinderSettings =
            tudat::root_finders::bisectionRootFinderSettings( 1.0E-6, TUDAT_NAN, TUDAT_NAN, 30 );

    // Bounds for the free parameter.
    std::vector< double > freeParameterLowerBoundVector = { -1.0, 1.0e-6, -1.0e-2 };
    std::vector< double > freeParameterUpperBoundVector = { 5.0e-1, 1.0e-1, 1.0e-2 };

    // Define initial state.
    Eigen::Vector6d initialState = pointerToDepartureBodyEphemeris->getCartesianState( julianDate );

    for ( unsigned int currentTestCase = 0 ; currentTestCase < numberOfRevolutionsVector.size( ) ; currentTestCase++ )
    {
        // Define final state.
        Eigen::Vector6d finalState = pointerToArrivalBodyEphemeris->getCartesianState(
                    julianDate + timeOfFlightVector.at( currentTestCase )* physical_constants::JULIAN_DAY );

        // Compute shaped trajectory.
        SphericalShaping sphericalShaping = SphericalShaping(
                    initialState, finalState, timeOfFlightVector.at( currentTestCase )* physical_constants::JULIAN_DAY,
                    spice_interface::getBodyGravitationalParameter( "Sun" ),
                    numberOfRevolutionsVector.at( currentTestCase ),
                    0.000703, rootFinderSettings, freeParameterLowerBoundVector.at( currentTestCase ),
                    freeParameterUpperBoundVector.at( currentTestCase ) );

        // Check consistency of final azimuth angle value with required number of revolutions.
        double initialAzimuthAngle = sphericalShaping.getInitialValueInpendentVariable( );
        double finalAzimuthAngle = sphericalShaping.getFinalValueInpendentVariable( );

        double expectedInitialAzimuthAngle = coordinate_conversions::convertCartesianToSphericalState( initialState )[ 1 ];
        if ( expectedInitialAzimuthAngle < 0.0 )
        {
            expectedInitialAzimuthAngle += 2.0 * mathematical_constants::PI;
        }
        double expectedFinalAzimuthAngle = coordinate_conversions::convertCartesianToSphericalState( finalState )[ 1 ];
        if ( expectedFinalAzimuthAngle < 0.0 )
        {
            expectedFinalAzimuthAngle += 2.0 * mathematical_constants::PI;
        }

        if ( expectedFinalAzimuthAngle - expectedInitialAzimuthAngle < 0 )
        {
            expectedFinalAzimuthAngle += 2.0 * mathematical_constants::PI * ( numberOfRevolutionsVector.at( currentTestCase )+ 1 );
        }
        else
        {
            expectedFinalAzimuthAngle += 2.0 * mathematical_constants::PI * numberOfRevolutionsVector.at( currentTestCase );
        }

        BOOST_CHECK_SMALL( std::fabs(  initialAzimuthAngle - expectedInitialAzimuthAngle ), 1.0e-15 );
        BOOST_CHECK_SMALL( std::fabs(  finalAzimuthAngle - expectedFinalAzimuthAngle ), 1.0e-15 );

        // Check consistency of expected and calculated states (both at departure and arrival).
        for ( int i = 0 ; i < 6 ; i++ )
        {
            BOOST_CHECK_SMALL( std::fabs( ( initialState[ i ] - sphericalShaping.computeCurrentStateVector( initialAzimuthAngle )[ i ] )
                                          / initialState[ i ] ), 1.0e-12 );
            BOOST_CHECK_SMALL( std::fabs( ( finalState[ i ] - sphericalShaping.computeCurrentStateVector( finalAzimuthAngle )[ i ] )
                                          / finalState[ i ] ), 1.0e-12 );
        }
    }
}

SystemOfBodies getTestBodyMap( )
{
    // Create central, departure and arrival bodies.
    std::vector< std::string > bodiesToCreate;
    bodiesToCreate.push_back( "Sun" );
    bodiesToCreate.push_back( "Earth" );
    bodiesToCreate.push_back( "Mars" );
    bodiesToCreate.push_back( "Jupiter" );

    std::string frameOrigin = "SSB";
    std::string frameOrientation = "ECLIPJ2000";

    BodyListSettings bodySettings =
            getDefaultBodySettings( bodiesToCreate, frameOrigin, frameOrientation );

    // Define central body ephemeris settings.
    bodySettings.at( "Sun" )->ephemerisSettings = std::make_shared< ConstantEphemerisSettings >(
                ( Eigen::Vector6d( ) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ).finished( ), frameOrigin, frameOrientation );

    // Create system of bodies.
    SystemOfBodies bodies = createSystemOfBodies( bodySettings );

    bodies.createEmptyBody( "Vehicle" );

    return bodies;

}

BOOST_AUTO_TEST_CASE( test_spherical_shaping_full_propagation )
{

    int numberOfRevolutions = 1;
    double julianDate = 8174.5 * physical_constants::JULIAN_DAY;
    double  timeOfFlight = 580.0;

    // Ephemeris for arrival and departure body.
    EphemerisPointer pointerToDepartureBodyEphemeris = std::make_shared< ApproximateJplEphemeris>(
                "Earth"  );
    EphemerisPointer pointerToArrivalBodyEphemeris = std::make_shared< ApproximateJplEphemeris >(
                "Mars"  );
    Eigen::Vector6d stateAtDeparture = pointerToDepartureBodyEphemeris->getCartesianState( julianDate );
    Eigen::Vector6d stateAtArrival = pointerToArrivalBodyEphemeris->getCartesianState(
                julianDate + timeOfFlight * physical_constants::JULIAN_DAY );

    // Define root finder settings (used to update the updated value of the free coefficient, so that it matches the required time of flight).
    std::shared_ptr< RootFinderSettings > rootFinderSettings =
            tudat::root_finders::bisectionRootFinderSettings( 1.0E-6, TUDAT_NAN, TUDAT_NAN, 30 );

    // Compute shaped trajectory.
    std::shared_ptr< SphericalShaping > sphericalShaping = std::make_shared< SphericalShaping >(
                stateAtDeparture, stateAtArrival, timeOfFlight * physical_constants::JULIAN_DAY,
                spice_interface::getBodyGravitationalParameter( "Sun" ),
                numberOfRevolutions, 0.000703,
                rootFinderSettings, 1.0e-6, 1.0e-1 );

    std::map< double, Eigen::VectorXd > fullPropagationResults;
    std::map< double, Eigen::Vector6d > shapingMethodResults;
    std::map< double, Eigen::VectorXd > dependentVariablesHistory;

    // Create system of bodies
    SystemOfBodies bodies = getTestBodyMap( );
    bodies.at( "Vehicle" )->setBodyMassFunction(  [ = ]( const double currentTime ){ return 2000.0; } );

    // Define integrator settings
    std::shared_ptr< numerical_integrators::IntegratorSettings< double > > integratorSettings =
            std::make_shared< numerical_integrators::IntegratorSettings< double > > (
                numerical_integrators::rungeKutta4, 0.0, timeOfFlight * physical_constants::JULIAN_DAY / ( 1000.0 ) );

    // Define mass and specific impulse functions of the vehicle.
    std::function< double( const double ) > specificImpulseFunction =
            [ ]( const double ){ return 3000.0; };

    // Create object with list of dependent variables
    std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesList;
    dependentVariablesList.push_back( std::make_shared< SingleAccelerationDependentVariableSaveSettings >(
                                          basic_astrodynamics::thrust_acceleration, "Vehicle", "Vehicle", 0 ) );
    std::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave =
            std::make_shared< DependentVariableSaveSettings >( dependentVariablesList, false );

    // Create complete propagation settings (backward and forward propagations).
    basic_astrodynamics::AccelerationMap lowThrustAccelerationsMap =
            retrieveLowThrustAccelerationMap(
                sphericalShaping, bodies, "Vehicle", "Sun", specificImpulseFunction, 0.0 );
    std::pair< std::shared_ptr< PropagatorSettings< double > >,
            std::shared_ptr< PropagatorSettings< double > > > propagatorSettings =
            createLowThrustTranslationalStatePropagatorSettings(
                sphericalShaping, "Vehicle", "Sun", lowThrustAccelerationsMap, dependentVariablesToSave );

    // Compute shaped trajectory and propagated trajectory.
    computeLowThrustLegSemiAnalyticalAndFullPropagation(
                sphericalShaping, bodies, integratorSettings, propagatorSettings,
                fullPropagationResults, shapingMethodResults, dependentVariablesHistory );

    // Check difference between full propagation and shaping method at arrival
    // (disregarding the very last values because of expected interpolation errors).
    int numberOfDisregardedValues = 7;
    std::map< double, Eigen::VectorXd >::iterator itr = fullPropagationResults.end();
    for( int i = 0 ; i < numberOfDisregardedValues ; i++ )
    {
        itr--;
    }

    // Check results consistency between full propagation and shaped trajectory at arrival.
    for ( int i = 0 ; i < 6 ; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( shapingMethodResults[ itr->first ][ i ] - itr->second[ i ] ) /
                shapingMethodResults[ itr->first ][ i ] , 1.0e-6 );
    }

    // Check difference between full propagation and shaping method at departure
    // (disregarding the very first values because of expected interpolation errors).
    itr = fullPropagationResults.begin();
    for( int i = 0 ; i < numberOfDisregardedValues ; i++ )
    {
        itr++;
    }

    // Check results consistency between full propagation and shaped trajectory at departure.
    for ( int i = 0 ; i < 6 ; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( shapingMethodResults[ itr->first ][ i ] - itr->second[ i ] ) /
                shapingMethodResults[ itr->first ][ i ] , 1.0e-6 );
    }
}

//BOOST_AUTO_TEST_CASE( test_spherical_shaping_full_propagation_mass_propagation )
//{

//    spice_interface::loadStandardSpiceKernels( );

//    int numberOfRevolutions = 1;
//    double julianDate = 8174.5 * physical_constants::JULIAN_DAY;
//    double  timeOfFlight = 580.0;
//    double initialMass = 2000.0;
//    std::function< double( const double ) > specificImpulseFunction = [ = ]( const double ) { return 3000.0; };

//    // Ephemeris for arrival and departure body.
//    EphemerisPointer pointerToDepartureBodyEphemeris = std::make_shared< ApproximateJplEphemeris>(
//                "Earth"  );
//    EphemerisPointer pointerToArrivalBodyEphemeris = std::make_shared< ApproximateJplEphemeris >(
//                "Mars"  );
//    Eigen::Vector6d stateAtDeparture = pointerToDepartureBodyEphemeris->getCartesianState( julianDate );
//    Eigen::Vector6d stateAtArrival = pointerToArrivalBodyEphemeris->getCartesianState( julianDate + timeOfFlight * physical_constants::JULIAN_DAY );

//    // Define root finder settings (used to update the updated value of the free coefficient, so that it matches the required time of flight).
//    std::shared_ptr< RootFinderSettings > rootFinderSettings =
//            std::make_shared< RootFinderSettings >( bisection_root_finder, 1.0e-6, 30 );

//    // Compute shaped trajectory.
//    SphericalShaping sphericalShaping = SphericalShaping(
//                stateAtDeparture, stateAtArrival, timeOfFlight * physical_constants::JULIAN_DAY,
//                spice_interface::getBodyGravitationalParameter( "Sun" ),
//                numberOfRevolutions, 0.000703,
//                rootFinderSettings, 1.0e-6, 1.0e-1, initialMass );

//    std::map< double, Eigen::VectorXd > fullPropagationResults;
//    std::map< double, Eigen::Vector6d > shapingMethodResults;
//    std::map< double, Eigen::VectorXd > dependentVariablesHistory;

//    // Create system of bodies
//    SystemOfBodies bodies = getTestBodyMap( );
//    bodies.at( "Vehicle" )->setConstantBodyMass( initialMass );

//    // Define integrator settings
//    std::shared_ptr< numerical_integrators::IntegratorSettings< double > > integratorSettings =
//            std::make_shared< numerical_integrators::IntegratorSettings< double > > (
//                numerical_integrators::rungeKutta4, 0.0,
//                timeOfFlight * physical_constants::JULIAN_DAY / ( 1000.0 ) );

//    // Define list of dependent variables to save.
//    std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesList;
//    dependentVariablesList.push_back( std::make_shared< SingleAccelerationDependentVariableSaveSettings >(
//                                          basic_astrodynamics::thrust_acceleration, "Vehicle", "Vehicle", 0 ) );
//    dependentVariablesList.push_back( std::make_shared< SingleDependentVariableSaveSettings >(
//                                          total_mass_rate_dependent_variables, "Vehicle" ) );

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

//    // Create termination conditions settings.
//    std::pair< std::shared_ptr< PropagationTerminationSettings >, std::shared_ptr< PropagationTerminationSettings > > terminationConditions =
//            std::make_pair( std::make_shared< PropagationTimeTerminationSettings >( 0.0 ),
//                            std::make_shared< PropagationTimeTerminationSettings >( timeOfFlight * physical_constants::JULIAN_DAY ) );

//    // Create complete propagation settings (backward and forward propagations).
//    std::pair< std::shared_ptr< PropagatorSettings< double > >,
//            std::shared_ptr< PropagatorSettings< double > > > propagatorSettings = sphericalShaping.createLowThrustPropagatorSettings(
//                bodies, "Vehicle", "Sun", specificImpulseFunction, basic_astrodynamics::AccelerationMap( ), integratorSettings, dependentVariablesToSave );

//    // Compute shaped trajectory and propagated trajectory.
//    sphericalShaping.computeSemiAnalyticalAndFullPropagation(
//                bodies, integratorSettings, propagatorSettings,
//                fullPropagationResults, shapingMethodResults, dependentVariablesHistory );

//    // Check difference between full propagation and shaping method at arrival
//    // (disregarding the very last values because of expected interpolation errors).
//    int numberOfDisregardedValues = 7;
//    std::map< double, Eigen::VectorXd >::iterator itr = fullPropagationResults.end();
//    for( int i = 0 ; i < numberOfDisregardedValues ; i++ )
//    {
//        itr--;
//    }

//    // Check results consistency between full propagation and shaped trajectory at arrival.
//    for ( int i = 0 ; i < 6 ; i++ )
//    {
////        BOOST_CHECK_SMALL( std::fabs( shapingMethodResults[ itr->first ][ i ] - itr->second[ i ] ) / shapingMethodResults[ itr->first ][ i ] , 1.0e-6 );
//    }

//    // Check difference between full propagation and shaping method at departure
//    // (disregarding the very first values because of expected interpolation errors).
//    itr = fullPropagationResults.begin();
//    for( int i = 0 ; i < numberOfDisregardedValues ; i++ )
//    {
//        itr++;
//    }

//    // Check results consistency between full propagation and shaped trajectory at departure.
//    for ( int i = 0 ; i < 6 ; i++ )
//    {
//        BOOST_CHECK_SMALL( std::fabs( shapingMethodResults[ itr->first ][ i ] - itr->second[ i ] ) / shapingMethodResults[ itr->first ][ i ] , 1.0e-6 );
//    }

//    // Check consistency between current and expected mass rates.
//    for ( std::map< double, Eigen::VectorXd >::iterator itr = dependentVariablesHistory.begin() ; itr != dependentVariablesHistory.end() ; itr++ )
//    {
//        Eigen::Vector3d currentThrustAccelerationVector = itr->second.segment( 0, 3 );
//        double currentMass = fullPropagationResults.at( itr->first )( 6 );
//        double currentMassRate = - itr->second( 3 );
//        double expectedMassRate = currentThrustAccelerationVector.norm() * currentMass /
//                ( specificImpulseFunction( itr->first ) * physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION );
//        BOOST_CHECK_SMALL( std::fabs( currentMassRate - expectedMassRate ), 1.0e-15 );

//    }

//    // Test trajectory function.
//    std::vector< double > epochsVector;
//    epochsVector.push_back( 0.0 );
//    epochsVector.push_back( timeOfFlight / 4.0 * physical_constants::JULIAN_DAY );
//    epochsVector.push_back( timeOfFlight / 2.0 * physical_constants::JULIAN_DAY );
//    epochsVector.push_back( 3.0 * timeOfFlight / 4.0 * physical_constants::JULIAN_DAY );
//    epochsVector.push_back( timeOfFlight * physical_constants::JULIAN_DAY );

//    std::map< double, Eigen::Vector6d > trajectory;
//    std::map< double, Eigen::VectorXd > massProfile;
//    std::map< double, Eigen::VectorXd > thrustProfile;
//    std::map< double, Eigen::VectorXd > thrustAccelerationProfile;

//    sphericalShaping.getTrajectory( epochsVector, trajectory );
//    sphericalShaping.getMassProfile( epochsVector, massProfile, specificImpulseFunction, integratorSettings );
////    sphericalShaping.getThrustForceProfile( epochsVector, thrustProfile, specificImpulseFunction, integratorSettings );
////    sphericalShaping.getThrustAccelerationProfile( epochsVector, thrustAccelerationProfile, specificImpulseFunction, integratorSettings );

//    for ( int i = 0 ; i < 3 ; i ++ )
//    {
//        BOOST_CHECK_SMALL( std::fabs( ( trajectory.begin( )->second[ i ] - stateAtDeparture[ i ] ) /
//                                      physical_constants::ASTRONOMICAL_UNIT ), 1.0e-6 );
//        BOOST_CHECK_SMALL( std::fabs( ( trajectory.begin( )->second[ i + 3 ] - stateAtDeparture[ i + 3 ] ) /
//                ( physical_constants::ASTRONOMICAL_UNIT / physical_constants::JULIAN_YEAR ) ), 1.0e-6 );
//        BOOST_CHECK_SMALL( std::fabs( ( trajectory.rbegin( )->second[ i ] - stateAtArrival[ i ] ) /
//                                      physical_constants::ASTRONOMICAL_UNIT ), 1.0e-6 );
//        BOOST_CHECK_SMALL( std::fabs( ( trajectory.rbegin( )->second[ i + 3 ] - stateAtArrival[ i + 3 ] ) /
//                ( physical_constants::ASTRONOMICAL_UNIT / physical_constants::JULIAN_YEAR ) ), 1.0e-6 );
//    }

//    for ( std::map< double, Eigen::Vector6d >::iterator itr = trajectory.begin( ) ; itr != trajectory.end( ) ; itr++ )
//    {
//        double independentVariable = sphericalShaping.convertTimeToIndependentVariable( itr->first );
//        Eigen::Vector6d stateVector = sphericalShaping.computeCurrentStateVector( independentVariable );
//        Eigen::Vector3d thrustAccelerationVector = sphericalShaping.computeCurrentThrustAcceleration( itr->first, specificImpulseFunction, integratorSettings );
//        Eigen::Vector3d thrustVector = sphericalShaping.computeCurrentThrustForce( itr->first, specificImpulseFunction, integratorSettings );
//        double mass = sphericalShaping.computeCurrentMass( itr->first, specificImpulseFunction, integratorSettings );

//        for ( int i = 0 ; i < 3 ; i++ )
//        {
//            BOOST_CHECK_SMALL( std::fabs( itr->second[ i ] - stateVector[ i ] ), 1.0e-6 );
//            BOOST_CHECK_SMALL( std::fabs( itr->second[ i + 3 ] - stateVector[ i + 3 ] ), 1.0e-12 );
//            BOOST_CHECK_SMALL( std::fabs( thrustAccelerationProfile[ itr->first ][ i ] - thrustAccelerationVector[ i ] ), 1.0e-6 );
//            BOOST_CHECK_SMALL( std::fabs( thrustProfile[ itr->first ][ i ] - thrustVector[ i ] ), 1.0e-12 );
//        }
//        BOOST_CHECK_SMALL( std::fabs( massProfile[ itr->first ][ 0 ] - mass ), 1.0e-10 );
//    }

//}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 * Created on June 4, 2020, 10:13 AM
 */

#include "PermeabilityMohrCoulombFailureIndexModel.h"

#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <limits>

#include "BaseLib/Error.h"
#include "MaterialLib/MPL/Medium.h"
#include "MaterialLib/MPL/Utils/FormEigenTensor.h"
#include "MaterialLib/MPL/Utils/GetSymmetricTensor.h"
#include "MathLib/KelvinVector.h"
#include "MathLib/MathTools.h"
#include "ParameterLib/CoordinateSystem.h"
#include "ParameterLib/Parameter.h"

namespace MaterialPropertyLib
{
template <int DisplacementDim>
PermeabilityMohrCoulombFailureIndexModel<DisplacementDim>::
    PermeabilityMohrCoulombFailureIndexModel(
        std::string name, ParameterLib::Parameter<double> const& k0,
        double const kr, double const b, double const c, double const phi,
        double const k_max, double const t_sigma_max,
        ParameterLib::CoordinateSystem const* const local_coordinate_system)
    : k0_(k0),
      kr_(kr),
      b_(b),
      c_(c),
      phi_(boost::math::constants::degree<double>() * phi),
      k_max_(k_max),
      t_sigma_max_(t_sigma_max),
      local_coordinate_system_(local_coordinate_system)
{
    const double t_sigma_upper = c_ / std::tan(phi_);
    if (t_sigma_max_ <= 0.0 || t_sigma_max_ > t_sigma_upper ||
        std::fabs(t_sigma_max_ - t_sigma_upper) <
            std::numeric_limits<double>::epsilon())
    {
        OGS_FATAL(
            "Tensile strength parameter of {:e} is out of the range (0, "
            "c/tan(phi)) = (0, {:e})",
            t_sigma_max_, t_sigma_upper);
    }

    name_ = std::move(name);
}

template <int DisplacementDim>
void PermeabilityMohrCoulombFailureIndexModel<DisplacementDim>::checkScale()
    const
{
    if (!std::holds_alternative<Medium*>(scale_))
    {
        OGS_FATAL(
            "The property 'PermeabilityMohrCoulombFailureIndexModel' is "
            "implemented on the 'medium' scale only.");
    }
}

template <int DisplacementDim>
PropertyDataType
PermeabilityMohrCoulombFailureIndexModel<DisplacementDim>::value(
    VariableArray const& variable_array,
    ParameterLib::SpatialPosition const& pos, double const t,
    double const /*dt*/) const
{
    auto const& stress_vector = std::get<SymmetricTensor<DisplacementDim>>(
        variable_array[static_cast<int>(Variable::total_stress)]);

    auto const& stress_tensor =
        formEigenTensor<3>(static_cast<PropertyDataType>(stress_vector));

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>>
        eigenvalue_solver(stress_tensor);

    // Principle stress
    auto const sigma = eigenvalue_solver.eigenvalues();

    auto k_data = k0_(t, pos);

    double const max_sigma = std::max(std::fabs(sigma[0]), std::fabs(sigma[2]));

    if (max_sigma < std::numeric_limits<double>::epsilon())
    {
        return fromVector(k_data);
    }

    double const sigma_m = 0.5 * (sigma[2] + sigma[0]);

    double const tau_m = 0.5 * std::fabs(sigma[2] - sigma[0]);
    double f = 0.0;
    if (sigma_m > t_sigma_max_)
    {
        // tensile failure criterion
        f = sigma_m / t_sigma_max_;

        double const tau_tt =
            c_ * std::cos(phi_) - t_sigma_max_ * std::sin(phi_);

        f = std::max(f, tau_m / tau_tt);
    }
    else
    {
        // Mohr Coulomb failure criterion
        f = tau_m / (c_ * std::cos(phi_) - sigma_m * std::sin(phi_));
    }

    if (f >= 1.0)
    {
        const double exp_value = std::exp(b_ * f);
        for (auto& k_i : k_data)
        {
            k_i = std::min(k_i + kr_ * exp_value, k_max_);
        }
    }

    // Local coordinate transformation is only applied for the case that the
    // initial intrinsic permeability is given with orthotropic assumption.
    if (local_coordinate_system_ && (k_data.size() == DisplacementDim))
    {
        Eigen::Matrix<double, DisplacementDim, DisplacementDim> const e =
            local_coordinate_system_->transformation<DisplacementDim>(pos);
        Eigen::Matrix<double, DisplacementDim, DisplacementDim> k =
            Eigen::Matrix<double, DisplacementDim, DisplacementDim>::Zero();

        for (int i = 0; i < DisplacementDim; ++i)
        {
            Eigen::Matrix<double, DisplacementDim, DisplacementDim> const
                ei_otimes_ei = e.col(i) * e.col(i).transpose();

            k += k_data[i] * ei_otimes_ei;
        }
        return k;
    }

    return fromVector(k_data);
}

template <int DisplacementDim>
PropertyDataType
PermeabilityMohrCoulombFailureIndexModel<DisplacementDim>::dValue(
    VariableArray const& /*variable_array*/, Variable const variable,
    ParameterLib::SpatialPosition const& /*pos*/, double const /*t*/,
    double const /*dt*/) const
{
    if (variable == Variable::mechanical_strain)
    {
        return 0.;
    }

    OGS_FATAL(
        "The derivative of the intrinsic permeability k(sigma, ...) with "
        "respect to stress tensor (sigma) is not implemented because that "
        "dk/du is normally omitted.");
}
template class PermeabilityMohrCoulombFailureIndexModel<2>;
template class PermeabilityMohrCoulombFailureIndexModel<3>;
}  // namespace MaterialPropertyLib


// Author: Senthil Kumar Thangavelu kingjuliyen@gmail.com

/*
  Compile:
  g++  HiddenFactorsLearner.cpp \
       -I/opt/boost/1_61_0/include/						\
       -L/opt/boost/1_61_0/lib						\
       -lboost_program_options \
       -O3 -o /tmp/hidden-factor-learner

  Usage:
  export DYLD_LIBRARY_PATH=/opt/boost/1_61_0/lib:$DYLD_LIBRARY_PATH

  DYLD_LIBRARY_PATH=/opt/boost/1_61_0/lib:$DYLD_LIBRARY_PATH \
  time /tmp/hidden-factor-learner --num-factors 5 \
          --default-rating 2.8485 \
          --regularization-param-p 0.002 \
          --regularization-param-q 0.002 \
          --learning-rate-p 0.000002 \
          --learning-rate-q 0.000002 \
          --training-sample-percentage 0.7 \
          --gradient-descent-iteration-count 100 \
          --max-row-dimension 100000000 \
          --max-column-dimension 100000000 \
          --verbose-mode-level 1 \
          --loop-mode-count 3 \
          --input-csv-file-path /tmp/1000.csv \
          --p-q-matrix-output-file-path /tmp/

e.g csv file (remove header)
usrid  itemid rating
785166 6066 3
521295 4972 3
1665652 11701 5
1963419 15887 3

  Linux:

  LD_LIBRARY_PATH=/opt/boost/1_61_0/lib:$LD_LIBRARY_PATH \
  time /tmp/hidden-factor-learner --num-factors 2 \
          --default-rating 2.8485 \
          --regularization-param-p 0.002 \
          --regularization-param-q 0.002 \
          --learning-rate-p 0.000002 \
          --learning-rate-q 0.000002 \
          --training-sample-percentage 0.7 \
          --gradient-descent-iteration-count 100 \
          --max-row-dimension 100000000 \
          --max-column-dimension 100000000 \
          --verbose-mode-level 1 \
          --loop-mode-count 3 \
          --input-csv-file-path /tmp/sf41.csv \
          --p-q-matrix-output-file-path /tmp/

  Install boost:
  ./bootstrap.sh --prefix=/opt/boost/1_61_0
  ./b2 install
*/

#include <iostream>
#include <string>
#include <boost/program_options.hpp>

#include "HiddenFactorsLearner.hpp"

namespace ProgOpts = boost::program_options;

const char * num_factors_str =
  "Number of hidden factors aka K ";
const char * default_rating_str =
  "Default rating used in the matrix factorization"
  " at start of gradient descent ";
const char * regularization_param_p_str =
  "Regularization parameter for matrix P ";
const char * regularization_param_q_str =
  "Regularization parameter for matrix Q ";
const char * learning_rate_p_str =
  "Learning rate for matrix P parameters in gradient descent ";
const char * learning_rate_q_str =
  "Learning rate for matrix Q parameters in gradient descent ";
const char * gradient_descent_iteration_count_str =
  "Number of times gradient descent iterations need to be run ";
const char * training_sample_percentage_str =
  "Percentage of data set to be used for training ";
  //"remaining percentage will be used for validation ";
const char * max_row_dim_str= "Max dimension for rows in matrix";
const char * max_col_dim_str = "Max dimension for columns in matrix";
const char * input_csv_str = "Path of input csv file to read rating "
  "entries from ";
const char * verbose_mode_level_str = "Show debug info about inner workings ";
const char * loop_mode_count_str = "Run repeatedly in loop mode for same input ";
const char * P_Q_matrix_output_file_path_str = "path to store P and Q matrix output";

bool processInputArgs(int argc, char * argv[], ProgOpts::variables_map &varMap,
        MatrixFactorizationParams &params)
{
  try {
    ProgOpts::options_description desc("Allowed Options");

    desc.add_options()
      ("help", "produce help message")
      ("num-factors", ProgOpts::value<INT_T>(), num_factors_str)
      ("default-rating", ProgOpts::value<FLT_T>(), default_rating_str)
      ("regularization-param-p", ProgOpts::value<FLT_T>(), regularization_param_p_str)
      ("regularization-param-q", ProgOpts::value<FLT_T>(), regularization_param_q_str)
      ("learning-rate-p", ProgOpts::value<FLT_T>(), learning_rate_p_str)
      ("learning-rate-q", ProgOpts::value<FLT_T>(), learning_rate_q_str)
      ("gradient-descent-iteration-count", ProgOpts::value<INT_T>(), gradient_descent_iteration_count_str)
      ("training-sample-percentage", ProgOpts::value<FLT_T>(), training_sample_percentage_str)
      ("max-row-dimension", ProgOpts::value<INT_T>(), max_row_dim_str)
      ("max-column-dimension", ProgOpts::value<INT_T>(), max_col_dim_str)
      ("input-csv-file-path", ProgOpts::value<STRING_T>(), input_csv_str)
      ("verbose-mode-level", ProgOpts::value<INT_T>(), verbose_mode_level_str)
      ("loop-mode-count", ProgOpts::value<INT_T>(), loop_mode_count_str)
      ("p-q-matrix-output-file-path", ProgOpts::value<STRING_T>(), P_Q_matrix_output_file_path_str)
      ; // leave this semi colon at end don't move this

    ProgOpts::store(ProgOpts::parse_command_line(argc, argv, desc), varMap);
    ProgOpts::notify(varMap);

    if (varMap.count("help")) {
      cout << desc << "\n";
      return false;
    }

    OPT(num_factors , "num-factors", INT_T,  5);
    OPT(default_rating ,"default-rating" , FLT_T,  2.8485);
    OPT(regularization_param_p ,"regularization-param-p", FLT_T,  0.002);
    OPT(regularization_param_q ,"regularization-param-q", FLT_T,  0.002);
    OPT(learning_rate_p ,"learning-rate-p", FLT_T,  0.000002);
    OPT(learning_rate_q ,"learning-rate-q", FLT_T,  0.000002);
    OPT(gradient_descent_iteration_count ,"gradient-descent-iteration-count", INT_T,  100);
    OPT(training_sample_percentage ,"training-sample-percentage", FLT_T,  0.70);
    OPT(max_row_dim ,"max-row-dimension", INT_T,  100000000);
    OPT(max_col_dim ,"max-column-dimension", INT_T,  100000000);
    OPT(csv_input_file_path ,"input-csv-file-path", STRING_T,  STRING_T("unknown_bad.csv"));
    OPT(verbose_mode_level ,"verbose-mode-level", INT_T,  0);
    OPT(loop_mode_count ,"loop-mode-count", INT_T,  1);
    OPT(p_q_matrix_output_file_path, "p-q-matrix-output-file-path", STRING_T,  STRING_T("P_Q_matrix.mtx"));

  }
  catch(exception &e)
  {
    cerr << "\n processInputArgs error: " << e.what() << "\n";
    return false;
  }
  return true;
}

void Sleep() {
  cout << "\n\n\n\n\n\nSleeping for 10 seconds \n";
  system("sleep 10");
}

void printHeader() {
  cout << "\n\n\n=================================================\n";
  cout << "=============== START TRAINING ==================\n";
  cout << "=================================================\n\n";
}

int startMatrixFactorization_Trainer(int argc, char * argv[])
{
  ProgOpts::variables_map vmp; // varmap
  MatrixFactorizationParams params;

  bool b = processInputArgs(argc, argv, vmp, params);
  if(!b)
    return 3;

  printHeader();
  FLT_T rmseSum = 0;
  FLT_T iterationCount = 0;

  for(int i=0; i<params.loop_mode_count; i++) {
    MatrixFactorization trainer(params);
    if(!trainer.startTraining()) {
      cout << "Training failed check input files and training params\n";
      return 4;
    }
    cout << "Trial " << i << " finalRMSE " << trainer.getFinalRMSE()
      << " num iterations " << trainer.getNumIterations() << "\n";
    rmseSum += trainer.getFinalRMSE();
    iterationCount += 1;
    trainer.printPredictedValues(10);
  }
  cout << " avg RMSE " << (rmseSum/iterationCount)
    << " found using "<< iterationCount << " trials \n";
  return 0;
}

int main(int argc, char * argv[])
{
  return startMatrixFactorization_Trainer(argc, argv);
}


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/dls_pnp.h"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <cmath>
#include <vector>

#include "theia/util/random.h"
#include "theia/sfm/pose/dls_impl.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Quaterniond;
using Eigen::Vector2d;
using Eigen::Vector3d;
using dls_impl::CreateMacaulayMatrix;
using dls_impl::ExtractJacobianCoefficients;
using dls_impl::LeftMultiplyMatrix;

// This implementation is ported from the Matlab version provided by the authors
// of "A Direct Least-Squares (DLS) Method for PnP". The general approach is to
// first rewrite the reprojection constraint (i.e., cost function) such that all
// unknowns appear linearly in terms of the rotation parameters (which are 3
// parameters in the Cayley-Gibss-Rodriguez formulation). Then we create a
// system of equations from the jacobian of the cost function, and solve these
// equations via a Macaulay matrix to obtain the roots (i.e., the 3 parameters
// of rotation). The translation can then be obtained through back-substitution.
void DlsPnp(const std::vector<Vector2d>& feature_position,
            const std::vector<Vector3d>& world_point,
            std::vector<Quaterniond>* solution_rotation,
            std::vector<Vector3d>* solution_translation) {
  CHECK_GE(feature_position.size(), 3);
  CHECK_EQ(feature_position.size(), world_point.size());

  const int num_correspondences = feature_position.size();

  // Holds the normalized feature positions cross multiplied with itself
  // i.e. n * n^t. This value is used multiple times so it is efficient to
  // pre-compute it.
  std::vector<Matrix3d> normalized_feature_cross;
  normalized_feature_cross.reserve(num_correspondences);
  for (int i = 0; i < num_correspondences; i++) {
    const Vector3d normalized_feature_pos =
        feature_position[i].homogeneous().normalized();
    normalized_feature_cross.push_back(normalized_feature_pos *
                                       normalized_feature_pos.transpose());
  }

  // The bottom-right symmetric block matrix of inverse(A^T * A). Matrix H from
  // Eq. 25 in the Appendix of the DLS paper.
  Matrix3d h_inverse = num_correspondences * Matrix3d::Identity();
  for (int i = 0; i < num_correspondences; i++) {
    h_inverse = h_inverse - normalized_feature_cross[i];
  }
  const Matrix3d h_matrix = h_inverse.inverse();

  // Compute V*W*b with the rotation parameters factored out. This is the
  // translation parameterized by the 9 entries of the rotation matrix.
  Matrix<double, 3, 9> translation_factor = Matrix<double, 3, 9>::Zero();
  for (int i = 0; i < num_correspondences; i++) {
    translation_factor = translation_factor +
                         (normalized_feature_cross[i] - Matrix3d::Identity()) *
                             LeftMultiplyMatrix(world_point[i]);
  }

  translation_factor = h_matrix * translation_factor;

  // Compute the cost function J' of Eq. 17 in DLS paper. This is a factorized
  // version where the rotation matrix parameters have been pulled out. The
  // entries to this equation are the coefficients to the cost function which is
  // a quartic in the rotation parameters.
  Matrix<double, 9, 9> ls_cost_coefficients = Matrix<double, 9, 9>::Zero();
  for (int i = 0; i < num_correspondences; i++) {
    ls_cost_coefficients =
        ls_cost_coefficients +
        (LeftMultiplyMatrix(world_point[i]) + translation_factor).transpose() *
            (Matrix3d::Identity() - normalized_feature_cross[i]) *
            (LeftMultiplyMatrix(world_point[i]) + translation_factor);
  }

  // Extract the coefficients of the jacobian (Eq. 18) from the
  // ls_cost_coefficients matrix. The jacobian represent 3 monomials in the
  // rotation parameters. Each entry of the jacobian will be 0 at the roots of
  // the polynomial, so we can arrange a system of polynomials from these
  // equations.
  double f1_coeff[20];
  double f2_coeff[20];
  double f3_coeff[20];
  ExtractJacobianCoefficients(ls_cost_coefficients, f1_coeff, f2_coeff,
                              f3_coeff);

  // We create one equation with random terms that is generally non-zero at the
  // roots of our system.
  const double macaulay_term[4] = { RandDouble(0.0, 100.0),
                                    RandDouble(0.0, 100.0),
                                    RandDouble(0.0, 100.0),
                                    RandDouble(0.0, 100.0) };

  // Create Macaulay matrix that will be used to solve our polynonomial system.
  const MatrixXd& macaulay_matrix =
      CreateMacaulayMatrix(f1_coeff, f2_coeff, f3_coeff, macaulay_term);

  // Via the Schur complement trick, the top-left of the Macaulay matrix
  // contains a multiplication matrix whose eigenvectors correspond to solutions
  // to our system of equations.
  const MatrixXd solution_polynomial =
      macaulay_matrix.block<27, 27>(0, 0) -
      (macaulay_matrix.block<27, 93>(0, 27) *
       macaulay_matrix.block<93, 93>(27, 27).partialPivLu().solve(
           macaulay_matrix.block<93, 27>(27, 0)));

  // Extract eigenvectors of the solution polynomial to obtain the roots which
  // are contained in the entries of the eigenvectors.
  const Eigen::EigenSolver<MatrixXd> eigen_solver(solution_polynomial);

  // Many of the eigenvectors will contain complex solutions so we must filter
  // them to find the real solutions.
  const auto eigen_vectors = eigen_solver.eigenvectors();
  for (int i = 0; i < 27; i++) {
    // The first entry of the eigenvector should equal 1 according to our
    // polynomial, so we must divide each solution by the first entry.
    std::complex<double> s1 = eigen_vectors(9, i) / eigen_vectors(0, i);
    std::complex<double> s2 = eigen_vectors(3, i) / eigen_vectors(0, i);
    std::complex<double> s3 = eigen_vectors(1, i) / eigen_vectors(0, i);

    // If the rotation solutions are real, treat this as a valid candidate
    // rotation.
    const double kEpsilon = 1e-6;
    if (fabs(s1.imag()) < kEpsilon && fabs(s2.imag()) < kEpsilon &&
        fabs(s3.imag()) < kEpsilon) {
      // Compute the rotation (which is the transpose rotation of our solution)
      // and translation.
      Quaterniond soln_rotation(1.0, s1.real(), s2.real(), s3.real());
      soln_rotation = soln_rotation.inverse().normalized();

      const Matrix3d rot_mat = soln_rotation.inverse().toRotationMatrix();
      const Eigen::Map<const Matrix<double, 9, 1> > rot_vec(rot_mat.data());
      const Vector3d soln_translation = translation_factor * rot_vec;

      // TODO(cmsweeney): evaluate cost function and return it as an output
      // variable.

      // Check that all points are in front of the camera. Discard the solution
      // if this is not the case.
      bool all_points_in_front_of_camera = true;
      for (int j = 0; j < num_correspondences; j++) {
        const Vector3d transformed_point =
            soln_rotation * world_point[j] + soln_translation;
        if (transformed_point.z() < 0) {
          all_points_in_front_of_camera = false;
          break;
        }
      }

      if (all_points_in_front_of_camera) {
        solution_rotation->push_back(soln_rotation);
        solution_translation->push_back(soln_translation);
      }
    }
  }
}

}  // namespace theia


#include "cnn/dnn.h"
#include "cnn/macros.h"
#include <string>
#include <cassert>
#include <vector>
#include <iostream>
#include <boost/lexical_cast.hpp>
#include "cnn/nodes.h"
#include "cnn/expr-xtra.h"

using namespace std;
using namespace cnn::expr;
using namespace cnn;

namespace cnn {

    enum { X2H = 0, X2HB };

    void DNNBuilder::display(ComputationGraph& cg) {

        for (unsigned i = 0; i < layers; ++i) {
            const std::vector<Expression>& vars = param_vars[i];
            for (size_t i = 0; i < vars.size(); i++)
                display_value(vars[i], cg);
        }
    }

    DNNBuilder::DNNBuilder(unsigned ilayers,
        const vector<unsigned>& dims,
        Model* model,
        cnn::real iscale,
        string name)
    {
        unsigned input_dim = dims[INPUT_LAYER];
        unsigned hidden_dim = dims[HIDDEN_LAYER];
        unsigned output_dim = dims[OUTPUT_LAYER];

        layers = ilayers;
        unsigned int layer_input_dim = input_dim;
        input_dims = vector<unsigned>(layers, layer_input_dim);

        for (unsigned i = 0; i < layers; ++i) {
            input_dims[i] = layer_input_dim;

            unsigned int odim = (i == layers - 1) ? output_dim : hidden_dim;
            string i_name = "";
            if (name.size() > 0)
                i_name = name + "p_x2h" + boost::lexical_cast<string>(i);
            Parameters* p_x2h = model->add_parameters({ (odim), layer_input_dim }, iscale, i_name);
            if (name.size() > 0)
                i_name = name + "p_x2hb" + boost::lexical_cast<string>(i);
            Parameters* p_x2hb = model->add_parameters({ (odim) }, iscale, i_name);
            vector<Parameters*> ps = { p_x2h, p_x2hb };
            params.push_back(ps);
            layer_input_dim = hidden_dim;
        }
    }

    void DNNBuilder::new_graph_impl(ComputationGraph& cg) {
        param_vars.clear();
        for (unsigned i = 0; i < layers; ++i) {
            Parameters* p_x2h = params[i][X2H];
            Parameters* p_x2hb = params[i][X2HB];
            Expression i_x2h = parameter(cg, p_x2h);
            Expression i_x2hb = parameter(cg, p_x2hb);
            vector<Expression> vars = { i_x2h, i_x2hb };

            param_vars.push_back(vars);
        }
        set_data_in_parallel(1);
    }

    void DNNBuilder::set_data_in_parallel(int n)
    {
        dparallel = n;

        biases.clear();
        for (unsigned i = 0; i < layers; ++i) {
            const vector<Expression>& vars = param_vars[i];
            Expression bimb = concatenate_cols(vector<Expression>(data_in_parallel(), vars[X2HB]));

            vector<Expression> b = { bimb };
            biases.push_back(b);
        }
    }

    Expression DNNBuilder::add_input_impl(const Expression &in) {
        h.resize(layers);

        Expression x = in;

        for (unsigned i = 0; i < layers; ++i) {
            const vector<Expression>& vars = param_vars[i];

            Expression y = affine_transform({ biases[i][0], vars[0], x });

            x = h[i] = tanh(y);
        }
        return h.back();
    }

    void DNNBuilder::copy(const DNNBuilder & rnn) {
        const DNNBuilder& rnn_simple = (const DNNBuilder&)rnn;
        assert(params.size() == rnn_simple.params.size());
        for (size_t i = 0; i < rnn_simple.params.size(); ++i) {
            params[i][0]->copy(*rnn_simple.params[i][0]);
            params[i][1]->copy(*rnn_simple.params[i][1]);
        }
    }

    Expression ReluDNNBuilder::add_input_impl(const Expression &in)  {
        h.resize(layers);

        Expression x = in;

        for (unsigned i = 0; i < layers; ++i) {
            const vector<Expression>& vars = param_vars[i];

            Expression y = affine_transform({ biases[i][0], vars[0], x });

            x = h[i] = rectify(y);
        }
        return h.back();
    }

} // namespace cnn


#pragma once

#include "../Math/math.hh"

#include <Eigen/Core>

namespace bold
{
/*
  enum class PointType
  {
    /// The point is on the lower bound of an occlusion.
    /// In 2D, this is the bottom of the image.
    /// In 3D this is the nearest point in the occluded area along a ray from the viewer.
    Occlusion = 1,
    /// The point is visually at the top of an area considered as the field, although this may be the camera edge
    FieldEdge = 2,
    /// Point is located at the boundary of the camera image
    CameraEdge = 3
  };
*/

  template<typename T,int Dim=2>
  class OcclusionRay
  {
  public:
    typedef Eigen::Matrix<T,Dim,1> Point;

    OcclusionRay(Point near, Point far/*, PointType nearType, PointType farType*/)
    : d_near(near),
      d_far(far)/*,
      d_nearType(nearType),
      d_farType(farType)*/
    {}

    Point const& near() const { return d_near; }
    Point const& far() const { return d_far; }
//    PointType const& nearType() const { return d_nearType; }
//    PointType const& farType() const { return d_farType; }

    double constexpr norm() const { return (d_near - d_far).norm(); }
    double constexpr angle() const { return Math::angleToPoint(d_near); }

//    double distance() const;
//    bool isOpenField() const;
//    bool isOcclusion() const;
//    bool isFieldEdge() const;

  private:
    Point d_near;
    Point d_far;
//    PointType d_nearType;
//    PointType d_farType;
  };
}


#ifndef icvEigenMatrixData_hxx
#define icvEigenMatrixData_hxx

#include "OpenICV/Core/icvDataObject.h"

#include <Eigen/Core>
#include <boost/utility/enable_if.hpp>

namespace icv
{
    namespace eigen
    {
        template <typename _Scalar, int _Rows, int _Cols,
            int _Options = Eigen::AutoAlign |
                ( (_Rows == 1 && _Cols != 1) ? Eigen::RowMajor
                : (_Cols == 1 && _Rows != 1) ? Eigen::ColMajor
                : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION),
            int _MaxRows = _Rows, int _MaxCols = _Cols>
        class icvEigenMatrixData : icv::core::icvDataObject
        {
        public:
            typedef Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> UType;
            typedef icvEigenMatrixData<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> SelfType;

        public:
            icvEigenMatrixData()
            {
                BOOST_STATIC_ASSERT_MSG(UType::SizeAtCompileTime != Eigen::Dynamic,
                    "You shouldn't initialize a dynamic matrix without a shape.");
            }
            icvEigenMatrixData(uint32_t dim) : _init_arg1(dim)
            {
                BOOST_STATIC_ASSERT_MSG((UType::IsVectorAtCompileTime && UType::SizeAtCompileTime == Eigen::Dynamic),
                    "You should initialize a dynamic vector giving a dimension.");
            }
            icvEigenMatrixData(uint32_t rows, uint32_t cols) : _init_arg1(rows), _init_arg2(cols)
            {
                // FIXME: Following statement doesn;t work properly...
                // BOOST_STATIC_ASSERT_MSG((!UType::IsVectorAtCompileTime && UType::SizeAtCompileTime == Eigen::Dynamic),
                //     "You should initialize a dynamic matrix giving a shape.");
            }

            virtual void Reserve() ICV_OVERRIDE
            {
                if (!_data)
                {
                    _new();
                }
            }
            virtual void Dispose() ICV_OVERRIDE
            {
                if (_data) delete _data;
                _data = ICV_NULLPTR;
            }

            virtual Uint64 GetActualMemorySize() ICV_OVERRIDE
            {
                if (_data) return _data->rows() * _data->cols() * sizeof(_Scalar);
                else return 0;
            }

            virtual void Serialize(std::stringstream& out, const uint32_t& version) const ICV_OVERRIDE
            {
                ICV_THROW_MESSAGE("Directly serialize icvEigenMatrixData is not supported, please convert it to icvTensorData");
            }
            virtual void Deserialize(std::stringstream& in, const uint32_t& version) ICV_OVERRIDE
            {
                ICV_THROW_MESSAGE("Directly serialize icvEigenMatrixData is not supported, please convert it from icvTensorData");
            }

            virtual icv::core::icvDataObject* DeepCopy() ICV_OVERRIDE
            {
                SelfType* copy = new SelfType(_init_arg1, _init_arg2);
                copy->_sourceTime = _sourceTime;
                copy->_init_arg1 = _init_arg1;
                copy->_init_arg2 = _init_arg2;
                copy->Reserve(); *(copy->_data) = *_data;
                return copy;
            }

            virtual std::string Print() ICV_OVERRIDE
            {
                return "Eigen::Matrix";
            }

            operator const UType&() const { return *_data; }

            UType* operator->() { return _data; }
            const UType* operator->() const { return _data; }

            SelfType& operator = (const UType& data)
            {
                *_data = data;
                return *this;
            }

        private:
            UType * _data = ICV_NULLPTR;

            // Params only for initialization
            uint32_t _init_arg1, _init_arg2;

        private:
            template<class M = UType, typename boost::enable_if<boost::integral_constant<bool,
                M::IsVectorAtCompileTime && M::SizeAtCompileTime == Eigen::Dynamic>, int>::type = 0>
            void _new()
            {
                _data = new M(_init_arg1);
            }

            template<class M = UType, typename boost::enable_if<boost::integral_constant<bool,
                !M::IsVectorAtCompileTime && M::SizeAtCompileTime == Eigen::Dynamic>, int>::type = 0>
            void _new()
            {
                _data = new M(_init_arg1, _init_arg2);
            }

            template<class M = UType, typename boost::enable_if<boost::integral_constant<bool,
                M::SizeAtCompileTime != Eigen::Dynamic>, int>::type = 0>
            void _new()
            {
                _data = new M();
            }
        };

        #define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)                             \
        /** \ingroup matrixtypedefs */                                                              \
        typedef icvEigenMatrixData<Type, Size, Size> icvEigenMatrix##SizeSuffix##TypeSuffix##Data;  \
        /** \ingroup matrixtypedefs */                                                              \
        typedef icvEigenMatrixData<Type, Size, 1>    icvEigenVector##SizeSuffix##TypeSuffix##Data;  \
        /** \ingroup matrixtypedefs */                                                              \
        typedef icvEigenMatrixData<Type, 1, Size>    icvEigenRowVector##SizeSuffix##TypeSuffix##Data;

        #define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size)                                         \
        /** \ingroup matrixtypedefs */                                                                    \
        typedef icvEigenMatrixData<Type, Size, Eigen::Dynamic> icvEigenMatrix##Size##X##TypeSuffix##Data; \
        /** \ingroup matrixtypedefs */                                                                    \
        typedef icvEigenMatrixData<Type, Eigen::Dynamic, Size> icvEigenMatrix##X##Size##TypeSuffix##Dara;

        #define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
                EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \
                EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \
                EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \
                EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Eigen::Dynamic, X) \
                EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 2) \
                EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 3) \
                EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 4)

        EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
        EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
        EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
        // EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
        // EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)

        #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
        #undef EIGEN_MAKE_TYPEDEFS
        #undef EIGEN_MAKE_FIXED_TYPEDEFS
    }
}

#endif // icvEigenMatrixData_hxx


#include <algorithm>
#include <fstream>
#include <functional>
#include <iostream>
#include <numeric>
#include <set>
#include <string>
#include <unordered_map>
#include <vector>

#include <boost/iterator/counting_iterator.hpp>

template<typename T>
class Range {
	private:
		boost::counting_iterator<T, boost::use_default, T> begin_, end_;
	public:
		Range(T b, T e): begin_(b), end_(e) {}
		auto begin() const { return begin_; }
		auto end() const { return end_; }
};

using PersonID = unsigned;

struct SeatingPair {
	PersonID left_id;
	PersonID right_id;
};

auto operator==(const SeatingPair& lhs, const SeatingPair& rhs) {
	return ((lhs.left_id == rhs.left_id) && (lhs.right_id == rhs.right_id))
		   || ((lhs.left_id == rhs.right_id) && (lhs.right_id == rhs.left_id));
}

using Person = std::string;

struct SeatingPairEntry {
	Person left_person;
	Person right_person;
	unsigned score;
};

auto& operator>>(std::istream& in, SeatingPairEntry& entry) {
	return in >> entry.left_person >> entry.score >> entry.right_person;
}

namespace std {
	template<>
	struct hash<SeatingPair> {
		auto operator()(const SeatingPair& pair) const {
			const auto [min, max] = std::minmax(pair.left_id, pair.right_id);
			return (min * 10) + max;
		}
	};
}

template<typename T>
auto factorial(T n) {
	auto range = Range<T>{2, (n + 1)};
	return std::accumulate(range.begin(), range.end(), T{1}, std::multiplies{});
}

int main() {

	const auto filename = std::string{"happiness.txt"};
	auto file = std::fstream{filename};

	if(file.is_open()) {

		// std::set is used for its automatic ordering of keys,
		// which is important when iterating through permutations
		// of seating orders later on
		auto family_set = std::set<PersonID>{};
		auto family_map = std::unordered_map<Person, PersonID>{};

		using PairHappiness = int;
		auto happymeter = std::unordered_map<SeatingPair, PairHappiness>{};

		const auto add_family_member = [&family_map, &family_set] (const auto& member) {

			const auto is_new = family_map.insert({member, {}}).second;

			if(is_new) {
				const auto size = family_map.size();
				family_map[member] = size;
				family_set.insert(size);
			}
		};

		SeatingPairEntry entry;

		while(file >> entry) {

			const auto& left_person = entry.left_person;
			const auto& right_person = entry.right_person;

			add_family_member(left_person);
			add_family_member(right_person);

			const auto left_id = family_map[left_person];
			const auto right_id = family_map[right_person];

			happymeter[{left_id, right_id}] += entry.score;
		}

		// elements must be sorted, fortunately std::set has already done that job for us
		auto seating = std::vector<PersonID>{family_set.begin(), family_set.end()};

		// only a subset of all possible permutations of seating orders is relevant for us,
		// so we create an upper limit to count down from when looping through permutations
		const auto size = family_map.size();
		auto limit = (factorial(size) / size) * (size - 1);

		auto max_happiness = PairHappiness{};

		do {
			// since the seating is circular, the pair [first, last] won't be
			// added in the algorithm, so we initialize the accumulator with it
			const auto begin1 = seating.begin();
			const auto end1	  = std::prev(seating.end());
			const auto begin2 = std::next(begin1);
			const auto acc	  = happymeter[{seating.front(), seating.back()}];
			const auto op1	  = std::plus{};
			const auto op2	  = [&happymeter] (auto a, auto b) { return happymeter[{a, b}];};

			// inner_product resolves to acc = op1(acc, op2(begin1, begin2)) in a loop
			auto sum = std::inner_product(begin1, end1, begin2, acc, op1, op2);

			max_happiness = std::max(max_happiness, sum);

		} while((limit--) > 0 && std::next_permutation(seating.begin(), seating.end()));

		std::cout << max_happiness << std::endl;

	} else {
		std::cerr << "Error! Could not open \"" << filename << "\"!" << std::endl;
	}

	return 0;
}


// Copyright 2010-2014, D. E. Shaw Research.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt )

#ifndef BOOST_RANDOM_PHILOX_HPP
#define BOOST_RANDOM_PHILOX_HPP
#include <boost/array.hpp>
#include <boost/static_assert.hpp>
#include <boost/cstdint.hpp>
#include <boost/limits.hpp>
#include <boost/random/detail/mulhilo.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/range_c.hpp>

namespace boost{
namespace random{

template <unsigned _N, typename Uint>
struct philox_constants{
    // specializations will hold the Mutlipliers: M0, M1
    // and the Weyl constants:  W0, W1
};

template <>
struct philox_constants<2, uint64_t>{
    static const uint64_t M0 = UINT64_C(0xD2B74407B1CE6E93);
    static const uint64_t W0 = UINT64_C(0x9E3779B97F4A7C15);
};

template <>
struct philox_constants<2, uint32_t>{
    static const uint32_t M0 = UINT32_C(0xD256D193);
    static const uint32_t W0 = UINT32_C(0x9E3779B9);
};

template <>
struct philox_constants<4, uint64_t>{
    static const uint64_t M0 = UINT64_C(0xD2E7470EE14C6C93);
    static const uint64_t M1 = UINT64_C(0xCA5A826395121157);
    static const uint64_t W0 = UINT64_C(0x9E3779B97F4A7C15);  /* golden ratio */
    static const uint64_t W1 = UINT64_C(0xBB67AE8584CAA73B);  /* sqrt(3)-1 */
};

template <>
struct philox_constants<4, uint32_t>{
    static const uint32_t M0 = UINT32_C(0xD2511F53);
    static const uint32_t M1 = UINT32_C(0xCD9E8D57);
    static const uint32_t W0 = UINT64_C(0x9E3779B9);  /* golden ratio */
    static const uint32_t W1 = UINT64_C(0xBB67AE85);  /* sqrt(3)-1 */
};

template <unsigned N, typename Uint, unsigned R=10, typename Constants = philox_constants<N, Uint> >
struct philox{
    BOOST_STATIC_ASSERT( N%2 == 0 );
};

template <typename Uint, unsigned R, typename Constants>
struct philox<2, Uint, R, Constants> {
    typedef array<Uint, 2> domain_type;
    typedef array<Uint, 2> range_type;
    typedef array<Uint, 1> key_type ;

    philox() : k(){}
    philox(key_type _k) : k(_k) {}
    philox(const philox& v) : k(v.k){}

    void setkey(key_type _k){
        k = _k;
    }

    key_type getkey() const{
        return k;
    }

    bool operator==(const philox& rhs) const{
        return k == rhs.k;
    }

    bool operator!=(const philox& rhs) const{
        return k != rhs.k;
    }

    range_type operator()(domain_type c){
        key_type kcopy = k;
#if 0   // using mpl to unroll the loop doesn't seem to help much.
        _roundapplyer ra(c, kcopy);
        mpl::for_each<mpl::range_c<unsigned, 0, R> >(ra);
#else
        for(unsigned r=0; r<R; ++r)
            round(c, kcopy);
#endif
        return c;
    }
protected:
    static inline void round(domain_type& ctr, key_type& key){
        Uint hi;
        Uint lo = detail::mulhilo(Constants::M0, ctr[0], hi);
        domain_type out = {{hi^key[0]^ctr[1], lo}};
        ctr = out;
        key[0] += Constants::W0;
    }

    struct _roundapplyer{
        domain_type& c;
        key_type& k;
        _roundapplyer(domain_type& _c, key_type& _k): c(_c), k(_k){}
        void operator()(unsigned){ round(c, k); }
    };
    key_type k;
};

template<typename Uint, unsigned R, typename Constants>
struct philox<4, Uint, R, Constants> {
    typedef array<Uint, 4> domain_type;
    typedef array<Uint, 4> range_type;
    typedef array<Uint, 2> key_type ;
public:

    philox() : k(){}
    philox(key_type _k) : k(_k) {}
    philox(const philox& v) : k(v.k){}

    void setkey(key_type _k){
        k = _k;
    }

    key_type getkey() const{
        return k;
    }

    bool operator==(const philox& rhs) const{
        return k == rhs.k;
    }

    bool operator!=(const philox& rhs) const{
        return k != rhs.k;
    }

    range_type operator()(domain_type c){
        key_type kcopy = k;
#if 0   // using mpl to unroll the loop doesn't seem to help much.
        _roundapplyer ra(c, kcopy);
        mpl::for_each<mpl::range_c<unsigned, 0, R> >(ra);
#else
        for(unsigned r=0; r<R; ++r)
            round(c, kcopy);
#endif
        return c;
    }

protected:
    static inline void round(domain_type& ctr, key_type& key){
        Uint hi0;
        Uint hi1;
        Uint lo0 = detail::mulhilo(Constants::M0, ctr[0], hi0);
        Uint lo1 = detail::mulhilo(Constants::M1, ctr[2], hi1);
        domain_type out = {{hi1^ctr[1]^key[0], lo1,
                                      hi0^ctr[3]^key[1], lo0}};
        ctr = out;
        key[0] += Constants::W0;
        key[1] += Constants::W1;
    }

    struct _roundapplyer{
        domain_type& c;
        key_type& k;
        _roundapplyer(domain_type& _c, key_type& _k): c(_c), k(_k){}
        void operator()(unsigned){ round(c, k); }
    };

    key_type k;
};

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_PHILOX_HPP


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2012 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if !defined(TREE_LENGTH_DISTRIBUTION_HPP)
#define TREE_LENGTH_DISTRIBUTION_HPP

#if defined(_MSC_VER)
#	pragma warning(disable: 4267)	// warning about loss of data when converting size_t to int
#endif

#include <cmath>
#include "ncl/nxsdefs.h"

#include <boost/shared_ptr.hpp>
#include <boost/format.hpp>
#include "basic_cdf.hpp"
#include "basic_lot.hpp"
#include "basic_tree.hpp"
#include "probability_distribution.hpp"
#include "multivariate_probability_distribution.hpp"

namespace phycas
{

/*------------------------------------------------------------------------------------------------------------------------------------------------------------------
|	This is the joint distribution of tree length and edge lengths. The marginal distribution of tree length is Gamma(alphaT, betaT), while the marginal
|   of edge lengths conditional on tree length is Dirichlet(a_1, a_2, ..., a_m, b_1, b_2, ..., b_n) where a_1=a_2=...=a_m=alpha, b_1=b_2=...=b_n=c*alpha,
|   m is the number of external edges, and n is the number of internal edges. This prior was described in the following paper:
|
|   Rannala, Bruce, Tianqi Zhu, and Ziheng Yang. 2012. Tail paradox, partial identifiability, and influential priors in Bayesian branch length inference.
|   Mol. Biol. Evol. 29(1):325-335.
|
|   Note 1: this class follows the conventions of the paper in that the gamma distribution mean equals shape/scale, whereas everywhere else a gamma distribution
|   appears in Phycas it has the property that mean equals shape*scale.
|
|   Note 2: this class is unusual in that it represents a probability distribution but is not derived from either ProbabilityDistribution or
|   MultivariateProbabilityDistribution. This is because it is a compound distribution combining a univariate (gamma) distribution with a multivariate
|   (Dirichlet) distribution, and requires a tree as input to its GetLnPDF function.
*/
class TreeLengthDistribution   // TREE_LENGTH_DISTRIBUTION
	{
	public:
											TreeLengthDistribution();
											TreeLengthDistribution(double alphaT, double betaT, double alpha, double c);
                        					TreeLengthDistribution(const TreeLengthDistribution & other);
		virtual								~TreeLengthDistribution() {}

		virtual void						SetLot(Lot * other);
		virtual void						ResetLot();
		virtual void						SetSeed(unsigned rnseed);

        TreeLengthDistribution * 			cloneAndSetLot(Lot * other) const;
        TreeLengthDistribution * 			Clone() const;

		virtual std::string                 GetDistributionName() const;
		virtual std::string 				GetDistributionDescription() const;
		virtual std::vector<double>			Sample(unsigned num_external, unsigned num_internal);
		virtual double						GetLnPDF(TreeShPtr t) const;
		virtual double						GetRelativeLnPDF(TreeShPtr t) const;

        double                              getShape() const {return _alphaT;}
        double                              getScale() const {return _betaT;}
        double                              getExtEdgelenParam() const {return _alpha;}
        double                              getIntExtEdgelenRatio() const {return _c;}

    protected:

        void                                SetupSamplingDistributions(unsigned num_external, unsigned num_internal);

    private:

		double								_alphaT;                /**< The shape parameter of the gamma tree length distribution */
		double								_betaT;                 /**< The scale paramter of the gamma tree length distribution */
		double								_alpha;                 /**< The parameter governing the Dirichlet distribution for external edge lengths (internal edge lengths have parameter c*alpha) */
		double								_c;                     /**< The ratio of the mean internal/external edge length (normally less than 1.0) */

        CDF                                 _cdf;                   /**< Used in GetLnPDF to compute log of gamma function */
		Lot                                 _myLot;                 /**< Own random number generator */
		Lot *                               _lot;                   /**< Points to either _myLot or an external random number generator object */

        ProbDistShPtr                       _tldist;                /**< Points to a gamma distribution of tree lengths used when sampling from this distribution */
        MultivarProbDistShPtr               _eldist;                /**< Points to a Dirichlet distribution of edge length proportions used when sampling from this distribution */
        unsigned                            _num_internal_edges;    /**< The number of internal edges specified the last time Sample was called */
        unsigned                            _num_external_edges;    /**< The number of external edges specified the last time Sample was called */
	};

typedef boost::shared_ptr<TreeLengthDistribution> TreeLengthDistributionShPtr;

} // namespace phycas

#endif


// Boost.GIL (Generic Image Library) - tests
//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are 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)
//

#include <boost/core/lightweight_test.hpp>
#include <boost/gil/rasterization/circle.hpp>
#include <cstddef>
#include <vector>

namespace gil = boost::gil;

template <typename Rasterizer>
void test_rasterizer_follows_equation(std::ptrdiff_t radius, Rasterizer rasterizer)
{

    std::vector<gil::point_t> circle_points(rasterizer.point_count(radius));
    std::ptrdiff_t const r_squared = radius * radius;
    rasterizer(radius, {0, 0}, circle_points.begin());
    std::vector<gil::point_t> first_octant(rasterizer.point_count(radius) / 8);

    for (std::size_t i = 0, octant_index = 0; i < circle_points.size(); i += 8, ++octant_index)
    {
        first_octant[octant_index] = circle_points[i];
    }

    for (const auto& point : first_octant)
    {
        double y_exact = std::sqrt(r_squared - point.x * point.x);
        std::ptrdiff_t lower_result = static_cast<std::ptrdiff_t>(std::floor(y_exact));
        std::ptrdiff_t upper_result = static_cast<std::ptrdiff_t>(std::ceil(y_exact));
        BOOST_TEST(point.y >= lower_result && point.y <= upper_result);
    }
}

template <typename Rasterizer>
void test_connectivity(std::ptrdiff_t radius, Rasterizer rasterizer)
{
    std::vector<gil::point_t> circle_points(rasterizer.point_count(radius));
    rasterizer(radius, {radius, radius}, circle_points.begin());
    for (std::size_t i = 0; i < 8; ++i)
    {
        std::vector<gil::point_t> octant(circle_points.size() / 8);
        for (std::size_t octant_index = i, index = 0; octant_index < circle_points.size();
             octant_index += 8, ++index)
        {
            octant[index] = circle_points[octant_index];
        }

        for (std::size_t index = 1; index < octant.size(); ++index)
        {
            const auto diff_x = std::abs(octant[index].x - octant[index - 1].x);
            const auto diff_y = std::abs(octant[index].y - octant[index - 1].y);
            BOOST_TEST_LE(diff_x, 1);
            BOOST_TEST_LE(diff_y, 1);
        }
    }
}

int main()
{
    for (std::ptrdiff_t radius = 5; radius <= 512; ++radius)
    {
        test_rasterizer_follows_equation(radius, gil::midpoint_circle_rasterizer{});
        // TODO: find out a new testing procedure for trigonometric rasterizer
        // test_equation_following(radius, gil::trigonometric_circle_rasterizer{});
        test_connectivity(radius, gil::midpoint_circle_rasterizer{});
        test_connectivity(radius, gil::trigonometric_circle_rasterizer{});
    }

    return boost::report_errors();
}


#pragma once
#ifndef CANNON_MATH_RANDOM_DOUBLE_H
#define CANNON_MATH_RANDOM_DOUBLE_H

/*!
 * \file cannon/math/random_double.hpp
 * \brief File containing utilities for generating random doubles and vectors.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace math {

    /*!
     * \brief Generate a random double uniformly at random between 0 and 1 in a
     * thread-safe way.
     *
     * \returns The generated double.
     */
    double random_double();

    /*!
     * \brief Generate a random double between min and max.
     *
     * \param min The minimum number that can be generated.
     * \param max The maximum number that can be generated.
     *
     * \returns The generated double.
     */
    double random_double(double min, double max);

    /*!
     * \brief Generate a random vector with entries sampled uniformly at random
     * between 0 and 1.
     *
     * \returns The generated vector.
     */
    Vector3d random_vec();

    /*!
     * \brief Generate a random vector with entries sampled uniformly at random
     * between the input minimum and maximum.
     *
     * \param min The minimum value to generate.
     * \param max The maximum value to generate.
     *
     * \returns The generated vector.
     */
    Vector3d random_vec(double min, double max);

    /*!
     * \brief Generate a random vector in the unit sphere.
     *
     * \returns The generated vector.
     */
    Vector3d random_in_unit_sphere();

    /*!
     * \brief Generate a random unit vector.
     *
     * \returns The generated vector.
     */
    Vector3d random_unit_vec();

    /*!
     * \brief Generate a random vector in the unit hemisphere around the input
     * normal vector.
     *
     * \param normal The normal vector to sample around.
     *
     * \returns The generated vector.
     */
    Vector3d random_in_hemisphere(const Vector3d& normal);

    /*!
     * \brief Generate a random vector in the unit disk in the X-Y plane.
     *
     * \returns The generated vector.
     */
    Vector3d random_in_disk();

  } // namespace math
} // namespace cannon

#endif /* ifndef CANNON_MATH_RANDOM_DOUBLE_H */


#include <vector>

#include <aslam/cameras/camera-pinhole.h>
#include <aslam/common/memory.h>
#include <aslam/common/stl-helpers.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <glog/logging.h>
#include <opencv2/calib3d/calib3d.hpp>

#include "aslam/calibration/target-observation.h"
#include "aslam/calibration/helpers.h"

namespace aslam {
namespace calibration {

// Initializes the intrinsics vector based on one views of a calibration targets.
// On success it returns true. These functions are based on functions from Lionel Heng and
// the excellent camodocal: https://github.com/hengli/camodocal.
// This algorithm can be used with high distortion lenses.
//
// C. Hughes, P. Denny, M. Glavin, and E. Jones,
// Equidistant Fish-Eye Calibration and Rectification by Vanishing Point
// Extraction, PAMI 2010
// Find circles from rows of chessboard corners, and for each pair
// of circles, find vanishing points: v1 and v2.
// f = ||v1 - v2|| / PI;
bool initFocalLengthVanishingPoints(
    const std::vector<TargetObservation::Ptr>& observations, Eigen::VectorXd* intrinsics) {
  CHECK_NOTNULL(intrinsics);
  CHECK(!observations.empty()) << "Need at least one observation.";

  const double cu = (observations.at(0)->getImageWidth() - 1.0) / 2.0;
  const double cv = (observations.at(0)->getImageHeight() - 1.0) / 2.0;

  const size_t num_obs = observations.size();
  std::vector<double> f_guesses;

  for (size_t obs_idx = 0; obs_idx < num_obs; ++obs_idx) {
    TargetObservation::ConstPtr obs = observations[obs_idx];
    CHECK(obs);
    TargetBase::ConstPtr current_target = obs->getTarget();
    CHECK(current_target) << "The TargetObservation has no target object.";

    // We can only process complete image observations.
    if (!obs->allCornersObservered()) {
      continue;
    }

    // Try to fit circles to each row of observations.
    Aligned<std::vector, Eigen::Vector2d> center(current_target->rows());
    std::vector<double> radius(current_target->rows());

    for (size_t r = 0u; r < current_target->rows(); ++r) {
      std::vector<cv::Point2d> points_on_circle;
      for (size_t c = 0u; c < current_target->cols(); ++c) {
        const size_t corner_idx = r * current_target->cols() + c;
        Eigen::Vector2d obs_corner;
        bool success = obs->getObservedCornerById(corner_idx, &obs_corner);
        if (success == true){
          points_on_circle.emplace_back(
              obs_corner[0],
              obs_corner[1]);
        }
      }
      InitializerHelpers::fitCircle(points_on_circle, &center[r](0), &center[r](1), &radius[r]);
    }

    // Intersect all circles to find the focal length guesses.
    for (size_t j = 0u; j < current_target->rows(); ++j) {
      for (size_t k = j + 1u; k < current_target->rows(); ++k) {
        // Find the distance between pair of vanishing points which
        // correspond to intersection points of 2 circles.
        std::vector<cv::Point2d> intersection_points;
        CHECK_LT(j, center.size());
        CHECK_LT(k, center.size());
        CHECK_LT(j, radius.size());
        CHECK_LT(k, radius.size());
        InitializerHelpers::intersectCircles(center[j](0), center[j](1), radius[j],
                                             center[k](0), center[k](1), radius[k],
                                             &intersection_points);
        if (intersection_points.size() >= 2) {
          const double f_guess = cv::norm(intersection_points[0] - intersection_points[1]) / M_PI;
          f_guesses.emplace_back(f_guess);
        }
      }
    }
  }

  // Gets the median of the guesses.
  if (f_guesses.empty()) {
    return false;
  }
  const double f0 = aslam::common::median(f_guesses.begin(), f_guesses.end());

  // Sets the first intrinsics estimate.
  intrinsics->resize(aslam::PinholeCamera::parameterCount());
  (*intrinsics)(PinholeCamera::kFu) = f0;
  (*intrinsics)(PinholeCamera::kFv) = f0;
  (*intrinsics)(PinholeCamera::kCu) = cu;
  (*intrinsics)(PinholeCamera::kCv) = cv;

  return true;
}

// Initializes the intrinsics vector based on one view of a gridded calibration target.
// On success it returns true. These functions are based on functions from Lionel Heng
// and the excellent camodocal https://github.com/hengli/camodocal.
//
// Z. Zhang
// A Flexible New Technique for Camera Calibration,
// Extraction, PAMI 2000
// Intrinsics estimation with image of absolute conic;
bool initFocalLengthAbsoluteConic(
  const std::vector<TargetObservation::Ptr>& observations, Eigen::VectorXd* intrinsics) {
 CHECK_NOTNULL(intrinsics);
 CHECK(!observations.empty()) << "Need at least one observation.";

 const double cu = (observations.at(0)->getImageWidth() - 1.0) / 2.0;
 const double cv = (observations.at(0)->getImageHeight() - 1.0) / 2.0;

 const size_t num_obs = observations.size();
 cv::Mat A(num_obs * 2, 2, CV_64F);
 cv::Mat b(num_obs * 2, 1, CV_64F);

 for (size_t obs_idx = 0u; obs_idx < num_obs; ++obs_idx) {
   TargetObservation::ConstPtr obs = observations[obs_idx];
   CHECK(obs);
   TargetBase::ConstPtr current_target = obs->getTarget();
   CHECK(current_target) << "The TargetObservation has no target object.";

   // We can only process complete image observations.
   if (!obs->allCornersObservered()) {
     continue;
   }

   std::vector<cv::Point2f> image_corners(obs->numObservedCorners());
   std::vector<cv::Point2f> M(obs->numObservedCorners());

   for (size_t j = 0; j < image_corners.size(); ++j) {
     Eigen::Vector2d obs_corner;
     bool success = obs->getObservedCornerById(j, &obs_corner);
     if (success == true){
       image_corners[j] = cv::Point2f(obs_corner[0],
                                      obs_corner[1]);
       M[j] = cv::Point2f(current_target->point(j)[0],
                          current_target->point(j)[1]);
     }
   }

   cv::Mat H = cv::findHomography(M, image_corners);

   H.at<double>(0,0) -= H.at<double>(2,0) * cu;
   H.at<double>(0,1) -= H.at<double>(2,1) * cu;
   H.at<double>(0,2) -= H.at<double>(2,2) * cu;
   H.at<double>(1,0) -= H.at<double>(2,0) * cv;
   H.at<double>(1,1) -= H.at<double>(2,1) * cv;
   H.at<double>(1,2) -= H.at<double>(2,2) * cv;

   double h[3], v[3], d1[3], d2[3];
   double n[4] = {0,0,0,0};

   for (int j = 0; j < 3; ++j) {
     double t0 = H.at<double>(j,0);
     double t1 = H.at<double>(j,1);
     h[j] = t0; v[j] = t1;
     d1[j] = (t0 + t1) * 0.5;
     d2[j] = (t0 - t1) * 0.5;
     n[0] += t0 * t0; n[1] += t1 * t1;
     n[2] += d1[j] * d1[j]; n[3] += d2[j] * d2[j];
   }

   for (int j = 0; j < 4; ++j) {
     n[j] = 1.0 / sqrt(n[j]);
   }

   for (int j = 0; j < 3; ++j) {
     h[j] *= n[0]; v[j] *= n[1];
     d1[j] *= n[2]; d2[j] *= n[3];
   }

   A.at<double>(obs_idx * 2, 0) = h[0] * v[0];
   A.at<double>(obs_idx * 2, 1) = h[1] * v[1];
   A.at<double>(obs_idx * 2 + 1, 0) = d1[0] * d2[0];
   A.at<double>(obs_idx * 2 + 1, 1) = d1[1] * d2[1];
   b.at<double>(obs_idx * 2, 0) = -h[2] * v[2];
   b.at<double>(obs_idx * 2 + 1, 0) = -d1[2] * d2[2];
 }

 cv::Mat f(2, 1, CV_64F);
 cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU);

 // Sets the first intrinsics estimate.
 intrinsics->resize(aslam::PinholeCamera::parameterCount());
 (*intrinsics)(PinholeCamera::kFu) = sqrt(fabs(1.0 / f.at<double>(0)));
 (*intrinsics)(PinholeCamera::kFv) = sqrt(fabs(1.0 / f.at<double>(1)));
 (*intrinsics)(PinholeCamera::kCu) = cu;
 (*intrinsics)(PinholeCamera::kCv) = cv;

 return true;
}

}  // namespace calibration
}  // namespace aslam


#ifndef BART_SRC_QUADRATURE_CALCULATORS_ANGULAR_FLUX_INTEGRATOR_I_HPP_
#define BART_SRC_QUADRATURE_CALCULATORS_ANGULAR_FLUX_INTEGRATOR_I_HPP_

#include <map>

#include <deal.II/lac/vector.h>

#include "quadrature/quadrature_types.h"
#include "utility/named_type.h"

namespace bart::quadrature::calculators {
/*! \brief Interface for a class that integrates angular flux using a quadrature set.
 *
 * The provided functions calculate the net current at a degree of freedom \f$i\f$:
 * \f[
 * \vec{J}_i = \int \hat{\Omega} \psi(\hat{\Omega})_i d\hat{\Omega} = \sum_{m = 0}^M w_m\hat{\Omega}_m\psi_{i,m}\;,
 * \f]
 * the magnitude of the current in direction \f$\hat{n}\f$:
 * \f[
 * j_{\hat{n}} = \int_{\hat{n} \cdot \hat{\Omega} \ge 0} |\hat{n} \cdot \hat{\Omega}| \psi(\hat{\Omega})_i d\hat{\Omega} = \sum_{\Omega_m \mid \hat{n} \cdot \Omega \ge 0} w_m|\hat{n} \cdot \hat{\Omega}_m|\psi_{i,m}\;,
 * \f]
 * and the integrated angular flux in direction \f$\hat{n}\f$:
 *
 * \f[
 * \phi_{\hat{n}} = \int_{\hat{n} \cdot \hat{\Omega} \ge 0} \psi(\hat{\Omega})_i d\hat{\Omega} = \sum_{\Omega_m \mid \hat{n} \cdot \Omega \ge 0} w_m\psi_{i,m}\;.
 * \f]
 */
class AngularFluxIntegratorI {
 public:
  virtual ~AngularFluxIntegratorI() = default;
  using Vector = dealii::Vector<double>;
  using VectorPtr = std::shared_ptr<dealii::Vector<double>>;
  using VectorMap = std::map<quadrature::QuadraturePointIndex, VectorPtr>;
  using DegreeOfFreedom = utility::NamedType<int, struct DegreeOfFreedomParam>;

  virtual auto NetCurrent(const VectorMap&) const -> std::vector<Vector> = 0;
  virtual auto NetCurrent(const VectorMap&, const DegreeOfFreedom) const -> Vector = 0;
  virtual auto DirectionalCurrent(const VectorMap&, const Vector normal) const -> std::vector<double> = 0;
  virtual auto DirectionalCurrent(const VectorMap&, const Vector normal, const DegreeOfFreedom) const -> double = 0;
  virtual auto DirectionalFlux(const VectorMap&, const Vector normal) const -> std::vector<double> = 0;
  virtual auto DirectionalFlux(const VectorMap&, const Vector normal, const DegreeOfFreedom) const -> double = 0;
};

} // namespace bart::quadrature::calculators

#endif //BART_SRC_QUADRATURE_CALCULATORS_ANGULAR_FLUX_INTEGRATOR_I_HPP_


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2016 Igor Babuschkin <igor@babuschk.in>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <limits>
#include <numeric>
#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

template <int DataLayout, typename Type=float, bool Exclusive = false>
static void test_1d_scan()
{
  int size = 50;
  Tensor<Type, 1, DataLayout> tensor(size);
  tensor.setRandom();
  Tensor<Type, 1, DataLayout> result = tensor.cumsum(0, Exclusive);

  VERIFY_IS_EQUAL(tensor.dimension(0), result.dimension(0));

  float accum = 0;
  for (int i = 0; i < size; i++) {
    if (Exclusive) {
      VERIFY_IS_EQUAL(result(i), accum);
      accum += tensor(i);
    } else {
      accum += tensor(i);
      VERIFY_IS_EQUAL(result(i), accum);
    }
  }

  accum = 1;
  result = tensor.cumprod(0, Exclusive);
  for (int i = 0; i < size; i++) {
    if (Exclusive) {
      VERIFY_IS_EQUAL(result(i), accum);
      accum *= tensor(i);
    } else {
      accum *= tensor(i);
      VERIFY_IS_EQUAL(result(i), accum);
    }
  }
}

template <int DataLayout, typename Type=float>
static void test_4d_scan()
{
  int size = 5;
  Tensor<Type, 4, DataLayout> tensor(size, size, size, size);
  tensor.setRandom();

  Tensor<Type, 4, DataLayout> result(size, size, size, size);

  result = tensor.cumsum(0);
  float accum = 0;
  for (int i = 0; i < size; i++) {
    accum += tensor(i, 1, 2, 3);
    VERIFY_IS_EQUAL(result(i, 1, 2, 3), accum);
  }
  result = tensor.cumsum(1);
  accum = 0;
  for (int i = 0; i < size; i++) {
    accum += tensor(1, i, 2, 3);
    VERIFY_IS_EQUAL(result(1, i, 2, 3), accum);
  }
  result = tensor.cumsum(2);
  accum = 0;
  for (int i = 0; i < size; i++) {
    accum += tensor(1, 2, i, 3);
    VERIFY_IS_EQUAL(result(1, 2, i, 3), accum);
  }
  result = tensor.cumsum(3);
  accum = 0;
  for (int i = 0; i < size; i++) {
    accum += tensor(1, 2, 3, i);
    VERIFY_IS_EQUAL(result(1, 2, 3, i), accum);
  }
}

template <int DataLayout>
static void test_tensor_maps() {
  int inputs[20];
  TensorMap<Tensor<int, 1, DataLayout> > tensor_map(inputs, 20);
  tensor_map.setRandom();

  Tensor<int, 1, DataLayout> result = tensor_map.cumsum(0);

  int accum = 0;
  for (int i = 0; i < 20; ++i) {
    accum += tensor_map(i);
    VERIFY_IS_EQUAL(result(i), accum);
  }
}

EIGEN_DECLARE_TEST(cxx11_tensor_scan) {
  CALL_SUBTEST((test_1d_scan<ColMajor, float, true>()));
  CALL_SUBTEST((test_1d_scan<ColMajor, float, false>()));
  CALL_SUBTEST((test_1d_scan<RowMajor, float, true>()));
  CALL_SUBTEST((test_1d_scan<RowMajor, float, false>()));
  CALL_SUBTEST(test_4d_scan<ColMajor>());
  CALL_SUBTEST(test_4d_scan<RowMajor>());
  CALL_SUBTEST(test_tensor_maps<ColMajor>());
  CALL_SUBTEST(test_tensor_maps<RowMajor>());
}