Trajectory Trajectory::generatePolynomialTrajectoryThroughViapoint(const VectorXd& ts, const VectorXd& y_from, const VectorXd& y_yd_ydd_viapoint, double viapoint_time, const VectorXd& y_to)
{

    int n_dims = y_from.size();
    assert(n_dims==y_to.size());
    assert(3*n_dims==y_yd_ydd_viapoint.size()); // Contains y, yd and ydd, so *3

    int n_time_steps = ts.size();

    int viapoint_time_step = 0;
    while (viapoint_time_step<n_time_steps && ts[viapoint_time_step]<viapoint_time)
        viapoint_time_step++;

    if (viapoint_time_step>=n_time_steps)
    {
        cerr << __FILE__ << ":" << __LINE__ << ":";
        cerr << "ERROR: the time vector does not contain any time smaller than " << viapoint_time << ". Returning min-jerk trajectory WITHOUT viapoint." <<  endl;
        return Trajectory();
    }

    VectorXd yd_from        = VectorXd::Zero(n_dims);
    VectorXd ydd_from       = VectorXd::Zero(n_dims);

    VectorXd y_viapoint     = y_yd_ydd_viapoint.segment(0*n_dims,n_dims);
    VectorXd yd_viapoint    = y_yd_ydd_viapoint.segment(1*n_dims,n_dims);
    VectorXd ydd_viapoint   = y_yd_ydd_viapoint.segment(2*n_dims,n_dims);

    VectorXd yd_to          = VectorXd::Zero(n_dims);
    VectorXd ydd_to         = VectorXd::Zero(n_dims);

    Trajectory traj = Trajectory::generatePolynomialTrajectory(ts.segment(0, viapoint_time_step + 1), y_from, yd_from, ydd_from, y_viapoint, yd_viapoint, ydd_viapoint);
    traj.append(Trajectory::generatePolynomialTrajectory(ts.segment(viapoint_time_step, n_time_steps - viapoint_time_step), y_viapoint, yd_viapoint, ydd_viapoint, y_to, yd_to, ydd_to));

    return traj;
}

void XTCReader::read(Trajectory &trajectory) {
  while (true) {
    SmartPointer<Positions> positions = new Positions;
    if (!read(*positions, trajectory.getTopology().get())) break;
    trajectory.add(positions);
  }
}

void CalibrateAction::visualize_trajectory(Trajectory &traj)
{
    // publishing to rviz
    geometry_msgs::PoseArray poses;
    for(unsigned int i=0; i<traj.getPointsSize();i++)
    {
        geometry_msgs::Pose temp_pose;
        double x_, y_,th_;
        traj.getPoint(i, x_, y_, th_);
        tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
        temp_pose.position.x = x_;
        temp_pose.position.y = y_;
        poses.poses.push_back(temp_pose);
        //ROS_INFO("Pose: x: %f y: %f th: %f", x_, y_, th_);
    }
    geometry_msgs::Pose temp_pose, temp_pose2;
    double x_, y_,th_;
    traj.getPoint(0, x_, y_, th_);
    tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
    temp_pose.position.x = x_;
    temp_pose.position.y = y_;
    traj.getEndpoint(x_, y_, th_);
    tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose2.orientation);
    temp_pose2.position.x = x_;
    temp_pose2.position.y = y_;
    //ROS_INFO("Visualize trajectory with %i points. First Point (%2.1f,%2.1f), last Point (%2.1f,%2.1f)", traj.getPointsSize(),temp_pose.position.x,temp_pose.position.y,temp_pose2.position.x,temp_pose2.position.y);
    poses.header.frame_id = cost_map->getGlobalFrameID();;
    estTraj_pub.publish(poses);
}

