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

#include "Utils/FIRMUtils.h"
#include <boost/math/constants/constants.hpp>
#include <boost/date_time.hpp>
#include <utility>
#include <random>
#include <tinyxml.h>

void FIRMUtils::normalizeAngleToPiRange(double &theta)
{

    while(theta > boost::math::constants::pi<double>())
    {
        theta -= 2*boost::math::constants::pi<double>();
    }

    while(theta < -boost::math::constants::pi<double>())
    {
        theta += 2*boost::math::constants::pi<double>();
    }

}

int FIRMUtils::signum(const double d)
{
        if(d>0)
            return 1;

        if(d<0)
            return -1;
}

int FIRMUtils::generateRandomIntegerInRange(const int floor, const int ceiling)
{
    //std::random_device rd; // obtain a random number from hardware

    //std::mt19937 eng(rd()); // seed the generator

    //std::uniform_int_distribution<> distr(floor, ceiling); // define the range

    //return distr(eng);

    int r = rand()%(ceiling - floor + 1) + floor;

    return r;
}

void FIRMUtils::writeFIRMGraphToXML(const std::vector<std::pair<int,std::pair<arma::colvec,arma::mat> > > nodes, const std::vector<std::pair<std::pair<int,int>,FIRMWeight> > edgeWeights)
{
    TiXmlDocument doc;

 	TiXmlDeclaration* decl = new TiXmlDeclaration( "1.0", "", "" );
	doc.LinkEndChild( decl );

	TiXmlElement * Nodes = new TiXmlElement( "Nodes" );
	doc.LinkEndChild( Nodes );

	for(int i = 0; i < nodes.size(); i++)
	{
        TiXmlElement * node;
        node = new TiXmlElement( "node" );
        Nodes->LinkEndChild( node );

        int nodeID = nodes[i].first; // id of the node in the graph

        arma::colvec xVec = nodes[i].second.first; // x,y,yaw

        arma::mat cov = nodes[i].second.second; // covariance matrix

        node->SetAttribute("id", nodeID);
        node->SetDoubleAttribute("x", xVec(0));
        node->SetDoubleAttribute("y", xVec(1));
        node->SetDoubleAttribute("theta",xVec(2));
        node->SetDoubleAttribute("c11", cov(0,0));
        node->SetDoubleAttribute("c12", cov(0,1));
        node->SetDoubleAttribute("c13", cov(0,2));
        node->SetDoubleAttribute("c21", cov(1,0));
        node->SetDoubleAttribute("c22", cov(1,1));
        node->SetDoubleAttribute("c23", cov(1,2));
        node->SetDoubleAttribute("c31", cov(2,0));
        node->SetDoubleAttribute("c32", cov(2,1));
        node->SetDoubleAttribute("c33", cov(2,2));

   }

    TiXmlElement * Edges = new TiXmlElement( "Edges" );
	doc.LinkEndChild( Edges );

	for(int i = 0; i < edgeWeights.size(); i++)
	{
        TiXmlElement * edge;
        edge = new TiXmlElement( "edge" );
        Edges->LinkEndChild( edge );

        FIRMWeight w = edgeWeights[i].second;

        edge->SetAttribute("startVertexID", edgeWeights[i].first.first);
        edge->SetAttribute("endVertexID", edgeWeights[i].first.second);
        edge->SetDoubleAttribute("successProb", w.getSuccessProbability());
        edge->SetDoubleAttribute("cost", w.getCost());

   }

   // Generate time stamp for saving roadmap
    namespace pt = boost::posix_time;

    pt::ptime now = pt::second_clock::local_time();

    std::string timeStamp(to_iso_string(now)) ;

    std::string roadmapFileName =  "FIRMRoadMap-" + timeStamp + ".xml";

	doc.SaveFile(roadmapFileName);
}

bool FIRMUtils::readFIRMGraphFromXML(const std::string &pathToXML, std::vector<std::pair<int, arma::colvec> > &FIRMNodePosList, std::vector<std::pair<int, arma::mat> > &FIRMNodeCovarianceList, std::vector<std::pair<std::pair<int,int>,FIRMWeight> > &edgeWeights)
{

    TiXmlDocument doc(pathToXML);

    bool loadOkay = doc.LoadFile();

    if ( !loadOkay )
    {
        OMPL_INFORM("FIRMUtils: Could not load Graph from XML . Need to construct graph.");
        return false;
    }

    TiXmlNode* NodeList = 0;

    TiXmlElement* nodeElement = 0;

    TiXmlElement* itemElement = 0;

    NodeList = doc.FirstChild( "Nodes" );

    assert( NodeList );

    nodeElement = NodeList->ToElement(); //convert NodeList to element

    assert( nodeElement  );

    TiXmlNode* child = 0;

    while( (child = nodeElement->IterateChildren(child)))
    {
        assert( child );

        itemElement = child->ToElement();

        assert( itemElement );

        double x = 0, y = 0, theta = 0, c11 = 0, c12 = 0, c13 = 0, c21 = 0, c22 = 0, c23 = 0, c31 = 0, c32 = 0, c33 = 0;
        int id = 0;

        itemElement->QueryIntAttribute("id", &id) ;
        itemElement->QueryDoubleAttribute("x", &x) ;
        itemElement->QueryDoubleAttribute("y", &y) ;
        itemElement->QueryDoubleAttribute("theta", &theta) ;
        itemElement->QueryDoubleAttribute("c11", &c11) ;
        itemElement->QueryDoubleAttribute("c12", &c12) ;
        itemElement->QueryDoubleAttribute("c13", &c13) ;
        itemElement->QueryDoubleAttribute("c21", &c21) ;
        itemElement->QueryDoubleAttribute("c22", &c22) ;
        itemElement->QueryDoubleAttribute("c23", &c23) ;
        itemElement->QueryDoubleAttribute("c31", &c31) ;
        itemElement->QueryDoubleAttribute("c32", &c32) ;
        itemElement->QueryDoubleAttribute("c33", &c33) ;

        arma::colvec xVec(3);
        arma::mat cov(3,3);

        xVec(0) = x;
        xVec(1) = y;
        xVec(2) = theta;

        cov(0,0) = c11;
        cov(0,1) = c12;
        cov(0,2) = c13;
        cov(1,0) = c21;
        cov(1,1) = c22;
        cov(1,2) = c23;
        cov(2,0) = c31;
        cov(2,1) = c32;
        cov(2,2) = c33;

        FIRMNodePosList.push_back(std::make_pair(id,xVec));
        FIRMNodeCovarianceList.push_back(std::make_pair(id,cov));

    }

    //////////////////////
    TiXmlNode* edgeList = 0;

    TiXmlElement* edgeElement = 0;

    TiXmlElement* itemElement2 = 0;

    edgeList = doc.FirstChild( "Edges" );

    assert( edgeList );

    edgeElement = edgeList->ToElement(); //convert NodeList to element

    assert( edgeElement  );

    TiXmlNode* child2 = 0;

    while( (child2 = edgeElement->IterateChildren(child2)))
    {
        assert( child2 );

        itemElement2 = child2->ToElement();

        assert( itemElement2 );

        int startVertexID = 0, endVertexID = 0;
        double successProb = 0, cost = 0;

        itemElement2->QueryIntAttribute("startVertexID", &startVertexID) ;
        itemElement2->QueryIntAttribute("endVertexID", &endVertexID) ;
        itemElement2->QueryDoubleAttribute("successProb", &successProb) ;
        itemElement2->QueryDoubleAttribute("cost", &cost) ;

        FIRMWeight w(cost, successProb);

        edgeWeights.push_back(std::make_pair(std::make_pair(startVertexID, endVertexID),w));

    }

    return true;
}

