/*
 * Copyright 2020 Tier IV, Inc. All rights reserved.
 *
 * 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 <chrono>

#include <boost/optional.hpp>

#include <nav_msgs/MapMetaData.h>
#include <ros/console.h>
#include <tf2/utils.h>

#include <opencv2/core.hpp>

#include <osqp_interface/osqp_interface.h>

#include "obstacle_avoidance_planner/eb_path_optimizer.h"
#include "obstacle_avoidance_planner/mpt_optimizer.h"
#include "obstacle_avoidance_planner/process_cv.h"
#include "obstacle_avoidance_planner/util.h"
#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.h"
#include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.h"

MPTOptimizer::MPTOptimizer(
  const bool is_showing_debug_info, const QPParam & qp_param, const TrajectoryParam & traj_param,
  const ConstrainParam & constraint_param, const VehicleParam & vehicle_param,
  const MPTParam & mpt_param)
: is_showing_debug_info_(is_showing_debug_info)
{
  qp_param_ptr_ = std::make_unique<QPParam>(qp_param);
  traj_param_ptr_ = std::make_unique<TrajectoryParam>(traj_param);
  constraint_param_ptr_ = std::make_unique<ConstrainParam>(constraint_param);
  vehicle_param_ptr_ = std::make_unique<VehicleParam>(vehicle_param);
  mpt_param_ptr_ = std::make_unique<MPTParam>(mpt_param);

  vehicle_model_ptr_ = std::make_unique<KinematicsBicycleModel>(
    vehicle_param_ptr_->wheelbase, vehicle_param_ptr_->max_steer_rad,
    vehicle_param_ptr_->steer_tau);
}

MPTOptimizer::~MPTOptimizer() {}

boost::optional<MPTTrajs> MPTOptimizer::getModelPredictiveTrajectory(
  const bool enable_avoidance,
  const std::vector<autoware_planning_msgs::TrajectoryPoint> & smoothed_points,
  const std::vector<autoware_planning_msgs::PathPoint> & path_points,
  const std::unique_ptr<Trajectories> & prev_trajs, const CVMaps & maps,
  const geometry_msgs::Pose & ego_pose, DebugData * debug_data)
{
  auto t_start1 = std::chrono::high_resolution_clock::now();
  if (smoothed_points.empty()) {
    ROS_INFO_COND(is_showing_debug_info_, "return boost::none since smoothed_points is empty");
    return boost::none;
  }

  geometry_msgs::Pose origin_pose = smoothed_points.front().pose;
  if (prev_trajs) {
    const int prev_nearest_idx = util::getNearestIdx(
      prev_trajs->model_predictive_trajectory, smoothed_points.front().pose.position);
    origin_pose = prev_trajs->model_predictive_trajectory.at(prev_nearest_idx).pose;
  }
  std::vector<ReferencePoint> ref_points = getReferencePoints(
    origin_pose, ego_pose, smoothed_points, path_points, prev_trajs, maps, debug_data);
  if (ref_points.empty()) {
    ROS_INFO_COND(is_showing_debug_info_, "return boost::none since ref_points is empty");
    return boost::none;
  }

  const auto mpt_matrix = generateMPTMatrix(ref_points, path_points, prev_trajs);
  if (!mpt_matrix) {
    ROS_INFO_COND(is_showing_debug_info_, "return boost::none since matrix has nan");
    return boost::none;
  }

  const auto optimized_control_variables = executeOptimization(
    enable_avoidance, mpt_matrix.get(), ref_points, path_points, maps, debug_data);
  if (!optimized_control_variables) {
    ROS_INFO_COND(is_showing_debug_info_, "return boost::none since could not solve qp");
    return boost::none;
  }

  const auto mpt_points =
    getMPTPoints(ref_points, optimized_control_variables.get(), mpt_matrix.get(), smoothed_points);

  auto t_end1 = std::chrono::high_resolution_clock::now();
  float elapsed_ms1 = std::chrono::duration<float, std::milli>(t_end1 - t_start1).count();
  ROS_INFO_COND(is_showing_debug_info_, "MPT time: = %f [ms]", elapsed_ms1);
  MPTTrajs mpt_trajs;
  mpt_trajs.mpt = mpt_points;
  mpt_trajs.ref_points = ref_points;
  return mpt_trajs;
}

std::vector<ReferencePoint> MPTOptimizer::getReferencePoints(
  const geometry_msgs::Pose & origin_pose, const geometry_msgs::Pose & ego_pose,
  const std::vector<autoware_planning_msgs::TrajectoryPoint> & points,
  const std::vector<autoware_planning_msgs::PathPoint> & path_points,
  const std::unique_ptr<Trajectories> & prev_trajs, const CVMaps & maps,
  DebugData * debug_data) const
{
  const auto ref_points =
    convertToReferencePoints(points, path_points, prev_trajs, ego_pose, maps, debug_data);

  const int begin_idx = util::getNearestPointIdx(ref_points, origin_pose.position);
  const auto first_it = ref_points.begin() + begin_idx;
  int num_points =
    std::min((int)ref_points.size() - 1 - begin_idx, traj_param_ptr_->num_sampling_points);
  num_points = std::max(num_points, 0);
  std::vector<ReferencePoint> truncated_points(first_it, first_it + num_points);
  calcInitialState(&truncated_points, origin_pose);
  return truncated_points;
}

std::vector<ReferencePoint> MPTOptimizer::convertToReferencePoints(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> & points,
  const std::vector<autoware_planning_msgs::PathPoint> & path_points,
  const std::unique_ptr<Trajectories> & prev_trajs, const geometry_msgs::Pose & ego_pose,
  const CVMaps & maps, DebugData * debug_data) const
{
  const auto interpolated_points =
    util::getInterpolatedPoints(points, traj_param_ptr_->delta_arc_length_for_mpt_points);
  if (interpolated_points.empty()) {
    return std::vector<ReferencePoint>{};
  }

  auto reference_points = getBaseReferencePoints(interpolated_points, prev_trajs, debug_data);

  calcOrientation(&reference_points);
  calcVelocity(&reference_points, points);
  calcCurvature(&reference_points);
  calcArcLength(&reference_points);
  calcExtraPoints(&reference_points);
  return reference_points;
}

void MPTOptimizer::calcOrientation(std::vector<ReferencePoint> * ref_points) const
{
  if (!ref_points) {
    return;
  }
  for (int i = 0; i < ref_points->size(); i++) {
    if (ref_points->at(i).fix_state) {
      continue;
    }

    if (i > 0) {
      ref_points->at(i).q =
        util::getQuaternionFromPoints(ref_points->at(i).p, ref_points->at(i - 1).p);
    } else if (i == 0 && ref_points->size() > 1) {
      ref_points->at(i).q =
        util::getQuaternionFromPoints(ref_points->at(i + 1).p, ref_points->at(i).p);
    }
    ref_points->at(i).yaw = tf2::getYaw(ref_points->at(i).q);
  }
}

void MPTOptimizer::calcVelocity(
  std::vector<ReferencePoint> * ref_points,
  const std::vector<autoware_planning_msgs::TrajectoryPoint> & points) const
{
  if (!ref_points) {
    return;
  }
  for (int i = 0; i < ref_points->size(); i++) {
    ref_points->at(i).v = points[util::getNearestIdx(points, ref_points->at(i).p)].twist.linear.x;
  }
}

void MPTOptimizer::calcCurvature(std::vector<ReferencePoint> * ref_points) const
{
  if (!ref_points) {
    return;
  }

  int num_points = static_cast<int>(ref_points->size());

  /* calculate curvature by circle fitting from three points */
  geometry_msgs::Point p1, p2, p3;
  int max_smoothing_num = static_cast<int>(std::floor(0.5 * (num_points - 1)));
  int L = std::min(mpt_param_ptr_->num_curvature_sampling_points, max_smoothing_num);
  for (int i = L; i < num_points - L; ++i) {
    p1 = ref_points->at(i - L).p;
    p2 = ref_points->at(i).p;
    p3 = ref_points->at(i + L).p;
    double den = std::max(
      util::calculate2DDistance(p1, p2) * util::calculate2DDistance(p2, p3) *
        util::calculate2DDistance(p3, p1),
      0.0001);
    const double curvature =
      2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den;
    if (!ref_points->at(i).fix_state) {
      ref_points->at(i).k = curvature;
    }
  }

  /* first and last curvature is copied from next value */
  for (int i = 0; i < std::min(L, num_points); ++i) {
    if (!ref_points->at(i).fix_state) {
      ref_points->at(i).k = ref_points->at(std::min(L, num_points - 1)).k;
    }
    if (!ref_points->at(num_points - i - 1).fix_state) {
      ref_points->at(num_points - i - 1).k = ref_points->at(std::max(num_points - L - 1, 0)).k;
    }
  }
}

void MPTOptimizer::calcArcLength(std::vector<ReferencePoint> * ref_points) const
{
  if (!ref_points) {
    return;
  }
  for (int i = 0; i < ref_points->size(); i++) {
    if (i > 0) {
      geometry_msgs::Point a, b;
      a = ref_points->at(i).p;
      b = ref_points->at(i - 1).p;
      ref_points->at(i).s = ref_points->at(i - 1).s + util::calculate2DDistance(a, b);
    }
  }
}

void MPTOptimizer::calcExtraPoints(std::vector<ReferencePoint> * ref_points) const
{
  if (!ref_points) {
    return;
  }
  for (int i = 0; i < ref_points->size(); i++) {
    const double p1_target_s = ref_points->at(i).s + mpt_param_ptr_->top_point_dist_from_base_link;
    const int nearest_p1_idx = util::getNearestIdx(*ref_points, p1_target_s, i);
    ref_points->at(i).top_pose.position = ref_points->at(nearest_p1_idx).p;

    const double p2_target_s = ref_points->at(i).s + mpt_param_ptr_->mid_point_dist_from_base_link;
    const int nearest_p2_idx = util::getNearestIdx(*ref_points, p2_target_s, i);
    ref_points->at(i).mid_pose.position = ref_points->at(nearest_p2_idx).p;

    ref_points->at(i).top_pose.orientation =
      util::getQuaternionFromPoints(ref_points->at(i).top_pose.position, ref_points->at(i).p);
    if (i == nearest_p1_idx && i > 0) {
      ref_points->at(i).top_pose.orientation =
        util::getQuaternionFromPoints(ref_points->at(i).p, ref_points->at(i - 1).p);
    } else if (i == nearest_p1_idx) {
      ref_points->at(i).top_pose.orientation = ref_points->at(i).q;
    }
    ref_points->at(i).mid_pose.orientation = ref_points->at(i).top_pose.orientation;

    const double delta_yaw =
      tf2::getYaw(ref_points->at(i).top_pose.orientation) - ref_points->at(i).yaw;
    const double norm_delta_yaw = util::normalizeRadian(delta_yaw);
    ref_points->at(i).delta_yaw_from_p1 = norm_delta_yaw;
    ref_points->at(i).delta_yaw_from_p2 = ref_points->at(i).delta_yaw_from_p1;
  }
}

void MPTOptimizer::calcInitialState(
  std::vector<ReferencePoint> * ref_points, const geometry_msgs::Pose & origin_pose) const
{
  if (!ref_points) {
    return;
  }
  if (ref_points->empty()) {
    return;
  }

  boost::optional<int> begin_idx = boost::none;
  double accum_s = 0;
  for (int i = 0; i < ref_points->size(); i++) {
    double ds = 0.0;
    if (i < ref_points->size() - 1) {
      ds = ref_points->at(i + 1).s - ref_points->at(i).s;
    } else if (i == ref_points->size() - 1 && ref_points->size() > 1) {
      ds = ref_points->at(i).s - ref_points->at(i - 1).s;
    }
    accum_s += ds;
    constexpr double max_s_for_prev_point = 3;
    if (accum_s < max_s_for_prev_point && ref_points->at(i).fix_state) {
      begin_idx = i;
      break;
    }
  }

  Eigen::VectorXd initial_state;
  if (begin_idx) {
    *ref_points =
      std::vector<ReferencePoint>{ref_points->begin() + begin_idx.get(), ref_points->end()};
    ref_points->front().optimized_state = ref_points->front().fix_state.get();
  } else {
    ref_points->front().optimized_state = getState(origin_pose, ref_points->front());
  }
}

/*
 * predict equation: Xec = Aex * x0 + Bex * Uex + Wex
 * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Urefex) + Uex' * R2ex * Uex
 * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...])
 */
