/* git    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <string>
#include <thread>

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

#include "tudat/basics/testMacros.h"
#include "tudat/math/basic/linearAlgebra.h"
#include "tudat/astro/basic_astro/physicalConstants.h"
#include "tudat/astro/basic_astro/unitConversions.h"

#include "tudat/interface/spice/spiceInterface.h"
#include "tudat/math/integrators/rungeKuttaCoefficients.h"
#include "tudat/astro/basic_astro/accelerationModel.h"
#include "tudat/io/basicInputOutput.h"

#include "tudat/simulation/environment_setup/body.h"
#include "tudat/simulation/estimation_setup/variationalEquationsSolver.h"
#include "tudat/simulation/environment_setup/defaultBodies.h"
#include "tudat/simulation/environment_setup/createBodies.h"
#include "tudat/simulation/estimation_setup/createNumericalSimulator.h"
#include "tudat/simulation/estimation_setup/createEstimatableParameters.h"

namespace tudat
{

namespace unit_tests
{

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

BOOST_AUTO_TEST_SUITE( test_variational_equation_calculation )

template< typename TimeType = double , typename StateScalarType  = double >
        std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >,
std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > >
executeMultiArcEarthMoonSimulation(
        const std::vector< std::string > centralBodies,
        const Eigen::Matrix< StateScalarType, 12, 1 > initialStateDifference = Eigen::Matrix< StateScalarType, 12, 1 >::Zero( ),
        const int propagationType = 0,
        const bool patchArcsTogether = 0,
        const Eigen::Vector3d parameterPerturbation = Eigen::Vector3d::Zero( ),
        const bool propagateVariationalEquations = 1,
        const std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > forcedArcInitialStates =
        std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >( ),
        const double arcDuration = 5.0E5,
        const double arcOverlap  = 5.0E3 )
{

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

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

    // Specify initial time
    TimeType initialEphemerisTime = TimeType( 1.0E7 );
    TimeType finalEphemerisTime = initialEphemerisTime + 0.5E7;
    double maximumTimeStep = 3600.0;

    double buffer = 10.0 * maximumTimeStep;

    // Create bodies needed in simulation
    BodyListSettings bodySettings =
            getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer );
    bodySettings.at( "Moon" )->ephemerisSettings->resetMakeMultiArcEphemeris( true );
    bodySettings.at( "Earth" )->ephemerisSettings->resetMakeMultiArcEphemeris( true );

    SystemOfBodies bodies = createSystemOfBodies( bodySettings );

    // Set accelerations between bodies that are to be taken into account.
    SelectedAccelerationMap accelerationMap;
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfEarth;
    accelerationsOfEarth[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationsOfEarth[ "Moon" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationMap[ "Earth" ] = accelerationsOfEarth;

    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfMoon;
    accelerationsOfMoon[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationsOfMoon[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationMap[ "Moon" ] = accelerationsOfMoon;

    // Set bodies for which initial state is to be estimated and integrated.
    std::vector< std::string > bodiesToIntegrate;
    bodiesToIntegrate.push_back( "Moon" );
    bodiesToIntegrate.push_back( "Earth" );

    unsigned int numberOfNumericalBodies = bodiesToIntegrate.size( );

    // Define propagator settings.
    std::map< std::string, std::string > centralBodyMap;

    for( unsigned int i = 0; i < numberOfNumericalBodies; i++ )
    {
        centralBodyMap[ bodiesToIntegrate[ i ] ] = centralBodies[ i ];
    }

    // Create acceleration models
    AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodies, accelerationMap, centralBodyMap );

    // Create integrator settings
    std::shared_ptr< IntegratorSettings< TimeType > > integratorSettings =
            std::make_shared< IntegratorSettings< TimeType > >
            ( rungeKutta4, TimeType( initialEphemerisTime ), 1800.0 );

    // Define arc times.
    std::vector< double > arcStartTimes, arcEndTimes;
    TimeType integrationStartTime = initialEphemerisTime;
    TimeType integrationEndTime = finalEphemerisTime;

    TimeType currentStartTime = integrationStartTime;
    TimeType currentEndTime = integrationStartTime + arcDuration;

    do
    {
        arcStartTimes.push_back( currentStartTime );
        arcEndTimes.push_back( currentEndTime );

        currentStartTime = currentEndTime - arcOverlap;
        currentEndTime = currentStartTime + arcDuration;
    }
    while( currentEndTime < ( integrationEndTime ) );

    // Set (perturbed) initial state.
    std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > systemInitialStates;
    for( unsigned int i = 0; i < arcStartTimes.size( ); i++ )
    {
        if( forcedArcInitialStates.size( ) == 0 )
        {
            systemInitialStates.push_back( getInitialStatesOfBodies< TimeType, StateScalarType >(
                                               bodiesToIntegrate, centralBodies, bodies, arcStartTimes.at( i ) ) );
        }
        else
        {
            systemInitialStates.push_back( forcedArcInitialStates.at( i ) );
        }
        systemInitialStates[ i ] += initialStateDifference;
    }

    // Create propagator settings
    TranslationalPropagatorType propagatorType;
    if( propagationType == 0 )
    {
        propagatorType = cowell;
    }
    else if( propagationType == 1 )
    {
        propagatorType = encke;
    }
    std::vector< std::shared_ptr< SingleArcPropagatorSettings< StateScalarType > > > propagatorSettingsList;

    for( unsigned int i = 0; i < arcStartTimes.size( ); i++ )
    {
        propagatorSettingsList.push_back( std::make_shared< TranslationalStatePropagatorSettings< StateScalarType > >
                                          ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialStates.at( i ),
                                            arcEndTimes.at( i ), propagatorType ) );
    }
    std::shared_ptr< MultiArcPropagatorSettings< StateScalarType > > multiArcPropagatorSettings;

    if( patchArcsTogether && forcedArcInitialStates.size( ) == 0 )
    {
        multiArcPropagatorSettings =
                std::make_shared< MultiArcPropagatorSettings< StateScalarType > >(
                    propagatorSettingsList, patchArcsTogether );
    }
    else
    {
        multiArcPropagatorSettings =
                std::make_shared< MultiArcPropagatorSettings< StateScalarType > >(
                    propagatorSettingsList );
    }

    // Define parameters.
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    {
        parameterNames =
                getInitialMultiArcParameterSettings< >( multiArcPropagatorSettings, bodies, arcStartTimes );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Moon", gravitational_parameter ) );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Earth", gravitational_parameter ) );
        parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Sun", gravitational_parameter ) );

    }

    // Create parameters
    std::shared_ptr< estimatable_parameters::EstimatableParameterSet< StateScalarType > > parametersToEstimate =
            createParametersToEstimate( parameterNames, bodies );

    // Perturb parameters.
    Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > parameterVector =
            parametersToEstimate->template getFullParameterValues< StateScalarType >( );

    parameterVector.block( parameterVector.rows( ) - 3, 0, 3, 1 ) += parameterPerturbation;
    parametersToEstimate->resetParameterValues( parameterVector );

    std::pair< std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > >,
            std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > > results;
    {
        // Create dynamics simulator
        MultiArcVariationalEquationsSolver< StateScalarType, TimeType > variationalEquations =
                MultiArcVariationalEquationsSolver< StateScalarType, TimeType >(
                    bodies, integratorSettings, multiArcPropagatorSettings, parametersToEstimate, arcStartTimes );

        // Propagate requested equations.
        if( propagateVariationalEquations )
        {
            variationalEquations.integrateVariationalAndDynamicalEquations( multiArcPropagatorSettings->getInitialStateList( ), 1 );
        }
        else
        {
            variationalEquations.integrateDynamicalEquationsOfMotionOnly( multiArcPropagatorSettings->getInitialStateList( ) );
        }

        // Retrieve test data
        unsigned int numberOfArcs = arcEndTimes.size( );
        for( unsigned int arc = 0; arc < numberOfArcs; arc++ )
        {
            double testEpoch = arcEndTimes.at( arc ) - 2.0E4;
            Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > testStates =
                    Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 >::Zero( 12 );
            testStates.block( 0, 0, 6, 1 ) = bodies.at( "Moon" )->getStateInBaseFrameFromEphemeris( testEpoch );

            testStates.block( 6, 0, 6, 1 ) = bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( testEpoch );

            if( propagateVariationalEquations )
            {
                results.first.push_back( variationalEquations.getStateTransitionMatrixInterface( )->
                                         getCombinedStateTransitionAndSensitivityMatrix( testEpoch ) );
                results.second.push_back( multiArcPropagatorSettings->getInitialStateList( ).at( arc ) );
                Eigen::MatrixXd testMatrixDirect =
                        variationalEquations.getStateTransitionMatrixInterface( )->
                          getCombinedStateTransitionAndSensitivityMatrix( testEpoch );
                Eigen::MatrixXd testMatrixFull=
                        variationalEquations.getStateTransitionMatrixInterface( )->
                          getFullCombinedStateTransitionAndSensitivityMatrix( testEpoch );

                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            testMatrixDirect.block( 0, 0, 12, 12 ),
                            testMatrixFull.block( 0, 12 * arc, 12, 12 ),
                            std::numeric_limits< double >::epsilon( ) );

                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            testMatrixDirect.block( 0, 12, 12, 3 ),
                            testMatrixFull.block( 0, 12 * numberOfArcs, 12, 3 ),
                            std::numeric_limits< double >::epsilon( ) );
            }
            else
            {
                results.second.push_back( testStates );
            }
        }
    }
    return results;
}

BOOST_AUTO_TEST_CASE( testEarthMoonMultiArcVariationalEquationCalculation )
{
    std::pair< std::vector< Eigen::MatrixXd >, std::vector< Eigen::VectorXd > > currentOutput;

    std::vector< std::vector< std::string > > centralBodiesSet;
    std::vector< std::string > centralBodies;

    // Define central bodt settings
    centralBodies.resize( 2 );

    centralBodies[ 0 ] = "SSB";
    centralBodies[ 1 ] = "SSB";
    centralBodiesSet.push_back( centralBodies );

    centralBodies[ 0 ] = "Earth";
    centralBodies[ 1 ] = "Sun";
    centralBodiesSet.push_back( centralBodies );

    // Define variables for numerical differentiation
    Eigen::Matrix< double, 12, 1>  perturbedState;
    Eigen::Vector3d perturbedParameter;

    Eigen::Matrix< double, 12, 1> statePerturbation;
    Eigen::Vector3d parameterPerturbation;

    for( unsigned int i = 0; i < 2; i++ )
    {
        // Define parameter perturbation
        parameterPerturbation  = ( Eigen::Vector3d( ) << 1.0E11, 1.0E11, 1.0E15 ).finished( );

        // Define state perturbation
        if( i == 0 )
        {
            statePerturbation = ( Eigen::Matrix< double, 12, 1>( ) <<
                                  100000.0, 100000.0, 100000.0, 1.0, 0.1, 1.0,
                                  100000.0, 100000.0, 100000.0, 1.0, 0.1, 1.0 ).finished( );
        }
        else if( i == 1 )
        {
            statePerturbation = ( Eigen::Matrix< double, 12, 1>( ) <<
                                  100000.0, 100000.0, 100000.0, 1.0, 0.1, 1.0,
                                  10000000.0, 1000000.0, 100000000.0, 100.0, 100.0, 1000.0 ).finished( );
        }

        // Only perform Encke propagation is origin is not SSB
        unsigned int maximumPropagatorType = 1;
        if( i == 1 )
        {
            maximumPropagatorType = 1;
        }

        for( unsigned int patchArcs = 0; patchArcs < 2; patchArcs++ )
        {
            // Test for all requested propagator types.
            for( unsigned int k = 0; k < maximumPropagatorType; k++ )
            {
                std::cout << "Test: " << i << " " << patchArcs << " " << k << std::endl;
                // Compute state transition and sensitivity matrices
                currentOutput = executeMultiArcEarthMoonSimulation < double, double >(
                            centralBodiesSet[ i ], Eigen::Matrix< double, 12, 1 >::Zero( ), k, patchArcs );
                std::vector< Eigen::MatrixXd > stateTransitionAndSensitivityMatrixAtEpoch = currentOutput.first;
                std::vector< Eigen::VectorXd > nominalArcStartStates;
                if( patchArcs )
                {
                    nominalArcStartStates = currentOutput.second;
                }

                std::vector< Eigen::MatrixXd > manualPartial;
                manualPartial.resize( stateTransitionAndSensitivityMatrixAtEpoch.size( ) );
                for( unsigned int arc = 0; arc < manualPartial.size( ); arc++ )
                {
                    manualPartial[ arc ] = Eigen::MatrixXd::Zero( 12, 15 );
                }
                // Numerically compute state transition matrix
                for( unsigned int j = 0; j < 12; j++ )
                {
                    std::vector< Eigen::VectorXd > upPerturbedState, downPerturbedState;
                    perturbedState.setZero( );
                    perturbedState( j ) += statePerturbation( j );
                    upPerturbedState = executeMultiArcEarthMoonSimulation< double, double >(
                                centralBodiesSet[ i ], perturbedState, k, patchArcs, Eigen::Vector3d::Zero( ), 0, nominalArcStartStates ).second;

                    perturbedState.setZero( );
                    perturbedState( j ) -= statePerturbation( j );
                    downPerturbedState = executeMultiArcEarthMoonSimulation< double, double >(
                                centralBodiesSet[ i ], perturbedState, k, patchArcs, Eigen::Vector3d::Zero( ), 0, nominalArcStartStates ).second;

                    for( unsigned int arc = 0; arc < upPerturbedState.size( ); arc++ )
                    {
                        manualPartial[ arc ].block( 0, j, 12, 1 ) =
                                ( upPerturbedState[ arc ] - downPerturbedState[ arc ] ) / ( 2.0 * statePerturbation( j ) );
                    }
                }

                // Numerically compute sensitivity matrix
                for( unsigned int j = 0; j < 3; j ++ )
                {
                    std::vector< Eigen::VectorXd > upPerturbedState, downPerturbedState;
                    perturbedState.setZero( );
                    Eigen::Vector3d upPerturbedParameter, downPerturbedParameter;
                    perturbedParameter.setZero( );
                    perturbedParameter( j ) += parameterPerturbation( j );
                    upPerturbedState = executeMultiArcEarthMoonSimulation< double, double >(
                                centralBodiesSet[ i ], perturbedState, k, patchArcs, perturbedParameter, 0, nominalArcStartStates ).second;

                    perturbedParameter.setZero( );
                    perturbedParameter( j ) -= parameterPerturbation( j );
                    downPerturbedState = executeMultiArcEarthMoonSimulation< double, double >(
                                centralBodiesSet[ i ], perturbedState, k, patchArcs, perturbedParameter, 0, nominalArcStartStates ).second;

                    for( unsigned int arc = 0; arc < upPerturbedState.size( ); arc++ )
                    {
                        manualPartial[ arc ].block( 0, j + 12, 12, 1 ) =
                                ( upPerturbedState[ arc ] - downPerturbedState[ arc ] ) / ( 2.0 * parameterPerturbation( j ) );
                    }
                }

                // Check results
                for( unsigned int arc = 0; arc < manualPartial.size( ); arc++ )
                {
                    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                                stateTransitionAndSensitivityMatrixAtEpoch.at( arc ).block( 0, 0, 12, 15 ),
                                manualPartial.at( arc ).block( 0, 0, 12, 15 ), 5.0E-4 );
                }

            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


#include "PCA9685/pca9685.h"
#include "pevents/pevents.h"	//Includes event handling (https://github.com/NeoSmart/PEvents)
#include <pthread.h>
#include <Eigen/Dense>
#include "threads/pca_thread.h"

using Eigen::Matrix;
using namespace neosmart;

#define TERMINATE 6

extern pthread_mutex_t PCA_Mutex;
extern pthread_mutex_t stateMachine_Mutex;
extern Matrix<float, 4, 1> PCA_Data;

extern neosmart_event_t e_Timeout; //Always false event for forcing timeout of WaitForEvent
extern neosmart_event_t e_PCA_trigger;
extern I2C i2c;

extern int threadCount;
extern int currentState;

pca9685 pca(&i2c);

void *PCA_Timer(void *threadID){

	printf("PCA_Timer has started!\n");
	int SamplingTime = 8;	//Sampling time in milliseconds
	int localCurrentState;

	while(1){
		WaitForEvent(e_Timeout,SamplingTime);

		//Check system state
		pthread_mutex_lock(&stateMachine_Mutex);
		localCurrentState = currentState;
		pthread_mutex_unlock(&stateMachine_Mutex);

		//check if system should be terminated
		if(localCurrentState == TERMINATE){
			break;
		}

		SetEvent(e_PCA_trigger);
	}

	printf("PCA_Timer stopping...\n");
	threadCount -= 1;
	pthread_exit(NULL);
}

void *PCA_Task(void *threadID){
	printf("PCA_Task has started!\n");
	Matrix<float, 4, 1> localPCA_Data;
	int localCurrentState;

	WaitForEvent(e_PCA_trigger,-1); //Wait until trigger thread finishes loading

	pca.initialize();
	float freq = 500;

	if(pca.pwmFreq(freq)!=1)
	    printf("ERROR: setting pwm frequency\n");
	else
	    printf("Frequency set to %f Hz.\n", freq);

	while(1){

		WaitForEvent(e_PCA_trigger,500);

		//Check system state
		pthread_mutex_lock(&stateMachine_Mutex);
		localCurrentState = currentState;
		pthread_mutex_unlock(&stateMachine_Mutex);

		//check if system should be terminated
		if(localCurrentState == TERMINATE){
			break;
		}

		//Grab motor commands
		pthread_mutex_lock(&PCA_Mutex);
		localPCA_Data = PCA_Data;
		pthread_mutex_unlock(&PCA_Mutex);

		//localPCA_Data.v[0] = 4090;
		//localPCA_Data.v[1] = 4090;
		//localPCA_Data.v[2] = 4090;
		//localPCA_Data.v[3] = 4090;
		//PrintVec4(localPCA_Data, "PCA");

		//Send motor pulses
		pca.pwmPulse(0, 0, localPCA_Data(1)*2048 + 2048);
        pca.pwmPulse(1, 0, localPCA_Data(2)*2048 + 2048);
        pca.pwmPulse(2, 0, localPCA_Data(3)*2048 + 2048);
		pca.pwmPulse(3, 0, localPCA_Data(0)*2048 + 2048);

	}

		//Send motor pulses
		pca.pwmPulse(0, 0, 0*2048 + 2048);
        pca.pwmPulse(1, 0, 0*2048 + 2048);
        pca.pwmPulse(2, 0, 0*2048 + 2048);
		pca.pwmPulse(3, 0, 0*2048 + 2048);

	pca.resetDev();
	printf("PCA_Task stopping...\n");
	threadCount -= 1;
	pthread_exit(NULL);
}


/* ----------------------------------------------------------------------- *//**
 *
 * @file HandleMap_proto.hpp
 *
 *//* ----------------------------------------------------------------------- */

