/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#include "faster.hpp"

#include <pcl/kdtree/kdtree.h>
#include <Eigen/StdVector>
#include <stdio.h>
#include <math.h>
#include <algorithm>
#include <vector>
#include <stdlib.h>

using namespace JPS;
using namespace termcolor;

// Uncomment the type of timer you want:
// typedef ROSTimer MyTimer;
// typedef ROSWallTimer MyTimer;
typedef Timer MyTimer;

Faster::Faster(parameters par) : par_(par)
{
  drone_status_ == DroneStatus::YAWING;
  // mtx_G.lock();
  G_.pos << 0, 0, 0;
  // mtx_G.unlock();
  G_term_.pos << 0, 0, 0;

  mtx_initial_cond.lock();
  stateA_.setZero();
  mtx_initial_cond.unlock();

  // Setup of jps_manager
  std::cout << "par_.wdx / par_.res =" << par_.wdx / par_.res << std::endl;
  jps_manager_.setNumCells((int)par_.wdx / par_.res, (int)par_.wdy / par_.res, (int)par_.wdz / par_.res);
  jps_manager_.setFactorJPS(par_.factor_jps);
  jps_manager_.setResolution(par_.res);
  jps_manager_.setInflationJPS(par_.inflation_jps);
  jps_manager_.setZGroundAndZMax(par_.z_ground, par_.z_max);
  // jps_manager_.setVisual(par_.visual);
  jps_manager_.setDroneRadius(par_.drone_radius);

  double max_values[3] = { par_.v_max, par_.a_max, par_.j_max };

  // Setup of sg_whole_
  sg_whole_.setN(par_.N_whole);
  sg_whole_.createVars();
  sg_whole_.setDC(par_.dc);
  sg_whole_.setBounds(max_values);
  sg_whole_.setForceFinalConstraint(true);
  sg_whole_.setFactorInitialAndFinalAndIncrement(1, 10, par_.increment_whole);
  sg_whole_.setVerbose(par_.gurobi_verbose);
  sg_whole_.setThreads(par_.gurobi_threads);
  sg_whole_.setWMax(par_.w_max);

  // Setup of sg_safe_
  sg_safe_.setN(par_.N_safe);
  sg_safe_.createVars();
  sg_safe_.setDC(par_.dc);
  sg_safe_.setBounds(max_values);
  sg_safe_.setForceFinalConstraint(false);
  sg_safe_.setFactorInitialAndFinalAndIncrement(1, 10, par_.increment_safe);
  sg_safe_.setVerbose(par_.gurobi_verbose);
  sg_safe_.setThreads(par_.gurobi_threads);
  sg_safe_.setWMax(par_.w_max);

  pclptr_unk_ = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
  pclptr_map_ = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

  changeDroneStatus(DroneStatus::GOAL_REACHED);
  resetInitialization();
}

void Faster::createMoreVertexes(vec_Vecf<3>& path, double d)
{
  for (int j = 0; j < path.size() - 1; j++)
  {
    double dist = (path[j + 1] - path[j]).norm();
    int vertexes_to_add = floor(dist / d);
    Eigen::Vector3d v = (path[j + 1] - path[j]).normalized();
    // std::cout << "Vertexes to add=" << vertexes_to_add << std::endl;
    if (dist > d)
    {
      for (int i = 0; i < vertexes_to_add; i++)
      {
        path.insert(path.begin() + j + 1, path[j] + v * d);
        j = j + 1;
      }
    }
  }
}

void Faster::updateMap(pcl::PointCloud<pcl::PointXYZ>::Ptr pclptr_map, pcl::PointCloud<pcl::PointXYZ>::Ptr pclptr_unk)
{
  mtx_map.lock();
  mtx_unk.lock();

  // TODO
  pclptr_map_ = pclptr_map;
  pclptr_unk_ = pclptr_unk;

  jps_manager_.updateJPSMap(pclptr_map_, state_.pos);  // Update even where there are no points

  if (pclptr_map_->width != 0 && pclptr_map_->height != 0)  // Point Cloud is not empty
  {
    kdtree_map_.setInputCloud(pclptr_map_);
    kdtree_map_initialized_ = 1;
    jps_manager_.vec_o_ = pclptr_to_vec(pclptr_map_);
  }
  else
  {
    std::cout << "Occupancy Grid received is empty, maybe map is too small?" << std::endl;
  }

  if (pclptr_unk_->points.size() == 0)
  {
    std::cout << "Unkown cloud has 0 points" << std::endl;
    return;
  }
  else
  {
    kdtree_unk_.setInputCloud(pclptr_unk_);
    kdtree_unk_initialized_ = 1;
    jps_manager_.vec_uo_ = pclptr_to_vec(pclptr_unk_);  // insert unknown space
    jps_manager_.vec_uo_.insert(jps_manager_.vec_uo_.end(), jps_manager_.vec_o_.begin(),
                                jps_manager_.vec_o_.end());  // append known space
  }

  mtx_map.unlock();
  mtx_unk.unlock();
}

void Faster::setTerminalGoal(state& term_goal)
{
  mtx_G_term.lock();
  mtx_G.lock();
  mtx_state.lock();
  mtx_planner_status_.lock();

  G_term_.pos = term_goal.pos;
  Eigen::Vector3d temp = state_.pos;
  G_.pos = projectPointToBox(temp, G_term_.pos, par_.wdx, par_.wdy, par_.wdz);
  if (drone_status_ == DroneStatus::GOAL_REACHED)
  {
    changeDroneStatus(DroneStatus::YAWING);  // not done when drone_status==traveling
  }
  terminal_goal_initialized_ = true;

  mtx_state.unlock();
  mtx_G.unlock();
  mtx_G_term.unlock();
  mtx_planner_status_.unlock();
}

void Faster::getG(state& G)
{
  G = G_;
}

void Faster::getState(state& data)
{
  mtx_state.lock();
  data = state_;
  mtx_state.unlock();
}

int Faster::findIndexR(int indexH)
{
  // Ignore z to obtain this heuristics (if not it can become very conservative)
  // mtx_X_U_temp.lock();
  Eigen::Vector2d posHk;
  posHk << sg_whole_.X_temp_[indexH].pos(0), sg_whole_.X_temp_[indexH].pos(1);
  int indexR = indexH;

  for (int i = 0; i <= indexH; i = i + 1)  // Loop from A to H
  {
    Eigen::Vector2d vel;
    vel << sg_whole_.X_temp_[i].vel(0), sg_whole_.X_temp_[i].vel(1);  //(i, 3), sg_whole_.X_temp_(i, 4);

    Eigen::Vector2d pos;
    pos << sg_whole_.X_temp_[i].pos(0), sg_whole_.X_temp_[i].pos(1);

    Eigen::Vector2d braking_distance =
        (vel.array() * (posHk - pos).array()).sign() * vel.array().square() / (2 * par_.delta_a * par_.a_max);

    // std::cout << "braking_distance=" << braking_distance.transpose() << std::endl;
    // std::cout << "(posHk - pos).cwiseAbs().array())=" << (posHk - pos).cwiseAbs().array().transpose() << std::endl;

    bool thereWillBeCollision =
        (braking_distance.array() > (posHk - pos).cwiseAbs().array()).any();  // Any of the braking distances (in x, y,
                                                                              // z) is bigger than the distance to the
                                                                              // obstacle in that direction
    if (thereWillBeCollision)
    {
      indexR = i;

      if (indexR == 0)
      {
        std::cout << bold << red << "R was taken in A" << reset << std::endl;
      }

      break;
    }
  }
  std::cout << blue << "indexR=" << indexR << " /" << sg_whole_.X_temp_.size() - 1 << reset << std::endl;
  // std::cout << red << bold << "indexH=" << indexH << " /" << sg_whole_.X_temp_.rows() - 1 << reset << std::endl;
  // mtx_X_U_temp.unlock();

  return indexR;
}

