/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2018, Trenton Schulz
*  Originally based on code:
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* Author: Trenton Schulz
*********************************************************************/

#include "trajectory_planner.h"
#include <costmap_2d/footprint.h>
#include <string>
#include <sstream>
#include <math.h>
#include <cstring>
#include <algorithm>
#include <utility>
#include <angles/angles.h>

#include <boost/algorithm/string.hpp>

#include <ros/console.h>

// for computing path distance
#include <queue>

using namespace std;
using namespace costmap_2d;

namespace siso_local_planner
{
double cubicInDerivative(double time)
{
  return 3.0 * time * time;
}

double cubicOutDerivative(double time)
{
  // 3 * (time - 1)^2
  return cubicInDerivative(time - 1.0);
}

double linearAccelerationDerivative(double time)
{
  return 3 * time;
}

double linearDecelerationDerivative(double time)
{
  return 3 - linearAccelerationDerivative(time);
}

SisoTrajectoryPlanner::VelocityCurve decode_velocity_curve_string(const std::string& velocity_curve)
{
  // Since this will can get data from the keyboard,
  // always lowercase to help out the occasional capitalization typos.
  // This is only ASCII stuff, so we can get away with tolower().
  std::string lower_case_curve;
  lower_case_curve.resize(velocity_curve.size());
  std::transform(velocity_curve.begin(), velocity_curve.end(), lower_case_curve.begin(), ::tolower);

  if (strcmp("siso", lower_case_curve.c_str()) == 0)
  {
    return SisoTrajectoryPlanner::VelocityCurve::SlowInSlowOut;
  }
  else if (strcmp("classic", lower_case_curve.c_str()) == 0)
  {
    return SisoTrajectoryPlanner::VelocityCurve::Classic;
  }
  else if (strcmp("linear", lower_case_curve.c_str()) == 0)
  {
    return SisoTrajectoryPlanner::VelocityCurve::Linear;
  }
  else
  {
    return SisoTrajectoryPlanner::VelocityCurve::Unknown;
  }
}

void SisoTrajectoryPlanner::reconfigure(SisoLocalPlannerConfig& cfg)
{
  SisoLocalPlannerConfig config(cfg);

  boost::mutex::scoped_lock l(configuration_mutex_);

  acc_lim_x_ = config.acc_lim_x;
  acc_lim_y_ = config.acc_lim_y;
  acc_lim_theta_ = config.acc_lim_theta;

  max_vel_x_ = config.max_vel_x;
  min_vel_x_ = config.min_vel_x;

  max_vel_th_ = config.max_vel_theta;
  min_vel_th_ = config.min_vel_theta;
  min_in_place_vel_th_ = config.min_in_place_vel_theta;

  sim_time_ = config.sim_time;
  sim_granularity_ = config.sim_granularity;
  angular_sim_granularity_ = config.angular_sim_granularity;

  pdist_scale_ = config.pdist_scale;
  gdist_scale_ = config.gdist_scale;
  occdist_scale_ = config.occdist_scale;

  if (meter_scoring_)
  {
    // if we use meter scoring, then we want to multiply the biases by the resolution of the costmap
    double resolution = costmap_.getResolution();
    gdist_scale_ *= resolution;
    pdist_scale_ *= resolution;
    occdist_scale_ *= resolution;
  }

  oscillation_reset_dist_ = config.oscillation_reset_dist;
  escape_reset_dist_ = config.escape_reset_dist;
  escape_reset_theta_ = config.escape_reset_theta;

  vx_samples_ = config.vx_samples;
  vtheta_samples_ = config.vtheta_samples;

  if (vx_samples_ <= 0)
  {
    config.vx_samples = 1;
    vx_samples_ = config.vx_samples;
    ROS_WARN("You've specified that you don't want any samples in the x dimension. We'll at least assume that you want "
             "to sample one value... so we're going to set vx_samples to 1 instead");
  }
  if (vtheta_samples_ <= 0)
  {
    config.vtheta_samples = 1;
    vtheta_samples_ = config.vtheta_samples;
    ROS_WARN("You've specified that you don't want any samples in the theta dimension. We'll at least assume that you "
             "want to sample one value... so we're going to set vtheta_samples to 1 instead");
  }

  heading_lookahead_ = config.heading_lookahead;

  holonomic_robot_ = config.holonomic_robot;

  backup_vel_ = config.escape_vel;

  dwa_ = config.dwa;

  heading_scoring_ = config.heading_scoring;
  heading_scoring_timestep_ = config.heading_scoring_timestep;

  simple_attractor_ = config.simple_attractor;

  // y-vels
  string y_string = config.y_vels;
  vector<string> y_strs;
  boost::split(y_strs, y_string, boost::is_any_of(", "), boost::token_compress_on);

  vector<double> y_vels;
  for (vector<string>::iterator it = y_strs.begin(); it != y_strs.end(); ++it)
  {
    istringstream iss(*it);
    double temp;
    iss >> temp;
    y_vels.push_back(temp);
    // ROS_INFO("Adding y_vel: %e", temp);
  }

  y_vels_ = y_vels;
  velocity_curve_ = decode_velocity_curve_string(config.velocity_curve);
  syncEasingCurves();
  total_acceleration_time_ = max_vel_x_ / acc_lim_x_;
}

void SisoTrajectoryPlanner::syncEasingCurves()
{
  switch (velocity_curve_)
  {
    default:
    case Linear:
    case Classic:
      acceleration_curve_ = linearAccelerationDerivative;
      deceleration_curve_ = linearDecelerationDerivative;
      break;
    case SlowInSlowOut:
      acceleration_curve_= cubicInDerivative;
      deceleration_curve_= cubicOutDerivative;
      break;
  }
}

SisoTrajectoryPlanner::SisoTrajectoryPlanner(
    WorldModel& world_model, const Costmap2D& costmap, const std::string& velocity_curve,
    std::vector<geometry_msgs::Point> footprint_spec, double acc_lim_x, double acc_lim_y, double acc_lim_theta,
    double sim_time, double sim_granularity, int vx_samples, int vtheta_samples, double pdist_scale, double gdist_scale,
    double occdist_scale, double heading_lookahead, double oscillation_reset_dist, double escape_reset_dist,
    double escape_reset_theta, bool holonomic_robot, double max_vel_x, double min_vel_x, double max_vel_th,
    double min_vel_th, double min_in_place_vel_th, double backup_vel, bool dwa, bool heading_scoring,
    double heading_scoring_timestep, bool meter_scoring, bool simple_attractor, vector<double> y_vels,
    double stop_time_buffer, double sim_period, double angular_sim_granularity)
  : path_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY())
  , goal_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY())
  , costmap_(costmap)
  , world_model_(world_model)
  , footprint_spec_(footprint_spec)
  , sim_time_(sim_time)
  , sim_granularity_(sim_granularity)
  , angular_sim_granularity_(angular_sim_granularity)
  , vx_samples_(vx_samples)
  , vtheta_samples_(vtheta_samples)
  , pdist_scale_(pdist_scale)
  , gdist_scale_(gdist_scale)
  , occdist_scale_(occdist_scale)
  , acc_lim_x_(acc_lim_x)
  , acc_lim_y_(acc_lim_y)
  , acc_lim_theta_(acc_lim_theta)
  , prev_x_(0)
  , prev_y_(0)
  , escape_x_(0)
  , escape_y_(0)
  , escape_theta_(0)
  , heading_lookahead_(heading_lookahead)
  , oscillation_reset_dist_(oscillation_reset_dist)
  , escape_reset_dist_(escape_reset_dist)
  , escape_reset_theta_(escape_reset_theta)
  , holonomic_robot_(holonomic_robot)
  , max_vel_x_(max_vel_x)
  , min_vel_x_(min_vel_x)
  , max_vel_th_(max_vel_th)
  , min_vel_th_(min_vel_th)
  , min_in_place_vel_th_(min_in_place_vel_th)
  , backup_vel_(backup_vel)
  , dwa_(dwa)
  , heading_scoring_(heading_scoring)
  , heading_scoring_timestep_(heading_scoring_timestep)
  , simple_attractor_(simple_attractor)
  , y_vels_(y_vels)
  , stop_time_buffer_(stop_time_buffer)
  , sim_period_(sim_period)
  , velocity_curve_(decode_velocity_curve_string(velocity_curve))
  , total_acceleration_time_(max_vel_x_ / acc_lim_x_)
{
  // the robot is not stuck to begin with
  stuck_left = false;
  stuck_right = false;
  stuck_left_strafe = false;
  stuck_right_strafe = false;
  rotating_left = false;
  rotating_right = false;
  strafe_left = false;
  strafe_right = false;

  escaping_ = false;
  final_goal_position_valid_ = false;
  syncEasingCurves();

  costmap_2d::calculateMinAndMaxDistances(footprint_spec_, inscribed_radius_, circumscribed_radius_);
}

SisoTrajectoryPlanner::~SisoTrajectoryPlanner()
{
}