#ifndef MADLIB_DBAL_EIGEN_HANDLEMAP_PROTO_HPP
#define MADLIB_DBAL_EIGEN_HANDLEMAP_PROTO_HPP

#include <boost/type_traits.hpp>
#include <boost/utility/enable_if.hpp>

namespace madlib {

namespace dbal {

namespace eigen_integration {

/**
 * @brief Wrapper class for linear-algebra types based on Eigen
 */
template <class EigenType, class Handle, int MapOptions = Eigen::Unaligned>
class HandleMap : public Eigen::Map<EigenType, MapOptions> {
public:
    typedef Eigen::Map<EigenType, MapOptions> Base;
    typedef typename Base::Scalar Scalar;
    typedef typename Base::Index Index;

    using Base::operator=;

    /**
     * @brief Default constructor
     *
     * Using the HandleMap before a rebind() is undefined and will likely
     * crash the application.
     */
    inline HandleMap()
      : Base(NULL, 1, 1), mMemoryHandle(NULL) { }

    /**
     * @brief Initialize HandleMap backed by the given handle
     *
     * This constructor requires that Handle has a size() method, which is
     * called to determine the length of the vector.
     */
    inline HandleMap(const Handle &inHandle)
      : Base(const_cast<Scalar*>(inHandle.ptr()), inHandle.size()),
        mMemoryHandle(inHandle) { }

    template <class Derived>
    HandleMap(const Eigen::MapBase<Derived>& inMappedData,
        typename boost::enable_if_c<Derived::IsVectorAtCompileTime>::type* = 0)
      : Base(inMappedData.data(), inMappedData.size()),
        mMemoryHandle(inMappedData.data()) {

        madlib_assert(inMappedData.innerStride() == 1,
            std::runtime_error("HandleMap does not support non-contiguous "
                "memory blocks."));
    }

    template <class Derived>
    HandleMap(const Eigen::MapBase<Derived>& inMappedData,
        typename boost::enable_if_c<!Derived::IsVectorAtCompileTime>::type* = 0)
      : Base(inMappedData.data(), inMappedData.rows(), inMappedData.cols()),
        mMemoryHandle(inMappedData.data()) {

        madlib_assert(inMappedData.innerStride() == 1 &&
            ((Derived::IsRowMajor && inMappedData.outerStride() == inMappedData.cols())
            || (!Derived::IsRowMajor && inMappedData.outerStride() == inMappedData.rows())),
            std::runtime_error("HandleMap does not support non-contiguous "
                "memory blocks."));
    }

    /**
     * @brief Initialize HandleMap backed by the given handle
     *
     * This ignores any size information Handle may have. This
     * constructor can be used with any Handle.
     */
    HandleMap(const Handle &inHandle, Index inNumElem)
      : Base(const_cast<Scalar*>(inHandle.ptr()), inNumElem),
        mMemoryHandle(inHandle) { }