double FIRMUtils::degree2Radian(double deg)
{
    return boost::math::constants::pi<double>()*deg/180.0;
}

double FIRMUtils::radian2Degree(double rads)
{
    return rads*180.0/boost::math::constants::pi<double>();
}


#include <kr_trackers/traj_gen.h>

#include <Eigen/LU>

TrajectoryGenerator::TrajectoryGenerator(unsigned int continuous_derivative_order, unsigned int minimize_derivative)
    : N_(2 * (continuous_derivative_order + 1)), R_(minimize_derivative)
{
}

void TrajectoryGenerator::setInitialConditions(const Vec3f &pos, const vec_Vec3f &derivatives)
{
  clearWaypoints();
  waypoints_.push_back(pos);

  initial_derivatives_.clear();
  initial_derivatives_.resize(N_ / 2);
  for(size_t i = 0; i < std::min(initial_derivatives_.size(), derivatives.size()); ++i)
  {
    initial_derivatives_[i] = derivatives[i];
  }
}

void TrajectoryGenerator::addWaypoint(const Vec3f &x)
{
  if(waypoints_.empty())
    setInitialConditions(x, vec_Vec3f());
  else
    waypoints_.push_back(x);
}

void TrajectoryGenerator::clearWaypoints(void)
{
  waypoints_.clear();
  coefficients_.clear();
  waypoint_times_.clear();
  initial_derivatives_.clear();
}

std::vector<float> TrajectoryGenerator::computeTimesTrapezoidSpeed(float v_des, float a_des) const
{
  std::vector<float> waypoint_times;
  waypoint_times.reserve(waypoints_.size());

  waypoint_times.push_back(0);  // First waypoint, t = 0

  if(waypoints_.size() < 2)
    return waypoint_times;

  const Vec3f &initial_vel_ = initial_derivatives_[0];
  const int initial_vel_sign = (initial_vel_.dot(waypoints_[1] - waypoints_[0]) >= 0) ? 1 : -1;
  const float v_initial = initial_vel_sign * initial_vel_.norm();

  std::vector<float> accumulated_dist;
  accumulated_dist.reserve(waypoints_.size());
  accumulated_dist.push_back(0);
  for(unsigned int i = 1; i < waypoints_.size(); ++i)
  {
    const float dist = (waypoints_[i] - waypoints_[i - 1]).norm();
    accumulated_dist.push_back(accumulated_dist[i - 1] + dist);
  }
  const float total_dist = accumulated_dist.back();

  float d_accel = std::abs(v_des * v_des - v_initial * v_initial) / 2 / a_des;
  float d_decel = v_des * v_des / 2 / a_des;
  float d_constant = (total_dist - (d_accel + d_decel));
  if(d_accel + d_decel > total_dist)
  {
    d_constant = 0;
    d_accel = total_dist / 2 - v_initial * v_initial / 4 / a_des;
    d_decel = total_dist - d_accel;
  }
  const float t_accel = std::sqrt(v_initial * v_initial + 2 * a_des * d_accel) / a_des;
  const float t_constant = d_constant / v_des;
  const float t_decel = std::sqrt(2 * a_des * d_decel) / a_des;

  for(unsigned int i = 1; i < waypoints_.size(); ++i)
  {
    if(accumulated_dist[i] <= d_accel)  // Accel
    {
      waypoint_times.push_back((std::sqrt(v_initial * v_initial + 2 * a_des * accumulated_dist[i]) - v_initial) /
                               a_des);
    }
    else if(accumulated_dist[i] <= d_accel + d_constant)  // Constant
    {
      waypoint_times.push_back(t_accel + (accumulated_dist[i] - d_accel) / v_des);
    }
    else  // Decel
    {
      waypoint_times.push_back(t_accel + t_constant + t_decel -
                               std::sqrt(2 * (total_dist - accumulated_dist[i]) / a_des));
    }
  }

  return waypoint_times;
}

std::vector<float> TrajectoryGenerator::computeTimesConstantSpeed(float avg_speed) const
{
  std::vector<float> waypoint_times;
  waypoint_times.reserve(waypoints_.size());

  waypoint_times.push_back(0);  // First waypoint, t = 0

  for(unsigned int i = 1; i < waypoints_.size(); ++i)
  {
    waypoint_times.push_back(waypoint_times[i - 1] + std::sqrt((waypoints_[i] - waypoints_[i - 1]).norm()) / avg_speed);
  }
  return waypoint_times;
}

const std::vector<float> &TrajectoryGenerator::getWaypointTimes() const
{
  return waypoint_times_;
}

float TrajectoryGenerator::getTotalTime() const
{
  return waypoint_times_.back();
}

// From https://stackoverflow.com/a/33454406
template <typename T>
T powInt(T x, unsigned int n)
{
  if(n == 0)
    return T{1};

  auto y = T{1};
  while(n > 1)
  {
    if(n % 2 == 1)
      y *= x;
    x *= x;
    n /= 2;
  }
  return x * y;
}