bool SisoTrajectoryPlanner::getCellCosts(int cx, int cy, float& path_cost, float& goal_cost, float& occ_cost,
                                         float& total_cost)
{
  MapCell cell = path_map_(cx, cy);
  MapCell goal_cell = goal_map_(cx, cy);
  if (cell.within_robot)
  {
    return false;
  }
  occ_cost = costmap_.getCost(cx, cy);
  if (cell.target_dist == path_map_.obstacleCosts() || cell.target_dist == path_map_.unreachableCellCosts() ||
      occ_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
  {
    return false;
  }
  path_cost = cell.target_dist;
  goal_cost = goal_cell.target_dist;
  total_cost = pdist_scale_ * path_cost + gdist_scale_ * goal_cost + occdist_scale_ * occ_cost;
  return true;
}

/**
 * create and score a trajectory given the current pose of the robot and selected velocities
 */
void SisoTrajectoryPlanner::generateTrajectory(const double x, const double y, const double theta, const double vx,
                                               const double vy, const double vtheta, const double vx_samp,
                                               const double vy_samp, const double vtheta_samp, const double acc_x,
                                               const double acc_y, const double acc_theta, const double acc_progress,
                                               const double impossible_cost, Trajectory& traj)
{
  // make sure the configuration doesn't change mid run
  boost::mutex::scoped_lock l(configuration_mutex_);

  auto x_i = x;
  auto y_i = y;
  auto theta_i = theta;
  auto vx_i = vx;
  auto acc_progress_i = acc_progress;
  auto vy_i = vy;
  auto vtheta_i = vtheta;

  // compute the magnitude of the velocities
  const auto vmag = hypot(vx_samp, vy_samp);

  // compute the number of steps we must take along this trajectory to be "safe"
  int num_steps;
  if (!heading_scoring_)
  {
    num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5);
  }
  else
  {
    num_steps = int(sim_time_ / sim_granularity_ + 0.5);
  }

  // we at least want to take one step... even if we won't move, we want to score our current position
  if (num_steps == 0)
  {
    num_steps = 1;
  }

  const auto dt = sim_time_ / num_steps;
  const auto acc_progress_dp = (sim_time_ / total_acceleration_time_) / num_steps;
  double time = 0.0;
  // create a potential trajectory
  traj.resetPoints();
  traj.xv_ = vx_samp;
  traj.yv_ = vy_samp;
  traj.thetav_ = vtheta_samp;
  traj.cost_ = -1.0;

  // initialize the costs for the trajectory
  double path_dist = 0.0;
  double goal_dist = 0.0;
  double occ_cost = 0.0;
  double heading_diff = 0.0;

  for (int i = 0; i < num_steps; ++i)
  {
    // get map coordinates of a point
    unsigned int cell_x, cell_y;

    // we don't want a path that goes off the known map
    if (!costmap_.worldToMap(x_i, y_i, cell_x, cell_y))
    {
      traj.cost_ = -1.0;
      return;
    }

    // check the point on the trajectory for legality
    const auto footprint_cost = footprintCost(x_i, y_i, theta_i);

    // if the footprint hits an obstacle this trajectory is invalid
    if (footprint_cost < 0)
    {
      traj.cost_ = -1.0;
      return;
      // TODO: Really look at getMaxSpeedToStopInTime... dues to discretization errors and high acceleration limits,
      // it can actually cause the robot to hit obstacles. There may be something to be done to fix, but I'll have to
      // come back to it when I have time. Right now, pulling it out as it'll just make the robot a bit more
      // conservative,
      // but safe.
      /*
      double max_vel_x, max_vel_y, max_vel_th;
      //we want to compute the max allowable speeds to be able to stop
      //to be safe... we'll make sure we can stop some time before we actually hit
      getMaxSpeedToStopInTime(time - stop_time_buffer_ - dt, max_vel_x, max_vel_y, max_vel_th);

      //check if we can stop in time
      if(fabs(vx_samp) < max_vel_x && fabs(vy_samp) < max_vel_y && fabs(vtheta_samp) < max_vel_th){
        ROS_ERROR("v: (%.2f, %.2f, %.2f), m: (%.2f, %.2f, %.2f) t:%.2f, st: %.2f, dt: %.2f", vx_samp, vy_samp,
      vtheta_samp, max_vel_x, max_vel_y, max_vel_th, time, stop_time_buffer_, dt);
        //if we can stop... we'll just break out of the loop here.. no point in checking future points
        break;
      }
      else{
        traj.cost_ = -1.0;
        return;
      }
      */
    }

    occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));

    // Is the robot simply just attracted to the goal? (Fetch is not)
    if (simple_attractor_)
    {
      goal_dist = (x_i - global_plan_[global_plan_.size() - 1].pose.position.x) *
                      (x_i - global_plan_[global_plan_.size() - 1].pose.position.x) +
                  (y_i - global_plan_[global_plan_.size() - 1].pose.position.y) *
                      (y_i - global_plan_[global_plan_.size() - 1].pose.position.y);
    }
    else
    {
      bool update_path_and_goal_distances = true;

      // with heading scoring, we take into account heading diff, and also only score
      // path and goal distance for one point of the trajectory
      if (heading_scoring_)
      {
        if (time >= heading_scoring_timestep_ && time < heading_scoring_timestep_ + dt)
        {
          heading_diff = headingDiff(cell_x, cell_y, x_i, y_i, theta_i);
        }
        else
        {
          update_path_and_goal_distances = false;
        }
      }

      if (update_path_and_goal_distances)
      {
        // update path and goal distances
        path_dist = path_map_(cell_x, cell_y).target_dist;
        goal_dist = goal_map_(cell_x, cell_y).target_dist;

        // if a point on this trajectory has no clear path to goal it is invalid
        if (impossible_cost <= goal_dist || impossible_cost <= path_dist)
        {
          //            ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f",
          //                goal_dist, path_dist, impossible_cost);
          traj.cost_ = -2.0;
          return;
        }
      }
    }

    // the point is legal... add it to the trajectory
    traj.addPoint(x_i, y_i, theta_i);

    // calculate velocities
    vx_i = computeNewXVelocity(vx_samp, vx_i, acc_x, acc_progress, acc_progress_dp, dt);
    vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
    vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);

    // calculate positions
    x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
    y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
    theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);

    // increment time
    time += dt;

    // sync progress to the current speed (since we sometimes keep our speed our progress doesn't always change.
    acc_progress_i = progressForSpeed(vx_i);
  }  // end for i < numsteps

  // ROS_INFO("OccCost: %f, vx: %.2f, vy: %.2f, vtheta: %.2f", occ_cost, vx_samp, vy_samp, vtheta_samp);
  double cost = -1.0;
  if (!heading_scoring_)
  {
    cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
  }
  else
  {
    cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;
  }
  traj.cost_ = cost;
}

double SisoTrajectoryPlanner::progressForSpeed(const double vi) const
{
  switch (velocity_curve_)
  {
    default:
      ROS_INFO("progressForSpeed: Unknown curve using classic!");
    case Linear:
    case Classic:
      return std::min(1.0, (vi / acc_lim_x_) / total_acceleration_time_);
    case SlowInSlowOut:
      return std::min(1.0, sqrt(vi / max_vel_x_));
  }
}

double SisoTrajectoryPlanner::velocityUp(const double vg, const double vi, const double a_max, const double dt,
                                         const double acc_progress, const double dp) const
{
  switch (velocity_curve_)
  {
    default:
    case Classic:
      return std::min(vg, vi + a_max * dt);
    case Linear:
    case SlowInSlowOut:
    {
      const auto accel_step = std::min(1.0, acc_progress + dp);
      return std::min(vg, (acceleration_curve_(accel_step) / 3.0) * max_vel_x_);
    }
  }
}

double SisoTrajectoryPlanner::velocityDown(const double vg, const double vi, const double a_max, const double dt,
                                           const double acc_progress, const double dp) const
{
  switch (velocity_curve_)
  {
    default:
    case Classic:
      return std::max(vg, vi - a_max * dt);
    case Linear:
    case SlowInSlowOut:
    {
      const auto decel_step = std::min(1.0, 1.0 - acc_progress + dp);
      return std::max(vg, (deceleration_curve_(decel_step) / 3.0) * max_vel_x_);
    }
  }
}

double SisoTrajectoryPlanner::computeNewXVelocity(const double vg, const double vi, const double a_max,
                                                  const double acc_progress, const double dt, double dp) const
{
  if (vg - vi >= 0)
  {
    return velocityUp(vg, vi, a_max, dt, acc_progress, dp);
  }
  return velocityDown(vg, vi, a_max, dt, acc_progress, dp);
}

double SisoTrajectoryPlanner::headingDiff(int cell_x, int cell_y, double x, double y, double heading)
{
  unsigned int goal_cell_x, goal_cell_y;

  // find a clear line of sight from the robot's cell to a farthest point on the path
  for (int i = global_plan_.size() - 1; i >= 0; --i)
  {
    if (costmap_.worldToMap(global_plan_[i].pose.position.x, global_plan_[i].pose.position.y, goal_cell_x, goal_cell_y))
    {
      if (lineCost(cell_x, goal_cell_x, cell_y, goal_cell_y) >= 0)
      {
        double gx, gy;
        costmap_.mapToWorld(goal_cell_x, goal_cell_y, gx, gy);
        return fabs(angles::shortest_angular_distance(heading, atan2(gy - y, gx - x)));
      }
    }
  }
  return DBL_MAX;
}

// calculate the cost of a ray-traced line
double SisoTrajectoryPlanner::lineCost(int x0, int x1, int y0, int y1)
{
  // Bresenham Ray-Tracing
  int deltax = abs(x1 - x0);  // The difference between the x's
  int deltay = abs(y1 - y0);  // The difference between the y's
  int x = x0;                 // Start x off at the first pixel
  int y = y0;                 // Start y off at the first pixel

  int xinc1, xinc2, yinc1, yinc2;
  int den, num, numadd, numpixels;

  double line_cost = 0.0;
  double point_cost = -1.0;

  if (x1 >= x0)  // The x-values are increasing
  {
    xinc1 = 1;
    xinc2 = 1;
  }
  else  // The x-values are decreasing
  {
    xinc1 = -1;
    xinc2 = -1;
  }

  if (y1 >= y0)  // The y-values are increasing
  {
    yinc1 = 1;
    yinc2 = 1;
  }
  else  // The y-values are decreasing
  {
    yinc1 = -1;
    yinc2 = -1;
  }

  if (deltax >= deltay)  // There is at least one x-value for every y-value
  {
    xinc1 = 0;  // Don't change the x when numerator >= denominator
    yinc2 = 0;  // Don't change the y for every iteration
    den = deltax;
    num = deltax / 2;
    numadd = deltay;
    numpixels = deltax;  // There are more x-values than y-values
  }
  else
  {             // There is at least one y-value for every x-value
    xinc2 = 0;  // Don't change the x for every iteration
    yinc1 = 0;  // Don't change the y when numerator >= denominator
    den = deltay;
    num = deltay / 2;
    numadd = deltax;
    numpixels = deltay;  // There are more y-values than x-values
  }

  for (int curpixel = 0; curpixel <= numpixels; curpixel++)
  {
    point_cost = pointCost(x, y);  // Score the current point

    if (point_cost < 0)
    {
      return -1;
    }

    if (line_cost < point_cost)
    {
      line_cost = point_cost;
    }

    num += numadd;  // Increase the numerator by the top of the fraction
    if (num >= den)
    {              // Check if numerator >= denominator
      num -= den;  // Calculate the new numerator value
      x += xinc1;  // Change the x as appropriate
      y += yinc1;  // Change the y as appropriate
    }
    x += xinc2;  // Change the x as appropriate
    y += yinc2;  // Change the y as appropriate
  }

  return line_cost;
}

double SisoTrajectoryPlanner::pointCost(int x, int y)
{
  unsigned char cost = costmap_.getCost(x, y);
  // if the cell is in an obstacle the path is invalid
  if (cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION)
  {
    return -1;
  }

  return cost;
}

void SisoTrajectoryPlanner::updatePlan(const vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists)
{
  global_plan_.resize(new_plan.size());
  for (unsigned int i = 0; i < new_plan.size(); ++i)
  {
    global_plan_[i] = new_plan[i];
  }

  if (global_plan_.size() > 0)
  {
    geometry_msgs::PoseStamped& final_goal_pose = global_plan_[global_plan_.size() - 1];
    final_goal_x_ = final_goal_pose.pose.position.x;
    final_goal_y_ = final_goal_pose.pose.position.y;
    final_goal_position_valid_ = true;
  }
  else
  {
    final_goal_position_valid_ = false;
  }

  if (compute_dists)
  {
    // reset the map for new operations
    path_map_.resetPathDist();
    goal_map_.resetPathDist();

    // make sure that we update our path based on the global plan and compute costs
    path_map_.setTargetCells(costmap_, global_plan_);
    goal_map_.setLocalGoal(costmap_, global_plan_);
    ROS_DEBUG("Path/Goal distance computed");
  }
}

bool SisoTrajectoryPlanner::checkTrajectory(double x, double y, double theta, double vx, double vy, double vtheta,
                                            double vx_samp, double vy_samp, double vtheta_samp)
{
  Trajectory t;

  double cost = scoreTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp);

  // if the trajectory is a legal one... the check passes
  if (cost >= 0)
  {
    return true;
  }
  ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vx_samp, vy_samp, vtheta_samp, cost);

  // otherwise the check fails
  return false;
}

double SisoTrajectoryPlanner::scoreTrajectory(const double x, const double y, const double theta, const double vx,
                                              const double vy, const double vtheta, const double vx_samp,
                                              const double vy_samp, const double vtheta_samp)
{
  Trajectory t;
  const auto impossible_cost = path_map_.obstacleCosts();
  generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_lim_x_, acc_lim_y_, acc_lim_theta_,
                     progressForSpeed(vx), impossible_cost, t);

  // return the cost.
  return double(t.cost_);
}

double SisoTrajectoryPlanner::resampleXSpeed(const double vx_sample, const double dvx, const double acc_progress,
                                             const double dvp) const
{
  switch (velocity_curve_)
  {
    default:
      ROS_DEBUG("Unknown curve type using classic speed sample");
    case Classic:
      return vx_sample + dvx;
    case Linear:
    case SlowInSlowOut:
      return (acceleration_curve_(std::min(1.0, acc_progress + dvp)) / 3) * max_vel_x_;
  }
}

