#include "mex.h"
#include <iostream>
#include "drakeUtil.h"
#include "RigidBodyManipulator.h"
#include "math.h"
#include <Eigen/Sparse>

using namespace Eigen;
using namespace std;

/*
 * MATLAB signature: [d, n, D, dn, dD] = contactConstraintsmex(model_ptr, normals, idxA, idxB, xA, xB)
 *
 * Inputs:
 *  model_ptr = mex_model_ptr to the RBM
 *  normals = (3xm) contact normals (from B to A) for each of m possible contacts in world space
 *  idxA = 1-based body indexes of body A for m possible contacts
 *  idxB = 1-based body indexes of body B for m possible contacts
 *  xA = (3xm) matrix of contact positions in body A space
 *  xB = (3xm) matrix of contact positions in body B space
 * Outputs:
 *  d = {k}(3xm) surface tangent basis vectors for the friction cone approximation in world space
 *     **NOTE** k represents the half-count for basis vectors. (e.g. k = 2 means 4 basis vectors)
 *   n = m x (num_dof) dphi/dq normal vectors in joint coordinates
 *   D = {2k} m x (num_dof) dD/dq surface tangent basis vectors in joint coordinates
 *   dn = m*num_dof x num_dof dn/dq second derivative of contact distances with respect to state
 *   dD = {2k} m*num_dof x numdof dD/dq second derivate of basis vectors with respect to state
 */

inline void buildSparseMatrix(Matrix3xd const & pts, SparseMatrix<double> & sparse)
{
  const int m = pts.cols();
  const int numNonZero = 3*m;

  sparse.resize(m, numNonZero);
  sparse.reserve(VectorXi::Constant(numNonZero, 1));

  int j = 0;
  for (int i = 0 ; i < m ; i++) {
    for (int k = 0 ; k < 3 ; k++) {
     sparse.insert(i, j) =  pts(j);
     j++;
   }
 }
}

//forms a mex cell array and computes the surface tangents in-place using maps
inline mxArray* getTangentsArray(RigidBodyManipulator* const model, Map<Matrix3xd> const & normals)
{
  const int numContactPairs = normals.cols();
  const mwSize cellDims[] = {1, BASIS_VECTOR_HALF_COUNT};
  mxArray* tangentCells = mxCreateCellArray(2, cellDims);

  vector< Map<Matrix3xd> > tangents;
  for (int k = 0 ; k < BASIS_VECTOR_HALF_COUNT ; k++) {
    mxArray *cell = mxCreateDoubleMatrix(3, numContactPairs, mxREAL );
    tangents.push_back(Map<Matrix3xd>(mxGetPrSafe(cell), 3, numContactPairs));
    mxSetCell(tangentCells, k, cell);
  }

  model->surfaceTangents(normals, tangents);

  return tangentCells;
}

void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] ) {

  if (nrhs < 6) {
    mexErrMsgIdAndTxt("Drake:contactConstraintsmex:NotEnoughInputs","Usage: \n[d, n, D] = contactConstraintsmex(normal, model_ptr, idxA, idxB, xA, xB)\n[d, n, D, dn, dD] = contactConstraintsmex(normal, model_ptr, idxA, idxB, xA, xB)");
  }

  if (nrhs >= 4 && mxGetN(prhs[2]) != mxGetN(prhs[3])) {
    mexErrMsgIdAndTxt("Drake:contactConstraintsmex:InvalidBodyIndexes", "idxA and idxB must be the same size");
  }

  if (nrhs >= 6 && mxGetN(prhs[4]) != mxGetN(prhs[5])) {
    mexErrMsgIdAndTxt("Drake:contactConstraintsmex:InvalidBodyPoints", "xA and xB must be the same size");
  }

  if (nrhs >= 6 && (mxGetM(prhs[4]) != 3 || mxGetM(prhs[5]) != 3)) {
    mexErrMsgIdAndTxt("Drake:contactConstraintsmex:InvalidBodyPointsDimension", "body points xA and xB must be 3 dimensional");
  }

  if (nrhs >= 5 && mxGetN(prhs[1]) != mxGetN(prhs[4])) {
    mexErrMsgIdAndTxt("Drake:contactConstraintsmex:InvalidBodyPointsDimension", "normals must match the number of contact points");
  }

  const int numContactPairs = mxGetN(prhs[1]);
  const Map<Matrix3xd> normals(mxGetPrSafe(prhs[1]), 3, numContactPairs); //contact normals in world space
  const bool compute_second_derivatives = nlhs > 3;

  RigidBodyManipulator *model= (RigidBodyManipulator*) getDrakeMexPointer(prhs[0]);

  if (model != NULL) {
    if (nlhs > 0) {
      plhs[0] = getTangentsArray(model, normals);
    }

    const Map<VectorXi> idxA((int*)mxGetData(prhs[2]), numContactPairs); //collision pairs index of body A
    const Map<VectorXi> idxB((int*)mxGetData(prhs[3]), numContactPairs); //collision pairs index of body B
    const Map<Matrix3xd> xA(mxGetPrSafe(prhs[4]), 3, numContactPairs); //contact point in body A space
    const Map<Matrix3xd> xB(mxGetPrSafe(prhs[5]), 3, numContactPairs); //contact point in body B space
    const int nq = model->num_positions;

    if (nlhs > 1) {
      MatrixXd J;
      MatrixXd dJ;
      vector<int> bodyInds;
      SparseMatrix<double> sparseNormals;
      model->computeContactJacobians(idxA, idxB, xA, xB, compute_second_derivatives, J, dJ);
      buildSparseMatrix(normals, sparseNormals);
      plhs[1] = mxCreateDoubleMatrix(numContactPairs, nq, mxREAL);
      Map<MatrixXd> n(mxGetPrSafe(plhs[1]), numContactPairs, nq);
      n = sparseNormals * J; //dphi/dq
      if (nlhs > 2) {
        const mwSize cellDims[] = {1, 2*BASIS_VECTOR_HALF_COUNT};
        plhs[2] = mxCreateCellArray(2, cellDims);
        if (nlhs > 4) {
          plhs[4] = mxCreateCellArray(2, cellDims);
        }
        for (int k = 0 ; k < BASIS_VECTOR_HALF_COUNT ; k++) { //for each friction cone basis vector
          Map<Matrix3xd> dk(mxGetPrSafe(mxGetCell(plhs[0], k)), 3, numContactPairs);
          SparseMatrix<double> sparseTangents;
          buildSparseMatrix(dk, sparseTangents);
          mxArray *D_cell = mxCreateDoubleMatrix(numContactPairs, nq, mxREAL);
          mxArray *D_cell_reflected = mxCreateDoubleMatrix(numContactPairs, nq, mxREAL);
          Map<MatrixXd> Dk(mxGetPrSafe(D_cell), numContactPairs, nq);
          Map<MatrixXd> Dk_reflected(mxGetPrSafe(D_cell_reflected), numContactPairs, nq);
          Dk = sparseTangents * J; //dd/dq
          Dk_reflected = -Dk;
          mxSetCell(plhs[2], k, D_cell);
          mxSetCell(plhs[2], k + BASIS_VECTOR_HALF_COUNT, D_cell_reflected);
          if (compute_second_derivatives) {
            if (nlhs > 4) {
              mxArray *dD_cell = mxCreateDoubleMatrix(numContactPairs * nq, nq, mxREAL);
              Map<MatrixXd> dD(mxGetPrSafe(dD_cell), numContactPairs , nq * nq);
              dD = sparseTangents * dJ;
              mxSetCell(plhs[4], k, dD_cell);
              mxArray *dD_cell_reflected = mxCreateDoubleMatrix(numContactPairs * nq, nq, mxREAL);
              Map<MatrixXd> dD_reflected(mxGetPrSafe(dD_cell_reflected), numContactPairs, nq*nq);
              dD_reflected = -dD;
              mxSetCell(plhs[4], k + BASIS_VECTOR_HALF_COUNT, dD_cell_reflected);
            }
          }
        }
      }

      if (nlhs > 3) {
       plhs[3] = mxCreateDoubleMatrix(numContactPairs * nq, nq, mxREAL);
       Map<MatrixXd> dn(mxGetPrSafe(plhs[3]), numContactPairs, nq*nq);
       dn = sparseNormals * dJ;
     }
   }
 }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_FAST_SINCOSD_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_FAST_SINCOSD_HPP_INCLUDED
#include <nt2/toolbox/trigonometric/functions/fast_sincosd.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/simd/meta/is_real_convertible.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/toolbox/trigonometric/functions/simd/common/impl/trigo.hpp>

/////////////////////////////////////////////////////////////////////////////
// reference based Implementation
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION_IF(nt2::tag::fast_sincosd_, boost::simd::tag::simd_,(A0)(A1)(X),
                             (boost::mpl::equal_to<nt2::meta::cardinal_of<A0>,
                                                     nt2::meta::cardinal_of<A1> >),
                             ((simd_ < arithmetic_<A0>,X > ))
                             ((simd_ < floating_<A1>,X > ))
                             ((simd_ < floating_<A1>,X > ))
                             )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a0,A1 & a1,A1 & a2) const
    {
      a1 = impl::trig_base <A1,degree_tag,
                       tag::not_simd_type>::sincosa(tofloat(a0),a2);
      return 0;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION_IF(nt2::tag::fast_sincosd_, boost::simd::tag::simd_,(A0)(A1)(X),
                                (boost::mpl::equal_to<nt2::meta::cardinal_of<A0>,
                                                 nt2::meta::cardinal_of<A1>
                                        >
                                ),
                                ((simd_ < arithmetic_<A0>,X > ))
                                ((simd_ < floating_<A1>,X > ))
                             )
  {
    typedef A1 result_type;
    inline result_type operator()(A0 const& a0,A1 & a2) const
    {
      return impl::trig_base <A1,degree_tag,fast_tag,
                       tag::simd_type>::sincosa(tofloat(a0),a2);
    }
  };
  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type  is arithmetic_
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::fast_sincosd_, boost::simd::tag::simd_,
                               (A0)(X),
                               ((simd_<arithmetic_<A0>,X>))
                              )
  {
      typedef typename meta::as_floating<A0>::type  rtype;
      typedef boost::fusion::tuple<rtype, rtype> result_type;

    NT2_FUNCTOR_CALL_REPEAT(1)
    {
      result_type res;
      typedef typename  boost::fusion::result_of::value_at_c<result_type,0>::type type;
      boost::fusion::at_c<0>(res) = impl::trig_base < type,degree_tag
        ,  tag::simd_type, clipped_pio4_
        >::sincosa( tofloat(a0)
                    , boost::fusion::at_c<1>(res)
                    );
      return res;
    }
  };
} }
#endif


