//
// Copyright (c) 2016-2020 CNRS INRIA
//

#ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__
#define __pinocchio_multibody_liegroup_vector_space_operation_hpp__

#include "pinocchio/multibody/liegroup/liegroup-base.hpp"

#include <stdexcept>
#include <boost/integer/static_min_max.hpp>

namespace pinocchio
{
  template<int Dim, typename Scalar, int Options = 0> struct VectorSpaceOperationTpl;

  template<int Dim, typename _Scalar, int _Options>
  struct traits< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum {
      Options = _Options,
      NQ = Dim,
      NV = Dim
    };
  };

  template<int Dim, typename _Scalar, int _Options>
  struct VectorSpaceOperationTpl
  : public LieGroupBase< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
  {
    PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(VectorSpaceOperationTpl);

    /// Constructor
    /// \param size size of the vector space: should be the equal to template
    ///        argument for static sized vector-spaces.
    VectorSpaceOperationTpl(int size = boost::static_signed_max<0,Dim>::value)
    : size_(size)
    {
      assert(size_.value() >= 0);
    }

    /// Constructor
    /// \param other other VectorSpaceOperationTpl from which to retrieve size
    VectorSpaceOperationTpl(const VectorSpaceOperationTpl & other)
    : Base(), size_(other.size_.value())
    {
      assert(size_.value() >= 0);
    }

    Index nq () const
    {
      return size_.value();
    }
    Index nv () const
    {
      return size_.value();
    }

    ConfigVector_t neutral () const
    {
      return ConfigVector_t::Zero(size_.value());
    }

    std::string name () const
    {
      std::ostringstream oss; oss << "R^" << nq();
      return oss.str ();
    }

    template <class ConfigL_t, class ConfigR_t, class Tangent_t>
    static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                const Eigen::MatrixBase<ConfigR_t> & q1,
                                const Eigen::MatrixBase<Tangent_t> & d)
    {
      PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d) = q1 - q0;
    }

    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
    void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> &,
                           const Eigen::MatrixBase<ConfigR_t> &,
                           const Eigen::MatrixBase<JacobianOut_t> & J) const
    {
      if (arg == ARG0)
        PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J) = -JacobianMatrix_t::Identity();
      else if (arg == ARG1)
        PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).setIdentity();
    }

    template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
    static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
                               const Eigen::MatrixBase<Velocity_t> & v,
                               const Eigen::MatrixBase<ConfigOut_t> & qout)
    {
      PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q + v;
    }

    template <class Config_t, class Jacobian_t>
    static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> &,
                                                const Eigen::MatrixBase<Jacobian_t> & J)
    {
      PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).setIdentity();
    }

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                   const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                   const Eigen::MatrixBase<JacobianOut_t> & J,
                                   const AssignmentOperatorType op)
    {
      Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
      switch(op)
        {
        case SETTO:
          Jout.setIdentity();
          break;
        case ADDTO:
          Jout.diagonal().array() += Scalar(1);
          break;
        case RMTO:
          Jout.diagonal().array() -= Scalar(1);
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
        }
    }

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                   const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                   const Eigen::MatrixBase<JacobianOut_t> & J,
                                   const AssignmentOperatorType op)
    {
      Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
      switch(op)
        {
        case SETTO:
          Jout.setIdentity();
          break;
        case ADDTO:
          Jout.diagonal().array() += Scalar(1);
          break;
        case RMTO:
          Jout.diagonal().array() -= Scalar(1);
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
        }
    }

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
                                     const Eigen::MatrixBase<JacobianOut_t> & Jout) const
    {
      PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
    }

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
                                     const Eigen::MatrixBase<JacobianOut_t> & Jout) const
    {
      PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
    }

    template <class Config_t, class Tangent_t, class Jacobian_t>
    void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                     const Eigen::MatrixBase<Jacobian_t> & /*J*/) const {}

    template <class Config_t, class Tangent_t, class Jacobian_t>
    void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                     const Eigen::MatrixBase<Jacobian_t> & /*J*/) const {}

    // template <class ConfigL_t, class ConfigR_t>
    // static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                       // const Eigen::MatrixBase<ConfigR_t> & q1)

    template <class Config_t>
    static void normalize_impl (const Eigen::MatrixBase<Config_t>& /*qout*/)
    {}

    template <class Config_t>
    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
    {
      PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).setRandom();
    }

    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
    void randomConfiguration_impl
    (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
     const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
     const Eigen::MatrixBase<ConfigOut_t> & qout) const
    {
      ConfigOut_t & res = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).derived();
      for (int i = 0; i < nq (); ++i)
      {
        if(lower_pos_limit[i] == -std::numeric_limits<typename ConfigL_t::Scalar>::infinity() ||
           upper_pos_limit[i] ==  std::numeric_limits<typename ConfigR_t::Scalar>::infinity() )
        {
          std::ostringstream error;
          error << "non bounded limit. Cannot uniformly sample joint at rank " << i;
          // assert(false && "non bounded limit. Cannot uniformly sample joint revolute");
          throw std::range_error(error.str());
        }
        res[i] = lower_pos_limit[i] + (( upper_pos_limit[i] - lower_pos_limit[i]) * rand())/RAND_MAX;
      }
    }

  private:

    Eigen::internal::variable_if_dynamic<Index, Dim> size_;
  }; // struct VectorSpaceOperationTpl

} // namespace pinocchio

#endif // ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__


//#include <bits/stdc++.h>
#include <armadillo>

using namespace std;

class Naive_bayes{
public:
    int number_of_variables;
	vector<int> categorical_cols;
	vector<int> cont_cols;
    Naive_bayes(int num,vector<int> cat_cols);

    void train(arma::mat X_train, arma::mat y_train);

    arma::mat predict(arma::mat X_predict);
};


// Copyright (c) 2020 fortiss GmbH
//
// Authors: Julian Bernhard, Klemens Esterle, Patrick Hart and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <Eigen/Core>
#include "gtest/gtest.h"

#include "bark/commons/params/setter_params.hpp"
#include "bark/geometry/commons.hpp"
#include "bark/geometry/line.hpp"
#include "bark/geometry/polygon.hpp"
#include "bark/models/behavior/constant_acceleration/constant_acceleration.hpp"
#include "bark/models/behavior/idm/idm_classic.hpp"
#include "bark/models/execution/interpolation/interpolate.hpp"
#include "bark/world/observed_world.hpp"
#include "bark/world/tests/make_test_world.hpp"

using namespace bark::models::dynamic;
using namespace bark::models::execution;
using namespace bark::commons;
using namespace bark::models::behavior;
using namespace bark::world::map;
using namespace bark::models::dynamic;
using namespace bark::world;
using namespace bark::geometry;
using namespace bark::world::tests;

using bark::geometry::standard_shapes::GenerateGoalRectangle;

class DummyBehaviorIDM : public BehaviorIDMClassic {
 public:
  DummyBehaviorIDM(const ParamsPtr& params)
      : BehaviorIDMClassic(params), BehaviorModel(params) {}
  virtual double CalculateLongitudinalAcceleration(
      const ObservedWorld& observed_world) {
    std::pair<AgentPtr, FrenetPosition> leading_vehicle =
        observed_world.GetAgentInFront();
    std::shared_ptr<const Agent> ego_agent = observed_world.GetEgoAgent();

    float vel_i = ego_agent->GetCurrentState()(StateDefinition::VEL_POSITION);

    double acc;
    if (leading_vehicle.first) {
      double net_distance = CalcNetDistance(ego_agent, leading_vehicle.first);
      State other_vehicle_state = leading_vehicle.first->GetCurrentState();
      double vel_other = other_vehicle_state(StateDefinition::VEL_POSITION);
      acc = CalcIDMAcc(net_distance, vel_i, vel_other);
    } else {
      acc = GetMaxAcceleration() * CalcFreeRoadTerm(vel_i);
    }
    return acc;
  }

  virtual std::shared_ptr<BehaviorModel> Clone() const {
    std::shared_ptr<DummyBehaviorIDM> model_ptr =
        std::make_shared<DummyBehaviorIDM>(*this);
    return std::dynamic_pointer_cast<BehaviorModel>(model_ptr);
  }
};

TEST(free_road_term, behavior_idm_classic) {
  auto params = std::make_shared<SetterParams>();
  DummyBehaviorIDM behavior(params);
  const float desired_velocity = behavior.GetDesiredVelocity();
  const float max_acceleration = behavior.GetMaxAcceleration();
  const int exponent = behavior.GetExponent();

  // Create an observed world with specific goal definition and the
  // corresponding mcts state
  Polygon polygon = GenerateGoalRectangle(6,3);
  std::shared_ptr<Polygon> goal_polygon(
      std::dynamic_pointer_cast<Polygon>(polygon.Translate(
          Point2d(50, -2))));  // < move the goal polygon into the driving
                               // corridor in front of the ego vehicle
  auto goal_definition_ptr =
      std::make_shared<GoalDefinitionPolygon>(*goal_polygon);

  float ego_velocity = desired_velocity, rel_distance = 7.0,
        velocity_difference = 0.0;
  auto observed_world = make_test_observed_world(
      0, rel_distance, ego_velocity, velocity_difference, goal_definition_ptr);
  double idm_acceleration =
      behavior.CalculateLongitudinalAcceleration(observed_world);

  // no vehicle is in front only free road term should give acceleration
  double desired_acceleration =
      max_acceleration * (1 - pow(ego_velocity / desired_velocity, exponent));
  EXPECT_NEAR(idm_acceleration, desired_acceleration, 0.0001);

  ego_velocity = desired_velocity + 10;
  observed_world = make_test_observed_world(0, rel_distance, ego_velocity,
                                            velocity_difference);
  idm_acceleration = behavior.CalculateLongitudinalAcceleration(observed_world);

  // no vehicle is in front only free road term should give acceleration
  desired_acceleration =
      max_acceleration * (1 - pow(ego_velocity / desired_velocity, exponent));
  EXPECT_NEAR(idm_acceleration, desired_acceleration, 0.0001);

  ego_velocity = desired_velocity - 12;
  observed_world = make_test_observed_world(0, rel_distance, ego_velocity,
                                            velocity_difference);
  idm_acceleration = behavior.CalculateLongitudinalAcceleration(observed_world);

  // no vehicle is in front only free road term should give acceleration
  desired_acceleration =
      max_acceleration * (1 - pow(ego_velocity / desired_velocity, exponent));
  EXPECT_NEAR(idm_acceleration, desired_acceleration, 0.0001);
}