/*
 * create the trajectories we wish to score
 */
Trajectory SisoTrajectoryPlanner::createTrajectories(const double x, const double y, const double theta,
                                                     const double vx, const double vy, const double vtheta,
                                                     const double acc_x, const double acc_y, const double acc_theta)
{
  // compute feasible velocity limits in robot space
  double max_vel_x = max_vel_x_, max_vel_theta;
  double min_vel_x, min_vel_theta;

  const auto duration = dwa_ ? sim_period_ : sim_time_;
  if (final_goal_position_valid_)
  {
    const auto final_goal_dist = hypot(final_goal_x_ - x, final_goal_y_ - y);
    max_vel_x = min(max_vel_x, final_goal_dist / duration);
  }

  const auto progress_change_over_duration = duration / total_acceleration_time_;
  const auto acc_progress = progressForSpeed(vx);
  max_vel_x = std::max(velocityUp(max_vel_x, vx, acc_x, duration, acc_progress, progress_change_over_duration), min_vel_x_);
  min_vel_x = velocityDown(min_vel_x_, vx, acc_x, duration, acc_progress, progress_change_over_duration);
  max_vel_theta = min(max_vel_th_, vtheta + acc_theta * duration);
  min_vel_theta = max(min_vel_th_, vtheta - acc_theta * duration);

  // we want to sample the velocity space regularly
  const auto dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1);
  const auto dvtheta = (max_vel_theta - min_vel_theta) / (vtheta_samples_ - 1);
  const auto dvp = (progressForSpeed(max_vel_x) - progressForSpeed(min_vel_x)) / (vx_samples_ - 1);

  auto vx_samp = min_vel_x;
  auto acc_progress_samp = progressForSpeed(vx_samp);
  auto vtheta_samp = min_vel_theta;
  auto vy_samp = 0.0;

  // keep track of the best trajectory seen so far
  Trajectory best_traj;
  best_traj.cost_ = -1.0;

  Trajectory comp_traj;
  comp_traj.cost_ = -1.0;

  // any cell with a cost greater than the size of the map is impossible
  const auto impossible_cost = path_map_.obstacleCosts();

  // if we're performing an escape we won't allow moving forward
  if (!escaping_)
  {
    // loop through all x velocities
    for (int i = 0; i < vx_samples_; ++i)
    {
      vtheta_samp = 0;
      // first sample the straight trajectory
      generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta,
                         acc_progress, impossible_cost, comp_traj);

      // if the new trajectory is better... let's take it
      if (comp_traj.cost_ >= 0 && (comp_traj.cost_ < best_traj.cost_ || best_traj.cost_ < 0))
      {
	std::swap(best_traj, comp_traj);
      }

      vtheta_samp = min_vel_theta;
      // next sample all theta trajectories
      for (int j = 0; j < vtheta_samples_ - 1; ++j)
      {
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta,
                           acc_progress, impossible_cost, comp_traj);

        // if the new trajectory is better... let's take it
        if (comp_traj.cost_ >= 0 && (comp_traj.cost_ < best_traj.cost_ || best_traj.cost_ < 0))
        {
	  std::swap(best_traj, comp_traj);
        }
        vtheta_samp += dvtheta;
      }
      vx_samp = std::min(max_vel_x, resampleXSpeed(vx_samp, dvx, acc_progress_samp, dvp));
      acc_progress_samp = std::min(1.0, acc_progress_samp + dvp);
    }

    // only explore y velocities with holonomic robots (Fetch is not one of these)!
    if (holonomic_robot_)
    {
      // explore trajectories that move forward but also strafe slightly
      vx_samp = 0.1;
      vy_samp = 0.1;
      vtheta_samp = 0.0;
      acc_progress_samp = progressForSpeed(vx_samp);
      generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta,
                         acc_progress, impossible_cost, comp_traj);

      // if the new trajectory is better... let's take it
      if (comp_traj.cost_ >= 0 && (comp_traj.cost_ < best_traj.cost_ || best_traj.cost_ < 0))
      {
	std::swap(best_traj, comp_traj);
      }

      vx_samp = 0.1;
      vy_samp = -0.1;
      vtheta_samp = 0.0;
      acc_progress_samp = progressForSpeed(vx_samp);
      generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta,
                         acc_progress, impossible_cost, comp_traj);

      // if the new trajectory is better... let's take it
      if (comp_traj.cost_ >= 0 && (comp_traj.cost_ < best_traj.cost_ || best_traj.cost_ < 0))
      {
	std::swap(best_traj, comp_traj);
      }
    }
  }  // end if not escaping

  // next we want to generate trajectories for rotating in place
  vtheta_samp = min_vel_theta;
  vx_samp = 0.0;
  vy_samp = 0.0;
  acc_progress_samp = 0.0;

  // let's try to rotate toward open space
  double heading_dist = DBL_MAX;

  for (int i = 0; i < vtheta_samples_; ++i)
  {
    // enforce a minimum rotational velocity because the base can't handle small in-place rotations
    double vtheta_samp_limited =
        vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_) : min(vtheta_samp, -1.0 * min_in_place_vel_th_);

    generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited, acc_x, acc_y, acc_theta,
                       acc_progress, impossible_cost, comp_traj);

    // if the new trajectory is better... let's take it...
    // note if we can legally rotate in place we prefer to do that rather than move with y velocity
    if (comp_traj.cost_ >= 0 &&
        (comp_traj.cost_ <= best_traj.cost_ || best_traj.cost_ < 0 || best_traj.yv_ != 0.0) &&
        (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta))
    {
      double x_r, y_r, th_r;
      comp_traj.getEndpoint(x_r, y_r, th_r);
      x_r += heading_lookahead_ * cos(th_r);
      y_r += heading_lookahead_ * sin(th_r);
      unsigned int cell_x, cell_y;

      // make sure that we'll be looking at a legal cell
      if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y))
      {
        double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
        if (ahead_gdist < heading_dist)
        {
          // if we haven't already tried rotating left since we've moved forward
          if (vtheta_samp < 0 && !stuck_left)
          {
	    std::swap(best_traj, comp_traj);
            heading_dist = ahead_gdist;
          }
          // if we haven't already tried rotating right since we've moved forward
          else if (vtheta_samp > 0 && !stuck_right)
          {
	    std::swap(best_traj, comp_traj);
            heading_dist = ahead_gdist;
          }
        }
      }
    }

    vtheta_samp += dvtheta;
  }

  // do we have a legal trajectory (consider folding this in with the slightly different stuff on line 1014).
  if (best_traj.cost_ >= 0)
  {
    // avoid oscillations of in place rotation and in place strafing
    if (!(best_traj.xv_ > 0))
    {
      if (best_traj.thetav_ < 0)
      {
        if (rotating_right)
        {
          stuck_right = true;
        }
        rotating_right = true;
      }
      else if (best_traj.thetav_ > 0)
      {
        if (rotating_left)
        {
          stuck_left = true;
        }
        rotating_left = true;
      }
      else if (best_traj.yv_ > 0)
      {
        if (strafe_right)
        {
          stuck_right_strafe = true;
        }
        strafe_right = true;
      }
      else if (best_traj.yv_ < 0)
      {
        if (strafe_left)
        {
          stuck_left_strafe = true;
        }
        strafe_left = true;
      }

      // set the position we must move a certain distance away from
      prev_x_ = x;
      prev_y_ = y;
    }

    double dist = hypot(x - prev_x_, y - prev_y_);
    if (dist > oscillation_reset_dist_)
    {
      rotating_left = false;
      rotating_right = false;
      strafe_left = false;
      strafe_right = false;
      stuck_left = false;
      stuck_right = false;
      stuck_left_strafe = false;
      stuck_right_strafe = false;
    }

    dist = hypot(x - escape_x_, y - escape_y_);
    if (dist > escape_reset_dist_ ||
        fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_)
    {
      escaping_ = false;
    }

    return best_traj;
  }

  // only explore y velocities with holonomic robots
  if (holonomic_robot_)
  {
    // if we can't rotate in place or move forward... maybe we can move sideways and rotate
    vtheta_samp = min_vel_theta;
    vx_samp = 0.0;
    acc_progress_samp = 0.0;

    // loop through all y velocities
    for (unsigned int i = 0; i < y_vels_.size(); ++i)
    {
      vtheta_samp = 0;
      vy_samp = y_vels_[i];
      // sample completely horizontal trajectories
      generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta,
                         acc_progress_samp, impossible_cost, comp_traj);

      // if the new trajectory is better... let's take it
      if (comp_traj.cost_ >= 0 && (comp_traj.cost_ <= best_traj.cost_ || best_traj.cost_ < 0))
      {
        double x_r, y_r, th_r;
        comp_traj.getEndpoint(x_r, y_r, th_r);
        x_r += heading_lookahead_ * cos(th_r);
        y_r += heading_lookahead_ * sin(th_r);
        unsigned int cell_x, cell_y;

        // make sure that we'll be looking at a legal cell
        if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y))
        {
          double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
          if (ahead_gdist < heading_dist)
          {
            // if we haven't already tried strafing left since we've moved forward
            if (vy_samp > 0 && !stuck_left_strafe)
            {
	      std::swap(best_traj, comp_traj);
              heading_dist = ahead_gdist;
            }
            // if we haven't already tried rotating right since we've moved forward
            else if (vy_samp < 0 && !stuck_right_strafe)
            {
	      std::swap(best_traj, comp_traj);
              heading_dist = ahead_gdist;
            }
          }
        }
      }
    }
  }

  // do we have a legal trajectory
  if (best_traj.cost_ >= 0)
  {
    if (!(best_traj.xv_ > 0))
    {
      if (best_traj.thetav_ < 0)
      {
        if (rotating_right)
        {
          stuck_right = true;
        }
        rotating_left = true;
      }
      else if (best_traj.thetav_ > 0)
      {
        if (rotating_left)
        {
          stuck_left = true;
        }
        rotating_right = true;
      }
      else if (best_traj.yv_ > 0)
      {
        if (strafe_right)
        {
          stuck_right_strafe = true;
        }
        strafe_left = true;
      }
      else if (best_traj.yv_ < 0)
      {
        if (strafe_left)
        {
          stuck_left_strafe = true;
        }
        strafe_right = true;
      }

      // set the position we must move a certain distance away from
      prev_x_ = x;
      prev_y_ = y;
    }

    double dist = hypot(x - prev_x_, y - prev_y_);
    if (dist > oscillation_reset_dist_)
    {
      rotating_left = false;
      rotating_right = false;
      strafe_left = false;
      strafe_right = false;
      stuck_left = false;
      stuck_right = false;
      stuck_left_strafe = false;
      stuck_right_strafe = false;
    }

    dist = hypot(x - escape_x_, y - escape_y_);
    if (dist > escape_reset_dist_ ||
        fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_)
    {
      escaping_ = false;
    }

    return best_traj;
  }

  // and finally, if we can't do anything else, we want to generate trajectories that move backwards slowly
  vtheta_samp = 0.0;
  vx_samp = backup_vel_;
  vy_samp = 0.0;
  generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta,
                     acc_progress, impossible_cost, comp_traj);

  // if the new trajectory is better... let's take it
  /*
     if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
     swap = best_traj;
     best_traj = comp_traj;
     comp_traj = swap;
     }
     */

  // we'll allow moving backwards slowly even when the static map shows it as blocked
  std::swap(best_traj, comp_traj);

  double dist = hypot(x - prev_x_, y - prev_y_);
  if (dist > oscillation_reset_dist_)
  {
    rotating_left = false;
    rotating_right = false;
    strafe_left = false;
    strafe_right = false;
    stuck_left = false;
    stuck_right = false;
    stuck_left_strafe = false;
    stuck_right_strafe = false;
  }

  // only enter escape mode when the planner has given a valid goal point
  if (!escaping_ && best_traj.cost_ > -2.0)
  {
    escape_x_ = x;
    escape_y_ = y;
    escape_theta_ = theta;
    escaping_ = true;
  }

  dist = hypot(x - escape_x_, y - escape_y_);

  if (dist > escape_reset_dist_ || fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_)
  {
    escaping_ = false;
  }

  // if the trajectory failed because the footprint hits something, we're still going to back up
  if (best_traj.cost_ == -1.0)
    best_traj.cost_ = 1.0;

  return best_traj;
}