boost::optional<MPTMatrix> MPTOptimizer::generateMPTMatrix(
  const std::vector<ReferencePoint> & reference_points,
  const std::vector<autoware_planning_msgs::PathPoint> & path_points,
  const std::unique_ptr<Trajectories> & prev_trajs) const
{
  const int N = reference_points.size();
  const int DIM_X = vehicle_model_ptr_->getDimX();
  const int DIM_U = vehicle_model_ptr_->getDimU();
  const int DIM_Y = vehicle_model_ptr_->getDimY();

  Eigen::MatrixXd Aex = Eigen::MatrixXd::Zero(DIM_X * N, DIM_X);      //state transition
  Eigen::MatrixXd Bex = Eigen::MatrixXd::Zero(DIM_X * N, DIM_U * N);  // control input
  Eigen::MatrixXd Wex = Eigen::MatrixXd::Zero(DIM_X * N, 1);          //
  Eigen::MatrixXd Cex = Eigen::MatrixXd::Zero(DIM_Y * N, DIM_X * N);
  Eigen::MatrixXd Qex = Eigen::MatrixXd::Zero(DIM_Y * N, DIM_Y * N);
  Eigen::MatrixXd R1ex = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N);
  Eigen::MatrixXd R2ex = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N);
  Eigen::MatrixXd Urefex = Eigen::MatrixXd::Zero(DIM_U * N, 1);

  /* weight matrix depends on the vehicle model */
  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y);
  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_U, DIM_U);
  Eigen::MatrixXd Q_adaptive = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y);
  Eigen::MatrixXd R_adaptive = Eigen::MatrixXd::Zero(DIM_U, DIM_U);
  Q(0, 0) = mpt_param_ptr_->lat_error_weight;
  if (!prev_trajs) {
    const double initial_lat_error_weight = mpt_param_ptr_->lat_error_weight * 1000;
    Q(0, 0) = initial_lat_error_weight;
  }
  Q(1, 1) = mpt_param_ptr_->yaw_error_weight;
  R(0, 0) = mpt_param_ptr_->steer_input_weight;

  Eigen::MatrixXd Ad(DIM_X, DIM_X);
  Eigen::MatrixXd Bd(DIM_X, DIM_U);
  Eigen::MatrixXd Wd(DIM_X, 1);
  Eigen::MatrixXd Cd(DIM_Y, DIM_X);
  Eigen::MatrixXd Uref(DIM_U, 1);

  geometry_msgs::Pose last_ref_pose;
  last_ref_pose.position = reference_points.back().p;
  last_ref_pose.orientation = reference_points.back().q;
  const auto last_extended_point = util::getLastExtendedPoint(
    path_points.back(), last_ref_pose, traj_param_ptr_->delta_yaw_threshold_for_closest_point,
    traj_param_ptr_->max_dist_for_extending_end_point);

  /* predict dynamics for N times */
  for (int i = 0; i < N; ++i) {
    const double ref_k = reference_points[i].k;
    double ds = 0.0;
    if (i < N - 1) {
      ds = reference_points[i + 1].s - reference_points[i].s;
    } else if (i == N - 1 && N > 1) {
      ds = reference_points[i].s - reference_points[i - 1].s;
    }

    /* get discrete state matrix A, B, C, W */
    vehicle_model_ptr_->setCurvature(ref_k);
    vehicle_model_ptr_->calculateDiscreteMatrix(&Ad, &Bd, &Cd, &Wd, ds);

    Q_adaptive = Q;
    R_adaptive = R;
    if (i == N - 1 && last_extended_point) {
      Q_adaptive(0, 0) = mpt_param_ptr_->terminal_path_lat_error_weight;
      Q_adaptive(1, 1) = mpt_param_ptr_->terminal_path_yaw_error_weight;
    } else if (i == N - 1) {
      Q_adaptive(0, 0) = mpt_param_ptr_->terminal_lat_error_weight;
      Q_adaptive(1, 1) = mpt_param_ptr_->terminal_yaw_error_weight;
    }

    /* update mpt matrix */
    int idx_x_i = i * DIM_X;
    int idx_x_i_prev = (i - 1) * DIM_X;
    int idx_u_i = i * DIM_U;
    int idx_y_i = i * DIM_Y;
    if (i == 0) {
      Aex.block(0, 0, DIM_X, DIM_X) = Ad;
      Bex.block(0, 0, DIM_X, DIM_U) = Bd;
      Wex.block(0, 0, DIM_X, 1) = Wd;
    } else {
      Aex.block(idx_x_i, 0, DIM_X, DIM_X) = Ad * Aex.block(idx_x_i_prev, 0, DIM_X, DIM_X);
      for (int j = 0; j < i; ++j) {
        int idx_u_j = j * DIM_U;
        Bex.block(idx_x_i, idx_u_j, DIM_X, DIM_U) =
          Ad * Bex.block(idx_x_i_prev, idx_u_j, DIM_X, DIM_U);
      }
      Wex.block(idx_x_i, 0, DIM_X, 1) = Ad * Wex.block(idx_x_i_prev, 0, DIM_X, 1) + Wd;
    }
    Bex.block(idx_x_i, idx_u_i, DIM_X, DIM_U) = Bd;
    Cex.block(idx_y_i, idx_x_i, DIM_Y, DIM_X) = Cd;
    Qex.block(idx_y_i, idx_y_i, DIM_Y, DIM_Y) = Q_adaptive;
    R1ex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive;

    /* get reference input (feed-forward) */
    vehicle_model_ptr_->calculateReferenceInput(&Uref);
    if (std::fabs(Uref(0, 0)) < mpt_param_ptr_->zero_ff_steer_angle * M_PI / 180.0) {
      Uref(0, 0) = 0.0;  // ignore curvature noise
    }
    Urefex.block(i * DIM_U, 0, DIM_U, 1) = Uref;
  }

  addSteerWeightR(&R1ex, reference_points);

  MPTMatrix m;
  m.Aex = Aex;
  m.Bex = Bex;
  m.Wex = Wex;
  m.Cex = Cex;
  m.Qex = Qex;
  m.R1ex = R1ex;
  m.R2ex = R2ex;
  m.Urefex = Urefex;
  if (
    m.Aex.array().isNaN().any() || m.Bex.array().isNaN().any() || m.Cex.array().isNaN().any() ||
    m.Wex.array().isNaN().any() || m.Qex.array().isNaN().any() || m.R1ex.array().isNaN().any() ||
    m.R2ex.array().isNaN().any() || m.Urefex.array().isNaN().any()) {
    ROS_WARN("[Avoidance] MPT matrix includes NaN.");
    return boost::none;
  }
  return m;
}

void MPTOptimizer::addSteerWeightR(
  Eigen::MatrixXd * R, const std::vector<ReferencePoint> & ref_points) const
{
  const int N = ref_points.size();
  constexpr double DT = 0.1;
  constexpr double ctrl_period = 0.03;

  /* add steering rate : weight for (u(i) - u(i-1) / dt )^2 */
  for (int i = 0; i < N - 1; ++i) {
    const double steer_rate_r = mpt_param_ptr_->steer_rate_weight / (DT * DT);
    (*R)(i + 0, i + 0) += steer_rate_r;
    (*R)(i + 1, i + 0) -= steer_rate_r;
    (*R)(i + 0, i + 1) -= steer_rate_r;
    (*R)(i + 1, i + 1) += steer_rate_r;
  }
  if (N > 1) {
    // steer rate i = 0
    (*R)(0, 0) += mpt_param_ptr_->steer_rate_weight / (ctrl_period * ctrl_period);
  }

  /* add steering acceleration : weight for { (u(i+1) - 2*u(i) + u(i-1)) / dt^2 }^2 */
  const double steer_acc_r = mpt_param_ptr_->steer_acc_weight / std::pow(DT, 4);
  const double steer_acc_r_cp1 = mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 3) * ctrl_period);
  const double steer_acc_r_cp2 =
    mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 2) * std::pow(ctrl_period, 2));
  const double steer_acc_r_cp4 = mpt_param_ptr_->steer_acc_weight / std::pow(ctrl_period, 4);
  for (int i = 1; i < N - 1; ++i) {
    (*R)(i - 1, i - 1) += (steer_acc_r);
    (*R)(i - 1, i + 0) += (steer_acc_r * -2.0);
    (*R)(i - 1, i + 1) += (steer_acc_r);
    (*R)(i + 0, i - 1) += (steer_acc_r * -2.0);
    (*R)(i + 0, i + 0) += (steer_acc_r * 4.0);
    (*R)(i + 0, i + 1) += (steer_acc_r * -2.0);
    (*R)(i + 1, i - 1) += (steer_acc_r);
    (*R)(i + 1, i + 0) += (steer_acc_r * -2.0);
    (*R)(i + 1, i + 1) += (steer_acc_r);
  }
  if (N > 1) {
    // steer acc i = 1
    (*R)(0, 0) += steer_acc_r * 1.0 + steer_acc_r_cp2 * 1.0 + steer_acc_r_cp1 * 2.0;
    (*R)(1, 0) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0;
    (*R)(0, 1) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0;
    (*R)(1, 1) += steer_acc_r * 1.0;
    // steer acc i = 0
    (*R)(0, 0) += steer_acc_r_cp4 * 1.0;
  }
}

void MPTOptimizer::addSteerWeightF(Eigen::VectorXd * f) const
{
  constexpr double DT = 0.1;
  constexpr double ctrl_period = 0.03;
  constexpr double raw_steer_cmd_prev = 0;
  constexpr double raw_steer_cmd_pprev = 0;

  if (f->rows() < 2) {
    return;
  }

  // steer rate for i = 0
  (*f)(0) += -2.0 * mpt_param_ptr_->steer_rate_weight / (std::pow(DT, 2)) * 0.5;

  // const double steer_acc_r = mpt_param_.weight_steer_acc / std::pow(DT, 4);
  const double steer_acc_r_cp1 = mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 3) * ctrl_period);
  const double steer_acc_r_cp2 =
    mpt_param_ptr_->steer_acc_weight / (std::pow(DT, 2) * std::pow(ctrl_period, 2));
  const double steer_acc_r_cp4 = mpt_param_ptr_->steer_acc_weight / std::pow(ctrl_period, 4);

  // steer acc  i = 0
  (*f)(0) += ((-2.0 * raw_steer_cmd_prev + raw_steer_cmd_pprev) * steer_acc_r_cp4) * 0.5;

  // steer acc for i = 1
  (*f)(0) += (-2.0 * raw_steer_cmd_prev * (steer_acc_r_cp1 + steer_acc_r_cp2)) * 0.5;
  (*f)(1) += (2.0 * raw_steer_cmd_prev * steer_acc_r_cp1) * 0.5;
}

boost::optional<Eigen::VectorXd> MPTOptimizer::executeOptimization(
  const bool enable_avoidance, const MPTMatrix & m, const std::vector<ReferencePoint> & ref_points,
  const std::vector<autoware_planning_msgs::PathPoint> & path_points, const CVMaps & maps,
  DebugData * debug_data)
{
  auto t_start1 = std::chrono::high_resolution_clock::now();

  const auto x0 = ref_points.front().optimized_state;
  ObjectiveMatrix obj_m = getObjectiveMatrix(x0, m);
  ConstraintMatrix const_m =
    getConstraintMatrix(enable_avoidance, x0, m, maps, ref_points, path_points, debug_data);

  osqp_solver_ptr_ = std::make_unique<osqp::OSQPInterface>(
    obj_m.hessian, const_m.linear, obj_m.gradient, const_m.lower_bound, const_m.upper_bound,
    1.0e-3);
  osqp_solver_ptr_->updateEpsRel(1.0e-3);
  const auto result = osqp_solver_ptr_->optimize();

  int solution_status = std::get<3>(result);
  if (solution_status != 1) {
    util::logOSQPSolutionStatus(solution_status);
    return boost::none;
  }

  std::vector<double> result_vec = std::get<0>(result);
  const Eigen::VectorXd optimized_control_variables =
    Eigen::Map<Eigen::VectorXd>(&result_vec[0], ref_points.size());

  auto t_end1 = std::chrono::high_resolution_clock::now();
  float elapsed_ms1 = std::chrono::duration<float, std::milli>(t_end1 - t_start1).count();
  ROS_INFO_COND(is_showing_debug_info_, "mpt opt time: = %f [ms]", elapsed_ms1);
  return optimized_control_variables;
}

double MPTOptimizer::calcLateralError(
  const geometry_msgs::Point & target_point, const ReferencePoint & ref_point) const
{
  const double err_x = target_point.x - ref_point.p.x;
  const double err_y = target_point.y - ref_point.p.y;
  const double ref_yaw = tf2::getYaw(ref_point.q);
  const double lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y;
  return lat_err;
}

Eigen::VectorXd MPTOptimizer::getState(
  const geometry_msgs::Pose & target_pose, const ReferencePoint & nearest_ref_point) const
{
  const double lat_error = calcLateralError(target_pose.position, nearest_ref_point);
  const double yaw_error =
    util::normalizeRadian(tf2::getYaw(target_pose.orientation) - nearest_ref_point.yaw);
  Eigen::VectorXd x0 = Eigen::VectorXd::Zero(3);
  x0 << lat_error, yaw_error, std::atan(vehicle_param_ptr_->wheelbase * nearest_ref_point.k);
  return x0;
}