    /**
     * @brief Initialize HandleMap backed by the given handle
     *
     * This ignores any size information Handle may have. This
     * constructor can be used with any Handle.
     *
     * @internal
     *     We need to do a const cast here: There is no alternative to
     *     initializing from "const Handle&", but the base class constructor
     *     needs a non-const value. We therefore make sure that a
     *     non-mutable handle can only be assigned if EigenType is a
     *     constant type. See the static assert below.
     */
    HandleMap(const Handle &inHandle, Index inNumRows, Index inNumCols)
      : Base(const_cast<Scalar*>(inHandle.ptr()), inNumRows, inNumCols),
        mMemoryHandle(inHandle) { }

    HandleMap& operator=(const HandleMap& other);
    HandleMap& rebind(const Handle &inHandle);
    HandleMap& rebind(const Handle &inHandle, Index inSize);
    HandleMap& rebind(const Handle &inHandle, Index inRows, Index inCols);
    HandleMap& rebind(Index inSize);
    HandleMap& rebind(Index inRows, Index inCols);
    const Handle &memoryHandle() const;

protected:
    Handle mMemoryHandle;

private:
    BOOST_STATIC_ASSERT_MSG(
        Handle::isMutable || boost::is_const<EigenType>::value,
        "non-const matrix cannot be backed by immutable handle");
};

} // namespace eigen_integration

} // namespace dbal

} // namespace madlib

#endif // defined(MADLIB_DBAL_EIGEN_HANDLEMAP_PROTO_HPP)


//          Copyright Oliver Kowalke 2015.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <algorithm>
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <cstdio>
#include <deque>
#include <iomanip>
#include <iostream>
#include <mutex>
#include <sstream>
#include <string>
#include <thread>

#include <boost/assert.hpp>

#include <boost/fiber/all.hpp>

#include "barrier.hpp"

static std::atomic< bool > fini{ false };

boost::fibers::future< int > fibonacci( int);

int fibonacci_( int n) {
    boost::this_fiber::yield();
    if ( 0 == n) {
        return 0;
    } else if ( 1 == n || 2 == n) {
        return 1;
    } else {
        boost::fibers::future< int > f1 = fibonacci( n - 1);
        boost::fibers::future< int > f2 = fibonacci( n - 2);
        return f1.get() + f2.get();
    }
}

boost::fibers::future< int > fibonacci( int n) {
    boost::fibers::packaged_task< int() > pt( std::bind( fibonacci_, n) );
    boost::fibers::future< int > f( pt.get_future() );
    boost::fibers::fiber( boost::fibers::launch::dispatch, std::move( pt) ).detach();
    return f;
}

void thread( unsigned int max_idx, unsigned int idx, barrier * b) {
    boost::fibers::use_scheduling_algorithm< boost::fibers::algo::random_chase_lev >( max_idx, idx);
    b->wait();
    while ( ! fini) {
        // To guarantee progress, we must ensure that
        // threads that have work to do are not unreasonably delayed by (thief) threads
        // which are idle except for task-stealing.
        // This call yields the thief ’s processor to another thread, allowing
        // descheduled threads to regain a processor and make progress.
        std::this_thread::yield();
        boost::this_fiber::yield();
    }
}

int main() {
    unsigned int max_idx = 8;
    boost::fibers::use_scheduling_algorithm< boost::fibers::algo::random_chase_lev >( max_idx, max_idx);
    std::vector< std::thread > threads;
    barrier b( max_idx + 1);
    // launch a couple threads to help process them
    for ( unsigned int idx = 0; idx < max_idx; ++idx) {
        threads.push_back( std::thread( thread, max_idx, idx, & b) );
    };
    b.wait();
    int n = 30;
    // main fiber computes fibonacci( n)
    // wait on result
    int result = fibonacci( n).get();
//    BOOST_ASSERT( 55 == result);
    std::ostringstream buffer;
    buffer << "fibonacci(" << n << ") = " << result << '\n';
    std::cout << buffer.str() << std::flush;
    // set termination flag
    fini = true;
    // wait for threads to terminate
    for ( std::thread & t : threads) {
        t.join();
    }
    std::cout << "done." << std::endl;
    return EXIT_SUCCESS;
}


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/
#include <qle/pricingengines/crossccyswapengine.hpp>
#ifdef QL_USE_INDEXED_COUPON
#include <ql/cashflows/floatingratecoupon.hpp>
#endif

#include <qle/termstructures/crossccybasismtmresetswaphelper.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

namespace {
void no_deletion(YieldTermStructure*) {}
} // namespace

CrossCcyBasisMtMResetSwapHelper::CrossCcyBasisMtMResetSwapHelper(
    const Handle<Quote>& spreadQuote, const Handle<Quote>& spotFX, Natural settlementDays,
    const Calendar& settlementCalendar, const Period& swapTenor, BusinessDayConvention rollConvention,
    const boost::shared_ptr<QuantLib::IborIndex>& foreignCcyIndex,
    const boost::shared_ptr<QuantLib::IborIndex>& domesticCcyIndex,
    const Handle<YieldTermStructure>& foreignCcyDiscountCurve,
    const Handle<YieldTermStructure>& domesticCcyDiscountCurve,
    const Handle<YieldTermStructure>& foreignCcyFxFwdRateCurve,
    const Handle<YieldTermStructure>& domesticCcyFxFwdRateCurve, bool eom, bool spreadOnForeignCcy, bool invertFxIndex,
    boost::optional<Period> foreignTenor, boost::optional<Period> domesticTenor)
    : RelativeDateRateHelper(spreadQuote), spotFX_(spotFX), settlementDays_(settlementDays),
      settlementCalendar_(settlementCalendar), swapTenor_(swapTenor), rollConvention_(rollConvention),
      foreignCcyIndex_(foreignCcyIndex), domesticCcyIndex_(domesticCcyIndex),
      foreignCcyDiscountCurve_(foreignCcyDiscountCurve), domesticCcyDiscountCurve_(domesticCcyDiscountCurve),
      foreignCcyFxFwdRateCurve_(foreignCcyFxFwdRateCurve), domesticCcyFxFwdRateCurve_(domesticCcyFxFwdRateCurve),
      eom_(eom), spreadOnForeignCcy_(spreadOnForeignCcy), invertFxIndex_(invertFxIndex),
      foreignTenor_(foreignTenor ? *foreignTenor : foreignCcyIndex_->tenor()),
      domesticTenor_(domesticTenor ? *domesticTenor : domesticCcyIndex_->tenor()) {

    foreignCurrency_ = foreignCcyIndex_->currency();
    domesticCurrency_ = domesticCcyIndex_->currency();
    QL_REQUIRE(foreignCurrency_ != domesticCurrency_,
               "matching currencies not allowed on CrossCcyBasisMtMResetSwapHelper");

    bool foreignIndexHasCurve = !foreignCcyIndex_->forwardingTermStructure().empty();
    bool domesticIndexHasCurve = !domesticCcyIndex_->forwardingTermStructure().empty();
    bool haveForeignDiscountCurve = !foreignCcyDiscountCurve_.empty();
    bool haveDomesticDiscountCurve = !domesticCcyDiscountCurve_.empty();

    QL_REQUIRE(
        !(foreignIndexHasCurve && domesticIndexHasCurve && haveForeignDiscountCurve && haveDomesticDiscountCurve),
        "CrossCcyBasisMtMResetSwapHelper - Have all curves, nothing to solve for.");

    /* Link the curve being bootstrapped to the index if the index has
    no projection curve */
    if (foreignIndexHasCurve && haveForeignDiscountCurve) {
        if (!domesticIndexHasCurve) {
            domesticCcyIndex_ = domesticCcyIndex_->clone(termStructureHandle_);
            domesticCcyIndex_->unregisterWith(termStructureHandle_);
        }
        // if we have both index and discounting curve on foreign leg,
        // check foreignCcyFxFwdRateCurve and link it to foreign discount curve if empty
        // (we are bootstrapping on domestic leg in this instance, so foreign leg needs to be fully determined
        if (foreignCcyFxFwdRateCurve_.empty())
            foreignCcyFxFwdRateCurve_ = foreignCcyDiscountCurve_;
    } else if (domesticIndexHasCurve && haveDomesticDiscountCurve) {
        if (!foreignIndexHasCurve) {
            foreignCcyIndex_ = foreignCcyIndex_->clone(termStructureHandle_);
            foreignCcyIndex_->unregisterWith(termStructureHandle_);
        }
        // if we have both index and discounting curve on domestic leg,
        // check domesticCcyFxFwdRateCurve and link it to domestic discount curve if empty
        // (we are bootstrapping on foreign leg in this instance, so domestic leg needs to be fully determined
        if (domesticCcyFxFwdRateCurve_.empty())
            domesticCcyFxFwdRateCurve_ = domesticCcyDiscountCurve_;
    } else {
        QL_FAIL("Need one leg of the cross currency basis swap to "
                "have all of its curves.");
    }

    registerWith(spotFX_);
    registerWith(domesticCcyIndex_);
    registerWith(foreignCcyIndex_);
    registerWith(foreignCcyDiscountCurve_);
    registerWith(domesticCcyDiscountCurve_);
    registerWith(foreignCcyFxFwdRateCurve_);
    registerWith(domesticCcyFxFwdRateCurve_);

    initializeDates();
}

