/*    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.
 */

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include <Eigen/Core>

#include <tudat/astro/basic_astro/physicalConstants.h>
#include <tudat/basics/testMacros.h>
#include <tudat/math/basic/mathematicalConstants.h>
#include "tudat/astro/basic_astro/unitConversions.h"
#include <tudat/astro/basic_astro/orbitalElementConversions.h>
#include "tudat/astro/gravitation/librationPoint.h"
#include "tudat/astro/gravitation/unitConversionsCircularRestrictedThreeBodyProblem.h"
#include "tudat/simulation/propagation_setup/propagationCR3BPFullProblem.h"

#include <tudat/simulation/simulation.h>
#include <tudat/io/basicInputOutput.h>
#include <tudat/io/applicationOutput.h>
#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/astro/gravitation/unitConversionsCircularRestrictedThreeBodyProblem.h"

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

    spice_interface::loadStandardSpiceKernels( );

    // Global characteristics of the problem
    double distanceSunJupiter = 778.0e9;

    // Initialise the spacecraft state (B. Taylor, D. (1981). Horseshoe periodic orbits in the restricted problem of three bodies
    // for a sun-Jupiter mass ratio. Astronomy and Astrophysics. 103. 288-294.)
    Eigen::Vector6d initialState = Eigen::Vector6d::Zero();
    initialState[0] = - 7.992e11;
    initialState[4] =  -1.29e4;

    // Create integrator settings.
    double initialTime = 0.0;
    const double fixedStepSize = 100000.0;
    std::shared_ptr< numerical_integrators::IntegratorSettings< > > integratorSettings =
            std::make_shared < numerical_integrators::IntegratorSettings < > >
            ( numerical_integrators::rungeKutta4, initialTime, fixedStepSize );

    // Create system of bodies.
    std::vector < std::string > bodiesCR3BP;
    bodiesCR3BP.push_back( "Sun" );
    bodiesCR3BP.push_back( "Jupiter" );

    // Define propagator settings variables.
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;
    bodiesToPropagate.push_back( "Spacecraft" );
    centralBodies.push_back( "SSB" );

    // Define final time for the propagation.
    double gravitationalParameterSun = createGravityFieldModel(
                getDefaultGravityFieldSettings(
                    "Sun", TUDAT_NAN, TUDAT_NAN ), "Sun" )->getGravitationalParameter( );
    double gravitationalParameterJupiter = createGravityFieldModel(
                getDefaultGravityFieldSettings(
                    "Jupiter", TUDAT_NAN, TUDAT_NAN ), "Jupiter" )->getGravitationalParameter( );
    double finalTime = tudat::circular_restricted_three_body_problem::convertDimensionlessTimeToDimensionalTime(
                29.2386 * ( 2.0 * mathematical_constants::PI ), gravitationalParameterSun, gravitationalParameterJupiter, distanceSunJupiter);

    SystemOfBodies idealBodyMap = propagators::setupBodyMapCR3BP(
                distanceSunJupiter, "Sun", "Jupiter", "Spacecraft" );

    std::map< double, Eigen::Vector6d> fullPropagation;
    std::map< double, Eigen::Vector6d> cr3bpPropagation;

    /// Ideal case: full dynamics problem with the CR3BP assumtions
    {

        // Create acceleration map.
        basic_astrodynamics::AccelerationMap accelerationModelMap = propagators::setupAccelerationMapCR3BP(
                    "Sun", "Jupiter", bodiesToPropagate.at( 0 ), centralBodies.at( 0 ), idealBodyMap );

        // Calculate the difference between CR3BP and full problem.
        propagators::propagateCR3BPAndFullDynamicsProblem(
                    initialTime, finalTime, initialState, integratorSettings, accelerationModelMap,
                    bodiesToPropagate, centralBodies, idealBodyMap, bodiesCR3BP, fullPropagation,
                    cr3bpPropagation );

        Eigen::Vector6d stateDifference =
                fullPropagation.rbegin( )->second - cr3bpPropagation.rbegin( )->second;

        std::cout << "state difference at final time: " << stateDifference << std::endl;
    }

    std::map< double, Eigen::Vector6d> fullPropagationPerturbedCase;
    std::map< double, Eigen::Vector6d> cr3bpPropagationPerturbedCase;

    /// Perturbed case
    {

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

        SystemOfBodies perturbedBodyMap;

        std::vector< std::string > additionalBodies = { "Earth", "Mars", "Venus", "Saturn" };
        for( unsigned int i = 0; i < additionalBodies.size( ); i++ )
        {

            perturbedBodyMap[ additionalBodies.at( i ) ] = std::make_shared< Body >( );
            perturbedBodyMap[ additionalBodies.at( i ) ]->setEphemeris(
                        std::make_shared< ephemerides::ApproximatePlanetPositions>(
                                additionalBodies.at( i ) ) );
            perturbedBodyMap[ additionalBodies.at( i ) ]->setGravityFieldModel(
                        createGravityFieldModel(
                            std::make_shared< CentralGravityFieldSettings >(
                                spice_interface::getBodyGravitationalParameter(
                                    additionalBodies.at( i ) ) ), additionalBodies.at( i ) ) );
        }

        perturbedBodyMap[ "Sun" ] = idealBodyMap[ "Sun" ];
        perturbedBodyMap[ "Jupiter" ] = idealBodyMap[ "Jupiter" ];

        // Create the body to be propagated.
        perturbedBodyMap[ "Spacecraft" ] = std::make_shared< Body >( );
        perturbedBodyMap[ "Spacecraft" ]->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                                                            std::shared_ptr< interpolators::OneDimensionalInterpolator
                                                            < double, Eigen::Vector6d > >( ), "SSB", frameOrientation ) );

        setGlobalFrameBodyEphemerides( perturbedBodyMap, frameOrigin, frameOrientation );

        // Set of accelerations experienced by the spacecraft.
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > bodyToPropagateAccelerations;
        bodyToPropagateAccelerations["Sun"].push_back(std::make_shared< AccelerationSettings >(
                                                          basic_astrodynamics::central_gravity ) );
        bodyToPropagateAccelerations["Jupiter"].push_back(std::make_shared< AccelerationSettings >(
                                                              basic_astrodynamics::central_gravity ) );
        bodyToPropagateAccelerations["Earth"].push_back(std::make_shared< AccelerationSettings >(
                                                            basic_astrodynamics::central_gravity ) );
        bodyToPropagateAccelerations["Mars"].push_back(std::make_shared< AccelerationSettings >(
                                                           basic_astrodynamics::central_gravity ) );
        bodyToPropagateAccelerations["Venus"].push_back(std::make_shared< AccelerationSettings >(
                                                            basic_astrodynamics::central_gravity ) );
        bodyToPropagateAccelerations["Saturn"].push_back(std::make_shared< AccelerationSettings >(
                                                             basic_astrodynamics::central_gravity ) );

        SelectedAccelerationMap accelerationMap;
        accelerationMap[ "Spacecraft" ] = bodyToPropagateAccelerations;

        // Create the acceleration map.
        basic_astrodynamics::AccelerationMap accelerationModelMapPerturbedCase = createAccelerationModelsMap(
                    perturbedBodyMap, accelerationMap, bodiesToPropagate, centralBodies );

        // Calculate the difference between CR3BP and full problem.
        propagators::propagateCR3BPAndFullDynamicsProblem(
                    initialTime, finalTime, initialState, integratorSettings,
                    accelerationModelMapPerturbedCase,
                    bodiesToPropagate, centralBodies, perturbedBodyMap, bodiesCR3BP,
                    fullPropagationPerturbedCase,
                    cr3bpPropagationPerturbedCase );

        Eigen::Vector6d stateDifferencePerturbedCase =
                fullPropagationPerturbedCase.rbegin( )->second - cr3bpPropagationPerturbedCase.rbegin( )->second;

        std::cout << "state difference at final time for the perturbed case: " << stateDifferencePerturbedCase << std::endl;

    }

    /// Ouputs

    // Outputs for the ideal case
    {
        std::map< double, Eigen::Vector6d > fullPropagationNormalisedCoRotatingFrame;
        for( std::map< double, Eigen::Vector6d >::iterator itr = fullPropagation.begin( );
             itr != fullPropagation.end( ); itr++ ){
            fullPropagationNormalisedCoRotatingFrame[ itr->first ] = tudat::circular_restricted_three_body_problem::convertCartesianToCorotatingNormalizedCoordinates(
                        gravitationalParameterSun, gravitationalParameterJupiter, distanceSunJupiter, itr->second, itr->first);
        }

        std::map< double, Eigen::Vector6d > cr3bpNormalisedCoRotatingFrame;
        for( std::map< double, Eigen::Vector6d >::iterator itr = cr3bpPropagation.begin( );
             itr != cr3bpPropagation.end( ); itr++ ){
            cr3bpNormalisedCoRotatingFrame[ itr->first ] = tudat::circular_restricted_three_body_problem::convertCartesianToCorotatingNormalizedCoordinates(
                        gravitationalParameterSun, gravitationalParameterJupiter, distanceSunJupiter, itr->second, itr->first);
        }

        input_output::writeDataMapToTextFile( fullPropagation,
                                              "fullProblemPropagation.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( fullPropagationNormalisedCoRotatingFrame,
                                              "fullProblemPropagationNormalisedCoRotatingFrame.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( cr3bpPropagation,
                                              "CR3BPsolution.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( cr3bpNormalisedCoRotatingFrame,
                                              "CR3BPnormalisedCoRotatingFrame.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );
    }

    // Outputs for the perturbed case
    {
        std::map< double, Eigen::Vector6d > fullPropagationNormalisedCoRotatingFramePerturbedCase;
        for( std::map< double, Eigen::Vector6d >::iterator itr = fullPropagationPerturbedCase.begin( );
             itr != fullPropagationPerturbedCase.end( ); itr++ ){
            fullPropagationNormalisedCoRotatingFramePerturbedCase[ itr->first ] = tudat::circular_restricted_three_body_problem::
                    convertCartesianToCorotatingNormalizedCoordinates(gravitationalParameterSun, gravitationalParameterJupiter,
                                                                      distanceSunJupiter, itr->second, itr->first);
        }

        std::map< double, Eigen::Vector6d > cr3bpNormalisedCoRotatingFramePerturbedCase;
        for( std::map< double, Eigen::Vector6d >::iterator itr = cr3bpPropagationPerturbedCase.begin( );
             itr != cr3bpPropagationPerturbedCase.end( ); itr++ ){
            cr3bpNormalisedCoRotatingFramePerturbedCase[ itr->first ] = tudat::circular_restricted_three_body_problem::
                    convertCartesianToCorotatingNormalizedCoordinates(
                        gravitationalParameterSun, gravitationalParameterJupiter, distanceSunJupiter, itr->second, itr->first);
        }

        input_output::writeDataMapToTextFile( fullPropagationPerturbedCase,
                                              "fullProblemPropagationPerturbedCase.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( fullPropagationNormalisedCoRotatingFramePerturbedCase,
                                              "fullProblemPropagationNormalisedCoRotatingFramePerturbedCase.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( cr3bpPropagationPerturbedCase,
                                              "CR3BPsolutionPerturbedCase.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );

        input_output::writeDataMapToTextFile( cr3bpNormalisedCoRotatingFramePerturbedCase,
                                              "CR3BPnormalisedCoRotatingFramePerturbedCase.dat",
                                              tudat_applications::getOutputPath( ),
                                              "",
                                              std::numeric_limits< double >::digits10,
                                              std::numeric_limits< double >::digits10,
                                              "," );
    }

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


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

#include <thread>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Dense>

#include <stdio.h>

#include "patch.h"

using std::cout;
using std::endl;
using std::vector;

namespace OFC
{

  typedef __v4sf v4sf;

  PatClass::PatClass(
    const camparam* cpt_in,
    const camparam* cpo_in,
    const optparam* op_in,
    const int patchid_in)
  :
    cpt(cpt_in),
    cpo(cpo_in),
    op(op_in),
    patchid(patchid_in)
{
  pc = new patchstate;
  CreateStatusStruct(pc);

  tmp.resize(op->novals,1);
  dxx_tmp.resize(op->novals,1);
  dyy_tmp.resize(op->novals,1);
}

void PatClass::CreateStatusStruct(patchstate * psin)
{
  // get reference / template patch
  psin->pdiff.resize(op->novals,1);
  psin->pweight.resize(op->novals,1);
}

PatClass::~PatClass()
{
  delete pc;
}

void PatClass::InitializePatch(Eigen::Map<const Eigen::MatrixXf> * im_ao_in, Eigen::Map<const Eigen::MatrixXf> * im_ao_dx_in, Eigen::Map<const Eigen::MatrixXf> * im_ao_dy_in, const Eigen::Vector2f pt_ref_in)
{
  im_ao = im_ao_in;
  im_ao_dx = im_ao_dx_in;
  im_ao_dy = im_ao_dy_in;

  pt_ref = pt_ref_in;
  ResetPatch();

  getPatchStaticNNGrad(im_ao->data(), im_ao_dx->data(), im_ao_dy->data(), &pt_ref, &tmp, &dxx_tmp, &dyy_tmp);

  ComputeHessian();
}

void PatClass::ComputeHessian()
{
  #if (SELECTMODE==1)
  pc->Hes(0,0) = (dxx_tmp.array() * dxx_tmp.array()).sum();
  pc->Hes(0,1) = (dxx_tmp.array() * dyy_tmp.array()).sum();
  pc->Hes(1,1) = (dyy_tmp.array() * dyy_tmp.array()).sum();
  pc->Hes(1,0) = pc->Hes(0,1);
  if (pc->Hes.determinant()==0)
  {
    pc->Hes(0,0)+=1e-10;
    pc->Hes(1,1)+=1e-10;
  }
  #else
  pc->Hes(0,0) = (dxx_tmp.array() * dxx_tmp.array()).sum();
  if (pc->Hes.sum()==0)
    pc->Hes(0,0)+=1e-10;
  #endif
}

void PatClass::SetTargetImage(Eigen::Map<const Eigen::MatrixXf> * im_bo_in, Eigen::Map<const Eigen::MatrixXf> * im_bo_dx_in, Eigen::Map<const Eigen::MatrixXf> * im_bo_dy_in)
{
  im_bo = im_bo_in;
  im_bo_dx = im_bo_dx_in;
  im_bo_dy = im_bo_dy_in;

  ResetPatch();
}

void PatClass::ResetPatch()
{
  pc->hasconverged=0;
  pc->hasoptstarted=0;

  pc->pt_st = pt_ref;
  pc->pt_iter = pt_ref;

  pc->p_in.setZero();
  pc->p_iter.setZero();
  pc->delta_p.setZero();

  pc->delta_p_sqnorm = 1e-10;
  pc->delta_p_sqnorm_init = 1e-10;
  pc->mares = 1e20;
  pc->mares_old = 1e20;
  pc->cnt=0;
  pc->invalid = false;
}

#if (SELECTMODE==1)
void PatClass::OptimizeStart(const Eigen::Vector2f p_in_arg)
#else
void PatClass::OptimizeStart(const Eigen::Matrix<float, 1, 1> p_in_arg)
#endif
{
  pc->p_in   = p_in_arg;
  pc->p_iter = p_in_arg;

  // convert from input parameters to 2D query location(s) for patches
  paramtopt();

  // save starting location, only needed for outlier check
  pc->pt_st = pc->pt_iter;

  //Check if initial position is already invalid
  if (pc->pt_iter[0] < cpt->tmp_lb  || pc->pt_iter[1] < cpt->tmp_lb ||    // check if patch left valid image region
      pc->pt_iter[0] > cpt->tmp_ubw || pc->pt_iter[1] > cpt->tmp_ubh)
  {
    pc->hasconverged=1;
    pc->pdiff = tmp;
    pc->hasoptstarted=1;
  }
  else
  {
    pc->cnt=0; // reset iteration counter
    pc->delta_p_sqnorm = 1e-10;
    pc->delta_p_sqnorm_init = 1e-10;  // set to arbitrary low value, s.t. that loop condition is definitely true on first iteration
    pc->mares = 1e5;          // mean absolute residual
    pc->mares_old = 1e20; // for rate of change, keep mares from last iteration in here. Set high so that loop condition is definitely true on first iteration
    pc->hasconverged=0;

    OptimizeComputeErrImg();

    pc->hasoptstarted=1;
    pc->invalid = false;
  }
}

#if (SELECTMODE==1)
void PatClass::OptimizeIter(const Eigen::Vector2f p_in_arg, const bool untilconv)
#else
void PatClass::OptimizeIter(const Eigen::Matrix<float, 1, 1> p_in_arg, const bool untilconv)
#endif
{
  if (!pc->hasoptstarted)
  {
    ResetPatch();
    OptimizeStart(p_in_arg);
  }
  int oldcnt=pc->cnt;

  // optimize patch until convergence, or do only one iteration if DIS visualization is used
  while (  ! (pc->hasconverged || (untilconv == false && (pc->cnt > oldcnt)))  )
  {
    pc->cnt++;

    // Projection onto sd_images
    #if (SELECTMODE==1)
      pc->delta_p[0] = (dxx_tmp.array() * pc->pdiff.array()).sum();
      pc->delta_p[1] = (dyy_tmp.array() * pc->pdiff.array()).sum();
    #else
      pc->delta_p[0] = (dxx_tmp.array() * pc->pdiff.array()).sum();
    #endif

    pc->delta_p = pc->Hes.llt().solve(pc->delta_p); // solve linear system

    pc->p_iter -= pc->delta_p; // update flow vector

    #if (SELECTMODE==2) // if stereo depth
    if (cpt->camlr==0)
      pc->p_iter[0] = std::min(pc->p_iter[0],0.0f); // disparity in t can only be negative (in right image)
    else
      pc->p_iter[0] = std::max(pc->p_iter[0],0.0f); // ... positive (in left image)
    #endif

    // compute patch locations based on new parameter vector
    paramtopt();

    // check if patch(es) moved too far from starting location, if yes, stop iteration and reset to starting location
    if ((pc->pt_st - pc->pt_iter).norm() > op->outlierthresh  // check if query patch moved more than >padval from starting location -> most likely outlier
        ||
        pc->pt_iter[0] < cpt->tmp_lb  || pc->pt_iter[1] < cpt->tmp_lb ||    // check patch left valid image region
        pc->pt_iter[0] > cpt->tmp_ubw || pc->pt_iter[1] > cpt->tmp_ubh)
    {
      pc->p_iter = pc->p_in; // reset
      paramtopt();
      pc->hasconverged=1;
      pc->hasoptstarted=1;
    }

    OptimizeComputeErrImg();
  }
}

inline void PatClass::paramtopt()
{
    #if (SELECTMODE==1)
      pc->pt_iter = pt_ref + pc->p_iter;    // for optical flow the point displacement and the parameter vector are equivalent
    #else
      pc->pt_iter[0] = pt_ref[0] + pc->p_iter[0];
    #endif
}

void PatClass::LossComputeErrorImage(Eigen::Matrix<float, Eigen::Dynamic, 1>* patdest, Eigen::Matrix<float, Eigen::Dynamic, 1>* wdest, const Eigen::Matrix<float, Eigen::Dynamic, 1>* patin,  const Eigen::Matrix<float, Eigen::Dynamic, 1>*  tmpin)
{
  v4sf * pd = (v4sf*) patdest->data(),
       * pa = (v4sf*) patin->data(),
       * te = (v4sf*) tmpin->data(),
       * pw = (v4sf*) wdest->data();

  if (op->costfct==0) // L2 cost function
  {
    for (int i=op->novals/4; i--; ++pd, ++pa, ++te, ++pw)
    {
      (*pd) = (*pa)-(*te);  // difference image
      (*pw) = __builtin_ia32_andnps(op->negzero,  (*pd) );
    }
  }
  else if (op->costfct==1) // L1 cost function
  {
    for (int i=op->novals/4; i--; ++pd, ++pa, ++te, ++pw)
    {
      (*pd) = (*pa)-(*te);   // difference image
      (*pd) = __builtin_ia32_orps( __builtin_ia32_andps(op->negzero,  (*pd) )  , __builtin_ia32_sqrtps (__builtin_ia32_andnps(op->negzero,  (*pd) )) );  // sign(pdiff) * sqrt(abs(pdiff))
      (*pw) = __builtin_ia32_andnps(op->negzero,  (*pd) );
    }
  }
  else if (op->costfct==2) // Pseudo Huber cost function
  {
    for (int i=op->novals/4; i--; ++pd, ++pa, ++te, ++pw)
    {
      (*pd) = (*pa)-(*te);   // difference image
      (*pd) = __builtin_ia32_orps(__builtin_ia32_andps(op->negzero,  (*pd) ),
                                  __builtin_ia32_sqrtps (
                                    __builtin_ia32_mulps(                                                                                // PSEUDO HUBER NORM
                                          __builtin_ia32_sqrtps (op->ones + __builtin_ia32_divps(__builtin_ia32_mulps((*pd),(*pd)) , op->normoutlier_tmpbsq)) - op->ones, // PSEUDO HUBER NORM
                                          op->normoutlier_tmp2bsq)                                                                                // PSEUDO HUBER NORM
                                     )
                                    ); // sign(pdiff) * sqrt( 2*b^2*( sqrt(1+abs(pdiff)^2/b^2)+1)  )) // <- looks like this without SSE instruction
      (*pw) = __builtin_ia32_andnps(op->negzero,  (*pd) );
    }
  }
}

void PatClass::OptimizeComputeErrImg()
{
  getPatchStaticBil(im_bo->data(), &(pc->pt_iter), &(pc->pdiff));

  // Get photometric patch error
  LossComputeErrorImage(&pc->pdiff, &pc->pweight, &pc->pdiff, &tmp);

  // Compute step norm
  pc->delta_p_sqnorm = pc->delta_p.squaredNorm();
  if (pc->cnt==1)
    pc->delta_p_sqnorm_init = pc->delta_p_sqnorm;

  // Check early termination criterions
  pc->mares_old = pc->mares;
  pc->mares = pc->pweight.lpNorm<1>() / (op->novals);
  if ( !  ((pc->cnt < op->max_iter) &  (pc->mares  > op->res_thresh) &
          ((pc->cnt < op->min_iter) |  (pc->delta_p_sqnorm / pc->delta_p_sqnorm_init >= op->dp_thresh)) &
          ((pc->cnt < op->min_iter) |  (pc->mares / pc->mares_old <= op->dr_thresh)))  )
    pc->hasconverged=1;

}

// Extract patch on integer position, and gradients, No Bilinear interpolation
void PatClass::getPatchStaticNNGrad(const float* img, const float* img_dx, const float* img_dy,
                    const Eigen::Vector2f* mid_in,
                    Eigen::Matrix<float, Eigen::Dynamic, 1>* tmp_in_e,
                    Eigen::Matrix<float, Eigen::Dynamic, 1>*  tmp_dx_in_e,
                    Eigen::Matrix<float, Eigen::Dynamic, 1>* tmp_dy_in_e)
{
  float *tmp_in    = tmp_in_e->data();
  float *tmp_dx_in = tmp_dx_in_e->data();
  float *tmp_dy_in = tmp_dy_in_e->data();

  Eigen::Vector2i pos;
  Eigen::Vector2i pos_it;

  pos[0] = round((*mid_in)[0]) + cpt->imgpadding;
  pos[1] = round((*mid_in)[1]) + cpt->imgpadding;

  int posxx = 0;

  int lb = -op->p_samp_s/2;
  int ub = op->p_samp_s/2-1;

  for (int j=lb; j <= ub; ++j)
  {
    for (int i=lb; i <= ub; ++i, ++posxx)
    {
      pos_it[0] = pos[0]+i;
      pos_it[1] = pos[1]+j;
      int idx = pos_it[0] + pos_it[1] * cpt->tmp_w;

      #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // Single channel
      tmp_in[posxx] = img[idx];
      tmp_dx_in[posxx] = img_dx[idx];
      tmp_dy_in[posxx] = img_dy[idx];
      #else  // 3 RGB channels
      idx *= 3;
      tmp_in[posxx] = img[idx]; tmp_dx_in[posxx] = img_dx[idx]; tmp_dy_in[posxx] = img_dy[idx]; ++posxx; ++idx;
      tmp_in[posxx] = img[idx]; tmp_dx_in[posxx] = img_dx[idx]; tmp_dy_in[posxx] = img_dy[idx]; ++posxx; ++idx;
      tmp_in[posxx] = img[idx]; tmp_dx_in[posxx] = img_dx[idx]; tmp_dy_in[posxx] = img_dy[idx];
      #endif
    }
  }

  // PATCH NORMALIZATION
  if (op->patnorm>0) // Subtract Mean
    tmp_in_e->array() -= (tmp_in_e->sum() / op->novals);
}

// Extract patch on float position with bilinear interpolation, no gradients.
void PatClass::getPatchStaticBil(const float* img, const Eigen::Vector2f* mid_in,  Eigen::Matrix<float, Eigen::Dynamic, 1>* tmp_in_e)
{
  float *tmp_in    = tmp_in_e->data();

  Eigen::Vector2f resid;
  Eigen::Vector4f we; // bilinear weight vector
  Eigen::Vector4i pos;
  Eigen::Vector2i pos_it;

  // Compute the bilinear weight vector, for patch without orientation/scale change -> weight vector is constant for all pixels
  pos[0] = ceil((*mid_in)[0]+.00001f); // ensure rounding up to natural numbers
  pos[1] = ceil((*mid_in)[1]+.00001f);
  pos[2] = floor((*mid_in)[0]);
  pos[3] = floor((*mid_in)[1]);

  resid[0] = (*mid_in)[0] - (float)pos[2];
  resid[1] = (*mid_in)[1] - (float)pos[3];
  we[0] = resid[0]*resid[1];
  we[1] = (1-resid[0])*resid[1];
  we[2] = resid[0]*(1-resid[1]);
  we[3] = (1-resid[0])*(1-resid[1]);

  pos[0] += cpt->imgpadding;
  pos[1] += cpt->imgpadding;

  float * tmp_it = tmp_in;
  const float * img_a, * img_b, * img_c, * img_d, *img_e;

  #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // 1 channel image
    img_e = img    + pos[0]-op->p_samp_s/2;
  #else                                       // 3-channel RGB image
    img_e = img    + (pos[0]-op->p_samp_s/2)*3;
  #endif

  int lb = -op->p_samp_s/2;
  int ub = op->p_samp_s/2-1;

  for (pos_it[1]=pos[1]+lb; pos_it[1] <= pos[1]+ub; ++pos_it[1])
  {
    #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // 1 channel image
      img_a = img_e +  pos_it[1]    * cpt->tmp_w;
      img_c = img_e + (pos_it[1]-1) * cpt->tmp_w;
      img_b = img_a-1;
      img_d = img_c-1;
    #else                                     // 3-channel RGB image
      img_a = img_e +  pos_it[1]    * cpt->tmp_w * 3;
      img_c = img_e + (pos_it[1]-1) * cpt->tmp_w * 3;
      img_b = img_a-3;
      img_d = img_c-3;
    #endif

    for (pos_it[0]=pos[0]+lb; pos_it[0] <= pos[0]+ub; ++pos_it[0],
            ++tmp_it,++img_a,++img_b,++img_c,++img_d)
    {
      #if (SELECTCHANNEL==1 | SELECTCHANNEL==2)  // Single channel
        (*tmp_it)     = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d);
      #else // 3-channel RGB image
        (*tmp_it)     = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d); ++tmp_it; ++img_a; ++img_b; ++img_c; ++img_d;
        (*tmp_it)     = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d); ++tmp_it; ++img_a; ++img_b; ++img_c; ++img_d;
        (*tmp_it)     = we[0] * (*img_a) + we[1] * (*img_b) + we[2] * (*img_c) + we[3] * (*img_d);
      #endif
    }
  }
  // PATCH NORMALIZATION
  if (op->patnorm>0) // Subtract Mean
    tmp_in_e->array() -= (tmp_in_e->sum() / op->novals);
}

}