std::vector<autoware_planning_msgs::TrajectoryPoint> MPTOptimizer::getMPTPoints(
  std::vector<ReferencePoint> & ref_points, const Eigen::VectorXd & Uex,
  const MPTMatrix & mpt_matrix,
  const std::vector<autoware_planning_msgs::TrajectoryPoint> & optimized_points) const
{
  const int DIM_X = vehicle_model_ptr_->getDimX();
  const auto x0 = ref_points.front().optimized_state;
  Eigen::VectorXd Xex = mpt_matrix.Aex * x0 + mpt_matrix.Bex * Uex + mpt_matrix.Wex;

  const int N = ref_points.size();

  std::vector<autoware_planning_msgs::TrajectoryPoint> traj_points;
  {
    const double lat_error = x0(0);
    const double yaw_error = x0(1);
    autoware_planning_msgs::TrajectoryPoint traj_point;
    traj_point.pose.position.x = ref_points[0].p.x - std::sin(ref_points[0].yaw) * lat_error;
    traj_point.pose.position.y = ref_points[0].p.y + std::cos(ref_points[0].yaw) * lat_error;
    traj_point.twist.linear.x = ref_points.front().v;
    traj_points.push_back(traj_point);
  }

  for (int i = 1; i < ref_points.size(); ++i) {
    const double lat_error = Xex((i - 1) * DIM_X);
    const double yaw_error = Xex((i - 1) * DIM_X + 1);
    Eigen::Vector3d state = Xex.segment((i - 1) * DIM_X, DIM_X);
    setOptimizedState(&ref_points[i], state);
    autoware_planning_msgs::TrajectoryPoint traj_point;
    traj_point.pose.position.x = ref_points[i].p.x - std::sin(ref_points[i].yaw) * lat_error;
    traj_point.pose.position.y = ref_points[i].p.y + std::cos(ref_points[i].yaw) * lat_error;
    traj_point.twist.linear.x = ref_points[i].v;

    traj_points.push_back(traj_point);
  }
  for (int i = 0; i < traj_points.size(); ++i) {
    if (i > 0 && traj_points.size() > 1) {
      traj_points[i].pose.orientation = util::getQuaternionFromPoints(
        traj_points[i].pose.position, traj_points[i - 1].pose.position);
    } else if (traj_points.size() > 1) {
      traj_points[i].pose.orientation = util::getQuaternionFromPoints(
        traj_points[i + 1].pose.position, traj_points[i].pose.position);
    } else {
      traj_points[i].pose.orientation = ref_points[i].q;
    }
  }
  return traj_points;
}

void MPTOptimizer::setOptimizedState(
  ReferencePoint * ref_point, const Eigen::Vector3d & optimized_state) const
{
  ref_point->optimized_state = optimized_state;
}

std::vector<Bounds> MPTOptimizer::getReferenceBounds(
  const bool enable_avoidance, const std::vector<ReferencePoint> & ref_points, const CVMaps & maps,
  DebugData * debug_data) const
{
  std::vector<Bounds> ref_bounds;
  std::vector<geometry_msgs::Pose> debug_bounds_candidata_for_base_points;
  std::vector<geometry_msgs::Pose> debug_bounds_candidata_for_top_points;
  std::vector<geometry_msgs::Pose> debug_bounds_candidate_for_mid_points;
  int cnt = 0;
  for (const auto & point : ref_points) {
    ReferencePoint ref_base_point;
    ref_base_point.p = point.p;
    ref_base_point.yaw = point.yaw;

    ReferencePoint ref_top_point;
    ref_top_point.p = point.top_pose.position;
    ref_top_point.yaw = tf2::getYaw(point.top_pose.orientation);

    ReferencePoint ref_mid_point;
    ref_mid_point.p = point.mid_pose.position;
    ref_mid_point.yaw = tf2::getYaw(point.mid_pose.orientation);

    geometry_msgs::Pose debug_for_base_point;
    debug_for_base_point.position = ref_base_point.p;
    debug_for_base_point.orientation = point.q;
    debug_bounds_candidata_for_base_points.push_back(debug_for_base_point);

    geometry_msgs::Pose debug_for_top_point;
    debug_for_top_point.position = ref_top_point.p;
    debug_for_top_point.orientation = point.top_pose.orientation;
    debug_bounds_candidata_for_top_points.push_back(debug_for_top_point);

    geometry_msgs::Pose debug_for_mid_point;
    debug_for_mid_point.position = ref_mid_point.p;
    debug_for_mid_point.orientation = point.top_pose.orientation;
    debug_bounds_candidate_for_mid_points.push_back(debug_for_mid_point);

    if (
      !util::transformMapToOptionalImage(ref_base_point.p, maps.map_info) ||
      !util::transformMapToOptionalImage(ref_mid_point.p, maps.map_info) ||
      !util::transformMapToOptionalImage(ref_top_point.p, maps.map_info)) {
      Bounds bounds;
      bounds.c0 = {1, -1};
      bounds.c1 = {1, -1};
      bounds.c2 = {1, -1};
      ref_bounds.emplace_back(bounds);
      continue;
    }

    // Calculate boundaries.
    auto lat_bounds_0 = getBound(enable_avoidance, ref_base_point, maps);
    auto lat_bounds_1 = getBound(enable_avoidance, ref_top_point, maps);
    auto lat_bounds_2 = getBound(enable_avoidance, ref_mid_point, maps);
    if (!lat_bounds_0 || !lat_bounds_1 || !lat_bounds_2) {
      ROS_WARN_THROTTLE(1.0, "[Avoidance] Could not find driveable area for %i th point", cnt);
      ROS_INFO_COND(is_showing_debug_info_, "Path is blocked at %i ", cnt);
      Bounds bounds;
      bounds.c0 = {1, -1};
      bounds.c1 = {1, -1};
      bounds.c2 = {1, -1};
      ref_bounds.emplace_back(bounds);
      cnt++;
      continue;
    }
    Bounds bounds;
    bounds.c0 = lat_bounds_0.get();
    bounds.c1 = lat_bounds_1.get();
    bounds.c2 = lat_bounds_2.get();
    ref_bounds.emplace_back(bounds);
    cnt++;
  }
  debug_data->bounds = ref_bounds;
  debug_data->bounds_candidate_for_base_points = debug_bounds_candidata_for_base_points;
  debug_data->bounds_candidate_for_top_points = debug_bounds_candidata_for_top_points;
  debug_data->bounds_candidate_for_mid_points = debug_bounds_candidate_for_mid_points;
  return ref_bounds;
}

boost::optional<std::vector<double>> MPTOptimizer::getBound(
  const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps) const
{
  const auto rough_road_bound = getRoughBound(enable_avoidance, ref_point, maps);

  if (!rough_road_bound) {
    return boost::none;
  }

  std::vector<double> broad_bound;
  // initialize right bound with rough left bound
  double rel_right_bound = rough_road_bound.get()[0];
  auto t_start = std::chrono::high_resolution_clock::now();
  float elapsed_ms = 0.0;
  while (true) {
    const auto bound_candidate = getBoundCandidate(
      enable_avoidance, ref_point, maps, mpt_param_ptr_->clearance_from_road,
      mpt_param_ptr_->clearance_from_object, rel_right_bound, rough_road_bound.get());

    if (!bound_candidate) {
      break;
    }

    if (broad_bound.empty()) {
      broad_bound = bound_candidate.get();
    } else {
      const double bound_candidate_diff =
        std::fabs(bound_candidate.get()[0] - bound_candidate.get()[1]);
      const double broad_bound_diff = std::fabs(broad_bound[0] - broad_bound[1]);
      if (bound_candidate_diff > broad_bound_diff) {
        broad_bound = bound_candidate.get();
      }
    }
    rel_right_bound = bound_candidate.get()[1];
    auto t_end = std::chrono::high_resolution_clock::now();
    elapsed_ms = std::chrono::duration<float, std::milli>(t_end - t_start).count();
    constexpr float max_ms = 10;
    if (elapsed_ms > max_ms) {
      ROS_WARN_THROTTLE(1.0, "take too much time for calculating bound");
      return boost::none;
    }
  }

  if (broad_bound.empty()) {
    return boost::none;
  } else {
    return broad_bound;
  }
}

boost::optional<std::vector<double>> MPTOptimizer::getBoundCandidate(
  const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps,
  const double min_road_clearance, const double min_obj_clearance, const double rel_initial_lat,
  const std::vector<double> & rough_road_bound) const
{
  const bool search_expanding_side = true;
  const auto left_bound = getValidLatError(
    enable_avoidance, ref_point, rel_initial_lat, maps, min_road_clearance, min_obj_clearance,
    rough_road_bound, search_expanding_side);
  if (!left_bound) {
    return boost::none;
  }
  constexpr double min_valid_lat_error = 0.1;
  const double initial_right_bound = left_bound.get() - min_valid_lat_error;
  const auto right_bound = getValidLatError(
    enable_avoidance, ref_point, initial_right_bound, maps, min_road_clearance, min_obj_clearance,
    rough_road_bound);
  if (!right_bound) {
    return boost::none;
  }
  if (std::fabs(left_bound.get() - right_bound.get()) < 1e-5) {
    return boost::none;
  }
  std::vector<double> bound{left_bound.get(), right_bound.get()};
  return bound;
}

boost::optional<std::vector<double>> MPTOptimizer::getRoughBound(
  const bool enable_avoidance, const ReferencePoint & ref_point, const CVMaps & maps) const
{
  double left_bound = 0;
  double right_bound = 0;
  const double left_angle = util::normalizeRadian(ref_point.yaw + M_PI_2);
  const double right_angle = util::normalizeRadian(ref_point.yaw - M_PI_2);

  geometry_msgs::Point new_position;
  new_position.x = ref_point.p.x;
  new_position.y = ref_point.p.y;
  auto original_clearance = getClearance(maps.clearance_map, new_position, maps.map_info);
  auto original_object_clearance =
    getClearance(maps.only_objects_clearance_map, new_position, maps.map_info);
  if (!original_clearance || !original_object_clearance) {
    return boost::none;
  }
  if (!enable_avoidance) {
    *original_object_clearance = std::numeric_limits<double>::max();
  }
  constexpr double min_road_clearance = 0.1;
  constexpr double min_obj_clearance = 0.1;
  if (
    original_clearance.get() > min_road_clearance &&
    original_object_clearance.get() > min_obj_clearance) {
    const double initial_dist = 0;
    right_bound = -1 * getTraversedDistance(
                         enable_avoidance, ref_point, right_angle, initial_dist, maps,
                         min_road_clearance, min_obj_clearance);
    left_bound = getTraversedDistance(
      enable_avoidance, ref_point, left_angle, initial_dist, maps, min_road_clearance,
      min_obj_clearance);
  } else {
    const double initial_dist = 0;
    const bool search_expanding_side = true;
    const double right_s = getTraversedDistance(
      enable_avoidance, ref_point, right_angle, initial_dist, maps, min_road_clearance,
      min_obj_clearance, search_expanding_side);
    const double left_s = getTraversedDistance(
      enable_avoidance, ref_point, left_angle, initial_dist, maps, min_road_clearance,
      min_obj_clearance, search_expanding_side);
    if (left_s < right_s) {
      // Pick left side:
      right_bound = left_s;
      left_bound = getTraversedDistance(
        enable_avoidance, ref_point, left_angle, left_s, maps, min_road_clearance,
        min_obj_clearance);
    } else {
      // Pick right side:
      left_bound = -right_s;
      right_bound = -getTraversedDistance(
        enable_avoidance, ref_point, right_angle, right_s, maps, min_road_clearance,
        min_obj_clearance);
    }
  }
  if (std::fabs(left_bound - right_bound) < 1e-6) {
    return boost::none;
  }
  std::vector<double> bound{left_bound, right_bound};
  return bound;
}

boost::optional<double> MPTOptimizer::getClearance(
  const cv::Mat & clearance_map, const geometry_msgs::Point & map_point,
  const nav_msgs::MapMetaData & map_info) const
{
  const auto image_point = util::transformMapToOptionalImage(map_point, map_info);
  if (!image_point) {
    return boost::none;
  }
  const float clearance =
    clearance_map.ptr<float>((int)image_point.get().y)[(int)image_point.get().x] *
    map_info.resolution;
  return clearance;
}