TEST(interaction_term, behavior_idm_classic) {
  auto params = std::make_shared<SetterParams>();
  DummyBehaviorIDM behavior(params);
  const double desired_velocity = behavior.GetDesiredVelocity();
  const double minimum_spacing = behavior.GetMinimumSpacing();
  const double desired_time_headway = behavior.GetDesiredTimeHeadway();
  const double max_acceleration = behavior.GetMaxAcceleration();
  const double acc_lower_bound = behavior.GetAccelerationLowerBound();
  const double acc_upper_bound = behavior.GetAccelerationUpperBound();
  const double comfortable_braking_acceleration =
      behavior.GetComfortableBrakingAcceleration();

  // Create an observed world with specific goal definition and the
  // corresponding mcts state
  Polygon polygon = GenerateGoalRectangle(6,3);
  std::shared_ptr<Polygon> goal_polygon(
      std::dynamic_pointer_cast<Polygon>(polygon.Translate(
          Point2d(50, -2))));  // < move the goal polygon into the driving
                               // corridor in front of the ego vehicle
  auto goal_definition_ptr =
      std::make_shared<GoalDefinitionPolygon>(*goal_polygon);

  // vehicle is in front, zero velocity equal desired velocity thus only
  // interaction term
  double ego_velocity = desired_velocity, rel_distance = 7.0,
         velocity_difference = 0.0;
  auto observed_world = make_test_observed_world(
      1, rel_distance, ego_velocity, velocity_difference, goal_definition_ptr);
  double idm_acceleration =
      behavior.CalculateLongitudinalAcceleration(observed_world);
  double helper_state =
      minimum_spacing + ego_velocity * desired_time_headway +
      ego_velocity * velocity_difference /
          (2 * sqrt(max_acceleration * comfortable_braking_acceleration));
  double desired_acceleration = std::max(
      acc_lower_bound, -max_acceleration * pow(helper_state / rel_distance, 2));
  EXPECT_NEAR(idm_acceleration, desired_acceleration, 0.001);

  // velocity difference to other vehicle
  ego_velocity = desired_velocity, rel_distance = 7.0,
  velocity_difference = 5.0;
  observed_world = make_test_observed_world(
      1, rel_distance, ego_velocity, velocity_difference, goal_definition_ptr);
  idm_acceleration = behavior.CalculateLongitudinalAcceleration(observed_world);
  helper_state =
      minimum_spacing + ego_velocity * desired_time_headway +
      ego_velocity * velocity_difference /
          (2 * sqrt(max_acceleration * comfortable_braking_acceleration));
  desired_acceleration = std::max(
      acc_lower_bound, -max_acceleration * pow(helper_state / rel_distance, 2));
  EXPECT_NEAR(idm_acceleration, desired_acceleration, 0.001);
}

TEST(drive_free, behavior_idm_classic) {
  auto params = std::make_shared<SetterParams>();
  DummyBehaviorIDM behavior(params);
  const float desired_velocity = behavior.GetDesiredVelocity();

  // First case, we start with the desired velocity. After num steps, we should
  // advance
  float ego_velocity = desired_velocity, rel_distance = 7.0,
        velocity_difference = 0.0;
  float time_step = 0.2f;
  int num_steps = 10;
  WorldPtr world =
      make_test_world(0, rel_distance, ego_velocity, velocity_difference);

  float x_start = world->GetAgents()
                      .begin()
                      ->second->GetCurrentState()[StateDefinition::X_POSITION];
  for (int i = 0; i < num_steps; ++i) {
    world->Step(time_step);
  }
  float x_end = world->GetAgents()
                    .begin()
                    ->second->GetCurrentState()[StateDefinition::X_POSITION];
  float x_diff_desired = x_start + ego_velocity * num_steps * time_step - x_end;
  EXPECT_NEAR(x_diff_desired, 0, 0.01);
}

TEST(drive_leading_vehicle, behavior_idm_classic) {
  auto params = std::make_shared<SetterParams>();
  // IDM Classic
  params->SetReal("BehaviorIDMClassic::MinimumSpacing",
                  1.0f);  // Required for testing
  params->SetReal("BehaviorIDMClassic::DesiredTimeHeadway", 3.5);
  params->SetReal("BehaviorIDMClassic::MaxAcceleration",
                  1.0f);  // Required for testing
  params->SetReal("BehaviorIDMClassic::AccelerationLowerBound", -1000.0);
  params->SetReal("BehaviorIDMClassic::AccelerationUpperBound", 1000.0);
  params->SetReal("BehaviorIDMClassic::DesiredVelocity", 8.0f);
  params->SetReal("BehaviorIDMClassic::ComfortableBrakingAcceleration", 1.0f);
  params->SetReal("BehaviorIDMClassic::MinVelocity", 0.0f);
  params->SetReal("BehaviorIDMClassic::MaxVelocity", 50.0f);
  BehaviorIDMClassic behavior(params);
  const float desired_velocity = behavior.GetDesiredVelocity();
  const float minimum_spacing = behavior.GetMinimumSpacing();
  const float desired_time_headway = behavior.GetDesiredTimeHeadway();

  // First case, we start with the desired velocity. After num steps, we should
  // advance
  float ego_velocity = desired_velocity, rel_distance = 5.0,
        velocity_difference = 4;
  float time_step = 0.02f;  // Very small time steps to verify differential
                            // integration character
  int num_steps = 1000;

  // Create an observed world with specific goal definition and the
  // corresponding mcts state
  Polygon polygon = GenerateGoalRectangle(6,3);
  std::shared_ptr<Polygon> goal_polygon(
      std::dynamic_pointer_cast<Polygon>(polygon.Translate(
          Point2d(50, -2))));  // < move the goal polygon into the driving
                               // corridor in front of the ego vehicle
  auto goal_definition_ptr =
      std::make_shared<GoalDefinitionPolygon>(*goal_polygon);

  WorldPtr world = make_test_world(1, rel_distance, ego_velocity,
                                   velocity_difference, goal_definition_ptr);

  float v_start =
      world->GetAgents()
          .begin()
          ->second->GetCurrentState()[StateDefinition::VEL_POSITION];
  for (int i = 0; i < num_steps; ++i) {
    world->Step(time_step);
  }
  float v_end = world->GetAgents()
                    .begin()
                    ->second->GetCurrentState()[StateDefinition::VEL_POSITION];

  EXPECT_NEAR(v_end, ego_velocity - velocity_difference, 0.01);
}

TEST(coolness_factor_upper_eq_case, behavior_idm_classic) {
  auto params = std::make_shared<SetterParams>();
  // IDM Classic
  params->SetReal("BehaviorIDMClassic::MinimumSpacing",
                  1.0f);  // Required for testing
  params->SetReal("BehaviorIDMClassic::DesiredTimeHeadway", 3.5);
  params->SetReal("BehaviorIDMClassic::MaxAcceleration",
                  1.0f);  // Required for testing
  params->SetReal("BehaviorIDMClassic::AccelerationLowerBound", -1000.0);
  params->SetReal("BehaviorIDMClassic::AccelerationUpperBound", 1000.0);
  params->SetReal("BehaviorIDMClassic::DesiredVelocity", 8.0f);
  params->SetReal("BehaviorIDMClassic::ComfortableBrakingAcceleration", 1.0f);
  params->SetReal("BehaviorIDMClassic::MinVelocity", 0.0f);
  params->SetReal("BehaviorIDMClassic::MaxVelocity", 50.0f);
  params->SetBool("BehaviorIDMClassic::BrakeForLaneEnd", false);
  params->SetReal("BehaviorIDMClassic::BrakeForLaneEndEnabledDistance", 2.0f);
  params->SetReal("BehaviorIDMClassic::BrakeForLaneEndDistanceOffset", 2.0f);
  params->SetInt("BehaviorIDMClassic::NumTrajectoryTimePoints", 11);
  params->SetReal("BehaviorIDMClassic::CoolnessFactor", 0.89f);
  BehaviorIDMClassic behavior(params);
  const float desired_velocity = behavior.GetDesiredVelocity();
  const float minimum_spacing = behavior.GetMinimumSpacing();
  const float desired_time_headway = behavior.GetDesiredTimeHeadway();

  // First case, we start with the desired velocity. After num steps, we should
  // advance
  float ego_velocity = desired_velocity, rel_distance = 10.0,
        velocity_difference = -2, acc_ego = 2.0f, acc_other = -5.0f;
  float other_velocity = ego_velocity - velocity_difference;
  float time_step = 0.2f;  // Very small time steps to verify differential
                           // integration character
  int num_steps = 1000;

  // Create an observed world with specific goal definition and the
  // corresponding mcts state
  Polygon polygon = GenerateGoalRectangle(6,3);
  std::shared_ptr<Polygon> goal_polygon(
      std::dynamic_pointer_cast<Polygon>(polygon.Translate(
          Point2d(50, -2))));  // < move the goal polygon into the driving
                               // corridor in front of the ego vehicle
  auto goal_definition_ptr =
      std::make_shared<GoalDefinitionPolygon>(*goal_polygon);

  auto observed_world = make_test_observed_world(
      1, rel_distance, ego_velocity, velocity_difference, goal_definition_ptr,
      acc_ego, acc_other);

  auto traj = behavior.Plan(time_step, observed_world);
  auto action = behavior.GetLastAction();

  // upper case of equation 11.25
  const float b = behavior.GetComfortableBrakingAcceleration();
  const float acc_cah_desired =
      ego_velocity * ego_velocity * acc_other /
      (other_velocity * other_velocity - 2 * rel_distance * acc_other);
  const float acc_idm_desired =
      behavior.CalcRawIDMAcc(rel_distance, ego_velocity, other_velocity);
  const float c = 0.89;
  const float acc_acc_desired =
      (1 - c) * acc_idm_desired +
      c * (acc_cah_desired + b * tanh((acc_idm_desired - acc_cah_desired) / b));
  EXPECT_NEAR(boost::get<Continuous1DAction>(action), acc_acc_desired, 0.001);
}