void Dmp::train(const Trajectory& trajectory, string save_directory, bool overwrite)
{
  
  // Set tau, initial_state and attractor_state from the trajectory 
  set_tau(trajectory.duration());
  set_initial_state(trajectory.initial_y());
  set_attractor_state(trajectory.final_y());
  
  VectorXd fa_input_phase;
  MatrixXd f_target;
  computeFunctionApproximatorInputsAndTargets(trajectory, fa_input_phase, f_target);
  
  // Some checks before training function approximators
  assert(!function_approximators_.empty());
  
  for (unsigned int dd=0; dd<function_approximators_.size(); dd++)
  {
    // This is just boring stuff to figure out if and where to store the results of training
    string save_directory_dim;
    if (!save_directory.empty())
    {
      if (function_approximators_.size()==1)
        save_directory_dim = save_directory;
      else
        save_directory_dim = save_directory + "/dim" + boost::lexical_cast<string>(dd);
    }
    
    // Actual training is happening here.
    VectorXd fa_target = f_target.col(dd);
    if (function_approximators_[dd]->isTrained())
      function_approximators_[dd]->reTrain(fa_input_phase,fa_target,save_directory_dim,overwrite);
    else
      function_approximators_[dd]->train(fa_input_phase,fa_target,save_directory_dim,overwrite);
  }
}

void TrajectoryAnalyzer::analyze(Trajectory & trajectory,
                                 std::vector<std::shared_ptr<ProteinSegment>> & protein_segments,
                                 const double & temperature,
                                 const int ensemble_size) {

    std::vector<ProteinSegmentEnsemble> protein_segment_ensembles;

    LOGD << "Fitting protein segments with trajectory frames and computing force constants.";
    while (trajectory.has_next()) {
        for (std::shared_ptr<ProteinSegment> const & protein_segment : protein_segments) {
            protein_segment_ensembles.push_back(ProteinSegmentEnsemble(protein_segment));
        }

        LOGD << "adding frames to protein segment ensembles";
        int frame_nr = 0;
        while (trajectory.has_next() && ++frame_nr <= ensemble_size) {
            Frame frame = trajectory.get_next_frame();

            #pragma omp parallel for
            for (size_t i = 0; i < protein_segment_ensembles.size(); i++) {
                protein_segment_ensembles[i].add_frame(frame);
            }

            LOGV_IF(frame_nr % 100 == 0) << "read " << frame_nr << " frames";
        }
        LOGD << "read total number of " << frame_nr << " frames";

        LOGD << "computing force constants for protein segment ensembles.";
        #pragma omp parallel for
        for (size_t i = 0; i < protein_segment_ensembles.size(); i++) {
            protein_segment_ensembles[i].compute_force_constant(temperature);
        }
    }
    LOGD << "Finished analyzing trajectory";
}

Trajectory::Trajectory(const Trajectory& source_traj, const std::string& planning_group):
  planning_group_name_(planning_group),
  discretization_(source_traj.discretization_)
{
	ROS_INFO("Pezzotto safe at ChomTrajectory constructor");
  num_joints_ = source_traj.getNumJoints();

  num_points_ = source_traj.num_points_;
  start_index_ = source_traj.getStartIndex();
  end_index_ = (num_points_ - 1);
  duration_ = (num_points_ - 1)*discretization_;

  // allocate the memory:
  init();

  full_trajectory_index_.resize(num_points_);

  // now copy the trajectories over:
  for (int i=0; i<num_points_; i++)
  {
    full_trajectory_index_[i] = i;
    for (int j=0; j<num_joints_; j++)
    {
      (*this)(i,j) = source_traj(i, j);
    }
  }
}

// --------------------------------------------------------------------------
// commence le movement qui va vers l'entree d'un pont juste un peu decalle en y (~15cm)
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeEntryNear(Millimeter y)
{
    Trajectory t;
    disableBridgeCaptors();
    LOG_COMMAND("gotoBridgeEntry Hard Near:%d\n", (int)y);
    // on ne va pas loin en y donc on fait un joli mouvement en S
    // met les servos en position
    Millimeter y2 = RobotPos->y();
    if (y>1800) { // pont du bord
      Move->go2Target(Point(BRIDGE_ENTRY_NAVIGATE_X, y2), 3, 30);
    } else if (y<1500) { //  pont du bord ou pont contre le milieu
      Move->go2Target(Point(BRIDGE_ENTRY_NAVIGATE_X, y2), 3, 30);
    } else { // ponts au milieu
      t.push_back(Point(BRIDGE_DETECT_BUMP_X-100, y2));
      t.push_back(Point(BRIDGE_ENTRY_NAVIGATE_X, (2*y+y2)/3)); 
      Move->followTrajectory(t, TRAJECTORY_RECTILINEAR, 3, 30);
    }
     
    Events->wait(evtEndMove);
    Move->enableAccelerationController(false);
    if (checkEndEvents()) return false; 
    if(!Events->isInWaitResult(EVENTS_MOVE_END)) {
      // collision
      Move->backward(100);
      Events->wait(evtEndMoveNoCollision);
      return false;
    }
    
    // va en face du pont
    enableBridgeCaptors();
    MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
    Move->go2Target(BRIDGE_DETECT_BUMP_X, y, 2, 30);
    // dont need to wait, it is done by the upper function
    return true;
}