#include <iostream>
#include <fstream>
#include <cmath>
#include <boost/assign/list_of.hpp>
#include "source/newtonian/common/hllc.hpp"
#include "source/newtonian/common/ideal_gas.hpp"
#include "source/newtonian/one_dimensional/hdsim.hpp"
#include "source/newtonian/one_dimensional/pcm1d.hpp"
#include "source/newtonian/one_dimensional/plm1d.hpp"
#include "source/newtonian/one_dimensional/eos_consistent1d.hpp"
#include "source/newtonian/one_dimensional/spatial_distribution1d.hpp"
#include "source/newtonian/one_dimensional/eulerian1d.hpp"
#include "source/newtonian/one_dimensional/periodic_1d.hpp"
#include "source/newtonian/one_dimensional/zero_force_1d.hpp"
#include "source/misc/int2str.hpp"
#include "source/misc/utils.hpp"
#include "source/misc/simple_io.hpp"
#include "source/newtonian/one_dimensional/hdf5_diagnostics1d.hpp"
#include "source/newtonian/test_1d/main_loop_1d.hpp"

using namespace std;
using namespace interpolations1d;
using namespace simulation1d;
using namespace diagnostics1d;

namespace {
SpatialReconstruction1D const& choose_between
(SpatialReconstruction1D const& opt1,
 SpatialReconstruction1D const& opt2,
 SpatialReconstruction1D const& opt3,
 string const& name1,
 string const& name2,
 string const& name3,
 string const& choice)
{
  if(name1==choice)
    return opt1;
  else if(name2==choice)
    return opt2;
  else if(name3==choice)
    return opt3;
  else
    throw "Unknown option "+choice;
}

class SimData
{
public:

  SimData(string const& interp_method_name):
    vertices_(linspace(0,1,100)),
    eos_(5./3.),
    pcm_(),
    plm_naive_(),
    plm_(plm_naive_,eos_),
    interpm_(choose_between(pcm_,plm_naive_,plm_,
			    "pcm","plm_naive","plm",
			    interp_method_name)),
    density_(1,0.3,2,0.7,1),
    pressure_(1),
    xvelocity_(1),
    yvelocity_(0),
    rs_(),
    vm_(),
    bc_(),
    force_(),
    sim_(pg_,
	 vertices_,
	 interpm_,
	 density_,
	 pressure_,
	 xvelocity_,
	 yvelocity_,
	 eos_,
	 rs_,
	 vm_,
	 bc_,
	 force_) {}

  hdsim1D& getSim(void)
  {
    return sim_;
  }

private:
  const SlabSymmetry1D pg_;
  const vector<double> vertices_;
  const IdealGas eos_;
  PCM1D pcm_;
  PLM1D plm_naive_;
  EOSConsistent plm_;
  SpatialReconstruction1D const& interpm_;
  const TwoSteps density_;
  const Uniform pressure_;
  const Uniform xvelocity_;
  const Uniform yvelocity_;
  const Hllc rs_;
  const Eulerian1D vm_;
  const Periodic1D bc_;
  const ZeroForce1D force_;
  hdsim1D sim_;
};
}