TEST(coolness_factor_lower_eq_case_vel_diff_neg, behavior_idm_classic) {
  auto params = std::make_shared<SetterParams>();
  // IDM Classic
  params->SetReal("BehaviorIDMClassic::MinimumSpacing",
                  1.0f);  // Required for testing
  params->SetReal("BehaviorIDMClassic::DesiredTimeHeadway", 3.5);
  params->SetReal("BehaviorIDMClassic::MaxAcceleration",
                  1.0f);  // Required for testing
  params->SetReal("BehaviorIDMClassic::AccelerationLowerBound", -1000.0);
  params->SetReal("BehaviorIDMClassic::AccelerationUpperBound", 1000.0);
  params->SetReal("BehaviorIDMClassic::DesiredVelocity", 8.0f);
  params->SetReal("BehaviorIDMClassic::ComfortableBrakingAcceleration", 1.0f);
  params->SetReal("BehaviorIDMClassic::MinVelocity", 0.0f);
  params->SetReal("BehaviorIDMClassic::MaxVelocity", 50.0f);
  params->SetBool("BehaviorIDMClassic::BrakeForLaneEnd", false);
  params->SetReal("BehaviorIDMClassic::BrakeForLaneEndEnabledDistance", 2.0f);
  params->SetReal("BehaviorIDMClassic::BrakeForLaneEndDistanceOffset", 2.0f);
  params->SetInt("BehaviorIDMClassic::NumTrajectoryTimePoints", 11);
  params->SetReal("BehaviorIDMClassic::CoolnessFactor", 0.6f);
  BehaviorIDMClassic behavior(params);
  const float desired_velocity = behavior.GetDesiredVelocity();
  const float minimum_spacing = behavior.GetMinimumSpacing();
  const float desired_time_headway = behavior.GetDesiredTimeHeadway();

  // First case, we start with the desired velocity. After num steps, we should
  // advance
  float ego_velocity = desired_velocity, rel_distance = 20.0,
        velocity_difference = -1, acc_ego = 2.0f, acc_other = 1.0f;
  float other_velocity = ego_velocity - velocity_difference;
  float time_step = 0.2f;  // Very small time steps to verify differential
                           // integration character

  Polygon polygon = GenerateGoalRectangle(6,3);
  std::shared_ptr<Polygon> goal_polygon(
      std::dynamic_pointer_cast<Polygon>(polygon.Translate(
          Point2d(50, -2))));  // < move the goal polygon into the driving
                               // corridor in front of the ego vehicle
  auto goal_definition_ptr =
      std::make_shared<GoalDefinitionPolygon>(*goal_polygon);

  auto observed_world = make_test_observed_world(
      1, rel_distance, ego_velocity, velocity_difference, goal_definition_ptr,
      acc_ego, acc_other);

  auto traj = behavior.Plan(time_step, observed_world);
  auto action = behavior.GetLastAction();

  // upper case of equation 11.25
  const float b = behavior.GetComfortableBrakingAcceleration();
  const float acc_cah_desired = acc_other;
  const float acc_idm_desired =
      behavior.CalcRawIDMAcc(rel_distance, ego_velocity, other_velocity);
  const float c = 0.6;
  const float acc_acc_desired =
      (1 - c) * acc_idm_desired +
      c * (acc_cah_desired + b * tanh((acc_idm_desired - acc_cah_desired) / b));
  EXPECT_NEAR(boost::get<Continuous1DAction>(action), acc_acc_desired, 0.001);
}

TEST(coolness_factor_lower_eq_case_vel_diff_pos, behavior_idm_classic) {
  auto params = std::make_shared<SetterParams>();
  // IDM Classic
  params->SetReal("BehaviorIDMClassic::MinimumSpacing",
                  1.0f);  // Required for testing
  params->SetReal("BehaviorIDMClassic::DesiredTimeHeadway", 3.5);
  params->SetReal("BehaviorIDMClassic::MaxAcceleration",
                  1.0f);  // Required for testing
  params->SetReal("BehaviorIDMClassic::AccelerationLowerBound", -1000.0);
  params->SetReal("BehaviorIDMClassic::AccelerationUpperBound", 1000.0);
  params->SetReal("BehaviorIDMClassic::DesiredVelocity", 8.0f);
  params->SetReal("BehaviorIDMClassic::ComfortableBrakingAcceleration", 1.0f);
  params->SetReal("BehaviorIDMClassic::MinVelocity", 0.0f);
  params->SetReal("BehaviorIDMClassic::MaxVelocity", 50.0f);
  params->SetReal("BehaviorIDMClassic::CoolnessFactor", 0.6f);
  BehaviorIDMClassic behavior(params);
  const float desired_velocity = behavior.GetDesiredVelocity();
  const float minimum_spacing = behavior.GetMinimumSpacing();
  const float desired_time_headway = behavior.GetDesiredTimeHeadway();

  // First case, we start with the desired velocity. After num steps, we should
  // advance
  float ego_velocity = desired_velocity, rel_distance = 5.0,
        velocity_difference = 5, acc_ego = 2.0f, acc_other = 1.0f;
  float other_velocity = ego_velocity - velocity_difference;
  float time_step = 0.2f;  // Very small time steps to verify differential
                           // integration character
  int num_steps = 1000;

  // Create an observed world with specific goal definition and the
  // corresponding mcts state
  Polygon polygon = GenerateGoalRectangle(6,3);
  std::shared_ptr<Polygon> goal_polygon(
      std::dynamic_pointer_cast<Polygon>(polygon.Translate(
          Point2d(50, -2))));  // < move the goal polygon into the driving
                               // corridor in front of the ego vehicle
  auto goal_definition_ptr =
      std::make_shared<GoalDefinitionPolygon>(*goal_polygon);

  auto observed_world = make_test_observed_world(
      1, rel_distance, ego_velocity, velocity_difference, goal_definition_ptr,
      acc_ego, acc_other);

  auto traj = behavior.Plan(time_step, observed_world);
  auto action = behavior.GetLastAction();

  // upper case of equation 11.25
  const float b = behavior.GetComfortableBrakingAcceleration();
  const float acc_cah_desired = acc_other - velocity_difference *
                                                velocity_difference * 1.0 /
                                                (2.0 * rel_distance);
  const float acc_idm_desired =
      behavior.CalcRawIDMAcc(rel_distance, ego_velocity, other_velocity);
  const float c = 0.6;
  const float acc_acc_desired =
      (1 - c) * acc_idm_desired +
      c * (acc_cah_desired + b * tanh((acc_idm_desired - acc_cah_desired) / b));
  EXPECT_NEAR(boost::get<Continuous1DAction>(action), acc_acc_desired, 0.001);
}

