text: /*
   * Copyright (C) 2018-2021 LEIDOS.
   *
   * Licensed under the Apache License, Version 2.0 (the "License"); you may not
   * use this file except in compliance with the License. You may obtain a copy of
   * the License at
   *
   * http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
   * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
   * License for the specific language governing permissions and limitations under
   * the License.
   */
  
  #include "pure_pursuit_wrapper/pure_pursuit_wrapper.hpp"
  #include <trajectory_utils/conversions/conversions.h>
  #include <carma_wm/Geometry.h>
  #include <algorithm>
  
  
  namespace pure_pursuit_wrapper
  {
  PurePursuitWrapper::PurePursuitWrapper(PurePursuitWrapperConfig config, WaypointPub waypoint_pub, PluginDiscoveryPub plugin_discovery_pub)
    : config_(config), waypoint_pub_(waypoint_pub), plugin_discovery_pub_(plugin_discovery_pub)
  {
    plugin_discovery_msg_.name = "Pure Pursuit";
    plugin_discovery_msg_.versionId = "v1.0";
    plugin_discovery_msg_.available = true;
    plugin_discovery_msg_.activated = true;
    plugin_discovery_msg_.type = cav_msgs::Plugin::CONTROL;
    plugin_discovery_msg_.capability = "control_pure_pursuit_plan/plan_controls";
  }
  
  bool PurePursuitWrapper::onSpin()
  {
    plugin_discovery_pub_(plugin_discovery_msg_);
    return true;
  }
  
  
  void PurePursuitWrapper::trajectoryPlanHandler(const cav_msgs::TrajectoryPlan::ConstPtr& tp)
  {
    ROS_DEBUG_STREAM("Received TrajectoryPlanCurrentPosecallback message");
  
    std::vector<double> times;
    std::vector<double> downtracks;
  
    //Remove trajectory points with duplicated timestamps
    std::vector<cav_msgs::TrajectoryPlanPoint> trajectory_points = remove_repeated_timestamps(tp->trajectory_points);
    ROS_DEBUG_STREAM("Original Trajectory size:"<<tp->trajectory_points.size() <<" Size after removing duplicate timestamps:"<<trajectory_points.size());
  
    trajectory_utils::conversions::trajectory_to_downtrack_time(trajectory_points, &downtracks, &times);
  
    std::vector<double> speeds;
    trajectory_utils::conversions::time_to_speed(downtracks, times, tp->initial_longitudinal_velocity, &speeds);
  
    if (speeds.size() != trajectory_points.size())
    {
      throw std::invalid_argument("Speeds and trajectory points sizes do not match");
    }
  
    for (size_t i =0; i < speeds.size(); i++) { // Ensure 0 is min speed
      speeds[i] = std::max(0.0, speeds[i]);
    }
  
    std::vector<double> lag_speeds = apply_response_lag(speeds, downtracks, config_.vehicle_response_lag); // This call requires that the first speed point be current speed to work as expected
  
    autoware_msgs::Lane lane;
    lane.header = tp->header;
    std::vector<autoware_msgs::Waypoint> waypoints;
    waypoints.reserve(trajectory_points.size());
  
    for (int i = 0; i < trajectory_points.size(); i++)
    {
      autoware_msgs::Waypoint wp;
  
      wp.pose.pose.position.x = trajectory_points[i].x;
      wp.pose.pose.position.y = trajectory_points[i].y;
      wp.twist.twist.linear.x = lag_speeds[i];
      ROS_DEBUG_STREAM("Setting waypoint idx: " << i <<", with planner: << " << trajectory_points[i].planner_plugin_name << ", x: " << trajectory_points[i].x << 
                              ", y: " << trajectory_points[i].y <<
                              ", speed: " << lag_speeds[i]* 2.23694 << "mph");
      waypoints.push_back(wp);
    }
  
    lane.waypoints = waypoints;
    waypoint_pub_(lane);
  };
  
  std::vector<double> PurePursuitWrapper::apply_response_lag(const std::vector<double>& speeds, const std::vector<double> downtracks, double response_lag) const { // Note first speed is assumed to be vehicle speed
    if (speeds.size() != downtracks.size()) {
      throw std::invalid_argument("Speed list and downtrack list are not the same size.");
    }
  
    std::vector<double> output;
    if (speeds.empty()) {
      return output;
    }
  
    double lookahead_distance = speeds[0] * response_lag;
  
    double downtrack_cutoff = downtracks[0] + lookahead_distance;
    size_t lookahead_count = std::lower_bound(downtracks.begin(),downtracks.end(), downtrack_cutoff) - downtracks.begin(); // Use binary search to find lower bound cutoff point
    output = trajectory_utils::shift_by_lookahead(speeds, (unsigned int) lookahead_count);
    return output;
  }
  
  std::vector<cav_msgs::TrajectoryPlanPoint> PurePursuitWrapper::remove_repeated_timestamps(const std::vector<cav_msgs::TrajectoryPlanPoint>& traj_points){
    
    std::vector<cav_msgs::TrajectoryPlanPoint> new_traj_points;
  
    cav_msgs::TrajectoryPlanPoint prev_point;
    bool first = true;
  
    for(auto point : traj_points){
  
      if(first){
        first = false;
        prev_point = point;
        new_traj_points.push_back(point);
        continue;
      }
  
      if(point.target_time != prev_point.target_time){
        new_traj_points.push_back(point);
        prev_point = point;
      }
      else{
        ROS_DEBUG_STREAM("Duplicate point found");
      }
    }
  
    return new_traj_points;
  
  }
  
  }  // namespace pure_pursuit_wrapper
  