int main(void)
{
  const vector<string> interp_names =
    boost::assign::list_of("pcm")("plm_naive")("plm");
  for(size_t i=0;i<interp_names.size();++i){
    SimData sim_data(interp_names[i]);

    main_loop(sim_data.getSim(),
	      1, 1e6, 2,
	      "time.txt");

    write_snapshot_to_hdf5(sim_data.getSim(),
			   interp_names[i]+"_final.h5");
  }

  return 0;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2005 Aurelien Chanudet
 Copyright (C) 2005 Plamen Neykov
 Copyright (C) 2005, 2006 Eric Ehlers
 Copyright (C) 2006, 2007 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

#ifdef HAVE_CONFIG_H
#include <qlo/config.hpp>
#endif

#include <qlo/yieldtermstructures.hpp>
#include <qlo/ratehelpers.hpp>

#include <ql/time/date.hpp>
#include <ql/termstructures/yield/discountcurve.hpp>
#include <ql/termstructures/yield/forwardcurve.hpp>
#include <ql/termstructures/yield/zerocurve.hpp>
#include <ql/termstructures/yield/impliedtermstructure.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/termstructures/yield/forwardspreadedtermstructure.hpp>
#include <ql/math/interpolations/cubicinterpolation.hpp>
#include <ql/math/interpolations/forwardflatinterpolation.hpp>
#include <ql/math/interpolations/backwardflatinterpolation.hpp>

#include <boost/algorithm/string/case_conv.hpp>

using boost::algorithm::to_upper_copy;
using boost::shared_ptr;

using ObjectHandler::ValueObject;

using QuantLib::CubicInterpolation;
using QuantLib::InterpolatedDiscountCurve;
using QuantLib::InterpolatedZeroCurve;
using QuantLib::InterpolatedForwardCurve;

namespace QuantLibAddin {

    DiscountCurve::DiscountCurve(
        const shared_ptr<ValueObject>& prop,
        const std::vector<QuantLib::Date>& dates,
        const std::vector<QuantLib::DiscountFactor>& dfs,
        const QuantLib::DayCounter& dayCounter,
        bool perm) : YieldTermStructure(prop, perm)
    {
        QL_REQUIRE(!dates.empty(), "no input dates given");
        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::DiscountCurve(dates, dfs, dayCounter));
    }

    ZeroCurve::ZeroCurve(const shared_ptr<ValueObject>& prop,
                         const std::vector<QuantLib::Date>& dates,
                         const std::vector<QuantLib::Rate>& zeroRates,
                         const QuantLib::DayCounter& dayCounter,
                         bool perm) : YieldTermStructure(prop, perm)
    {
        QL_REQUIRE(!dates.empty(), "no input dates given");
        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::ZeroCurve(dates, zeroRates, dayCounter));
    }

    ForwardCurve::ForwardCurve(const shared_ptr<ValueObject>& prop,
                               const std::vector<QuantLib::Date>& dates,
                               const std::vector<QuantLib::Rate>& fwdRates,
                               const QuantLib::DayCounter& dayCounter,
                               bool perm) : YieldTermStructure(prop, perm)
    {
        QL_REQUIRE(!dates.empty(), "no input dates given");
        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::ForwardCurve(dates, fwdRates, dayCounter));
    }

    FlatForward::FlatForward(const shared_ptr<ValueObject>& prop,
                             QuantLib::Natural nDays,
                             const QuantLib::Calendar& calendar,
                             const QuantLib::Handle<QuantLib::Quote>& forward,
                             const QuantLib::DayCounter& dayCounter,
                             QuantLib::Compounding compounding,
                             QuantLib::Frequency frequency,
                             bool perm)
    : YieldTermStructure(prop, perm)
    {
        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::FlatForward(nDays, calendar, forward, dayCounter,
                                  compounding, frequency));
    }

    ForwardSpreadedTermStructure::ForwardSpreadedTermStructure(
            const shared_ptr<ValueObject>& prop,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& hYTS,
            const QuantLib::Handle<QuantLib::Quote>& spread,
            bool perm) : YieldTermStructure(prop, perm) {

        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::ForwardSpreadedTermStructure(hYTS, spread));
    }

    ImpliedTermStructure::ImpliedTermStructure(
            const shared_ptr<ValueObject>& prop,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& hYTS,
            const QuantLib::Date& referenceDate,
            bool perm)
    : YieldTermStructure(prop, perm)
    {
        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::ImpliedTermStructure(hYTS, referenceDate));
    }

    InterpolatedYieldCurve::InterpolatedYieldCurve(
            const shared_ptr<ValueObject>& prop,
            const std::vector<QuantLib::Date>& dates,
            const std::vector<QuantLib::Real>& data,
            const QuantLib::Calendar& calendar,
            const QuantLib::DayCounter& dayCounter,
            const std::vector<QuantLib::Handle<QuantLib::Quote> >& jumps,
            const std::vector<QuantLib::Date>& jumpDates,
            const std::string& traitsID,
            const std::string& interpolatorID,
            bool perm)
    : YieldTermStructure(prop, perm),
      traitsID_(to_upper_copy(traitsID)),
      interpolatorID_(to_upper_copy(interpolatorID))
    {
        if (traitsID_=="DISCOUNT") {
            if (interpolatorID_=="BACKWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::BackwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="FORWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::ForwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Linear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LOGLINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogLinear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="CUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, false,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, false,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, true,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, true,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="KRUGERCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="KRUGERLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="PARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic, false)));
            } else if (interpolatorID_=="LOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, false)));
            } else if (interpolatorID_=="MONOTONICPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic, true)));
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, true)));
            } else
                QL_FAIL("unknown interpolatorID: " << interpolatorID_);
        } else if (traitsID_=="ZEROYIELD") {
            if (interpolatorID_=="BACKWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::BackwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="FORWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::ForwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Linear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LOGLINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogLinear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="CUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, false,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, false,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, true,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, true,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="KRUGERCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="KRUGERLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="PARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic, false)));
            } else if (interpolatorID_=="LOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, false)));
            } else if (interpolatorID_=="MONOTONICPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic, true)));
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, true)));
            } else
                QL_FAIL("unknown interpolatorID: " << interpolatorID_);
        } else if (traitsID_=="FORWARDRATE") {
            if (interpolatorID_=="BACKWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::BackwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="FORWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::ForwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Linear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LOGLINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogLinear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="CUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, false,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, false,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, true,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, true,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="KRUGERCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="KRUGERLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="PARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic)));
            } else if (interpolatorID_=="LOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, false)));
            } else if (interpolatorID_=="MONOTONICPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic, true)));
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, true)));
            } else
                QL_FAIL("unknown interpolatorID: " << interpolatorID_);
        } else
            QL_FAIL("unknown traitsID: " << traitsID_);

    }

    #define RESOLVE_TEMPLATE(NAME) \
        if (traitsID_=="DISCOUNT") { \
            if (interpolatorID_=="BACKWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::BackwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FORWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::ForwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Linear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGLINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogLinear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="CUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="PARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else \
                QL_FAIL("unknown interpolatorID: " << interpolatorID_); \
        } else if (traitsID_=="ZEROYIELD") { \
            if (interpolatorID_=="BACKWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::BackwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FORWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::ForwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Linear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGLINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogLinear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="CUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="PARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else \
                QL_FAIL("unknown interpolatorID: " << interpolatorID_); \
        } else if (traitsID_=="FORWARDRATE") { \
            if (interpolatorID_=="BACKWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::BackwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FORWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::ForwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Linear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGLINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogLinear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="CUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="PARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else \
                QL_FAIL("unknown interpolatorID: " << interpolatorID_); \
        } else \
            QL_FAIL("unknown traitsID: " << traitsID_);

    const std::vector<QuantLib::Time>& InterpolatedYieldCurve::times() const {
        RESOLVE_TEMPLATE(times)
    }

    const std::vector<QuantLib::Date>& InterpolatedYieldCurve::dates() const {
        RESOLVE_TEMPLATE(dates)
    }

    const std::vector<QuantLib::Real>& InterpolatedYieldCurve::data() const {
        RESOLVE_TEMPLATE(data)
    }

    const std::vector<QuantLib::Time>& InterpolatedYieldCurve::jumpTimes() const {
        RESOLVE_TEMPLATE(jumpTimes)
    }

    const std::vector<QuantLib::Date>& InterpolatedYieldCurve::jumpDates() const {
        RESOLVE_TEMPLATE(jumpDates)
    }

    InterpolatedYieldCurve::InterpolatedYieldCurve(
            const shared_ptr<ValueObject>& prop,
            const std::string& traitsID,
            const std::string& interpolatorID,
            bool perm)
    : YieldTermStructure(prop, perm),
      traitsID_(to_upper_copy(traitsID)),
      interpolatorID_(to_upper_copy(interpolatorID))
    {
    }

    // Stream operator to write a InterpolatedYieldCurvePair to a stream - for logging / error handling.
    std::ostream &operator<<(std::ostream &out,
                             InterpolatedYieldCurvePair tokenPair)
    {
        out << "InterpolatedYieldCurve<";

        switch (tokenPair.first) {
            case InterpolatedYieldCurve::Discount:
                out << "<Discount, ";
                break;
            case InterpolatedYieldCurve::ForwardRate:
                out << "<ForwardRate, ";
                break;
            case InterpolatedYieldCurve::ZeroYield:
                out << "<ZeroYield, ";
                break;
            default:
                OH_FAIL("Unknown value for enumeration QuantLibAddin::InterpolatedYieldCurve::Traits");
        }

        switch (tokenPair.second) {
            case InterpolatedYieldCurve::BackwardFlat:
                out << "BackwardFlat>";
                break;
            case InterpolatedYieldCurve::ForwardFlat:
                out << "ForwardFlat>";
                break;
            case InterpolatedYieldCurve::Linear:
                out << "Linear>";
                break;
            case InterpolatedYieldCurve::LogLinear:
                out << "LogLinear>";
                break;
            case InterpolatedYieldCurve::CubicNaturalSpline:
                out << "CubicNaturalSpline>";
                break;
            case InterpolatedYieldCurve::LogCubicNaturalSpline:
                out << "LogCubicNaturalSpline>";
                break;
            case InterpolatedYieldCurve::MonotonicCubicNaturalSpline:
                out << "MonotonicCubicNaturalSpline>";
                break;
            case InterpolatedYieldCurve::MonotonicLogCubicNaturalSpline:
                out << "MonotonicLogCubicNaturalSpline>";
                break;
            case InterpolatedYieldCurve::KrugerCubic:
                out << "KrugerCubic>";
                break;
            case InterpolatedYieldCurve::KrugerLogCubic:
                out << "KrugerLogCubic>";
                break;
            case InterpolatedYieldCurve::FritschButlandCubic:
                out << "FritschButlandCubic>";
                break;
            case InterpolatedYieldCurve::FritschButlandLogCubic:
                out << "FritschButlandLogCubic>";
                break;
            case InterpolatedYieldCurve::Parabolic:
                out << "Parabolic>";
                break;
            case InterpolatedYieldCurve::LogParabolic:
                out << "LogParabolic>";
                break;
            case InterpolatedYieldCurve::MonotonicParabolic:
                out << "MonotonicParabolic>";
                break;
            case InterpolatedYieldCurve::MonotonicLogParabolic:
                out << "MonotonicLogParabolic>";
                break;
            default:
                OH_FAIL("Unknown value for enumeration QuantLibAddin::InterpolatedYieldCurve::Interpolator");
        }

        return out;
    }

}


#pragma once

#include <functional>

#include <boost/math/special_functions/prime.hpp>
#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/miller_rabin.hpp>
#include <boost/random.hpp>
#include <boost/random/random_device.hpp>
#include <exrandom/discrete_normal_distribution.hpp>
#include <nfl/params.hpp>

#include <gmpxx.h>
#include <sodium/randombytes.h>
#include "Common.hpp"

/** Overriding abs for basic int type **/
namespace boost {
namespace multiprecision {
RegularInteger abs(RegularInteger in) { return in; }
}  // namespace multiprecision
}  // namespace boost

namespace ligero {
namespace math {

/** Scaling function to bring randomness within a desired range
 * @param val input, modulusIdx raw randomness, prime index
 * @return scaled randomness
 */

uint64_t scaleToPrime(uint64_t input, size_t modulusIdx) {
    double scaleFactor = static_cast<double>(nfl::params<uint64_t>::P[modulusIdx])/static_cast<double>(1ull << 62);
    uint64_t input62 = input & ((1ull << 62) - 1);
    return static_cast<uint64_t>(static_cast<double>(input62)*scaleFactor);
}

uint64_t scaleToUpperBound(uint64_t input, size_t upperBound) {
    float scaleFactor = static_cast<double>(upperBound)/static_cast<double>(1ull << 62);
    uint64_t input62 = input & ((1ull << 62) - 1);
    return static_cast<uint64_t>(static_cast<double>(input62)*scaleFactor);
}

/** Compute hash for given mpz value
 * @param val MPZ_class value
 * @return Hash of @p val
 */
std::string computeHash(mpz_class val) {
    std::string s = val.get_str();
    std::string os = val.get_str();

    unsigned char hash[crypto_generichash_BYTES + 1];
    const unsigned char *message = reinterpret_cast<const unsigned char *> (s.c_str());
    crypto_generichash(hash, crypto_generichash_BYTES,
            message, s.length(),
            NULL, 0);
    hash[crypto_generichash_BYTES] = 0;
    DBG("For value os = " << os << " s = " << s << " generating hash = >>>" << hash << "<<<");
    return std::move(std::string(reinterpret_cast<char*>(hash)));
}

/** Compute hash for given NFL polynomial
 * @param val NFL polynomial
 * @return Hash of @p val
 */
template <typename T, size_t Degree, size_t NbPrimesQ>
std::string computeHash(nfl::poly_p<T, Degree, NbPrimesQ> val) {
    std::stringstream ss(std::stringstream::out | std::stringstream::binary);
    val.serialize_manually(ss);
    std::string s = ss.str();

    unsigned char hash[crypto_generichash_BYTES + 1];
    const unsigned char *message = reinterpret_cast<const unsigned char *> (s.c_str());
    crypto_generichash(hash, crypto_generichash_BYTES,
            message, s.length(),
            NULL, 0);
    hash[crypto_generichash_BYTES] = 0;
    std::string result = std::string(reinterpret_cast<char*>(hash));
    return result;
}

/** Compute hash for given value
 * @param val ((a, b), c), where a, b is vector of mpz_class
 * @return Hash of c
 */
std::string computeHash(std::pair<std::pair<std::vector<mpz_class>, std::vector<mpz_class>>, mpz_class> val) {
    mpz_class v = val.second;
    return computeHash(v);
}

/** Default hash function for any type, will throw a runtime exception */
template <typename T>
std::string computeHash(T val) {
    LOG(FATAL) << "computeHash for this type is not implemented yet";
}

/** Generate a vector of random numbers
 * @param seed Seed to initialize the random generator
 * @param size Length of the output random vector
 * @param numBits Number of bits of each value, default to 128 bits
 * @return A vector of random numbers
 **/
std::vector<mpz_class> generateRandomVector(mpz_class seed, size_t size, size_t numBits = 128) {
    std::vector<mpz_class> result(size);

    const int numBytes = lround(double(numBits) / 8);
    const size_t bufferSize = size * (numBytes + 1);

    char* buf = new char[bufferSize];

    size_t seedSize = (mpz_sizeinbase (seed.get_mpz_t(), 2) + CHAR_BIT-1) / CHAR_BIT;
    unsigned char seedArray[32U];
    std::vector<unsigned char> seedVector(seedSize);

    mpz_export(&seedVector[0], &seedSize, 1, 1, 0, 0, seed.get_mpz_t());
    for (size_t i = 0; i < seedSize && i < 32; ++i) {
        seedArray[i] = seedVector[i];
    }
    for (size_t i = seedSize; i < 32; ++i) {
        seedArray[i] = static_cast<unsigned char>(0);
    }

    randombytes_buf_deterministic((void*)buf, bufferSize, seedArray);

    size_t k = 0;
    for (size_t i = 0; i < result.size(); ++i) {
        assert(k < bufferSize);
        mpz_import(result[i].get_mpz_t(), numBytes, 1, 1, 0, 0, &buf[k]);
        k += numBytes;
        assert(result[i] > 0);
    }
    delete[] buf;

    return result;
}

/** Generate a single mpz random value
 * @param seed Seed to initialize random generator
 * @param numBits Number of bits of generated value
 * @return @p numBits bits random value
 */
mpz_class generateRandomValue(mpz_class seed, size_t numBits = 128) {
    mpz_class result;
    {
        std::vector t = generateRandomVector(seed, 1, numBits);
        result = t[0];
    }
    return result;
}

/** A quick wrapper around GMP's powm implementation for different types. */
mpz_class powm(const mpz_class& b, const mpz_class& p, const mpz_class& m) {
    mpz_class x(b);
    mpz_powm(x.get_mpz_t(), x.get_mpz_t(), p.get_mpz_t(), m.get_mpz_t());

    return x;
}

/** A quick wrapper around GMP's powm implementation for different types. */
template <typename NumberType>
NumberType powm(const NumberType& b, const NumberType& p, const NumberType& m) {
    MPInt x(b);
    MPInt y(p);
    MPInt z(m);

    MPInt o;

    mpz_powm(o.backend().data(),
             x.backend().data(),
             y.backend().data(),
             z.backend().data());

    return NumberType(o);
}

/** A quick wrapper around GMP's powm implementation for different types. */
RegularInteger powm (const RegularInteger& b, const long p, const RegularInteger& m) {
    MPInt x (b);
    MPInt y (p);
    MPInt z (m);

    mpz_powm(
        x.backend().data(),
        x.backend().data(),
        y.backend().data(),
        z.backend().data()
    );

    return RegularInteger(x);
}

/** Divide and round the quotient to nearest integer
 * @param a divident
 * @param b divisor
 * @return quotient rounded to nearest integer
 */
Int64 divideAndRound(const Int64& a, const Int64& b) {
    Int64 quotient, remainder;
    boost::multiprecision::divide_qr(a, b, quotient, remainder);
    if (remainder * 2 >= b) {
        quotient++;
    }
    return quotient;
}

/** 128 bits power
 * @param a 64 bits integer
 * @param exp 64 bits integer
 * @return 128 bits @p a ^ @p exp
 */
WideInteger pow(RegularInteger a, RegularInteger exp) {
    MPInt x(0);
    MPInt y(a);

    mpz_pow_ui(
        x.backend().data(),
        y.backend().data(),
        exp);

    return WideInteger(x);
}

WideInteger pow(RegularInteger a, int exp) {
    MPInt x(0);
    MPInt y(a);

    mpz_pow_ui(
        x.backend().data(),
        y.backend().data(),
        exp);

    return WideInteger(x);
}

/** N bits power
 * @param a N bits number of type T
 * @param exp 64 bits integer
 * @return N bits @p a ^ @p exp of type T
 */
template <typename NumberType>
NumberType pow(NumberType a, uint64_t exp) {
    MPInt x(0);
    MPInt y(a);

    mpz_pow_ui(
        x.backend().data(),
        y.backend().data(),
        exp);

    return NumberType(x);
}

/** Divide and round the quotient to nearest integer
 * @param a divident of type mpz_int
 * @param b divisor of type mpz_int
 * @return quotient rounded to nearest integer
 */
MPInt divideAndRound(const MPInt& a, const MPInt& b) {
    MPInt quotient, remainder;
    boost::multiprecision::divide_qr(a, b, quotient, remainder);
    if (remainder * 2 >= b) {
        quotient++;
    }
    return quotient;
}

/** Divide and round the quotient to nearest integer
 * @param a divident of type int
 * @param b divisor of type int
 * @return quotient rounded to nearest integer
 */
int64_t divideAndRound(int64_t a, int64_t b) {
    int64_t remainder = a % b;
    int64_t result = static_cast<int64_t>(a / b);
    if (remainder * 2 >= b) {
        result++;
    }
    return result;
}

/** Mod function overloading
 * @param a mpz_class
 * @param b mpz_class
 * @return @p a % @p b
 */
mpz_class mod(const mpz_class& a, const mpz_class& b) {
    return powm(a, 1, b);
}

/** Mod function overloading
 * @param a boost mpz_int
 * @param b boost mpz_int
 * @return @p a % @p b
 */
MPInt mod(MPInt a, MPInt b) {
    return boost::multiprecision::powm(a, 1, b);
}

/** Mod function overloading
 * @param a 64 bits integer
 * @param b 64 bits integer
 * @return @p a % @p b
 */
RegularInteger mod(RegularInteger a, RegularInteger b) {
// return static_cast<RegularInteger>(boost::multiprecision::powm(MPInt(a), 1, MPInt(b)));
   return static_cast<RegularInteger>((static_cast<WideInteger>(a % b) + static_cast<WideInteger>(b)) % static_cast<WideInteger>(b));
}

RegularInteger
mod(WideInteger a, WideInteger b) {
// return static_cast<RegularInteger>(boost::multiprecision::powm(MPInt(a), 1, MPInt(b)));
   return (((a % b) + b) % b);
}

/** Mod function overloading
 * @param a 64 bits integer
 * @param b 64 bits integer
 * @return @p a % @p b
 */
Int64 mod(Int64 a, Int64 b) {
    return boost::multiprecision::powm(a, 1, b);
}

/** Mod function overloading
 * @param a 64 bits integer
 * @param b 64 bits integer
 * @return @p a % @p b
 */
int64_t mod(int64_t a, int64_t b) {
    return ((a % b) + b) % b;
}

template <typename NumberType>
struct ChiDist {

    // This constant defines distributions standard deviation

    std::mt19937 g;
    double chiStd;

    ChiDist(int seed, double _chiStd) : g(seed), chiStd(_chiStd){ }
    ChiDist(int seed) : g(seed), chiStd(ligero::kDefaultChiStd){ }

    ChiDist() : g(std::random_device()()), chiStd(ligero::kDefaultChiStd){}

    NumberType operator()() {
        exrandom::discrete_normal_distribution N(0, 1, lround(chiStd * 10), 10);
        return boost::multiprecision::abs(NumberType(N(g)));
    }
};

/** Sample a vector of random field element
 * @param m Size of total samples
 * @param q Boundary
 * @param chiStd Standard deviation
 * @return A vector of random sample
 */
template <typename FieldT>
std::vector<FieldT> sampleRandomVectorOverField(size_t m, FieldT q, double chiStd) {
    //std::function<typename FieldT::underlyingType>
    auto dist = ligero::math::ChiDist<typename FieldT::underlyingType>(std::random_device()(), chiStd);
    std::vector<FieldT> e(m);

    for (int i = 0; i < static_cast<int>(m); i++) {
        typename FieldT::underlyingType tmp;
        do {
            tmp = static_cast<typename FieldT::underlyingType>(dist());
        } while (tmp>q.getValue());

        e[i] = FieldT(tmp);
    }

    return e;
}

/** A quick wrapper around GMP's modular inverse implementation for different types.
 *
 *  Note that the implementation here promotes arbitrary number types to our MPInt
 *  type so that we can call into `mpz_invert` directly with mpz types. This allows
 *  us to leverage GMP's existing implementation, but obviously has overhead. May
 *  be an optimization opportunity later if we want to consider writing our own modular
 *  inverse implementation.
 */
mpz_class
mod_inverse (const mpz_class& a, const mpz_class& b) {
    mpz_t z;
    mpz_init(z);

    int r = mpz_invert(z, a.get_mpz_t(), b.get_mpz_t());

    if (r == 0)
        throw std::runtime_error("Modular inverse does not exist for the given operands.");

    auto result = mpz_class(z);
    mpz_clear(z);

    return result;
}

/** Deconstruct input `v` by Chinese Remainder Theorem given a vector of moduli
 *  in the input `alphas`.
 */
std::vector<mpz_class> crt_deconstruct (mpz_class v, const std::vector<mpz_class>& alphas)
{
    std::vector<mpz_class> ds (alphas.size());

    for (int i = 0; i < alphas.size(); ++i) {
        ds[i] = mod(v, alphas[i]);
    }

    return ds;
}

/** Reconstruct a value `v` by Chinese Remainder Theorem given a vector of its
 *  components and a vector of moduli.
 */
mpz_class crt_reconstruct (const std::vector<mpz_class>& ds, std::vector<mpz_class>& coeffs, const std::vector<mpz_class>& alphas)
{
    if (ds.size() != alphas.size())
        throw std::runtime_error("Reconstruction vector lengths don't match.");

    coeffs.resize(ds.size());

    //mpz_class p = alphas.prod();
    mpz_class p = 1;
    for (size_t i = 0; i < alphas.size(); ++i) {
        p *= alphas[i];
    }

    for (size_t i = 0; i < ds.size(); ++i) {
        mpz_class pa = p / alphas[i];
        mpz_class x = mod(pa, alphas[i]);
        coeffs[i] = pa * mod_inverse(x, alphas[i]);
    }

    mpz_class dotProduct = 0;
    for (size_t i = 0; i < ds.size(); ++i) {
        dotProduct += ds[i] * coeffs[i];
    }

    return dotProduct % p;
}

/** A quick helper function for calculating the product of a vector of numbers. */
mpz_class vectorProduct(const std::vector<mpz_class>& xs) {
    mpz_class product = 1;

    for (auto& x : xs)
        product = product * x;

    return product;
}
/* fixed version */
std::pair<std::vector<mpz_class>, std::vector<size_t>>
fixed_bucket_n_primes (int productBitThreshold, size_t degree, int tauLimitBit)
{
    int number_of_buckets = std::ceil(double(productBitThreshold)/double(tauLimitBit)); // This rounds down.
    std::vector<mpz_class> alphas(number_of_buckets);
    std::vector<size_t> bucketSize(number_of_buckets);

    gmp_randclass grc(gmp_randinit_default);
    grc.seed(0);

    for (int i = 0; i < number_of_buckets; ++i) {
        mpz_class r = grc.get_z_bits(tauLimitBit);
        while (!boost::multiprecision::miller_rabin_test(MPInt(r.get_mpz_t()), 64) || mpz_sizeinbase(r.get_mpz_t(),2) < tauLimitBit) {
            r = grc.get_z_bits(tauLimitBit);
        }
        alphas[i] = r;
        // DBG("alphas[" << i << "] = " << r);
    }

    for (size_t i = 0; i < number_of_buckets; ++i) {
        bucketSize[i] = lrint(floor(double(degree) / double(number_of_buckets)));
        DBG("bucketSize[ " << i << "] = " << bucketSize[i]);
    }

    return std::pair{alphas, bucketSize};
}

/** Implements a prime bucketing algorithm, returning a vector of numbers such that
 *  each number is a product of subsequent primes that is at least 18bits, and such
 *  that the product of numbers in the vector is at least `productBitThreshold` bits.
 */
std::pair<std::vector<mpz_class>, std::vector<size_t>>
balanced_bucket_n_primes (int productBitThreshold, size_t degree, int tauLimitBit, int startJ = 1)
{
    int number_of_buckets = std::ceil(double(productBitThreshold)/double(tauLimitBit)); // This rounds down.
    std::vector<mpz_class> alphas(number_of_buckets);
    std::vector<mpz_class> primes;
    mpz_class alpha = 1;
    int j = startJ;
    while(1)
    {
        alpha*= boost::math::prime(j);
        primes.push_back(boost::math::prime(j));
        if(mpz_sizeinbase(alpha.get_mpz_t(), 2) >= static_cast<unsigned int>(productBitThreshold)) break;
        j++;
    }
    DBG("Maximum size of prime = " << mpz_sizeinbase((primes[primes.size()-1]).get_mpz_t(),2));

    std::vector<double> weights(number_of_buckets);
    for ( int i = 0; i < number_of_buckets; i++)
    {
      alphas[i] = 1;
      weights[i] = 1;
    }
    double sum = 0.0;
    for ( int i = primes.size()-1;i >= 0;  i--)
    {
      double maxweight = weights[0];
      int maxpos = 0;
      for (int j = 1; j < number_of_buckets; j++)
      {
	      mpz_class temp = alphas[j];
        if(weights[j] > maxweight && mpz_sizeinbase(temp.get_mpz_t(),2) <= tauLimitBit)
        {
          maxweight = weights[j];
          maxpos = j;
        }
      }
      alphas[maxpos] *= primes[i];
      weights[maxpos] *= (double(1.0) - (double(1)/primes[i].get_d()));
    }
    for ( int i = 0; i < number_of_buckets; i++)
    {
      weights[i] = double(1)/weights[i];
      sum += weights[i];
    }

    std::vector<size_t> bucketSize(weights.size());
    size_t total = 0;
    for (size_t i = 0; i < weights.size() - 1; ++i) {
        bucketSize[i] = lround(double(weights[i] * double(degree)) / double(sum));
        total += bucketSize[i];
        DBG("bucketSize[ " << i << "] = " << bucketSize[i]);
    }
    bucketSize[bucketSize.size() - 1] = degree - total;
    DBG("bucketSize[ " << bucketSize.size() - 1 << "] = " << bucketSize[bucketSize.size() - 1]);

    return std::pair{alphas, bucketSize};
    }

}//namespace math
}//namespace ligero