/*    Copyright (c) 2010-2018, 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.
 */

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <Eigen/Dense>
#include <math.h>
#include <iostream>

#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/astro/LowThrustTrajectories/simsFlanagan.h"
#include "tudat/astro/LowThrustTrajectories/simsFlanaganModel.h"
#include "tudat/astro/LowThrustTrajectories/simsFlanaganOptimisationSetup.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/hodographicShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/baseFunctionsHodographicShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/compositeFunctionHodographicShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/createBaseFunctionHodographicShaping.h"
#include "pagmo/algorithms/de1220.hpp"
#include "Problems/applicationOutput.h"
#include "tudat/astro/basic_astro/celestialBodyConstants.h"

int main( )
{
    using namespace tudat;
    using namespace tudat::input_output;
    using namespace tudat::simulation_setup;
    using namespace tudat::shape_based_methods;
    using namespace tudat::low_thrust_trajectories;
    using namespace shape_based_methods;

    spice_interface::loadStandardSpiceKernels( );

    double julianDate = 9264.5 * physical_constants::JULIAN_DAY;
    double timeOfFlight = 1000.0 * physical_constants::JULIAN_DAY;
    int numberOfRevolutions = 2;

    // Ephemeris departure body.
    ephemerides::EphemerisPointer pointerToDepartureBodyEphemeris = std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter );

    // Ephemeris arrival body.
    ephemerides::EphemerisPointer pointerToArrivalBodyEphemeris = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::mars );

    // Retrieve cartesian state at departure and arrival.
    Eigen::Vector6d cartesianStateDepartureBody = pointerToDepartureBodyEphemeris->getCartesianState( julianDate );
    Eigen::Vector6d cartesianStateArrivalBody = pointerToArrivalBodyEphemeris->getCartesianState( julianDate + timeOfFlight );

    double maximumThrust = 5.0;
    double specificImpulse = 3000.0;
    double mass = 2800.0;
    int numberSegments = 500;

    std::string bodyToPropagate = "Borzi";
    std::string centralBody = "Sun";

    // Create central, departure and arrival bodies.
    std::vector< std::string > bodiesToCreate;
    bodiesToCreate.push_back( "Sun" );

    std::map< std::string, std::shared_ptr< simulation_setup::BodySettings > > bodySettings =
            simulation_setup::getDefaultBodySettings( bodiesToCreate );

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

    // Define central body ephemeris settings.
    bodySettings[ centralBody ]->ephemerisSettings = std::make_shared< simulation_setup::ConstantEphemerisSettings >(
                ( Eigen::Vector6d( ) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ).finished( ), frameOrigin, frameOrientation );

    bodySettings[ centralBody ]->ephemerisSettings->resetFrameOrientation( frameOrientation );
    bodySettings[ centralBody ]->rotationModelSettings->resetOriginalFrame( frameOrientation );

    // Create system of bodies.
    simulation_setup::SystemOfBodies bodies = createBodies( bodySettings );

    bodies[ bodyToPropagate ] = std::make_shared< simulation_setup::Body >( );
    bodies.at( bodyToPropagate )->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                                                         std::shared_ptr< interpolators::OneDimensionalInterpolator
                                                         < double, Eigen::Vector6d > >( ), frameOrigin, frameOrientation ) );

    setGlobalFrameBodyEphemerides( bodies, frameOrigin, frameOrientation );

    // Set vehicle mass.
    bodies[ bodyToPropagate ]->setConstantBodyMass( mass );

    double frequency = 2.0 * mathematical_constants::PI / timeOfFlight;
    double scaleFactor = 1.0 / timeOfFlight;

    // Initialize free coefficients vector for radial velocity function.
    Eigen::VectorXd freeCoefficientsRadialVelocityFunction = Eigen::VectorXd::Zero( 0 );

    // Initialize free coefficients vector for normal velocity function.
    Eigen::VectorXd freeCoefficientsNormalVelocityFunction = Eigen::VectorXd::Zero( 0 );

    // Initialize free coefficients vector for axial velocity function.
    Eigen::VectorXd freeCoefficientsAxialVelocityFunction = Eigen::VectorXd::Zero( 0 );

    // Get recommended base functions for the radial velocity composite function.
    std::shared_ptr< BaseFunctionHodographicShapingSettings > firstRadialVelocityBaseFunctionSettings =
            std::make_shared< BaseFunctionHodographicShapingSettings >( );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > secondRadialVelocityBaseFunctionSettings =
            std::make_shared< PowerFunctionHodographicShapingSettings >( 1.0, scaleFactor );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > thirdRadialVelocityBaseFunctionSettings =
            std::make_shared< TrigonometricFunctionHodographicShapingSettings >( frequency );

    // Create components of the radial velocity composite function.
    std::vector< std::shared_ptr< BaseFunctionHodographicShaping > > radialVelocityFunctionComponents;
    radialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( constant, firstRadialVelocityBaseFunctionSettings ) );
    radialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPower, secondRadialVelocityBaseFunctionSettings ) );
    radialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( cosine, thirdRadialVelocityBaseFunctionSettings ) );

    // Create base function settings for the components of the normal velocity composite function.
    std::shared_ptr< BaseFunctionHodographicShapingSettings > firstNormalVelocityBaseFunctionSettings =
            std::make_shared< BaseFunctionHodographicShapingSettings >( );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > secondNormalVelocityBaseFunctionSettings =
            std::make_shared< PowerFunctionHodographicShapingSettings >( 1.0, scaleFactor );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > thirdNormalVelocityBaseFunctionSettings =
            std::make_shared< TrigonometricFunctionHodographicShapingSettings >( frequency );

    // Get recommended base functions for normal velocity composition function.
    std::vector< std::shared_ptr< shape_based_methods::BaseFunctionHodographicShaping > > normalVelocityFunctionComponents;
    normalVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( constant, firstNormalVelocityBaseFunctionSettings ) );
    normalVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPower, secondNormalVelocityBaseFunctionSettings ) );
    normalVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( cosine, thirdNormalVelocityBaseFunctionSettings ) );

    // Create base function settings for the components of the axial velocity composite function.
    std::shared_ptr< BaseFunctionHodographicShapingSettings > firstAxialVelocityBaseFunctionSettings =
            std::make_shared< TrigonometricFunctionHodographicShapingSettings >( ( numberOfRevolutions + 0.5 ) * frequency );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > secondAxialVelocityBaseFunctionSettings =
            std::make_shared< PowerTimesTrigonometricFunctionHodographicShapingSettings >
            ( 3.0, ( numberOfRevolutions + 0.5 ) * frequency, scaleFactor );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > thirdAxialVelocityBaseFunctionSettings =
            std::make_shared< PowerTimesTrigonometricFunctionHodographicShapingSettings >(
                3.0, ( numberOfRevolutions + 0.5 ) * frequency, scaleFactor );

    // Get recommended base functions for axial velocity composition function.
    std::vector< std::shared_ptr< shape_based_methods::BaseFunctionHodographicShaping > > axialVelocityFunctionComponents;
    axialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( cosine, firstAxialVelocityBaseFunctionSettings ) );
    axialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPowerCosine, secondAxialVelocityBaseFunctionSettings ) );
    axialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPowerSine, thirdAxialVelocityBaseFunctionSettings ) );

    // Create hodographic-shaping object with defined velocity functions and boundary conditions.
    std::shared_ptr< shape_based_methods::HodographicShaping > hodographicShaping =
            std::make_shared< shape_based_methods::HodographicShaping >(
                cartesianStateDepartureBody, cartesianStateArrivalBody, timeOfFlight, numberOfRevolutions,
                bodies, bodyToPropagate, centralBody, radialVelocityFunctionComponents, normalVelocityFunctionComponents,
                axialVelocityFunctionComponents, freeCoefficientsRadialVelocityFunction, freeCoefficientsNormalVelocityFunction,
                freeCoefficientsAxialVelocityFunction );

    // Save results
    int numberOfSteps = 10000;
    double stepSize = timeOfFlight / static_cast< double >( numberOfSteps );

    // Define specific impulse function.
    std::function< double( const double ) > specificImpulseFunction = [ = ]( const double time )
    {
        return specificImpulse;
    };

    // Define integrator settings.
    std::shared_ptr< numerical_integrators::IntegratorSettings< double > > integratorSettings =
            std::make_shared< numerical_integrators::IntegratorSettings< double > > ( numerical_integrators::rungeKutta4, 0.0, stepSize );

    std::vector< double > epochsToSaveResults;
    for ( int i = 0 ; i <= numberOfSteps ; i++ )
    {
        epochsToSaveResults.push_back( i * stepSize );
    }

    std::map< double, Eigen::Vector6d > hodographicShapingTrajectory;
    std::map< double, Eigen::VectorXd > hodographicShapingMassProfile;
    std::map< double, Eigen::VectorXd > hodographicShapingThrustProfile;
    std::map< double, Eigen::VectorXd > hodographicShapingThrustAcceleration;
    hodographicShaping->getTrajectory( epochsToSaveResults, hodographicShapingTrajectory );
    hodographicShaping->getMassProfile( epochsToSaveResults, hodographicShapingMassProfile, specificImpulseFunction, integratorSettings );
    hodographicShaping->getThrustProfile( epochsToSaveResults, hodographicShapingThrustProfile, specificImpulseFunction, integratorSettings );
    hodographicShaping->getThrustAccelerationProfile( epochsToSaveResults, hodographicShapingThrustAcceleration,
                                                      specificImpulseFunction, integratorSettings );

    bodies[ bodyToPropagate ]->setConstantBodyMass( mass );

    std::shared_ptr< ShapeBasedMethod > shapeBased = std::dynamic_pointer_cast< ShapeBasedMethod >( hodographicShaping );

    std::function< Eigen::Vector3d( const double ) > initialGuessThrustFromShaping =
            getInitialGuessFunctionFromShaping( shapeBased, numberSegments, timeOfFlight, specificImpulseFunction, integratorSettings );

    std::vector< double > initialGuessVector = convertToSimsFlanaganThrustModel( initialGuessThrustFromShaping, maximumThrust,
                                                                                timeOfFlight, ( numberSegments + 1 ) / 2, numberSegments / 2 );

    std::map< double, Eigen::Vector3d > initialGuessThrustProfile;
    std::map< double, Eigen::Vector3d > initialGuessThrustAccelerationProfile;
    std::map< double, Eigen::Vector1d > initialGuessMassProfile;

    for ( unsigned int i = 0 ; i < epochsToSaveResults.size( ) ; i++ )
    {
        double currentTime = epochsToSaveResults[ i ];
        initialGuessThrustProfile[ currentTime ] =  initialGuessThrustFromShaping( currentTime );

        if ( i == 0 )
        {
            initialGuessMassProfile[ currentTime ] = ( Eigen::Vector1d( ) << mass ).finished( );
        }
        else
        {
            initialGuessMassProfile[ currentTime ] = ( Eigen::Vector1d( ) << initialGuessMassProfile[ epochsToSaveResults[ i - 1 ] ][ 0 ]
                    - initialGuessThrustProfile[ currentTime ].norm( ) /
                    ( specificImpulse * physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION ) * stepSize ).finished( );
        }

        initialGuessThrustAccelerationProfile[ currentTime ] = initialGuessThrustProfile[ currentTime ] / initialGuessMassProfile[ currentTime ][ 0 ];

    }

    bodies[ bodyToPropagate ]->setConstantBodyMass( mass );

    // Define optimisation algorithm.
    algorithm optimisationAlgorithm{ pagmo::de1220() };

    std::shared_ptr< OptimisationSettings > optimisationSettings = std::make_shared< OptimisationSettings >(
                optimisationAlgorithm, 10, 1024, 1.0e-6, std::make_pair( initialGuessVector, 0.3 ) );

    SimsFlanagan simsFlanagan = SimsFlanagan(
                cartesianStateDepartureBody, cartesianStateArrivalBody, maximumThrust, specificImpulseFunction, numberSegments,
                timeOfFlight, bodies, bodyToPropagate, centralBody, optimisationSettings );

    std::map< double, Eigen::Vector6d > SimsFlanaganTrajectory;
    std::map< double, Eigen::VectorXd > SimsFlanaganMassProfile;
    std::map< double, Eigen::VectorXd > SimsFlanaganThrustProfile;
    std::map< double, Eigen::VectorXd > SimsFlanaganThrustAcceleration;
    simsFlanagan.getTrajectory( epochsToSaveResults, SimsFlanaganTrajectory );
    simsFlanagan.getMassProfile( epochsToSaveResults, SimsFlanaganMassProfile, specificImpulseFunction, integratorSettings );
    simsFlanagan.getThrustProfile( epochsToSaveResults, SimsFlanaganThrustProfile, specificImpulseFunction, integratorSettings );
    simsFlanagan.getThrustAccelerationProfile( epochsToSaveResults, SimsFlanaganThrustAcceleration, specificImpulseFunction, integratorSettings );

    input_output::writeDataMapToTextFile( hodographicShapingTrajectory,
                                          "hodographicShapingTrajectory.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( hodographicShapingMassProfile,
                                          "hodographicShapingMassProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( hodographicShapingThrustProfile,
                                          "hodographicShapingThrustProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( hodographicShapingThrustAcceleration,
                                          "hodographicShapingThrustAcceleration.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( SimsFlanaganTrajectory,
                                          "SimsFlanaganTrajectory.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( SimsFlanaganMassProfile,
                                          "SimsFlanaganMassProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( SimsFlanaganThrustProfile,
                                          "SimsFlanaganThrustProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( SimsFlanaganThrustAcceleration,
                                          "SimsFlanaganThrustAcceleration.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( initialGuessThrustProfile,
                                          "initialGuessThrustProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( initialGuessThrustAccelerationProfile,
                                          "initialGuessThrustAccelerationProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( initialGuessMassProfile,
                                          "initialGuessMassProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    std::cout << "DELTAV SIMS FLANAGAN: " << simsFlanagan.computeDeltaV( ) << "\n\n";
    std::cout << "DELTAV SHAPE BASED: " << hodographicShaping->computeDeltaV( ) << "\n\n";

    // Final statement.
    // The exit code EXIT_SUCCESS indicates that the program was successfully executed.
    return EXIT_SUCCESS;
}