// given the current state of the robot, find a good trajectory
Trajectory SisoTrajectoryPlanner::findBestPath(const tf::Stamped<tf::Pose>& global_pose,
                                               const tf::Stamped<tf::Pose>& global_vel,
                                               tf::Stamped<tf::Pose>& drive_velocities)
{
  const Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(),
                            tf::getYaw(global_pose.getRotation()));
  const Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(),
                            tf::getYaw(global_vel.getRotation()));

  // reset the map for new operations
  path_map_.resetPathDist();
  goal_map_.resetPathDist();

  // temporarily remove obstacles that are within the footprint of the robot
  const auto footprint_list = footprint_helper_.getFootprintCells(pos, footprint_spec_, costmap_, true);

  // mark cells within the initial footprint of the robot
  for (const auto& footprint : footprint_list)
  {
    path_map_(footprint.x, footprint.y).within_robot = true;
  }

  // make sure that we update our path based on the global plan and compute costs
  path_map_.setTargetCells(costmap_, global_plan_);
  goal_map_.setLocalGoal(costmap_, global_plan_);
  ROS_DEBUG("Path/Goal distance computed");

  // rollout trajectories and find the minimum cost one
  Trajectory best = createTrajectories(pos[0], pos[1], pos[2], vel[0], vel[1], vel[2], acc_lim_x_, acc_lim_y_,
                                       acc_lim_theta_);
  //ROS_INFO("best is %f %f", best.xv_, best.cost_);
  ROS_DEBUG("Trajectories created");

  /*
  //If we want to print a ppm file to draw goal dist
  char buf[4096];
  sprintf(buf, "base_local_planner.ppm");
  FILE *fp = fopen(buf, "w");
  if(fp){
    fprintf(fp, "P3\n");
    fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
    fprintf(fp, "255\n");
    for(int j = map_.size_y_ - 1; j >= 0; --j){
      for(unsigned int i = 0; i < map_.size_x_; ++i){
        int g_dist = 255 - int(map_(i, j).goal_dist);
        int p_dist = 255 - int(map_(i, j).path_dist);
        if(g_dist < 0)
          g_dist = 0;
        if(p_dist < 0)
          p_dist = 0;
        fprintf(fp, "%d 0 %d ", g_dist, 0);
      }
      fprintf(fp, "\n");
    }
    fclose(fp);
  }
  */

  if (best.cost_ < 0)
  {
    drive_velocities.setIdentity();
  }
  else
  {
    tf::Vector3 start(best.xv_, best.yv_, 0);
    drive_velocities.setOrigin(start);
    tf::Matrix3x3 matrix;
    matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
    drive_velocities.setBasis(matrix);
  }

  return best;
}

// we need to take the footprint of the robot into account when we calculate cost to obstacles
double SisoTrajectoryPlanner::footprintCost(double x_i, double y_i, double theta_i)
{
  // check if the footprint is legal
  return world_model_.footprintCost(x_i, y_i, theta_i, footprint_spec_, inscribed_radius_, circumscribed_radius_);
}

void SisoTrajectoryPlanner::getLocalGoal(double& x, double& y)
{
  x = path_map_.goal_x_;
  y = path_map_.goal_y_;
}
};


// Copyright (c) 2017 Graphcore Ltd. All rights reserved.
#include "Constraint.hpp"
#include "Scheduler.hpp"
#include <memory>
#define BOOST_TEST_MODULE LessOrEqual
#include <boost/test/unit_test.hpp>

using namespace popsolver;

BOOST_AUTO_TEST_CASE(PropagateNoChange) {
  Variable a(0), b(1);
  auto lessOrEqual = std::unique_ptr<LessOrEqual>(new LessOrEqual(a, b));
  Domains domains;
  domains.push_back({DataType{1}, DataType{5}}); // a
  domains.push_back({DataType{7}, DataType{8}}); // b
  Scheduler scheduler(domains, {lessOrEqual.get()});
  bool success = lessOrEqual->propagate(scheduler);
  BOOST_CHECK(success);
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{1});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{5});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].min(), DataType{7});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].max(), DataType{8});
}

BOOST_AUTO_TEST_CASE(PropagateChangeA) {
  Variable a(0), b(1);
  auto lessOrEqual = std::unique_ptr<LessOrEqual>(new LessOrEqual(a, b));
  Domains domains;
  domains.push_back({DataType{5}, DataType{10}}); // a
  domains.push_back({DataType{7}, DataType{8}});  // b
  Scheduler scheduler(domains, {lessOrEqual.get()});
  bool success = lessOrEqual->propagate(scheduler);
  BOOST_CHECK(success);
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{5});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{8});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].min(), DataType{7});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].max(), DataType{8});
}

BOOST_AUTO_TEST_CASE(PropagateChangeB) {
  Variable a(0), b(1);
  auto lessOrEqual = std::unique_ptr<LessOrEqual>(new LessOrEqual(a, b));
  Domains domains;
  domains.push_back({DataType{5}, DataType{10}}); // a
  domains.push_back({DataType{1}, DataType{11}}); // b
  Scheduler scheduler(domains, {lessOrEqual.get()});
  bool success = lessOrEqual->propagate(scheduler);
  BOOST_CHECK(success);
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{5});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{10});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].min(), DataType{5});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].max(), DataType{11});
}

BOOST_AUTO_TEST_CASE(PropagateChangeBoth) {
  Variable a(0), b(1);
  auto lessOrEqual = std::unique_ptr<LessOrEqual>(new LessOrEqual(a, b));
  Domains domains;
  domains.push_back({DataType{5}, DataType{10}}); // a
  domains.push_back({DataType{1}, DataType{8}});  // b
  Scheduler scheduler(domains, {lessOrEqual.get()});
  bool success = lessOrEqual->propagate(scheduler);
  BOOST_CHECK(success);
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{5});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{8});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].min(), DataType{5});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].max(), DataType{8});
}

BOOST_AUTO_TEST_CASE(PropagateFail) {
  Variable a(0), b(1);
  LessOrEqual c(a, b);
  Domains domains;
  domains.push_back({DataType{5}, DataType{6}});
  domains.push_back({DataType{1}, DataType{4}});
  Scheduler scheduler(domains, {&c});
  bool success = c.propagate(scheduler);
  BOOST_CHECK(!success);
}


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

#include "Path.h"
#include "Mesh.h"
#include "IntersectionFace.h"

namespace {
    using namespace Geotree;

    class PathTest : public ::testing::Test {
    };

    TEST_F(PathTest, first) {
        Mesh mesh;
        PathT path(mesh);

        path.add(Vector3d(0, 0, 0));

        EXPECT_EQ(path.size(), 1);

        EXPECT_EQ(path.get(0), Vector3d(0, 0, 0));
    }

    TEST_F(PathTest, addPoints) {
        Mesh mesh;
        PathT path(mesh);

        path.add(Vector3d(1, 0, 0));
        path.add(Vector3d(0, 2, 0));
        path.add(Vector3d(0, 0, 3));

        EXPECT_EQ(path.size(), 3);

        EXPECT_EQ(path.get(0), Vector3d(1, 0, 0));
        EXPECT_EQ(path.get(1), Vector3d(0, 2, 0));
        EXPECT_EQ(path.get(2), Vector3d(0, 0, 3));
    }

    TEST_F(PathTest, addOperator) {
        Mesh mesh;
        PathT path(mesh);

        path.add(Vector3d(1, 4, 0));
        path += Vector3d(0, 2, 5);
        path += Vector3d(6, 0, 3);

        EXPECT_EQ(path.size(), 3);

        EXPECT_EQ(path.get(0), Vector3d(1, 4, 0));
        EXPECT_EQ(path.get(1), Vector3d(0, 2, 5));
        EXPECT_EQ(path.get(2), Vector3d(6, 0, 3));
    }

    TEST_F(PathTest, eqOperatorFlip) {
        Mesh mesh;
        PathT path(mesh);
        PathT path2(mesh);
        PathT path3(mesh);
        PathT path4(mesh);

        path.add(Vector3d(1, 0, 0));
        path.add(Vector3d(0, 2, 0));
        path.add(Vector3d(0, 0, 3));

        path2.addExisting(0);
        path2.addExisting(1);
        path2.addExisting(2);

        path3.addExisting(2);
        path3.addExisting(1);
        path3.addExisting(0);

        path4.addExisting(0);
        path4.addExisting(1);

        EXPECT_TRUE(path == path2);
        EXPECT_TRUE(path == path3);

        EXPECT_FALSE(path == path4);

    }

    TEST_F(PathTest, eqOperatorStartPoint) {
        Mesh mesh;
        PathT path1(mesh);
        PathT path2(mesh);
        PathT path3(mesh);
        PathT path4(mesh);
        PathT path5(mesh);
        PathT path6(mesh);

        path1.add(Vector3d(1, 0, 0));
        path1.add(Vector3d(2, 0, 0));
        path1.add(Vector3d(3, 0, 0));
        path1.add(Vector3d(4, 0, 0));
        path1.add(Vector3d(5, 0, 0));

        path2.addExisting(1);
        path2.addExisting(2);
        path2.addExisting(3);
        path2.addExisting(4);
        path2.addExisting(0);

        path3.addExisting(4);
        path3.addExisting(0);
        path3.addExisting(1);
        path3.addExisting(2);
        path3.addExisting(3);

        path4.addExisting(3);
        path4.addExisting(2);
        path4.addExisting(1);
        path4.addExisting(0);
        path4.addExisting(4);

        path5.addExisting(0);
        path5.addExisting(4);
        path5.addExisting(3);
        path5.addExisting(2);
        path5.addExisting(1);

        path6.addExisting(3);
        path6.addExisting(0);
        path6.addExisting(1);
        path6.addExisting(2);
        path6.addExisting(4);

        EXPECT_EQ(path1, path2);
        EXPECT_EQ(path1, path3);
        EXPECT_EQ(path1, path4);
        EXPECT_EQ(path1, path5);
        EXPECT_NE(path1, path6);
    }

    TEST_F(PathTest, eqOperatorNoSharedStartPoint) {
        Mesh mesh;

        PathT path1(mesh);
        PathT path2(mesh);
        PathT path3(mesh);
        PathT path4(mesh);
        PathT path5(mesh);
        PathT path6(mesh);

        path1.add(Vector3d(1, 0, 0));
        path1.add(Vector3d(2, 0, 0));
        path1.add(Vector3d(3, 0, 0));
        path1.add(Vector3d(4, 0, 0));
        path1.add(Vector3d(5, 0, 0));
        path1.add(Vector3d(5, 0, 0));

        path2.addExisting(0);
        path2.addExisting(1);
        path2.addExisting(2);
        path2.addExisting(3);
        path2.addExisting(4);

        path3.addExisting(1);
        path3.addExisting(2);
        path3.addExisting(3);
        path3.addExisting(4);
        path3.addExisting(5);

        path4.addExisting(1);
        path4.addExisting(2);
        path4.addExisting(3);
        path4.addExisting(0);
        path4.addExisting(5);

        EXPECT_NE(path2, path3);
        EXPECT_NE(path2, path4);

    }