ObjectiveMatrix MPTOptimizer::getObjectiveMatrix(
  const Eigen::VectorXd & x0, const MPTMatrix & m) const
{
  const int DIM_U_N = m.Urefex.rows();
  const Eigen::MatrixXd CB = m.Cex * m.Bex;
  const Eigen::MatrixXd QCB = m.Qex * CB;
  // Eigen::MatrixXd H = CB.transpose() * QCB + m.R1ex + m.R2ex; // This calculation is heavy. looking for a good way.
  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(DIM_U_N, DIM_U_N);
  H.triangularView<Eigen::Upper>() = CB.transpose() * QCB;
  H.triangularView<Eigen::Upper>() += m.R1ex + m.R2ex;
  H.triangularView<Eigen::Lower>() = H.transpose();
  Eigen::VectorXd f =
    (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Urefex.transpose() * m.R1ex;
  addSteerWeightF(&f);

  constexpr int num_lat_constraint = 3;
  const int num_objective_variables = DIM_U_N * (1 + num_lat_constraint);
  Eigen::MatrixXd extend_h = Eigen::MatrixXd::Zero(DIM_U_N, DIM_U_N);
  Eigen::VectorXd extend_f = Eigen::VectorXd::Ones(DIM_U_N);
  Eigen::MatrixXd concat_h =
    Eigen::MatrixXd::Zero(num_objective_variables, num_objective_variables);
  Eigen::VectorXd concat_f = Eigen::VectorXd::Zero(num_objective_variables);
  concat_h.block(0, 0, DIM_U_N, DIM_U_N) = H;
  concat_h.block(DIM_U_N, DIM_U_N, DIM_U_N, DIM_U_N) = extend_h;
  concat_h.block(DIM_U_N * 2, DIM_U_N * 2, DIM_U_N, DIM_U_N) = extend_h;
  concat_h.block(DIM_U_N * 3, DIM_U_N * 3, DIM_U_N, DIM_U_N) = extend_h;
  concat_f << f, mpt_param_ptr_->base_point_weight * extend_f,
    mpt_param_ptr_->top_point_weight * extend_f, mpt_param_ptr_->mid_point_weight * extend_f;
  ObjectiveMatrix obj_matrix;
  obj_matrix.hessian = concat_h;
  obj_matrix.gradient = {concat_f.data(), concat_f.data() + concat_f.rows()};

  return obj_matrix;
}

// Set constraint: lb <= Ax <= ub
// decision variable x := [u0, ..., uN-1 | z00, ..., z0N-1 | z10, ..., z1N-1 | z20, ..., z2N-1] \in \mathbb{R}^{N * (N_point + 1)}
ConstraintMatrix MPTOptimizer::getConstraintMatrix(
  const bool enable_avoidance, const Eigen::VectorXd & x0, const MPTMatrix & m, const CVMaps & maps,
  const std::vector<ReferencePoint> & ref_points,
  const std::vector<autoware_planning_msgs::PathPoint> & path_points, DebugData * debug_data) const
{
  std::vector<double> dist_vec{
    mpt_param_ptr_->base_point_dist_from_base_link, mpt_param_ptr_->top_point_dist_from_base_link,
    mpt_param_ptr_->mid_point_dist_from_base_link};

  const size_t N_ref = m.Urefex.rows();
  const size_t N_state = vehicle_model_ptr_->getDimX();
  const size_t N_point = dist_vec.size();
  const size_t N_dec = N_ref * (N_point + 1);

  const auto bounds = getReferenceBounds(enable_avoidance, ref_points, maps, debug_data);

  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3 * N_ref * N_point + N_ref, N_dec);
  Eigen::VectorXd lb = Eigen::VectorXd::Constant(3 * N_ref * N_point + N_ref, -osqp::INF);
  Eigen::VectorXd ub = Eigen::VectorXd::Constant(3 * N_ref * N_point + N_ref, osqp::INF);

  // Define constraint matrices and vectors
  // Gap from reference point around vehicle base_link
  {
    // C := [I | O | O]
    Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state);
    for (size_t i = 0; i < N_ref; ++i) {
      C(i, N_state * i) = 1;
    }
    // bias := Cast * (Aex * x0 + Wex)
    Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex);
    // A_blk := [C * Bex | I | O | O
    //          -C * Bex | I | O | O
    //               O   | I | O | O]
    Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(3 * N_ref, N_dec);
    A_blk.block(0, 0, N_ref, N_ref) = C * m.Bex;
    A_blk.block(0, N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref);
    A_blk.block(N_ref, 0, N_ref, N_ref) = -C * m.Bex;
    A_blk.block(N_ref, N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref);
    A_blk.block(2 * N_ref, N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref);
    // lb_blk := [-bias + bounds.lb
    //             bias - bounds.ub
    //             0]
    Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(3 * N_ref);
    for (size_t i = 0; i < N_ref; ++i) {
      if (i == N_ref - 1) {
        lb_blk(i) = -bias(i) + bounds[i].c0.lb;
        lb_blk(N_ref + i) = bias(i) - bounds[i].c0.ub;
      } else {
        lb_blk(i) = -bias(i) + bounds[i + 1].c0.lb;
        lb_blk(N_ref + i) = bias(i) - bounds[i + 1].c0.ub;
      }
    }
    // Assign
    A.block(0, 0, 3 * N_ref, N_dec) = A_blk;
    lb.segment(0, 3 * N_ref) = lb_blk;
  }

  // Gap from reference point around vehicle top
  {
    // C := diag([cos(alpha1) | l1*cos(alpha1) | 0])
    Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state);
    for (size_t i = 0; i < N_ref; ++i) {
      Eigen::MatrixXd Cast = Eigen::MatrixXd::Zero(1, N_state);
      if (i == N_ref - 1) {
        Cast(0, 0) = std::cos(ref_points[i].delta_yaw_from_p1);
        Cast(0, 1) = dist_vec[1] * std::cos(ref_points[i].delta_yaw_from_p1);
      } else {
        Cast(0, 0) = std::cos(ref_points[i + 1].delta_yaw_from_p1);
        Cast(0, 1) = dist_vec[1] * std::cos(ref_points[i + 1].delta_yaw_from_p1);
      }
      C.block(i, N_state * i, 1, N_state) = Cast;
    }
    // bias := Cast * (Aex * x0 + Wex) - l1 * sin(alpha1)
    Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex);
    for (size_t i = 0; i < N_ref; ++i) {
      if (i == N_ref - 1) {
        bias(i) -= dist_vec[1] * std::sin(ref_points[i].delta_yaw_from_p1);
      } else {
        bias(i) -= dist_vec[1] * std::sin(ref_points[i + 1].delta_yaw_from_p1);
      }
    }
    // A_blk := [C * Bex | O | I | O
    //          -C * Bex | O | I | O
    //               O   | O | I | O]
    Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(3 * N_ref, N_dec);
    A_blk.block(0, 0, N_ref, N_ref) = C * m.Bex;
    A_blk.block(0, 2 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref);
    A_blk.block(N_ref, 0, N_ref, N_ref) = -C * m.Bex;
    A_blk.block(N_ref, 2 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref);
    A_blk.block(2 * N_ref, 2 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref);
    // lb_blk := [-bias + bounds.lb
    //             bias - bounds.ub
    //             0]
    Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(3 * N_ref);
    for (size_t i = 0; i < N_ref; ++i) {
      if (i == N_ref - 1) {
        lb_blk(i) = -bias(i) + bounds[i].c1.lb;
        lb_blk(N_ref + i) = bias(i) - bounds[i].c1.ub;
      } else {
        lb_blk(i) = -bias(i) + bounds[i + 1].c1.lb;
        lb_blk(N_ref + i) = bias(i) - bounds[i + 1].c1.ub;
      }
    }
    // Assign
    A.block(3 * N_ref, 0, 3 * N_ref, N_dec) = A_blk;
    lb.segment(3 * N_ref, 3 * N_ref) = lb_blk;
  }
  // Gap from reference point around vehicle middle
  {
    // C := [diag(cos(alpha2)) | diag(l2*cos(alpha2)) | O]
    Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state);
    for (size_t i = 0; i < N_ref; ++i) {
      Eigen::MatrixXd Cast = Eigen::MatrixXd::Zero(1, N_state);
      if (i == N_ref - 1) {
        Cast(0, 0) = std::cos(ref_points[i].delta_yaw_from_p2);
        Cast(0, 1) = dist_vec[2] * std::cos(ref_points[i].delta_yaw_from_p2);
      } else {
        Cast(0, 0) = std::cos(ref_points[i + 1].delta_yaw_from_p2);
        Cast(0, 1) = dist_vec[2] * std::cos(ref_points[i + 1].delta_yaw_from_p2);
      }
      C.block(i, N_state * i, 1, N_state) = Cast;
    }
    // bias := Cast * (Aex * x0 + Wex) - l2 * sin(alpha2)
    Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex);
    for (size_t i = 0; i < N_ref; ++i) {
      if (i == N_ref - 1) {
        bias(i) -= dist_vec[2] * std::sin(ref_points[i].delta_yaw_from_p2);
      } else {
        bias(i) -= dist_vec[2] * std::sin(ref_points[i + 1].delta_yaw_from_p2);
      }
    }
    // A_blk := [C * Bex | O | O | I
    //          -C * Bex | O | O | I
    //               O   | O | O | I]
    Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(3 * N_ref, N_dec);
    A_blk.block(0, 0, N_ref, N_ref) = C * m.Bex;
    A_blk.block(0, 3 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref);
    A_blk.block(N_ref, 0, N_ref, N_ref) = -C * m.Bex;
    A_blk.block(N_ref, 3 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref);
    A_blk.block(2 * N_ref, 3 * N_ref, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref);
    // lb_blk := [-bias + bounds.lb
    //             bias - bounds.ub
    //             0]
    Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(3 * N_ref);
    for (size_t i = 0; i < N_ref; ++i) {
      if (i == N_ref - 1) {
        lb_blk(i) = -bias(i) + bounds[i].c2.lb;
        lb_blk(N_ref + i) = bias(i) - bounds[i].c2.ub;
      } else {
        lb_blk(i) = -bias(i) + bounds[i + 1].c2.lb;
        lb_blk(N_ref + i) = bias(i) - bounds[i + 1].c2.ub;
      }
    }
    // Assign
    A.block(2 * 3 * N_ref, 0, 3 * N_ref, N_dec) = A_blk;
    lb.segment(2 * 3 * N_ref, 3 * N_ref) = lb_blk;
  }

  // Fixed points constraint
  {
    // C := [I | O | O]
    Eigen::MatrixXd C = Eigen::MatrixXd::Zero(N_ref, N_ref * N_state);
    for (size_t i = 0; i < N_ref; ++i) {
      C(i, N_state * i) = 1;
    }
    // bias := Cast * (Aex * x0 + Wex)
    Eigen::VectorXd bias = C * (m.Aex * x0 + m.Wex);

    // Assign
    A.block(3 * N_point * N_ref, 0, N_ref, N_ref) = C * m.Bex;
    for (size_t i = 0; i < N_ref; ++i) {
      if (ref_points[i + 1].fix_state && i + 1 < N_ref) {
        lb(3 * N_point * N_ref + i) = ref_points[i + 1].fix_state.get()(0) - bias(i);
        ub(3 * N_point * N_ref + i) = ref_points[i + 1].fix_state.get()(0) - bias(i);
      } else if (i == ref_points.size() - 1 && mpt_param_ptr_->is_hard_fixing_terminal_point) {
        lb(3 * N_point * N_ref + i) = -bias(i);
        ub(3 * N_point * N_ref + i) = -bias(i);
      }
    }
  }

  ConstraintMatrix constraint_matrix;

  constraint_matrix.linear = A;

  for (size_t i = 0; i < lb.size(); ++i) {
    constraint_matrix.lower_bound.push_back(lb(i));
    constraint_matrix.upper_bound.push_back(ub(i));
  }

  return constraint_matrix;
}

std::vector<ReferencePoint> MPTOptimizer::getBaseReferencePoints(
  const std::vector<geometry_msgs::Point> & interpolated_points,
  const std::unique_ptr<Trajectories> & prev_trajs, DebugData * debug_data) const
{
  std::vector<ReferencePoint> reference_points;
  for (int i = 0; i < interpolated_points.size(); i++) {
    ReferencePoint ref_point;
    ref_point.p = interpolated_points[i];
    reference_points.push_back(ref_point);
  }
  if (!prev_trajs) {
    return reference_points;
  }
  if (prev_trajs->model_predictive_trajectory.size() != prev_trajs->mpt_ref_points.size()) {
    return reference_points;
  }

  // re-calculating points' position for fixing
  std::vector<geometry_msgs::Point> cropped_interpolated_points;
  double accum_s_for_interpolated = 0;
  for (int i = 0; i < interpolated_points.size(); i++) {
    if (i > 0) {
      accum_s_for_interpolated +=
        util::calculate2DDistance(interpolated_points[i], interpolated_points[i - 1]);
    }
    if (
      accum_s_for_interpolated >
      traj_param_ptr_->num_sampling_points * traj_param_ptr_->delta_arc_length_for_mpt_points) {
      break;
    }
    cropped_interpolated_points.push_back(interpolated_points[i]);
  }
  constexpr double fine_resolution = 0.005;
  std::vector<geometry_msgs::Point> fine_interpolated_points;
  fine_interpolated_points =
    util::getInterpolatedPoints(cropped_interpolated_points, fine_resolution);
  const double max_s =
    traj_param_ptr_->backward_fixing_distance + traj_param_ptr_->forward_fixing_mpt_distance;
  const int num_points = std::min(
    (int)(reference_points.size()),
    (int)(max_s / traj_param_ptr_->delta_arc_length_for_mpt_points));

  for (int i = 0; i < num_points; i++) {
    const int nearest_prev_idx =
      util::getNearestPointIdx(prev_trajs->mpt_ref_points, reference_points[i].p);
    if (
      util::calculate2DDistance(
        prev_trajs->mpt_ref_points[nearest_prev_idx].p, reference_points[i].p) >=
      std::fabs(traj_param_ptr_->delta_arc_length_for_mpt_points)) {
      continue;
    }
    const int nearest_idx =
      util::getNearestIdx(fine_interpolated_points, prev_trajs->mpt_ref_points[nearest_prev_idx].p);
    if (
      util::calculate2DDistance(
        fine_interpolated_points[nearest_idx], prev_trajs->mpt_ref_points[nearest_prev_idx].p) <
      fine_resolution) {
      reference_points[i] = prev_trajs->mpt_ref_points[nearest_prev_idx];
      reference_points[i].fix_state = prev_trajs->mpt_ref_points[nearest_prev_idx].optimized_state;
    }
  }

  std::vector<ReferencePoint> trimmed_reference_points;
  for (int i = 0; i < reference_points.size(); i++) {
    if (i > 0) {
      const double dx = reference_points[i].p.x - reference_points[i - 1].p.x;
      const double dy = reference_points[i].p.y - reference_points[i - 1].p.y;
      if (std::fabs(dx) < 1e-6 && std::fabs(dy) < 1e-6) {
        continue;
      }
    }
    trimmed_reference_points.push_back(reference_points[i]);
  }

  for (int i = 0; i < trimmed_reference_points.size(); i++) {
    if (trimmed_reference_points[i].fix_state) {
      geometry_msgs::Point rel_point;
      rel_point.y = trimmed_reference_points[i].fix_state.get()(0);
      geometry_msgs::Pose origin;
      origin.position = trimmed_reference_points[i].p;
      origin.orientation = trimmed_reference_points[i].q;
      geometry_msgs::Pose debug_pose;
      debug_pose.position = util::transformToAbsoluteCoordinate2D(rel_point, origin);
      debug_data->fixed_mpt_points.push_back(debug_pose);
      continue;
    }
  }
  return trimmed_reference_points;
}