bool TrajectoryGenerator::calculate(const std::vector<float> &waypoint_times)
{
  if(waypoints_.size() < 2)
    return false;

  if(waypoint_times.size() != waypoints_.size())
  {
    printf("waypoint_times.size() != waypoints_.size()\n");
    return false;
  }

  const unsigned int num_waypoints = waypoints_.size();
  const unsigned int num_segments = num_waypoints - 1;
  // printf("num_segments: %d\n", num_segments);

  Eigen::MatrixXf A = Eigen::MatrixXf::Zero(num_segments * N_, num_segments * N_);  // Linear constraints
  Eigen::MatrixXf Q = Eigen::MatrixXf::Zero(num_segments * N_, num_segments * N_);  // Quadratic cost matrix
  for(unsigned int i = 0; i < num_segments; i++)
  {
    float seg_time = waypoint_times[i + 1] - waypoint_times[i];
    for(unsigned int n = 0; n < N_; n++)
    {
      // A_0
      if(n < N_ / 2)
      {
        int val = 1;
        for(unsigned int m = 0; m < n; m++)
          val *= (n - m);
        A(i * N_ + n, i * N_ + n) = val;
      }
      // A_T
      for(unsigned int r = 0; r < N_ / 2; r++)
      {
        if(r <= n)
        {
          int val = 1;
          for(unsigned int m = 0; m < r; m++)
            val *= (n - m);
          A(i * N_ + N_ / 2 + r, i * N_ + n) = val * powInt(seg_time, n - r);
        }
      }
      // Q
      for(unsigned int r = 0; r < N_; r++)
      {
        if(r >= R_ && n >= R_)
        {
          int val = 1;
          for(unsigned int m = 0; m < R_; m++)
            val *= (r - m) * (n - m);
          Q(i * N_ + r, i * N_ + n) = 2 * val * powInt(seg_time, r + n - 2 * R_ + 1) / (r + n - 2 * R_ + 1);
        }
      }
    }
  }
  const unsigned int num_fixed_derivatives = num_waypoints - 2 + N_;
  const unsigned int num_free_derivatives = num_waypoints * N_ / 2 - num_fixed_derivatives;
  // printf("num_fixed_derivatives: %d, num_free_derivatives: %d\n",
  //        num_fixed_derivatives, num_free_derivatives);

  // M
  Eigen::MatrixXf M = Eigen::MatrixXf::Zero(num_segments * N_, num_waypoints * N_ / 2);
  M.block(0, 0, N_ / 2, N_ / 2) = Eigen::MatrixXf::Identity(N_ / 2, N_ / 2);
  M.block(num_segments * N_ - N_ / 2, N_ / 2 + num_waypoints - 2, N_ / 2, N_ / 2) =
      Eigen::MatrixXf::Identity(N_ / 2, N_ / 2);
  for(unsigned int i = 0; i < num_waypoints - 2; i++)
  {
    M((2 * i + 1) * N_ / 2, N_ / 2 + i) = 1;
    M((2 * i + 2) * N_ / 2, N_ / 2 + i) = 1;
    for(unsigned int j = 1; j < N_ / 2; j++)
    {
      M((2 * i + 1) * N_ / 2 + j, num_fixed_derivatives - 1 + i * (N_ / 2 - 1) + j) = 1;
      M((2 * i + 2) * N_ / 2 + j, num_fixed_derivatives - 1 + i * (N_ / 2 - 1) + j) = 1;
    }
  }
  // Eigen::MatrixXf A_inv = A.inverse();
  Eigen::MatrixXf A_inv_M = A.partialPivLu().solve(M);
  Eigen::MatrixXf R = A_inv_M.transpose() * Q * A_inv_M;
#if 0
  //std::cout << "A:\n" << A << std::endl;
  std::cout << "Q:\n" << Q << std::endl;
  //std::cout << "M:\n" << M << std::endl;
  //std::cout << "A_inv_M:\n" << A_inv_M << std::endl;
  std::cout << "R:\n" << R << std::endl;
#endif
  Eigen::MatrixXf Rpp =
      R.block(num_fixed_derivatives, num_fixed_derivatives, num_free_derivatives, num_free_derivatives);
  Eigen::MatrixXf Rpf = R.block(num_fixed_derivatives, 0, num_free_derivatives, num_fixed_derivatives);

  // Fixed derivatives
  Eigen::MatrixX3f Df = Eigen::MatrixX3f(num_fixed_derivatives, 3);
  // First point
  Df.row(0) = waypoints_[0].transpose();
  for(unsigned int i = 1; i < N_ / 2; i++)
  {
    Df.row(i) = initial_derivatives_[i - 1].transpose();
  }
  // Middle waypoints
  for(unsigned int i = 1; i < num_waypoints - 1; i++)
  {
    Df.row((N_ / 2) - 1 + i) = waypoints_[i].transpose();
  }
  // End point
  Df.row(N_ / 2 + (num_waypoints - 2)) = waypoints_[num_waypoints - 1].transpose();
  for(unsigned int i = 1; i < N_ / 2; i++)
  {
    Df.row((N_ / 2) + (num_waypoints - 2) + i) = Vec3f::Zero().transpose();
  }
  // std::cout << "Df:\n" << Df << std::endl;
  Eigen::MatrixX3f D = Eigen::MatrixX3f(num_waypoints * N_ / 2, 3);
  D.topRows(num_fixed_derivatives) = Df;
  if(num_waypoints > 2 && num_free_derivatives > 0)
  {
    Eigen::MatrixX3f Dp = -Rpp.partialPivLu().solve(Rpf * Df);
    // std::cout << "Dp:\n" << Dp << std::endl;
    D.bottomRows(num_free_derivatives) = Dp;
  }
  Eigen::MatrixX3f d = M * D;
  // std::cout << "d:\n" << d << std::endl;
  coefficients_.clear();
  for(unsigned int i = 0; i < num_segments; i++)
  {
    const Eigen::MatrixX3f p = A.block(i * N_, i * N_, N_, N_).partialPivLu().solve(d.block(i * N_, 0, N_, 3));
    // std::cout << "p:\n" << p << std::endl;
    coefficients_.push_back(p);
  }
  waypoint_times_ = waypoint_times;
  return true;
}

bool TrajectoryGenerator::getCommand(const float time, Vec3f &pos, Vec3f &vel, Vec3f &acc, Vec3f &jrk) const
{
  if(time < 0)
    return false;

  int cur_idx = -1;
  for(unsigned int i = 1; i < waypoint_times_.size(); i++)
  {
    if(time <= waypoint_times_[i])
    {
      cur_idx = i - 1;
      break;
    }
  }
  if(cur_idx == -1)
    return false;

  const float t_traj = time - waypoint_times_[cur_idx];
  const Eigen::MatrixX3f &p = coefficients_[cur_idx];
  pos = Vec3f::Zero();
  for(unsigned int i = 0; i < p.rows(); i++)
    pos += p.row(i).transpose() * powInt(t_traj, i);

  vel = Vec3f::Zero();
  for(unsigned int i = 1; i < p.rows(); i++)
    vel += p.row(i).transpose() * (i * powInt(t_traj, i - 1));

  acc = Vec3f::Zero();
  for(unsigned int i = 2; i < p.rows(); i++)
    acc += p.row(i).transpose() * (i * (i - 1) * powInt(t_traj, i - 2));

  jrk = Vec3f::Zero();
  for(unsigned int i = 3; i < p.rows(); i++)
    jrk += p.row(i).transpose() * (i * (i - 1) * (i - 2) * powInt(t_traj, i - 3));

  return true;
}

void TrajectoryGenerator::calcMaxPerSegment(std::vector<float> &max_vel, std::vector<float> &max_acc,
                                            std::vector<float> &max_jrk) const
{
  const unsigned int num_samples_per_seg = 10;
  for(unsigned int seg_idx = 0; seg_idx < waypoint_times_.size() - 1; ++seg_idx)
  {
    const float seg_start_time = waypoint_times_[seg_idx];
    const float seg_end_time = waypoint_times_[seg_idx + 1];
    float seg_duration = seg_end_time - seg_start_time;
    const float dt = seg_duration / num_samples_per_seg;

    float seg_max_vel = 0, seg_max_acc = 0, seg_max_jrk = 0;
    for(unsigned int sample_idx = 0; sample_idx < num_samples_per_seg; ++sample_idx)
    {
      const float t_traj = sample_idx * dt;
      const Eigen::MatrixX3f &p = coefficients_[seg_idx];
      Vec3f vel = Vec3f::Zero();
      for(unsigned int i = 1; i < p.rows(); i++)
        vel += p.row(i).transpose() * (i * powInt(t_traj, i - 1));
      if(vel.norm() > seg_max_vel)
        seg_max_vel = vel.norm();

      Vec3f acc = Vec3f::Zero();
      for(unsigned int i = 2; i < p.rows(); i++)
        acc += p.row(i).transpose() * (i * (i - 1) * powInt(t_traj, i - 2));
      if(acc.norm() > seg_max_acc)
        seg_max_acc = acc.norm();

      Vec3f jrk = Vec3f::Zero();
      for(unsigned int i = 3; i < p.rows(); i++)
        jrk += p.row(i).transpose() * (i * (i - 1) * (i - 2) * powInt(t_traj, i - 3));
      if(jrk.norm() > seg_max_jrk)
        seg_max_jrk = jrk.norm();
    }
    // printf("seg idx: %d\n", seg_idx);
    // printf("max_vel: %f\n", seg_max_vel);
    // printf("max_acc: %f\n", seg_max_acc);
    // printf("max_jrk: %f\n", seg_max_jrk);
    max_vel.push_back(seg_max_vel);
    max_acc.push_back(seg_max_acc);
    max_jrk.push_back(seg_max_jrk);
  }
  return;
}