        TEST_F(PathTest, getPoint) {
        Mesh mesh;
        PathT path1(mesh);

        path1.add(Vector3d(1, 0, 0));
        path1.add(Vector3d(2, 0, 0));
        path1.add(Vector3d(3, 0, 0));
        path1.add(Vector3d(4, 0, 0));
        path1.add(Vector3d(5, 0, 0));

        Point point = path1.begin();
        EXPECT_EQ(point.getIndex(), 0);
        EXPECT_EQ(point.getVector(), Vector3d(1, 0, 0));

        Point point2 = path1.back();
        EXPECT_EQ(point2.getIndex(), 4);
        EXPECT_EQ(point2.getVector(), Vector3d(5, 0, 0));

    }

    TEST_F(PathTest, addPoint) {
        Mesh mesh;
        PathT path(mesh);

        Point point(mesh, 0);

        path.add(point);

        EXPECT_EQ(path.size(), 1);
    }

    TEST_F(PathTest, EdgeToEdge)
    {

        Mesh mesh;

        PathX<FacePoint> pathSegment(mesh);
        pathSegment.add(FacePoint(mesh, Vector(0.5, 0, 0), SEGMENT, 0));
        pathSegment.add(FacePoint(mesh, Vector(0, 0.5, 0), SEGMENT, 2));

        EXPECT_TRUE(pathSegment.edgeToEdge());

        PathX<FacePoint> pathFace(mesh);

        pathFace.add(FacePoint(mesh, Vector(0.1, 0.1, 0), FACE, 0));
        pathFace.add(FacePoint(mesh, Vector(0.5, 0.1, 0), FACE, 0));
        pathFace.add(FacePoint(mesh, Vector(0.1, 0.5, 0), FACE, 0));

        EXPECT_FALSE(pathFace.edgeToEdge());
    }

    TEST_F(PathTest, Triangulate)
    {
        Mesh mesh;

        PathX<Point> path(mesh);

        path.add(Point(mesh, mesh.add(Vector3d(0,0,0))));
        path.add(Point(mesh, mesh.add(Vector3d(1,0,0))));
        path.add(Point(mesh, mesh.add(Vector3d(1,1,0))));
        path.add(Point(mesh, mesh.add(Vector3d(0,1,0))));

        std::vector <PathX<Point>> holes;

        MatrixX3i triangles = path.triangulate(holes);

        MatrixX3i expected(2,3);
        expected << 0,1,2, 1,2,3;

        EXPECT_EQ(triangles, expected);

        std::cout << triangles << std::endl;
    }

    TEST_F(PathTest, TriangulateHole)
    {
        Mesh mesh;

        PathX<Point> path(mesh);
        path.add(Point(mesh, mesh.add(Vector3d(0,0,0))));
        path.add(Point(mesh, mesh.add(Vector3d(1,0,0))));
        path.add(Point(mesh, mesh.add(Vector3d(1,1,0))));
        path.add(Point(mesh, mesh.add(Vector3d(0,1,0))));

        PathX<Point> hole(mesh);
        hole.add(Point(mesh, mesh.add(Vector3d(0.1,0.1,0))));
        hole.add(Point(mesh, mesh.add(Vector3d(0.9,0.1,0))));
        hole.add(Point(mesh, mesh.add(Vector3d(0.9,0.9,0))));
        hole.add(Point(mesh, mesh.add(Vector3d(0.1,0.9,0))));

        std::vector <PathX<Point>> holes;

        holes.push_back(hole);

        MatrixX3i triangles = path.triangulate(holes);

        MatrixX3i expected(8,3);
        //expected << 0,1,2, 1,2,3;

        EXPECT_EQ(triangles, expected);

        std::cout << triangles << std::endl;
    }

}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DEFINITION_SLIDE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_SLIDE_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG(ext, slide_, boost::dispatch::abstract_<slide_>);
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, slide_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::slide_,slide);
  }
} }

#include <boost/simd/function/detail/slide.hpp>

namespace boost { namespace simd
{
  template<int N, typename T> BOOST_FORCEINLINE T slide(T const& a0, T const& a1)
  {
    return detail::slider<T,N,cardinal_of<T>::value,(N>=0)>::call(a0,a1);
  }

  template<int N, typename T> BOOST_FORCEINLINE T slide(T const& a0)
{
    return detail::slider<T,N,cardinal_of<T>::value,(N>=0)>::call(a0);
  }
} }

#endif


#include "lelantus_test_fixture.h"

#include "../../sigma/openssl_context.h"
#include "../joinsplit.h"

#include <boost/test/unit_test.hpp>
#include <openssl/rand.h>

namespace lelantus {

class JoinSplitTests : public LelantusTestingSetup
{
public:
    JoinSplitTests()
    {
    }

public:
    std::vector<PrivateCoin> GenerateCoins(std::vector<CAmount> const &amounts) {
        std::vector<PrivateCoin> privs;

        for (auto a : amounts) {
            std::vector<unsigned char> ecdsaKey;
            ecdsaKey.resize(32);

            // Create a key pair
            secp256k1_pubkey pubkey;
            do {
                if (RAND_bytes(ecdsaKey.data(), ecdsaKey.size()) != 1) {
                    throw std::runtime_error("Unable to generate randomness");
                }
            } while (!secp256k1_ec_pubkey_create(
                OpenSSLContext::get_context(), &pubkey, ecdsaKey.data()));

            // Hash the public key in the group to obtain a serial number
            auto serial = PrivateCoin::serialNumberFromSerializedPublicKey(
                OpenSSLContext::get_context(), &pubkey);

            Scalar randomness;
            randomness.randomize();

            privs.emplace_back(params, serial, a, randomness, ecdsaKey, LELANTUS_TX_VERSION_4);
        }

        return privs;
    }

    std::vector<PublicCoin> BuildPublicCoins(std::vector<GroupElement> const &es) {
        std::vector<PublicCoin> pubs;
        pubs.reserve(es.size());

        for (auto const &e : es) {
            pubs.emplace_back(e);
        }

        return pubs;
    }
};

BOOST_FIXTURE_TEST_SUITE(lelantus_joinsplit_tests, JoinSplitTests)

BOOST_AUTO_TEST_CASE(verify)
{
    auto privs = GenerateCoins({1 * COIN, 10 * COIN, 100 * COIN, 99 * COIN});
    std::vector<std::pair<PrivateCoin, uint32_t>> cin = {
        {privs[0], 1},
        {privs[1], 1},
        {privs[2], 2}
    };

    std::map<uint32_t, std::vector<PublicCoin>> anons = {
        {1, BuildPublicCoins(GenerateGroupElements(10))},
        {2, BuildPublicCoins(GenerateGroupElements(10))},
    };

    anons[1][0] = privs[0].getPublicCoin();
    anons[1][1] = privs[1].getPublicCoin();
    anons[2][0] = privs[2].getPublicCoin();

    std::map<uint32_t, uint256> groupBlockHashes = {
        {1, ArithToUint256(1)},
        {2, ArithToUint256(3)},
    };

    // inputs = 111
    // outputs = 0.01(fee) + 99(mint) + 11.99(vout)
    auto vout = 12 * COIN - CENT;
    JoinSplit joinSplit(
        params,
        cin,
        anons,
        vout, // vout
        {privs[3]}, // cout
        CENT, // fee
        groupBlockHashes,
        ArithToUint256(3));

    std::vector<uint32_t> expectedGroupIds = {1, 1, 2};
    BOOST_CHECK(expectedGroupIds == joinSplit.getCoinGroupIds());
    BOOST_CHECK(joinSplit.Verify(anons, {privs[3].getPublicCoin()}, vout, ArithToUint256(3)));
}

BOOST_AUTO_TEST_SUITE_END()

} // namespace lelantus


#ifndef CURVATURE_HH
#define CURVATURE_HH

#include <Eigen/Dense>
#include <MeshFEM/Utilities/MeshConversion.hh>

struct CurvatureInfo {
    // Per vertex curvature quantities
    Eigen::MatrixX3d d_1, d_2;
    Eigen::VectorXd kappa_1, kappa_2;

    Eigen::VectorXd     meanCurvature() const { return 0.5 * (kappa_1 + kappa_2); }
    Eigen::VectorXd gaussianCurvature() const { return kappa_1.array() * kappa_2.array(); }

    const Eigen::MatrixX3d &d(size_t i) const {
        if (i == 0) return d_1;
        if (i == 1) return d_2;
        throw std::runtime_error("Index i out of bounds");
    }

    CurvatureInfo(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F);

    template<class Mesh>
    CurvatureInfo(const Mesh &m) : CurvatureInfo(getV(m), getF(m)) { }
};

#endif /* end of include guard: CURVATURE_HH */


#include <sstream>
#include <Eigen/Geometry>
#include "Geometry.h"

#include <iostream>

using namespace Eigen;

floatOptional Geometry::intersect(const Ray &r) {
    if (transformed) {
        Vector4f newOrigin;
        newOrigin << r.origin(), 1;
        Vector4f newDir;
        newDir << r.direction(), 0;

        newOrigin = inverseModelMatrix * newOrigin;
        newDir = inverseModelMatrix * newDir;
        const Ray transformedR = Ray(newOrigin.head(3), newDir.head(3));
        return objectIntersect(transformedR);
    }
    else {
        return objectIntersect(r);
    }
}

Eigen::Vector3f Geometry::normalAtPoint(const Eigen::Vector3f &p) {
    if (transformed) {
        Vector4f localP, n;
        localP << p, 1;
        localP = inverseModelMatrix * localP;
        n << objectNormal(localP.head(3)), 0;
        n = inverseModelMatrix.transpose() * n;
        return n.head(3).normalized();
    }
    else {
        return objectNormal(p);
    }
}

Eigen::Vector3f Geometry::minVector(const Eigen::Vector3f &a, const Eigen::Vector3f &b) {
    Vector3f minVec;
    minVec.x() = std::min(a.x(), b.x());
    minVec.y() = std::min(a.y(), b.y());
    minVec.z() = std::min(a.z(), b.z());
    return minVec;
}

Eigen::Vector3f Geometry::maxVector(const Eigen::Vector3f &a, const Eigen::Vector3f &b) {
    Vector3f maxVec;
    maxVec.x() = std::max(a.x(), b.x());
    maxVec.y() = std::max(a.y(), b.y());
    maxVec.z() = std::max(a.z(), b.z());
    return maxVec;
}

void Geometry::worldSpaceBoundingBox(Eigen::Vector3f &min, Eigen::Vector3f &max) {
    // clockwise on bottom, then top
    Vector4f v[8];
    // bottom vertices
    v[0] << min, 1;
    v[1] << max.x(), min.y(), min.z(), 1;
    v[2] << max.x(), min.y(), max.z(), 1;
    v[3] << min.x(), min.y(), max.z(), 1;
    // top vertices
    v[4] << min.x(), max.y(), min.z(), 1;
    v[5] << max.x(), max.y(), min.z(), 1;
    v[6] << max, 1;
    v[7] << min.x(), max.y(), max.z(), 1;

    min << INFINITY, INFINITY, INFINITY;
    max << -INFINITY, -INFINITY, -INFINITY;
    for (int i = 0; i < 8; i++) {
        v[i] = modelMatrix * v[i];
        min = minVector(min, v[i].head(3));
        max = maxVector(max, v[i].head(3));
    }
}

void Geometry::boundingBox(Eigen::Vector3f &min, Eigen::Vector3f &max) {
    objectBoundingBox(min, max);
    if (transformed) {
        worldSpaceBoundingBox(min, max);
    }
}