double MPTOptimizer::getTraversedDistance(
  const bool enable_avoidance, const ReferencePoint & ref_point, const double traverse_angle,
  const double initial_value, const CVMaps & maps, const double min_road_clearance,
  const double min_obj_clearance, const bool search_expanding_side) const
{
  constexpr double ds = 0.1;
  constexpr double lane_width = 7.5;
  int n = static_cast<int>(lane_width / ds);

  double traversed_dist = initial_value;
  for (int i = 0; i < n; ++i) {
    traversed_dist += ds;
    geometry_msgs::Point new_position;
    new_position.x = ref_point.p.x + traversed_dist * std::cos(traverse_angle);
    new_position.y = ref_point.p.y + traversed_dist * std::sin(traverse_angle);
    auto clearance = getClearance(maps.clearance_map, new_position, maps.map_info);
    auto obj_clearance = getClearance(maps.only_objects_clearance_map, new_position, maps.map_info);
    if (!clearance || !obj_clearance) {
      *clearance = 0;
      *obj_clearance = 0;
    }
    if (!enable_avoidance) {
      *obj_clearance = std::numeric_limits<double>::max();
    }
    if (search_expanding_side) {
      if (clearance.get() > min_road_clearance && obj_clearance.get() > min_obj_clearance) {
        break;
      }
    } else {
      if (clearance.get() < min_road_clearance || obj_clearance.get() < min_obj_clearance) {
        break;
      }
    }
  }
  return traversed_dist;
}

boost::optional<double> MPTOptimizer::getValidLatError(
  const bool enable_avoidance, const ReferencePoint & ref_point, const double initial_value,
  const CVMaps & maps, const double min_road_clearance, const double min_obj_clearance,
  const std::vector<double> & rough_road_bound, const bool search_expanding_side) const
{
  constexpr double ds = 0.01;
  constexpr double lane_width = 7.5;
  int n = static_cast<int>(lane_width / ds);

  double rel_value = initial_value;
  for (int i = 0; i < n; ++i) {
    rel_value -= ds;
    if (rel_value > rough_road_bound[0] || rel_value < rough_road_bound[1]) {
      return boost::none;
    }
    geometry_msgs::Point rel_point;
    rel_point.y = rel_value;
    geometry_msgs::Pose origin;
    origin.position = ref_point.p;
    origin.orientation = util::getQuaternionFromYaw(ref_point.yaw);
    const auto new_position = util::transformToAbsoluteCoordinate2D(rel_point, origin);
    auto clearance = getClearance(maps.clearance_map, new_position, maps.map_info);
    auto obj_clearance = getClearance(maps.only_objects_clearance_map, new_position, maps.map_info);
    if (!clearance || !obj_clearance) {
      return boost::none;
    }
    if (!enable_avoidance) {
      *obj_clearance = std::numeric_limits<double>::max();
    }
    if (search_expanding_side) {
      if (clearance.get() > min_road_clearance && obj_clearance.get() > min_obj_clearance) {
        break;
      }
    } else {
      if (clearance.get() < min_road_clearance || obj_clearance.get() < min_obj_clearance) {
        break;
      }
    }
  }
  return rel_value;
}


#include "dSFMTsearch.hpp"
#include "Annihilate.h"
#include <errno.h>
#include <stdlib.h>
#include <getopt.h>
#include <errno.h>
#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <MTToolBox/period.hpp>
#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>
#include "printBinary.h"
#include "calc_fixpoint.h"

using namespace MTToolBox;
using namespace std;

static bool check = false;
static bool get_fixpoint(dSFMT& dsfmt);
static bool chk_fix1(w128_t& fix, const dSFMT& dsfmt, const GF2X& irreducible,
                const GF2X& quotient);
static bool chk_fix2(const dSFMT& fix, const dSFMT& con, const GF2X& idf);

static bool chk_fix1(w128_t& fix, const dSFMT& dsfmt, const GF2X& irreducible,
                     const GF2X& quotient)
{
    GF2X a;
    GF2X b;
    GF2X d;
    dSFMT dsfmt_const(dsfmt);
    /* a*irreducible + b*quotient = d */
    XGCD(d, a, b, irreducible, quotient);
    if (deg(d) != 0) {
        cout << "failure d != 1" << endl;
        return false;
    }
#if defined(DEBUG)
    cout << "irreducible" << endl;
    printBinary(stdout, irreducible);
    cout << "quotient" << endl;
    printBinary(stdout, quotient);
    cout << "a" << endl;
    printBinary(stdout, a);
    cout << "b" << endl;
    printBinary(stdout, b);
#endif
    b *= quotient;
    GF2X idf(b);
    a *= irreducible;
    dsfmt_const.setConst();
    dSFMT dsfmt_const0(dsfmt_const);
#if defined(DEBUG)
    cout << "dsfmt_const after setConst" << endl;
    dsfmt_const.d_p();
    cout << "b" << endl;
    printBinary(stdout, b);
    cout << "dsfmt_const0" << endl;
    dsfmt_const0.d_p();
#endif
    annihilate<w128_t>(&dsfmt_const, b);
#if defined(DEBUG)
    cout << "dsfmt_const after annihilate" << endl;
    dsfmt_const.d_p();
    cout << "dsfmt_const0" << endl;
    dsfmt_const0.d_p();
#endif
    dSFMT const_L_save = dsfmt_const;
    dSFMT dsfmt_const2(dsfmt);
    dsfmt_const2.setConst();
    annihilate<w128_t>(&dsfmt_const2, a);
#if defined(DEBUG)
    cout << "dsfmt_const2 after annihilate" << endl;
    dsfmt_const2.d_p();
    cout << "dsfmt_const0" << endl;
    dsfmt_const0.d_p();
#endif
    dsfmt_const2.add(dsfmt_const);
#if defined(DEBUG)
    cout << "dsfmt_const2 after add" << endl;
    dsfmt_const2.d_p();
    cout << "dsfmt_const0" << endl;
    dsfmt_const0.d_p();
#endif
    if (!dsfmt_const2.equals(dsfmt_const0)) {
        cout << "modoranai" << endl;
        cout << "dsfmt_const2" << endl;
        dsfmt_const2.d_p();
        cout << "dsfmt_const0" << endl;
        dsfmt_const0.d_p();
        dsfmt_const2.add(dsfmt_const0);
        cout << "sa" << endl;
        dsfmt_const2.d_p();
        return false;
    }
    GF2X t1(1, 1);
    SetCoeff(t1, 0);
    /* a*irreducible + b*t1 = d */
    XGCD(d, a, b, irreducible, t1);
    if (deg(d) != 0) {
        cout << "failure d != 1" << endl;
        cout << "deg(d) = " << dec << deg(d) << endl;
        cout << "deg(irreducible) = " << dec << deg(irreducible) << endl;
        cout << "deg(t1) = " << dec << deg(t1) << endl;
        throw new logic_error("failure d != 1");
    }
    annihilate<w128_t>(&dsfmt_const, b);

    fix = dsfmt_const.getParityValue();
    if (!chk_fix2(dsfmt_const, const_L_save, idf)) {
        cout << "chk_fix2 error" << endl;
        return false;
    }
    return true;
}

static bool chk_fix2(const dSFMT& fix, const dSFMT& con, const GF2X& idf)
{
    dSFMT tmp(fix);
    tmp.generate();
    annihilate<w128_t>(&tmp, idf);
    tmp.add(&con);
    return tmp.equals(fix);
}

static bool option(dSFMT_param& param, int argc, char * argv[])
{
    int c;
    bool error = false;
    check = false;
    for (;;) {
        c = getopt(argc, argv, "hc");
        if (error) {
            break;
        }
        if (c == -1) {
            break;
        }
        switch (c) {
        case 'c':
            check = true;
            break;
        case 'h':
        default:
            error = true;
            break;
        }
    }
    char * para = argv[optind];
    param.mexp = strtoul(para, &para, 10);
    para++;
    param.pos1 = strtoul(para, &para, 10);
    para++;
    param.sl1 = strtoul(para, &para, 10);
    para++;
    param.msk1 = strtoull(para, &para, 16);
    para++;
    param.msk2 = strtoull(para, &para, 16);
    return true;
}

int main(int argc, char *argv[]) {
    dSFMT_param param;
    if (!option(param, argc, argv)) {
        return -1;
    }
    //cout << param.get_string() << endl;
    dSFMT dsfmt(param);
#if defined(DEBUG) && 0
    dsfmt.setConst();
    cout << "dsfmt_const after setConst" << endl;
    dsfmt.d_p();
    dsfmt.generate();
    cout << "dsfmt_const after generate" << endl;
    dsfmt.d_p();
#endif
    int mexp = dsfmt.getMexp();
    int maxdegree = dsfmt.bitSize();
    if (check) {
        printf("mexp = %d, maxdegree = %d\n", mexp, maxdegree);
    }
    w128_t w;
    w.u64[0] = 1;
    dsfmt.seed(w);
    get_fixpoint(dsfmt);
    return 0;
}

static bool get_fixpoint(dSFMT& dsfmt)
{
    GF2X poly;
    GF2X lcmpoly;
    GF2X smallpoly;
    GF2X tmp;
    GF2X rempoly;
    GF2X filler;
    w128_t fix;

    minpoly<w128_t>(poly, dsfmt);
    GF2X irreducible = poly;
    if (!hasFactorOfDegree(irreducible, dsfmt.getMexp())) {
        cout << "error does not have factor of degree "
             << dec << dsfmt.getMexp() << endl;
        cout << "degree = " << dec << deg(irreducible) << endl;
        return false;
    }
    lcmpoly = poly;
    getLCMPoly(lcmpoly, dsfmt);
    if (check) {
        printf("deg lcm poly = %ld\n", deg(lcmpoly));
    }
    DivRem(smallpoly, rempoly, lcmpoly, irreducible);
    if (deg(rempoly) >= 0) {
        cout << "deg(rempoly) = " << deg(rempoly) << endl;
        return false;
    }
    if (check) {
        printf("deg small poly = %ld\n", deg(smallpoly));
    }
    if (check) {
        if (!chk_fix1(fix, dsfmt, irreducible, smallpoly)) {
            cout << "chk_fix1 error" << endl;
            return false;
        }
    } else {
        fix = calc_fixpoint(dsfmt, irreducible, smallpoly);
    }
    dsfmt.setFixPoint(fix);
    cout << dsfmt.getParamString() << endl;
    if (check) {
        cout << "fix1 = " << setfill('0') << setw(16) << hex
             << fix.u64[0] << endl;
        cout << "fix2 = " << setfill('0') << setw(16) << hex
             << fix.u64[1] << endl;
    }
    return true;
}


// Boost.Geometry

// Copyright (c) 2018 Oracle and/or its affiliates.

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

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

#ifndef BOOST_GEOMETRY_FORMULAS_QUARTER_MERIDIAN_HPP
#define BOOST_GEOMETRY_FORMULAS_QUARTER_MERIDIAN_HPP

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

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

#include <boost/geometry/formulas/flattening.hpp>

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

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace formula_dispatch
{

template <typename ResultType, typename Geometry, typename Tag = typename tag<Geometry>::type>
struct quarter_meridian
    : not_implemented<Tag>
{};

template <typename ResultType, typename Geometry>
struct quarter_meridian<ResultType, Geometry, srs_spheroid_tag>
{
    //https://en.wikipedia.org/wiki/Meridian_arc#Generalized_series
    //http://www.wolframalpha.com/input/?i=(sum(((2*j-3)!!%2F(2*j)!!)%5E2*n%5E(2*j),j,0,8))
    static inline ResultType apply(Geometry const& geometry)
    {
        //order 8 expansion
        ResultType const C[] =
        {
            1073741824,
            268435456,
            16777216,
            4194304,
            1638400,
            802816,
            451584,
            278784,
            184041
        };

        ResultType const c2 = 2;
        ResultType const c4 = 4;
        ResultType const f = formula::flattening<ResultType>(geometry);
        ResultType const n = f / (c2 - f);
        ResultType const ab4 = (get_radius<0>(geometry)
                                + get_radius<2>(geometry)) / c4;
        return geometry::math::pi<ResultType>() * ab4 *
                 horner_evaluate(n*n, C, C+8) / C[0];
    }

private :
    //TODO: move the following to a more general space to be used by other
    //      classes as well
    /*
        Evaluate the polynomial in x using Horner's method.
    */
    template <typename NT, typename IteratorType>
    static inline NT horner_evaluate(NT x,
                                     IteratorType begin,
                                     IteratorType end)
    {
        NT result(0);
        if (begin == end)
        {
            return result;
        }
        IteratorType it = end;
        do
        {
            result = result * x + *--it;
        }
        while (it != begin);
        return result;
    }
};

} // namespace formula_dispatch
#endif // DOXYGEN_NO_DISPATCH

#ifndef DOXYGEN_NO_DETAIL
namespace formula
{

template <typename ResultType, typename Geometry>
ResultType quarter_meridian(Geometry const& geometry)
{
    return formula_dispatch::quarter_meridian<ResultType, Geometry>::apply(geometry);
}

} // namespace formula
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_FORMULAS_QUARTER_MERIDIAN_HPP


#include <iostream>
#include <fstream>
#include <algorithm>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <cmath>

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_utility.hpp>

#include "network_graph.hpp"
#include "protocol.hpp"

using namespace boost;

const unsigned int MAX_NUM_VERTICES=6;

NetworkGraph::NetworkGraph()
{
	_grid.resize(_grid_dim * _grid_dim);
}