double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
  double cost = 0;
  double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
  double px, py, pth;
  if (footprint_spec_.size() == 0) {
    // Bug, should never happen
    ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
    return -9;
  }

  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    double f_cost = footprintCost(px, py, pth,
        scale, footprint_spec_,
        costmap_, world_model_);

    if(f_cost < 0){
        return f_cost;
    }

    if(sum_scores_)
        cost +=  f_cost;
    else
        cost = f_cost;
  }
  return cost;
}

  /**
   * return a score for trajectory traj
   */
  virtual double scoreTrajectory(Trajectory &traj)
  { 
      if(traj.getPointsSize()==0)
        return 0.0;

      double px, py, pth;
      traj.getPoint(0, px, py, pth);
      
      double dist_to_goal = hypot(px-goal_x_, py-goal_y_);
      if(dist_to_goal < .2){
        return 0.0;
      }

      
      double start_diff;
      if(!getAngleDiff(px,py,pth,&start_diff))
        return -4.0;

      if(fabs(start_diff) < max_trans_angle_){
	return 0.0;
      }

      if(fabs(traj.xv_) > 0.0 || fabs(traj.yv_) > 0.0)
        return -3.0;
      else if( sign(start_diff) != sign(traj.thetav_) )
        return -2.0;
        
      traj.getPoint(traj.getPointsSize()-1, px, py, pth);
      double end_diff;
      if(!getAngleDiff(px,py,pth,&end_diff))
        return -1.0;
        
      return fabs(end_diff);
  }

void Scene::AddTrajectoryAsSpaceObject(const SpaceObject& obj, Frame frame, Date startingDate, const SpaceObject& relativeTo)
{
	Trajectory tj = Trajectory(obj, frame, Units::Metric::kilometers, distanceScale);
	tj.SetRelativeTo(relativeTo);
	standaloneTrajectories.push_back(/*Trajectory(obj, frame, Units::Metric::kilometers)*/tj);
	Time time(10000, Units::Common::days);
	standaloneTrajectories.at(standaloneTrajectories.size() - 1).SetIncrementalParams(startingDate, time, 10000);
}

QPainterPath getTrajectoryPath(const Trajectory& traj, double timespanMs, double timeLCMs) {
    QPainterPath p;
    p.moveTo(traj.x(0)*fieldXConvert, traj.y(0)*fieldXConvert);
    for (int i = 1; i*timeLCMs < timespanMs; i++) {
        p.lineTo(traj.x(i*timeLCMs*0.001)*fieldXConvert, traj.y(i*timeLCMs*0.001)*fieldXConvert);
    }
    return p;
}

bool HasMajorAirports(Trajectory &trajectory, 
 std::vector<std::string> &airports)
{
  return std::binary_search(airports.begin(),airports.end(),
   trajectory.front().string_property("arr")) &&
   std::binary_search(airports.begin(),airports.end(),
   trajectory.front().string_property("dep"));
}

 void traj_cb(const occgrid_planner::TrajectoryConstPtr & msg) {
     frame_id_ = msg->header.frame_id;
     delay_ = 0.0;
     traj_.clear();
     for (unsigned int i=0;i<msg->Ts.size();i++) {
         traj_.insert(Trajectory::value_type(msg->Ts[i].header.stamp.toSec(), msg->Ts[i]));
     }
     ROS_INFO("Trajectory received");
 }