Eigen::Vector3f Geometry::getCenter() {
    Vector4f localCenter;
    localCenter << objectCenter(), 1;
    if (transformed) {
        localCenter = modelMatrix * localCenter;
    }
    return localCenter.head(3);
}

std::string Geometry::to_string() {
    std::stringstream str;
    str << "- Color: " << formatVector(pigment) << "\n";
    str << "- Material:\n";
    str << "  - Ambient: " << finish.ambient << "\n";
    str << "  - Diffuse: " << finish.diffuse << "\n";
    str << "  - Specular: " << finish.specular << "\n";
    str << "  - Roughness: " << finish.roughness << "\n";
    str << "  - Metallic: " << finish.metallic << "\n";
    str << "  - IOR: " << finish.ior << "\n";
    str << "  - Reflection: " << finish.reflection << "\n";
    str << "  - Refraction: " << finish.filter << "\n";

    return str.str();
}

void Geometry::scale(const Eigen::Vector3f s) {
    Transform<float, 3, Projective> sc;
    sc = Scaling(s);
    modelMatrix = sc.matrix() * modelMatrix;
    transformed = true;
}

void Geometry::rotate(const Eigen::Vector3f r) {
    Transform<float, 3, Projective> rot;
    rot = AngleAxisf(r.z() * M_PI / 180, Vector3f::UnitZ())
        * AngleAxisf(r.y() * M_PI / 180, Vector3f::UnitY())
        * AngleAxisf(r.x() * M_PI / 180, Vector3f::UnitX());
    modelMatrix = rot.matrix() * modelMatrix;
    transformed = true;
}

void Geometry::translate(const Eigen::Vector3f t) {
    Matrix4f tr = Matrix4f::Identity();
    tr.col(3).head(3) = t;
    modelMatrix = tr * modelMatrix;
    transformed = true;
}

void Geometry::finalizeTransform() {
    if (transformed) {
        inverseModelMatrix = modelMatrix.inverse();
    }
}

Ray Geometry::getTransformedRay(const Ray &r) {
    if (transformed) {
        Vector4f newOrigin;
        newOrigin << r.origin(), 1;
        Vector4f newDir;
        newDir << r.direction(), 0;

        newOrigin = modelMatrix * newOrigin;
        newDir = modelMatrix * newDir;
        return Ray(newOrigin.head(3), newDir.head(3));
    }
    else {
        return r;
    }
}


#pragma once

#include <mtao/types.hpp>
#include <Eigen/Sparse>

// TODO: implement this instead of interpolated
namespace mandoline {
    template <int SimplexDim, int ChildPolygonVertexCount = Eigen::Dynamic>
struct SimplexChildPolygon {

    mtao::Matrix<double,SimplexDim,ChildPolygonVertexCount> coordinates;
    int parent_id = -1;

    // reads off the vertex info in the format {cut-vertex index, triangle-vertex} = barycentric
    // this format is creates
    template <typename PolygonContainer>
    std::vector<Eigen::Triplet<double>> sparse_matrix_entries(const PolygonIndexContainer &indices, const mtao::ColVectors<double,SimplexDim> &S) const;

    template <typename PolygonContainer>
    std::map<std::array<int, 2>, double> sparse_matrix_entries(const CoordMaskedGeometry<D,PolygonContainer> &pg, const Eigen::MatrixBase<Derived> &S) const {
        return sparse_matrix_entries(pg.indices, S);
    }

    // no default impl exists, others need to impl it
    double volume() const;
};
}// namespace mandoline
#include "mandoline/simplex_child_polygon_impl.hpp"


#include "SecurityPCH.h"
#include <sne/security/cipher/RsaCipher.h>
#ifdef _MSC_VER
#  pragma warning (push)
#  pragma warning (disable: 4100 4189 4244 4267 4702 4706)
#endif
#include <cryptopp/rsa.h>
#include <cryptopp/randpool.h>
#include <cryptopp/filters.h>
#ifdef _MSC_VER
#  pragma warning (pop)
#endif
#include <boost/noncopyable.hpp>

using CryptoPP::byte;

namespace sne { namespace security {

namespace detail {

/**
* @class RsaCipherImpl
*
* RSA cipher implementation
* - https://www.cryptopp.com/wiki/Keys_and_Formats
*/
class RsaCipherImpl : public boost::noncopyable
{
    typedef CryptoPP::RSAES_OAEP_SHA_Decryptor Decryptor;
    typedef CryptoPP::RSAES_OAEP_SHA_Encryptor Encryptor;
    typedef CryptoPP::RSASSA_PKCS1v15_SHA_Signer Signer;
    typedef CryptoPP::RSASSA_PKCS1v15_SHA_Verifier Verifier;

public:
    RsaCipherImpl() {
        initRandomPool(RsaCipher::Seed());
    }

    RsaCipherImpl(size_t privateKeyLength, const RsaCipher::Seed& seed) {
        initRandomPool(seed);

        std::string privKey;
        Decryptor privDecryptor(randomPool_, privateKeyLength);
        CryptoPP::StringSink privKeySink(privKey);
        privDecryptor.AccessKey().DEREncode(privKeySink);
        privKeySink.MessageEnd();

        std::string pubKey;
        Encryptor pubEncryptor(privDecryptor);
        CryptoPP::StringSink pubKeySink(pubKey);
        pubEncryptor.AccessKey().DEREncode(pubKeySink);
        pubKeySink.MessageEnd();

        privateKey_.assign(privKey.c_str(), privKey.length());
        publicKey_.assign(pubKey.c_str(), pubKey.length());
    }

    RsaCipherImpl(const std::string& publicKey, const std::string& privateKey,
        const RsaCipher::Seed& seed) {
        initRandomPool(seed);
        publicKey_ = publicKey;
        privateKey_ = privateKey;
    }

    size_t encrypt(const void* message, size_t messageLength) {
        return encryptOtherPublic(message, messageLength, publicKey_);
    }

    size_t encryptOtherPublic(const void* message, size_t messageLength,
        const std::string& otherPublicKey) {
        CryptoPP::StringSource key(otherPublicKey, true);
        Encryptor encryptor(key);

        assert(messageLength <= encryptor.FixedMaxPlaintextLength());
        assert(encryptor.FixedCiphertextLength() <=
            RsaCipher::maxBufferLength);

        encryptor.Encrypt(randomPool_,
            static_cast<const byte*>(message), messageLength, outputBuffer_);
        return encryptor.FixedCiphertextLength();
    }

    size_t decrypt(const void* encrypted, size_t encryptedLength) {
        CryptoPP::StringSource key(privateKey_, true);
        Decryptor decryptor(key);

        assert(decryptor.FixedMaxPlaintextLength() <=
            RsaCipher::maxBufferLength);

        if (encryptedLength < decryptor.FixedCiphertextLength()) {
            return 0;
        }

        CryptoPP::DecodingResult result =
            decryptor.Decrypt(randomPool_, static_cast<const byte*>(encrypted),
                decryptor.FixedCiphertextLength(), outputBuffer_);

        if (! result.isValidCoding) {
            return 0;
        }

        return result.messageLength;
    }

    size_t sign(const void* message, size_t messageLength) {
        CryptoPP::StringSource key(privateKey_, true);
        Signer signer(key);

        CryptoPP::SignerFilter* filter =
            new CryptoPP::SignerFilter(randomPool_, signer,
                new CryptoPP::ArraySink(outputBuffer_,
                    RsaCipher::maxBufferLength));

        CryptoPP::StringSource source(static_cast<const byte*>(message),
            messageLength, true, filter);
        return signer.SignatureLength();
    }

    bool verify(const void* message, size_t messageLength,
        const void* signature, size_t signatureLength) {
        return verifyOtherPublic(message, messageLength,
            signature, signatureLength, publicKey_);
    }

    bool verifyOtherPublic(const void* message, size_t messageLength,
        const void* signature, size_t signatureLength,
        const std::string& otherPublicKey) {
        CryptoPP::StringSource key(otherPublicKey, true);
        Verifier verifier(key);
        if (signatureLength != verifier.SignatureLength()) {
            return false;
        }

        byte result = 0;
        CryptoPP::SignatureVerificationFilter* filter =
            new CryptoPP::SignatureVerificationFilter(verifier,
                new CryptoPP::ArraySink(&result, 1));
        filter->Put(static_cast<const byte*>(signature),
            verifier.SignatureLength());

        CryptoPP::StringSource source(static_cast<const byte*>(message),
            messageLength, true, filter);
        return result == 1;
    }

    const std::string& getPublicKey() const {
        return publicKey_;
    }

    const std::string& getPrivateKey() const {
        return privateKey_;
    }

    const uint8_t* getOutputBuffer() const {
        return outputBuffer_;
    }

private:
    void initRandomPool(const RsaCipher::Seed& seed) {
        randomPool_.IncorporateEntropy(
            reinterpret_cast<const byte *>(seed.c_str()), seed.length());
    }

private:
    CryptoPP::RandomPool randomPool_;
    std::string publicKey_;
    std::string privateKey_;
    uint8_t outputBuffer_[RsaCipher::maxBufferLength];
};

} // namespace detail

// = RsaCipher

RsaCipher::RsaCipher() :
    impl_(std::make_unique<detail::RsaCipherImpl>())
{
}

RsaCipher::RsaCipher(size_t privateKeyLength, const Seed& seed) :
    impl_(std::make_unique<detail::RsaCipherImpl>(privateKeyLength, seed))
{
}

RsaCipher::RsaCipher(const std::string& publicKey,
    const std::string& privateKey, const Seed& seed) :
    impl_(std::make_unique<detail::RsaCipherImpl>(publicKey, privateKey, seed))
{
}

RsaCipher::~RsaCipher()
{
}

size_t RsaCipher::encrypt(const void* message, size_t messageLength)
{
    try {
        return impl_->encrypt(message, messageLength);
    }
    catch (const std::exception& e) {
        lastErrorMessage_ = e.what();
    }
    return 0;
}

size_t RsaCipher::encryptOtherPublic(const void* message, size_t messageLength,
    const std::string& otherPublicKey)
{
    try {
        return impl_->encryptOtherPublic(message, messageLength, otherPublicKey);
    }
    catch (const std::exception& e) {
        lastErrorMessage_ = e.what();
    }
    return 0;
}

size_t RsaCipher::decrypt(const void* encrypted, size_t encryptedLength)
{
    try {
        return impl_->decrypt(encrypted, encryptedLength);
    }
    catch (const std::exception& e) {
        lastErrorMessage_ = e.what();
    }
    return 0;
}

size_t RsaCipher::sign(const void* message, size_t messageLength)
{
    try {
        return impl_->sign(message, messageLength);
    }
    catch (const std::exception& e) {
        lastErrorMessage_ = e.what();
    }
    return 0;
}

bool RsaCipher::verify(const void* message, size_t messageLength,
    const void* signature, size_t signatureLength)
{
    try {
        return impl_->verify(message, messageLength, signature, signatureLength);
    }
    catch (const std::exception& e) {
        lastErrorMessage_ = e.what();
    }
    return false;
}

bool RsaCipher::verifyOtherPublic(const void* message, size_t messageLength,
    const void* signature, size_t signatureLength,
    const std::string& otherPublicKey)
{
    try {
        return impl_->verifyOtherPublic(message, messageLength,
            signature, signatureLength, otherPublicKey);
    }
    catch (const std::exception& e) {
        lastErrorMessage_ = e.what();
    }
    return false;
}

const std::string& RsaCipher::getPublicKey() const
{
    return impl_->getPublicKey();
}

const std::string& RsaCipher::getPrivateKey() const
{
    return impl_->getPrivateKey();
}

const uint8_t* RsaCipher::getOutputBuffer() const
{
    return impl_->getOutputBuffer();
}

RsaCipher::Seed RsaCipher::generateSeed(size_t length)
{
    //const unsigned int randomValue = time(0);
    //CryptoPP::RandomPool pool;
    //pool.Put(reinterpret_cast<const byte*>(&randomValue), sizeof(randomValue));
    //std::string seed(length, '\0');
    //for (size_t i = 0; i < length; ++i) {
    //    seed[i] = pool.GenerateByte(); // stack이 꼬여버림. 원인 파악 안됨.
    //}
    //return seed;

    std::string seed(length, '\0');
    for (size_t i = 0; i < length; ++i) {
        seed[i] = static_cast<char>(rand());
    }
    return seed;
}

}} // namespace sne { namespace security {


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