void NetworkGraph::graph_file(std::string filename)
{
	_graph_filename = filename;
}

std::string NetworkGraph::graph_file(void) const
{
	return _graph_filename;
}

void NetworkGraph::sqr_weight(unsigned int weight)
{
	_sqr_weight = weight;
}

unsigned int NetworkGraph::sqr_weight(void) const
{
	return _sqr_weight;
}

unsigned int NetworkGraph::grid_dim(void) const
{
	return _grid_dim;
}

void NetworkGraph::add_routes_from_file()
{
	if (_graph_filename.empty()) {
		std::cerr << "_graph_filename is empty!" << std::endl;
		exit(1);
	}

	std::ifstream routes_file(_graph_filename);
	std::string line;
	std::vector<std::string> tokens;
	std::vector<unsigned int> ve_attr;
	Vertex *vertex_0 = nullptr, *vertex_1 = nullptr;
	bool vertex_0_found, vertex_1_found;

	// Protocol message
	ProtoMsg src_proto_msg, dst_proto_msg;

	while (std::getline(routes_file, line)) {
		if (line == "")
			continue;

		// zerofy control structs
		vertex_0_found = vertex_1_found = false;
		ve_attr.clear();

		// retrieve data from graph file, which follows per line:
		// <v0> <v1> <cost> <v0_x> <v0_y> <v1_x> <v0_y>
		// and covert to unsigned int
		split(tokens, line, is_any_of(" "));
		for (auto it = tokens.begin(); it != tokens.end(); it++)
			ve_attr.push_back(static_cast<unsigned int>(std::stoul(*it)));

		for (auto vertex_lut : _vertex_lut) {
			Vertex *vertex = vertex_lut.second;
			if (!vertex_0_found && vertex->id == ve_attr.at(0)) {
				vertex_0_found = true;
				vertex_0 = vertex;
			} else if (!vertex_1_found && vertex->id == ve_attr.at(1)) {
				vertex_1_found = true;
				vertex_1 = vertex;
			}
		}

		src_proto_msg = {};
		dst_proto_msg = {};

		// in case vertex doesn't already exists, create new one and add
		// it to LUT
		if (!vertex_0_found) {
			vertex_0 = new Vertex({ve_attr.at(0),
								   {ve_attr.at(3), ve_attr.at(4)}});
			vertex_0->self = add_vertex(*vertex_0, _graph);
			src_proto_msg.dst_id = BROADCAST_ID;
			src_proto_msg.payload.msg = MSG_HELLO;
			src_proto_msg.src_id = vertex_0->id;
			vertex_0->msg_table.push_back(src_proto_msg);

			for (auto adj_item : _adj_list) {
				if (adj_item.second.first == vertex_0->id ||
					adj_item.second.second == vertex_0->id) {
					for (auto lut_item : _vertex_lut) {
						if (lut_item.first == adj_item.second.second ||
							lut_item.first == adj_item.second.first) {
							lut_item.second->msg_table.push_back(src_proto_msg);

							dst_proto_msg.src_id = lut_item.second->id;
							dst_proto_msg.dst_id = vertex_0->id;
							dst_proto_msg.payload.msg = MSG_UPDATE;
//							for (auto route : vertex_1->routing_table) {
//								unsigned int route_cost = route.first.cost;
//								std::vector<unsigned int> vertices_id;
//								for (auto vertex : route.second) {
//									vertices_id.push_back(vertex->id);
//								}
//								dst_proto_msg.payload.routing_table.push_back(
//											std::make_pair(route_cost, vertices_id));
//							}
							vertex_0->msg_table.push_back(dst_proto_msg);
							lut_item.second->msg_table.push_back(dst_proto_msg);
						}
					}
				}
			}
			_vertex_lut.push_back(std::make_pair(vertex_0->id, vertex_0));
		}

		src_proto_msg = {};
		dst_proto_msg = {};

		if (!vertex_1_found) {
			vertex_1 = new Vertex({ve_attr.at(1),
								   {ve_attr.at(5), ve_attr.at(6)}});
			vertex_1->self = add_vertex(*vertex_1, _graph);
			src_proto_msg.dst_id = BROADCAST_ID;
			src_proto_msg.payload.msg = MSG_HELLO;
			src_proto_msg.src_id = vertex_1->id;
			vertex_1->msg_table.push_back(src_proto_msg);
			std::cout << "vertex_1 = " << vertex_1->id << std::endl;

			for (auto adj_item : _adj_list) {
				if (adj_item.second.first == vertex_1->id ||
					adj_item.second.second == vertex_1->id) {
					for (auto lut_item : _vertex_lut) {
						if (lut_item.first == adj_item.second.second ||
							lut_item.first == adj_item.second.first) {
							lut_item.second->msg_table.push_back(src_proto_msg);

							dst_proto_msg.src_id = lut_item.second->id;
							dst_proto_msg.dst_id = vertex_1->id;
							dst_proto_msg.payload.msg = MSG_UPDATE;
//							for (auto route : vertex_1->routing_table) {
//								unsigned int route_cost = route.first.cost;
//								std::vector<unsigned int> vertices_id;
//								for (auto vertex : route.second) {
//									vertices_id.push_back(vertex->id);
//								}
//								dst_proto_msg.payload.routing_table.push_back(
//											std::make_pair(route_cost, vertices_id));
//							}
							lut_item.second->msg_table.push_back(dst_proto_msg);
							vertex_1->msg_table.push_back(dst_proto_msg);
						}
					}
				}
			}
			_vertex_lut.push_back(std::make_pair(vertex_1->id, vertex_1));
		}

		if (vertex_0 == nullptr || vertex_1 == nullptr) {
			std::cerr << "ERROR: vertices are null" << std::endl;
			exit(1);
		}

		add_edge(vertex_0->self, vertex_1->self, {ve_attr.at(2)}, _graph);
		// sort vertex LUT to allow binary search
		std::sort(_vertex_lut.begin(), _vertex_lut.end());
	}

#ifdef DEBUG
	std::cout << "vertices found: ";
	print_vertices(_graph, get(&Vertex::id, _graph));
	auto vpair = vertices(_graph);
	for (auto v_it = vpair.first; v_it != vpair.second; v_it++) {
		std::cout << "edges incident to vertex " << _graph[*v_it].id <<
			" are: " << std::endl;
		auto epair = out_edges(*v_it, _graph);
		for (auto e_it = epair.first; e_it != epair.second; e_it++) {
			std::cout << "(" << _graph[source(*e_it, _graph)].id <<
				"," << _graph[target(*e_it, _graph)].id <<
				")" << " = " << _graph[*e_it].cost << std::endl;
		}
	}

	std::cout << std::endl;
	print_graph(_graph, get(&Vertex::id, _graph));
	std::cout << std::endl;
#endif
}

void NetworkGraph::add_routes_random()
{
	std::vector<unsigned int> busy_pixel;
	unsigned int id_count = 0;
	bool maxed_id = false;

	// place the resulting vector to a temp file to be processed afterwards
	std::string tmp_filename(".wn_tmp");
	std::ofstream out_file(tmp_filename);

	// populate the grid with a certain propability
	// TODO: drop hardcoded probability and allow user selection
	for (unsigned int y = 0; y < _grid_dim; y++) {
		for (unsigned int x = 0; x < _grid_dim; x++) {
			if ((rand() % 8) < 1) {
				unsigned int pixel = x + y * _grid_dim;
				Vertex *vertex = new Vertex({id_count++, {x, y}});
				_vertex_lut.push_back(std::make_pair(vertex->id, vertex));
				_grid.at(pixel) = vertex;
				busy_pixel.push_back(pixel);
			}
			if (id_count > MAX_NUM_VERTICES) {
				maxed_id = true;
				break;
			}
		}
		if (maxed_id)
			break;
	}

#ifdef DEBUG
	std::cout << "random> grid created with " << busy_pixel.size() <<
				 " vertices" << std::endl;
#endif
	_calc_adjacent_list(busy_pixel);

	for (auto item : _adj_list) {
		out_file << item.second.first << ' ' << item.second.second << ' ' <<
					item.first << ' ' <<
					_vertex_lut[item.second.first].second->coord.x << ' ' <<
					_vertex_lut[item.second.first].second->coord.y << ' ' <<
					_vertex_lut[item.second.second].second->coord.x << ' ' <<
					_vertex_lut[item.second.second].second->coord.y << std::endl;
//		for (auto msg : _vertex_lut[item.second.second].second->msg_table) {
//			std::cout << msg.payload.msg << std::endl;
//		}
	}

	_vertex_lut.clear();
	out_file.close();
	_graph_filename = tmp_filename;

#ifdef DEBUG
	std::cout << "filename: " << tmp_filename << std::endl;
#endif
}

void NetworkGraph::_calc_adjacent_list(std::vector<unsigned int> busy_pixel)
{
	// the distance between pixel will be considered 1du (distance units)
	const unsigned int dist_pixel = 1;

	// maximum field of view (range) for which we consider when a vertex can
	// actually "see" other vertex.
	const unsigned int max_fov = 4 * dist_pixel;

	// traverse the vector of pixels calculating the euclidean distance
	// between every vertex to determine what which can actually see
	for (unsigned int i = 0; i < busy_pixel.size(); i++) {
		unsigned int src_pixel = busy_pixel.at(i);
		Vertex *src_vertex = _grid.at(src_pixel);

		for (unsigned int j = i + 1; j < busy_pixel.size(); j++) {
//		for (unsigned int j = 0; j < busy_pixel.size(); j++) {
			unsigned int dst_pixel = busy_pixel.at(j);
			Vertex *dst_vertex = _grid.at(dst_pixel);

			unsigned int dx = dist_pixel * (src_vertex->coord.x -
							dst_vertex->coord.x);
			unsigned int dy = dist_pixel * (src_vertex->coord.y -
							dst_vertex->coord.y);
			unsigned int euc_dist = static_cast<unsigned int>(
										std::sqrt(dx * dx + dy * dy));
			if (euc_dist < max_fov) {
				_adj_list.push_back(std::make_pair(euc_dist,
												   std::make_pair(src_vertex->id,
																  dst_vertex->id)));
			}
		}
	}

#ifdef DEBUG
	std::cout << "random> distance vector defined" << std::endl;
	for (auto item : _adj_list) {
		std::cout << item.second.first << " " << item.second.second << " "
			<< item. first << std::endl;
	}
#endif
}

int NetworkGraph::calc_routing_graph()
{
#ifdef PRE_PROCESSING
	std::vector<unsigned int> distance(num_vertices(_graph));
	vertex_desc_t curr_vertex_desc = 0;

	// run the whole graph collecting each distance for all vertex
	for (auto src_vpair : _vertex_lut) {
		dijkstra_shortest_paths(_graph, src_vpair.second->self,
			predecessor_map(get(&Vertex::prev, _graph))
			.distance_map(make_iterator_property_map(distance.begin(),
				get(vertex_index, _graph)))
			.weight_map(get(&Edge::cost, _graph)));

		for (auto dst_vpair : _vertex_lut) {
			route_t route;
			route.first.cost = distance[dst_vpair.second->self];
			curr_vertex_desc = dst_vpair.second->self;
			while (curr_vertex_desc != src_vpair.second->self) {
				// if the following condition is true and the while didn't
				// finish, it means the vertices are not in the same graph
				if (curr_vertex_desc == _graph[curr_vertex_desc].prev)
					return -1;

				route.second.push_back(_vertex_lut[curr_vertex_desc].second);
				_vertex_lut[curr_vertex_desc].second->prev = _graph[curr_vertex_desc].prev;
				curr_vertex_desc = _vertex_lut[curr_vertex_desc].second->prev;
			}
			_vertex_lut[src_vpair.second->self].second->routing_table.push_back(route);
		}
	}
#endif // PRE_PROCESSING
	return 0;
}

route_t NetworkGraph::find_shortest_path(unsigned int src, unsigned int dst)
{
#ifdef PRE_PROCESSING
	return _vertex_lut[src].second->routing_table[dst];
#else
	std::vector<unsigned int> distance(num_vertices(_graph));
	vertex_desc_t curr_vertex_desc = 0;
	route_t route;

	// run the whole graph collecting each distance for all vertex
	dijkstra_shortest_paths(_graph, _vertex_lut[src].second->self,
		predecessor_map(get(&Vertex::prev, _graph))
		.distance_map(make_iterator_property_map(distance.begin(),
			get(vertex_index, _graph)))
		.weight_map(get(&Edge::cost, _graph)));

	route.first = distance[_vertex_lut[dst].second->self];
	curr_vertex_desc = _vertex_lut[dst].second->self;
	while (curr_vertex_desc != _vertex_lut[src].second->self) {
		// if the following condition is true and the while didn't finish,
		// it means the vertices are not in the same graph
		if (curr_vertex_desc == _graph[curr_vertex_desc].prev)
			return std::make_pair(0, std::vector<unsigned int>{});

		route.second.push_back(_graph[curr_vertex_desc].id);
		curr_vertex_desc = _graph[curr_vertex_desc].prev;
	}
	return route;
#endif // PRE_PROCESSING
}