void Trajectory::copyFrom(Trajectory &other)
{
    m_states_vec.clear();
    for(int i=0; i<other.length(); i++) {
//        other.getStation(i).getPosition().print(std::cout);
        Station st = other.getStation(i);
        this->appendState(st);
    }
    this->cost = other.cost;
}

bool TrajectoryTest::equal(const Trajectory<CvPoint>& t1, const Trajectory<CvPoint>& t2) const
{
	if (t1.getId() != t2.getId())
	{
		CPPUNIT_ASSERT (t1.getId() == t2.getId());
		return false;
	}

	if (t1.size() != t2.size())
	{
		return false;
	}

	for (unsigned i = 0; i < t1.size(); ++i)
	{
		const TrajectoryElement<CvPoint> e1 = t1.getTrajectoryElement(i);
		const TrajectoryElement<CvPoint> e2 = t2.getTrajectoryElement(i);
		if (e1 != e2)
		{
			return false;
		}
	}

	return true;
}

double FutureObstacleCost::cost(const Trajectory& trajectory)
{
    const int num_milestones = trajectory.getNumMilestones();
    const int num_interpolation_samples = trajectory.getNumInterpolationSamples();
    
    for (int i=0; i<num_milestones; i++)
    {
        for (int j = -1; j < num_interpolation_samples; j++)
        {
        }
    }
}

void DmpWithGainSchedules::train(const Trajectory& trajectory, std::string save_directory, bool overwrite)
{
  // First, train the DMP
  Dmp::train(trajectory,save_directory,overwrite);
  
  // Get phase from trajectory
  // Integrate analytically to get phase states
  MatrixXd xs_ana;
  MatrixXd xds_ana;
  Dmp::analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
  MatrixXd xs_phase  = xs_ana.PHASEM(trajectory.length());


  // Get targets from trajectory
  MatrixXd targets = trajectory.misc();
  
  // The dimensionality of the extra variables in the trajectory must be the same as the number of
  // function approximators.
  assert(targets.cols()==(int)function_approximators_gains_.size());
  
  // Train each fa_gains, see below
  // Some checks before training function approximators
  assert(!function_approximators_gains_.empty());
  
  for (unsigned int dd=0; dd<function_approximators_gains_.size(); dd++)
  {
    // This is just boring stuff to figure out if and where to store the results of training
    string save_directory_dim;
    if (!save_directory.empty())
    {
      if (function_approximators_gains_.size()==1)
        save_directory_dim = save_directory;
      else
        save_directory_dim = save_directory + "/gains" + to_string(dd);
    }
    
    // Actual training is happening here.
    VectorXd cur_target = targets.col(dd);
    if (function_approximators_gains_[dd]==NULL)
    {
      cerr << __FILE__ << ":" << __LINE__ << ":";
      cerr << "WARNING: function approximator cannot be trained because it is NULL." << endl;
    }
    else
    {
      if (function_approximators_gains_[dd]->isTrained())
        function_approximators_gains_[dd]->reTrain(xs_phase,cur_target,save_directory_dim,overwrite);
      else
        function_approximators_gains_[dd]->train(xs_phase,cur_target,save_directory_dim,overwrite);
    }

  }
}