void TrajectoryGenerator::optimizeWaypointTimes(const float max_vel, const float max_acc, const float max_jrk)
{
  std::vector<float> segment_times;
  for(unsigned int i = 0; i < waypoint_times_.size() - 1; ++i)
    segment_times.push_back(waypoint_times_[i + 1] - waypoint_times_[i]);

  std::vector<float> best_segment_times = segment_times;
  float best_cost = std::numeric_limits<float>::max();

  for(int traj_iter = 0; traj_iter < 20; ++traj_iter)
  {
    // std::cout << std::string(20, '=') << " " << traj_iter << " "
    //           << std::string(20, '=') << "\n";

    for(unsigned int i = 1; i < waypoint_times_.size(); ++i)
    {
      waypoint_times_[i] = waypoint_times_[i - 1] + segment_times[i - 1];
    }
    calculate(waypoint_times_);

    std::vector<float> seg_max_vel, seg_max_acc, seg_max_jrk;
    calcMaxPerSegment(seg_max_vel, seg_max_acc, seg_max_jrk);

    const float vel_cost = 1.0f, acc_cost = 0.1f, jrk_cost = 0.01f;
    float cost = 0;

    const auto rectified_square = [](float x) { return (x > 0) ? x * x : 0; };

    const std::vector<float> prev_segment_times = segment_times;
    bool done = true;
    for(unsigned int i = 0; i < seg_max_vel.size(); ++i)
    {
      cost += vel_cost * rectified_square(seg_max_vel[i] - max_vel) +
              acc_cost * rectified_square(seg_max_acc[i] - max_acc) +
              jrk_cost * rectified_square(seg_max_jrk[i] - max_jrk);

      if(seg_max_vel[i] > max_vel || seg_max_acc[i] > max_acc || seg_max_jrk[i] > max_jrk)
      {
        segment_times[i] *= 1.035f;
        done = false;
      }
      else if(seg_max_vel[i] < 0.9f * max_vel && seg_max_acc[i] < 0.9f * max_acc && seg_max_jrk[i] < 0.9f * max_jrk)
      {
        segment_times[i] *= 0.966f;
        done = false;
      }
    }

    // std::cout << "cost: " << cost << "\n";
    // If no solution found, keep the "best" one (with lowest violations)
    if(cost <= best_cost)
    {
      best_segment_times = prev_segment_times;
      best_cost = cost;
    }
    if(done)
    {
      // std::cout << "Early exit! num iter: " << traj_iter << "\n";
      break;
    }
  }
}


#include "A_Star.h"

//#include <queue>
//#include <stdio.h>
//#include <math.h>
//#include <vector>
//#include <cassert>
//#include <typeinfo>
//#include <vector>

#include <boost/math/constants/constants.hpp>
#include <iostream>
#include <vector>

using namespace std;

int A_Star::Get_Shortest_Path(const vector<vector<int>>& array,int Max_Search_Time,bool Debug_Info_Switch){
    //为简单，干脆把把下面数组转为链表结构的数组
    //约定：0是可走的，1表示障碍物不可走，2表示起点，3表示终点
//    vector<vector<int>> array={
//        { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
//        { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 0, 1, 3, 0, 0, 0 },
//        { 0, 0, 2, 0, 0, 1, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
//        { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } };
    debug_info_switch=Debug_Info_Switch;
    int Error_flag=0;
    bool Input_OK=false;
    Input_OK =Input_Verify(array);
    if(!Input_OK) return Error_flag;
    map.Build_Map(array);

    //PNode *Start_node;
//    Start_node=map.Start_node;
    //PNode *End_node;
//    End_node=map.End_node;

    map.Start_node->G=0;
    map.Start_node->H=H_Calculat(map.Start_node,map.End_node);
    map.Start_node->F=map.Start_node->G+map.Start_node->H;
    map.Start_node->open_flag=1;
    open_List.Heap_push(make_pair(map.Start_node->F,map.Start_node));
//   cout<<"hello world!  "<<Open_List->next->G<<" , "<<Open_List->next->H<<endl;

/********** main search ****************/
    Shortest_Path_Long=-1;
    Count=0;
    int Search_end_flag;
    while(1)
    {
        //cout<<"Count=  "<<Count<<endl;
        Search_end_flag=Search();
        if (Search_end_flag==1)
         {
            if(debug_info_switch)  cout<<" Having find the useable path !!! "<<endl;
            Error_flag=0;
            break;
         }
        if (Count>Max_Search_Time)
         {
            cout<<" Path finding timeout !!! "<<endl;
            Error_flag=1;
            break;
         }
        if (Search_end_flag==2)
         {
            if(debug_info_switch)  cout<<" Can not find the useable path !!! "<<endl;
            Error_flag=1;
            break;
         }

        Count++;
 //       cout<<"opt_node->[G,H]  "<<opt_node->G<<" , "<<opt_node->H<<endl;
    }
    if (Error_flag==0)
    {
        PNode *tmp=map.End_node;
        while(tmp)
        {
            //cout<<" ["<<tmp->x<<" , "<<tmp->y<<"] -> ";
            Shortest_Path.push_back({tmp->x,tmp->y});
            tmp=tmp->path_before;
            Shortest_Path_Long++;
        }
        reverse(Shortest_Path.begin(),Shortest_Path.end());  // " use of undlclared identifier 'reverse' ..."  ----it is why?  need #include <boost/math/constants/constants.hpp>
        //for(size_t i=0;i<Shortest_Path.size()/2;i++){
        //    swap(Shortest_Path[i],Shortest_Path[Shortest_Path.size()-1-i]);
        //}

    }

    //map.Clear_Map();

    return Error_flag;
}

bool A_Star::Input_Verify(const vector<vector<int>>& array){
    int Start_Point_Count=0,End_Point_Count=0;
    for(size_t i=0;i<array.size();i++){
        if(array[i].size() != array[0].size()){
            cout<<" Fatal Wrong , The Input Must be a Mutrix !!! "<<endl;
            return false;
        }
        for(size_t j=0;j<array[i].size();j++){
            if (array[i][j]<0 || array[i][j]>3){
                cout<<" Fatal Wrong , The Value in the Input Mutrix must be 0, 1, 2, 3  !!! "<<endl;
                return false;
            }
            if (array[i][j]==2) Start_Point_Count++;
            if (array[i][j]==3) End_Point_Count++;
        }
    }
    if (Start_Point_Count==1 && End_Point_Count==1) return true;
    else{
        if (Start_Point_Count!=1) cout<<" Fatal Wrong , Cannot find the start point or more than one start point !!! "<<endl;
        if (End_Point_Count!=1) cout<<" Fatal Wrong , Cannot find the end point or more than one end point !!! "<<endl;
        return false;
    }
}

float A_Star::H_Calculat(PNode *cur,PNode *end)
{
    float res=float_abs(cur->x-end->x)+float_abs(cur->y-end->y);
    return res;
}

float A_Star::float_abs(float x){
    return x > 0 ? x:-x;
}