#include "log.hpp"
#include "printfn.hpp"
#include "error_out.hpp"
#include <utility>
#include <boost/property_tree/xml_parser.hpp>
#include <string>
extern "C" {
#include "rsa_util.h"
}
Log::Log(int id_, std::string hostname_, vector<uint8_t> seed) : id(id_), sk(crypto::generate_secret_key(std::move(seed))), pk(crypto::secret_to_public_key(sk)), pkv(crypto::secret_to_public_key_vec(sk)), hostname(std::move(hostname_)) {
    rsa_new(this->rsa_pk);
    rsa_new(this->rsa_sk);
    auto r = cp_rsa_gen(this->rsa_pk, this->rsa_sk, RSA_KEY_LEN);
    if(r != RLC_OK) {
        throw std::logic_error("RLC IS FUCKED");
    }
}

std::optional<Log> Log::read_log(const std::string& filename, int id) {
    pt::ptree tree;
    pt::read_xml(filename, tree);
    for(pt::ptree::value_type &v : tree.get_child("lpp.logs")) {
        const std::string& key = v.first;
        lpp::printfn("Read key: {}\n", key);
        const pt::ptree& subtree = v.second;
        if(subtree.empty()) {
            lpp::error_out("Subtree of key {} is empty!", key);
        }
        auto lid = subtree.get<int>("id");
        if(lid == id) {
            auto hostname = subtree.get<std::string>("host");
            auto sks = subtree.get<std::string>("sk");
            auto skb = bls::Util::HexToBytes(sks);
            auto pks = subtree.get<std::string>("pk");
            auto pkb = bls::Util::HexToBytes(sks);
            auto rsa_pk = subtree.get<std::string>("rsa_pk");
            auto rsa_sk = subtree.get<std::string>("rsa_sk");
            lpp::printfn("Constructing Log with id {} on host: {}\nSK: {}\nPK: {}, RSA PK: {}, RSA SK: {}\n", lid, hostname, sks, pks, rsa_pk, rsa_sk);
            return Log{lid, hostname, skb, pkb, rsa_pk, rsa_sk};
        }
    }
    //lpp::error_out("ERROR");
    return {};
}