int Faster::findIndexH(bool& needToComputeSafePath)
{
  int n = 1;  // find one neighbour
  std::vector<int> pointIdxNKNSearch(n);
  std::vector<float> pointNKNSquaredDistance(n);

  needToComputeSafePath = false;

  mtx_unk.lock();
  mtx_X_U_temp.lock();
  int indexH = sg_whole_.X_temp_.size() - 1;

  for (int i = 0; i < sg_whole_.X_temp_.size(); i = i + 10)
  {  // Sample points along the trajectory

    Eigen::Vector3d tmp = sg_whole_.X_temp_[i].pos;
    pcl::PointXYZ searchPoint(tmp(0), tmp(1), tmp(2));

    if (kdtree_unk_.nearestKSearch(searchPoint, n, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    {
      if (sqrt(pointNKNSquaredDistance[0]) < par_.drone_radius)
      {
        needToComputeSafePath = true;  // There is intersection, so there is need to compute safe path
        indexH = (int)(par_.delta_H * i);
        break;
      }
    }
  }
  std::cout << blue << "indexH=" << indexH << " /" << sg_whole_.X_temp_.size() - 1 << reset << std::endl;
  mtx_unk.unlock();
  mtx_X_U_temp.unlock();

  return indexH;
}

void Faster::updateState(state data)
{
  state_ = data;

  if (state_initialized_ == false)
  {
    state tmp;
    tmp.pos = data.pos;
    tmp.yaw = data.yaw;
    plan_.push_back(tmp);
  }

  state_initialized_ = true;
}

bool Faster::initializedAllExceptPlanner()
{
  if (!state_initialized_ || !kdtree_map_initialized_ || !kdtree_unk_initialized_ || !terminal_goal_initialized_)
  {
    std::cout << "state_initialized_= " << state_initialized_ << std::endl;
    std::cout << "kdtree_map_initialized_= " << kdtree_map_initialized_ << std::endl;
    std::cout << "kdtree_unk_initialized_= " << kdtree_unk_initialized_ << std::endl;
    std::cout << "terminal_goal_initialized_= " << terminal_goal_initialized_ << std::endl;
    return false;
  }
  return true;
}

bool Faster::initialized()
{
  if (!state_initialized_ || !kdtree_map_initialized_ || !kdtree_unk_initialized_ || !terminal_goal_initialized_ ||
      !planner_initialized_)
  {
    std::cout << "state_initialized_= " << state_initialized_ << std::endl;
    std::cout << "kdtree_map_initialized_= " << kdtree_map_initialized_ << std::endl;
    std::cout << "kdtree_unk_initialized_= " << kdtree_unk_initialized_ << std::endl;
    std::cout << "terminal_goal_initialized_= " << terminal_goal_initialized_ << std::endl;
    std::cout << "planner_initialized_= " << planner_initialized_ << std::endl;
    return false;
  }
  return true;
}

void Faster::replan(vec_Vecf<3>& JPS_safe_out, vec_Vecf<3>& JPS_whole_out, vec_E<Polyhedron<3>>& poly_safe_out,
                    vec_E<Polyhedron<3>>& poly_whole_out, std::vector<state>& X_safe_out,
                    std::vector<state>& X_whole_out)
{
  MyTimer replanCB_t(true);
  if (initializedAllExceptPlanner() == false)
  {
    return;
  }

  sg_whole_.ResetToNormalState();
  sg_safe_.ResetToNormalState();

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// G <-- Project GTerm ////////////////////////////
  //////////////////////////////////////////////////////////////////////////

  mtx_state.lock();
  mtx_G.lock();
  mtx_G_term.lock();

  state state_local = state_;
  state G;
  G.pos = projectPointToBox(state_local.pos, G_term_.pos, par_.wdx, par_.wdy, par_.wdz);
  state G_term = G_term_;  // Local copy of the terminal terminal goal

  mtx_G.unlock();
  mtx_G_term.unlock();
  mtx_state.unlock();

  // Check if we have reached the goal
  double dist_to_goal = (G_term.pos - state_local.pos).norm();
  if (dist_to_goal < par_.goal_radius)
  {
    changeDroneStatus(DroneStatus::GOAL_REACHED);
  }

  // Don't plan if drone is not traveling
  if (drone_status_ == DroneStatus::GOAL_REACHED || (drone_status_ == DroneStatus::YAWING))
  {
    std::cout << "No replanning needed because" << std::endl;
    print_status();
    return;
  }

  std::cout << bold << on_red << "************IN REPLAN CB*********" << reset << std::endl;

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Select state A /////////////////////////////////
  //////////////////////////////////////////////////////////////////////////

  state A;
  int k_safe, k_end_whole;

  // If k_end_whole=0, then A = plan_.back() = plan_[plan_.size() - 1]
  k_end_whole = std::max((int)plan_.size() - deltaT_, 0);
  A = plan_[plan_.size() - 1 - k_end_whole];

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Solve JPS //////////////////////////////////////
  //////////////////////////////////////////////////////////////////////////

  bool solvedjps = false;
  MyTimer timer_jps(true);

  vec_Vecf<3> JPSk = jps_manager_.solveJPS3D(A.pos, G.pos, &solvedjps, 1);

  if (solvedjps == false)
  {
    std::cout << bold << red << "JPS didn't find a solution" << std::endl;
    return;
  }

  //////////////////////////////////////////////////////////////////////////
  ///////////////////////// Find JPS_in ////////////////////////////////////
  //////////////////////////////////////////////////////////////////////////

  double ra = std::min((dist_to_goal - 0.001), par_.Ra);  // radius of the sphere S
  bool noPointsOutsideS;
  int li1;  // last index inside the sphere of JPSk
  state E;
  E.pos = getFirstIntersectionWithSphere(JPSk, ra, JPSk[0], &li1, &noPointsOutsideS);
  vec_Vecf<3> JPS_in(JPSk.begin(), JPSk.begin() + li1 + 1);
  if (noPointsOutsideS == false)
  {
    JPS_in.push_back(E.pos);
  }
  // createMoreVertexes in case dist between vertexes is too big
  createMoreVertexes(JPS_in, par_.dist_max_vertexes);

  //////////////////////////////////////////////////////////////////////////
  ///////////////// Solve with GUROBI Whole trajectory /////////////////////
  //////////////////////////////////////////////////////////////////////////

  if (par_.use_faster == true)
  {
    vec_Vecf<3> JPS_whole = JPS_in;
    deleteVertexes(JPS_whole, par_.max_poly_whole);
    E.pos = JPS_whole[JPS_whole.size() - 1];

    // Convex Decomp around JPS_whole
    MyTimer cvx_ellip_decomp_t(true);
    jps_manager_.cvxEllipsoidDecomp(JPS_whole, OCCUPIED_SPACE, l_constraints_whole_, poly_whole_out);
    // std::cout << "poly_whole_out= " << poly_whole_out.size() << std::endl;

    // Check if G is inside poly_whole
    bool isGinside_whole = l_constraints_whole_[l_constraints_whole_.size() - 1].inside(G.pos);
    E.pos = (isGinside_whole == true) ? G.pos : E.pos;

    // Set Initial cond, Final cond, and polytopes for the whole traj
    sg_whole_.setX0(A);
    sg_whole_.setXf(E);
    sg_whole_.setPolytopes(l_constraints_whole_);

    /*    std::cout << "Initial Position is inside= " << l_constraints_whole_[l_constraints_whole_.size() -
       1].inside(A.pos)
                  << std::endl;
        std::cout << "Final Position is inside= " << l_constraints_whole_[l_constraints_whole_.size() - 1].inside(E.pos)
                  << std::endl;
    */
    // Solve with Gurobi
    MyTimer whole_gurobi_t(true);
    bool solved_whole = sg_whole_.genNewTraj();

    if (solved_whole == false)
    {
      std::cout << bold << red << "No solution found for the whole trajectory" << reset << std::endl;
      return;
    }

    // Get Results
    sg_whole_.fillX();

    // Copy for visualization
    X_whole_out = sg_whole_.X_temp_;
    JPS_whole_out = JPS_whole;
  }
  else
  {  // Dummy whole trajectory
    state dummy;
    std::vector<state> dummy_vector;
    dummy_vector.push_back(dummy);
    sg_whole_.X_temp_ = dummy_vector;
  }

  /*  std::cout << "This is the WHOLE TRAJECTORY" << std::endl;
    printStateVector(sg_whole_.X_temp_);
    std::cout << "===========================" << std::endl;*/

  //////////////////////////////////////////////////////////////////////////
  ///////////////// Solve with GUROBI Safe trajectory /////////////////////
  //////////////////////////////////////////////////////////////////////////

  vec_Vecf<3> JPSk_inside_sphere_tmp = JPS_in;
  bool thereIsIntersection2;
  // state M;
  M_.pos = getFirstCollisionJPS(JPSk_inside_sphere_tmp, &thereIsIntersection2, UNKNOWN_MAP,
                                RETURN_INTERSECTION);  // results saved in JPSk_inside_sphere_tmp

  bool needToComputeSafePath;
  int indexH = findIndexH(needToComputeSafePath);

  std::cout << "NeedToComputeSafePath=" << needToComputeSafePath << std::endl;

  if (par_.use_faster == false)
  {
    needToComputeSafePath = true;
  }

  if (needToComputeSafePath == false)
  {
    k_safe = indexH;
    sg_safe_.X_temp_ = std::vector<state>();  // 0 elements
  }
  else
  {
    mtx_X_U_temp.lock();

    k_safe = findIndexR(indexH);
    state R = sg_whole_.X_temp_[k_safe];

    mtx_X_U_temp.unlock();

    /*    if (ARisInFreeSpace(indexR_) == false and takeoff_done_ == true)
        {
          std::cout << red << bold << "The piece A-->R is not in Free Space" << std::endl;
          return;
        }*/

    JPSk_inside_sphere_tmp[0] = R.pos;

    if (par_.use_faster == false)
    {
      JPSk_inside_sphere_tmp[0] = A.pos;
    }

    vec_Vecf<3> JPS_safe = JPSk_inside_sphere_tmp;

    // delete extra vertexes
    deleteVertexes(JPS_safe, par_.max_poly_safe);
    M_.pos = JPS_safe[JPS_safe.size() - 1];

    // compute convex decomposition of JPS_safe
    jps_manager_.cvxEllipsoidDecomp(JPS_safe, UNKOWN_AND_OCCUPIED_SPACE, l_constraints_safe_, poly_safe_out);

    JPS_safe_out = JPS_safe;

    bool isGinside = l_constraints_safe_[l_constraints_safe_.size() - 1].inside(G.pos);
    M_.pos = (isGinside == true) ? G.pos : M_.pos;

    state x0_safe;
    x0_safe = R;

    if (par_.use_faster == false)
    {
      x0_safe = stateA_;
    }

    bool shouldForceFinalConstraint_for_Safe = (par_.use_faster == false) ? true : false;

    if (l_constraints_safe_[0].inside(x0_safe.pos) == false)
    {
      std::cout << red << "First point of safe traj is outside" << reset << std::endl;
    }

    sg_safe_.setX0(x0_safe);
    sg_safe_.setXf(M_);  // only used to compute dt
    sg_safe_.setPolytopes(l_constraints_safe_);
    sg_safe_.setForceFinalConstraint(shouldForceFinalConstraint_for_Safe);
    MyTimer safe_gurobi_t(true);
    std::cout << "Calling to Gurobi" << std::endl;
    bool solved_safe = sg_safe_.genNewTraj();

    if (solved_safe == false)
    {
      std::cout << red << "No solution found for the safe path" << reset << std::endl;
      return;
    }

    // Get the solution
    sg_safe_.fillX();
    X_safe_out = sg_safe_.X_temp_;
  }

  /*  std::cout << "This is the SAFE TRAJECTORY" << std::endl;
    printStateVector(sg_safe_.X_temp_);
    std::cout << "===========================" << std::endl;*/

  ///////////////////////////////////////////////////////////
  ///////////////       Append RESULTS    ////////////////////
  ///////////////////////////////////////////////////////////

  if (appendToPlan(k_end_whole, sg_whole_.X_temp_, k_safe, sg_safe_.X_temp_) != true)
  {
    return;
  }

  /*  mtx_plan_.lock();
    std::cout << "This is the COMMITED TRAJECTORY" << std::endl;
    printStateDeque(plan_);
    std::cout << "===========================" << std::endl;
    mtx_plan_.unlock();*/

  ///////////////////////////////////////////////////////////
  ///////////////       OTHER STUFF    //////////////////////
  //////////////////////////////////////////////////////////

  // Check if we have planned until G_term
  state F = plan_.back();  // Final point of the safe path (\equiv final point of the comitted path)

  // F.print();
  double dist = (G_term_.pos - F.pos).norm();

  if (dist < par_.goal_radius)
  {
    changeDroneStatus(DroneStatus::GOAL_SEEN);
  }

  mtx_offsets.lock();

  int states_last_replan = ceil(replanCB_t.ElapsedMs() / (par_.dc * 1000));  // Number of states that
                                                                             // would have been needed for
                                                                             // the last replan
  mtx_offsets.unlock();

  // Time allocation
  double new_init_whole = std::max(sg_whole_.factor_that_worked_ - par_.gamma_whole, 1.0);
  double new_final_whole = sg_whole_.factor_that_worked_ + par_.gammap_whole;
  sg_whole_.setFactorInitialAndFinalAndIncrement(new_init_whole, new_final_whole, par_.increment_whole);

  double new_init_safe = std::max(sg_safe_.factor_that_worked_ - par_.gamma_safe, 1.0);
  double new_final_safe = sg_safe_.factor_that_worked_ + par_.gammap_safe;
  sg_safe_.setFactorInitialAndFinalAndIncrement(new_init_safe, new_final_safe, par_.increment_safe);

  planner_initialized_ = true;

  std::cout << bold << blue << "Replanning took " << replanCB_t.ElapsedMs() << " ms" << reset << std::endl;

  return;
}

void Faster::resetInitialization()
{
  planner_initialized_ = false;
  state_initialized_ = false;
  kdtree_map_initialized_ = false;
  kdtree_unk_initialized_ = false;
  terminal_goal_initialized_ = false;
}

bool Faster::appendToPlan(int k_end_whole, const std::vector<state>& whole, int k_safe, const std::vector<state>& safe)
{
  mtx_plan_.lock();

  bool output;
  int plan_size = plan_.size();
  /*  std::cout << "plan_.size()= " << plan_.size() << std::endl;
    std::cout << "plan_size - k_end_whole = " << plan_size - k_end_whole << std::endl;*/
  if ((plan_size - 1 - k_end_whole) < 0)
  {
    std::cout << bold << red << "Already publised the point A" << reset << std::endl;
    output = false;
  }
  else
  {
    /*    std::cout << "(plan_.size() - k_end_whole)= " << (plan_.size() - k_end_whole) << std::endl;
        std::cout << "plan_.size()= " << plan_.size() << std::endl;
        std::cout << "k_end_whole)= " << k_end_whole << std::endl;*/

    plan_.erase(plan_.end() - k_end_whole - 1, plan_.end());

    /*    std::cout << "Erased" << std::endl;
        std::cout << "k_safe = " << k_safe << std::endl;
        std::cout << "whole.size() = " << whole.size() << std::endl;*/
    for (int i = 0; i <= k_safe; i++)
    {
      plan_.push_back(whole[i]);
    }

    /*    std::cout << "k_safe = " << k_safe << std::endl;
        std::cout << "whole.size() = " << whole.size() << std::endl;*/
    for (int i = 0; i < safe.size(); i++)
    {
      plan_.push_back(safe[i]);
    }
    // std::cout << "Pushed everything back" << std::endl;

    output = true;
  }

  mtx_plan_.unlock();
  return output;
}

void Faster::yaw(double diff, state& next_goal)
{
  saturate(diff, -par_.dc * par_.w_max, par_.dc * par_.w_max);
  double dyaw_not_filtered;

  dyaw_not_filtered = copysign(1, diff) * par_.w_max;

  dyaw_filtered_ = (1 - par_.alpha_filter_dyaw) * dyaw_not_filtered + par_.alpha_filter_dyaw * dyaw_filtered_;
  next_goal.dyaw = dyaw_filtered_;

  // std::cout << "Before next_goal.yaw=" << next_goal.yaw << std::endl;

  next_goal.yaw = previous_yaw_ + dyaw_filtered_ * par_.dc;
  // std::cout << "After next_goal.yaw=" << next_goal.yaw << std::endl;
}

void Faster::getDesiredYaw(state& next_goal)
{
  double diff = 0.0;
  double desired_yaw = 0.0;

  switch (drone_status_)
  {
    case DroneStatus::YAWING:
      desired_yaw = atan2(G_term_.pos[1] - next_goal.pos[1], G_term_.pos[0] - next_goal.pos[0]);
      diff = desired_yaw - state_.yaw;
      // std::cout << "diff1= " << diff << std::endl;
      break;
    case DroneStatus::TRAVELING:
    case DroneStatus::GOAL_SEEN:
      desired_yaw = atan2(M_.pos[1] - next_goal.pos[1], M_.pos[0] - next_goal.pos[0]);
      diff = desired_yaw - state_.yaw;
      break;
    case DroneStatus::GOAL_REACHED:
      next_goal.dyaw = 0.0;
      next_goal.yaw = previous_yaw_;
      return;
  }

  angle_wrap(diff);
  if (fabs(diff) < 0.04 && drone_status_ == DroneStatus::YAWING)
  {
    changeDroneStatus(DroneStatus::TRAVELING);
  }
  // std::cout << "diff2= " << diff << std::endl;
  yaw(diff, next_goal);
  // std::cout << "yaw3= " << next_goal.yaw << std::endl;
}

bool Faster::getNextGoal(state& next_goal)
{
  if (initializedAllExceptPlanner() == false)
  {
    std::cout << "Not publishing new goal!!" << std::endl;
    return false;
  }

  mtx_goals.lock();
  mtx_plan_.lock();

  next_goal.setZero();
  next_goal = plan_.front();
  if (plan_.size() > 1)
  {
    plan_.pop_front();
  }
  getDesiredYaw(next_goal);

  previous_yaw_ = next_goal.yaw;

  mtx_goals.unlock();
  mtx_plan_.unlock();
  return true;
}

bool Faster::ARisInFreeSpace(int index)
{  // We have to check only against the unkown space (A-R won't intersect the obstacles for sure)

  // std::cout << "In ARisInFreeSpace, radius_drone= " << par_.drone_radius << std::endl;
  int n = 1;  // find one neighbour

  std::vector<int> pointIdxNKNSearch(n);
  std::vector<float> pointNKNSquaredDistance(n);

  bool isFree = true;

  // std::cout << "Before mtx_unk" << std::endl;
  mtx_unk.lock();
  mtx_X_U_temp.lock();
  // std::cout << "After mtx_unk. index=" << index << std::endl;
  for (int i = 0; i < index; i = i + 10)
  {  // Sample points along the trajectory
     // std::cout << "i=" << i << std::endl;
    Eigen::Vector3d tmp = sg_whole_.X_temp_[i].pos;
    pcl::PointXYZ searchPoint(tmp(0), tmp(1), tmp(2));

    if (kdtree_unk_.nearestKSearch(searchPoint, n, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    {
      if (sqrt(pointNKNSquaredDistance[0]) < 0.2)
      {  // TODO: 0.2 is the radius of the drone.
        std::cout << "A->R collides, with d=" << sqrt(pointNKNSquaredDistance[0])
                  << ", radius_drone=" << par_.drone_radius << std::endl;
        isFree = false;
        break;
      }
    }
  }

  mtx_unk.unlock();
  mtx_X_U_temp.unlock();

  return isFree;
}

// Returns the first collision of JPS with the map (i.e. with the known obstacles). Note that JPS will collide with a
// map B if JPS was computed using an older map A
// If type_return==Intersection, it returns the last point in the JPS path that is at least par_.inflation_jps from map
Eigen::Vector3d Faster::getFirstCollisionJPS(vec_Vecf<3>& path, bool* thereIsIntersection, int map, int type_return)
{
  vec_Vecf<3> original = path;

  Eigen::Vector3d first_element = path[0];
  Eigen::Vector3d last_search_point = path[0];
  Eigen::Vector3d inters = path[0];
  pcl::PointXYZ pcl_search_point = eigenPoint2pclPoint(path[0]);

  Eigen::Vector3d result;

  // occupied (map)
  int n = 1;
  std::vector<int> id_map(n);
  std::vector<float> dist2_map(n);  // squared distance
  double r = 1000000;
  // printElementsOfJPS(path);
  // printf("In 2\n");

  mtx_map.lock();
  mtx_unk.lock();

  // Find the next eig_search_point
  int last_id = -1;  // this is the last index inside the sphere
  int iteration = 0;
  while (path.size() > 0)
  {
    // std::cout<<red<<"New Iteration, iteration="<<iteration<<reset<<std::endl;
    // std::cout << red << "Searching from point=" << path[0].transpose() << reset << std::endl;
    pcl_search_point = eigenPoint2pclPoint(path[0]);

    int number_of_neigh;

    if (map == MAP)
    {
      number_of_neigh = kdtree_map_.nearestKSearch(pcl_search_point, n, id_map, dist2_map);
    }
    else  // map == UNKNOWN_MAP
    {
      number_of_neigh = kdtree_unk_.nearestKSearch(pcl_search_point, n, id_map, dist2_map);
      // std::cout << "In unknown_map, number of neig=" << number_of_neigh << std::endl;
    }
    // printf("************NearestSearch: TotalTime= %0.2f ms\n", 1000 * (ros::Time::now().toSec() - before));

    if (number_of_neigh > 0)
    {
      r = sqrt(dist2_map[0]);

      // std::cout << "r=" << r << std::endl;
      // std::cout << "Point=" << r << std::endl;

      if (r < par_.drone_radius)  // collision of the JPS path and an inflated obstacle --> take last search point
      {
        // std::cout << "Collision detected" << std::endl;  // We will return the search_point
        // pubJPSIntersection(inters);
        // inters = path[0];  // path[0] is the search_point I'm using.
        if (iteration == 0)
        {
          std::cout << red << bold << "The first point is in collision --> Hacking" << reset << std::endl;
        }
        switch (type_return)
        {
          case RETURN_LAST_VERTEX:
            result = last_search_point;
            break;
          case RETURN_INTERSECTION:
            if (iteration == 0)
            {  // Hacking (TODO)
              Eigen::Vector3d tmp;
              tmp << original[0](0) + 0.01, original[0](1), original[0](2);
              path.clear();
              path.push_back(original[0]);
              path.push_back(tmp);
              result = path[path.size() - 1];
              // result=original[original.size() - 1];
            }
            else
            {
              // std::cout << "In Return Intersection, last_id=" << last_id<<el_eliminated<< std::endl;
              int vertexes_eliminated_tmp = original.size() - path.size() + 1;
              // std::cout << "In Return Intersection, vertexes_eliminated_tmp=" << vertexes_eliminated_tmp <<
              // std::endl;
              original.erase(original.begin() + vertexes_eliminated_tmp,
                             original.end());  // Now original contains all the elements eliminated
              original.push_back(path[0]);

              /*              std::cout << "Result before reduceJPSbyDistance" << original[original.size() -
                 1].transpose()
                                      << std::endl;*/

              // This is to force the intersection point to be at least par_.drone_radius away from the obstacles
              reduceJPSbyDistance(original, par_.drone_radius);

              result = original[original.size() - 1];

              // std::cout<<"Result here is"<<result.transpose()<<std::endl;

              path = original;
            }
            // Copy the resulting path to the reference
            /*     std::reverse(original.begin(), original.end());  // flip all the vector
               result = getFirstIntersectionWithSphere(original, par_.inflation_jps, original[0]);*/
            break;
        }

        *thereIsIntersection = true;

        break;  // Leave the while loop
      }

      bool no_points_outside_sphere = false;

      inters = getFirstIntersectionWithSphere(path, r, path[0], &last_id, &no_points_outside_sphere);
      // printf("**********Found it*****************\n");
      if (no_points_outside_sphere == true)
      {  // JPS doesn't intersect with any obstacle
        *thereIsIntersection = false;
        /*        std::cout << "JPS provided doesn't intersect any obstacles, returning the first element of the path
           you gave " "me\n"
                          << std::endl;*/
        result = first_element;

        if (type_return == RETURN_INTERSECTION)
        {
          result = original[original.size() - 1];
          path = original;
        }

        break;  // Leave the while loop
      }
      // printf("In 4\n");

      last_search_point = path[0];
      // Remove all the points of the path whose id is <= to last_id:
      path.erase(path.begin(), path.begin() + last_id + 1);

      // and add the intersection as the first point of the path
      path.insert(path.begin(), inters);
    }
    else
    {  // There is no neighbours
      *thereIsIntersection = false;
      ROS_INFO("JPS provided doesn't intersect any obstacles, returning the first element of the path you gave me\n");
      result = first_element;

      if (type_return == RETURN_INTERSECTION)
      {
        result = original[original.size() - 1];
        path = original;
      }

      break;
    }
    iteration = iteration + 1;
  }
  mtx_map.unlock();
  mtx_unk.unlock();

  return result;
}

// Debugging functions
void Faster::changeDroneStatus(int new_status)
{
  if (new_status == drone_status_)
  {
    return;
  }

  std::cout << "Changing DroneStatus from ";
  switch (drone_status_)
  {
    case DroneStatus::YAWING:
      std::cout << bold << "status_=YAWING" << reset;
      break;
    case DroneStatus::TRAVELING:
      std::cout << bold << "status_=TRAVELING" << reset;
      break;
    case DroneStatus::GOAL_SEEN:
      std::cout << bold << "status_=GOAL_SEEN" << reset;
      break;
    case DroneStatus::GOAL_REACHED:
      std::cout << bold << "status_=GOAL_REACHED" << reset;
      break;
  }
  std::cout << " to ";

  switch (new_status)
  {
    case DroneStatus::YAWING:
      std::cout << bold << "status_=YAWING" << reset;
      break;
    case DroneStatus::TRAVELING:
      std::cout << bold << "status_=TRAVELING" << reset;
      break;
    case DroneStatus::GOAL_SEEN:
      std::cout << bold << "status_=GOAL_SEEN" << reset;
      break;
    case DroneStatus::GOAL_REACHED:
      std::cout << bold << "status_=GOAL_REACHED" << reset;
      break;
  }

  std::cout << std::endl;

  drone_status_ = new_status;
}

void Faster::print_status()
{
  switch (drone_status_)
  {
    case DroneStatus::YAWING:
      std::cout << bold << "status_=YAWING" << reset << std::endl;
      break;
    case DroneStatus::TRAVELING:
      std::cout << bold << "status_=TRAVELING" << reset << std::endl;
      break;
    case DroneStatus::GOAL_SEEN:
      std::cout << bold << "status_=GOAL_SEEN" << reset << std::endl;
      break;
    case DroneStatus::GOAL_REACHED:
      std::cout << bold << "status_=GOAL_REACHED" << reset << std::endl;
      break;
  }
}


#ifndef BOOST_MPL_AUX_VECTOR_ITERATOR_HPP_INCLUDED
#define BOOST_MPL_AUX_VECTOR_ITERATOR_HPP_INCLUDED

// Copyright Aleksey Gurtovoy 2000-2004
//
// 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)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#include <boost/mpl/advance_fwd.hpp>
#include <boost/mpl/aux_/config/ctps.hpp>
#include <boost/mpl/aux_/config/workaround.hpp>
#include <boost/mpl/aux_/nttp_decl.hpp>
#include <boost/mpl/aux_/value_wknd.hpp>
#include <boost/mpl/distance_fwd.hpp>
#include <boost/mpl/iterator_tags.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/vector/aux_/at.hpp>

namespace boost
{
namespace mpl
{

template <typename Vector, BOOST_MPL_AUX_NTTP_DECL(long, n_)>
struct v_iter
{
    typedef aux::v_iter_tag tag;
    typedef random_access_iterator_tag category;
    typedef typename v_at<Vector, n_>::type type;

    typedef Vector vector_;
    typedef mpl::long_<n_> pos;

#if defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)
    enum
    {
        next_ = n_ + 1,
        prior_ = n_ - 1,
        pos_ = n_
    };

    typedef v_iter<Vector, next_> next;
    typedef v_iter<Vector, prior_> prior;
#endif
};

#if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION)

template <typename Vector, BOOST_MPL_AUX_NTTP_DECL(long, n_)>
struct next<v_iter<Vector, n_>>
{
    typedef v_iter<Vector, (n_ + 1)> type;
};

template <typename Vector, BOOST_MPL_AUX_NTTP_DECL(long, n_)>
struct prior<v_iter<Vector, n_>>
{
    typedef v_iter<Vector, (n_ - 1)> type;
};

template <typename Vector, BOOST_MPL_AUX_NTTP_DECL(long, n_), typename Distance>
struct advance<v_iter<Vector, n_>, Distance>
{
    typedef v_iter<Vector,
                   (n_ + BOOST_MPL_AUX_NESTED_VALUE_WKND(long, Distance))>
        type;
};

template <typename Vector, BOOST_MPL_AUX_NTTP_DECL(long, n_),
          BOOST_MPL_AUX_NTTP_DECL(long, m_)>
struct distance<v_iter<Vector, n_>, v_iter<Vector, m_>> : mpl::long_<(m_ - n_)>
{
};

#else // BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION

template <>
struct advance_impl<aux::v_iter_tag>
{
    template <typename Iterator, typename N>
    struct apply
    {
        enum
        {
            pos_ = Iterator::pos_,
            n_ = N::value
        };
        typedef v_iter<typename Iterator::vector_, (pos_ + n_)> type;
    };
};

template <>
struct distance_impl<aux::v_iter_tag>
{
    template <typename Iter1, typename Iter2>
    struct apply
    {
        enum
        {
            pos1_ = Iter1::pos_,
            pos2_ = Iter2::pos_
        };
        typedef long_<(pos2_ - pos1_)> type;
        BOOST_STATIC_CONSTANT(long, value = (pos2_ - pos1_));
    };
};

#endif

} // namespace mpl
} // namespace boost

#endif // BOOST_MPL_AUX_VECTOR_ITERATOR_HPP_INCLUDED


//
// Copyright (c) 2018-2019 CNRS INRIA
//

#ifndef __pinocchio_joint_collection_hpp__
#define __pinocchio_joint_collection_hpp__

#include "pinocchio/multibody/joint/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"

#include <boost/variant.hpp>
#include <boost/variant/recursive_wrapper.hpp>

namespace pinocchio
{

  template<typename _Scalar, int _Options>
  struct JointCollectionDefaultTpl
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };

    // Joint Revolute
    typedef JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    typedef JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
    typedef JointModelRevoluteTpl<Scalar,Options,2> JointModelRZ;

    // Joint Revolute Unaligned
    typedef JointModelRevoluteUnalignedTpl<Scalar,Options> JointModelRevoluteUnaligned;

    // Joint Revolute UBounded
    typedef JointModelRevoluteUnboundedTpl<Scalar,Options,0> JointModelRUBX;
    typedef JointModelRevoluteUnboundedTpl<Scalar,Options,1> JointModelRUBY;
    typedef JointModelRevoluteUnboundedTpl<Scalar,Options,2> JointModelRUBZ;

    // Joint Prismatic
    typedef JointModelPrismaticTpl<Scalar,Options,0> JointModelPX;
    typedef JointModelPrismaticTpl<Scalar,Options,1> JointModelPY;
    typedef JointModelPrismaticTpl<Scalar,Options,2> JointModelPZ;

    // Joint Prismatic Unaligned
    typedef JointModelPrismaticUnalignedTpl<Scalar,Options> JointModelPrismaticUnaligned;

    // Joint Spherical
    typedef JointModelSphericalTpl<Scalar,Options> JointModelSpherical;

    // Joint Spherical ZYX
    typedef JointModelSphericalZYXTpl<Scalar,Options> JointModelSphericalZYX;

    // Joint Translation
    typedef JointModelTranslationTpl<Scalar,Options> JointModelTranslation;

    // Joint FreeFlyer
    typedef JointModelFreeFlyerTpl<Scalar,Options> JointModelFreeFlyer;

    // Joint Planar
    typedef JointModelPlanarTpl<Scalar,Options> JointModelPlanar;

    // Joint Composite
    typedef JointModelCompositeTpl<Scalar,Options,::pinocchio::JointCollectionDefaultTpl> JointModelComposite;

    typedef boost::variant<
//    JointModelVoid,
    JointModelRX, JointModelRY, JointModelRZ
    , JointModelFreeFlyer, JointModelPlanar
    , JointModelRevoluteUnaligned
    , JointModelSpherical, JointModelSphericalZYX
    , JointModelPX, JointModelPY, JointModelPZ
    , JointModelPrismaticUnaligned
    , JointModelTranslation
    , JointModelRUBX, JointModelRUBY, JointModelRUBZ
    , boost::recursive_wrapper<JointModelComposite>
    > JointModelVariant;

    // Joint Revolute
    typedef JointDataRevoluteTpl<Scalar,Options,0> JointDataRX;
    typedef JointDataRevoluteTpl<Scalar,Options,1> JointDataRY;
    typedef JointDataRevoluteTpl<Scalar,Options,2> JointDataRZ;

    // Joint Revolute Unaligned
    typedef JointDataRevoluteUnalignedTpl<Scalar,Options> JointDataRevoluteUnaligned;

    // Joint Revolute UBounded
    typedef JointDataRevoluteUnboundedTpl<Scalar,Options,0> JointDataRUBX;
    typedef JointDataRevoluteUnboundedTpl<Scalar,Options,1> JointDataRUBY;
    typedef JointDataRevoluteUnboundedTpl<Scalar,Options,2> JointDataRUBZ;

    // Joint Prismatic
    typedef JointDataPrismaticTpl<Scalar,Options,0> JointDataPX;
    typedef JointDataPrismaticTpl<Scalar,Options,1> JointDataPY;
    typedef JointDataPrismaticTpl<Scalar,Options,2> JointDataPZ;

    // Joint Prismatic Unaligned
    typedef JointDataPrismaticUnalignedTpl<Scalar,Options> JointDataPrismaticUnaligned;

    // Joint Spherical
    typedef JointDataSphericalTpl<Scalar,Options> JointDataSpherical;

    // Joint Spherical ZYX
    typedef JointDataSphericalZYXTpl<Scalar,Options> JointDataSphericalZYX;

    // Joint Translation
    typedef JointDataTranslationTpl<Scalar,Options> JointDataTranslation;

    // Joint FreeFlyer
    typedef JointDataFreeFlyerTpl<Scalar,Options> JointDataFreeFlyer;

    // Joint Planar
    typedef JointDataPlanarTpl<Scalar,Options> JointDataPlanar;

    // Joint Composite
    typedef JointDataCompositeTpl<Scalar,Options,::pinocchio::JointCollectionDefaultTpl> JointDataComposite;

    typedef boost::variant<
//    JointDataVoid
    JointDataRX, JointDataRY, JointDataRZ
    , JointDataFreeFlyer, JointDataPlanar
    , JointDataRevoluteUnaligned
    , JointDataSpherical, JointDataSphericalZYX
    , JointDataPX, JointDataPY, JointDataPZ
    , JointDataPrismaticUnaligned
    , JointDataTranslation
    , JointDataRUBX, JointDataRUBY, JointDataRUBZ
    , boost::recursive_wrapper<JointDataComposite>
    > JointDataVariant;

  };

  typedef JointCollectionDefault::JointModelVariant JointModelVariant;
  typedef JointCollectionDefault::JointDataVariant JointDataVariant;

} // namespace pinocchio

#endif // ifndef __pinocchio_joint_collection_hpp__


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

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

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

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <ored/portfolio/builders/cpicapfloor.hpp>
#include <ored/utilities/log.hpp>

#include <qle/pricingengines/cpiblackcapfloorengine.hpp>

#include <boost/make_shared.hpp>

namespace ore {
namespace data {

boost::shared_ptr<PricingEngine> CpiCapFloorEngineBuilder::engineImpl(const string& indexName) {
    Handle<ZeroInflationIndex> cpiIndex = market_->zeroInflationIndex(indexName, configuration(MarketContext::pricing));
    // QL_REQUIRE(!cpiIndex.empty(), "engineFactory error, cpi index " << indexName << " not found");
    std::string ccyCode = cpiIndex->currency().code();
    Handle<YieldTermStructure> discountCurve = market_->discountCurve(ccyCode, configuration(MarketContext::pricing));
    Handle<QuantLib::CPIVolatilitySurface> ovs =
        market_->cpiInflationCapFloorVolatilitySurface(indexName, configuration(MarketContext::pricing));
    // QL_REQUIRE(!ovs.empty(),
    //            "engineFactory error: cpi cap/floor vol surface for index " << indexName << " not found");

    return boost::make_shared<QuantExt::CPIBlackCapFloorEngine>(discountCurve, ovs);
}
} // namespace data
} // namespace ore


#include "AffineTreeVisitor.h"
#include <boost/lexical_cast.hpp>
#include "TreeInfixPrinter.h"
#include <exception>
#include "Backend.h"
#include "Logger.h"
#include "kv/affine.hpp"

using namespace std;
using namespace hydla::symbolic_expression;
using namespace boost;

#define HYDLA_LOGGER_NODE_VALUE \
  ;
//  HYDLA_LOGGER_DEBUG("node: ", node->get_node_type_name(), ", expr: ", get_infix_string(node), ", current_val: ", current_val_)

#define HYDLA_LOGGER_NODE_VISIT \
  ;
//  HYDLA_LOGGER_DEBUG("visit node: ", node->get_node_type_name(), ", expr: ", get_infix_string(node))

namespace hydla {
namespace interval {

/// share constants to take advantage of dependency
itvd AffineTreeVisitor::pi = kv::constants<kv::interval<double> >::pi();
itvd AffineTreeVisitor::e = kv::constants<kv::interval<double> >::e();

class ApproximateException:public std::runtime_error{
public:
  ApproximateException(const std::string& msg):
    std::runtime_error("error occurred in approximation: " + msg){}
};

AffineTreeVisitor::AffineTreeVisitor(parameter_idx_map_t &map, variable_map_t &vm):parameter_idx_map_(&map), variable_map(vm)
{}

AffineTreeVisitor::AffineTreeVisitor(variable_map_t &vm): variable_map(vm), pm_external(true)
{
  parameter_idx_map_ = new parameter_idx_map_t();
  pm_external = true;
}

AffineTreeVisitor::~AffineTreeVisitor()
{
  if(pm_external)delete parameter_idx_map_;
}

void AffineTreeVisitor::set_current_time(itvd itv)
{
  ++time_idx;
  current_time = itv;
}

AffineMixedValue AffineTreeVisitor::approximate(const node_sptr &node)
{
  differential_count = 0;
  accept(node);
  return current_val_;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Plus> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  accept(node->get_lhs());
  AffineMixedValue lhs = current_val_;
  accept(node->get_rhs());
  AffineMixedValue rhs = current_val_;
  current_val_ = lhs + rhs;
  HYDLA_LOGGER_NODE_VALUE;
  return;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Subtract> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  accept(node->get_lhs());
  AffineMixedValue lhs = current_val_;
  accept(node->get_rhs());
  AffineMixedValue rhs = current_val_;
  current_val_ = lhs - rhs;
  HYDLA_LOGGER_NODE_VALUE;
  return;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Times> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  accept(node->get_lhs());
  AffineMixedValue lhs = current_val_;
  accept(node->get_rhs());
  AffineMixedValue rhs = current_val_;
  current_val_ = lhs * rhs;
  HYDLA_LOGGER_NODE_VALUE;
  return;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Divide> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  accept(node->get_lhs());
  AffineMixedValue lhs = current_val_;
  accept(node->get_rhs());
  AffineMixedValue rhs = current_val_;
  current_val_ = lhs / rhs;
  HYDLA_LOGGER_NODE_VALUE;
  return;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Power> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  accept(node->get_lhs());
  AffineMixedValue lhs = current_val_;
  // TODO: 文字列以外で判定する
  std::string rhs_str = get_infix_string(node->get_rhs());
  if(rhs_str == "1/2")
  {
    if(lhs.type == INTEGER)current_val_ = sqrt(itvd(lhs.integer));
    else if(lhs.type == INTERVAL)current_val_ = AffineMixedValue(sqrt(lhs.interval));
    else current_val_ = sqrt(lhs.affine_value);
  }
  else if(rhs_str == "(-1)/2" || rhs_str == "-1/2")
  {
    if(lhs.type == INTEGER)current_val_ = 1/sqrt(itvd(lhs.integer));
    else if(lhs.type == INTERVAL)current_val_ = 1/sqrt(lhs.interval);
    else current_val_ = 1/sqrt(lhs.affine_value);
  }
  else if(rhs_str == "2" && lhs.type == AFFINE)
  {
    current_val_ = square(lhs.affine_value);
  }
  else
  {
    accept(node->get_rhs());
    AffineMixedValue rhs = current_val_;
    current_val_ = lhs ^ rhs;
  }
  HYDLA_LOGGER_NODE_VALUE;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Negative> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  accept(node->get_child());
  current_val_ = -current_val_;
  HYDLA_LOGGER_NODE_VALUE;
  return;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Positive> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  accept(node->get_child());
  HYDLA_LOGGER_NODE_VALUE;
  return;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Pi> node)
{
  current_val_.interval = pi;
  current_val_.type = INTERVAL;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::E> node)
{
  current_val_ = AffineMixedValue(e);
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Number> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  std::string number_str = node->get_number();

  // try translation to int
  try{
    int integer = lexical_cast<int>(number_str);
    current_val_ = AffineMixedValue(integer);
    HYDLA_LOGGER_NODE_VALUE;
    return;
  }catch(const bad_lexical_cast &){
  }

  //try approximation as double with upper rounding
  kv::interval<double> itv = kv::interval<double>(number_str);
  current_val_ = AffineMixedValue(itv);
  HYDLA_LOGGER_NODE_VALUE;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Float> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  current_val_ = AffineMixedValue(itvd(node->get_number()));
  HYDLA_LOGGER_NODE_VALUE;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Function> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  std::string name = node->get_name();
  if(name == "log")
  {
    if(node->get_arguments_size() != 1)invalid_node(*node);
    accept(node->get_argument(0) );
    if(current_val_.type == INTEGER)current_val_.interval = log(itvd(current_val_.integer));
    else if(current_val_.type == INTERVAL)current_val_.interval = log(current_val_.interval);
    else current_val_.affine_value = log(current_val_.affine_value);
  }
  else if(name == "sin")
  {
    if(node->get_arguments_size() != 1)invalid_node(*node);
    accept(node->get_argument(0) );
    if(current_val_.type == INTEGER)current_val_.interval = sin(itvd(current_val_.integer));
    if(current_val_.type == INTERVAL)current_val_.interval = sin(current_val_.interval);
    else current_val_.affine_value = sin(current_val_.affine_value);
  }
  else if(name == "cos")
  {
    if(node->get_arguments_size() != 1)invalid_node(*node);
    accept(node->get_argument(0) );
    if(current_val_.type == INTEGER)current_val_.interval = cos(itvd(current_val_.integer));
    if(current_val_.type == INTERVAL)current_val_.interval = cos(current_val_.interval);
    else current_val_.affine_value = cos(current_val_.affine_value);
  }
  else if(name == "sinh")
  {
    if(node->get_arguments_size() != 1)invalid_node(*node);
    accept(node->get_argument(0) );
    if(current_val_.type == INTEGER)current_val_.interval = sinh(itvd(current_val_.integer));
    if(current_val_.type == INTERVAL)current_val_.interval = sinh(current_val_.interval);
    else current_val_.affine_value = sinh(current_val_.affine_value);
  }
  else if(name == "cosh")
  {
    if(node->get_arguments_size() != 1)invalid_node(*node);
    accept(node->get_argument(0) );
    if(current_val_.type == INTEGER)current_val_.interval = cosh(itvd(current_val_.integer));
    if(current_val_.type == INTERVAL)current_val_.interval = cosh(current_val_.interval);
    else current_val_.affine_value = cosh(current_val_.affine_value);
  }
  else
  {
    invalid_node(*node);
  }
  HYDLA_LOGGER_NODE_VALUE;
}

void AffineTreeVisitor::visit(boost::shared_ptr<hydla::symbolic_expression::Parameter> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  current_val_ = affine_t();
  parameter_t param(node->get_name(),
                    node->get_differential_count(),
                    node->get_phase_id());
  parameter_idx_map_t::left_iterator it = parameter_idx_map_->left.find(param);
  int idx;
  if(it == parameter_idx_map_->left.end())
  {
    idx = ++affine_t::maxnum();
    parameter_idx_map_->insert(
      parameter_idx_t(param, affine_t::maxnum()));
  }
  else
  {
    idx = it->second;
  }
  current_val_.affine_value.a.resize(idx + 1);
  current_val_.affine_value.a(idx) = 1;
  current_val_.affine_value.er = 0;
  HYDLA_LOGGER_NODE_VALUE;
  return;
}

void AffineTreeVisitor::visit(boost::shared_ptr<symbolic_expression::Variable> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  simulator::Variable variable(node->get_name(), differential_count);
  if(variable_map.find(variable) == variable_map.end())throw ApproximateException("unknown variable: " + variable.get_string() );
  if(!variable_map[variable].unique())throw ApproximateException("the value of a variable must be unique: "+ variable.get_string());
  accept(variable_map[variable].get_unique_value().get_node());
  HYDLA_LOGGER_NODE_VALUE;
  return;
}

void AffineTreeVisitor::visit(boost::shared_ptr<Differential> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  differential_count++;
  // TODO: 変数以外の微分値は扱えないので、その判定もしたい。
  accept(node->get_child());
  differential_count--;
  return;
}

void AffineTreeVisitor::visit(boost::shared_ptr<SymbolicT> node)
{
  HYDLA_LOGGER_NODE_VISIT;
  current_val_ = AffineMixedValue(affine_t());
  parameter_t param("t",
                    -1,
                    time_idx);
  parameter_idx_map_t::left_iterator it = parameter_idx_map_->left.find(param);
  int idx;
  if(it == parameter_idx_map_->left.end())
  {
    idx = ++affine_t::maxnum();
    parameter_idx_map_->insert(
      parameter_idx_t(param, affine_t::maxnum()));
  }
  else
  {
    idx = it->second;
  }
  current_val_.affine_value = affine_t(current_time);
  HYDLA_LOGGER_NODE_VALUE;
  return;
}

void AffineTreeVisitor::invalid_node(symbolic_expression::Node& node)
{
  throw ApproximateException("invalid node: " + node.get_string());
}

#define DEFINE_INVALID_NODE(NODE_NAME)                           \
void AffineTreeVisitor::visit(boost::shared_ptr<NODE_NAME> node) \
{                                                                \
  HYDLA_LOGGER_DEBUG("");                                        \
  invalid_node(*node);                                           \
}

DEFINE_INVALID_NODE(ConstraintDefinition)
DEFINE_INVALID_NODE(ProgramDefinition)
DEFINE_INVALID_NODE(ConstraintCaller)
DEFINE_INVALID_NODE(ProgramCaller)
DEFINE_INVALID_NODE(Constraint)
DEFINE_INVALID_NODE(Ask)
DEFINE_INVALID_NODE(Tell)

DEFINE_INVALID_NODE(Equal)
DEFINE_INVALID_NODE(UnEqual)

DEFINE_INVALID_NODE(Less)
DEFINE_INVALID_NODE(LessEqual)

DEFINE_INVALID_NODE(Greater)
DEFINE_INVALID_NODE(GreaterEqual)

DEFINE_INVALID_NODE(LogicalAnd)
DEFINE_INVALID_NODE(LogicalOr)

DEFINE_INVALID_NODE(Weaker)
DEFINE_INVALID_NODE(Parallel)

DEFINE_INVALID_NODE(Always)

DEFINE_INVALID_NODE(Previous)

DEFINE_INVALID_NODE(Print)
DEFINE_INVALID_NODE(PrintPP)
DEFINE_INVALID_NODE(PrintIP)
DEFINE_INVALID_NODE(Scan)
DEFINE_INVALID_NODE(Exit)
DEFINE_INVALID_NODE(Abort)
DEFINE_INVALID_NODE(SVtimer)

DEFINE_INVALID_NODE(Not)

DEFINE_INVALID_NODE(UnsupportedFunction)

DEFINE_INVALID_NODE(ImaginaryUnit)
DEFINE_INVALID_NODE(Infinity)
DEFINE_INVALID_NODE(True)
DEFINE_INVALID_NODE(False)

DEFINE_INVALID_NODE(ProgramList)
DEFINE_INVALID_NODE(ConditionalProgramList)
DEFINE_INVALID_NODE(ExpressionList)
DEFINE_INVALID_NODE(ConditionalExpressionList)
DEFINE_INVALID_NODE(EachElement)
DEFINE_INVALID_NODE(DifferentVariable)
DEFINE_INVALID_NODE(ExpressionListElement)
DEFINE_INVALID_NODE(ExpressionListCaller)
DEFINE_INVALID_NODE(ExpressionListDefinition)
DEFINE_INVALID_NODE(ProgramListElement)
DEFINE_INVALID_NODE(ProgramListCaller)
DEFINE_INVALID_NODE(ProgramListDefinition)
DEFINE_INVALID_NODE(Range)
DEFINE_INVALID_NODE(Union)
DEFINE_INVALID_NODE(Intersection)
DEFINE_INVALID_NODE(SumOfList)
DEFINE_INVALID_NODE(MulOfList)
DEFINE_INVALID_NODE(SizeOfList)

}
}


// Copyright 2015, Sensics, Inc.
// Copyright 2021, Collabora, Ltd.
// SPDX-License-Identifier: Apache-2.0
//
// latency-testing hardware firmware source code.
// Code for "motion turn-around" test.
//
// Original author: Russell Taylor working for Sensics.com through ReliaSolve.com
// Ported to PlatformIO and multi-board, multi-app framework by Ryan Pavlik for Collabora.

#ifdef APP_TURNAROUND
// Must come before Arduino because of abs
#include <Eigen/Core>
#include "apps.h"

using Eigen::Array3f;
using Eigen::Vector3f;

#include <Arduino.h>
#include "defines.h"
#include "gyroProc.h"

#include "motionShared.h"

GyroProc gyroProc{};

// Debugging definitions.
#undef VERBOSE
#undef VERBOSE2
#define PRINT_TRACE

#undef abs

// Thresholds
const int ACCEL_CHANGE_THRESHOLD = 500;
const float GYRO_MIN_SPEED_THRESHOLD = 0.75f;
const int BRIGHTNESS_THRESHOLD = 5;
const float GYRO_CALIBRATION_THRESHOLD = 4.0f;
const int BRIGHTNESS_CALIBRATION_THRESHOLD = 7;
const unsigned long TIMEOUT_USEC = 2000000L;
const unsigned long CALIBRATE_USEC = 1000000L;

// Arrays to hold measurements surrounding the reversal for debugging
#ifdef PRINT_TRACE
const int TRACE_SIZE = 250;
const int TRACE_SKIP = 5;
static int trace_count = 0;
static int trace_skip_count = 0;
float trace_gyroY[TRACE_SIZE];
unsigned char trace_bright[TRACE_SIZE];
#endif

// Determine if we are moving in the positive direction above
// threshold (1), in the negative direction below threshold (-1),
// or within threshold of the origin (0);
//*****************************************************
inline int gyro_direction(int value)
//*****************************************************
{
  if (value >= GYRO_THRESHOLD) {
    return 1;
  }

  if (value <= -GYRO_THRESHOLD) {
    return -1;
  }

  return 0;
}

// Determine if we brightness changed in the positive direction above
// threshold (1), in the negative direction below threshold (-1),
// or not (0);
//*****************************************************
inline int brightness_direction(int value)
//*****************************************************
{
  if (value >= BRIGHTNESS_THRESHOLD) {
    return 1;
  }

  if (value <= -BRIGHTNESS_THRESHOLD) {
    return -1;
  }

  return 0;
}

//*****************************************************
void turnaroundSetup()
//*****************************************************
{
    Serial.println("latency_hardware_firmware turnaround test v02.00.00");
    Serial.println(" Mount the photosensor rigidly on the eyepiece or screen.");
    Serial.println(" Rotate the inertial sensor along with the tracking hardware.");
    Serial.println(" Make the app vary brightness darker in one direction");
    Serial.println(" and lighter in the other.");
    Serial.println(" Latencies reported in microseconds, 2-second timeout");
    Serial.println(" Hold the device still for 2 seconds.");

    delay(100);
}

//*****************************************************
void turnaroundLoop(Board &board)
//*****************************************************
{
    startupImu(board, gyroProc);

    // Read the values from the inertial sensors and photosensor.
    // Record the time we read these values.
    auto data = doRead(board, gyroProc);
    if (!data.dataGood)
    {
        return;
    }
    int brightness = readBrightness();
    unsigned long now = data.timestamp;

#ifdef VERBOSE2
    /* Debugging info to help figure out what the thresholds should be */
    Serial.print(data.gyro.x());
    Serial.print(" ");
    Serial.print(data.gyro.y());
    Serial.print(" ");
    Serial.print(data.gyro.z());
    Serial.print(", ");
    Serial.println(brightness);
    delay(1000);
#endif

    // Keep track of the reversals in brightness and the time at
    // which the brightness reached within threshold of the value
    // it held before the inverted jump.  We keep track of the
    // previous change away from threshold and check it against
    // a current change to see if the polarities are in the opposite
    // direction.  If so, then we set last_brightness_reach_end_time to
    // now.  Brightness change direction and last brightness to
    // reach the end should both be set to 0 when entering the
    // S_REVERSE_BRIGHTNESS state so that they will have to be
    // filled in by actual changes.
    static int last_brightness_change_direction = 0;
    static unsigned long int last_brightness_reach_end_time = 0;

    // Keep track of when the last time the gyroscope's motion
    // dropped below threshold.  Used by the brightness state to
    // determine latency.  Set to 0 when entering the S_REVERSE_DIRECTION
    // state.
    static unsigned long last_gyroscope_settling_time = 0;

    // We run a finite-state machine that cycles through cases of waiting for
    // a period of non motion, determining the axis and brightness direction
    // associated with motion, looking for extrema of motion, looking for
    // extrema of brightness, and reporting statistics.
    static enum { S_CALM,
                  S_CALIBRATE,
                  S_REVERSE_DIRECTION,
                  S_REVERSE_BRIGHTNESS } state = S_CALM;
    static unsigned long calm_start = now;
    static unsigned long calibration_start = now;

    // Which axis to use for tracking the motion?  Initially set to
    // -1 and then set in the calibration code to the axis with the
    // largest motion.
    static int tracking_axis_index = -1;

    // Statistics of latencies.
    static int latency_count = 0;
    static unsigned long latency_sum = 0;
    static unsigned long latency_max = 0;
    static unsigned long latency_min = TIMEOUT_USEC;

    // Whether or not we're in calm mode, if we hold still for the
    // timeout duration, we reset statistics and go into calibrate
    // mode.
    if (moving(data.gyro))
    {
        calm_start = now;
    }
    else if (now - calm_start >= TIMEOUT_USEC)
    {
        if (latency_count > 0)
        {
            Serial.print("Min latency: ");
            Serial.println(latency_min);
            Serial.print("Max latency: ");
            Serial.println(latency_max);
            Serial.print("Mean latency: ");
            Serial.println(latency_sum / latency_count);
        }
        Serial.println("Statistics reset.  Initiate periodic motion to calibrate.");

        latency_count = 0;
        latency_sum = 0;
        latency_max = 0;
        latency_min = TIMEOUT_USEC;

        calm_start = now;
        calibration_start = now;
        tracking_axis_index = -1;
        state = S_CALIBRATE;
    }

#ifdef PRINT_TRACE
    if ((state != S_CALIBRATE) && (state != S_CALM))
    {
        if (++trace_skip_count >= TRACE_SKIP)
        {
            trace_skip_count = 0;
            if (trace_count < TRACE_SIZE)
            {
                trace_gyroY[trace_count] = data.gyro.y();
                trace_bright[trace_count] = brightness / 4;
                trace_count++;
            }
        }
    }
#endif

    switch (state)
    {
    // Wait for a period where there is no motion above
    // the motion threshold.
    case S_CALM:
    {
        // Nothing extra to do; we're always checking for calm above.
    }
    break;

    // Determine which of the rotational axes is moving the most and
    // then wait for the start of a move in the direction that makes
    // the photosensor brighter.
    case S_CALIBRATE:
    {
        // The first loop iteration, we reset our variables.
        static Array3f minGyro, maxGyro;
        static int minBright, maxBright;
        if (calibration_start == now)
        {
            minGyro = data.gyro.array();
            maxGyro = data.gyro.array();
            minBright = maxBright = brightness;
        }

        // Keep track of the maximum and minimum in all 3 axes;
        minGyro = minGyro.min(data.gyro.array());
        maxGyro = maxGyro.max(data.gyro.array());
        if (brightness < minBright)
        {
            minBright = brightness;
        }
        if (brightness > maxBright)
        {
            maxBright = brightness;
        }

        // If it has been long enough, and the tracking axis has not yet been
        // set, set it to the one that has the largest range.
        // First, check to make sure we've seen sufficient change in the
        // maximum axis and in the brightness.
        if ((tracking_axis_index < 0) && (now - calibration_start >= CALIBRATE_USEC))
        {
            Array3f range = maxGyro - minGyro;
            size_t index = 0;
            float maxRange = range.maxCoeff(&index);
            tracking_axis_index = index;
            if ((maxRange < GYRO_CALIBRATION_THRESHOLD) ||
                (maxBright - minBright < BRIGHTNESS_CALIBRATION_THRESHOLD))
            {
                Serial.println("Insufficient change for calibrating, retrying");
                Serial.print("  Gyroscope difference = ");
                Serial.print(maxRange);
                Serial.println("");
                Serial.print("  Brightness difference = ");
                Serial.println(maxBright - minBright);
                tracking_axis_index = -1;
                calibration_start = now;
            }
            else
            {
                // We have not yet dropped below motion threshold
                // for this iteration.
                last_gyroscope_settling_time = 0;
                state = S_REVERSE_DIRECTION;
                Serial.print("Calibration complete, measuring latencies on ");
                constexpr const char * axis[] = {"X axis", "Y axis", "Z axis"};
                Serial.print(axis[tracking_axis_index]);
                Serial.print(" (brightness difference ");
                Serial.print(maxBright - minBright);
                Serial.println(")");
                Serial.println("Continue periodic motion to test latency.");
            }
        }
    }
    break;

    // Wait until the motion settles down (so that we were moving
    // but now have stopped.
    case S_REVERSE_DIRECTION:
    {
        // Don't say that we were moving until we get to a speed that
        // is above a larger threshold to keep us from getting lots
        // of spurious false stopping estimates.
        static bool have_moved_fast = false;

        // Keep track of when the gyroscope value most recently
        // dropped below threshold (was above threshold in the previous
        // time step and dropped below threshold this time).
        static float last_gyroscope_value = 0;
        if (tracking_axis_index >= 0)
        {
            float tracking_axis = data.gyro[tracking_axis_index];
            if ((tracking_axis > GYRO_MIN_SPEED_THRESHOLD) ||
                (tracking_axis < -GYRO_MIN_SPEED_THRESHOLD))
            {
                have_moved_fast = true;
            }
            if (have_moved_fast &&
                (gyro_direction(tracking_axis) == 0) &&
                (gyro_direction(last_gyroscope_value) != 0))
            {
                last_gyroscope_settling_time = now;
            }
            last_gyroscope_value = tracking_axis;
        }
        else
        {
            last_gyroscope_value = 0;
        }

        if (last_gyroscope_settling_time != 0)
        {
            //Serial.println("Stopped");

            // We have dropped to a stop, so we need to set the trigger for
            // moving fast again and make sure we actually move before the
            // last gyro value registers a move.
            have_moved_fast = false;
            last_gyroscope_value = 0;

            // Flush the change status so that we have to get a brightness change
            // in one direction followed by a change in the other starting now.
            last_brightness_reach_end_time = 0;
            last_brightness_change_direction = 0;
            state = S_REVERSE_BRIGHTNESS;
        }
    }
    break;

    // Wait until the brightness changes direction and then report
    // the time difference between when motion dropped below
    // threshold and when brightness came within threshold of its
    // extreme value before reversing direction.
    case S_REVERSE_BRIGHTNESS:
    {
        static int last_unchanged_brightness_value = 0;
        static unsigned long last_brightness_change_time = 0;
        int this_change = brightness_direction(brightness - last_unchanged_brightness_value);
        if (this_change != 0)
        {
            if (this_change * last_brightness_change_direction == -1)
            {
                last_brightness_reach_end_time = last_brightness_change_time;
                /*
            if (this_change == -1) {
              Serial.print(" +");
            } else {
              Serial.print(" -");
            }
            Serial.print(last_unchanged_brightness_value);
            Serial.print("->");
            Serial.println(brightness);
            */
            }
            last_unchanged_brightness_value = brightness;
            last_brightness_change_direction = this_change;
            last_brightness_change_time = now;
        }

        if (last_brightness_reach_end_time != 0)
        {
            if (last_brightness_reach_end_time <= last_gyroscope_settling_time)
            {
                Serial.print("Error: Inverted settling times: gyro settling time ");
                Serial.print(last_gyroscope_settling_time);
                Serial.println(" (resetting)");
                state = S_CALM;
            }

#ifdef PRINT_TRACE
            Serial.println("GyroY/100*100,brightness/4*4");
            for (int i = 0; i < trace_count; i++)
            {
                Serial.print(static_cast<int>(trace_gyroY[i]) * 10);
                Serial.print(",");
                Serial.println(static_cast<int>(trace_bright[i]) * 4);
                Serial.println("");
            }
            Serial.println();
            trace_count = 0;
#endif
            unsigned long value = last_brightness_reach_end_time - last_gyroscope_settling_time;
            Serial.println(value);

            // Track statistics;
            latency_count++;
            if (value < latency_min)
            {
                latency_min = value;
            }
            if (value > latency_max)
            {
                latency_max = value;
            }
            latency_sum += value;

            last_gyroscope_settling_time = 0;
            state = S_REVERSE_DIRECTION;
        }
    }
    break;

    default:
        Serial.println("Error: Unrecognized state; restarting");
        state = S_CALM;
        break;
    }
}

#endif


#ifndef STAN_OPTIMIZATION_BFGS_HPP
#define STAN_OPTIMIZATION_BFGS_HPP

#include <stan/math/prim/mat.hpp>
#include <stan/model/log_prob_propto.hpp>
#include <stan/model/log_prob_grad.hpp>
#include <stan/optimization/bfgs_linesearch.hpp>
#include <stan/optimization/bfgs_update.hpp>
#include <stan/optimization/lbfgs_update.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <limits>
#include <string>
#include <vector>

namespace stan {
  namespace optimization {
    typedef enum {
      TERM_SUCCESS = 0,
      TERM_ABSX = 10,
      TERM_ABSF = 20,
      TERM_RELF = 21,
      TERM_ABSGRAD = 30,
      TERM_RELGRAD = 31,
      TERM_MAXIT = 40,
      TERM_LSFAIL = -1
    } TerminationCondition;

    template<typename Scalar = double>
    class ConvergenceOptions {
    public:
      ConvergenceOptions() {
        maxIts = 10000;
        fScale = 1.0;

        tolAbsX = 1e-8;
        tolAbsF = 1e-12;
        tolAbsGrad = 1e-8;

        tolRelF = 1e+4;
        tolRelGrad = 1e+3;
      }
      size_t maxIts;
      Scalar tolAbsX;
      Scalar tolAbsF;
      Scalar tolRelF;
      Scalar fScale;
      Scalar tolAbsGrad;
      Scalar tolRelGrad;
    };

    template<typename Scalar = double>
    class LSOptions {
    public:
      LSOptions() {
        c1 = 1e-4;
        c2 = 0.9;
        alpha0 = 1e-3;
        minAlpha = 1e-12;
        maxLSIts = 20;
        maxLSRestarts = 10;
      }
      Scalar c1;
      Scalar c2;
      Scalar alpha0;
      Scalar minAlpha;
      Scalar maxLSIts;
      Scalar maxLSRestarts;
    };
    template<typename FunctorType, typename QNUpdateType,
             typename Scalar = double, int DimAtCompile = Eigen::Dynamic>
    class BFGSMinimizer {
    public:
      typedef Eigen::Matrix<Scalar, DimAtCompile, 1> VectorT;
      typedef Eigen::Matrix<Scalar, DimAtCompile, DimAtCompile> HessianT;

    protected:
      FunctorType &_func;
      VectorT _gk, _gk_1, _xk_1, _xk, _pk, _pk_1;
      Scalar _fk, _fk_1, _alphak_1;
      Scalar _alpha, _alpha0;
      size_t _itNum;
      std::string _note;
      QNUpdateType _qn;

    public:
      LSOptions<Scalar> _ls_opts;
      ConvergenceOptions<Scalar> _conv_opts;

      QNUpdateType &get_qnupdate() { return _qn; }
      const QNUpdateType &get_qnupdate() const { return _qn; }

      const Scalar &curr_f() const { return _fk; }
      const VectorT &curr_x() const { return _xk; }
      const VectorT &curr_g() const { return _gk; }
      const VectorT &curr_p() const { return _pk; }

      const Scalar &prev_f() const { return _fk_1; }
      const VectorT &prev_x() const { return _xk_1; }
      const VectorT &prev_g() const { return _gk_1; }
      const VectorT &prev_p() const { return _pk_1; }
      Scalar prev_step_size() const { return _pk_1.norm()*_alphak_1; }

      inline Scalar rel_grad_norm() const {
        return -_pk.dot(_gk) / std::max(std::fabs(_fk), _conv_opts.fScale);
      }
      inline Scalar rel_obj_decrease() const {
        return std::fabs(_fk_1 - _fk) / std::max(std::fabs(_fk_1),
                                                 std::max(std::fabs(_fk),
                                                          _conv_opts.fScale));
      }

      const Scalar &alpha0() const { return _alpha0; }
      const Scalar &alpha() const { return _alpha; }
      const size_t iter_num() const { return _itNum; }

      const std::string &note() const { return _note; }

      std::string get_code_string(int retCode) {
        switch (retCode) {
          case TERM_SUCCESS:
            return std::string("Successful step completed");
          case TERM_ABSF:
            return std::string("Convergence detected: absolute change "
                               "in objective function was below tolerance");
          case TERM_RELF:
            return std::string("Convergence detected: relative change "
                               "in objective function was below tolerance");
          case TERM_ABSGRAD:
            return std::string("Convergence detected: "
                               "gradient norm is below tolerance");
          case TERM_RELGRAD:
            return std::string("Convergence detected: relative "
                               "gradient magnitude is below tolerance");
          case TERM_ABSX:
            return std::string("Convergence detected: "
                               "absolute parameter change was below tolerance");
          case TERM_MAXIT:
            return std::string("Maximum number of iterations hit, "
                               "may not be at an optima");
          case TERM_LSFAIL:
            return std::string("Line search failed to achieve a sufficient "
                               "decrease, no more progress can be made");
          default:
            return std::string("Unknown termination code");
        }
      }

      explicit BFGSMinimizer(FunctorType &f) : _func(f) { }

      void initialize(const VectorT &x0) {
        int ret;
        _xk = x0;
        ret = _func(_xk, _fk, _gk);
        if (ret) {
          throw std::runtime_error("Error evaluating initial BFGS point.");
        }
        _pk = -_gk;

        _itNum = 0;
        _note = "";
      }

      int step() {
        Scalar gradNorm, stepNorm;
        VectorT sk, yk;
        int retCode(0);
        int resetB(0);

        _itNum++;

        if (_itNum == 1) {
          resetB = 1;
          _note = "";
        } else {
          resetB = 0;
          _note = "";
        }

        while (true) {
          if (resetB) {
            // Reset the Hessian approximation
            _pk.noalias() = -_gk;
          }

          // Get an initial guess for the step size (alpha)
          if (_itNum > 1 && resetB != 2) {
            // use cubic interpolation based on the previous step
            _alpha0 = _alpha = std::min(1.0,
                                        1.01*CubicInterp(_gk_1.dot(_pk_1),
                                                         _alphak_1,
                                                         _fk - _fk_1,
                                                         _gk.dot(_pk_1),
                                                         _ls_opts.minAlpha,
                                                         1.0));
          } else {
            // On the first step (or, after a reset) use the default step size
            _alpha0 = _alpha = _ls_opts.alpha0;
          }

          // Perform the line search.  If successful, the results are in the
          // variables: _xk_1, _fk_1 and _gk_1.
          retCode = WolfeLineSearch(_func, _alpha, _xk_1, _fk_1, _gk_1,
                                    _pk, _xk, _fk, _gk,
                                    _ls_opts.c1, _ls_opts.c2,
                                    _ls_opts.minAlpha,
                                    _ls_opts.maxLSIts,
                                    _ls_opts.maxLSRestarts);
          if (retCode) {
            // Line search failed...
            if (resetB) {
              // did a Hessian reset and it still failed,
              // and nothing left to try
              retCode = TERM_LSFAIL;
              return retCode;
            } else {
              // try resetting the Hessian approximation
              resetB = 2;
              _note += "LS failed, Hessian reset";
              continue;
            }
          } else {
            break;
          }
        }

        // Swap things so that k is the most recent iterate
        std::swap(_fk, _fk_1);
        _xk.swap(_xk_1);
        _gk.swap(_gk_1);
        _pk.swap(_pk_1);

        sk.noalias() = _xk - _xk_1;
        yk.noalias() = _gk - _gk_1;

        gradNorm = _gk.norm();
        stepNorm = sk.norm();

        // Update QN approximation
        if (resetB) {
          // If the QN approximation was reset, automatically scale it
          // and update the step-size accordingly
          Scalar B0fact = _qn.update(yk, sk, true);
          _pk_1 /= B0fact;
          _alphak_1 = _alpha*B0fact;
        } else {
          _qn.update(yk, sk);
          _alphak_1 = _alpha;
        }
        // Compute search direction for next step
        _qn.search_direction(_pk, _gk);

        // Check for convergence
        if (std::fabs(_fk_1 - _fk) < _conv_opts.tolAbsF) {
          // Objective function improvement wasn't sufficient
          retCode = TERM_ABSF;
        } else if (gradNorm < _conv_opts.tolAbsGrad) {
          retCode = TERM_ABSGRAD;  // Gradient norm was below threshold
        } else if (stepNorm < _conv_opts.tolAbsX) {
          retCode = TERM_ABSX;  // Change in x was too small
        } else if (_itNum >= _conv_opts.maxIts) {
          retCode = TERM_MAXIT;  // Max number of iterations hit
        } else if (rel_obj_decrease()
                 < _conv_opts.tolRelF
                 * std::numeric_limits<Scalar>::epsilon()) {
          // Relative improvement in objective function wasn't sufficient
          retCode = TERM_RELF;
        } else if (rel_grad_norm()
                   < _conv_opts.tolRelGrad
                   * std::numeric_limits<Scalar>::epsilon()) {
          // Relative gradient norm was below threshold
          retCode = TERM_RELGRAD;
        } else {
          // Step was successful more progress to be made
          retCode = TERM_SUCCESS;
        }

        return retCode;
      }

      int minimize(VectorT &x0) {
        int retcode;
        initialize(x0);
        while (!(retcode = step()))
          continue;
        x0 = _xk;
        return retcode;
      }
    };

    template <class M>
    class ModelAdaptor {
    private:
      M& _model;
      std::vector<int> _params_i;
      std::ostream* _msgs;
      std::vector<double> _x, _g;
      size_t _fevals;

    public:
      ModelAdaptor(M& model,
                   const std::vector<int>& params_i,
                   std::ostream* msgs)
      : _model(model), _params_i(params_i), _msgs(msgs), _fevals(0) {}

      size_t fevals() const { return _fevals; }
      int operator()(const Eigen::Matrix<double, Eigen::Dynamic, 1> &x,
                     double &f) {
        using Eigen::Matrix;
        using Eigen::Dynamic;
        using stan::math::index_type;
        using stan::model::log_prob_propto;
        typedef typename index_type<Matrix<double, Dynamic, 1> >::type idx_t;

        _x.resize(x.size());
        for (idx_t i = 0; i < x.size(); i++)
          _x[i] = x[i];

        try {
          f = - log_prob_propto<false>(_model, _x, _params_i, _msgs);
        } catch (const std::exception& e) {
          if (_msgs)
            (*_msgs) << e.what() << std::endl;
          return 1;
        }

        if (boost::math::isfinite(f)) {
          return 0;
        } else {
          if (_msgs)
            *_msgs << "Error evaluating model log probability: "
                      "Non-finite function evaluation." << std::endl;
          return 2;
        }
      }
      int operator()(const Eigen::Matrix<double, Eigen::Dynamic, 1> &x,
                     double &f,
                     Eigen::Matrix<double, Eigen::Dynamic, 1> &g) {
        using Eigen::Matrix;
        using Eigen::Dynamic;
        using stan::math::index_type;
        using stan::model::log_prob_grad;
        typedef typename index_type<Matrix<double, Dynamic, 1> >::type idx_t;

        _x.resize(x.size());
        for (idx_t i = 0; i < x.size(); i++)
          _x[i] = x[i];

        _fevals++;

        try {
          f = - log_prob_grad<true, false>(_model, _x, _params_i, _g, _msgs);
        } catch (const std::exception& e) {
          if (_msgs)
            (*_msgs) << e.what() << std::endl;
          return 1;
        }

        g.resize(_g.size());
        for (size_t i = 0; i < _g.size(); i++) {
          if (!boost::math::isfinite(_g[i])) {
            if (_msgs)
              *_msgs << "Error evaluating model log probability: "
                                 "Non-finite gradient." << std::endl;
            return 3;
          }
          g[i] = -_g[i];
        }

        if (boost::math::isfinite(f)) {
          return 0;
        } else {
          if (_msgs)
            *_msgs << "Error evaluating model log probability: "
                   << "Non-finite function evaluation."
                   << std::endl;
          return 2;
        }
      }
      int df(const Eigen::Matrix<double, Eigen::Dynamic, 1> &x,
             Eigen::Matrix<double, Eigen:: Dynamic, 1> &g) {
        double f;
        return (*this)(x, f, g);
      }
    };

    template<typename M, typename QNUpdateType, typename Scalar = double,
             int DimAtCompile = Eigen::Dynamic>
    class BFGSLineSearch
      : public BFGSMinimizer<ModelAdaptor<M>, QNUpdateType,
                             Scalar, DimAtCompile> {
    private:
      ModelAdaptor<M> _adaptor;

    public:
      typedef BFGSMinimizer<ModelAdaptor<M>, QNUpdateType, Scalar, DimAtCompile>
      BFGSBase;
      typedef typename BFGSBase::VectorT vector_t;
      typedef typename stan::math::index_type<vector_t>::type idx_t;

      BFGSLineSearch(M& model,
                     const std::vector<double>& params_r,
                     const std::vector<int>& params_i,
                     std::ostream* msgs = 0)
        : BFGSBase(_adaptor),
          _adaptor(model, params_i, msgs) {
        initialize(params_r);
      }

      void initialize(const std::vector<double>& params_r) {
        Eigen::Matrix<double, Eigen::Dynamic, 1> x;
        x.resize(params_r.size());
        for (size_t i = 0; i < params_r.size(); i++)
          x[i] = params_r[i];
        BFGSBase::initialize(x);
      }

      size_t grad_evals() { return _adaptor.fevals(); }
      double logp() { return -(this->curr_f()); }
      double grad_norm() { return this->curr_g().norm(); }
      void grad(std::vector<double>& g) {
        const vector_t &cg(this->curr_g());
        g.resize(cg.size());
        for (idx_t i = 0; i < cg.size(); i++)
          g[i] = -cg[i];
      }
      void params_r(std::vector<double>& x) {
        const vector_t &cx(this->curr_x());
        x.resize(cx.size());
        for (idx_t i = 0; i < cx.size(); i++)
          x[i] = cx[i];
      }
    };

  }

}

#endif


/**
 *****************************************************************************
 * @author     This file is part of libsnark, developed by SCIPR Lab
 *             and contributors (see AUTHORS).
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/
#include <fstream>
#include <iostream>
#ifndef MINDEPS
#include <boost/program_options.hpp>
#endif

#include "common/default_types/tinyram_ppzksnark_pp.hpp"
#include "zk_proof_systems/ppzksnark/ram_ppzksnark/ram_ppzksnark.hpp"
#include "relations/ram_computations/rams/tinyram/tinyram_params.hpp"

#ifndef MINDEPS
namespace po = boost::program_options;

bool process_verifier_command_line(const int argc, const char** argv,
                                   std::string &processed_assembly_fn,
                                   std::string &verification_key_fn,
                                   std::string &primary_input_fn,
                                   std::string &proof_fn,
                                   std::string &verification_result_fn)
{
    try
    {
        po::options_description desc("Usage");
        desc.add_options()
            ("help", "print this help message")
            ("processed_assembly", po::value<std::string>(&processed_assembly_fn)->required())
            ("verification_key", po::value<std::string>(&verification_key_fn)->required())
            ("primary_input", po::value<std::string>(&primary_input_fn)->required())
            ("proof", po::value<std::string>(&proof_fn)->required())
            ("verification_result", po::value<std::string>(&verification_result_fn)->required());

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);

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

        po::notify(vm);
    }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << "\n";
        return false;
    }

    return true;
}
#endif

using namespace libsnark;

int main(int argc, const char * argv[])
{
    default_tinyram_ppzksnark_pp::init_public_params();

#ifdef MINDEPS
    std::string processed_assembly_fn = "processed.txt";
    std::string verification_key_fn = "verification_key.txt";
    std::string proof_fn = "proof.txt";
    std::string primary_input_fn = "primary_input.txt";
    std::string verification_result_fn = "verification_result.txt";
#else
    std::string processed_assembly_fn;
    std::string verification_key_fn;
    std::string proof_fn;
    std::string primary_input_fn;
    std::string verification_result_fn;

    if (!process_verifier_command_line(argc, argv, processed_assembly_fn, verification_key_fn, primary_input_fn, proof_fn, verification_result_fn))
    {
        return 1;
    }
#endif
    start_profiling();

    ram_ppzksnark_verification_key<default_tinyram_ppzksnark_pp> vk;
    std::ifstream vk_file(verification_key_fn);
    vk_file >> vk;
    vk_file.close();

    std::ifstream processed(processed_assembly_fn);
    tinyram_program program = load_preprocessed_program(vk.ap, processed);

    std::ifstream f_primary_input(primary_input_fn);
    tinyram_input_tape primary_input = load_tape(f_primary_input);

    std::ifstream proof_file(proof_fn);
    ram_ppzksnark_proof<default_tinyram_ppzksnark_pp> pi;
    proof_file >> pi;
    proof_file.close();

    const ram_boot_trace<default_tinyram_ppzksnark_pp> boot_trace = tinyram_boot_trace_from_program_and_input(vk.ap, vk.primary_input_size_bound, program, primary_input);
    const bool bit = ram_ppzksnark_verifier<default_tinyram_ppzksnark_pp>(vk, boot_trace, pi);

    printf("================================================================================\n");
    printf("The verification result is: %s\n", (bit ? "PASS" : "FAIL"));
    printf("================================================================================\n");
    std::ofstream vr_file(verification_result_fn);
    vr_file << bit << "\n";
    vr_file.close();
}


// Boost.Geometry

// Copyright (c) 2020-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_RELATE_GEOGRAPHIC_HPP
#define BOOST_GEOMETRY_STRATEGIES_RELATE_GEOGRAPHIC_HPP

// TEMP - move to strategy
#include <boost/geometry/strategies/agnostic/point_in_box_by_side.hpp>
#include <boost/geometry/strategies/cartesian/box_in_box.hpp>
#include <boost/geometry/strategies/geographic/intersection.hpp>
#include <boost/geometry/strategies/geographic/point_in_poly_winding.hpp>
#include <boost/geometry/strategies/spherical/point_in_point.hpp>
#include <boost/geometry/strategies/spherical/disjoint_box_box.hpp>

#include <boost/geometry/strategies/envelope/geographic.hpp>
#include <boost/geometry/strategies/relate/services.hpp>
#include <boost/geometry/strategies/detail.hpp>

#include <boost/geometry/strategy/geographic/area.hpp>
#include <boost/geometry/strategy/geographic/area_box.hpp>

#include <boost/geometry/util/type_traits.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace relate
{

template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
    : public strategies::envelope::geographic<FormulaPolicy, Spheroid, CalculationType>
{
    using base_t = strategies::envelope::geographic<FormulaPolicy, Spheroid, CalculationType>;

public:
    geographic() = default;

    explicit geographic(Spheroid const& spheroid)
        : base_t(spheroid)
    {}

    // area

    template <typename Geometry>
    auto area(Geometry const&,
              std::enable_if_t<! util::is_box<Geometry>::value> * = nullptr) const
    {
        return strategy::area::geographic
            <
                FormulaPolicy,
                strategy::default_order<FormulaPolicy>::value,
                Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry>
    auto area(Geometry const&,
              std::enable_if_t<util::is_box<Geometry>::value> * = nullptr) const
    {
        return strategy::area::geographic_box
            <
                Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    // covered_by

    template <typename Geometry1, typename Geometry2>
    static auto covered_by(Geometry1 const&, Geometry2 const&,
                           std::enable_if_t
                                <
                                    util::is_pointlike<Geometry1>::value
                                 && util::is_box<Geometry2>::value
                                > * = nullptr)
    {
        return strategy::covered_by::spherical_point_box();
    }

    template <typename Geometry1, typename Geometry2>
    static auto covered_by(Geometry1 const&, Geometry2 const&,
                           std::enable_if_t
                            <
                                util::is_box<Geometry1>::value
                             && util::is_box<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::covered_by::spherical_box_box();
    }

    // disjoint

    template <typename Geometry1, typename Geometry2>
    static auto disjoint(Geometry1 const&, Geometry2 const&,
                         std::enable_if_t
                            <
                                util::is_box<Geometry1>::value
                             && util::is_box<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::disjoint::spherical_box_box();
    }

    template <typename Geometry1, typename Geometry2>
    auto disjoint(Geometry1 const&, Geometry2 const&,
                  std::enable_if_t
                    <
                        util::is_segment<Geometry1>::value
                        && util::is_box<Geometry2>::value
                    > * = nullptr) const
    {
        // NOTE: Inconsistent name
        // The only disjoint(Seg, Box) strategy that takes CalculationType.
        return strategy::disjoint::segment_box_geographic
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    // relate

    template <typename Geometry1, typename Geometry2>
    static auto relate(Geometry1 const&, Geometry2 const&,
                       std::enable_if_t
                            <
                                util::is_pointlike<Geometry1>::value
                             && util::is_pointlike<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::within::spherical_point_point();
    }

    template <typename Geometry1, typename Geometry2>
    auto relate(Geometry1 const&, Geometry2 const&,
                std::enable_if_t
                    <
                        util::is_pointlike<Geometry1>::value
                        && ( util::is_linear<Geometry2>::value
                        || util::is_polygonal<Geometry2>::value )
                    > * = nullptr) const
    {
        return strategy::within::geographic_winding
                <
                    void, void, FormulaPolicy, Spheroid, CalculationType
                >(base_t::m_spheroid);
    }

    //template <typename Geometry1, typename Geometry2>
    auto relate(/*Geometry1 const&, Geometry2 const&,
                std::enable_if_t
                    <
                        ( util::is_linear<Geometry1>::value
                        || util::is_polygonal<Geometry1>::value )
                        && ( util::is_linear<Geometry2>::value
                        || util::is_polygonal<Geometry2>::value )
                    > * = nullptr*/) const
    {
        return strategy::intersection::geographic_segments
            <
                FormulaPolicy,
                strategy::default_order<FormulaPolicy>::value,
                Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    // side

    auto side() const
    {
        return strategy::side::geographic
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    // within

    template <typename Geometry1, typename Geometry2>
    static auto within(Geometry1 const&, Geometry2 const&,
                       std::enable_if_t
                            <
                                util::is_pointlike<Geometry1>::value
                                && util::is_box<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::within::spherical_point_box();
    }

    template <typename Geometry1, typename Geometry2>
    static auto within(Geometry1 const&, Geometry2 const&,
                       std::enable_if_t
                            <
                                util::is_box<Geometry1>::value
                             && util::is_box<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::within::spherical_box_box();
    }
};

namespace services
{

template <typename Geometry1, typename Geometry2>
struct default_strategy<Geometry1, Geometry2, geographic_tag, geographic_tag>
{
    using type = strategies::relate::geographic<>;
};

template <typename FormulaPolicy, typename Spheroid, typename CalculationType>
struct strategy_converter<strategy::disjoint::segment_box_geographic<FormulaPolicy, Spheroid, CalculationType>>
{
    static auto get(strategy::disjoint::segment_box_geographic<FormulaPolicy, Spheroid, CalculationType> const& s)
    {
        return strategies::relate::geographic
            <
                FormulaPolicy,
                Spheroid,
                CalculationType
            >(s.model());
    }
};

template <typename P1, typename P2, typename FormulaPolicy, typename Spheroid, typename CalculationType>
struct strategy_converter<strategy::within::geographic_winding<P1, P2, FormulaPolicy, Spheroid, CalculationType>>
{
    static auto get(strategy::within::geographic_winding<P1, P2, FormulaPolicy, Spheroid, CalculationType> const& s)
    {
        return strategies::relate::geographic
            <
                FormulaPolicy,
                Spheroid,
                CalculationType
            >(s.model());
    }
};

template <typename FormulaPolicy, std::size_t SeriesOrder, typename Spheroid, typename CalculationType>
struct strategy_converter<strategy::intersection::geographic_segments<FormulaPolicy, SeriesOrder, Spheroid, CalculationType>>
{
    struct altered_strategy
        : strategies::relate::geographic<FormulaPolicy, Spheroid, CalculationType>
    {
        typedef strategies::relate::geographic<FormulaPolicy, Spheroid, CalculationType> base_t;

        explicit altered_strategy(Spheroid const& spheroid)
            : base_t(spheroid)
        {}

        template <typename Geometry>
        auto area(Geometry const&) const
        {
            return strategy::area::geographic
                <
                    FormulaPolicy, SeriesOrder, Spheroid, CalculationType
                >(base_t::m_spheroid);
        }

        using base_t::relate;

        auto relate(/*...*/) const
        {
            return strategy::intersection::geographic_segments
                <
                    FormulaPolicy, SeriesOrder, Spheroid, CalculationType
                >(base_t::m_spheroid);
        }
    };

    static auto get(strategy::intersection::geographic_segments<FormulaPolicy, SeriesOrder, Spheroid, CalculationType> const& s)
    {
        return altered_strategy(s.model());
    }
};

template <typename FormulaPolicy, typename Spheroid, typename CalculationType>
struct strategy_converter<strategy::within::geographic_point_box_by_side<FormulaPolicy, Spheroid, CalculationType>>
{
    struct altered_strategy
        : strategies::relate::geographic<FormulaPolicy, Spheroid, CalculationType>
    {
        altered_strategy(Spheroid const& spheroid)
            : strategies::relate::geographic<FormulaPolicy, Spheroid, CalculationType>(spheroid)
        {}

        template <typename Geometry1, typename Geometry2>
        auto covered_by(Geometry1 const&, Geometry2 const&,
                        std::enable_if_t
                            <
                                util::is_pointlike<Geometry1>::value
                                && util::is_box<Geometry2>::value
                            > * = nullptr) const
        {
            return strategy::covered_by::geographic_point_box_by_side
                <
                    FormulaPolicy, Spheroid, CalculationType
                >(this->model());
        }

        template <typename Geometry1, typename Geometry2>
        auto within(Geometry1 const&, Geometry2 const&,
                    std::enable_if_t
                        <
                            util::is_pointlike<Geometry1>::value
                            && util::is_box<Geometry2>::value
                        > * = nullptr) const
        {
            return strategy::within::geographic_point_box_by_side
                <
                    FormulaPolicy, Spheroid, CalculationType
                >(this->model());
        }
    };

    static auto get(strategy::covered_by::geographic_point_box_by_side<FormulaPolicy, Spheroid, CalculationType> const& s)
    {
        return altered_strategy(s.model());
    }

    static auto get(strategy::within::geographic_point_box_by_side<FormulaPolicy, Spheroid, CalculationType> const& s)
    {
        return altered_strategy(s.model());
    }
};

template <typename CalculationType>
struct strategy_converter<strategy::covered_by::geographic_point_box_by_side<CalculationType>>
    : strategy_converter<strategy::within::geographic_point_box_by_side<CalculationType>>
{};

} // namespace services

}} // namespace strategies::relate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_RELATE_GEOGRAPHIC_HPP