int A_Star::Search()
{
     if (open_List.Heap_size()==0){
         if(debug_info_switch)  cout<<" The Open_List_Min_Heap is empty !!! "<<endl;
         return 2;
     }
     PNode *Opt_node;
     Opt_node=open_List.Heap_top().second;
     Opt_node->open_flag=0;
     Opt_node->close_flag=1;
     open_List.Heap_pop();
    for(size_t i=0;i<Opt_node->adjacent_node.size();i++)
    {
        PNode* Tmp_adjacent_next_node=Opt_node->adjacent_node[i];
        if (Tmp_adjacent_next_node->nodetype==EndPoint)       // End point has been found!!!
        {
            Tmp_adjacent_next_node->path_before=Opt_node;
            return 1;
        }

        if (Tmp_adjacent_next_node->open_flag==0 && Tmp_adjacent_next_node->close_flag==0 && Tmp_adjacent_next_node->nodetype !=UnReachable)
        {
            Tmp_adjacent_next_node->path_before=Opt_node;
            Tmp_adjacent_next_node->G=Opt_node->G+1;
            Tmp_adjacent_next_node->H=H_Calculat(Tmp_adjacent_next_node,map.End_node);
            Tmp_adjacent_next_node->F=Tmp_adjacent_next_node->G+Tmp_adjacent_next_node->H;
            Tmp_adjacent_next_node->open_flag=1;    //The open_flag should be modified! --20200312
            open_List.Heap_push(make_pair(Tmp_adjacent_next_node->F,Tmp_adjacent_next_node));
        }

        else if(Tmp_adjacent_next_node->open_flag==1 && Tmp_adjacent_next_node->close_flag==0)
        {
            float tmp_G=Opt_node->G+1;
            float tmp_H=H_Calculat(Tmp_adjacent_next_node,map.End_node);
            float tmp_F=tmp_G+tmp_H;
            if (tmp_F<Tmp_adjacent_next_node->F){
                //update thr f which is in open list!!!!
                //update thr f which is in open list!!!!
                //update thr f which is in open list!!!!
                Tmp_adjacent_next_node->G=tmp_G;
                Tmp_adjacent_next_node->H=tmp_H;
                Tmp_adjacent_next_node->F=tmp_F;
                Tmp_adjacent_next_node->path_before=Opt_node;
                //open_List.Heap_delect(Tmp_adjacent_next_node);
                //open_List.Heap_push(make_pair(Tmp_adjacent_next_node->F,Tmp_adjacent_next_node));
                open_List.Heap_modify(Tmp_adjacent_next_node,Tmp_adjacent_next_node->F);

            }
            else {
                //no action!
            }

        }
        else if(Tmp_adjacent_next_node->open_flag==0 && Tmp_adjacent_next_node->close_flag==1)
        {
            // no action!
        }
        else if((Tmp_adjacent_next_node->open_flag==0 && Tmp_adjacent_next_node->close_flag==0 && Tmp_adjacent_next_node->nodetype ==UnReachable))
        {
            Tmp_adjacent_next_node->close_flag=1;
        }
        else {
            cout<<"Fatel Error!!!"<<endl;
             return 2;
        }

    }

    return 0;
}

void A_Star::clear(){
    Shortest_Path.clear();
    open_List.Heap_clear();
    map.Clear_Map();
}


#ifndef __INTEGRATE_UTIL_HH__
#define __INTEGRATE_UTIL_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Integration wapper functions)
LIBRARY DEPENDENCY:
      ((../src/integrate.cpp))
*******************************************************************************/

#include <armadillo>
#include <cassert>
#include <vector>
#include "Rocket_Flight_DM.hh"

#define INTEGRATE(in, diff)                         \
  do {                                              \
    double in##d_new = diff;                        \
    in = integrate(in##d_new, in##d, in, int_step); \
    in##d = in##d_new;                              \
  } while (0)

#define INTEGRATE_MAT(in, diff)                     \
  do {                                              \
    arma::mat in##D_NEW = diff;                     \
    in = integrate(in##D_NEW, in##D, in, int_step); \
    in##D = in##D_NEW;                              \
  } while (0)

/**
 * \brief Integration of scalar state variable.
 * Modified Euler Midpoint method
 * Example first order lag:
 *   phid_new=(phic-phi)/tphi;
 *   phi=integrate(phid_new,phid,phi,int_step);
 *   phid=phid_new;
 */
double integrate(const double &dydx_new, const double &dydx, const double &y,
                 const double &int_step);

arma::mat integrate(arma::mat &DYDX_NEW, arma::mat &DYDX, arma::mat &Y,
                    const double int_step);
template <typename T>
void IntegratorRK4(std::vector<arma::vec> V_in, std::vector<arma::vec> &V_out,
                   void (T::*fp)(std::vector<arma::vec> Var_in,
                                 std::vector<arma::vec> &Var_out),
                   T *ClassPointer, double int_step) {
  {
    std::vector<std::vector<arma::vec>> KMAT;
    for (unsigned int i = 0; i < V_in.size(); i++) {
      std::vector<arma::vec> KROW(4);
      KMAT.push_back(KROW);
    }

    V_out = V_in;

    ((ClassPointer)->*fp)(V_out, KMAT[0]);

    for (unsigned int i = 0; i < V_in.size(); i++) {
      V_out[i] = V_in[i] + KMAT[0][i] * 0.5 * int_step;
    }

    ((ClassPointer)->*fp)(V_out, KMAT[1]);

    for (unsigned int i = 0; i < V_in.size(); i++) {
      V_out[i] = V_in[i] + KMAT[1][i] * 0.5 * int_step;
    }

    ((ClassPointer)->*fp)(V_out, KMAT[2]);

    for (unsigned int i = 0; i < V_in.size(); i++) {
      V_out[i] = V_in[i] + KMAT[2][i] * int_step;
    }

    ((ClassPointer)->*fp)(V_out, KMAT[3]);

    for (unsigned int i = 0; i < V_in.size(); i++) {
      V_out[i] = V_in[i] +
                 (int_step / 6.0) * (KMAT[0][i] + 2.0 * KMAT[1][i] +
                                     2.0 * KMAT[2][i] + KMAT[3][i]);
    }
  }
}

template <typename T>
void IntegratorEuler(std::vector<arma::vec> V_in, std::vector<arma::vec> &V_out,
                     void (T::*fp)(std::vector<arma::vec> Var_in,
                                   std::vector<arma::vec> &Var_out),
                     T *ClassPointer, double int_step) {
  std::vector<arma::vec> K_TEMP;

  for (unsigned int i = 0; i < V_in.size(); i++) {
    arma::vec temp;
    K_TEMP.push_back(temp);
  }

  ((ClassPointer)->*fp)(V_in, K_TEMP);

  for (unsigned int i = 0; i < V_in.size(); i++) {
    V_out[i] = V_in[i] + K_TEMP[i] * int_step;
  }
}
#endif  // __INTEGRATE_UTIL_HH__


#ifndef _SOT_CORE_CAUSAL_FILTER_H_
#define _SOT_CORE_CAUSAL_FILTER_H_
/*
 * Copyright 2017-, Rohan Budhirja, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-torque-control is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <Eigen/Core>

/** \addtogroup Filters
    \section subsec_causalfilter CausalFilter
    Filter data with an IIR or FIR filter.

    Filter a data sequence, \f$x\f$, using a digital filter.
    The filter is a direct form II transposed implementation
    of the standard difference equation.
    This means that the filter implements:

    \f$ a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)]
    - a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)] \f$

    where \f$m\f$ is the degree of the numerator,
    \f$n\f$ is the degree of the denominator,
    and \f$N\f$ is the sample number

 */
namespace dynamicgraph {
namespace sot {

class CausalFilter {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /** --- CONSTRUCTOR ----
      \param[in] timestep
      \param[in] xSize
      \param[in] filter_numerator
      \param[in] filter_denominator

      xSize is
  */
  CausalFilter(const double &timestep, const int &xSize,
               const Eigen::VectorXd &filter_numerator,
               const Eigen::VectorXd &filter_denominator);

  void get_x_dx_ddx(const Eigen::VectorXd &base_x,
                    Eigen::VectorXd &x_output_dx_ddx);

  void switch_filter(const Eigen::VectorXd &filter_numerator,
                     const Eigen::VectorXd &filter_denominator);

private:
  /// sampling timestep of the input signal
  double m_dt;
  /// Size
  int m_x_size;
  /// Size of the numerator \f$m\f$
  Eigen::VectorXd::Index m_filter_order_m;
  /// Size of the denominator \f$n\f$
  Eigen::VectorXd::Index m_filter_order_n;

  /// Coefficients of the numerator \f$b\f$
  Eigen::VectorXd m_filter_numerator;
  /// Coefficients of the denominator \f$a\f$
  Eigen::VectorXd m_filter_denominator;
  bool m_first_sample;
  ///
  int m_pt_numerator;
  int m_pt_denominator;
  Eigen::MatrixXd m_input_buffer;
  Eigen::MatrixXd m_output_buffer;
}; // class CausalFilter
} // namespace sot
} // namespace dynamicgraph
#endif /* _SOT_CORE_CAUSAL_FILTER_H_ */


#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {

template<>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template<>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template <>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_axpy<float>(const int N, const float alpha, const float* X,
    float* Y) { cblas_saxpy(N, alpha, X, 1, Y, 1); }

template <>
void caffe_axpy<double>(const int N, const double alpha, const double* X,
    double* Y) { cblas_daxpy(N, alpha, X, 1, Y, 1); }

template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <typename Dtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifndef CPU_ONLY
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    }
  }
}