#include <votca/csg/map.h>
#include <votca/csg/topology.h>
#include <iostream>
#include <votca/tools/matrix.h>
#include <votca/tools/tokenizer.h>
#include <numeric>
#include <boost/lexical_cast.hpp>

namespace votca { namespace csg {

using namespace std;

Map::~Map()
{
    vector<BeadMap *>::iterator iter;
    for(iter=_maps.begin();iter!=_maps.end();++iter)
        delete (*iter);
    _maps.clear();
}

void Map::Apply()
{
    vector<BeadMap *>::iterator iter;
    for(iter=_maps.begin();iter!=_maps.end();++iter)
        (*iter)->Apply();
}

void Map_Sphere::Initialize(Molecule *in, Bead *out, Property *opts_bead, Property *opts_map) {
    BeadMap::Initialize(in, out, opts_bead, opts_map);

    vector<string> beads;
    vector<double> weights;
    vector<double> fweights;

    // get the beads
    string s(_opts_bead->get("beads").value());
    Tokenizer tok_beads(s, " \n\t");
    tok_beads.ToVector(beads);

    // get vector of weights
    Tokenizer tok_weights(_opts_map->get("weights").value(), " \n\t");
    tok_weights.ConvertToVector<double>(weights);

    // check weather weights and # beads matches
    if (beads.size() != weights.size())
        throw runtime_error(string("number of subbeads in " +
                opts_bead->get("name").as<string>()
                + " and number of weights in map "
                + opts_map->get("name").as<string>() + " do not match"));

    // normalize the weights
    double norm = 1./ std::accumulate(weights.begin(), weights.end(), 0.);

    transform(weights.begin(), weights.end(), weights.begin(), bind2nd(multiplies<double>(), norm));
    // get the d vector if exists or initialize same as weights
    vector<double> d;
    if(_opts_map->exists("d")) {
        Tokenizer tok_weights(_opts_map->get("d").value(), " \n\t");
        tok_weights.ConvertToVector(d);
        // normalize d coefficients
        norm = 1./std::accumulate(d.begin(), d.end(), 0.);
        transform(d.begin(), d.end(), d.begin(), bind2nd(multiplies<double>(), norm));
    } else {
        // initialize force-weights with weights
        d.resize(weights.size());
        copy(weights.begin(), weights.end(), d.begin());
    }

    // check weather number of d coeffs is correct
    if (beads.size() != d.size()) {
        throw runtime_error(string("number of subbeads in " +
            opts_bead->get("name").as<string>()
            + " and number of d-coefficients in map "
            + opts_map->get("name").as<string>() + " do not match"));
    }

    fweights.resize(weights.size());
    // calculate force weights by d_i/w_i
    for(size_t i=0; i<weights.size(); ++i) {
        if(weights[i] == 0 && d[i]!=0) {
            throw runtime_error(
                "A d coefficient is nonzero while weights is zero in mapping "
                + opts_map->get("name").as<string>());
        }
        if(weights[i] != 0)
            fweights[i] = d[i] / weights[i];
        else
            fweights[i] = 0;
    }

    for (size_t i = 0; i < beads.size(); ++i) {
        int iin = in->getBeadByName(beads[i]);
        if (iin < 0)
            throw std::runtime_error(string("mapping error: molecule " + beads[i] + " does not exist"));
        AddElem(in->getBead(iin), weights[i], fweights[i]);
    }
}

void Map_Sphere::Apply()
{
    vector<element_t>::iterator iter;
    vec cg(0., 0., 0.), f(0.,0.,0.), vel(0.,0.,0.);
    bool bPos, bVel, bF;
    bPos=bVel=bF=false;
    _out->ParentBeads().clear();

    // the following is needed for pbc treatment
    Topology *top = _out->getParent();
    double max_dist = 0.5*top->ShortestBoxSize();
    vec r0 = vec(0,0,0);
    string name0;
    int id0;
    if(_matrix.size() > 0) {
        if(_matrix.front()._in->HasPos()) {
            r0=_matrix.front()._in->getPos();
            name0 = _matrix.front()._in->getName();
            id0 = _matrix.front()._in->getId();
        }
    }

    double M = 0;

    for(iter = _matrix.begin(); iter != _matrix.end(); ++iter) {
        Bead *bead = iter->_in;
        _out->ParentBeads().push_back(bead->getId());
        M+=bead->getM();
        if(bead->HasPos()) {
            vec r = top->BCShortestConnection(r0, bead->getPos());
            if(abs(r) > max_dist) {
                cout << r0 << " " << bead->getPos() << endl;
                throw std::runtime_error("coarse-grained bead is bigger than half the box \n (atoms "
                        + name0 + " (id " + boost::lexical_cast<string>(id0+1) + ")" + ", " + bead->getName() + " (id " + boost::lexical_cast<string>(bead->getId()+1) + ")" +  +" , molecule "
                        + boost::lexical_cast<string>(bead->getMolecule()->getId()+1) + ")" );
            }
            cg += (*iter)._weight * (r+r0);
            bPos=true;
        }
        if(bead->HasVel()) {
            vel += (*iter)._weight * bead->getVel();
            bVel = true;
        }
        if(bead->HasF()) {
            f += (*iter)._force_weight * bead->getF();
            bF = true;
        }
    }
    _out->setM(M);
    if(bPos)
        _out->setPos(cg);
    if(bVel)
        _out->setVel(vel);
    if(bF)
        _out->setF(f);
}

/// \todo implement this function
void Map_Ellipsoid::Apply()
{
    vector<element_t>::iterator iter;
    vec cg(0., 0., 0.), c(0., 0., 0.), f(0.,0.,0.), vel(0.,0.,0.);
    matrix m(0.);
     bool bPos, bVel, bF;
    bPos=bVel=bF=false;

    // the following is needed for pbc treatment
    Topology *top = _out->getParent();
    double max_dist = 0.5*top->ShortestBoxSize();
    vec r0 = vec(0,0,0);
    if(_matrix.size() > 0) {
        if(_matrix.front()._in->HasPos()) {
            r0=_matrix.front()._in->getPos();
        }
    }

    int n;
    n = 0;
    _out->ParentBeads().clear();
    for(iter = _matrix.begin(); iter != _matrix.end(); ++iter) {
       Bead *bead = iter->_in;
       _out->ParentBeads().push_back(bead->getId());
       if(bead->HasPos()) {
            vec r = top->BCShortestConnection(r0, bead->getPos());
            if(abs(r) > max_dist)
                throw std::runtime_error("coarse-grained bead is bigger than half the box");
            cg += (*iter)._weight * (r+r0);
            bPos=true;
        }
        if(bead->HasVel() == true) {
            vel += (*iter)._weight * bead->getVel();
            bVel = true;
        }
        if(bead->HasF()) {
            /// \todo fix me, right calculation should be F_i = m_cg / sum(w_i) * sum(w_i/m_i*F_i)
            //f += (*iter)._weight * _in->getBeadF((*iter)._in);
            f += (*iter)._force_weight * bead->getF();
            bF = true;
        }

        if((*iter)._weight>0 && bead->HasPos()) {
            c += bead->getPos();
            n++;
        }
    }

    if(bPos)
        _out->setPos(cg);
    if(bVel)
        _out->setVel(vel);
    if(bF)
        _out->setF(f);

    if(!_matrix[0]._in->HasPos()) {
        _out->setU(vec(1.0,0,0));
        _out->setV(vec(.0,1,0));
        _out->setW(vec(.0,0,1));
        return;
    }

    // calculate the tensor of gyration
    c=c/(double)n;
    for(iter = _matrix.begin(); iter != _matrix.end(); ++iter) {
        if((*iter)._weight == 0) continue;
        Bead *bead = iter->_in;
            vec v = bead->getPos() - c;
            //v = vec(1, 0.5, 0) * 0.*(drand48()-0.5)
            //    + vec(0.5, -1, 0) * (drand48()-0.5)
            //    + vec(0, 0, 1) * (drand48()-0.5);

            //Normalize the tensor with 1/number_of_atoms_per_bead
            m[0][0] += v.getX()*v.getX()/(double)_matrix.size();
            m[0][1] += v.getX()*v.getY()/(double)_matrix.size();
            m[0][2] += v.getX()*v.getZ()/(double)_matrix.size();
            m[1][1] += v.getY()*v.getY()/(double)_matrix.size();
            m[1][2] += v.getY()*v.getZ()/(double)_matrix.size();
            m[2][2] += v.getZ()*v.getZ()/(double)_matrix.size();

    }
    m[1][0] = m[0][1];
    m[2][0] = m[0][2];
    m[2][1] = m[1][2];

    // calculate the eigenvectors
    matrix::eigensystem_t es;
    m.SolveEigensystem(es);

    //vec eigenv1=es.eigenvecs[0];
    //vec eigenv2=es.eigenvecs[1];
    //vec eigenv3=es.eigenvecs[2];

/*    _out->seteigenvec1(eigenv1);
    _out->seteigenvec2(eigenv2);
    _out->seteigenvec3(eigenv3);
  */

    vec u = es.eigenvecs[0];
    vec v = _matrix[1]._in->getPos() - _matrix[0]._in->getPos();
    v.normalize();

    _out->setV(v);

    vec w = _matrix[2]._in->getPos() - _matrix[0]._in->getPos();
    w.normalize();

    if((v^w)*u < 0) u=vec(0.,0.,0.)-u;
    _out->setU(u);

    //write out w
    w=u^v;
    w.normalize();
    _out->setW(w);

    //out.BeadV(_out) = v;

    //out.BeadW(_out) = es.eigenvecs[2];
}

}}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_ARG_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_ARG_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
      @brief  arg generic tag

      Represents the arg function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct arg_ : ext::elementwise_<arg_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<arg_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_arg_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::arg_, Site> dispatching_arg_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::arg_, Site>();
    }
    template<class... Args>
    struct impl_arg_;
  }
  /*!
    Computes the angular orientation of its parameter.

    @par semantic:
    For any given value @c x of floating type @c T:

    @code
    T r = arg(x);
    @endcode

    is equivalent to:

    @code
    T r = (is_nan(x)) ? x :(x < zero) ? pi : zero;
    @endcode

    @par Note:

    Always 0 or \f$\pi\f$ according to the input sign,  or Nan
    is the input is Nan.
    This function is the restriction to real numbers of the complex
    argument function.

    @par Alias

    angle

    @param  a0

    @return      a value of the type T.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::arg_, arg, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::arg_, angle, 1)
} }

#endif


#include <iostream>
#include <sstream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#include <glog/logging.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>

#include "localization/IOUtils.h"
#include "datastruct/Lidar3DStruct.h"
#include "param_config/ParamConfig.h"
#include "sensor_geometry/MultiRayLidar.h"
#include "sensor_geometry/PanoAxis.h"
#include "sensor_geometry/PinholeCamera.h"
#include "sensor_geometry/SensorFactory.h"
#include "util/FileSystemTools.h"
#include "util/LabelmeUtils.h"
#include "Utils.h"

bool projectLaserScan2Camera(const common::PinholeCameraPtr &cam_ptr,
                             const sensorhandler::PointCloudPtr &laser_scan_ptr,
                             const Eigen::Matrix4d &T_c_l, cv::Mat &out_img) {
  if (cam_ptr == nullptr || laser_scan_ptr == nullptr) {
    LOG(ERROR) << "Invalid input pointer!";
    return false;
  }
  if (T_c_l.hasNaN()) {
    LOG(ERROR) << "Invalid transformation matrix!";
    return false;
  }

  int img_width = cam_ptr->resolution().width;
  int img_height = cam_ptr->resolution().height;
  int point_num = laser_scan_ptr->points.size();
  // LOG(INFO) << "Process " << point_num << " laser points";

  cv::Scalar color_start(0, 255, 0);
  cv::Scalar color_end(0,0,255);
  const double max_distant = 10.0;
  double weight_b = (color_end[0] - color_start[0]) / max_distant;
  double weight_g = (color_end[1] - color_start[1]) / max_distant;
  double weight_r = (color_end[2] - color_start[2]) / max_distant;

  int cnt = 0;
  for (int i = 0; i < point_num; ++i) {
    Eigen::Vector3d p_l(laser_scan_ptr->points[i].x,
                        laser_scan_ptr->points[i].y,
                        laser_scan_ptr->points[i].z);

    Eigen::Vector3d p_c = (T_c_l * p_l.homogeneous()).hnormalized();
    if (p_c[2] < 0)
        continue;

    Eigen::Vector2d p_img;
    cam_ptr->project(p_c, &p_img);
    if (p_img[0] < 0 || p_img[0] >= img_width || p_img[1] < 0 || p_img[1] >= img_height)
      continue;

    // draw color square as projected laser point
    {
      double color_scale = p_c[2];
      cv::Scalar color(0, 255 + weight_g*color_scale , weight_r * color_scale);
      cv::Point p(p_img[0], p_img[1]);
      cv::circle(out_img, p, 1, color, -1);
      cnt ++;
    }
  }

  LOG(INFO) << "Project " << cnt << " laser points into camera.";
  return true;
}

// this exe visualize lidar2cam extrinsic by backprojecting laser point cloud into image plane

int main(int argc, char **argv) {
  if (argc < 3) {
    LOG(ERROR)
        << "Usage: ./test_lidar2tehce_evaluation [config.yaml] [data_folder] \n \
    \t [config.yaml] is backpack configuration file. \n \
    \t [data_folder] is dataset fodler for evaluation.  \n";
    return -1;
  }

  std::string config_file(argv[1]);
  std::string data_folder(argv[2]);
  std::string lidar_scan_folder = common::concatenateFolderAndFileName(data_folder, "scans");
  std::string teche_image_folder = common::concatenateFolderAndFileName(data_folder, "undist_pano_images");

  if (!common::fileExists(config_file)) {
    LOG(ERROR) << " Configuration file does not exit, quit...\n";
    return -1;
  }
  if (!common::pathExists(lidar_scan_folder)) {
    LOG(ERROR) << " Lidar folder does not exit, quit...\n";
    return -1;
  }
  if (!common::pathExists(teche_image_folder)) {
    LOG(ERROR) << " Teche image folder does not exit, quit...\n";
    return -1;
  }

  common::ParamConfig::setParameterFile(config_file);

  // teche cameras
  std::vector<common::PinholeCameraPtr> v_teche_cameras_ptr;
  for (size_t i = 0; i < 4; ++i) {
    common::PinholeCameraPtr cam_ptr = std::make_shared<common::PinholeCamera>("teche_" + std::to_string(i));
    if (cam_ptr != nullptr) {
      v_teche_cameras_ptr.push_back(cam_ptr);
    } else {
      LOG(ERROR) << "FAIL to construct teche camera from configuration file!";
      return -1;
    }
  }

  common::MultiRayLidarPtr vertical_lidar_ptr = common::SensorFactory::createMultiRayLidar("vertical_lidar");
  common::MultiRayLidarPtr horizontal_lidar_ptr = common::SensorFactory::createMultiRayLidar("horizon_lidar");

  // pano
  common::PanoAxisPtr pano_axis_ptr = common::SensorFactory::createPanoAxis("teche_pano_axis");

  Eigen::Matrix4d T_base_l0 = horizontal_lidar_ptr->extrinsics();
  Eigen::Matrix4d T_base_l1 = vertical_lidar_ptr->extrinsics();
  Eigen::Matrix4d T_base_pano_axis = pano_axis_ptr->extrinsics();
  std::string horizon_pc_filepath = common::concatenateFolderAndFileName(lidar_scan_folder, "0_0.ply");
  std::string vertical_pc_filepath = common::concatenateFolderAndFileName(lidar_scan_folder, "1_0.ply");

  PointCloudPtr vertical_sweep_cloud_ptr(new PointCloud);
  PointCloudPtr horizon_sweep_cloud_ptr(new PointCloud);

  if (!common::fileExists(vertical_pc_filepath)) {
    LOG(ERROR) << "cannot find pc file: " << vertical_pc_filepath;
    return -1;
  }
  if (!common::fileExists(horizon_pc_filepath)) {
    LOG(ERROR) << "cannot find pc file: " << vertical_pc_filepath;
    return -1;
  }

  pcl::io::loadPLYFile(vertical_pc_filepath, *vertical_sweep_cloud_ptr);
  pcl::io::loadPLYFile(horizon_pc_filepath, *horizon_sweep_cloud_ptr);

  double perpendicular_angle = 0.0;
  Eigen::Vector3d lidar0_corner_point = calcCornerPoint(lidar_scan_folder, "lidar0", perpendicular_angle);
  Eigen::Vector3d lidar1_corner_point = calcCornerPoint(lidar_scan_folder, "lidar1", perpendicular_angle);
  if(lidar0_corner_point.norm() > 1e3 && lidar1_corner_point.norm() > 1e3){
    LOG(ERROR) << "Invalid corner points! Please check input plane cloud";
    return -1;
  }

  std::vector<std::string> v_img_paths;
  std::vector<std::string> paths;
  paths.push_back(teche_image_folder);
  common::getFileLists(paths, true, "jpg", &v_img_paths);

  int img_num = v_img_paths.size();
  for (int i = 0; i < img_num; ++i) {
    std::string &img_path = v_img_paths[i];
    std::string img_folder, img_name;
    common::splitPathAndFilename(img_path, &img_folder, &img_name);

    int cam_index = img_name[0] - 48;
    if (cam_index < 1 || cam_index > 4) {
      LOG(ERROR) << "Invalid teche images!";
      continue;
    }

    // read corner coordinates on image
    std::string corner_file_name = img_name.substr(0, img_name.size()-4) + ".json";
    std::string corner_file_path = common::concatenateFolderAndFileName(teche_image_folder, corner_file_name);
    // skip images that corner is unsivisable
    if (!common::fileExists(corner_file_path)) {
      LOG(WARNING) << "cannot find labelme file: " << corner_file_path;
      continue;
    }
    std::vector<Eigen::Vector2d> v_corners;
    common::readLabelMeResult(corner_file_path, v_corners);

    cv::Mat out_img = cv::imread(img_path, cv::IMREAD_UNCHANGED);
    if(out_img.empty()){
        LOG(ERROR) << "Fail to read image  " << img_path;
        return -1;
    }

    common::PinholeCameraPtr teche_cam_ptr = v_teche_cameras_ptr[cam_index - 1];
    Eigen::Matrix4d T_pano_axis_c = teche_cam_ptr->extrinsics();
    Eigen::Matrix3d intrinsic = teche_cam_ptr->cameraMatrix();

    // project horizontal sweeps into camera
    // Eigen::Matrix4d T_l0_c = (T_base_l0.inverse()) * T_base_pano_axis * T_pano_axis_c;
    Eigen::Matrix4d T_l0_c = (T_base_l0.inverse()) * T_pano_axis_c;
    std::cout << "T_l0_c" << cam_index-1 << " : " << T_l0_c << "\n";
    Eigen::Matrix4d T_c_l0 = T_l0_c.inverse();
    bool sts = projectLaserScan2Camera(teche_cam_ptr, horizon_sweep_cloud_ptr,
                                       T_c_l0, out_img);
    if (!sts) {
      LOG(ERROR) << "Fail to project laser point into image!";
      return -1;
    }

    double reproject_err = calcCornerPointReprojectError(lidar0_corner_point, T_c_l0, intrinsic, v_corners[0], out_img);
    LOG(INFO) << "Reproject error of Corner extract from lidar0 sweeps : " << reproject_err;
    std::string save_img_path = common::concatenateFolderAndFileName(img_folder, "eva_lidar0_" + img_name);
    cv::imwrite(save_img_path, out_img);

    if(lidar1_corner_point.norm() > 1e6){
      LOG(WARNING) << " Cannot find corner point in Lidara1";
      continue;
    }
    out_img = cv::imread(img_path, cv::IMREAD_UNCHANGED);
    // project vertical sweeps into camera
    // Eigen::Matrix4d T_l1_c = (T_base_l1.inverse()) * T_base_pano_axis * T_pano_axis_c;
    Eigen::Matrix4d T_l1_c = (T_base_l1.inverse()) * T_pano_axis_c;
    // T_l1_c << -0.305399684008826,  -0.951924037185201,  0.0239094214973372,   0.640044537317772,
    // -0.0215547405557131, -0.0181916807166895,  -0.999602148813356,  -0.075209496121445,
    //   0.951980265639412,  -0.305793541759355, -0.0149627421078072,  -0.039993200341565,
    //   0, 0, 0, 1;
    // T_l1_c <<  0.298707809405327,  -0.953858732908545, -0.0304493391451621,   0.662864589002141,
    // 0.0389234600194619, -0.0197027160624481,   0.999047930401678,  0.0639108552539182,
    // -0.953550527691314,  -0.299608612416047,  0.0312421271002314, -0.0762731696475622,
    // 0, 0, 0, 1;
    std::cout << "T_l1_c" << cam_index-1 << " : " << T_l1_c << "\n";
    Eigen::Matrix4d T_c_l1 = T_l1_c.inverse();
    sts = projectLaserScan2Camera(teche_cam_ptr, vertical_sweep_cloud_ptr,
                                  T_c_l1, out_img);
    if (!sts) {
      LOG(ERROR) << "Fail to project laser point into image!";
      return -1;
    }

    reproject_err = calcCornerPointReprojectError(lidar1_corner_point, T_c_l1, intrinsic, v_corners[0], out_img);
    LOG(INFO) << "Reproject error of Corner extract from lidar1 sweeps : " << reproject_err;
    save_img_path = common::concatenateFolderAndFileName(img_folder, "eva_lidar1_" + img_name);
    cv::imwrite(save_img_path, out_img);

    Eigen::Matrix4d T_l0_l1 = Eigen::Matrix4d::Identity();
    T_l0_l1 = T_l0_c * T_l1_c.inverse();
    std::cout << T_l0_l1(0,0) << ", " << T_l0_l1(0,1) << ", " << T_l0_l1(0,2) << ", " << T_l0_l1(0,3) << "\n";
    std::cout << T_l0_l1(1,0) << ", " << T_l0_l1(1,1) << ", " << T_l0_l1(1,2) << ", " << T_l0_l1(1,3) << "\n";
    std::cout << T_l0_l1(2,0) << ", " << T_l0_l1(2,1) << ", " << T_l0_l1(2,2) << ", " << T_l0_l1(2,3) << "\n";
    std::cout << T_l0_l1(3,0) << ", " << T_l0_l1(3,1) << ", " << T_l0_l1(3,2) << ", " << T_l0_l1(3,3) << "\n";
  }

  return 0;
}