std::vector<edge_t> NetworkGraph::graph_edges() const
{
	std::vector<edge_t> edges_list;

	auto epair = edges(_graph);
	for (auto it = epair.first; it != epair.second; it++) {
		Vertex *src_v = _vertex_lut[_graph[source(*it, _graph)].id].second;
		Vertex *dst_v = _vertex_lut[_graph[target(*it, _graph)].id].second;
		Edge edge = _graph[*it];

		auto vpair = std::make_pair(src_v, dst_v);
		edges_list.push_back(std::make_pair(edge, vpair));
	}

#ifdef DEBUG
	std::cout << "Uniq edges:" << std::endl;
	for (auto edge : edges_list)
		std::cout << edge.second.first->id << "," << edge.second.second->id <<
					 " = " << edge.first.cost << std::endl;
#endif

	return edges_list;
}

// clear everything
void NetworkGraph::clear_graph()
{
	_vertex_lut.clear();
	_grid.clear();
	_grid.resize(_grid_dim * _grid_dim);
	_adj_list.clear();
	_graph.clear();
	_graph_filename.clear();
}

Vertex * NetworkGraph::vertex(unsigned int id)
{
	return _vertex_lut[id].second;
}


#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>
#include <boost/thread/mutex.hpp>

#define RIGHT_STICK_LR (3)
#define RIGHT_STICK_UD (4)

#define MAX_DELTA_LINEAR (.5)
#define MAX_DELTA_ANGULAR (.5)

boost::mutex set_point;
double linear=0, angular=0;

void joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
	boost::mutex::scoped_lock lock(set_point);

	linear = joy->axes[RIGHT_STICK_UD];
	angular = -joy->axes[RIGHT_STICK_LR];
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "cylbot_xbox_teleop");

	ros::NodeHandle nh;
	ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("cylbot/cmd_vel", 1);

	ros::Subscriber joy_sub = nh.subscribe<sensor_msgs::Joy>("joy", 10, &joyCallback);

	geometry_msgs::Twist cmd_msg;
	double p = .05;

	double dt = 1/100.0;
	ros::Rate loop_rate(100.0);
	while(ros::ok())
	{
		{
			boost::mutex::scoped_lock lock(set_point);

			double delta_linear = linear - cmd_msg.linear.x;
			double delta_angular = angular - cmd_msg.angular.z;

			// saturate the linear command
			if(delta_linear/dt > MAX_DELTA_LINEAR)
				delta_linear = MAX_DELTA_LINEAR*dt;
			else if(delta_linear/dt < -MAX_DELTA_LINEAR)
				delta_linear = -MAX_DELTA_LINEAR*dt;

			// saturate the angular command
			if(delta_angular/dt > MAX_DELTA_ANGULAR)
				delta_angular = MAX_DELTA_ANGULAR*dt;
			else if(delta_angular/dt < -MAX_DELTA_ANGULAR)
				delta_angular = -MAX_DELTA_ANGULAR*dt;

			cmd_msg.linear.x += delta_linear;
			cmd_msg.angular.z += delta_angular;
		}

		cmd_pub.publish(cmd_msg);

		ros::spinOnce();
		loop_rate.sleep();
	}
}


#include <iostream>
#include <fstream>
#include <deque>
#include "map"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <string>
#include <sstream>
#include <iomanip>
#include <bitset>
#include <climits>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/gzip.hpp>

using namespace std;
//ofstream OutFile;
FILE * trace;
map<long int, bitset<64>> unique_map; //pfn, unique_CCs
map<long int, long int> refs_per_page; //pfn, number of accesses
long int cnt_arr[65];
long int pages=0;
long int pages1=0;
// The running count of instructions is kept here
// make it static to help the compiler optimize docount

void processMap(long int addr) {
  long int page_num = 0;
  long int cc = 0;
  page_num = addr>>12;
  cc = (addr>>6)&0x3f;
  auto mapit = unique_map.find(page_num);
  if(mapit == unique_map.end()) {
    bitset<64> newbit_vector;
    newbit_vector[cc] = 1;
    unique_map[page_num] = newbit_vector;
    //unique_map[page_num] = 1;
    pages1++;
  } else {
    (mapit->second)[cc] = 1;
  }
}

void processMap1(long int addr) {
  long int page_num = addr>>12;
  auto fit = refs_per_page.find(page_num);
  if(fit == refs_per_page.end()) {
    refs_per_page[page_num] = 1;
  } else {
    fit->second = (fit->second) + 1;
  }
}
/* ===================================================================== */
/* Main                                                                  */
/* ===================================================================== */

int main(int argc, char * argv[]) {

  ifstream file(argv[1], ios_base::in | ios_base::binary);
  boost::iostreams::filtering_istream in;
  in.push(boost::iostreams::gzip_decompressor());
  in.push(file);

  string line;
  long int prev_addr=0;
  for(string str; getline(in, str);) {
    istringstream iss(str);
    long int cyc;
    char rw;
    string hexaddr;
    long int addr;
    if(!(iss>>rw>>hexaddr)) { break; }
    else {
      addr=0;
      hexaddr.erase(0,2);
      addr = std::stoul(hexaddr, NULL, 16);
      processMap(addr);
      processMap1(addr);
    }
  }
  for(int x=0; x<65; x++)
    cnt_arr[x]=0;

  //iterate the unique map to find the unique CCs for that page
  //find the memory refs for the page
  //increment the array which maps the unique CCs to references
  for(auto it = unique_map.begin(); it != unique_map.end(); it++) {
    bitset<64> bitvec = it->second;
    int uniq = bitvec.count();
    long int refs = refs_per_page[it->first];
    cnt_arr[uniq] = cnt_arr[uniq] + refs;
  }
  for(int x=0; x<65; x++)
    cout<<x<<","<<cnt_arr[x]<<endl;
  cout<<" Unique Pages= "<<pages<<endl;
  cout<<" Unique Pages= "<<pages1<<endl;
}