template void caffe_copy<int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int>(const int N, const unsigned int* X,
    unsigned int* Y);
template void caffe_copy<float>(const int N, const float* X, float* Y);
template void caffe_copy<double>(const int N, const double* X, double* Y);

template <>
void caffe_scal<float>(const int N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double>(const int N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

template <>
void caffe_cpu_axpby<float>(const int N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double>(const int N, const double alpha, const double* X,
                             const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_add<float>(const int n, const float* a, const float* b,
    float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double>(const int n, const double* a, const double* b,
    double* y) {
  vdAdd(n, a, b, y);
}

template <>
void caffe_sub<float>(const int n, const float* a, const float* b,
    float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double>(const int n, const double* a, const double* b,
    double* y) {
  vdSub(n, a, b, y);
}

template <>
void caffe_mul<float>(const int n, const float* a, const float* b,
    float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double>(const int n, const double* a, const double* b,
    double* y) {
  vdMul(n, a, b, y);
}

template <>
void caffe_div<float>(const int n, const float* a, const float* b,
    float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double>(const int n, const double* a, const double* b,
    double* y) {
  vdDiv(n, a, b, y);
}

template <>
void caffe_powx<float>(const int n, const float* a, const float b,
    float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double>(const int n, const double* a, const double b,
    double* y) {
  vdPowx(n, a, b, y);
}

template <>
void caffe_sqr<float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_exp<float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
    vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
    vdAbs(n, a, y);
}

unsigned int caffe_rng_rand() {
  return (*caffe_rng())();
}

template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(
      b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

template <typename Dtype>
void caffe_rng_uniform(const int n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_uniform<float>(const int n, const float a, const float b,
                              float* r);

template
void caffe_rng_uniform<double>(const int n, const double a, const double b,
                               double* r);

template <typename Dtype>
void caffe_rng_gaussian(const int n, const Dtype a,
                        const Dtype sigma, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_gaussian<float>(const int n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double>(const int n, const double mu,
                                const double sigma, double* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, int* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, unsigned int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, unsigned int* r);

template <>
float caffe_cpu_strided_dot<float>(const int n, const float* x, const int incx,
    const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

template <>
double caffe_cpu_strided_dot<double>(const int n, const double* x,
    const int incx, const double* y, const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

template <typename Dtype>
Dtype caffe_cpu_dot(const int n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template
float caffe_cpu_dot<float>(const int n, const float* x, const float* y);

template
double caffe_cpu_dot<double>(const int n, const double* x, const double* y);

template <>
int caffe_cpu_hamming_distance<float>(const int n, const float* x,
                                  const float* y) {
  int dist = 0;
  for (int i = 0; i < n; ++i) {
    dist += __builtin_popcount(static_cast<uint32_t>(x[i]) ^
                               static_cast<uint32_t>(y[i]));
  }
  return dist;
}

template <>
int caffe_cpu_hamming_distance<double>(const int n, const double* x,
                                   const double* y) {
  int dist = 0;
  for (int i = 0; i < n; ++i) {
    dist += __builtin_popcountl(static_cast<uint64_t>(x[i]) ^
                                static_cast<uint64_t>(y[i]));
  }
  return dist;
}

template <>
float caffe_cpu_asum<float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}

template <>
void caffe_cpu_scale<float>(const int n, const float alpha, const float *x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double>(const int n, const double alpha, const double *x,
                             double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}
/* changes */

//calculate gamma in kernel function
//K: dimension of data
//S: size of all data
//N: num of output
//W: weights, N*K
//X: X (input of this layer)
template <>
float cal_gamma_cpu<float>(const int K, const int S, const int N, const float* W, const float* X, float* tempX1, float* tempX2){
    srand((unsigned int)time(0));
    float gamma = 0;
    float temp = 0;

    //random sample S pair to calculate gamma
    for(int i = 0;i < S;++i){
        memset(tempX1, 0, sizeof(float) * K);
        memset(tempX2, 0, sizeof(float) * K);
        int s1 = rand() % S;
        int s2 = rand() % S;
        s2 = (s1 != s2) ? s2 : (s2 + 1) % S;

        const float* x1 = X + s1 * K;
        const float* x2 = X + s2 * K;

        caffe_cpu_gemv<float>(CblasNoTrans, N, K, 1.0, W, x1, 0.0, tempX1);
        caffe_cpu_gemv<float>(CblasNoTrans, N, K, 1.0, W, x2, 0.0, tempX2);

        //caffe_cpu_sub<float>(K, tempX1, tempX2, tempX2);
        caffe_cpu_axpby(K, 1.0f, tempX1, -1.0f, tempX2);
        temp = caffe_cpu_dot<float>(K, tempX2, tempX2);
        gamma += temp;
    }
    return S / gamma;
}

//output:
//  tempX1: W*x1-W*x2
//  tempX2: x1-x2
//  KK: co * (x1-x2)^T * W^T  should be 1*N
template<>
void cal_add_item_cpu<float>(const float co, const int N, const int K, const float* W, const float* x1, float* tempX1,
    const float* x2, float* tempX2, const float gamma, float* KK){

    memset(tempX1, 0, sizeof(float) * K);
    memset(tempX2, 0, sizeof(float) * K);

    caffe_cpu_gemv<float>(CblasNoTrans, N, K, 1.0, W, x1, 0.0, tempX1);
    caffe_cpu_gemv<float>(CblasNoTrans, N, K, 1.0, W, x2, 0.0, tempX2);

    float square_sum = 0;

    caffe_cpu_axpby(K, -1.0f, tempX2, 1.0f, tempX1);
    caffe_cpu_axpby(K, 1.0f, x1, 0.0f, tempX2);
    caffe_cpu_axpby(K, -1.0f, x2, 1.0f, tempX2);
    square_sum = caffe_cpu_dot<float>(K, tempX1, tempX1);

    //calculate 2 * \gamma * kernel
    float kernel = 0.0f;
    float tempGamma = gamma / 4.0f;
    for(int i = 0;i < 5;++i){
        float temp = (0.0 - tempGamma) * square_sum;
        temp = exp(temp);
        kernel += 2 * tempGamma* temp;
        tempGamma = tempGamma * 2;
    }
    /*float kernel = (0.0 - gamma) * square_sum;
    kernel = exp(kernel);
    kernel = 2 * gamma * kernel;
*/
    //calculate KK <- co * kernel * X^T * W + 1 * KK
    caffe_cpu_gemm<float>(CblasNoTrans, CblasTrans, 1, N, K, co*kernel, tempX2, W, 0.0, KK);
}

template<>
void cal_add_item_cpu<double>(const double co, const int N, const int K, const double* W, const double* x1, double* tempX1,
    const double* x2, double* tempX2, const double gamma, double* KK){
    //TODO: complete double version of this function
}

// Gradient with respect to weight for MMD
      //N: number of output neuron
      //K: dimension of the feature
      //M: size of all data
      //S: size of source data in a batch
      //W: weight of this layer
      //X: input of this layer
      //gamma: gamma / learning rate
      //delta_W: gredient of weight

template<>
void caffe_cpu_mmd<float>(const int N, const int K, const int M, const int S, const int labeledTargetSize,
    const float* W, const float* X, const float gamma, float* delta_W){
    srand((unsigned int)time(0));

    //output the value of delta_W before MMD gradient
    float sum = 0;
    for(int i = 0;i < N;++i){
        for(int j = 0;j < K;++j){
            sum += (delta_W[i * N + j] > 0) ? delta_W[i*N+j] : (-1 * delta_W[i*N+j]);
        }
    }
    LOG(INFO) << "delta_W before MMD, sum = " << sum << ", average = " << sum / (N*K);

    float *KK = new float[N];
    float *tempX1 = new float[K];
    float *tempX2 = new float[K];

    float kernel_gamma = cal_gamma_cpu(K, M, N, W, X, tempX1, tempX2);
    int SS = (S>(M-S)) ? S : M-S;

    for(int i = 0;i < SS;++i){
        //random
        int s1 = rand() % S;
        int s2 = rand() % S;
        if(s1 == s2){
            s2 = (s2 + 100) % S;
        }
        int t1 = rand() % (M - S - labeledTargetSize);
        int t2 = rand() % (M - S - labeledTargetSize);
        if(t1 == t2){
            t2 = (t2 + 100) % (M - S - labeledTargetSize);
        }
        t1 = t1 + S + labeledTargetSize;
        t2 = t2 + S + labeledTargetSize;

        const float *x_s1 = X + s1 * K;
        const float *x_s2 = X + s2 * K;
        const float *x_t1 = X + t1 * K;
        const float *x_t2 = X + t2 * K;
        const float tempS = 1.0;

        //calculate four items of MMD gradient
        memset(KK, 0, sizeof(float) * N);
        cal_add_item_cpu<float>(-1, N, K, W, x_s1, tempX1, x_s2, tempX2, kernel_gamma, KK);
        caffe_cpu_gemm<float>(CblasNoTrans, CblasTrans, N, K, 1, tempS * gamma, KK,tempX2, 1.0, delta_W);

        memset(KK, 0, sizeof(float) * N);
        cal_add_item_cpu<float>(1, N, K, W, x_s1, tempX1, x_t2, tempX2, kernel_gamma, KK);
        caffe_cpu_gemm<float>(CblasNoTrans, CblasTrans, N, K, 1, tempS * gamma, KK,tempX2, 1.0, delta_W);

        memset(KK, 0, sizeof(float) * N);
        cal_add_item_cpu<float>(1, N, K, W, x_s2, tempX1, x_t1, tempX2, kernel_gamma, KK);
        caffe_cpu_gemm<float>(CblasNoTrans, CblasTrans, N, K, 1, tempS * gamma, KK,tempX2, 1.0, delta_W);

        memset(KK, 0, sizeof(float) * N);
        cal_add_item_cpu<float>(-1, N, K, W, x_t1, tempX1, x_t2, tempX2, kernel_gamma, KK);
        caffe_cpu_gemm<float>(CblasNoTrans, CblasTrans, N, K, 1, tempS * gamma, KK,tempX2, 1.0, delta_W);
    }
    //output the value of delta_W after MMD gradient
    sum = 0;
    for(int i = 0;i < N;++i){
        for(int j = 0;j < K;++j){
            sum += (delta_W[i * N + j] > 0) ? delta_W[i*N+j] : (-1 * delta_W[i*N+j]);
        }
    }
    LOG(INFO) << "delta_W after MMD, sum = " << sum << ", average = " << sum / (N*K);

    delete [] KK;
    delete [] tempX1;
    delete [] tempX2;
}

template<>
void caffe_cpu_mmd<double>(const int N, const int K, const int M, const int S, const int labeledTargetSize,
    const double* W, const double* X, const double gamma, double* delta_W){
    //TODO: complete the double version of this function
}

}  // namespace caffe


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "../include/structures.h"
#include "../include/transformations.h"
#include "../../rectangular_object_with_unknown_width_height_tait_bryan_wc_jacobian.h"
#include "../../rectangular_object_with_unknown_width_height_rodrigues_wc_jacobian.h"
#include "../../rectangular_object_with_unknown_width_height_quaternion_wc_jacobian.h"
#include "../../quaternion_constraint_jacobian.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

std::vector<std::vector<Eigen::Affine3d>> bundle_of_rays;
struct Rectangle{
	std::vector<Eigen::Vector3d> corners_local;
	double scale_x;
	double scale_y;
	Eigen::Affine3d pose;
};
Rectangle rectangle;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

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

	std::vector<Eigen::Affine3d> br;
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -2;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -2;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();
	for(size_t i = 0; i < 25; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 10;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + -5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 +  5;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.5;

		br.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}
	bundle_of_rays.push_back(br);
	br.clear();

	rectangle.scale_x = 1.0;
	rectangle.scale_y = 1.0;
	rectangle.pose = Eigen::Affine3d::Identity();
	rectangle.corners_local.emplace_back(-1,-1, 0);
	rectangle.corners_local.emplace_back( 1,-1, 0);
	rectangle.corners_local.emplace_back( 1, 1, 0);
	rectangle.corners_local.emplace_back(-1, 1, 0);

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("rectangular_object_with_unknown_width_height");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glColor3f(0,1,0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < bundle_of_rays.size(); i++){
		for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
			Eigen::Vector3d z_begin(0, 0,-100);
			Eigen::Vector3d z_end(0, 0, 100);

			Eigen::Vector3d z_begin_t = bundle_of_rays[i][j] * z_begin;
			Eigen::Vector3d z_end_t = bundle_of_rays[i][j] * z_end;

			glVertex3f(z_begin_t.x(), z_begin_t.y(), z_begin_t.z());
			glVertex3f(z_end_t.x(), z_end_t.y(), z_end_t.z());
		}
	}
	glEnd();

	std::vector<Eigen::Vector3d> corners_global;
	for(size_t i = 0; i < rectangle.corners_local.size(); i++){
		Eigen::Vector3d v(rectangle.corners_local[i].x() * rectangle.scale_x, rectangle.corners_local[i].y() * rectangle.scale_y, rectangle.corners_local[i].z());
		corners_global.push_back(rectangle.pose * v);
	}

	glLineWidth(5);
	glColor3f(1,0,0);
	glBegin(GL_LINE_STRIP);
		for(size_t i = 0 ; i < corners_global.size(); i++){
			glVertex3f(corners_global[i].x(), corners_global[i].y(), corners_global[i].z());
		}
		glVertex3f(corners_global[0].x(), corners_global[0].y(), corners_global[0].z());
	glEnd();
	glLineWidth(1);
	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(rectangle.pose);

			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					Eigen::Vector3d vx(bundle_of_rays[i][j](0,0), bundle_of_rays[i][j](1,0), bundle_of_rays[i][j](2,0));
					Eigen::Vector3d vy(bundle_of_rays[i][j](0,1), bundle_of_rays[i][j](1,1), bundle_of_rays[i][j](2,1));

					Eigen::Matrix<double, 2, 1> delta;
					rectangular_object_with_unknown_width_height_tait_bryan_wc(delta,
							pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					Eigen::Matrix<double, 2, 8> delta_jacobian;
					rectangular_object_with_unknown_width_height_tait_bryan_wc_jacobian(delta_jacobian,
							pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					int ir = tripletListB.size();
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir, k, -delta_jacobian(0,k));
					}
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir + 1, k, -delta_jacobian(1,k));
					}

					tripletListP.emplace_back(ir    , ir    ,  1);
					tripletListP.emplace_back(ir + 1, ir + 1,  1);

					tripletListB.emplace_back(ir    , 0,  delta(0,0));
					tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 8);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(8, 8);
			Eigen::SparseMatrix<double> AtPB(8, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
					std::cout << it.value() << std::endl;
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;

			if(h_x.size() == 8){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;
				TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(rectangle.pose);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.om += h_x[counter++];
				pose.fi += h_x[counter++];
				pose.ka += h_x[counter++];
				rectangle.pose = affine_matrix_from_pose_tait_bryan(pose);
				rectangle.scale_x += h_x[counter++];
				rectangle.scale_y += h_x[counter++];
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			TaitBryanPose pose_rand;
			pose_rand.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			rectangle.pose = rectangle.pose * affine_matrix_from_pose_tait_bryan(pose_rand);

			RodriguesPose pose = pose_rodrigues_from_affine_matrix(rectangle.pose);

			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					Eigen::Vector3d vx(bundle_of_rays[i][j](0,0), bundle_of_rays[i][j](1,0), bundle_of_rays[i][j](2,0));
					Eigen::Vector3d vy(bundle_of_rays[i][j](0,1), bundle_of_rays[i][j](1,1), bundle_of_rays[i][j](2,1));

					Eigen::Matrix<double, 2, 1> delta;
					rectangular_object_with_unknown_width_height_rodrigues_wc(delta,
							pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					Eigen::Matrix<double, 2, 8> delta_jacobian;
					rectangular_object_with_unknown_width_height_rodrigues_wc_jacobian(delta_jacobian,
							pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					int ir = tripletListB.size();
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir, k, -delta_jacobian(0,k));
					}
					for(size_t k = 0 ; k < 8; k++){
						tripletListA.emplace_back(ir + 1, k, -delta_jacobian(1,k));
					}

					tripletListP.emplace_back(ir    , ir    ,  1);
					tripletListP.emplace_back(ir + 1, ir + 1,  1);

					tripletListB.emplace_back(ir    , 0,  delta(0,0));
					tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 8);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(8, 8);
			Eigen::SparseMatrix<double> AtPB(8, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
					std::cout << it.value() << std::endl;
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;

			if(h_x.size() == 8){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;
				RodriguesPose pose = pose_rodrigues_from_affine_matrix(rectangle.pose);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.sx += h_x[counter++];
				pose.sy += h_x[counter++];
				pose.sz += h_x[counter++];
				rectangle.pose = affine_matrix_from_pose_rodrigues(pose);
				rectangle.scale_x += h_x[counter++];
				rectangle.scale_y += h_x[counter++];
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			TaitBryanPose pose_rand;
			pose_rand.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			pose_rand.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			rectangle.pose = rectangle.pose * affine_matrix_from_pose_tait_bryan(pose_rand);

			QuaternionPose pose = pose_quaternion_from_affine_matrix(rectangle.pose);

			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					Eigen::Vector3d vx(bundle_of_rays[i][j](0,0), bundle_of_rays[i][j](1,0), bundle_of_rays[i][j](2,0));
					Eigen::Vector3d vy(bundle_of_rays[i][j](0,1), bundle_of_rays[i][j](1,1), bundle_of_rays[i][j](2,1));

					Eigen::Matrix<double, 2, 1> delta;
					rectangular_object_with_unknown_width_height_quaternion_wc(delta,
							pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					Eigen::Matrix<double, 2, 9> delta_jacobian;
					rectangular_object_with_unknown_width_height_quaternion_wc_jacobian(delta_jacobian,
							pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							rectangle.corners_local[i].x(), rectangle.corners_local[i].y(), rectangle.corners_local[i].z(),
							bundle_of_rays[i][j](0,3), bundle_of_rays[i][j](1,3), bundle_of_rays[i][j](2,3),
							vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z(),
							rectangle.scale_x, rectangle.scale_y);

					int ir = tripletListB.size();
					for(size_t k = 0 ; k < 9; k++){
						tripletListA.emplace_back(ir, k, -delta_jacobian(0,k));
					}
					for(size_t k = 0 ; k < 9; k++){
						tripletListA.emplace_back(ir + 1, k, -delta_jacobian(1,k));
					}

					tripletListP.emplace_back(ir    , ir    ,  1);
					tripletListP.emplace_back(ir + 1, ir + 1,  1);

					tripletListB.emplace_back(ir    , 0,  delta(0,0));
					tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
				}
			}

			int ir = tripletListB.size();

			double delta;
			quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3);

			Eigen::Matrix<double, 1, 4> jacobian;
			quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3);

			tripletListA.emplace_back(ir, 3 , -jacobian(0,0));
			tripletListA.emplace_back(ir, 4 , -jacobian(0,1));
			tripletListA.emplace_back(ir, 5 , -jacobian(0,2));
			tripletListA.emplace_back(ir, 6 , -jacobian(0,3));

			tripletListP.emplace_back(ir, ir, 1000000.0);

			tripletListB.emplace_back(ir, 0, delta);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 9);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(9, 9);
			Eigen::SparseMatrix<double> AtPB(9, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
					std::cout << it.value() << std::endl;
				}
			}

			std::cout << "h_x.size(): " << h_x.size() << std::endl;

			if(h_x.size() == 9){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;
				QuaternionPose pose = pose_quaternion_from_affine_matrix(rectangle.pose);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.q0 += h_x[counter++];
				pose.q1 += h_x[counter++];
				pose.q2 += h_x[counter++];
				pose.q3 += h_x[counter++];
				rectangle.pose = affine_matrix_from_pose_quaternion(pose);
				rectangle.scale_x += h_x[counter++];
				rectangle.scale_y += h_x[counter++];
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'n':{
			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				for(size_t j = 0; j < bundle_of_rays[i].size(); j++){
					TaitBryanPose pose;

					pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.05;
					pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.05;
					pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.05;

					pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
					pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
					pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;

					bundle_of_rays[i][j] = bundle_of_rays[i][j] * affine_matrix_from_pose_tait_bryan(pose);
				}
			}
			TaitBryanPose pose;

			pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;

			pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
			pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
			pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;

			rectangle.pose = rectangle.pose * affine_matrix_from_pose_tait_bryan(pose);

			rectangle.scale_x += ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			rectangle.scale_y += ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 5;
			break;
		}
	}

	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: modify rays" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize (Rodrigues)" << std::endl;
	std::cout << "q: optimize (Quaternion)" << std::endl;
}