void CrossCcyBasisMtMResetSwapHelper::initializeDates() {

    Date refDate = evaluationDate_;
    // if the evaluation date is not a business day
    // then move to the next business day
    refDate = settlementCalendar_.adjust(refDate);

    Date settlementDate = settlementCalendar_.advance(refDate, settlementDays_, Days);
    Date maturityDate = settlementDate + swapTenor_;

    Schedule foreignLegSchedule = MakeSchedule()
                                      .from(settlementDate)
                                      .to(maturityDate)
                                      .withTenor(foreignTenor_)
                                      .withCalendar(settlementCalendar_)
                                      .withConvention(rollConvention_)
                                      .endOfMonth(eom_);

    Schedule domesticLegSchedule = MakeSchedule()
                                       .from(settlementDate)
                                       .to(maturityDate)
                                       .withTenor(domesticTenor_)
                                       .withCalendar(settlementCalendar_)
                                       .withConvention(rollConvention_)
                                       .endOfMonth(eom_);

    Real foreignNominal = 1.0;
    // build an FX index for forward rate projection (TODO - review settlement and calendar)
    boost::shared_ptr<FxIndex> fxIdx = boost::make_shared<FxIndex>(
        "dummy", settlementDays_, foreignCurrency_, domesticCurrency_, settlementCalendar_, spotFX_,
        foreignCcyFxFwdRateCurveRLH_, domesticCcyFxFwdRateCurveRLH_, invertFxIndex_);

    swap_ = boost::shared_ptr<CrossCcyBasisMtMResetSwap>(
        new CrossCcyBasisMtMResetSwap(foreignNominal, foreignCurrency_, foreignLegSchedule, foreignCcyIndex_, 0.0,
                                      domesticCurrency_, domesticLegSchedule, domesticCcyIndex_, 0.0, fxIdx));

    boost::shared_ptr<PricingEngine> engine;
    if (invertFxIndex_) {
        engine.reset(new CrossCcySwapEngine(foreignCurrency_, foreignDiscountRLH_, domesticCurrency_,
                                            domesticDiscountRLH_, spotFX_));
    } else {
        engine.reset(new CrossCcySwapEngine(domesticCurrency_, domesticDiscountRLH_, foreignCurrency_,
                                            foreignDiscountRLH_, spotFX_));
    }
    swap_->setPricingEngine(engine);

    earliestDate_ = swap_->startDate();
    latestDate_ = swap_->maturityDate();

/* May need to adjust latestDate_ if you are projecting libor based
   on tenor length rather than from accrual date to accrual date. */
#ifdef QL_USE_INDEXED_COUPON
    if (termStructureHandle_ == foreignCcyIndex_->forwardingTermStructure()) {
        Size numCashflows = swap_->leg(0).size();
        Date endDate = latestDate_;
        if (numCashflows > 0) {
            for (Size i = numCashflows - 1; i >= 0; i--) {
                boost::shared_ptr<FloatingRateCoupon> lastFloating =
                    boost::dynamic_pointer_cast<FloatingRateCoupon>(swap_->leg(0)[i]);
                if (!lastFloating)
                    continue;
                else {
                    Date fixingValueDate = foreignCcyIndex_->valueDate(lastFloating->fixingDate());
                    endDate = domesticCcyIndex_->maturityDate(fixingValueDate);
                    Date endValueDate = foreignCcyIndex_->maturityDate(fixingValueDate);
                    latestDate_ = std::max(latestDate_, endValueDate);
                    break;
                }
            }
        }
    }
    if (termStructureHandle_ == domesticCcyIndex_->forwardingTermStructure()) {
        Size numCashflows = swap_->leg(1).size();
        Date endDate = latestDate_;
        if (numCashflows > 0) {
            for (Size i = numCashflows - 1; i >= 0; i--) {
                boost::shared_ptr<FloatingRateCoupon> lastFloating =
                    boost::dynamic_pointer_cast<FloatingRateCoupon>(swap_->leg(1)[i]);
                if (!lastFloating)
                    continue;
                else {
                    Date fixingValueDate = domesticCcyIndex_->valueDate(lastFloating->fixingDate());
                    endDate = domesticCcyIndex_->maturityDate(fixingValueDate);
                    Date endValueDate = domesticCcyIndex_->maturityDate(fixingValueDate);
                    latestDate_ = std::max(latestDate_, endValueDate);
                    break;
                }
            }
        }
    }
#endif
}

void CrossCcyBasisMtMResetSwapHelper::setTermStructure(YieldTermStructure* t) {

    bool observer = false;
    boost::shared_ptr<YieldTermStructure> temp(t, no_deletion);

    termStructureHandle_.linkTo(temp, observer);

    if (foreignCcyDiscountCurve_.empty())
        foreignDiscountRLH_.linkTo(temp, observer);
    else
        foreignDiscountRLH_.linkTo(*foreignCcyDiscountCurve_, observer);

    if (domesticCcyDiscountCurve_.empty())
        domesticDiscountRLH_.linkTo(temp, observer);
    else
        domesticDiscountRLH_.linkTo(*domesticCcyDiscountCurve_, observer);

    // the below are the curves used for FX forward rate projection (for the resetting cashflows)
    if (foreignCcyFxFwdRateCurve_.empty())
        foreignCcyFxFwdRateCurveRLH_.linkTo(temp, observer);
    else
        foreignCcyFxFwdRateCurveRLH_.linkTo(*foreignCcyFxFwdRateCurve_, observer);

    if (domesticCcyFxFwdRateCurve_.empty())
        domesticCcyFxFwdRateCurveRLH_.linkTo(temp, observer);
    else
        domesticCcyFxFwdRateCurveRLH_.linkTo(*domesticCcyFxFwdRateCurve_, observer);

    RelativeDateRateHelper::setTermStructure(t);
}

Real CrossCcyBasisMtMResetSwapHelper::impliedQuote() const {
    QL_REQUIRE(termStructure_, "Term structure needs to be set");
    swap_->recalculate();
    if (spreadOnForeignCcy_)
        return swap_->fairForeignSpread();
    else
        return swap_->fairDomesticSpread();
}

void CrossCcyBasisMtMResetSwapHelper::accept(AcyclicVisitor& v) {
    Visitor<CrossCcyBasisMtMResetSwapHelper>* v1 = dynamic_cast<Visitor<CrossCcyBasisMtMResetSwapHelper>*>(&v);
    if (v1)
        v1->visit(*this);
    else
        RateHelper::accept(v);
}
} // namespace QuantExt


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/at.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/detail/variadic/at.hpp>
#include <boost/hana/detail/variadic/drop_into.hpp>
#include <boost/hana/detail/variadic/take.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/functional/id.hpp>
#include <boost/hana/functional/on.hpp>
#include <boost/hana/fwd/append.hpp>
#include <boost/hana/fwd/at.hpp>
#include <boost/hana/fwd/concat.hpp>
#include <boost/hana/fwd/concept/sequence.hpp>
#include <boost/hana/fwd/core/make.hpp>
#include <boost/hana/fwd/drop_front.hpp>
#include <boost/hana/fwd/empty.hpp>
#include <boost/hana/fwd/front.hpp>
#include <boost/hana/fwd/prepend.hpp>
#include <boost/hana/fwd/take_front.hpp>
#include <boost/hana/fwd/transform.hpp>
#include <boost/hana/fwd/unpack.hpp>
#include <boost/hana/fwd/zip_shortest_with.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/is_empty.hpp>
#include <boost/hana/length.hpp>
#include <boost/hana/min.hpp>
#include <boost/hana/minimum.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/unpack.hpp>

#include <utility>
namespace hana = boost::hana;

// An interesting way of implementing tuple using lambda captures.

struct lambda_tuple_tag { };

template <typename Storage>
struct lambda_tuple_t {
    explicit constexpr lambda_tuple_t(Storage&& s)
        : storage(std::move(s))
    { }

    using hana_tag = lambda_tuple_tag;
    Storage storage;
};

auto lambda_tuple = [](auto ...xs) {
    auto storage = [=](auto f) -> decltype(auto) { return f(xs...); };
    return lambda_tuple_t<decltype(storage)>{std::move(storage)};
};