/*
 *  testOrientationJacobians.cpp
 *  Trogdor6
 *
 *  Created by Paul C Hansen on 8/14/11.
 *  Copyright 2011 Stanford University. All rights reserved.
 *
 *  This file is covered by the MIT license.  See LICENSE.txt.
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE testFillFactorJacobians

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <cstdlib>
#include <ctime>
#include "Voxelize.h"
#include "SimpleMesh/Search.h"

using namespace std;
using namespace RLE;
using namespace SimpleMesh;

static vector<ControlVertex> sBlockVertices(Rect3d r, double angle = 0.0);

static vector<Vector3d> sSubtract(const vector<ControlVertex> & cv0,
    const vector<ControlVertex> & cv1, double dh = 1.0)
{
    vector<Vector3d> out(cv0.size());
    assert(cv0.size() == cv1.size());

    for (int nn = 0; nn < cv0.size(); nn++)
        out[nn] = (cv0[nn].point() - cv1[nn].point()) / dh;

    return out;
}

static Matrix3d sDot(Matrix3<Vector3d> lhs, Vector3d rhs)
{
    Matrix3d mat;
    for (int nn = 0; nn < 9; nn++)
        mat[nn] = dot(lhs[nn], rhs);

    return mat;
}

static void printJacobians(const map<unsigned int, DynamicRLE3<Matrix3<Vector3d> > > & m);

static void sStandardBlock(int chosenTriangle);

BOOST_AUTO_TEST_CASE( StandardBlock )
{
    for (int tt = 0; tt < 12; tt++)
    {
//        cerr << "Chosen triangle " << tt << "\n";
        sStandardBlock(tt);
    }

    sStandardBlock(-1);
}

static void sStandardBlock(int chosenTriangle)
{
    Rect3d bounds(-3,-2,-1,3,2,1);
    Rect3i voxels(-1,-1,-1,1,1,1);
    Voxelize vox(bounds, voxels);

    Rect3d rect0(-2, -4.0/3.0, -2.0/3.0, 2, 4.0/3, 2.0/3);

    std::vector<SimpleMesh::ControlVertex> controlVertices1, controlVertices2;
    std::vector<SimpleMesh::Triangle> meshTris1, meshTris2;
    SimpleMesh::Test::oneBoxMesh(controlVertices1, meshTris1, rect0);

    if (chosenTriangle != -1)
        meshTris1 = std::vector<SimpleMesh::Triangle>(1, meshTris1.at(chosenTriangle));

    DynamicRLE3<Matrix3d> orientations0 = vox.orientations(meshTris1);
    map<unsigned int, DynamicRLE3<Matrix3<Vector3d> > > DNNT =
        vox.orientationJacobians(meshTris1);

//    cerr << block0 << "\n";
//    cerr << "Orientations #0:\n" << orientations0 << "\n";
//
//    printJacobians(DNNT);

//    vector<ControlVertex> cvs = sBlockVertices(rect0);
//    copy(cvs.begin(), cvs.end(), ostream_iterator<ControlVertex>(cerr, "\n"));

    double epsilon = 1e-7;
    for (int translateXYZ = 0; translateXYZ < 3; translateXYZ++)
    {
//        cerr << "Translate " << char('x'+translateXYZ) << "\n";
        Rect3d rect1 = rect0 + epsilon*Vector3d::unit(translateXYZ);

        SimpleMesh::Test::oneBoxMesh(controlVertices2, meshTris2, rect1);
        if (chosenTriangle != -1)
            meshTris2 = std::vector<SimpleMesh::Triangle>(1, meshTris2.at(chosenTriangle));

        DynamicRLE3<Matrix3d> orientations1 = vox.orientations(meshTris2);
//        cerr << "Orientations #1:\n" << orientations1 << "\n";

        DynamicRLE3<Matrix3d> deltaOr = (orientations1 - orientations0);
        vector<Vector3d> deltaCV = SimpleMesh::Test::subtractPositions(controlVertices2, controlVertices1);

        // Test the jacobian now.
        DynamicRLE3<Matrix3d> orPredicted(orientations0);

        for (int vv = 0; vv < deltaCV.size(); vv++)
        {
            DynamicRLE3<Matrix3d> dotProd;
            RLE::scalarTransform(DNNT[vv], deltaCV[vv], dotProd, sDot);
            orPredicted = orPredicted + dotProd;
//            cerr << "v" << vv << ": dot =\n" << dotProd << "\n";
//            cerr << "orPredicted is " << orPredicted << "\n";
        }

        //cerr << "Prediction:\n" << ffPredicted << "\n";

        DynamicRLE3<Matrix3d> deltaOrMeas = (orientations1-orientations0);
        DynamicRLE3<Matrix3d> deltaOrPred = (orPredicted - orientations0);

//        cerr << "Derivative measured:\n" << dOrMeas;
//        cerr << "\nDerivative expected:\n" << dOrPred
//            << "\n";

        bool failedAnywhere = false;
        for (int zz = voxels.p1[2]; zz <= voxels.p2[2]; zz++)
        for (int yy = voxels.p1[1]; yy <= voxels.p2[1]; yy++)
        for (int xx = voxels.p1[0]; xx <= voxels.p2[0]; xx++)
        {
//            cerr << "xyz " << xx << ", " << yy << ", " << zz << "\n";
            bool failed = 0;
            for (int ii = 0; ii < 3; ii++)
            for (int jj = 0; jj < 3; jj++)
            {
//                BOOST_CHECK_SMALL(normInf(
//                    dOrPred.markAt(xx,yy,zz, Matrix3d()) -
//                    dOrMeas.markAt(xx,yy,zz, Matrix3d())), 1e-5);

                if (normInf(
                    deltaOrPred.markAt(xx,yy,zz, Matrix3d()) -
                    deltaOrMeas.markAt(xx,yy,zz, Matrix3d())) > 1e-5)
                {
                    failed = true;
                    failedAnywhere = true;
                }
            }

            if (failed)
            {
//                cerr << "FAIL: tri " << chosenTriangle << " @" << Vector3i(xx,yy,zz) << " translate "
//                    << char('x'+translateXYZ) << "\n";
//
//                cerr << "quickPatch(" << block0.triangles()[chosenTriangle]
//                    << ", 'g');\n";
//                cerr << "hold on;\n";
//
                Rect3d thisVoxel;

                thisVoxel = vox.voxel(xx, yy ,zz);
//                cerr << "quickRect(" << thisVoxel << ", 'r');\n";
//
//                cerr << "Predicted derivative:\n"
//                    << dOrPred.markAt(xx,yy,zz, Matrix3d()) << "\n"
//                    << "Measured derivative:\n"
//                    << dOrMeas.markAt(xx,yy,zz, Matrix3d()) << "\n";
            }
        }
        BOOST_CHECK(failedAnywhere == false);
    }
}

BOOST_AUTO_TEST_CASE( RotateBlock )
{
//    Rect3d bounds(-2, -2, -2, 2, 2, 2);
//    Rect3i voxels(-1,-1,-1,1,1,1);
//    Rect3i voxels(0,0,0,1,1,0);
    Rect3d bounds(0,-2,-2,2,0,0);
    Rect3i voxels(0,0,0,0,0,0);
    Voxelize vox(bounds, voxels);

    Rect3d rect0(-1, -1, -1, 1, 1, 1);

    std::vector<SimpleMesh::ControlVertex> controlVertices1, controlVertices2;
    std::vector<SimpleMesh::Triangle> meshTris1, meshTris2;
    SimpleMesh::Test::oneBoxMesh(controlVertices1, meshTris1, rect0);

//    TrackedPolyhedron block0 = sBlock(rect0);

    DynamicRLE3<Matrix3d> orientations0 = vox.orientations(meshTris1);
    map<unsigned int, DynamicRLE3<Matrix3<Vector3d> > > DNNT =
        vox.orientationJacobians(meshTris1);

//    cerr << "Orientations #0:\n" << orientations0 << "\n";
//
//    printJacobians(DNNT);

//    vector<ControlVertex> cvs = sBlockVertices(rect0);
//    copy(cvs.begin(), cvs.end(), ostream_iterator<ControlVertex>(cerr, "\n"));

    double epsilon = 1e-8;

    {
        double theta = epsilon*2*M_PI;

        SimpleMesh::Test::oneBoxMesh(controlVertices2, meshTris2, rect0, Matrix3d::rotation(theta, Vector3d(0,0,1)));
//        TrackedPolyhedron block1 = sBlock(rect0, theta);
        BOOST_CHECK(true);
        DynamicRLE3<Matrix3d> orientations1 = vox.orientations(meshTris2);

//        cerr << "Orientations #1:\n" << orientations1 << "\n";
        BOOST_CHECK(true);
        DynamicRLE3<Matrix3d> deltaOr = (orientations1 - orientations0);

        vector<Vector3d> deltaCV = SimpleMesh::Test::subtractPositions(controlVertices2, controlVertices1);

//        std::copy(deltaCV.begin(), deltaCV.end(), ostream_iterator<Vector3d>(cerr, "\n"));

        // Test the jacobian now.
        DynamicRLE3<Matrix3d> orPredicted(orientations0);

//        cerr << "orPredicted BEGINS as " << orPredicted << "\n";

        for (int vv = 0; vv < deltaCV.size(); vv++)
        if (DNNT.count(vv))
        {
            BOOST_CHECK(true);
            DynamicRLE3<Matrix3d> dotProd;
            BOOST_CHECK(true);
            RLE::scalarTransform(DNNT[vv], deltaCV[vv], dotProd, sDot);
            BOOST_CHECK(true);
            orPredicted = orPredicted + dotProd;
            BOOST_CHECK(true);
//            cerr << "v" << vv << ": dot =\n" << dotProd << "\n";
//            cerr << "orPredicted is " << orPredicted << "\n";
        }

        //cerr << "Prediction:\n" << ffPredicted << "\n";
        BOOST_CHECK(true);
        DynamicRLE3<Matrix3d> dOrMeas = (orientations1-orientations0)/epsilon;
        BOOST_CHECK(true);
        DynamicRLE3<Matrix3d> dOrPred = (orPredicted - orientations0)/epsilon;

//        cerr << "Derivative measured:\n" << dOrMeas;
//        cerr << "\nDerivative expected:\n" << dOrPred
//            << "\n";

        for (int zz = voxels.p1[2]; zz <= voxels.p2[2]; zz++)
        for (int yy = voxels.p1[1]; yy <= voxels.p2[1]; yy++)
        for (int xx = voxels.p1[0]; xx <= voxels.p2[0]; xx++)
        {
            for (int ii = 0; ii < 3; ii++)
            for (int jj = 0; jj < 3; jj++)
            {
                BOOST_CHECK_SMALL(normInf(
                    dOrPred.markAt(xx,yy,zz, Matrix3d()) -
                    dOrMeas.markAt(xx,yy,zz, Matrix3d())), 1e-5);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE( RotateTriangle )
{
//    cerr << "\n\n*** One tri ***\n\n";
//    Rect3d bounds(-2, -2, -2, 2, 2, 2);
//    Rect3i voxels(-1,-1,-1,1,1,1);
//    Rect3i voxels(0,0,0,1,1,0);
//    Rect3d bounds(0,0,0,2,2,2);
//    Rect3i voxels(0,0,0,0,0,0);

    Rect3d bounds(0,-2,-2,2,0,0);
    Rect3i voxels(0,0,0,0,0,0);
    Voxelize vox(bounds, voxels);

//    Triangle3d tri0(Vector3d(1, 1, -1), Vector3d(1, 1, 3),
//        Vector3d(1, -3, -1));

//    Triangle3d tri0(Vector3d(1,3,-1), Vector3d(1,-1,3), Vector3d(1,-1,-1));
    Triangle3d tri0(Vector3d(1,3,-3), Vector3d(1,-1,-3), Vector3d(1,-1,1));

    vector<Triangle> tris0;
    tris0.push_back(SimpleMesh::Test::oneTri(tri0[0], tri0[1], tri0[2]));

    BOOST_CHECK(true);
    DynamicRLE3<Matrix3d> orientations0 = vox.orientations(tris0);
    map<unsigned int, DynamicRLE3<Matrix3<Vector3d> > > DNNT =
        vox.orientationJacobians(tris0);

//    cerr << "Orientations #0:\n" << orientations0 << "\n";

//    printJacobians(DNNT);

//    vector<ControlVertex> cvs = sBlockVertices(rect0);
//    copy(cvs.begin(), cvs.end(), ostream_iterator<ControlVertex>(cerr, "\n"));

    double epsilon = 1e-8;

    {
        double theta = epsilon*2*M_PI;
        Matrix3d rot(cos(theta), -sin(theta), 0.0,
            sin(theta), cos(theta), 0.0,
            0.0, 0.0, 1.0);

        Triangle3d tri1 = rot*tri0;
        vector<Triangle> tris1;
        tris1.push_back(SimpleMesh::Test::oneTri(tri1[0], tri1[1], tri1[2]));

        DynamicRLE3<Matrix3d> orientations1 = vox.orientations(tris1);

//        cerr << "Orientations #1:\n" << orientations1 << "\n";

        DynamicRLE3<Matrix3d> deltaOr = (orientations1 - orientations0)/epsilon;

        BOOST_CHECK(true);
        vector<Vector3d> deltaCV(3);
        for (int nn = 0; nn < 3; nn++)
            deltaCV[nn] = tri1[nn] - tri0[nn];

//        cerr << "Change in CVs:\n";
//        std::copy(deltaCV.begin(), deltaCV.end(), ostream_iterator<Vector3d>(cerr, "\n"));

        // Test the jacobian now.
        DynamicRLE3<Matrix3d> orPredicted(orientations0);

        BOOST_CHECK(true);
//        cerr << "orPredicted BEGINS as " << orPredicted << "\n";

        for (int vv = 0; vv < deltaCV.size(); vv++)
        {
            BOOST_CHECK(true);
            DynamicRLE3<Matrix3d> dotProd;
            BOOST_CHECK(true);
            RLE::scalarTransform(DNNT[vv], deltaCV[vv], dotProd, sDot);
            BOOST_CHECK(true);
            orPredicted = orPredicted + dotProd;
            BOOST_CHECK(true);
//            cerr << "v" << vv << ": dot =\n" << dotProd << "\n";
//            cerr << "orPredicted is " << orPredicted << "\n";
        }

        //cerr << "Prediction:\n" << ffPredicted << "\n";

        BOOST_CHECK(true);
        DynamicRLE3<Matrix3d> dOrMeas = (orientations1-orientations0)/epsilon;

        BOOST_CHECK(true);
        DynamicRLE3<Matrix3d> dOrPred = (orPredicted - orientations0)/epsilon;

//        cerr << "Derivative measured:\n" << dOrMeas;
//        cerr << "\nDerivative expected:\n" << dOrPred
//            << "\n";

        for (int zz = voxels.p1[2]; zz <= voxels.p2[2]; zz++)
        for (int yy = voxels.p1[1]; yy <= voxels.p2[1]; yy++)
        for (int xx = voxels.p1[0]; xx <= voxels.p2[0]; xx++)
        {
            for (int ii = 0; ii < 3; ii++)
            for (int jj = 0; jj < 3; jj++)
            {
                BOOST_CHECK_SMALL(normInf(
                    dOrPred.markAt(xx,yy,zz, Matrix3d()) -
                    dOrMeas.markAt(xx,yy,zz, Matrix3d())), 1e-5);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE( OneTriangle )
{
//    return;
//    cerr << "OneTriangle\n";

    Rect3d bounds(0,0,0,1,1,1);
//    Rect3i voxels(0,0,0,0,0,0); // one voxel
    Rect3i voxels(0,0,0,1,0,0); // split X
//    Rect3i voxels(0,0,0,0,1,0); // split Y
//    Rect3i voxels(0,0,0,0,0,1); // split Z
    Voxelize vox(bounds, voxels);

//    Triangle3d t0(Vector3d(-0.5, 0.5, 0.5),
//        Vector3d(0.5, 0.5, 0.5), Vector3d(-0.5, 1.5, 0.5));
//    Triangle3d t0(Vector3d(0.1, 0.1, 0.1),
//        Vector3d(0.1, 0.9, 0.1), Vector3d(0.9, 0.1, 0.1)); // downward
    Triangle3d t0(Vector3d(0.1, 0.9, 0.1),
        Vector3d(0.9, 0.9, 0.1), Vector3d(0.9, 0.1, 0.1));

    vector<Triangle> triangles0;
    triangles0.push_back(SimpleMesh::Test::oneTri(t0[0], t0[1], t0[2]));

    DynamicRLE3<Matrix3d> orientations0 = vox.orientations(triangles0);
    map<unsigned int, DynamicRLE3<Matrix3<Vector3d> > > DNNT =
        vox.orientationJacobians(triangles0);

//    cerr << "Orientations #0:\n" << orientations0 << "\n";

//    printJacobians(DNNT);

    double epsilon = 1e-7;
    for (int translateXYZ = 0; translateXYZ < 3; translateXYZ++)
    {
//        cerr << "Translate " << char('x'+translateXYZ) << "\n";

        Triangle3d t1 = t0;
        for (int mm = 0; mm < 3; mm++)
            t1[mm] += epsilon*Vector3d::unit(translateXYZ);

        vector<Triangle> triangles1;
        triangles1.push_back(SimpleMesh::Test::oneTri(t1[0], t1[1], t1[2]));

        DynamicRLE3<Matrix3d> orientations1 = vox.orientations(triangles1);
        //vox.fillFactorJacobians(block1.triangles());

//        cerr << "Orientations #1:\n" << orientations1 << "\n";

        DynamicRLE3<Matrix3d> deltaOr = (orientations1 - orientations0)/epsilon;

        vector<Vector3d> deltaCV(3);
        for (int nn = 0; nn < 3; nn++)
            deltaCV[nn] = t1[nn] - t0[nn];

        //std::copy(deltaCV.begin(), deltaCV.end(), ostream_iterator<Vector3d>(cerr, "\n"));

        // Test the jacobian now.
        DynamicRLE3<Matrix3d> orPredicted(orientations0);

//        cerr << "orPredicted BEGINS as " << orPredicted << "\n";

        for (int vv = 0; vv < deltaCV.size(); vv++)
        {
            DynamicRLE3<Matrix3d> dotProd;
            RLE::scalarTransform(DNNT[vv], deltaCV[vv], dotProd, sDot);
            orPredicted = orPredicted + dotProd;
//            cerr << "v" << vv << ": dot =\n" << dotProd << "\n";
//            cerr << "orPredicted is " << orPredicted << "\n";
        }

        //cerr << "Prediction:\n" << ffPredicted << "\n";

        DynamicRLE3<Matrix3d> dOrMeas = (orientations1-orientations0)/epsilon;
        DynamicRLE3<Matrix3d> dOrPred = (orPredicted - orientations0)/epsilon;

//        cerr << "Derivative measured:\n" << dOrMeas;
//        cerr << "\nDerivative expected:\n" << dOrPred
//            << "\n";

        for (int zz = voxels.p1[2]; zz <= voxels.p2[2]; zz++)
        for (int yy = voxels.p1[1]; yy <= voxels.p2[1]; yy++)
        for (int xx = voxels.p1[0]; xx <= voxels.p2[0]; xx++)
        {
            bool problem = 0;

            for (int ii = 0; ii < 3; ii++)
            for (int jj = 0; jj < 3; jj++)
            {
//                BOOST_CHECK_SMALL(normInf(
//                    dOrPred.markAt(xx,yy,zz, Matrix3d()) -
//                    dOrMeas.markAt(xx,yy,zz, Matrix3d())), 1e-5);

                if (normInf(dOrPred.markAt(xx,yy,zz, Matrix3d()) -
                    dOrMeas.markAt(xx,yy,zz, Matrix3d())) > 1e-5)
                    problem = true;
            }
            if (problem)
            {
                cerr << "Triangle (" << triangles0[0] << " normal "
                    << triangles0[0].unitNormal() << ") perturbed along "
                    << char('x'+translateXYZ) << " is wrong in cell "
                    << Vector3i(xx,yy,zz) << ".\n";
            }
        }
    }
}

//static TrackedPolyhedron sBlock(Rect3d r, double angle)
//{
//    Matrix3d rot(cos(angle), -sin(angle), 0.0,
//        sin(angle), cos(angle), 0.0,
//        0.0, 0.0, 1.0);
//
//    NefBuilder nef = NefBuilder::block(r, 0, rot);
//
//    TrackedPolyhedron tp(nef.polyhedron(), nef.getControlVertexMap());
////    tp.cacheSensitivity();
//
//    return tp;
//}
//
//static vector<ControlVertex> sBlockVertices(Rect3d r, double angle)
//{
//    Matrix3d rot(cos(angle), -sin(angle), 0.0,
//        sin(angle), cos(angle), 0.0,
//        0.0, 0.0, 1.0);
//    NefBuilder nef = NefBuilder::block(r, 0, rot);
//
//    return nef.controlVertices();
//}

static void printJacobians(const map<unsigned int, DynamicRLE3<Matrix3<Vector3d> > > & m)
{
    map<unsigned int, DynamicRLE3<Matrix3<Vector3d> > >::const_iterator itr;

    for (itr = m.begin(); itr != m.end(); itr++)
    {
        cout << "CV" << itr->first << ": \n";
        cout << itr->second << "\n";
    }
}