int main(int argc, char** argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#ifndef LOAD_EXTRINSIC_HPP_
#define LOAD_EXTRINSIC_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <yaml-cpp/yaml.h>

inline bool loadExtrinsic(const std::string &file_path, Eigen::Isometry3d &extrinsic) {
  YAML::Node config = YAML::LoadFile(file_path);
  if (config["transform"]) {
    if (config["transform"]["matrix"]) {
      const YAML::Node &matrix = config["transform"]["matrix"];
      Eigen::Matrix4d m4d;
      if (matrix.size() != 16) {
        std::cout << "Transform::Matrix error" << matrix.size();
      }

      m4d << matrix[0].as<double>(), matrix[1].as<double>(),
          matrix[2].as<double>(), matrix[3].as<double>(),
          matrix[4].as<double>(), matrix[5].as<double>(),
          matrix[6].as<double>(), matrix[7].as<double>(),
          matrix[8].as<double>(), matrix[9].as<double>(),
          matrix[10].as<double>(), matrix[11].as<double>(),
          matrix[12].as<double>(), matrix[13].as<double>(),
          matrix[14].as<double>(), matrix[15].as<double>();

      extrinsic.matrix() = m4d;
      return true;
    }
  }
  return false;
}
#endif // LOAD_EXTRINSIC_HPP_


/*********************************************************************
* Rice University Software Distribution License
*
* Copyright (c) 2010, Rice University
* All Rights Reserved.
*
* For a full description see the file named LICENSE.
*
*********************************************************************/

/* Author: Mark Moll */

#include "omplapp/apps/KinematicCarPlanning.h"
#include <boost/math/constants/constants.hpp>

ompl::app::KinematicCarPlanning::KinematicCarPlanning()
    : AppBase<CONTROL>(constructControlSpace(), Motion_2D), timeStep_(1e-2), lengthInv_(1.), odeSolver(new control::ODEBasicSolver<>(si_, boost::bind(&KinematicCarPlanning::ode, this, _1, _2, _3)))
{
    name_ = std::string("Kinematic car");
    setDefaultControlBounds();

    si_->setStatePropagator(control::ODESolver::getStatePropagator(odeSolver, boost::bind(&KinematicCarPlanning::postPropagate, this, _1, _2, _3, _4)));
}

ompl::app::KinematicCarPlanning::KinematicCarPlanning(const control::ControlSpacePtr &controlSpace)
    : AppBase<CONTROL>(controlSpace, Motion_2D), timeStep_(1e-2), lengthInv_(1.), odeSolver(new control::ODEBasicSolver<>(si_, boost::bind(&KinematicCarPlanning::ode, this, _1, _2, _3)))
{
    setDefaultControlBounds();

    si_->setStatePropagator(control::ODESolver::getStatePropagator(odeSolver, boost::bind(&KinematicCarPlanning::postPropagate, this, _1, _2, _3, _4)));
}

ompl::base::ScopedState<> ompl::app::KinematicCarPlanning::getDefaultStartState(void) const
{
    base::ScopedState<base::SE2StateSpace> sSE2(getStateSpace());
    aiVector3D s = getRobotCenter(0);

    sSE2->setX(s.x);
    sSE2->setY(s.y);
    sSE2->setYaw(0.);
    return sSE2;
}

void ompl::app::KinematicCarPlanning::setDefaultControlBounds(void)
{
    base::RealVectorBounds cbounds(2);
    cbounds.low[0] = -5.;
    cbounds.high[0] = 5.;
    cbounds.low[1] = -boost::math::constants::pi<double>() * 30. / 180.;
    cbounds.high[1] = boost::math::constants::pi<double>() * 30. / 180.;
    getControlSpace()->as<control::RealVectorControlSpace>()->setBounds(cbounds);
}

void ompl::app::KinematicCarPlanning::ode(const control::ODESolver::StateType& q, const control::Control *ctrl, control::ODESolver::StateType& qdot)
{
    const double *u = ctrl->as<control::RealVectorControlSpace::ControlType>()->values;

    // zero out qdot
    qdot.resize (q.size (), 0);

    qdot[0] = u[0] * cos(q[2]);
    qdot[1] = u[0] * sin(q[2]);
    qdot[2] = u[0] * lengthInv_ * tan(u[1]);
}

void ompl::app::KinematicCarPlanning::postPropagate(const base::State* /*state*/, const control::Control* /*control*/, const double /*duration*/, base::State* result)
{
    // Normalize orientation value between 0 and 2*pi
    const base::SO2StateSpace* SO2 = getStateSpace()->as<base::SE2StateSpace>()
        ->as<base::SO2StateSpace>(1);
    base::SO2StateSpace::StateType* so2 = result->as<base::SE2StateSpace::StateType>()
        ->as<base::SO2StateSpace::StateType>(1);
    SO2->enforceBounds(so2);
}


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

#include <boost/random/detail/mulhilo.hpp>
#include <cassert>
#include <iostream>
#include <typeinfo>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

using boost::random::uniform_int_distribution;
using boost::random::mt11213b;
using boost::random::detail::mulhilo;
using boost::random::detail::mulhilo_halfword;

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

template <typename UINT>
void doit(){
    uniform_int_distribution<UINT> D;
    mt11213b mt;
    for(int i=0; i<1000000; ++i){
        UINT a = D(mt);
        UINT b = D(mt);

        UINT hi, lo;
        lo = mulhilo(a, b, hi);
        BOOST_CHECK_EQUAL(lo, (UINT)(a*b) );
        // Can't we say something about hi/a and b
        // and hi/b and a?

        UINT hi_hw, lo_hw;
        lo_hw = mulhilo_halfword(a, b, hi_hw);
        BOOST_CHECK_EQUAL(lo_hw, lo);
        BOOST_CHECK_EQUAL(hi_hw, hi);
        //std::cout << a << " * " << b << " = " << hi << "." << lo << "\n";
    }
}

BOOST_AUTO_TEST_CASE(test_mulhilo)
{
    doit<uint8_t>();
    doit<uint16_t>();
    doit<uint32_t>();
    doit<uint64_t>();
}


// Generate proposal bounding boxes from RGB-D data
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/io/pcd_io.h>

#include <pcl/features/integral_image_normal.h>

#include <pcl/segmentation/organized_connected_component_segmentation.h>
#include <pcl/segmentation/euclidean_cluster_comparator.h>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <opencv2/opencv.hpp>

#include <random>
#include <chrono>

#include "proposal_generator.h"

namespace fs = boost::filesystem;

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

namespace apc_densecap
{

class MyComparator : public pcl::EuclideanClusterComparator<pcl::PointXYZRGB, pcl::Normal, pcl::Label>
{
public:
	bool compare(int idx1, int idx2) const override
	{
		auto& point1 = input_->points[idx1];
		auto& point2 = input_->points[idx2];

		if(!pcl::isFinite(point1) || !pcl::isFinite(point2))
			return false;

		float dx = input_->points[idx1].x - input_->points[idx2].x;
		float dy = input_->points[idx1].y - input_->points[idx2].y;
		float dz = input_->points[idx1].z - input_->points[idx2].z;
		float dist = sqrtf (dx*dx + dy*dy + dz*dz);

		auto norm1 = normals_->points[idx1].getNormalVector3fMap();
		auto norm2 = normals_->points[idx2].getNormalVector3fMap();
		double normalAngle = norm1.dot(norm2) / norm1.norm() / norm2.norm();

		if(dist > distance_threshold_)
			return false;

		if(std::isfinite(normalAngle) && normalAngle < angular_threshold_)
			return false;

// 		float dist2shelf1 = m_dist2Shelf(idx1 / m_dist2Shelf.cols, idx1 % m_dist2Shelf.cols);
// 		float dist2shelf2 = m_dist2Shelf(idx2 / m_dist2Shelf.cols, idx2 % m_dist2Shelf.cols);
//
// 		if(dist2shelf1 < m_shelfThreshold || dist2shelf2 < m_shelfThreshold)
// 			return false;

		cv::Mat_<cv::Vec3f> rgbIn(1, 2);
		rgbIn(0, 0) = cv::Vec3f(point1.b / 255.0f, point1.g / 255.0f, point1.r / 255.0f);
		rgbIn(0, 1) = cv::Vec3f(point2.b / 255.0f, point2.g / 255.0f, point2.r / 255.0f);

		cv::Mat_<cv::Vec3f> hsvOut(1,2);

		cv::cvtColor(rgbIn, hsvOut, cv::COLOR_BGR2HSV);

		bool saturated1 = hsvOut(0,0)[1] > 0.3;
		bool saturated2 = hsvOut(0,1)[1] > 0.3;
		if(saturated1 != saturated2)
			return false;

		if(std::abs(hsvOut(0,0)[2] - hsvOut(0,1)[2]) > 0.5)
			return false;

		if(saturated1 && saturated2)
		{
			float angleDiff = hsvOut(0,0)[0] - hsvOut(0,1)[0];
			while(angleDiff > 180)
				angleDiff -= 360;
			while(angleDiff < -180)
				angleDiff += 360;

			if(std::abs(angleDiff) > color_threshold_)
				return false;
		}

		return true;
	}

	void setColorThreshold(float t)
	{ color_threshold_ = t; }

	void setDist2Shelf(const cv::Mat_<float>& dist)
	{ m_dist2Shelf = dist; }

	void setShelfThreshold(float shelf)
	{ m_shelfThreshold = shelf; }
private:
	float color_threshold_ = 10;
	cv::Mat_<float> m_dist2Shelf;
	float m_shelfThreshold = 0.02;
};

cv::Mat_<float> readRawFile(const fs::path& path)
{
	FILE* f = fopen(path.c_str(), "r");

	if(!f)
	{
		std::stringstream ss;
		ss << "Could not open data file: " << path.string();
		throw std::runtime_error(ss.str());
	}

	cv::Mat_<float> ret(1080, 1920);

	for(int y = 0; y < ret.rows; ++y)
	{
		if((int)fread(ret[y], sizeof(float), ret.cols, f) != ret.cols)
			throw std::runtime_error("Short read");
	}

	fclose(f);

	return ret;
}

Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> generateProposals(const PointCloud::ConstPtr& input, const pcl::PointCloud<pcl::Normal>::ConstPtr& normalCloud, const cv::Mat_<uint8_t>& mask_box, /*const cv::Mat_<float>& dist2Shelf,*/
	const ProposalParameters& params, cv::Mat_<cv::Vec3b>* vis)
{
	// Segment
	pcl::PointCloud<pcl::Label> labels;
	std::vector<pcl::PointIndices> labelIndices;
// 	{
		pcl::Label initialLabel;
		initialLabel.label = 0;
		auto initialLabels = boost::make_shared<pcl::PointCloud<pcl::Label>>(input->width, input->height, initialLabel);

		std::vector<bool> excludeLabels(input->size(), false);

		auto comparator = boost::make_shared<MyComparator>();
		comparator->setInputCloud(input);
		comparator->setInputNormals(normalCloud);
		comparator->setLabels(initialLabels);
		comparator->setExcludeLabels(excludeLabels);

		comparator->setAngularThreshold(params.angularThreshold * M_PI / 180.0f);
		comparator->setDistanceThreshold(params.distanceThreshold, false);
		comparator->setColorThreshold(params.colorThreshold);
		comparator->setShelfThreshold(params.shelfThreshold);
// 		comparator->setDist2Shelf(dist2Shelf);

		pcl::OrganizedConnectedComponentSegmentation<pcl::PointXYZRGB, pcl::Label> segmentation(comparator);

		segmentation.setInputCloud(input);
		segmentation.segment(labels, labelIndices);
// 	}

	if(vis)
		*vis = cv::Mat_<cv::Vec3b>(input->height, input->width);

	std::mt19937 rd;
	std::uniform_int_distribution<int> distr(0, 255);

	std::vector<cv::Rect> rectangles;

	{
		if(vis)
			*vis = cv::Vec3b(0, 0, 0);

		for(auto& indices : labelIndices)
		{
// 			if(indices.indices.size() < 600)
// 				continue;

// 			auto& color = COLORS[cnt % COLORS.size()];

			int minX = -1;
			int minY = -1;
			int maxX = -1;
			int maxY = -1;

			int numPoints = 0;

			cv::Vec3b color;

			if(vis)
				color = cv::Vec3b(distr(rd), distr(rd), distr(rd));

			for(auto& index : indices.indices)
			{
				int y = index / input->width;
				int x = index % input->width;

				if(mask_box(y,x) == 0)
					continue;

				if(minX < 0)
				{
					minX = indices.indices[0] % input->width;
					minY = indices.indices[0] / input->width;
					maxX = minX;
					maxY = minY;
				}

				minX = std::min(minX, x);
				minY = std::min(minY, y);
				maxX = std::max(maxX, x);
				maxY = std::max(maxY, y);

				numPoints++;

				if(vis)
					(*vis)(y,x) = color;
			}

			if(numPoints >= 200)
				rectangles.push_back(cv::Rect(cv::Point(minX, minY), cv::Point(maxX, maxY)));
		}
	}

	std::sort(rectangles.begin(), rectangles.end(), [](const cv::Rect& a, const cv::Rect& b) {
		return a.area() > b.area();
	});

	Eigen::MatrixXf rectData(1000, 4);
	unsigned int numValid = 0;

	for(size_t i = 0; i < std::min<size_t>(rectangles.size(), 1000); ++i)
	{
		if(rectangles[i].area() > (int)(input->width*input->height/2))
			continue;

		if(rectangles[i].area() < 100*100)
			continue;

		float scale = 720.0 / 1920.0;

		rectData(numValid, 0) = scale * (1920 - 1 - (rectangles[i].x + rectangles[i].width/2));
		rectData(numValid, 1) = scale * (1080 - 1 - (rectangles[i].y + rectangles[i].height/2));
		rectData(numValid, 2) = scale * rectangles[i].width;
		rectData(numValid, 3) = scale * rectangles[i].height;

		numValid++;
	}

	return rectData.topRows(numValid);
}

void compute(const fs::path& path, const ProposalParameters& params)
{
	PointCloud::Ptr input(new PointCloud);
	pcl::io::loadPCDFile((path / "frame.pcd").string(), *input);

	if(!input->isOrganized())
		throw std::runtime_error("Input is not organized");

	cv::Mat_<cv::Vec3b> mask_box = cv::imread(
		(path / "mask_box.png").string(), CV_LOAD_IMAGE_GRAYSCALE
	);

	cv::Mat_<float> dist2shelf = readRawFile(path / "feature_dist2shelf.raw");

	auto start = std::chrono::high_resolution_clock::now();

	// Compute normals
	pcl::PointCloud<pcl::Normal>::Ptr normalCloud(new pcl::PointCloud<pcl::Normal>);
	{
		pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;

		ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
		ne.setMaxDepthChangeFactor(0.8f);
		ne.setNormalSmoothingSize(80.0f);

		ne.setInputCloud(input);
		ne.compute(*normalCloud);
	}

	cv::Mat_<cv::Vec3b> vis;
	auto rectangles = generateProposals(input, normalCloud, mask_box, /*dist2shelf,*/ params, &vis);

	auto end = std::chrono::high_resolution_clock::now();
	printf("Proposal generator took %ld ms\n", std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count());

	cv::Mat_<cv::Vec3b> rgb = cv::imread((path / "rgb.png").string());

	std::mt19937 rd;
	std::uniform_int_distribution<int> distr(0, 255);

	for(int i = 0; i < rectangles.rows(); ++i)
	{
		float invScale = 1920.0 / 720.0;

		Eigen::VectorXf row = invScale * rectangles.row(i);
		cv::Rect rect(
			1920 - 1 - row[0] - row[2]/2,
			1080 - 1 - row[1] - row[3]/2,
			row[2], row[3]
		);

		cv::rectangle(rgb, rect, cv::Scalar(distr(rd), distr(rd), distr(rd)), 3);
	}

	cv::imwrite((path / "proposals.png").string(), rgb);
	cv::imwrite((path / "proposal_vis.png").string(), vis);

	{
		static_assert(decltype(rectangles)::IsRowMajor, "need row major storage for saving");

		FILE* f = fopen((path / "rgb.png.prop.bbox.bin").c_str(), "w");
		fwrite(rectangles.data(), sizeof(float), rectangles.rows()*4, f);
		fclose(f);
	}
}

}

int main(int argc, char** argv)
{
	namespace po = boost::program_options;

	std::string pathStr = ".";
	apc_densecap::ProposalParameters params;

	po::options_description opt("Options");
	opt.add_options()
		("help", "Help message")
		("angular",
			po::value(&params.angularThreshold)->default_value(params.angularThreshold),
			"Angular threshold for normal direction"
		)
		("distance",
			po::value(&params.distanceThreshold)->default_value(params.distanceThreshold),
			"Distance threshold"
		)
		("color",
			po::value(&params.colorThreshold)->default_value(params.colorThreshold),
			"Color threshold"
		)
		("shelf",
			po::value(&params.shelfThreshold)->default_value(params.shelfThreshold),
			"Shelf distance threshold"
		)
		("path",
			po::value(&pathStr)->default_value(pathStr),
			"Path to operate on"
		)
	;

	po::positional_options_description positional;
	positional.add("path", 1);

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

	if(vm.count("help"))
	{
		std::cout << opt << std::endl;
		return 0;
	}

	// Gather list of images
	std::vector<fs::path> jobs;
	{
		auto it = fs::recursive_directory_iterator(pathStr);
		auto end = fs::recursive_directory_iterator();

		for(; it != end; ++it)
		{
			fs::path path = *it;
			if(fs::is_directory(path) && fs::exists(path / "polygons.yaml"))
				jobs.push_back(fs::canonical(path));
		}
	}

	unsigned int finished = 0;

	printf("\n");

	#pragma omp parallel for
	for(size_t i = 0; i < jobs.size(); ++i)
	{
		apc_densecap::compute(jobs[i], params);

		#pragma omp critical (data)
		{
			finished++;
			printf("\r%03u/%03lu", finished, jobs.size());
			fflush(stdout);
		}
	}

	printf("\n");

	return 0;
}


//####### Test module for Schwinger Pair engine (SI units) ######################

//Define Module name
 #define BOOST_TEST_MODULE "Schwinger Pair engine engine (SI)"

//Will automatically define a main for this test
 #define BOOST_TEST_DYN_LINK

 #include <memory>

//Include Boost unit tests library
#include <boost/test/unit_test.hpp>

//SI units are used for this test
#define PXRMP_USE_SI_UNITS
#include "schwinger_pair_engine.hpp"

#include "rng_wrapper.hpp"

using namespace picsar::multi_physics;

//Helper function
template<typename REAL>
schwinger_pair_engine<REAL, stl_rng_wrapper<REAL>>
get_schwinger_stl_set_lambda(uint64_t seed)
{
    stl_rng_wrapper<REAL> wrap{seed};
    auto schw_engine =  schwinger_pair_engine<REAL, stl_rng_wrapper<REAL>>
        {std::move(wrap), 1.0};
    return schw_engine;
}

// ------------- Tests --------------

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-3;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-2;

//Templated tolerance
template <typename T>
T tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

//***SI UNITS***
//SI units for momenta
const double me_c = electron_mass * light_speed;
//SI units for fields
double lambda = 800.0 * si_nanometer;
double eref = 2.0*pi*electron_mass*light_speed*light_speed/
            (lambda*elementary_charge);
double bref = eref/light_speed;
//SI units for dt and rate
double dtref = lambda/(2.0*pi*light_speed);
double rateref = 1.0/dtref;

//Test get/set lambda for breit_wheeler_engine generic
template <typename T>
void schwinger_pair_engine_gs()
{
    auto schw_engine = get_schwinger_stl_set_lambda<T>(390109317);
    BOOST_CHECK_EQUAL( static_cast<T>(1.0), schw_engine.get_lambda());
}

//Test get/set lambda for breit_wheeler_engine (double precision)
BOOST_AUTO_TEST_CASE( schwinger_pair_engine_gs_double_1 )
{
    schwinger_pair_engine_gs<double>();
}

//Test get/set lambda for breit_wheeler_engine (single precision)
BOOST_AUTO_TEST_CASE( schwinger_pair_engine_gs_single_1 )
{
    schwinger_pair_engine_gs<float>();
}

// ------------- Pair positions --------------
//Test relative position of the new pair for the schwinger_pair_engine
template <typename T>
void schwinger_pair_engine_pos()
{
    auto schw_engine = get_schwinger_stl_set_lambda<T>(390109317);
    auto res = schw_engine.get_new_pair_position();
    for (auto val: res)
        BOOST_TEST ( (val >= static_cast<T>(0.0) && val <= static_cast<T>(1.0)) );
}

//Test get new optical depth (double precision)
BOOST_AUTO_TEST_CASE( schwinger_pair_engine_pos_double_1 )
{
    schwinger_pair_engine_pos<double>();
}

//Test get new optical depth (single precision)
BOOST_AUTO_TEST_CASE( schwinger_pair_engine_pos_single_1 )
{
    schwinger_pair_engine_pos<float>();
}

// ------------- Pair production rates --------------

/*
//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_6()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);

    T px =  static_cast<T>(-965.61*me_c);
    T py =  static_cast<T>(-3975.11*me_c);
    T pz =  static_cast<T>(6917.22*me_c);
    T ex =  static_cast<T>(11.17*eref);
    T ey =  static_cast<T>(-2117.72*eref);
    T ez =  static_cast<T>(-1407.19*eref);
    T bx =  static_cast<T>( 6259.79*bref);
    T by =  static_cast<T>(7557.54*bref);
    T bz =  static_cast<T>(773.11*bref);

    T exp = static_cast<T>(3.51855878777*rateref);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz})*light_speed, chi_photon(px,py,pz,ex,ey,ez,bx,by,bz) );

    BOOST_CHECK_SMALL((exp-res)/exp, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_6 )
{
    breit_wheeler_engine_prod_6<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_6 )
{
    breit_wheeler_engine_prod_6<float>();
}
*/


#include <boost/proto/proto.hpp>
#include <boost/proto/context/default.hpp>
#include <boost/mpl/int.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/cmath.hpp>
#include <type_traits>

struct x_var {};

template <int v>
struct derivative_constant
{
	using result_type = int;
	static const int value = v;

	template <typename ...Args>
	result_type operator()(const Args&... a) const { return v; }

	operator result_type() const { return v; }
};

template <int V>
using derivative_constant_terminal = typename boost::proto::terminal<derivative_constant<V>>::type;

struct derivative_grammar;

template<typename Expr>
struct derivative_expr;

namespace derivative_detail
{
	struct optimize
		: boost::proto::switch_<struct optimize_cases_>
	{};

	struct expr_generator
	{
		BOOST_PROTO_CALLABLE()
		BOOST_PROTO_USE_BASIC_EXPR()

		template<typename Sig>
		struct result;

		template<typename This, typename Expr>
		struct result<This(Expr)>
		{
			typedef derivative_expr<Expr> type;
		};

		template<typename This, typename Expr>
		struct result<This(Expr &)>
		{
			typedef derivative_expr<Expr> type;
		};

		template<typename This, typename Expr>
		struct result<This(Expr const &)>
		{
			typedef derivative_expr<Expr> type;
		};

		template<typename Expr>
		derivative_expr<Expr> operator ()(Expr const &e) const
		{
			derivative_expr<Expr> that = { e };
			return that;
		}
	};

}

struct domain
	: boost::proto::domain< derivative_detail::expr_generator, derivative_grammar >
{
	template< typename T >
	struct as_child
		: proto_base_domain::as_expr< T >
	{};
};

template< int Exp >
struct pow_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;
		BOOST_FORCEINLINE static type apply(const T& t) { using std::pow; return pow(t, Exp); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units,T>)>
	{
		using type = decltype(boost::units::pow<Exp>(std::declval<boost::units::quantity<Units,T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units,T>& t) { using boost::units::pow; return pow<Exp>(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<pow_fun<Exp>(T)>::type operator()(const T& d) const
	{
		return result<pow_fun<Exp>(T)>::apply(d);
	}
};

template< int Exp, typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, pow_fun< Exp >, Arg>::type const pow(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(pow_fun<Exp>(), arg);
}

struct sin_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::sin; return sin(t); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::sin(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::sin; return sin(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<sin_fun(T)>::type operator()(const T& d) const
	{
		return result<sin_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, sin_fun, Arg>::type const sin(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(sin_fun(), arg);
}

struct cos_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::cos; return cos(t); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::cos(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::cos; return cos(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<cos_fun(T)>::type operator()(const T& d) const
	{
		return result<cos_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, cos_fun, Arg>::type const cos(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(cos_fun(), arg);
}

struct exp_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::exp; return exp(t); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::exp(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::exp; return exp(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<exp_fun(T)>::type operator()(const T& d) const
	{
		return result<exp_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, exp_fun, Arg>::type const exp(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(exp_fun(), arg);
}

struct tan_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::tan; return tan(t); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::tan(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::tan; return tan(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<tan_fun(T)>::type operator()(const T& d) const
	{
		return result<tan_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, tan_fun, Arg>::type const tan(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(tan_fun(), arg);
}

struct sqrt_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::sqrt; using std::abs; return sqrt(abs(t)); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::root<2>(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::abs; return boost::units::root<2>(abs(t)); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<sqrt_fun(T)>::type operator()(const T& d) const
	{
		return result<sqrt_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, sqrt_fun, Arg>::type const sqrt(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(sqrt_fun(), arg);
}

struct log_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::log; return log(t); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::log(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::log; return log(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<log_fun(T)>::type operator()(const T& d) const
	{
		return result<log_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, log_fun, Arg>::type const log(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(log_fun(), arg);
}

template <typename T>
struct evaluation_context : boost::proto::callable_context<evaluation_context<T> const> {
	T value;

	explicit evaluation_context(T value)
		: value(value)
	{}

	template <typename Sig>
	struct result;

	template <typename This, typename Tag, int V>
	struct result<This(Tag, derivative_constant<V>)>
	{
		using type = double;
	};

	template <typename This, typename Tag, int V>
	struct result<This(Tag, const derivative_constant<V>&)>
	{
		using type = double;
	};

	template <typename This, typename Tag>
	struct result<This(Tag, x_var)>
	{
		using type = T;
	};

	template <typename This, typename Tag>
	struct result<This(Tag, const x_var&)>
	{
		using type = T;
	};

	template <int V>
	BOOST_FORCEINLINE double operator()(boost::proto::tag::terminal, derivative_constant<V>) const
	{
		return V;
	}

	BOOST_FORCEINLINE T operator()(boost::proto::tag::terminal, x_var) const
	{
		return value;
	}
};

template<typename Expr>
struct derivative_expr : boost::proto::extends<Expr, derivative_expr<Expr>, domain>
{
	typedef boost::proto::extends<Expr, derivative_expr<Expr>, domain> base_type;

	derivative_expr(Expr const& expr = Expr())
		: base_type(expr)
	{}

	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using raw_type = typename boost::proto::result_of::eval<Expr, evaluation_context<T>>::type;
		using type = typename boost::remove_const<typename boost::remove_reference<raw_type>::type>::type;
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<derivative_expr(T)>::type operator()(T d) const
	{
		evaluation_context<T> context(d);
		return boost::proto::eval(*this, context);
	}
};

//! Constants
struct constant_rule : boost::proto::callable
{
	using result_type = derivative_expr <boost::proto::terminal<derivative_constant<0>>::type>;

	BOOST_FORCEINLINE result_type operator()() const
	{
		return result_type();
	}
};

//! Derivative Variable
struct derivative_variable_rule : boost::proto::callable
{
	using result_type = derivative_expr <boost::proto::terminal<derivative_constant<1>>::type>;

	BOOST_FORCEINLINE result_type operator()() const
	{
		return result_type();
	}
};

//! Addition

namespace derivative_detail {

	template<typename Tag>
	struct make_optimized_binary
	{
		BOOST_PROTO_CALLABLE();
		BOOST_PROTO_POLY_FUNCTION();

		using Domain = boost::proto::domainns_::deduce_domain;

		template<typename Sig>
		struct result;

		template<typename This, typename A0, typename A1>
		struct result<This(A0, A1)>
		{
			using raw_type = typename boost::proto::result_of::make_expr<Tag, Domain, A0, A1>::type;

			using type = typename boost::result_of<optimize(raw_type)>::type;
		};

		/// Construct an expression node with tag type \c Tag
		/// and in the domain \c Domain.
		///
		/// \return <tt>proto::make_expr\<Tag, Domain\>(a0,...aN)</tt>
		template<typename A0, typename A1>
		BOOST_FORCEINLINE
		typename result<make_optimized_binary<Tag>(A0,A1)>::type const operator ()(A0 const &a0, A1 const& a1) const
		{
			auto result = optimize()(boost::proto::detail::make_expr_<Tag, Domain, A0 const, A1 const>()(a0, a1));
			//boost::proto::display_expr(result, std::cout);
			return result;
		}

		/// INTERNAL ONLY
		///
		template<typename A0, typename A1>
		struct impl
			: boost::proto::detail::make_expr_<Tag, Domain, A0, A1>
		{};
	};

	struct optimize_cases_
	{
		template <typename Tag, int D = 0>
		struct case_ : boost::proto::not_<boost::proto::_> {};

		template <int D>
		struct case_<boost::proto::tag::plus, D>
			: boost::proto::or_
			  <
				boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::plus, boost::proto::_, boost::proto::terminal<derivative_constant<0>> >
				,	optimize(boost::proto::_left)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::plus, boost::proto::terminal<derivative_constant<0>>, boost::proto::_ >
				,	optimize(boost::proto::_right)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::plus, boost::proto::_, boost::proto::_ >
				,	make_optimized_binary<boost::proto::tag::plus>(optimize(boost::proto::_left), optimize(boost::proto::_right))
				>
				, boost::proto::otherwise<boost::proto::_expr>
			  >
		{};

		template <int D>
		struct case_<boost::proto::tag::minus, D>
			: boost::proto::or_
			  <
				boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::minus, boost::proto::_, boost::proto::terminal<derivative_constant<0>> >
				,	optimize(boost::proto::_left)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::minus, boost::proto::terminal<derivative_constant<0>>, boost::proto::_ >
				,	boost::proto::_make_negate(optimize(boost::proto::_right))
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::minus, boost::proto::_, boost::proto::_ >
				,	boost::proto::call<make_optimized_binary<boost::proto::tag::minus>(optimize(boost::proto::_left), optimize(boost::proto::_right))>
				>
				, boost::proto::otherwise<boost::proto::_expr>
			  >
		{};

		template <int D>
		struct case_<boost::proto::tag::multiplies, D>
			: boost::proto::or_
			  <
				boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::multiplies, boost::proto::_, boost::proto::terminal<derivative_constant<0>> >
				,	constant_rule()
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::multiplies, boost::proto::terminal<derivative_constant<0>>, boost::proto::_ >
				,	constant_rule()
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::multiplies, boost::proto::_, boost::proto::terminal<derivative_constant<1>> >
				,	optimize(boost::proto::_left)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::multiplies, boost::proto::terminal<derivative_constant<1>>, boost::proto::_ >
				,	optimize(boost::proto::_right)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::multiplies, boost::proto::_, boost::proto::_ >
				,	boost::proto::call<make_optimized_binary<boost::proto::tag::multiplies>(optimize(boost::proto::_left), optimize(boost::proto::_right))>
				>
				, boost::proto::otherwise<boost::proto::_expr>
			  >
		{};

		template <int D>
		struct case_<boost::proto::tag::divides, D>
			: boost::proto::or_
			  <
				boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::divides, boost::proto::terminal<derivative_constant<0>>, boost::proto::_ >
				,	constant_rule()
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::divides, boost::proto::_, boost::proto::terminal<derivative_constant<1>> >
				,	optimize(boost::proto::_left)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::divides, boost::proto::_, boost::proto::_ >
				,	boost::proto::call<make_optimized_binary<boost::proto::tag::divides>(optimize(boost::proto::_left), optimize(boost::proto::_right))>
				>
				, boost::proto::otherwise<boost::proto::_expr>
			  >
		{};

		template <int D>
		struct case_<boost::proto::tag::function, D>
			: boost::proto::when
		      <
				boost::proto::_
			  , boost::proto::_make_function(boost::proto::_left, optimize(boost::proto::_right))
			  >
		{};
	};

	template <typename T1, typename T2>
	struct plus
	{
		using type = decltype(std::declval<T1>() + std::declval<T2>());
	};

	template <typename T1>
	struct plus<T1, derivative_constant<0>>
	{
		using type = T1;
	};

	template <typename T2>
	struct plus<derivative_constant<0>, T2>
	{
		using type = T2;
	};

}//! namespace derivative_detail;

namespace boost {
	namespace proto {
		template<typename Tag>
		struct is_callable<derivative_detail::make_optimized_binary<Tag> >
			: mpl::true_
		{};
} }

struct addition_rule : boost::proto::callable
{
	template <typename Sig>
	struct result;

	template <typename This, typename T1, typename T2>
	struct result<This(T1, T2)>
	{
	private:
		using t1_t = typename boost::remove_const<typename boost::remove_reference<T1>::type>::type;
		using t2_t = typename boost::remove_const<typename boost::remove_reference<T2>::type>::type;

	public:
		using raw_type = typename derivative_detail::plus<t1_t, t2_t>::type;
		using type = typename boost::result_of<derivative_detail::optimize(raw_type)>::type;
	};

	template <typename T1, typename T2>
	typename result<addition_rule(T1, T2)>::type operator()(const T1& t1, const T2& t2) const
	{
		return derivative_detail::optimize()(t1 + t2);
	}
};

//! Subtraction

namespace derivative_detail {

	template <typename T1, typename T2>
	struct minus
	{
		using type = decltype(std::declval<T1>() - std::declval<T2>());
	};

	template <typename T1>
	struct minus<T1, derivative_constant<0>>
	{
		using type = T1;
	};

	template <typename T2>
	struct minus<derivative_constant<0>, T2>
	{
		using type = decltype(-std::declval<T2>());
	};

}//! namespace derivative_detail;

struct subtraction_rule : boost::proto::callable
{
	template <typename Sig>
	struct result;

	template <typename This, typename T1, typename T2>
	struct result<This(T1, T2)>
	{
	private:
		using t1_t = typename boost::remove_const<typename boost::remove_reference<T1>::type>::type;
		using t2_t = typename boost::remove_const<typename boost::remove_reference<T2>::type>::type;

	public:
		using raw_type = typename derivative_detail::minus<t1_t, t2_t>::type;
		using type = typename boost::result_of<derivative_detail::optimize(raw_type)>::type;
	};

	template <typename T1, typename T2>
	typename result<subtraction_rule(T1, T2)>::type operator()(const T1& t1, const T2& t2) const
	{
		return derivative_detail::optimize()(t1 - t2);
	}
};

//! Multiplication
namespace derivative_detail {

	template <typename T1, typename T2>
	struct multiplies
	{
		using type = decltype(std::declval<T1>() * std::declval<T2>());
	};

	template <typename T1>
	struct multiplies<T1, derivative_constant<0>>
	{
		using type = derivative_constant<0>;
	};

	template <typename T2>
	struct multiplies<derivative_constant<0>, T2>
	{
		using type = derivative_constant<0>;
	};

	template <typename T1>
	struct multiplies<T1, derivative_constant<1>>
	{
		using type = T1;
	};

	template <typename T2>
	struct multiplies<derivative_constant<1>, T2>
	{
		using type = T2;
	};

	template <typename Arg1, typename dArg1, typename Arg2, typename dArg2>
	struct product_rule_result_helper
	{
		using termA = decltype(std::declval<dArg1>() * std::declval<Arg2>());
		using termB = decltype(std::declval<Arg1>() * std::declval<dArg2>());
		using otermA = typename boost::result_of<optimize(termA)>::type;
		using otermB = typename boost::result_of<optimize(termB)>::type;
		using type = decltype(std::declval<otermA>() + std::declval<otermB>());
	};

}//! namespace derivative_detail;

struct product_rule : boost::proto::callable
{
	template <typename Sig>
	struct result;

	template <typename This, typename Arg1, typename dArg1, typename Arg2, typename dArg2>
	struct result<This(Arg1, dArg1, Arg2, dArg2)>
	{
	private:

		using arg1_t = typename boost::remove_const<typename boost::remove_reference<Arg1>::type>::type;
		using darg1_t = typename boost::remove_const<typename boost::remove_reference<dArg1>::type>::type;
		using arg2_t = typename boost::remove_const<typename boost::remove_reference<Arg2>::type>::type;
		using darg2_t = typename boost::remove_const<typename boost::remove_reference<dArg2>::type>::type;
		using raw_type = typename derivative_detail::product_rule_result_helper<arg1_t, darg1_t, arg2_t, darg2_t>::type;

	public:
		using type = typename boost::result_of<derivative_detail::optimize(raw_type)>::type;
	};

	template <typename Arg1, typename dArg1, typename Arg2, typename dArg2>
	typename result<product_rule(Arg1, dArg1, Arg2, dArg2)>::type operator()(const Arg1& a1, const dArg1& da1, const Arg2& a2, const dArg2& da2) const
	{
		using result_type = typename result<product_rule(Arg1, dArg1, Arg2, dArg2)>::type;
		derivative_detail::optimize opt;
		auto result = opt(opt(da1 * a2) + opt(a1 * da2));
		return result;
	}
};

//! Division
struct quotient_rule : boost::proto::callable
{
	template <typename Sig>
	struct result;

	template <typename This, typename Arg1, typename dArg1, typename Arg2, typename dArg2>
	struct result<This(Arg1, dArg1, Arg2, dArg2)>
	{
	private:
		using termA_t = decltype(std::declval<dArg1>() * std::declval<Arg2>());
		using otermA_t = typename boost::result_of<derivative_detail::optimize(termA_t)>::type;
		using termB_t = decltype(std::declval<dArg2>() * std::declval<Arg1>());
		using otermB_t = typename boost::result_of<derivative_detail::optimize(termB_t)>::type;

		using num_t = typename boost::result_of<derivative_detail::optimize(decltype(std::declval<otermA_t>() - std::declval<otermB_t>()))>::type;
		using denom_t = typename boost::result_of<derivative_detail::optimize(decltype(std::declval<Arg2>() * std::declval<Arg2>()))>::type;
		using raw_type = decltype(std::declval<num_t>() / std::declval<denom_t>());
	public:

		using type = typename boost::result_of<derivative_detail::optimize(raw_type)>::type;
	};

	template <typename Arg1, typename dArg1, typename Arg2, typename dArg2>
	typename result<quotient_rule(Arg1, dArg1, Arg2, dArg2)>::type operator()(const Arg1& a1, const dArg1& da1, const Arg2& a2, const dArg2& da2) const
	{
		using result_type = typename result<quotient_rule(Arg1, dArg1, Arg2, dArg2)>::type;
		derivative_detail::optimize opt;
		auto result = opt( opt(opt(da1 * a2) - opt(da2 * a1)) / opt(a2 * a2) );
		return result;
	}
};

namespace derivative_detail
{
	template <typename Fn, typename Arg, typename dArg>
	struct chain_rule_result_helper;

	template <int Exp, typename Arg, typename dArg>
	struct chain_rule_result_helper<pow_fun<Exp>, Arg, dArg>
	{
		using type = decltype(static_cast<double>(Exp) * pow<Exp - 1>(std::declval<Arg>()) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			return static_cast<double>(Exp) * pow<Exp - 1>(arg) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<pow_fun<2>, Arg, dArg>
	{
		using type = decltype(2 * std::declval<Arg>() * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			return 2 * arg * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<pow_fun<1>, Arg, dArg>
	{
		using type = dArg;

		static type apply(const Arg& arg, const dArg& darg)
		{
			return darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<cos_fun, Arg, dArg>
	{
		using type = decltype( -1.0 * sin( std::declval<Arg>() ) * std::declval<dArg>() );

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::sin;
			return -1.0 * sin(arg) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<sin_fun, Arg, dArg>
	{
		using type = decltype(cos(std::declval<Arg>()) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::cos;
			return cos(arg) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<exp_fun, Arg, dArg>
	{
		using type = decltype(exp(std::declval<Arg>()) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::exp;
			return exp(arg) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<tan_fun, Arg, dArg>
	{
		using type = decltype(sec(std::declval<Arg>()) * sec(std::declval<Arg>()) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::cos;
			auto r = cos(arg);
			return (1.0 / (r * r)) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<log_fun, Arg, dArg>
	{
		using type = decltype( (1.0 / std::declval<Arg>()) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::log;
			return (1.0 / arg) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<sqrt_fun, Arg, dArg>
	{
		using type = decltype((0.5 / sqrt(std::declval<Arg>())) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::sqrt;
			return (0.5 / sqrt(arg)) * darg;
		}
	};

}//! namespace derivative_detail;

//! Chain rule
struct chain_rule : boost::proto::callable
{
	template <typename Sig>
	struct result;

	template <typename This, typename Fn, typename Arg, typename dArg>
	struct result<This(Fn, Arg, dArg)>
	{
	private:
		using fn_t = typename boost::remove_const<typename boost::remove_reference<Fn>::type>::type;
		using arg_t = typename boost::remove_const<typename boost::remove_reference<Arg>::type>::type;
		using darg_t = typename boost::remove_const<typename boost::remove_reference<dArg>::type>::type;
		using raw_type = typename derivative_detail::chain_rule_result_helper<fn_t, arg_t, darg_t>::type;

	public:

		using type = typename boost::result_of<derivative_detail::optimize(raw_type)>::type;
	};

	template <typename Fn, typename Arg, typename dArg>
	typename result<chain_rule(Fn, Arg, dArg)>::type operator() (const Fn&, const Arg& arg, const dArg& darg) const
	{
		auto result = derivative_detail::optimize()(derivative_detail::chain_rule_result_helper<Fn, Arg, dArg>::apply(arg, darg));
		//boost::proto::display_expr(result, std::cout);
		return result;
	}
};

struct get_value
	: boost::proto::when<boost::proto::terminal<boost::proto::_>, boost::proto::_value>
{};

struct derivative_grammar
	: boost::proto::switch_<struct cases_>
{};

struct cases_
{
	template <typename Tag, int D=0>
	struct case_ : boost::proto::not_<boost::proto::_> {};

	template <int D>
	struct case_<boost::proto::tag::terminal, D>
		: boost::proto::or_
		  <
			boost::proto::when
			<
				boost::proto::terminal<x_var>
			,	derivative_variable_rule()
			>
		  , boost::proto::when
			<
				boost::proto::terminal<boost::proto::_>
			,	constant_rule()
			>
		  >
	{};

	template <int D>
	struct case_<boost::proto::tag::function, D>
		: boost::proto::when
		  <
			boost::proto::_
		  ,	chain_rule(get_value(boost::proto::_left), boost::proto::_right, derivative_grammar(boost::proto::_right))
		  >
	{};

	template <int D>
	struct case_<boost::proto::tag::divides, D>
		: boost::proto::when
		  <
			boost::proto::_
		  ,	quotient_rule(boost::proto::_left, derivative_grammar(boost::proto::_left), boost::proto::_right, derivative_grammar(boost::proto::_right))
		  >
	{};

	template <int D>
	struct case_<boost::proto::tag::multiplies, D>
		: boost::proto::when
		  <
			boost::proto::_
		  ,	product_rule(boost::proto::_left, derivative_grammar(boost::proto::_left), boost::proto::_right, derivative_grammar(boost::proto::_right))
		  >
	{};

	template <int D>
	struct case_<boost::proto::tag::plus, D>
		: boost::proto::when
		  <
			boost::proto::_
		  ,	addition_rule(derivative_grammar(boost::proto::_left), derivative_grammar(boost::proto::_right))
		  >
	{};

	template <int D>
	struct case_<boost::proto::tag::minus, D>
		: boost::proto::when
		  <
			boost::proto::_
		  ,	subtraction_rule(derivative_grammar(boost::proto::_left), derivative_grammar(boost::proto::_right))
		  >
	{};
};


//============================================================================
// Name        : boost_rng_bindings.hpp
// Author      : Mikail Sheikh
// Date        : 28/11/2013
// Copyright   : Distributed under the MIT License (MIT)
// Description : A QuantLib wrapper for Boost random number generators
//============================================================================

#ifndef BOOSTRNG_HPP_
#define BOOSTRNG_HPP_

#include <ql/types.hpp>
#include <ql/math/randomnumbers/seedgenerator.hpp>
#include <ql/math/randomnumbers/rngtraits.hpp>
#include <ql/methods/montecarlo/sample.hpp>

#include <boost/static_assert.hpp>
#include <boost/random.hpp>

using namespace QuantLib;

template<class RNG>
class GenericBoostIntRNG
{
  private:
    mutable RNG generator;
    static Real ceil_value;
  public:
    typedef Sample<Real> sample_type;
    typedef typename RNG::result_type result_type;

    BOOST_STATIC_ASSERT (std::numeric_limits<result_type>::is_integer);

    GenericBoostIntRNG(result_type seed = 0);

    //! return a random number in the (0.0, 1.0)-interval
    //! For random number generators with a precision larger than
    Real nextReal() const
    {
      return (Real(generator()) + 0.5) / ceil_value;
    }

    /*! returns a sample with weight 1.0 containing a random number
     in the (0.0, 1.0) interval  */
    sample_type next() const
    {
      return sample_type(nextReal(), 1.0);
    }

  private:
    result_type getSeed(result_type seed);
};

template<class RNG>
class GenericBoostFloatRNG
{
  private:
    mutable RNG generator;
    static Real ceil_value;
  public:
    typedef Sample<Real> sample_type;

    typedef typename RNG::result_type result_type;
    typedef boost::uint32_t seed_type;
    BOOST_STATIC_ASSERT (std::numeric_limits<result_type>::is_float);

    GenericBoostFloatRNG(seed_type seed = 0);

    /*! returns a sample with weight 1.0 containing a random number
     in the (0.0, 1.0) interval  */
    sample_type next() const
    {
      return sample_type(nextReal(), 1.0);
    }
    //! return a random number in the (0.0, 1.0)-interval
    result_type nextFloat() const
    {
      return generator();
    }

  private:
    seed_type getSeed(seed_type seed);
};

typedef GenericBoostIntRNG<boost::mt11213b> BoostMT11213bUniformRng;
typedef GenericPseudoRandom<
    BoostMT11213bUniformRng,
    InverseCumulativeNormal> PseudoRandomBoostMT11213b;

typedef GenericBoostIntRNG<boost::mt19937> BoostMT19937UniformRng;
typedef GenericPseudoRandom<
    BoostMT19937UniformRng,
    InverseCumulativeNormal> PseudoRandomBoostMT19937;

template<class RNG>
Real GenericBoostIntRNG<RNG>::ceil_value = Real(RNG::max()) + 1.0;

template<class RNG>
GenericBoostIntRNG<RNG>::GenericBoostIntRNG(result_type seed) :
    generator(GenericBoostIntRNG<RNG>::getSeed(seed))
{
}

template<class RNG>
typename GenericBoostIntRNG<RNG>::result_type GenericBoostIntRNG<RNG>::getSeed(
    result_type seed)
{
  unsigned long s = (seed != 0 ? seed : SeedGenerator::instance().get());
  return (result_type) (s % RNG::max());
}

template<class RNG>
GenericBoostFloatRNG<RNG>::GenericBoostFloatRNG(result_type seed) :
    generator(GenericBoostFloatRNG<RNG>::getSeed(seed))
{
}

template<class RNG>
typename GenericBoostFloatRNG<RNG>::seed_type GenericBoostFloatRNG<RNG>::getSeed(
    seed_type seed)
{
  unsigned long s = (seed != 0 ? seed : SeedGenerator::instance().get());
  return (seed_type) (s % 0xffffffff);
}

#endif /* BOOSTRNG_HPP_ */


/*
 * RbpfModelGraph.hpp
 *
 *  Represents a directed graph with weighted transitions.
 *  Each node in the graph is a RbpfParticleModel, which contains the
 *  transition model for a Kalman filter. The graph's edges represent
 *  transition probabilities.
 *
 *  To build the graph, add all the nodes using addModel() then set the
 *  transition probabilities using setTransProb().
 *
 *  Author: Philip Rogers, Nov 8th 2009
 */

#ifndef RBPFMODELGRAPH_HPP_
#define RBPFMODELGRAPH_HPP_

#include <iostream>
#include <boost/ptr_container/ptr_vector.hpp>
#include <LinearAlgebra.hpp>
#include "RbpfModel.hpp"

class RbpfModelGraph {
public:
    typedef boost::ptr_vector<RbpfModel> ModelVector;

    RbpfModelGraph();

    ~RbpfModelGraph();

    // adds a model as a node in the graph and initializes transition
    // probabilities to zero.
    void addModel(RbpfModel* model);

    // returns a model from the graph that corresponds to index modelIndex
    RbpfModel* getModel(int modelIndex);

    // sets the transition probability from model A to model B
    void setTransProb(int indexA, int indexB, double weight);

    // get the transition probability from model A to model B
    double getTransProb(int indexA, int indexB);

    // Used for printing this modelGraph to a stream
    friend std::ostream& operator<<(std::ostream& out,
                                    const RbpfModelGraph& graph);

    int j;  // number of models in graph

protected:
    LinAlg::Matrix adjacencyMatrix;
    ModelVector modelVector;
};

#endif /* RBPFMODELGRAPH_HPP_ */


//
// Created by michel on 28-12-21.
//

#include "org-simple/util/text/StreamPredicate.h"
#include <org-simple/util/text/StringStream.h>
#include <org-simple/util/text/NumberParser.h>
#include <org-simple/core/Bits.h>
#include "test-helper.h"
#include "boost-unit-tests.h"
#include <boost/math/special_functions/relative_difference.hpp>
#include <cstdio>

namespace {
template <typename Value>
using Stream = org::simple::util::text::StringInputStream<char>;
using Result = org::simple::util::text::NumberParser::Result;
using NumberParser = org::simple::util::text::NumberParser;

template <typename Value = double> struct Scenario {
  static_assert(std::is_floating_point<Value>());
  std::string input;
  Result expectedResult;
  Value expectedValue;

  // A scenario with a number without prefix or anything
  Scenario(const char * string, Result result, Value value) : input(string), expectedResult(result), expectedValue(value) {

  }

  Scenario(long double number, Result result, Value value) : Scenario("", result, value) {
    static constexpr size_t FMT_LEN = 10;
    static constexpr int MANTISSA_DIGITS = std::numeric_limits<Value>::max_digits10;
    static constexpr size_t EXP_DIGITS = org::simple::core::Bits<unsigned>::most_significant(std::numeric_limits<Value>::max_exponent10) / 2;
    static constexpr size_t SIGNS_EXP_DECIMAL = 4;
    static constexpr size_t LEN = MANTISSA_DIGITS + EXP_DIGITS + SIGNS_EXP_DECIMAL + 10;
    char format[FMT_LEN+1];
    char buffer[LEN + 1];

    std::snprintf(format, FMT_LEN, "%%.%ulle", MANTISSA_DIGITS);
    std::snprintf(buffer, LEN, format, number);
    input = buffer;
  }

  static void test(const Scenario &scenario) {
    Stream<char> stream(scenario.input);
    Value actualValue;
    Result actualResult = NumberParser::readRealValueFromStream<char>(stream, actualValue);

    BOOST_CHECK_EQUAL(scenario.expectedResult, actualResult);
    if (actualResult == Result::Ok) {
      if (boost::math::relative_difference(scenario.expectedValue, actualValue) > std::numeric_limits<Value>::epsilon()) {
        BOOST_CHECK_EQUAL(scenario.expectedValue, actualValue);
      }
    }
  }

  void writeTo(std::ostream &out) const;
};

template <typename Value>
static void testScenario(const Scenario<Value> &scenario) {
  return Scenario<Value>::test(scenario);
}

} // namespace

namespace std {
static ostream &operator<<(ostream &out, Result result) {
  out << NumberParser ::resultToString(result);
  return out;
}

template <typename Value>
static ostream &operator<<(ostream &out, const Scenario<Value> &scenario) {
  scenario.writeTo(out);
  return out;
}

} // namespace std

template <typename Value>
void Scenario<Value>::writeTo(std::ostream &out) const {
  out << "IntegralValueReader<float_" << (sizeof(Value) * 8);
  out << "_t>(\""<< input << "\") -> " << expectedResult;

  if (expectedResult == Result::Ok) {
      out << "(value = " << expectedValue << ")";
  }
}

template <typename Value>
static std::vector<Scenario<Value>> &generateFLoatTestSamples() {
  static std::vector<Scenario<Value>> results;
  typedef long double testValueType;
  static constexpr testValueType max =
      static_cast<testValueType>(std::numeric_limits<Value>::max());
  static constexpr testValueType min =
      static_cast<testValueType>(std::numeric_limits<Value>::lowest());
  std::vector<testValueType> testValues;

  if (std::is_same<bool, Value>()) {
    testValues.push_back(0);
    testValues.push_back(1);
    testValues.push_back(2);
  } else {
    testValues.push_back(0);
    testValues.push_back(1);
    testValues.push_back(max / 2);
    testValues.push_back(max - 1);
    testValues.push_back(max);
    testValues.push_back(max + 1);
    testValues.push_back(max * 2);
    testValues.push_back(min - 1);
    if constexpr (std::is_signed<Value>()) {
      testValues.push_back(std::numeric_limits<Value>::min());
    }
    size_t count = testValues.size();
    for (size_t i = 1; i < count; i++) {
      testValues.push_back(-testValues.at(i));
    }
  }
//
  for (auto testValue : testValues) {
    Result result = testValue >= min && testValue <= max ? Result::Ok : Result::OutOfRange;
    results.push_back({testValue, result, static_cast<Value>(testValue)});
  }
  return results;
}

template <typename Value>
static std::vector<Scenario<Value>> &generateValidTestcases() {
  static std::vector<Scenario<Value>> results;

  results.push_back({"0", Result::Ok, 0});
  results.push_back({".0", Result::Ok, 0});
  results.push_back({"0.", Result::Ok, 0});
  results.push_back({"0.0", Result::Ok, 0});
  results.push_back({"+0", Result::Ok, 0});
  results.push_back({"+.0", Result::Ok, 0});
  results.push_back({"+0.", Result::Ok, 0});
  results.push_back({"+0.0", Result::Ok, 0});
  results.push_back({"-0", Result::Ok, 0});
  results.push_back({"-.0", Result::Ok, 0});
  results.push_back({"-0.", Result::Ok, 0});
  results.push_back({"-0.0", Result::Ok, 0});

  results.push_back({"1", Result::Ok, 1});
  results.push_back({".1", Result::Ok, 0.1});
  results.push_back({"1.", Result::Ok, 1});
  results.push_back({"1.0", Result::Ok, 1});
  results.push_back({"0.1", Result::Ok, 0.1});

  results.push_back({"+1", Result::Ok, 1});
  results.push_back({"+.1", Result::Ok, 0.1});
  results.push_back({"+1.", Result::Ok, 1});
  results.push_back({"+1.0", Result::Ok, 1});
  results.push_back({"+0.1", Result::Ok, 0.1});

  results.push_back({"-1", Result::Ok, -1});
  results.push_back({"-.1", Result::Ok, -0.1});
  results.push_back({"-1.", Result::Ok, -1});
  results.push_back({"-1.0", Result::Ok, -1});
  results.push_back({"-0.1", Result::Ok, -0.1});

  return results;
}

BOOST_AUTO_TEST_SUITE(test_org_simple_util_config_NumberParserFloatTest)

BOOST_DATA_TEST_CASE(testUnsignedCharScenarios,
                     generateFLoatTestSamples<double>()) {
  testScenario(sample);
}

//BOOST_DATA_TEST_CASE(testSignedCharScenarios,
//                     generateUnsignedSamples<double>()) {
//  testScenario(sample);
//}
//
//BOOST_DATA_TEST_CASE(testUnsignedShortScenarios,
//                     generateUnsignedSamples<long double>()) {
//  testScenario(sample);
//}

BOOST_DATA_TEST_CASE(testSimpleValisUsecases, generateValidTestcases<double>()) {
  testScenario(sample);
}
BOOST_AUTO_TEST_CASE(testValidDouble) {

}

BOOST_AUTO_TEST_SUITE_END()