namespace boost { namespace hana {
    //////////////////////////////////////////////////////////////////////////
    // Foldable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct unpack_impl<lambda_tuple_tag> {
        template <typename Xs, typename F>
        static constexpr decltype(auto) apply(Xs&& xs, F&& f) {
            return static_cast<Xs&&>(xs).storage(static_cast<F&&>(f));
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Functor
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct transform_impl<lambda_tuple_tag> {
        template <typename Xs, typename F>
        static constexpr decltype(auto) apply(Xs&& xs, F f) {
            return static_cast<Xs&&>(xs).storage(
                [f(std::move(f))](auto&& ...xs) -> decltype(auto) {
                    return lambda_tuple(f(static_cast<decltype(xs)&&>(xs))...);
                }
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Iterable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct front_impl<lambda_tuple_tag> {
        template <typename Xs>
        static constexpr decltype(auto) apply(Xs&& xs) {
            return static_cast<Xs&&>(xs).storage(
                [](auto&& x, auto&& ...) -> decltype(auto) {
                    return id(static_cast<decltype(x)&&>(x));
                }
            );
        }
    };

    template <>
    struct is_empty_impl<lambda_tuple_tag> {
        template <typename Xs>
        static constexpr decltype(auto) apply(Xs&& xs) {
            return static_cast<Xs&&>(xs).storage(
                [](auto const& ...xs) -> decltype(auto) {
                    return hana::bool_c<sizeof...(xs) == 0>;
                }
            );
        }
    };

    template <>
    struct at_impl<lambda_tuple_tag> {
        template <typename Xs, typename Index>
        static constexpr decltype(auto) apply(Xs&& xs, Index const&) {
            return static_cast<Xs&&>(xs).storage(
                detail::variadic::at<Index::value>
            );
        }
    };

    template <>
    struct drop_front_impl<lambda_tuple_tag> {
        template <typename Xs, typename N>
        static constexpr decltype(auto) apply(Xs&& xs, N const& n) {
            auto m = min(n, length(xs));
            return static_cast<Xs&&>(xs).storage(
                detail::variadic::drop_into<hana::value(m)>(lambda_tuple)
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // MonadPlus
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct concat_impl<lambda_tuple_tag> {
        template <typename Xs, typename Ys>
        static constexpr decltype(auto) apply(Xs&& xs, Ys&& ys) {
            return static_cast<Xs&&>(xs).storage(
                [ys(static_cast<Ys&&>(ys))](auto&& ...xs) -> decltype(auto) {
                    return std::move(ys).storage(
                        // We can't initialize the capture with perfect
                        // forwarding since that's not supported by the
                        // language.
                        [=](auto&& ...ys) -> decltype(auto) {
                            return lambda_tuple(
                                std::move(xs)...,
                                static_cast<decltype(ys)&&>(ys)...
                            );
                        }
                    );
                }
            );
        }
    };

    template <>
    struct prepend_impl<lambda_tuple_tag> {
        template <typename Xs, typename X>
        static constexpr decltype(auto) apply(Xs&& xs, X&& x) {
            return static_cast<Xs&&>(xs).storage(
                [x(static_cast<X&&>(x))](auto&& ...xs) -> decltype(auto) {
                    return lambda_tuple(
                        std::move(x),
                        static_cast<decltype(xs)&&>(xs)...
                    );
                }
            );
        }
    };

    template <>
    struct append_impl<lambda_tuple_tag> {
        template <typename Xs, typename X>
        static constexpr decltype(auto) apply(Xs&& xs, X&& x) {
            return static_cast<Xs&&>(xs).storage(
                [x(static_cast<X&&>(x))](auto&& ...xs) -> decltype(auto) {
                    return lambda_tuple(
                        static_cast<decltype(xs)&&>(xs)...,
                        std::move(x)
                    );
                }
            );
        }
    };

    template <>
    struct empty_impl<lambda_tuple_tag> {
        static BOOST_HANA_CONSTEXPR_LAMBDA decltype(auto) apply() {
            return lambda_tuple();
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Sequence
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct Sequence<lambda_tuple_tag> {
        static constexpr bool value = true;
    };

    template <>
    struct take_front_impl<lambda_tuple_tag> {
        template <typename Xs, typename N>
        static constexpr decltype(auto) apply(Xs&& xs, N const& n) {
            auto m = min(n, length(xs));
            return static_cast<Xs&&>(xs).storage(
                detail::variadic::take<decltype(m)::value>
            )(lambda_tuple);
        }
    };

    template <>
    struct zip_shortest_with_impl<lambda_tuple_tag> {
        template <typename F, typename ...Xss>
        static constexpr auto apply(F f, Xss ...tuples) {
            auto go = [=](auto index, auto ...nothing) {
                return always(f)(nothing...)(at(tuples, index)...);
            };
            auto zip_length = minimum(lambda_tuple(length(tuples)...));
            return unpack(make_range(size_c<0>, zip_length),
                on(lambda_tuple, go)
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // make
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct make_impl<lambda_tuple_tag> {
        template <typename ...Xs>
        static constexpr decltype(auto) apply(Xs&& ...xs) {
            return lambda_tuple(static_cast<Xs&&>(xs)...);
        }
    };
}} // end namespace boost::hana

int main() {
    auto xs = lambda_tuple(1, '2', 3.3);
    static_assert(!decltype(hana::is_empty(xs))::value, "");
}


/*

Copyright (c) 2005-2018, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

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

*/

#ifndef _LINEARSYSTEM_HPP_
#define _LINEARSYSTEM_HPP_

#include "ChasteSerialization.hpp"
#include "UblasCustomFunctions.hpp" // needs to be 'first'

#include "PetscTools.hpp"
#include "PetscVecTools.hpp"
#include "PetscMatTools.hpp"
#include "OutputFileHandler.hpp"
#include "PCBlockDiagonal.hpp"
#include "PCLDUFactorisation.hpp"
#include "PCTwoLevelsBlockDiagonal.hpp"
#include "ArchiveLocationInfo.hpp"
//#include <boost/serialization/shared_ptr.hpp>

#include <petscvec.h>
#include <petscmat.h>
#include <petscksp.h>
#include <petscviewer.h>

#include <string>
#include <cassert>

/**
 * Linear System class. Stores and solves a linear equation of the form Ax=b,
 * where A is a square matrix and x and b are column vectors.
 * The class uses PETSc.
 */
class LinearSystem
{
    friend class TestLinearSystem;
    friend class TestPCBlockDiagonal;
    friend class TestPCTwoLevelsBlockDiagonal;
    friend class TestPCLDUFactorisation;
    friend class TestChebyshevIteration;

private:

    Mat mLhsMatrix;  /**< The left-hand side matrix. */
    Mat mPrecondMatrix; /**< The matrix used for preconditioning. */
    Vec mRhsVector;  /**< The right-hand side vector. */
    PetscInt mSize;  /**< The size of the linear system. */

    /**
     * \todo Verify claim that ownership range for Vec and Mat is same.
     * This should only matter for efficiency if the claim is false.
     */
    PetscInt mOwnershipRangeLo; /**< For parallel code.  Stores lowest index of vectors and lowest row of matrix stored locally. */
    PetscInt mOwnershipRangeHi; /**< Stores <b>one more than</b> the highest index stored locally. */

    MatNullSpace mMatNullSpace; /**< PETSc null matrix. */

    /** Whether we need to destroy the PETSc matrix and vector in our destructor */
    bool mDestroyMatAndVec;

    KSP mKspSolver;   /**< The PETSc linear solver object */
    bool mKspIsSetup; /**< Used by Solve method to track whether KSP has been used. */
    double mNonZerosUsed;  /**< Yes, it really is stored as a double. */
    bool mMatrixIsConstant; /**< Whether the matrix is unchanged each time Solve() is called */
    double mTolerance; /**< absolute or relative tolerance of the KSP solver */
    /**
     * Sets either absolute or relative tolerance of the KSP solver.
     * Default is to false
     */
    bool mUseAbsoluteTolerance;
    std::string mKspType;/**< KSP solver type (see PETSc KSPSetType() ) */
    std::string mPcType;/**< Preconditioner type (see PETSc PCSetType() ) */

    Vec mDirichletBoundaryConditionsVector; /**< Storage for efficient application of Dirichlet BCs, see AbstractBoundaryConditionsContainer */

    /// \todo: #1082 Create an abstract class for the preconditioners and use a single pointer
    /** Stores a pointer to a purpose-build preconditioner*/
    PCBlockDiagonal* mpBlockDiagonalPC;
    /** Stores a pointer to a purpose-build preconditioner*/
    PCLDUFactorisation* mpLDUFactorisationPC;
    /** Stores a pointer to a purpose-build preconditioner*/
    PCTwoLevelsBlockDiagonal* mpTwoLevelsBlockDiagonalPC;

    /** Pointer to vector containing a list of bath nodes*/
    boost::shared_ptr<std::vector<PetscInt> > mpBathNodes;

    /** Whether the matrix used for preconditioning is the same as the LHS*/
    bool mPrecondMatrixIsNotLhs;

    /** The max number of nonzero entries expected on a LHS row */
    unsigned mRowPreallocation;

    /** Whether to use fixed number of iterations */
    bool mUseFixedNumberIterations;

    /**
     * When using fixed number of iterations, a solve with residual-based
     * stop criteria will be performed every mEvaluateNumItsEveryNSolves solves
     * to decide how many iterations perform for the next mEvaluateNumItsEveryNSolves-1 solves
     */
    unsigned mEvaluateNumItsEveryNSolves;

    /**  Context for KSPDefaultConverged() */
    void* mpConvergenceTestContext;

    /** Number of solves performed since the current object was created */
    unsigned mNumSolves;

    /** Preconditioned operator smallest eigenvalue */
    PetscReal mEigMin;

    /** Preconditioned operator largest eigenvalue */
    PetscReal mEigMax;

    /** Under certain circunstances you have to reevaluate the spectrum before the k*n-th, k=0,1,..., iteration*/
    bool mForceSpectrumReevaluation;

#ifdef TRACE_KSP
    unsigned mTotalNumIterations;
    unsigned mMaxNumIterations;
#endif
    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the member variables.
     *
     * @param archive
     * @param version
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        //MatNullSpace mMatNullSpace; // Gets re-created by calling code on load

        archive & mNonZerosUsed;
        archive & mMatrixIsConstant;
        archive & mTolerance;
        archive & mUseAbsoluteTolerance;
        archive & mKspType;
        archive & mPcType;

        //Vec mDirichletBoundaryConditionsVector; // Gets re-created by calling code on load
    }

public:

    /**
     * Constructor.
     *
     * @param lhsVectorSize  the size of the LHS vector
     * @param rowPreallocation the max number of nonzero entries expected on a row
     *  - a value of 0 is allowed: no preallocation is then done and the user must
     *    preallocate the memory for the matrix themselves.
     *  - the default value allows for small size systems to be set as dense matrices
     *    automatically.
     */
    LinearSystem(PetscInt lhsVectorSize, unsigned rowPreallocation=UINT_MAX);

    /**
     * Alternative constructor.
     *
     * Create a linear system, where the size is based on the size of a given
     * PETSc vec.
     *
     * The LHS & RHS vectors will be created by duplicating this vector's
     * settings.  This should avoid problems with using VecScatter on
     * bidomain simulation results.
     *
     * @param templateVector  a PETSc vec
     * @param rowPreallocation the max number of nonzero entries expected on a row
     * @param newAllocationError tells PETSc whether to set the MAT_NEW_NONZERO_ALLOCATION_ERR.
     *        ** currently only used in PETSc 3.3 and later **
     *        in PETSc 3.2 and earlier MAT_NEW_NONZERO_ALLOCATION_ERR defaults to false
     *        in PETSc 3.3 MAT_NEW_NONZERO_ALLOCATION_ERR defaults to true
     */
    LinearSystem(Vec templateVector, unsigned rowPreallocation, bool newAllocationError=true);

    /**
     * Alternative constructor.
     *
     * Create a linear system which wraps the provided PETSc objects so we can
     * access them using our API.  Either of the objects may be NULL, but at
     * least one of them must not be.
     *
     * Useful for storing residuals and jacobians when solving nonlinear PDEs.
     *
     * @param residualVector  the residual vector
     * @param jacobianMatrix  the Jacobian matrix
     */
    LinearSystem(Vec residualVector, Mat jacobianMatrix);

    /**
     * Alternative constructor for archiving.
     *
     * @param lhsVectorSize  the size of the LHS vector
     * @param lhsMatrix  the RHS matrix
     * @param rhsVector  the RHS vector
     */
    LinearSystem(PetscInt lhsVectorSize, Mat lhsMatrix, Vec rhsVector);

    /**
     * Destructor.
     */
    ~LinearSystem();

//    bool IsMatrixEqualTo(Mat testMatrix);
//    bool IsRhsVectorEqualTo(Vec testVector);

    /**
     * Change one of the entires of the matrix to the specified value.
     *
     * @param row  the row index
     * @param col  the column index
     * @param value  the value for this entry
     */
    void SetMatrixElement(PetscInt row, PetscInt col, double value);

    /**
     * Add the specified value to an entry of the matrix.
     *
     * @param row  the row index
     * @param col  the column index
     * @param value  the value for this entry
     */
    void AddToMatrixElement(PetscInt row, PetscInt col, double value);

    /**
     * Call this before Solve().
     *
     * This calls FinaliseLhsMatrix() and FinaliseRhsVector().
     */
    void AssembleFinalLinearSystem();

    /**
     * Should be called before AddToMatrixElement.
     *
     * This calls SwitchWriteModeLhsMatrix() and FinaliseRhsVector().
     */
    void AssembleIntermediateLinearSystem();

    /**
     * Sets up the PETSc matrix left-hand-side mLhsMatrix
     */
    void FinaliseLhsMatrix();

    /**
     * Sets up the PETSc matrix used for preconditioning.
     */
    void FinalisePrecondMatrix();

    /**
     * Sets up the PETSc matrix left-hand-side mLhsMatrix
     */
    void SwitchWriteModeLhsMatrix();

    /**
     * Sets up the PETSc vector right-hand-side mRhsVector
     */
    void FinaliseRhsVector();

    /**
     * Force PETSc to treat the matrix in this linear system as symmetric from now on.
     *
     * @param isSymmetric  whether the matrix is symmetric or not (defaults to true)
     */
    void SetMatrixIsSymmetric(bool isSymmetric=true);

    /**
     * Get whether PETSc considers the matrix in this linear system as symmetric or not.
     *
     * @return whether the matrix is symmetric or not.
     */
    bool IsMatrixSymmetric();

    /**
     * Set mMatrixIsConstant.
     *
     * @param matrixIsConstant  whether the matrix is constant
     */
    void SetMatrixIsConstant(bool matrixIsConstant);

    /**
     * Set the relative tolerance.
     *
     * @param relativeTolerance  the relative tolerance
     */
    void SetRelativeTolerance(double relativeTolerance);

    /**
     * Set the absolute tolerance.
     *
     * @param absoluteTolerance  the absolute tolerance
     */
    void SetAbsoluteTolerance(double absoluteTolerance);

    /**
     * Set the KSP solver type (see PETSc KSPSetType() for valid arguments).
     *
     * @param kspType  the KSP solver type
     */
    void SetKspType(const char* kspType);

    /**
     * Set the preconditioner type  (see PETSc PCSetType() for valid arguments).
     *
     * @param pcType the preconditioner type
     * @param pBathNodes the list of nodes defining the bath
     */
    /// \todo: #1082 is this the way of defining a null pointer as the default value of pBathNodes?
    void SetPcType(const char* pcType, boost::shared_ptr<std::vector<PetscInt> > pBathNodes=boost::shared_ptr<std::vector<PetscInt> >() );

    /**
     * Display the left-hand side matrix.
     */
    void DisplayMatrix();

    /**
     * Display the right-hand side vector.
     */
    void DisplayRhs();

    /**
     * Set all entries in a given row of a matrix to a certain value.
     * This must be called by the process who owns the row, (but other
     * processors will treat it as a null-op)
     *
     * @param row  the row index
     * @param value  the value to set each entry in this row
     */
    void SetMatrixRow(PetscInt row, double value);

    /**
     * Returns the i-th row of the LHS matrix as a distributed PETSc Vec
     *
     * @param rowIndex the row index
     * @return rowIndex-th row of the matrix in distributed format
     */
    Vec GetMatrixRowDistributed(unsigned rowIndex);

    /**
     * Zero several rows of the matrix, putting a given value in the diagonal entries.
     *
     * *Massively* less expensive than zeroing each matrix row individually
     *
     * @param rRows std::vector of rows to be zeroed
     * @param diagonalValue value to put in the diagonal entries (of the zeroed rows)
     */
    void ZeroMatrixRowsWithValueOnDiagonal(std::vector<unsigned>& rRows, double diagonalValue);

    /**
     * Zero several rows and columns of the matrix, putting a given value on the diagonal.
     *
     * @param rRowColIndices A list of indices. All the rows with these indices, and all the columns
     * with these indices, will be zeroed.
     * @param diagonalValue value to put in the diagonal entries (of the zeroed rows)
     */
    void ZeroMatrixRowsAndColumnsWithValueOnDiagonal(std::vector<unsigned>& rRowColIndices, double diagonalValue);

    /**
     * Zero a column of the left-hand side matrix.
     *
     * Unfortunately there is no equivalent method in Petsc, so this has to be
     * done carefully to ensure that the sparsity structure of the matrix
     * is not broken. Only owned entries which are non-zero are zeroed.
     *
     * @param col  the column index
     */
    void ZeroMatrixColumn(PetscInt col);

    /**
     * Zero all entries of the left-hand side matrix.
     */
    void ZeroLhsMatrix();

    /**
     * Zero all entries of the right-hand side vector.
     */
    void ZeroRhsVector();

    /**
     * Zero all entries of the left-hand side matrix and right-hand side vector.
     */
    void ZeroLinearSystem();

    /**
     * Solve the linear system.
     * @return the solution vector
     * @param lhsGuess  an optional initial guess for the solution (defaults to NULL)
     */
    Vec Solve(Vec lhsGuess=nullptr);

    /**
     * Set an element of the right-hand side vector to a given value.
     *
     * @param row  the row index
     * @param value  the value to set this entry
     */
    void SetRhsVectorElement(PetscInt row, double value);

    /**
     * Add a value to an element of the right-hand side vector.
     *
     * @param row  the row index
     * @param value  the value to set this entry
     */
    void AddToRhsVectorElement(PetscInt row, double value);

    /**
     * @return #mSize.
     */
    unsigned GetSize() const;

    /**
     * Set the null basis of the linear system.
     * In debug mode we test for orthonormality and throw EXCEPTIONs:
     *  - each direction is of unit length (to within a tolerance)
     *  - the directions are mutually orthogonal (to within a tolerance)
     * Note that in NDEBUG (optimised mode) these tests are skipped and the parameters
     * are passed directly into the LinearSystem.
     *
     * @param nullbasis  an array PETSc vectors containing orthogonal directions in the nullspace
     * @param numberOfBases the number of directions (size of nullbasis array)
     */
    void SetNullBasis(Vec nullbasis[], unsigned numberOfBases);

    /**
     * Remove the null space from the linear system.
     *
     * Use for example if Dirichlet BC are applied to a singular system and, therefore, there's
     * no null space anymore.
     */
    void RemoveNullSpace();

    /**
     * @return access to the RHS vector directly. Shouldn't generally need to be called.
     */
    Vec& rGetRhsVector();

    /**
     * @return access to the RHS vector for archiving
     */
    Vec GetRhsVector() const;

    /**
     * @return access to the LHS matrix directly. Shouldn't generally need to be called.
     */
    Mat& rGetLhsMatrix();

    /**
     * @return access to the LHS matrix for archiving
     */
    Mat GetLhsMatrix() const;

    /**
     * @return access to the matrix used for preconditioning.
     */
    Mat& rGetPrecondMatrix();

    /**
     * @return access to the dirichlet boundary conditions vector.
     *
     * Should only be used by the BoundaryConditionsContainer.
     */
    Vec& rGetDirichletBoundaryConditionsVector();

    // DEBUGGING CODE:
    /**
     * @return this process's ownership range of the contents of the system.
     *
     * @param lo  lowest index owned by this process
     * @param hi  highest index owned by this process
     */
    void GetOwnershipRange(PetscInt& lo, PetscInt& hi);

    /**
     * @return an element of the matrix.
     * May only be called for elements you own.
     *
     * @param row  the row index
     * @param col  the column index
     */
    double GetMatrixElement(PetscInt row, PetscInt col);

    /**
     * @return an element of the RHS vector.
     * May only be called for elements you own.
     *
     * @param row  the row index
     */
    double GetRhsVectorElement(PetscInt row);

    /**
     * @return the number of iterations taken by the last Solve()
     */
    unsigned GetNumIterations() const;

    /**
     * Add multiple values to the matrix of linear system.
     *
     * @param matrixRowAndColIndices mapping from index of the ublas matrix (see param below)
     *  to index of the PETSc matrix of this linear system
     * @param rSmallMatrix Ublas matrix containing the values to be added
     *
     * N.B. Values which are not local (ie the row is not owned) will be skipped.
     */
    template<size_t MATRIX_SIZE>
    void AddLhsMultipleValues(unsigned* matrixRowAndColIndices, c_matrix<double, MATRIX_SIZE, MATRIX_SIZE>& rSmallMatrix)
    {
        PetscMatTools::AddMultipleValues(mLhsMatrix, matrixRowAndColIndices, rSmallMatrix);
    }

    /**
     * Add multiple values to the RHS vector.
     *
     * @param vectorIndices mapping from index of the ublas vector (see param below)
     *  to index of the vector of this linear system
     * @param smallVector Ublas vector containing the values to be added
     *
     * N.B. Values which are not local (ie the row is not owned) will be skipped.
     */
    template<size_t VECTOR_SIZE>
    void AddRhsMultipleValues(unsigned* vectorIndices, c_vector<double, VECTOR_SIZE>& smallVector)
    {
        PetscVecTools::AddMultipleValues(mRhsVector, vectorIndices, smallVector);
    }

    /**
     * Set method for #mPrecondMatrixIsNotLhs.
     * @param precondIsDifferent  whether the matrix used for preconditioning is the same as the LHS.
     */
    void SetPrecondMatrixIsDifferentFromLhs(bool precondIsDifferent = true);

    /**
     * Set method for #mUseFixedNumberIterations
     * @param useFixedNumberIterations whether to use fixed number of iterations
     * @param evaluateNumItsEveryNSolves tells LinearSystem to perform a solve with convergence-based stop criteria every n solves to decide how many iterations perform for the next n-1 solves. Default is perfoming a single evaluation at the beginning of the simulation.
     */
    void SetUseFixedNumberIterations(bool useFixedNumberIterations = true, unsigned evaluateNumItsEveryNSolves = UINT_MAX);

    /**
     * Method to regenerate all KSP objects, including the solver and the preconditioner (e.g. after
     * changing the PDE time step when using time adaptivity).
     */
    void ResetKspSolver();
};

#include "SerializationExportWrapper.hpp"
// Declare identifier for the serializer
CHASTE_CLASS_EXPORT(LinearSystem)

namespace boost
{
namespace serialization
{
template<class Archive>
inline void save_construct_data(
    Archive & ar, const LinearSystem * t, const unsigned int file_version)
{

    std::string archive_filename_lhs = ArchiveLocationInfo::GetArchiveDirectory() + "lhs.mat";
    std::string archive_filename_rhs = ArchiveLocationInfo::GetArchiveDirectory() + "rhs.vec";
    const unsigned size = t->GetSize();
    ar << size;

    PetscTools::DumpPetscObject(t->GetRhsVector(), archive_filename_rhs);
    PetscTools::DumpPetscObject(t->GetLhsMatrix(), archive_filename_lhs);

    //Is the matrix structurally symmetric?
    PetscBool symm_set, is_symmetric;
    is_symmetric = PETSC_FALSE;
    //Note that the following call only changes is_symmetric when symm_set is true
    MatIsSymmetricKnown(t->GetLhsMatrix(), &symm_set, &is_symmetric);
    assert(symm_set == is_symmetric);
    ar << symm_set;
}

/**
 * Allow us to not need a default constructor, by specifying how Boost should
 * instantiate an instance (using existing constructor)
 */
template<class Archive>
inline void load_construct_data(
    Archive & ar, LinearSystem * t, const unsigned int file_version)
{
     std::string archive_filename_lhs = ArchiveLocationInfo::GetArchiveDirectory() + "lhs.mat";
     std::string archive_filename_rhs = ArchiveLocationInfo::GetArchiveDirectory() + "rhs.vec";

     PetscInt size;
     ar >> size;

     Vec new_vec;
     PetscTools::ReadPetscObject(new_vec, archive_filename_rhs);

     Mat new_mat;
     PetscTools::ReadPetscObject(new_mat, archive_filename_lhs);

     //This has to occur after the call to MatLoad as the matrix does not exist until MatLoad is called.
     //The property will be copied & set correctly in the LinearSystem constructor.
     PetscBool symm_set;

     ar >> symm_set;
     if (symm_set == PETSC_TRUE)
     {
        PetscMatTools::SetOption(new_mat, MAT_SYMMETRIC);
        PetscMatTools::SetOption(new_mat, MAT_SYMMETRY_ETERNAL);
     }

     ::new(t)LinearSystem(size, new_mat, new_vec);
}
}
} // namespace ...

#endif //_LINEARSYSTEM_HPP_


// Copyright 2018 The SAF Authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <gtest/gtest.h>
#include <boost/algorithm/string/replace.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <caffe/caffe.hpp>
#include <opencv2/opencv.hpp>

#include <assert.h>
#include <iostream>
#include <string>
#include <vector>

#include "camera/camera.h"
#include "common/serialization.h"
#include "common/types.h"
#include "model/model.h"
#include "model/model_manager.h"
#include "operator/neural_net_evaluator.h"
#include "stream/frame.h"
#include "stream/stream.h"

constexpr auto ALPHA = 0.001f;

constexpr auto CHANNELS = 3;
constexpr auto WIDTH = 224;
constexpr auto HEIGHT = 224;
const auto SHAPE = cv::Size(WIDTH, HEIGHT);

constexpr auto INPUT_IMAGE_FILEPATH = "data/input.jpg";
constexpr auto NETWORK_FILEPATH = "data/mobilenet/mobilenet_deploy.prototxt";
constexpr auto WEIGHTS_FILEPATH = "/tmp/mobilenet.caffemodel";

const std::vector<std::string> OUTPUTS = {
    "conv1",       "conv2_1/dw",  "conv2_1/sep", "conv2_2/dw",  "conv2_2/sep",
    "conv3_1/dw",  "conv3_1/sep", "conv3_2/dw",  "conv3_2/sep", "conv4_1/dw",
    "conv4_1/sep", "conv4_2/dw",  "conv4_2/sep", "conv5_1/dw",  "conv5_1/sep",
    "conv5_2/dw",  "conv5_2/sep", "conv5_3/dw",  "conv5_3/sep", "conv5_4/dw",
    "conv5_4/sep", "conv5_5/dw",  "conv5_5/sep", "conv5_6/dw",  "conv5_6/sep",
    "conv6/dw",    "conv6/sep",   "pool6",       "fc7",         "prob"};

bool FloatEqual(float lhs, float rhs) {
  if (lhs < 0) {
    lhs *= -1;
    rhs *= -1;
  }
  return lhs - lhs * ALPHA <= rhs && lhs + lhs * ALPHA >= rhs;
}

void CvMatEqual(cv::Mat lhs, cv::Mat rhs) {
  std::vector<int> cur_idx;
  if (lhs.dims != rhs.dims) {
    return;
  }
  CHECK_EQ(lhs.dims, rhs.dims);
  CHECK_GT(lhs.dims, 0);
  auto lhs_it = lhs.begin<float>();
  auto lhs_end = lhs.end<float>();
  auto rhs_it = rhs.begin<float>();
  auto rhs_end = rhs.end<float>();
  while (lhs_it != lhs_end && rhs_it != rhs_end) {
    CHECK(FloatEqual(*lhs_it, *rhs_it))
        << "Expects: " << *lhs_it << " Found: " << *rhs_it;
    ++lhs_it;
    ++rhs_it;
  }
}

cv::Mat Preprocess(const cv::Mat& img) {
  cv::Size input_geometry_ = cv::Size(WIDTH, HEIGHT);
  // Convert the input image to the input image format of the network.
  cv::Mat sample = img;

  cv::Mat sample_resized;
  if (sample.size() != input_geometry_)
    cv::resize(sample, sample_resized, input_geometry_);
  else
    sample_resized = sample;
  return sample_resized;
}

TEST(TestNneCaffe, TestExtractIntermediateActivationsCaffe) {
  std::ifstream f(WEIGHTS_FILEPATH);
  ASSERT_TRUE(f.good()) << "The Caffe model file \"" << WEIGHTS_FILEPATH
                        << "\" was not found. Download it by executing: "
                        << "curl -o " << WEIGHTS_FILEPATH
                        << " https://raw.githubusercontent.com/cdwat/"
                           "MobileNet-Caffe/master/mobilenet.caffemodel";
  f.close();

  // Construct model
  Shape input_shape(CHANNELS, WIDTH, HEIGHT);
  ModelDesc desc("TestExtractIntermediateActivationsCaffe", MODEL_TYPE_CAFFE,
                 NETWORK_FILEPATH, WEIGHTS_FILEPATH, WIDTH, HEIGHT, "", "prob");
  NeuralNetEvaluator nne(desc, input_shape, 1, OUTPUTS);

  // Configure the mean image
  ModelManager::GetInstance().SetMeanColors(cv::Scalar(104.0, 117.0, 123.0));

  // Read the input
  cv::Mat original_image = cv::imread(INPUT_IMAGE_FILEPATH);
  ASSERT_FALSE(original_image.empty()) << "Image empty. Is a library (i.e. "
                                          "libtensorflow) clobbering libjpeg "
                                          "symbols?";
  cv::Mat preprocessed_image = Preprocess(original_image);

  // Construct frame with input image in it
  auto input_frame = std::make_unique<Frame>();
  input_frame->SetValue(Camera::kCaptureTimeMicrosKey,
                        boost::posix_time::microsec_clock::local_time());
  input_frame->SetValue("frame_id", (unsigned long)0);
  input_frame->SetValue("original_image", original_image);
  input_frame->SetValue("image", preprocessed_image);

  // Prepare stream
  StreamPtr stream = std::make_shared<Stream>();
  nne.SetSource("input", stream, "");

  // We need to set up the StreamReaders before calling Start().
  StreamReader* reader = nne.GetSink()->Subscribe();

  nne.Start();
  stream->PushFrame(std::move(input_frame));
  auto output_frame = reader->PopFrame();
  ASSERT_FALSE(output_frame == nullptr) << "Unable to get frame";
  for (decltype(OUTPUTS.size()) i = 0; i < OUTPUTS.size(); ++i) {
    std::string filename = OUTPUTS.at(i);
    boost::replace_all(filename, "/", ".");
    filename = "data/mobilenet/caffe_ground_truth/" + filename + ".bin";
    // contains the activations of the output layer
    std::ifstream gt_file;
    gt_file.open(filename);
    cv::Mat expected_output;
    boost::archive::binary_iarchive expected_output_archive(gt_file);

    try {
      expected_output_archive >> expected_output;
    } catch (std::exception& e) {
      LOG(INFO) << "Ignoring empty layer\n";
      continue;
    }

    gt_file.close();

    cv::Mat actual_output = output_frame->GetValue<cv::Mat>(OUTPUTS.at(i));
    int num_channel = actual_output.channels();
    int height = actual_output.rows;
    int width = actual_output.cols;
    float* gt_data_ptr = (float*)expected_output.ptr();
    int per_channel_floats = height * width;
    std::vector<cv::Mat> gt_channels;
    for (int i = 0; i < num_channel; ++i) {
      cv::Mat cur_channel(HEIGHT, WIDTH, CV_32F);
      memcpy(cur_channel.data, gt_data_ptr + per_channel_floats * i,
             per_channel_floats * sizeof(float));
      gt_channels.push_back(cur_channel);
    }
    cv::Mat expected_output_transposed;
    cv::merge(gt_channels, expected_output_transposed);
    CvMatEqual(expected_output_transposed, actual_output);
  }
  nne.Stop();
}


#include <boost/foreach.hpp>
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dxtbx/masking/masking.h>
#include <dxtbx/masking/goniometer_shadow_masking.h>

namespace dxtbx { namespace masking { namespace boost_python {

  using scitbx::vec2;
  using scitbx::vec3;

  scitbx::af::shared<bool> is_inside_polygon_a(
    const scitbx::af::const_ref<vec2<double> > &poly,
    const scitbx::af::const_ref<vec2<double> > &xy) {
    scitbx::af::shared<bool> inside(xy.size(), false);
    for (std::size_t i = 0; i < xy.size(); i++) {
      inside[i] = is_inside_polygon(poly, xy[i][0], xy[i][1]);
    }
    return inside;
  }

  static boost::python::list GoniometerShadowMasker_project_extrema(
    GoniometerShadowMasker &masker,
    const Detector &detector,
    double scan_angle) {
    scitbx::af::shared<scitbx::af::shared<scitbx::vec2<double> > > result =
      masker.project_extrema(detector, scan_angle);
    boost::python::list list;
    BOOST_FOREACH (scitbx::af::shared<scitbx::vec2<double> > item, result) {
      list.append(item);
    }
    return list;
  }

  // Copied from dxtbx/boost_python/imageset_ext.cc
  template <typename T>
  boost::python::tuple image_as_tuple(const Image<T> &image) {
    boost::python::list result;
    for (std::size_t i = 0; i < image.n_tiles(); ++i) {
      result.append(image.tile(i).data());
    }
    return boost::python::tuple(result);
  }

  boost::python::tuple GoniometerShadowMasker_get_mask(

    GoniometerShadowMasker &masker,
    const Detector &detector,
    double scan_angle) {
    return image_as_tuple<bool>(masker.get_mask(detector, scan_angle));
  }

  struct GoniometerShadowMaskerPickleSuite : boost::python::pickle_suite {
    static boost::python::tuple getinitargs(const GoniometerShadowMasker &obj) {
      return boost::python::make_tuple(
        obj.goniometer(), obj.extrema_at_datum(), obj.axis());
    }
  };

  struct SmarGonShadowMaskerPickleSuite : boost::python::pickle_suite {
    static boost::python::tuple getinitargs(const SmarGonShadowMasker &obj) {
      return boost::python::make_tuple(obj.goniometer());
    }
  };

  using namespace boost::python;
  BOOST_PYTHON_MODULE(dxtbx_masking_ext) {
    def("mask_untrusted_rectangle", &mask_untrusted_rectangle);

    def("mask_untrusted_circle", &mask_untrusted_circle);

    def("mask_untrusted_resolution_range", &mask_untrusted_resolution_range);

    def("mask_untrusted_polygon", &mask_untrusted_polygon);

    def("is_inside_polygon", &is_inside_polygon);

    def("is_inside_polygon", &is_inside_polygon_a);

    class_<GoniometerShadowMasker>("GoniometerShadowMasker", no_init)
      .def(init<const MultiAxisGoniometer &,
                const scitbx::af::const_ref<scitbx::vec3<double> > &,
                const scitbx::af::const_ref<std::size_t> &,
                optional<bool> >())
      .def("extrema_at_scan_angle", &GoniometerShadowMasker::extrema_at_scan_angle)
      .def("set_goniometer_angles", &GoniometerShadowMasker::set_goniometer_angles)
      .def("project_extrema", GoniometerShadowMasker_project_extrema)
      .def("get_mask", GoniometerShadowMasker_get_mask)
      .def_pickle(GoniometerShadowMaskerPickleSuite());

    class_<SmarGonShadowMasker, bases<GoniometerShadowMasker> >("SmarGonShadowMasker",
                                                                no_init)
      .def(init<const MultiAxisGoniometer &>())
      .def("extrema_at_scan_angle", &SmarGonShadowMasker::extrema_at_scan_angle)
      .def_pickle(SmarGonShadowMaskerPickleSuite());
  }
}}}  // namespace dxtbx::masking::boost_python


#include "datafilewrapper.h"
#include "memorysizeliterals.h"
#include "utils.h"

#include <boost/asio/post.hpp>
#include <boost/pool/pool_alloc.hpp>

#include <filesystem>

namespace Parallel
{
namespace
{
constexpr size_t OptimalDataFrameSize = MB;
} // namespace

DataFileWrapper::DataFileWrapper(const std::string& path, const std::ios_base::openmode mode)
    : path_(path)
    , mode_(mode){};

DataFrameConfigsPtr DataFileWrapper::makeConfigs(const uintmax_t fileSize,
                                                 const size_t dataBlockSize,
                                                 LazyMemoryPoolPtr memoryPool)
{
    assert(dataBlockSize != 0);
    if (fileSize == 0)
        return std::make_shared<DataFrameConfigs>();

    const size_t dataBlocksInFile = ceilDevision(fileSize, dataBlockSize);
    size_t dataBlocksInFrame = ceilDevision(OptimalDataFrameSize, dataBlockSize);
    dataBlocksInFrame = std::min(dataBlocksInFrame, dataBlocksInFile);
    const auto dataFramesInFile = ceilDevision(dataBlocksInFile, dataBlocksInFrame);

    auto result = std::make_shared<DataFrameConfigs>();
    result->reserve(dataFramesInFile);
    for (size_t i = 0; i < dataFramesInFile; i++)
    {
        result->push_back(DataFrameConfig{.firstBlockIdx = i * dataBlocksInFrame,
                                          .blockSize = dataBlockSize,
                                          .blocksCount = dataBlocksInFrame,
                                          .memoryPool = memoryPool});
    }
    return result;
}

void DataFileWrapper::readAllAsDataFrames(ReadAllAsDataFramesParams prms)
{
    assert(futures_.size() == 0 && prms.tasksCount != 0);

    const auto configs = makeConfigs(
        std::filesystem::file_size(path_), prms.dataBlockSize, std::make_shared<LazyMemoryPool>());
    auto currentConfigIndex = makeSharedAtomic<size_t>(0);

    for (size_t i = 0; i < prms.tasksCount; i++)
    {
        std::packaged_task<void()> task([=]() {
            DataFile file(path_, mode_);
            while (true)
            {
                size_t configIdx = (*currentConfigIndex)++;
                if (configIdx >= configs->size())
                    break;
                auto frame = file.readDataBlocksAsFrame(std::move(configs->at(configIdx)));
                prms.dest.waitAndPush(std::move(frame));
            }
        });
        futures_.push_back(task.get_future());
        post(prms.pool, std::move(task));
    }
}

void DataFileWrapper::writeAllDataFrames(WriteAllDataFramesParams prms)
{
    assert(futures_.size() == 0);

    std::packaged_task<void()> writingTask([&, prms]() {
        DataFile file(path_, mode_);

        DataFrame frame;
        while (!prms.hasProducerFinished->load())
        {
            while (prms.src.waitAndPop(frame, std::chrono::milliseconds(100)))
                file.writeDataFrame(frame, prms.writingPosShift);
        }
        while (prms.src.tryPop(frame))
            file.writeDataFrame(frame, prms.writingPosShift);
    });
    futures_.push_back(writingTask.get_future());
    post(prms.pool, std::move(writingTask));
}

void DataFileWrapper::joinAndRethrowExceptions()
{
    for (auto& future : futures_)
        future.get();
    futures_.clear();
}

} // namespace Parallel


//=================================================================================================
// Copyright (c) 2011, Johannes Meyer, TU Darmstadt
// 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 Flight Systems and Automatic Control group,
//       TU Darmstadt, nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
//=================================================================================================

#include <hector_pose_estimation/measurements/magnetic.h>
#include <hector_pose_estimation/filter/set_filter.h>

#include <Eigen/Geometry>

namespace hector_pose_estimation {

template class Measurement_<MagneticModel>;

MagneticModel::MagneticModel()
  : declination_(0.0), inclination_(60.0 * M_PI/180.0), magnitude_(0.0)
{
  parameters().add("stddev", stddev_, 1.0);
  parameters().add("declination", declination_);
  parameters().add("inclination", inclination_);
  parameters().add("magnitude", magnitude_);
}

MagneticModel::~MagneticModel() {}

bool MagneticModel::init(PoseEstimation &estimator, Measurement &measurement, State &state)
{
  updateMagneticField();
  return true;
}

void MagneticModel::setReference(const GlobalReference::Heading &reference_heading) {
  magnetic_field_reference_.x() = reference_heading.cos * magnetic_field_north_.x() - reference_heading.sin * magnetic_field_north_.y();
  magnetic_field_reference_.y() = reference_heading.sin * magnetic_field_north_.x() + reference_heading.cos * magnetic_field_north_.y();
  magnetic_field_reference_.z() = magnetic_field_north_.z();
}

void MagneticModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init)
{
  if (init) {
    R(0,0) = R(1,1) = R(2,2) = pow(stddev_, 2);
  }
}

void MagneticModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
{
  const State::RotationMatrix &R = state.R();
  y_pred = R.transpose() * magnetic_field_reference_;
}

void MagneticModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool)
{
  if (state.orientation()) {
    const State::RotationMatrix &R = state.R();
    state.orientation()->cols(C)(X,Z) = R(0,0) * magnetic_field_reference_.y() - R(1,0) * magnetic_field_reference_.x();
    state.orientation()->cols(C)(Y,Z) = R(0,1) * magnetic_field_reference_.y() - R(1,1) * magnetic_field_reference_.x();
    state.orientation()->cols(C)(Z,Z) = R(0,2) * magnetic_field_reference_.y() - R(1,2) * magnetic_field_reference_.x();
  }
}

double MagneticModel::getMagneticHeading(const State& state, const MeasurementVector &y) const {
  MeasurementVector y_nav;
  y_nav = state.R() * y;
  return atan2(y_nav.y(), y_nav.x()) - state.getYaw();
}

double MagneticModel::getTrueHeading(const State& state, const MeasurementVector &y) const {
  return getMagneticHeading(state, y) + declination_;
}

void MagneticModel::updateMagneticField()
{
  double cos_inclination, sin_inclination;
  sincos(inclination_, &sin_inclination, &cos_inclination);

  double cos_declination, sin_declination;
  sincos(declination_, &sin_declination, &cos_declination);

  // return normalized magnetic field if magnitude is zero
  double magnitude = magnitude_;
  if (magnitude == 0.0) magnitude = 1.0;

  magnetic_field_north_.x() = magnitude * (cos_inclination *   cos_declination);
  magnetic_field_north_.y() = magnitude * (cos_inclination * (-sin_declination));
  magnetic_field_north_.z() = magnitude * (-sin_inclination);
}

Magnetic::Magnetic(const std::string &name)
  : Measurement_<MagneticModel>(name)
  , auto_heading_(true)
  , deviation_(3)
{
  deviation_.setZero();
  parameters().add("auto_heading", auto_heading_);
  parameters().add("deviation", deviation_);
}

void Magnetic::onReset() {
  reference_.reset();
}

const MagneticModel::MeasurementVector& Magnetic::getVector(const Magnetic::Update& update, const State& state) {
  y_ = Measurement_<MagneticModel>::getVector(update, state) + deviation_;
  if (getModel()->hasMagnitude()) return y_;

  double norm = y_.norm();
  if (norm < 1e-5) {
    y_.setZero();
  } else {
    y_ = y_ / norm;
  }
  return y_;
}

//const MagneticModel::NoiseVariance& Magnetic::getVariance(const Magnetic::Update& update, const State& state) {
//  if (getModel()->hasMagnitude()) return Measurement_<MagneticModel>::getVariance(update, state);

//  R_ = Measurement_<MagneticModel>::getVariance(update, state);
//  double norm = Measurement_<MagneticModel>::getVector(update, state).norm();
//  if (norm < 1e-5) {
//    R_ = NoiseVariance(1.0, 1.0, 1.0);
//  } else {
//    R_ =  R_ / (norm*norm);
//  }
//  return R_;
//}

bool Magnetic::prepareUpdate(State &state, const Update &update) {
  // reset reference position if Magnetic has not been updated for a while
  if (timedout()) reference_.reset();

  if (reference_ != GlobalReference::Instance()) {
    reference_ = GlobalReference::Instance();
    if (auto_heading_) reference_->setCurrentHeading(state, getModel()->getTrueHeading(state, update.getVector()));
  }

  getModel()->setReference(reference_->heading());
  return true;
}

} // namespace hector_pose_estimation


#ifndef VARIANT_HPP
#define VARIANT_HPP

#include <map>
#include <cmath>
#include "data/locus.hpp"
#include "data/biology.hpp"
#include <boost/format.hpp>

namespace Anaquin
{
    inline long var2hash(const SequinID &id, Variation type, const Locus &l)
    {
        const auto str = (boost::format("%1%_%2%_%3%_%4%") % id
                                                           % type
                                                           % l.start
                                                           % l.end).str();
        return std::hash<std::string>{}(str);
    }

    enum class Filter
    {
        Pass,
        NotFilted,
    };

    /*
     * This class represents a VCF variant. Not everything in a VCF file is important, only
     * the required fields are included.
     */

    struct Variant
    {
        operator const Locus &() const { return l; }

        inline bool operator<(const Locus &x) const { return l < x; }

        inline bool isSV() const
        {
            return alt == "<DUP>" || alt == "<INV>" || alt == "<INS>" || alt == "<DEL>";
        }

        inline Variation type() const
        {
            if (alt == "<DUP>" || alt[0] == '=')
            {
                return Variation::Duplication;
            }
            else if (alt == "<INV>" || alt[0] == '^')
            {
                return Variation::Inversion;
            }
            else if (alt == "<INS>" || alt[0] == '+')
            {
                return Variation::Insertion;
            }
            else if (alt == "<DEL>" || alt[0] == '-')
            {
                return Variation::Deletion;
            }
            else if (ref.size() == alt.size())
            {
                return Variation::SNP;
            }
            else if (ref.size() > alt.size())
            {
                return Variation::Deletion;
            }
            else
            {
                return Variation::Insertion;
            }
        }

        inline long key() const
        {
            return var2hash(name, type(), l);
        }

        ChrID cID;

        // Eg: GI_005
        SequinID name;

        // The reference position, with the 1st base having position 1
        Locus l;

        Genotype gt;

        // Reference and alternative allele
        Sequence ref, alt;

        Filter filter;

        // Allelle frequency
        Proportion allF = NAN;

        // Quality score
        double qual = NAN;

        // Eg: AD for VCF and REF for VarScan
        Counts readR = NAN;

        // Eg: AD for VCF and ALT for VarScan
        Counts readV = NAN;

        // Depth coverage (eg: DP for VCF and REF+ALT for VarScan)
        Counts depth = NAN;

        std::map<std::string, int> ifi;
        std::map<std::string, float> iff;
        std::map<std::string, std::string> ifs;

        std::map<std::string, int> fi;
        std::map<std::string, float> ff;

        void *hdr, *line;
    };
}

#endif