int main(int argc, char** argv)
{
  namespace bpo = boost::program_options;
  namespace bfs = boost::filesystem;
  bpo::options_description opts_desc("Allowed options");
  bpo::positional_options_description p;

  string sseq_path;
  string traj_path;
  string map_path;
  double resolution;
  double max_range;
  opts_desc.add_options()
    ("help,h", "produce help message")
    ("sseq", bpo::value(&sseq_path)->required(), "StreamSequence, i.e. asus data.")
    ("traj", bpo::value(&traj_path)->required(), "Trajectory from slam in CLAMS format.")
    ("map", bpo::value(&map_path)->required(), "Where to save the output map.")
    ("resolution", bpo::value(&resolution)->default_value(0.01), "Resolution of the voxel grid used for filtering.")
    ("max-range", bpo::value(&max_range)->default_value(MAX_RANGE_MAP), "Maximum range to use when building the map from the given trajectory, in meters.")
    ;

  p.add("sseq", 1);
  p.add("traj", 1);
  p.add("map", 1);
  
  bpo::variables_map opts;
  bool badargs = false;
  try {
    bpo::store(bpo::command_line_parser(argc, argv).options(opts_desc).positional(p).run(), opts);
    bpo::notify(opts);
  }
  catch(...) { badargs = true; }
  if(opts.count("help") || badargs) {
    cout << "Usage: " << bfs::basename(argv[0]) << " [OPTS] SSEQ TRAJ MAP" << endl;
    cout << "  A map will be generated from SSEQ and TRAJ.  It will be saved to MAP." << endl;
    cout << endl;
    cout << opts_desc << endl;
    return 1;
  }

  Trajectory traj;
  traj.load(traj_path);
  StreamSequenceBase::ConstPtr sseq = StreamSequenceBase::initializeFromDirectory(sseq_path);
  cout << "Building map from " << endl;
  cout << "  " << sseq_path << endl;
  cout << "  " << traj_path << endl;
  cout << "Saving to " << map_path << endl;
  Cloud::Ptr map = SlamCalibrator::buildMap(sseq, traj, max_range, resolution);
  pcl::io::savePCDFileBinary(map_path, *map);
  
  return 0;
}

// --------------------------------------------------------------------------
// va a l'endroit ou on detecte les pont par capteurs sharps 
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeDetection(bool gotoSiouxFirst)
{
  Events->disable(EVENTS_NO_BRIDGE_BUMP_LEFT);
  Events->disable(EVENTS_NO_BRIDGE_BUMP_RIGHT);
  while(true) {
      LOG_INFO("gotoBridgeDetection(%s)\n",
               gotoSiouxFirst?"Passe par le milieu":"Passe par un pont normal");
      Trajectory t;
      t.push_back(Point(RobotPos->x(), RobotPos->y()));
      // if looping (while(true)..) be careful not to go too close.
      if (RobotPos->x() < BRIDGE_DETECT_SHARP_X - 500)
	  t.push_back(Point(RobotPos->x()+250, RobotPos->y()));
      if (gotoSiouxFirst) {
          // va vers le pont du milieu
          t.push_back(Point(BRIDGE_DETECT_SHARP_X-250, BRIDGE_ENTRY_SIOUX_Y));
          t.push_back(Point(BRIDGE_DETECT_SHARP_X,     BRIDGE_ENTRY_SIOUX_Y));
      } else {
          // va vers la gauche du terrain pour detecter la position du pont d'un coup
          t.push_back(Point(BRIDGE_DETECT_SHARP_X-250, BRIDGE_ENTRY_MIDDLE_BORDURE_Y));
          t.push_back(Point(BRIDGE_DETECT_SHARP_X,     BRIDGE_ENTRY_MIDDLE_BORDURE_Y));
      }
      Move->followTrajectory(t, TRAJECTORY_RECTILINEAR, -1, 40); // gain, vitesse max
      Events->wait(evtEndMove);
      if (checkEndEvents()) return false;
      if (Events->isInWaitResult(EVENTS_MOVE_END)) {
	Move->rotate(0);
	Move->stop();

         Events->wait(evtEndMove);
	 if (checkEndEvents()) return false;
	 if (Events->isInWaitResult(EVENTS_MOVE_END)) {
	   return true;
	 }
	 return true;
      }
      if (RobotPos->x()>600 || Timer->time() > 6000) {
	// collision: on recule et on essaye de repartir par un autre endroit...
	Move->backward(150);
	Events->wait(evtEndMoveNoCollision);
	gotoSiouxFirst = !gotoSiouxFirst;
      }
      if (isZeroAngle(RobotPos->thetaAbsolute() - M_PI, M_PI_2) &&
	  RobotPos->x() < 600)
      {
	Move->backward(700);
	Events->wait(evtEndMoveNoCollision);
	gotoSiouxFirst = !gotoSiouxFirst;
      }
  }
  return false;
}

//*************************************************************
int fitter::GetQ(const Trajectory& traj){
  //*************************************************************
  
  if (_model.compare("particle/helix")!=0) return 0;
  double q;
  
  /// 
  q = traj.state(traj.last_fitted_node()).vector()[dim-1];
  
  if (q<0) q=-1; else q=1;
  
  return (int) q;
  
}