int Log::get_id() const { return this->id; }

pk_t Log::getPublicKey() const { return this->pk; }

pkv_t Log::getPublicKeyVec() const { return this->pkv; }

sigv_t Log::sign_rsa(const msg_t &msg) {
    uint8_t out[RLC_BN_BITS / 8 + 1];
    int ol = RLC_BN_BITS / 8 + 1;
    auto ret = cp_rsa_sig(out, &ol, (uint8_t*) msg.data(), msg.size(), 0, this->rsa_sk);
    if(ret != RLC_OK) {
        throw std::logic_error("RLC IS FUCKED");
    }
    sigv_t sig;
    sig.reserve(ol);
    sig.assign(out, out+ol);
    return sig;
}
std::string Log::get_rsa_pk() const {
    int pub_len = rsa_key_size_bin(this->rsa_pk);
    uint8_t pub_bin[pub_len];
    rsa_key_write_bin(pub_bin, this->rsa_pk);
    return bls::Util::HexStr(pub_bin, pub_len);
}

sigv_t Log::sign_rsa(msg_t &&msg) {
    uint8_t out[RLC_BN_BITS / 8 + 1];
    int ol = RLC_BN_BITS / 8 + 1;
    auto ret = cp_rsa_sig(out, &ol, msg.data(), msg.size(), 0, this->rsa_sk);
    if(ret != RLC_OK) {
        throw std::logic_error("RLC IS FUCKED");
    }
    sigv_t sig;
    sig.reserve(ol);
    sig.assign(out, out+ol);
    return sig;
}

signature_t Log::sign(const msg_t &msg) const {
    return crypto::sign(this->sk, msg);
}

signature_t Log::sign(msg_t &&msg) const {
    return crypto::sign(this->sk, msg);
}
Log::Log(int id_, std::string hostname_, skv_t skv, pkv_t pkv_, std::string rsa_pk_, std::string rsa_sk_) : id(id_), sk(bls::PrivateKey::FromByteVector(std::move(skv))), pk(crypto::secret_to_public_key(sk)), pkv(pk.Serialize()), hostname(std::move(hostname_)) {
    rsa_new(this->rsa_pk);
    rsa_new(this->rsa_sk);
    auto bs_pk = bls::Util::HexToBytes(std::move(rsa_pk_));
    rsa_key_read_bin(this->rsa_pk, bs_pk.data());
    auto bs_sk = bls::Util::HexToBytes(std::move(rsa_sk_));
    rsa_key_read_bin(this->rsa_sk, bs_sk.data());
}

pt::ptree Log::to_ptree() const {
    pt::ptree tree;
    std::string sks = bls::Util::HexStr(this->sk.Serialize());
    std::string pks = bls::Util::HexStr(this->pkv);
    tree.put("id", this->get_id());
    tree.put("host", this->get_hostname());
    tree.put("sk", sks);
    tree.put("pk", pks);
    int pub_len = rsa_key_size_bin(this->rsa_pk);
    uint8_t pub_bin[pub_len];
    rsa_key_write_bin(pub_bin, this->rsa_pk);
    std::string pub_str = bls::Util::HexStr(pub_bin, pub_len);

    int prv_len = rsa_key_size_bin(this->rsa_sk);
    uint8_t prv_bin[prv_len];
    rsa_key_write_bin(prv_bin, this->rsa_sk);
    std::string prv_str = bls::Util::HexStr(prv_bin, prv_len);
    tree.put("rsa_pk", pub_str);
    tree.put("rsa_sk", prv_str);
    return tree;
}

Log Log::read_leader(const std::string &filename) {
    pt::ptree tree;
    pt::read_xml(filename, tree);
    auto lid = tree.get<int>("lpp.leader.id");
    auto hostname = tree.get<std::string>("lpp.leader.host");
    auto sks = tree.get<std::string>("lpp.leader.sk");
    auto skb = bls::Util::HexToBytes(sks);
    auto pks = tree.get<std::string>("lpp.leader.pk");
    auto pkb = bls::Util::HexToBytes(sks);
    auto rsa_pk = tree.get<std::string>("lpp.leader.rsa_pk");
    auto rsa_sk = tree.get<std::string>("lpp.leader.rsa_sk");
    lpp::printfn("Constructing Leader with id {} on host: {}\nSK: {}\nPK: {}, RSA PK: {}, RSA SK: {}", lid, hostname, sks, pks, rsa_pk, rsa_sk);
    return Log{lid, hostname, skb, pkb, rsa_pk, rsa_sk};
}

std::string Log::get_hostname() const {
    return this->hostname;
}


/* Copyright (c) 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE MaterialDeposition

#include <Geometry.hh>
#include <ThermalPhysics.hh>
#include <material_deposition.hh>

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe_nothing.h>
#include <deal.II/fe/fe_q.h>

#include "main.cc"

BOOST_AUTO_TEST_CASE(read_material_deposition_file_2d)
{
  // Geometry database
  boost::property_tree::ptree geometry_database;
  geometry_database.put("material_deposition", true);
  geometry_database.put("material_deposition_file",
                        "material_deposition_2d.txt");

  std::vector<double> time_ref = {1., 2., 3.4, 4.98};
  std::vector<dealii::BoundingBox<2>> bounding_boxes_ref;
  bounding_boxes_ref.emplace_back(
      std::make_pair(dealii::Point<2>(0., 0.), dealii::Point<2>(1., 1.)));
  bounding_boxes_ref.emplace_back(
      std::make_pair(dealii::Point<2>(3., 2.4), dealii::Point<2>(7., 10.)));
  bounding_boxes_ref.emplace_back(
      std::make_pair(dealii::Point<2>(4., 2.4), dealii::Point<2>(7., 9.)));
  bounding_boxes_ref.emplace_back(
      std::make_pair(dealii::Point<2>(4., 3.4), dealii::Point<2>(6., 9.)));

  auto [bounding_boxes, time] =
      adamantine::read_material_deposition<2>(geometry_database);

  BOOST_CHECK(time == time_ref);
  BOOST_CHECK(bounding_boxes.size() == bounding_boxes_ref.size());
  double const tolerance = 1e-13;
  for (unsigned int i = 0; i < bounding_boxes_ref.size(); ++i)
  {
    auto points = bounding_boxes[i].get_boundary_points();
    auto points_ref = bounding_boxes_ref[i].get_boundary_points();
    for (int d = 0; d < 2; ++d)
    {
      BOOST_CHECK_CLOSE(points.first[d], points_ref.first[d], tolerance);
      BOOST_CHECK_CLOSE(points.second[d], points_ref.second[d], tolerance);
    }
  }
}

BOOST_AUTO_TEST_CASE(read_material_deposition_file_3d)
{
  // Geometry database
  boost::property_tree::ptree geometry_database;
  geometry_database.put("material_deposition", true);
  geometry_database.put("material_deposition_file",
                        "material_deposition_3d.txt");

  std::vector<double> time_ref = {1., 2., 3.4, 4.98};
  std::vector<dealii::BoundingBox<3>> bounding_boxes_ref;
  bounding_boxes_ref.emplace_back(std::make_pair(dealii::Point<3>(0., 0., 0.),
                                                 dealii::Point<3>(1., 1., 1.)));
  bounding_boxes_ref.emplace_back(std::make_pair(
      dealii::Point<3>(3., 2.4, 1.), dealii::Point<3>(7., 10., 2.)));
  bounding_boxes_ref.emplace_back(std::make_pair(dealii::Point<3>(4., 2.4, 2.),
                                                 dealii::Point<3>(7., 9., 4.)));
  bounding_boxes_ref.emplace_back(std::make_pair(dealii::Point<3>(4., 3.4, 3.),
                                                 dealii::Point<3>(6., 9., 8.)));

  auto [bounding_boxes, time] =
      adamantine::read_material_deposition<3>(geometry_database);

  BOOST_CHECK(time == time_ref);
  BOOST_CHECK(bounding_boxes.size() == bounding_boxes_ref.size());
  double const tolerance = 1e-13;
  for (unsigned int i = 0; i < bounding_boxes_ref.size(); ++i)
  {
    auto points = bounding_boxes[i].get_boundary_points();
    auto points_ref = bounding_boxes_ref[i].get_boundary_points();
    for (int d = 0; d < 2; ++d)
    {
      BOOST_CHECK_CLOSE(points.first[d], points_ref.first[d], tolerance);
      BOOST_CHECK_CLOSE(points.second[d], points_ref.second[d], tolerance);
    }
  }
}

BOOST_AUTO_TEST_CASE(get_elements_to_activate_2d)
{
  MPI_Comm communicator = MPI_COMM_WORLD;
  if (dealii::Utilities::MPI::n_mpi_processes(communicator) == 1)
  {
    // Geometry database
    boost::property_tree::ptree geometry_database;
    geometry_database.put("import_mesh", false);
    geometry_database.put("length", 12);
    geometry_database.put("length_divisions", 12);
    geometry_database.put("height", 6);
    geometry_database.put("height_divisions", 6);

    adamantine::Geometry<2> geometry(communicator, geometry_database);
    dealii::parallel::distributed::Triangulation<2> const &tria =
        geometry.get_triangulation();

    dealii::hp::FECollection<2> fe_collection;
    fe_collection.push_back(dealii::FE_Q<2>(1));
    fe_collection.push_back(dealii::FE_Nothing<2>());
    dealii::DoFHandler<2> dof_handler(tria);
    dof_handler.distribute_dofs(fe_collection);
    for (auto const &cell :
         dealii::filter_iterators(dof_handler.active_cell_iterators(),
                                  dealii::IteratorFilters::LocallyOwnedCell()))
      cell->set_active_fe_index(1);

    std::vector<dealii::BoundingBox<2>> bounding_boxes;
    bounding_boxes.emplace_back(
        std::make_pair(dealii::Point<2>(0.1, 0.1), dealii::Point<2>(0.2, 0.2)));
    bounding_boxes.emplace_back(
        std::make_pair(dealii::Point<2>(1.1, 0.1), dealii::Point<2>(2.2, 0.2)));
    bounding_boxes.emplace_back(
        std::make_pair(dealii::Point<2>(4.5, 4.5), dealii::Point<2>(5.5, 5.5)));

    auto elements_to_activate =
        adamantine::get_elements_to_activate(dof_handler, bounding_boxes);

    BOOST_CHECK(elements_to_activate.size() == bounding_boxes.size());

    std::vector<std::vector<dealii::CellId>> cell_id_ref(3);
    cell_id_ref[0].push_back(dealii::CellId("0_0:"));
    cell_id_ref[1].push_back(dealii::CellId("1_0:"));
    cell_id_ref[1].push_back(dealii::CellId("4_0:"));
    cell_id_ref[2].push_back(dealii::CellId("40_0:"));
    cell_id_ref[2].push_back(dealii::CellId("41_0:"));
    cell_id_ref[2].push_back(dealii::CellId("42_0:"));
    cell_id_ref[2].push_back(dealii::CellId("43_0:"));

    for (unsigned int i = 0; i < cell_id_ref.size(); ++i)
    {
      BOOST_CHECK(elements_to_activate[i].size() == cell_id_ref[i].size());
      for (unsigned int j = 0; j < cell_id_ref[i].size(); ++j)
      {
        BOOST_CHECK(elements_to_activate[i][j]->id() == cell_id_ref[i][j]);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(get_elements_to_activate_3d)
{
  MPI_Comm communicator = MPI_COMM_WORLD;
  if (dealii::Utilities::MPI::n_mpi_processes(communicator) == 1)
  {
    // Geometry database
    boost::property_tree::ptree geometry_database;
    geometry_database.put("import_mesh", false);
    geometry_database.put("length", 12);
    geometry_database.put("length_divisions", 12);
    geometry_database.put("width", 6);
    geometry_database.put("width_divisions", 6);
    geometry_database.put("height", 6);
    geometry_database.put("height_divisions", 6);

    adamantine::Geometry<3> geometry(communicator, geometry_database);
    dealii::parallel::distributed::Triangulation<3> const &tria =
        geometry.get_triangulation();

    dealii::hp::FECollection<3> fe_collection;
    fe_collection.push_back(dealii::FE_Q<3>(1));
    fe_collection.push_back(dealii::FE_Nothing<3>());
    dealii::DoFHandler<3> dof_handler(tria);
    dof_handler.distribute_dofs(fe_collection);
    for (auto const &cell :
         dealii::filter_iterators(dof_handler.active_cell_iterators(),
                                  dealii::IteratorFilters::LocallyOwnedCell()))
      cell->set_active_fe_index(1);

    std::vector<dealii::BoundingBox<3>> bounding_boxes;
    bounding_boxes.emplace_back(std::make_pair(
        dealii::Point<3>(0.1, 0.1, 0.1), dealii::Point<3>(0.2, 0.2, 0.2)));
    bounding_boxes.emplace_back(std::make_pair(
        dealii::Point<3>(1.1, 0.1, 0.1), dealii::Point<3>(2.2, 0.2, 0.2)));
    bounding_boxes.emplace_back(std::make_pair(
        dealii::Point<3>(4.5, 4.5, 4.5), dealii::Point<3>(5.5, 5.5, 5.5)));

    auto elements_to_activate =
        adamantine::get_elements_to_activate(dof_handler, bounding_boxes);

    BOOST_CHECK(elements_to_activate.size() == bounding_boxes.size());

    std::vector<std::vector<dealii::CellId>> cell_id_ref(3);
    cell_id_ref[0].push_back(dealii::CellId("0_0:"));
    cell_id_ref[1].push_back(dealii::CellId("1_0:"));
    cell_id_ref[1].push_back(dealii::CellId("8_0:"));
    cell_id_ref[2].push_back(dealii::CellId("272_0:"));
    cell_id_ref[2].push_back(dealii::CellId("273_0:"));
    cell_id_ref[2].push_back(dealii::CellId("276_0:"));
    cell_id_ref[2].push_back(dealii::CellId("277_0:"));
    cell_id_ref[2].push_back(dealii::CellId("274_0:"));
    cell_id_ref[2].push_back(dealii::CellId("275_0:"));
    cell_id_ref[2].push_back(dealii::CellId("278_0:"));
    cell_id_ref[2].push_back(dealii::CellId("279_0:"));

    for (unsigned int i = 0; i < cell_id_ref.size(); ++i)
    {
      BOOST_CHECK(elements_to_activate[i].size() == cell_id_ref[i].size());
      for (unsigned int j = 0; j < cell_id_ref[i].size(); ++j)
      {
        BOOST_CHECK(elements_to_activate[i][j]->id() == cell_id_ref[i][j]);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(material_deposition)
{
  int constexpr dim = 3;
  MPI_Comm communicator = MPI_COMM_WORLD;

  double const initial_temperature = 300.;

  boost::property_tree::ptree database;
  // Geometry database
  database.put("geometry.import_mesh", false);
  database.put("geometry.length", 10);
  database.put("geometry.length_divisions", 10);
  database.put("geometry.width", 10);
  database.put("geometry.width_divisions", 10);
  database.put("geometry.height", 10);
  database.put("geometry.height_divisions", 10);
  database.put("geometry.material_height", 6.);
  database.put("geometry.material_deposition", true);
  database.put("geometry.material_deposition_file",
               "material_path_test_material_deposition.txt");
  // Build Geometry
  boost::property_tree::ptree geometry_database =
      database.get_child("geometry");
  adamantine::Geometry<dim> geometry(communicator, geometry_database);

  // Material property
  database.put("materials.property_format", "polynomial");
  database.put("materials.initial_temperature", initial_temperature);
  database.put("materials.new_material_temperature", initial_temperature);
  database.put("materials.n_materials", 1);
  database.put("materials.material_0.solid.density", 1.);
  database.put("materials.material_0.liquid.density", 1.);
  database.put("materials.material_0.solid.specific_heat", 1.);
  database.put("materials.material_0.liquid.specific_heat", 1.);
  database.put("materials.material_0.solid.thermal_conductivity", 1.);
  database.put("materials.material_0.liquid.thermal_conductivity", 1.);
  // Source database
  database.put("sources.n_beams", 0);
  // Time-stepping database
  database.put("time_stepping.method", "forward_euler");
  // Boundary database
  database.put("boundary.type", "radiative");

  // Build ThermalPhysics
  adamantine::ThermalPhysics<dim, dim, dealii::MemorySpace::Host,
                             dealii::QGauss<1>>
      thermal_physics(communicator, database, geometry);
  thermal_physics.setup_dofs();
  thermal_physics.compute_inverse_mass_matrix();
  auto &dof_handler = thermal_physics.get_dof_handler();

  auto [material_deposition_boxes, deposition_times] =
      adamantine::read_material_deposition<dim>(geometry_database);
  auto elements_to_activate = adamantine::get_elements_to_activate(
      dof_handler, material_deposition_boxes);
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> solution;
  std::vector<adamantine::Timer> timers(adamantine::Timing::n_timers);
  thermal_physics.initialize_dof_vector(solution);
  thermal_physics.get_state_from_material_properties();
  std::vector<unsigned int> n_cells_ref = {610, 620, 630, 650, 650,
                                           660, 670, 680, 720, 720};
  double const time_step = 0.1;
  double time =
      thermal_physics.evolve_one_time_step(0., time_step, solution, timers);
  double const eps = time_step / 1e12;
  // The build is too slow in debug mode when using sanitizer. In that case
  // reduce the size of the loop
#ifdef ADAMANTINE_DEBUG
  unsigned int const i_max = 2;
#else
  unsigned int const i_max = 10;
#endif
  for (unsigned int i = 0; i < i_max; ++i)
  {
    auto activation_start =
        std::lower_bound(deposition_times.begin(), deposition_times.end(),
                         time - eps) -
        deposition_times.begin();
    auto activation_end =
        std::lower_bound(deposition_times.begin(), deposition_times.end(),
                         time + time_step - eps) -
        deposition_times.begin();
    for (unsigned int j = activation_start; j < activation_end; ++j)
    {
      thermal_physics.add_material(elements_to_activate[j], initial_temperature,
                                   solution);
    }

    time =
        thermal_physics.evolve_one_time_step(time, time_step, solution, timers);

    unsigned int n_cells = 0;
    for (auto const &cell : dealii::filter_iterators(
             dof_handler.active_cell_iterators(),
             dealii::IteratorFilters::LocallyOwnedCell(),
             dealii::IteratorFilters::ActiveFEIndexEqualTo(0)))
    {
      (void)cell;
      ++n_cells;
    }
    BOOST_CHECK(dealii::Utilities::MPI::sum(n_cells, communicator) ==
                n_cells_ref[i]);
  }
}

BOOST_AUTO_TEST_CASE(deposition_from_scan_path_2d)
{
  adamantine::ScanPath scan_path("scan_path.txt", "segment");

  boost::property_tree::ptree database;
  database.put("deposition_length", 0.0005);
  database.put("deposition_height", 0.1);
  database.put("deposition_width", 0.1);
  database.put("deposition_lead_time", 0.0);

  std::pair<std::vector<dealii::BoundingBox<2>>, std::vector<double>>
      boxes_and_times =
          adamantine::deposition_along_scan_path<2>(database, scan_path);

  double const tolerance = 1e-13;

  // Check the times
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(0), 1.0e-6, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(1), 6.26e-4, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(2), 1.251e-3, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(3), 1.876e-3, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(4), 2.501e-3, tolerance);

  // Check the first and last boxes
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().first(0),
                    -0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().first(1),
                    0.0, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().second(0),
                    0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().second(1),
                    0.1, tolerance);

  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().first(0),
                    0.002 - 0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().first(1),
                    0.0, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().second(0),
                    0.002 + 0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().second(1),
                    0.1, tolerance);
}

BOOST_AUTO_TEST_CASE(deposition_from_scan_path_3d)
{
  adamantine::ScanPath scan_path("scan_path.txt", "segment");

  boost::property_tree::ptree database;
  database.put("deposition_length", 0.0005);
  database.put("deposition_height", 0.1);
  database.put("deposition_width", 0.1);
  database.put("deposition_lead_time", 0.0);

  std::pair<std::vector<dealii::BoundingBox<3>>, std::vector<double>>
      boxes_and_times =
          adamantine::deposition_along_scan_path<3>(database, scan_path);

  double const tolerance = 1e-13;

  // Check the times
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(0), 1.0e-6, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(1), 6.26e-4, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(2), 1.251e-3, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(3), 1.876e-3, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(4), 2.501e-3, tolerance);

  // Check the first and last boxes
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().first(0),
                    -0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().first(1),
                    -0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().first(2),
                    0.0, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().second(0),
                    0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().second(1),
                    0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().second(2),
                    0.1, tolerance);

  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().first(0),
                    0.002 - 0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().first(1),
                    -0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().first(2),
                    0.0, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().second(0),
                    0.002 + 0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().second(1),
                    0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().second(2),
                    0.1, tolerance);
}

BOOST_AUTO_TEST_CASE(deposition_from_L_scan_path_3d)
{
  adamantine::ScanPath scan_path("scan_path_L.txt", "segment");

  boost::property_tree::ptree database;
  database.put("deposition_length", 0.0005);
  database.put("deposition_height", 0.1);
  database.put("deposition_width", 0.1);
  database.put("deposition_lead_time", 0.0);

  std::pair<std::vector<dealii::BoundingBox<3>>, std::vector<double>>
      boxes_and_times =
          adamantine::deposition_along_scan_path<3>(database, scan_path);

  double const tolerance = 1e-13;

  // Check the times
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(0), 1.0e-6, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(1), 6.26e-4, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(2), 1.251e-3, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(3), 1.876e-3, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(4), 2.501e-3, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(5), 2.501e-3, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(6), 3.126e-3, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(7), 3.751e-3, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(8), 4.376e-3, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.second.at(9), 4.876e-3, tolerance);

  // Check the first and last boxes for each segment
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().first(0),
                    -0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().first(1),
                    -0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().first(2),
                    0.0, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().second(0),
                    0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().second(1),
                    0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(0).get_boundary_points().second(2),
                    0.1, tolerance);

  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().first(0),
                    0.002 - 0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().first(1),
                    -0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().first(2),
                    0.0, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().second(0),
                    0.002 + 0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().second(1),
                    0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(4).get_boundary_points().second(2),
                    0.1, tolerance);

  BOOST_CHECK_CLOSE(boxes_and_times.first.at(5).get_boundary_points().first(0),
                    0.002 - 0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(5).get_boundary_points().first(1),
                    -0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(5).get_boundary_points().first(2),
                    0.0, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(5).get_boundary_points().second(0),
                    0.002 + 0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(5).get_boundary_points().second(1),
                    0.00025, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(5).get_boundary_points().second(2),
                    0.1, tolerance);

  BOOST_CHECK_CLOSE(boxes_and_times.first.at(9).get_boundary_points().first(0),
                    0.002 - 0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(9).get_boundary_points().first(1),
                    0.00175, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(9).get_boundary_points().first(2),
                    0.0, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(9).get_boundary_points().second(0),
                    0.002 + 0.05, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(9).get_boundary_points().second(1),
                    0.00205, tolerance);
  BOOST_CHECK_CLOSE(boxes_and_times.first.at(9).get_boundary_points().second(2),
                    0.1, tolerance);
}


#pragma once

#include "ibrain.hpp"
#include "memory.hpp"

#include <memory>

#include <boost/optional.hpp>

namespace Aronda::Trainer
{

class Agent
{
public:
    struct Step
    {
        State s;
        std::size_t a; // index of an Action to form 1-hot action vector
        double r;
        boost::optional<State> s_;
    };

    static constexpr double GAMMA = 0.99; // discount factor

    static constexpr double MAX_EPSILON = 0.2; // 1
    static constexpr double MIN_EPSILON = 0.05; // stay a bit curious even when getting old
    static constexpr double LAMBDA = 0.001; // speed of decay

    static constexpr std::size_t BATCH_SIZE = 256;

    static constexpr std::size_t MEMORY_CAPACITY = 10000; // 100000;

    Agent();

    ~Agent() = default;

    void saveModel(const std::string& path) const;

    std::size_t act(const State& s) const;

    void observe(Step step);

    void replay();

    double epsilon() const;

private:
    std::unique_ptr<IBrain> m_brain;
    Memory<Step> m_memory;
    std::size_t m_steps = 0;
    double m_epsilon = MAX_EPSILON;
};
}


/*
 * SketchSort.hpp
 * Copyright (c) 2017 Yasuo Tabei All Rights Reserved.
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE and * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#ifndef SKETCHSORT_HPP
#define SKETCHSORT_HPP

// for boost
#include <boost/pool/pool.hpp>
#include <boost/pool/object_pool.hpp>
#include <boost/random.hpp>

#include <random>
#include <iostream>
#include <algorithm>
#include <string>
#include <vector>
#include <list>
#include <map>
#include <cmath>
#include <cstring>
#include <string>
#include <iterator>
#include <fstream>
#include <strstream>
#include <iostream>
#include <sstream>
#include <cstdio>
#include <cstdlib>
#include <set>
#include <stdint.h>
#include <time.h>

#ifndef DBL_MAX
  #define  DBL_MAX 1.797693e+300
#endif

class Random{
public:
  Random() {
    srand( static_cast<uint32_t>( time(NULL) ) );
  }
  uint32_t operator()(uint32_t max) {
    double tmp = static_cast<double>( rand() ) / static_cast<double>( RAND_MAX );
    return static_cast<uint32_t>( tmp * max );
  }
};

struct pstat {
  int start;
  int end;
};

struct params {
  uint32_t numblocks;
  uint32_t numchunks;
  uint32_t projectDim;
  uint32_t chunk_dist;
  uint32_t chunks;
  uint32_t num_seq;
  uint32_t seq_len;
  uint32_t chunk_len;
  uint32_t start_chunk;
  uint32_t end_chunk;
  uint32_t cchunk;
  uint32_t *counter;
  pstat        *pos;
  pstat        *pchunks;
  float         minmaxDist;
  std::vector<uint32_t> ids;
  std::vector<int> blocks;
  std::ostream *os;
};

class SketchSort {
private:
  void readFeature(const char *fname);
  void projectVectors(uint32_t projectDim, std::vector<uint8_t*> &sig, params &param);
  void report(std::vector<uint8_t*> &sig, int l, int r, params &param);
  void radixsort(std::vector<uint8_t*> &sig, int spos, int epos, int l, int r, params &param);
  void insertionSort(std::vector<uint8_t*> &sig, int spos, int epos, int l, int r, params &param);
  bool calc_chunk_hamdist(uint8_t *seq1, uint8_t *seq2, const params &param);
  bool calc_hamdist(uint8_t *seq1, uint8_t *seq2, const params &param);
  bool check_canonical(uint8_t *seq1, uint8_t *seq2, const params &param);
  bool check_chunk_canonical(uint8_t *seq1, uint8_t *seq2, const params &param);
  float calcMinMaxDist(uint32_t id1, uint32_t id2);
  float check_upper_bound(uint32_t id1, uint32_t id2);
  double calcMissingEdgeRatio(params &param);
  void classify(std::vector<uint8_t*> &sig, int spos, int epos, int l, int r, int bpos, params &param);
  void turnParameters(float _missingratio, params &param);
  void multi_classification(std::vector<uint8_t*> &sig, int maxind, int l, int r, params &param);

  float log10(float val);
  void generateMatrix(uint32_t projectDim, std::vector<std::vector<float> > &matR, std::vector<std::vector<float> > &matC, std::vector<std::vector<float> > &matB);
  void convertVector(std::vector<float> &fv, std::vector<float> &fv2);
  uint64_t compMin(std::vector<float> &resA);
  void zNormalization();
  void MinMaxNormalization();
public:
  SketchSort() {};
  void run(const char *fname, const char *oname,
	   uint32_t _numblocks,
	   uint32_t _dist,
	   float        _minmaxDist,
	   uint32_t _numchunks,
	   bool         _autoFlag,
	   float        _missingratio,
	   bool         _zNormalization,
	   bool         _minmaxNormalization);
private:
  boost::pool<> *p;
  std::vector<std::vector<float> > fvs;
  uint64_t           dim_;
  uint32_t           num_char;
};

#endif // SKETCHSORT_HPP


#include <rabbitxx/graph.hpp>
#include <rabbitxx/log.hpp>

#include <boost/graph/breadth_first_search.hpp>
#include <boost/mpi.hpp>

#include <iostream>

using rabbitxx::logging;

class bfs_print_visitor : public boost::default_bfs_visitor
{
public:
    template<typename Vertex, typename Graph>
    void discover_vertex(Vertex v, const Graph& g) const
    {
        logging::debug() << "VERTEX # " << v << " " << g[v].name() << " @ " << g[v].id();
    }

    template<typename Edge, typename Graph>
    void examine_edge(Edge e, const Graph& g)
    {
        const auto src_v = source(e,g);
        const auto trg_v = target(e,g);

        logging::debug() << "EDGE from " << g[src_v].name() << " @ " << g[src_v].id()
            << " -> " << g[trg_v].name() << " @ " << g[trg_v].id();

    }
};

int main(int argc, char** argv)
{
    boost::mpi::environment env(argc, argv);
    boost::mpi::communicator world;

    if (argc < 2) {
        logging::fatal() << "usage ./" << argv[0] << " <input-trace> ";
        env.abort(1);
        return 1;
    }

    auto g = rabbitxx::make_graph<rabbitxx::graph::OTF2_Io_Graph_Builder>(argv[1], world);
    bfs_print_visitor vis;
    boost::breadth_first_search(*g.get(), vertex(0, *g.get()), boost::visitor(vis));

    return 0;
}


#define BOOST_TEST_MODULE dnaq test
#include <boost/test/unit_test.hpp>

#include "fragments_assembling.hpp"

#include <string>
#include <vector>
#include <iostream>
#include <iomanip>

#include "../kmer/kmer32_service.h"

#include "../common.h"
#include "../dna/simple_immutable_dna.h"
#include "../dna/dna.h"
#include "../dna/dnaq.h"
#include "../debrujin/compact_de_brujin_graph.hpp"
#include "../io/quality/illumina.hpp"
#include "../util.h"

using namespace boost;
using namespace std;

Illumina const illumina;

typedef Kmer32Service::Kmer Kmer;

Kmer string_to_kmer(string const & s)
{
    return dna_to_kmer<Kmer>(SimpleImmutableDna(s));
}

template<class KmerService>
bool add_edge(CompactDeBrujinGraph<KmerService> & graph, string edge)
{
    return graph.add_edge(string_to_kmer(edge));
}

BOOST_AUTO_TEST_CASE( assemble_fragment_test )
{
    string fragment = "AGCGTAAGATATAGTTGATCCTACTTAAAA";
    string reads =    "AGCGTAAGACA.......CCCTGCTTAAAA";
    string misreads = ".........!........!...!.......";
    string quals =    "rrbaGSBIIBB.......BBBHYEabhrrr";

    DnaQ left(reads.substr(0, reads.find('.')),
              quals.substr(0, quals.find('.')),
              illumina);

    DnaQ right(reads.substr(reads.rfind('.') + 1),
               quals.substr(quals.rfind('.') + 1),
               illumina);

    right.reverse().complement();

    FragmentAssembleParameters parameters(20, 20);

    const int k = 5;
    CompactDeBrujinGraph<Kmer32Service> graph(k, 256);

    size_t begin_pos = 1;
    size_t end_pos = fragment.length() - 2;

    for (size_t i = begin_pos; i + k + 1 <= end_pos; ++i)
    {
        BOOST_CHECK_EQUAL(add_edge(graph, fragment.substr(i, k + 1)), true);
    }

}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_ENVELOPE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_ENVELOPE_HPP

#include <boost/geometry/algorithms/envelope.hpp>

#include <boost/geometry/extensions/nsphere/core/radius.hpp>
#include <boost/geometry/extensions/nsphere/core/tags.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace envelope
{

/// Calculate envelope of an n-sphere, circle or sphere (currently only for Cartesian 2D points)
struct envelope_nsphere
{
    template <typename Nsphere, typename Box>
    static inline void apply(Nsphere const& nsphere, Box& mbr)
    {
        assert_dimension<Nsphere, 2>();
        assert_dimension<Box, 2>();

        typename radius_type<Nsphere>::type radius = get_radius<0>(nsphere);
        set<min_corner, 0>(mbr, get<0>(nsphere) - radius);
        set<min_corner, 1>(mbr, get<1>(nsphere) - radius);
        set<max_corner, 0>(mbr, get<0>(nsphere) + radius);
        set<max_corner, 1>(mbr, get<1>(nsphere) + radius);
    }
};

}} // namespace detail::envelope
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Nsphere>
struct envelope<Nsphere, nsphere_tag>
    : detail::envelope::envelope_nsphere
{};

} // namespace dispatch
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_ENVELOPE_HPP


//----------------------------------------------------------------------------
// Copyright (C) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the Server Side Public License, version 1,
// as published by the author.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// Server Side Public License for more details.
//
// You should have received a copy of the Server Side Public License
// along with this program. If not, see
// <https://github.com/NilFoundation/plugin/blob/master/LICENSE_1_0.txt>.
//----------------------------------------------------------------------------

#define BOOST_TEST_MODULE crypto_feistel_test

#include <cassert>

#include <boost/test/data/monomorphic.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>

#include <nil/filecoin/storage/proofs/core/crypto/feistel.hpp>

using namespace nil::filecoin;

BOOST_AUTO_TEST_SUITE(feistel_test_suite)

// Some sample n-values which are not powers of four and also don't coincidentally happen to
// encode/decode correctly.
constexpr static const std::array<index_type> BAD_NS = {5, 6, 8, 12, 17};    //
//
void encode_decode(index_type n, bool expect_success) {
    auto failed = false;
    const auto precomputed = precompute(n);
    for (std::size_t i = 0; i < n; i++) {
        const auto p = encode(i, {1, 2, 3, 4}, precomputed);
        const auto v = decode(p, {1, 2, 3, 4}, precomputed);
        const auto equal = i == v;
        const auto in_range = p <= n;
        if (expect_success) {
            BOOST_CHECK(equal, "failed to permute n = " + std::to_string(n));
            BOOST_CHECK(in_range, "output number is too big n = " + std::to_string(n));
        } else {
            if (!equal || !in_range) {
                failed = true;
            }
        }
    }
    if (!expect_success) {
        assert(failed, "expected failure n = " + std::to_string(n));
    }
}

BOOST_AUTO_TEST_CASE(test_feistel_power_of_4) {
    // Our implementation is guaranteed to produce a permutation when input size (number of elements)
    // is a power of our.
    auto n = 1;

    // Powers of 4 always succeed.
    for (std::size_t i = 0; i < 4; ++i) {
        n *= 4;
        encode_decode(n, true);
    }

    // Some non-power-of 4 also succeed, but here is a selection of examples values showing
    // that this is not guaranteed.
    for (i in BAD_NS.iter()) {
        encode_decode(*i, false);
    }
}

BOOST_AUTO_TEST_CASE(test_feistel_on_arbitrary_set) {
    for (n in BAD_NS.iter()) {
        const auto precomputed = precompute(*n as Index);
        for (i = 0; i < *n; ++i) {
            const auto p = permute(*n, i, &[ 1, 2, 3, 4 ], precomputed);
            const auto v = invert_permute(*n, p, &[ 1, 2, 3, 4 ], precomputed);
            // Since every element in the set is reversibly mapped to another element also in the set,
            // this is indeed a permutation.
            BOOST_ASSERT_MSG(i == v, "failed to permute");
            BOOST_ASSERT_MSG(p <= *n, "output number is too big");
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once
//===----------------------------------------------------------------------===//
#include "rank1_logic_surf.hpp"

#include <dtl/dtl.hpp>
#include <dtl/math.hpp>

#include <boost/dynamic_bitset.hpp>

#include <string>
#include <type_traits>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// Very fast rank1 implementation. The LuT has 64-bit granularity which
/// results in 50% additional space consumption.
template<
    /// The word type used to store bitmaps.
    typename _word_type = $u64,
    /// Inclusive [b,e] vs exclusive [b,e)
    u1 _inclusive = false>
struct rank1_logic_word_blocked {
  using word_type = typename std::remove_cv<_word_type>::type;
  using size_type = $u32;
  static constexpr u64 is_inclusive = _inclusive ? 1 : 0;

  static constexpr u64 word_bitlength = sizeof(word_type) * 8;
  static constexpr u64 block_bitlength = 64;
  static_assert(word_bitlength == block_bitlength,
      "A word and a block must be equal in size."); // TODO remove limitation

  using rank_helper =
      dtl::rank1_logic_surf<_word_type, _inclusive, block_bitlength>;

  // Pure static.
  rank1_logic_word_blocked() = delete;
  ~rank1_logic_word_blocked() = delete;

  /// Initializes the rank LuT in place. The function estimate_size_in_bytes()
  /// allows to predetermine the required memory.
  static void
  init_inplace(
      const word_type* const bitmap_begin,
      const word_type* const bitmap_end,
      size_type* lut) noexcept {
    rank_helper::init_inplace(bitmap_begin, bitmap_end, lut);
  }

  /// Returns the number of LuT entries for a bitmap of the given size.
  static constexpr u64
  lut_entry_cnt(u64 bitmap_size) noexcept {
    return rank_helper::lut_entry_cnt(bitmap_size);
  }

  /// Returns the size of the rank LuT in bytes for a bitmap of the given size.
  static constexpr u64
  estimate_size_in_bytes(u64 bitmap_size) noexcept {
    return lut_entry_cnt(bitmap_size) * sizeof(size_type);
  }

  /// Computes the rank1 of the bit at position 'idx'.
  static size_type __forceinline__
  get(const size_type* lut, u64 idx, const word_type* bitmap_ptr) noexcept {
    const word_type mask63 = ~word_type(0) >> 1;

    const auto bit_idx = idx + is_inclusive;
    const auto block_idx = bit_idx / block_bitlength;
    const auto lut_entry = lut[block_idx];
    const auto bitmap_word = bitmap_ptr[block_idx];

    const auto offset = bit_idx & (block_bitlength - 1);
    // The following works, because we never have to popcount a full word.
    // In cases where 'offset' is 0, we only need the value from the LuT.
    const auto bitmap_word_masked = (bitmap_word << (63 - offset)) & mask63;
    const auto popcnt = dtl::bits::pop_count(bitmap_word_masked);
    return lut_entry + popcnt;
  }

  /// Returns the name of the implementation.
  static std::string
  name() noexcept {
    return std::string("WordBlocked");
  }
};
//===----------------------------------------------------------------------===//
} // namespace dtl


#include <opencv2/opencv.hpp>

#include <iostream>
#include <iomanip>

#include <boost/program_options.hpp>

#include "LinearMath/Matrix3x3.h"

#include "../src/head_pose_estimation.hpp"

#define STR_EXPAND(tok) #tok
#define STR(tok) STR_EXPAND(tok)

using namespace std;
using namespace cv;
namespace po = boost::program_options;

inline double todeg(double rad) {
    return rad * 180 / M_PI;
}

int main(int argc, char **argv)
{
    Mat frame;
    bool show_frame = false;
    bool use_camera = false;

    po::positional_options_description p;
    p.add("image", 1);

    po::options_description desc("Allowed options");
    desc.add_options()
            ("help,h", "produce help message")
            ("version,v", "shows version and exits")
            ("show,s", "display the image with gaze estimation")
            ("model", po::value<string>(), "dlib's trained face model")
            ("image", po::value<string>(), "image to process (png, jpg)")
            ;

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv)
                        .options(desc)
                        .positional(p)
                        .run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
        cout << argv[0] << " " <<  STR(GAZR_VERSION) << "\n\n" << desc << "\n";
        return 1;
    }

    if (vm.count("version")) {
        cout << argv[0] << " " << STR(GAZR_VERSION) << "\n";
        return 0;
    }

    if (vm.count("show")) {
        show_frame = true;
    }

    if (vm.count("model") == 0) {
        cout << "You must specify the path to a trained dlib's face model\n"
             << "with the option --model." << endl;
        return 1;
    }

    if (vm.count("image") == 0) {
        use_camera = true;
    }

    auto estimator = HeadPoseEstimation(vm["model"].as<string>());

    VideoCapture video_in;

    if (use_camera) {
        video_in = VideoCapture(0);

        // adjust for your webcam!
        video_in.set(CV_CAP_PROP_FRAME_WIDTH, 640);
        video_in.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
        estimator.focalLength = 500;
        estimator.opticalCenterX = 320;
        estimator.opticalCenterY = 240;

        if(!video_in.isOpened()) {
            cerr << "Couldn't open camera" << endl;
            return 1;
        }
    }

    else {
        auto image = vm["image"].as<string>();

#ifdef OPENCV3
        frame = imread(image,IMREAD_COLOR);
#else
        frame = imread(image,CV_LOAD_IMAGE_COLOR);
#endif

        resize(frame, frame, Size(0,0), 0.2,0.2);

        estimator.focalLength = 85.0 / 22.3 * frame.size().width;
    }

    while(true) {
        if(use_camera) {
            auto ok = video_in.read(frame);
            if (!ok) break;
        }

        auto all_features = estimator.update(frame);

        auto poses = estimator.poses();

        int i = 0;
        cout << "{";

        for(auto pose : poses) {

            pose = pose.inv();

            double raw_yaw, raw_pitch, raw_roll;
            tf::Matrix3x3 mrot(
                    pose(0,0), pose(0,1), pose(0,2),
                    pose(1,0), pose(1,1), pose(1,2),
                    pose(2,0), pose(2,1), pose(2,2));
            mrot.getRPY(raw_roll, raw_pitch, raw_yaw);

            raw_roll = raw_roll - M_PI/2;
            raw_yaw = raw_yaw + M_PI/2;

            double yaw, pitch, roll;

            roll = raw_pitch;
            yaw = raw_yaw;
            pitch = -raw_roll;

            cout << "\"face_" << i << "\":";
            cout << setprecision(1) << fixed << "{\"yaw\":" << todeg(yaw) << ", \"pitch\":" << todeg(pitch) << ", \"roll\":" << todeg(roll) << ",";
            cout << setprecision(4) << fixed << "\"x\":" << pose(0,3) << ", \"y\":" << pose(1,3) << ", \"z\":" << pose(2,3) << "},";

            i++;
        }
        cout << "}\n" << flush;

        if (show_frame) {
            imshow("headpose", estimator.drawDetections(frame, all_features, poses));
            if(use_camera) {
                waitKey(10);
            }
            else {
                while(waitKey(10) != 1048603) {}
                break;
            }
        }
    }

}