diff --git a/basic_autonomy/include/basic_autonomy/basic_autonomy.h b/basic_autonomy/include/basic_autonomy/basic_autonomy.h
index 4fc09f497b..423e786f71 100644
--- a/basic_autonomy/include/basic_autonomy/basic_autonomy.h
+++ b/basic_autonomy/include/basic_autonomy/basic_autonomy.h
@@ -213,7 +213,7 @@ namespace basic_autonomy
      */
           std::vector<PointSpeedPair> create_lanefollow_geometry(const cav_msgs::Maneuver &maneuver, double max_starting_downtrack,
                                                                    const carma_wm::WorldModelConstPtr &wm, cav_msgs::VehicleState &ending_state_before_buffer,
-                                                                   const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config);
+                                                                   const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config, std::unordered_set<lanelet::Id>& visited_lanelets);
 
      /**
       * \brief Adds extra centerline points beyond required message length to lane follow maneuver points so that there's always enough points to calculate trajectory
diff --git a/basic_autonomy/src/basic_autonomy.cpp b/basic_autonomy/src/basic_autonomy.cpp
index 1fa26603af..d86839bdee 100644
--- a/basic_autonomy/src/basic_autonomy.cpp
+++ b/basic_autonomy/src/basic_autonomy.cpp
@@ -27,10 +27,13 @@ namespace basic_autonomy
             std::vector<PointSpeedPair> points_and_target_speeds;
             
             bool first = true;
+            std::unordered_set<lanelet::Id> visited_lanelets;
+
             ROS_DEBUG_STREAM("VehDowntrack:"<<max_starting_downtrack);
             for(const auto &maneuver : maneuvers)
             {
                 double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist);
+                
                 if(first){
                     starting_downtrack = std::min(starting_downtrack, max_starting_downtrack);
                     first = false;
@@ -39,12 +42,12 @@ namespace basic_autonomy
 
                 if(maneuver.type == cav_msgs::Maneuver::LANE_FOLLOWING){
                     ROS_DEBUG_STREAM("Creating Lane Follow Geometry");
-                    std::vector<PointSpeedPair> lane_follow_points = create_lanefollow_geometry(maneuver, max_starting_downtrack, wm, ending_state_before_buffer, general_config, detailed_config);
+                    std::vector<PointSpeedPair> lane_follow_points = create_lanefollow_geometry(maneuver, starting_downtrack, wm, ending_state_before_buffer, general_config, detailed_config, visited_lanelets);
                     points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end());
                 }
                 else if(maneuver.type == cav_msgs::Maneuver::LANE_CHANGE){
                     ROS_DEBUG_STREAM("Creating Lane Change Geometry");
-                    std::vector<PointSpeedPair> lane_change_points = create_lanechange_geometry(maneuver, max_starting_downtrack, wm, ending_state_before_buffer, state, detailed_config);
+                    std::vector<PointSpeedPair> lane_change_points = create_lanechange_geometry(maneuver, starting_downtrack, wm, ending_state_before_buffer, state, detailed_config);
                     points_and_target_speeds.insert(points_and_target_speeds.end(), lane_change_points.begin(), lane_change_points.end());
                 }
                 else{
@@ -63,14 +66,13 @@ namespace basic_autonomy
         }
 
         std::vector<PointSpeedPair> create_lanefollow_geometry(const cav_msgs::Maneuver &maneuver, double starting_downtrack,
-                                                                   const carma_wm::WorldModelConstPtr &wm, cav_msgs::VehicleState &ending_state_before_buffer,
-                                                                    const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config)
+                                                                const carma_wm::WorldModelConstPtr &wm, cav_msgs::VehicleState &ending_state_before_buffer,
+                                                                const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config, std::unordered_set<lanelet::Id> &visited_lanelets)
         {
             if(maneuver.type != cav_msgs::Maneuver::LANE_FOLLOWING){
                 throw std::invalid_argument("Create_lanefollow called on a maneuver type which is not LANE_FOLLOW");
             }
             std::vector<PointSpeedPair> points_and_target_speeds;
-            std::unordered_set<lanelet::Id> visited_lanelets;
 
             cav_msgs::LaneFollowingManeuver lane_following_maneuver = maneuver.lane_following_maneuver;
             
diff --git a/basic_autonomy/test/test_waypoint_generation.cpp b/basic_autonomy/test/test_waypoint_generation.cpp
index b626ab2dea..9ac8de2ae5 100644
--- a/basic_autonomy/test/test_waypoint_generation.cpp
+++ b/basic_autonomy/test/test_waypoint_generation.cpp
@@ -671,6 +671,47 @@ namespace basic_autonomy
         ASSERT_NEAR(lc_start_point.x(), lc_geom.front().x(), 0.000001);
     } 
 
+    TEST(BasicAutonomyTest, lanefollow_geometry_visited_lanelets)
+    {
+
+        double starting_downtrack = 0;
+        cav_msgs::VehicleState ending_state;
+        std::string trajectory_type = "lane_follow";
+        waypoint_generation::GeneralTrajConfig general_config = waypoint_generation::compose_general_trajectory_config(trajectory_type, 0, 0);
+        waypoint_generation::DetailedTrajConfig detailed_config = waypoint_generation::compose_detailed_trajectory_config(0, 0, 0, 0, 0, 5, 0, 0, 20);
+
+        lanelet::Id id1 = 1100;
+        std::unordered_set<lanelet::Id> visited_lanelets;
+        visited_lanelets.insert(id1);
+        std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
+        auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10);
+        wm->setMap(map);
+        carma_wm::test::setSpeedLimit(15_mph, wm);
+
+        carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm);
+
+        cav_msgs::Maneuver maneuver;
+        maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING;
+        maneuver.lane_following_maneuver.lane_ids.push_back(std::to_string(1200));
+        maneuver.lane_following_maneuver.start_dist = 5.0;
+        maneuver.lane_following_maneuver.start_time = ros::Time(0.0);
+        maneuver.lane_following_maneuver.start_speed = 0.0;
+
+        maneuver.lane_following_maneuver.end_dist = 14.98835712;
+        maneuver.lane_following_maneuver.end_speed = 6.7056;
+        maneuver.lane_following_maneuver.end_time = ros::Time(4.4704);
+
+        
+
+        std::vector<basic_autonomy::waypoint_generation::PointSpeedPair> points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, 
+                                                                                    starting_downtrack, wm, ending_state, general_config, detailed_config, visited_lanelets);
+
+        EXPECT_EQ(visited_lanelets.size(), 5);
+
+        EXPECT_TRUE(visited_lanelets.find(id1) != visited_lanelets.end()); // the lanelet previously in the visited lanelet set
+        EXPECT_TRUE(visited_lanelets.find(1200) != visited_lanelets.end()); // new lanelets added to the set with the new maneuver
+    }
+
 } //basic_autonomy namespace
 
 // Run all the tests
diff --git a/inlanecruising_plugin/CMakeLists.txt b/inlanecruising_plugin/CMakeLists.txt
index d3dbd2236d..bc3c846137 100644
--- a/inlanecruising_plugin/CMakeLists.txt
+++ b/inlanecruising_plugin/CMakeLists.txt
@@ -1,5 +1,5 @@
 #
-# Copyright (C) 2018-2020 LEIDOS.
+# 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
@@ -31,6 +31,7 @@ set( CATKIN_DEPS
   trajectory_utils
   autoware_msgs
   carma_wm
+  basic_autonomy
   tf
   tf2
   tf2_geometry_msgs
@@ -72,13 +73,13 @@ add_executable( ${PROJECT_NAME}
   src/main.cpp)
 add_library(inlanecruising_plugin_library 
   src/inlanecruising_plugin.cpp
-  src/smoothing/BSpline.cpp
 )
-target_link_libraries(inlanecruising_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
 add_dependencies(inlanecruising_plugin_library ${catkin_EXPORTED_TARGETS})
+target_link_libraries(inlanecruising_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
-target_link_libraries(${PROJECT_NAME} inlanecruising_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 add_dependencies(${PROJECT_NAME} inlanecruising_plugin_library ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} inlanecruising_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 add_library(helper src/helper.cpp)
 target_link_libraries(helper
@@ -118,7 +119,6 @@ if(CATKIN_ENABLE_TESTING)
 
   catkin_add_gmock(${PROJECT_NAME}-test 
     test/test_inlanecruising_plugin_planning.cpp
-    test/test_inlanecruising_plugin.cpp
   )
   target_link_libraries(${PROJECT_NAME}-test inlanecruising_plugin_library ${catkin_LIBRARIES})
 
diff --git a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.h b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.h
index 962aca24c9..b1fc3fd7d2 100644
--- a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.h
+++ b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.h
@@ -1,7 +1,7 @@
 #pragma once
 
 /*
- * Copyright (C) 2020 LEIDOS.
+ * Copyright (C) 2020-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
diff --git a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.h b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.h
index 24a1a511ac..da15de7c89 100644
--- a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.h
+++ b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.h
@@ -1,7 +1,7 @@
 #pragma once
 
 /*
- * Copyright (C) 2019-2020 LEIDOS.
+ * Copyright (C) 2019-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
@@ -24,16 +24,16 @@
 #include <carma_utils/CARMAUtils.h>
 #include <boost/geometry.hpp>
 #include <carma_wm/Geometry.h>
+#include <basic_autonomy/basic_autonomy.h>
 #include <cav_srvs/PlanTrajectory.h>
 #include <carma_wm/WMListener.h>
+#include <carma_debug_msgs/TrajectoryCurvatureSpeeds.h>
 #include <functional>
-#include <inlanecruising_plugin/smoothing/SplineI.h>
 #include "inlanecruising_config.h"
 #include <unordered_set>
 #include <autoware_msgs/Lane.h>
 #include <ros/ros.h>
 #include <carma_debug_msgs/TrajectoryCurvatureSpeeds.h>
-#include <basic_autonomy/helper_functions.h>
 
 namespace inlanecruising_plugin
 {
@@ -76,135 +76,6 @@ class InLaneCruisingPlugin
    */ 
   bool onSpin();
 
-  /**
-   * \brief Applies the provided speed limits to the provided speeds such that each element is capped at its corresponding speed limit if needed
-   * 
-   * \param speeds The speeds to limit
-   * \param speed_limits The speed limits to apply. Must have the same size as speeds
-   * 
-   * \return The capped speed limits. Has the same size as speeds
-   */ 
-  std::vector<double> apply_speed_limits(const std::vector<double> speeds, const std::vector<double> speed_limits);
-
-  /**
-   * \brief Returns a 2D coordinate frame which is located at p1 and oriented so p2 lies on the +X axis
-   * 
-   * \param p1 The origin point for the frame in the parent frame
-   * \param p2 A point in the parent frame that will define the +X axis relative to p1
-   * 
-   * \return A 2D coordinate frame transform
-   */ 
-  Eigen::Isometry2d compute_heading_frame(const lanelet::BasicPoint2d& p1, const lanelet::BasicPoint2d& p2);
-
-  /**
-   * \brief Reduces the input points to only those points that fit within the provided time boundary
-   * 
-   * \param points The input point speed pairs to reduce
-   * \param time_span The time span in seconds which the output points will fit within
-   * 
-   * \return The subset of points that fit within time_span
-   */ 
-  std::vector<PointSpeedPair> constrain_to_time_boundary(const std::vector<PointSpeedPair>& points, double time_span);
-
-  /**
-   * \brief Method converts a list of lanelet centerline points and current vehicle state into a usable list of trajectory points for trajectory planning
-   * 
-   * \param points The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. 
-   *               These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. 
-   * \param state The current state of the vehicle
-   * \param state_time The abosolute time which the provided vehicle state corresponds to
-   * 
-   * \return A list of trajectory points to send to the carma planning stack
-   */ 
-  std::vector<cav_msgs::TrajectoryPlanPoint>
-  compose_trajectory_from_centerline(const std::vector<PointSpeedPair>& points, const cav_msgs::VehicleState& state, const ros::Time& state_time);
-
-  /**
-   * \brief Method combines input points, times, orientations, and an absolute start time to form a valid carma platform trajectory
-   * 
-   * NOTE: All input vectors must be the same size. The output vector will take this size.
-   * 
-   * \param points The points in the map frame that the trajectory will follow. Units m
-   * \param times The times which at the vehicle should arrive at the specified points. First point should have a value of 0. Units s
-   * \param yaws The orientation the vehicle should achieve at each point. Units radians
-   * \param startTime The absolute start time which will be used to update the input relative times. Units s
-   * 
-   * \return A list of trajectory points built from the provided inputs.
-   */ 
-  std::vector<cav_msgs::TrajectoryPlanPoint> trajectory_from_points_times_orientations(
-      const std::vector<lanelet::BasicPoint2d>& points, const std::vector<double>& times,
-      const std::vector<double>& yaws, ros::Time startTime);
-
-  /**
-   * \brief Converts a set of requested LANE_FOLLOWING maneuvers to point speed limit pairs. 
-   * 
-   * \param maneuvers The list of maneuvers to convert
-   * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier.
-   *                               If the first maneuver exceeds this then it's downtrack will be shifted to this value.
-   * 
-   * \param wm Pointer to intialized world model for semantic map access
-   * 
-   * \return List of centerline points paired with speed limits
-   */ 
-  std::vector<PointSpeedPair> maneuvers_to_points(const std::vector<cav_msgs::Maneuver>& maneuvers,
-                                                  double max_starting_downtrack,
-                                                  const carma_wm::WorldModelConstPtr& wm);
-
-  /**
-   * \brief Computes a spline based on the provided points
-   * 
-   * \param basic_points The points to use for fitting the spline
-   * 
-   * \return A spline which has been fit to the provided points
-   */ 
-  std::unique_ptr<smoothing::SplineI> compute_fit(const std::vector<lanelet::BasicPoint2d>& basic_points);
-
-  /**
-   * \brief Returns the speeds of points closest to the lookahead distance.
-   * 
-   * \param points The points in the map frame that the trajectory will follow. Units m
-   * \param speeds Speeds assigned to points that trajectory will follow. Unit m/s
-   * \param lookahead The lookahead distance to obtain future points' speed. Unit m
-   * 
-   * \return A vector of speed values shifted by the lookahead distance.
-   */ 
-
-  std::vector<double> get_lookahead_speed(const std::vector<lanelet::BasicPoint2d>& points, const std::vector<double>& speeds, const double& lookahead);
-
-  /**
-   * \brief Applies the longitudinal acceleration limit to each point's speed
-   * 
-   * \param downtracks downtrack distances corresponding to each speed
-   * \param curv_speeds vehicle velocity in m/s.
-   * \param accel_limit vehicle longitudinal acceleration in m/s^2.
-   * 
-   * \return optimized speeds for each dowtrack points that satisfies longitudinal acceleration
-   */ 
-  std::vector<double> optimize_speed(const std::vector<double>& downtracks, const std::vector<double>& curv_speeds, double accel_limit);
-
-  /**
-   * \brief Given the curvature fit, computes the curvature at the given step along the curve
-   * 
-   * \param step_along_the_curve Value in double from 0.0 (curvature start) to 1.0 (curvature end) representing where to calculate the curvature
-   * 
-   * \param fit_curve curvature fit
-   * 
-   * \return Curvature (k = 1/r, 1/meter)
-   */ 
-  double compute_curvature_at(const inlanecruising_plugin::smoothing::SplineI& fit_curve, double step_along_the_curve) const;
-
-    /**
-   * \brief Attaches back_distance length of points in front of future points
-   * 
-   * \param points all point speed pairs
-   * \param nearest_pt_index idx of nearest point to the vehicle
-   * \param future_points future points before which to attach the points
-   * \param back_distance number of back distance in meters
-   * 
-   * \return point speed pairs with back distance length of points in front of future points
-   */ 
-  std::vector<PointSpeedPair> attach_back_points(const std::vector<PointSpeedPair>& points, const int nearest_pt_index, 
-                               std::vector<inlanecruising_plugin::PointSpeedPair> future_points, double back_distance) const;
   /**
    * \brief set the yield service
    * 
@@ -220,20 +91,11 @@ class InLaneCruisingPlugin
    * \return true or falss
    */
   bool validate_yield_plan(const cav_msgs::TrajectoryPlan& yield_plan);
+
+  cav_msgs::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later
   
 private:
 
-  /**
-   * \brief Returns the min, and its idx, from the vector of values, excluding given set of values
-   * 
-   * \param values vector of values
-   * 
-   * \param excluded set of excluded values
-   * 
-   * \return minimum value and its idx
-   */ 
-  std::pair<double, size_t> min_with_exclusions(const std::vector<double>& values, const std::unordered_set<size_t>& excluded) const;
-  
   carma_wm::WorldModelConstPtr wm_;
   InLaneCruisingPluginConfig config_;
   PublishPluginDiscoveryCB plugin_discovery_publisher_;
@@ -242,7 +104,6 @@ class InLaneCruisingPlugin
   cav_msgs::Plugin plugin_discovery_msg_;
   DebugPublisher debug_publisher_;
   carma_debug_msgs::TrajectoryCurvatureSpeeds debug_msg_;
-  cav_msgs::VehicleState ending_state_before_buffer; //state before applying extra points for curvature calculation that are removed later
 
 };
 
diff --git a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.h b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.h
index 257070f1ec..9a0c6173bc 100644
--- a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.h
+++ b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.h
@@ -1,7 +1,7 @@
 #pragma once
 
 /*
- * Copyright (C) 2019-2020 LEIDOS.
+ * Copyright (C) 2019-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
@@ -63,6 +63,7 @@ class InLaneCruisingPluginNode
                      config.speed_moving_average_window_size);
     pnh.param<int>("curvature_moving_average_window_size", config.curvature_moving_average_window_size,
                      config.curvature_moving_average_window_size);
+    pnh.param<double>("buffer_ending_downtrack", config.buffer_ending_downtrack, config.buffer_ending_downtrack);
     pnh.param<double>("/vehicle_acceleration_limit", config.max_accel, config.max_accel);
     pnh.param<double>("/vehicle_lateral_accel_limit", config.lateral_accel_limit, config.lateral_accel_limit);
     pnh.param<bool>("enable_object_avoidance", config.enable_object_avoidance, config.enable_object_avoidance);
diff --git a/inlanecruising_plugin/launch/inlanecruising_plugin.launch b/inlanecruising_plugin/launch/inlanecruising_plugin.launch
index 48b5f517b9..1592b0d40f 100644
--- a/inlanecruising_plugin/launch/inlanecruising_plugin.launch
+++ b/inlanecruising_plugin/launch/inlanecruising_plugin.launch
@@ -1,5 +1,5 @@
 <!--
-  Copyright (C) 2018-2020 LEIDOS.
+  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
diff --git a/inlanecruising_plugin/package.xml b/inlanecruising_plugin/package.xml
index 7a77b80577..06e391adf0 100644
--- a/inlanecruising_plugin/package.xml
+++ b/inlanecruising_plugin/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 
 <!--  
- Copyright (C) 2018-2020 LEIDOS.
+ 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
@@ -31,10 +31,10 @@
   <depend>carma_utils</depend>
   <depend>autoware_msgs</depend>
   <depend>carma_wm</depend>
+  <depend>basic_autonomy</depend>
   <depend>tf</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend> 
   <depend>trajectory_utils</depend>   
-  <depend>carma_debug_msgs</depend> 
-  <depend>basic_autonomy</depend>   
+  <depend>carma_debug_msgs</depend>  
 </package>
diff --git a/inlanecruising_plugin/src/inlanecruising_plugin.cpp b/inlanecruising_plugin/src/inlanecruising_plugin.cpp
index 76b3c22ddf..f285498d82 100644
--- a/inlanecruising_plugin/src/inlanecruising_plugin.cpp
+++ b/inlanecruising_plugin/src/inlanecruising_plugin.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2019-2020 LEIDOS.
+ * Copyright (C) 2019-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
@@ -29,11 +29,8 @@
 #include <Eigen/Geometry>
 #include <Eigen/LU>
 #include <Eigen/SVD>
-#include <inlanecruising_plugin/smoothing/SplineI.h>
-#include <inlanecruising_plugin/smoothing/BSpline.h>
 #include <inlanecruising_plugin/inlanecruising_plugin.h>
-#include <inlanecruising_plugin/log/log.h>
-#include <inlanecruising_plugin/smoothing/filters.h>
+
 
 
 
@@ -81,7 +78,24 @@ bool InLaneCruisingPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& r
       break;
     }
   }
-  auto points_and_target_speeds = maneuvers_to_points(maneuver_plan, std::max((double)0, current_downtrack - config_.back_distance), wm_); // Convert maneuvers to points
+
+  basic_autonomy:: waypoint_generation::DetailedTrajConfig wpg_detail_config;
+  basic_autonomy:: waypoint_generation::GeneralTrajConfig wpg_general_config;
+
+  wpg_general_config = basic_autonomy:: waypoint_generation::compose_general_trajectory_config("inlanecruising",
+                                                                              config_.default_downsample_ratio,
+                                                                              config_.turn_downsample_ratio);
+
+  wpg_detail_config = basic_autonomy:: waypoint_generation::compose_detailed_trajectory_config(config_.trajectory_time_length, 
+                                                                            config_.curve_resample_step_size, config_.minimum_speed, 
+                                                                            config_.max_accel * config_.max_accel_multiplier,
+                                                                            config_.lateral_accel_limit * config_.lat_accel_multiplier, 
+                                                                            config_.speed_moving_average_window_size, 
+                                                                            config_.curvature_moving_average_window_size, config_.back_distance,
+                                                                            config_.buffer_ending_downtrack);
+  
+  auto points_and_target_speeds = basic_autonomy::waypoint_generation::create_geometry_profile(maneuver_plan, std::max((double)0, current_downtrack - config_.back_distance),
+                                                                         wm_, ending_state_before_buffer_, req.vehicle_state, wpg_general_config, wpg_detail_config);
 
   ROS_DEBUG_STREAM("points_and_target_speeds: " << points_and_target_speeds.size());
 
@@ -92,9 +106,17 @@ bool InLaneCruisingPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& r
   original_trajectory.header.stamp = ros::Time::now();
   original_trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());
 
-  original_trajectory.trajectory_points = compose_trajectory_from_centerline(points_and_target_speeds, req.vehicle_state, req.header.stamp); // Compute the trajectory
+  original_trajectory.trajectory_points = basic_autonomy:: waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, 
+                                                                                req.vehicle_state, req.header.stamp, wm_, ending_state_before_buffer_, debug_msg_, 
+                                                                                wpg_detail_config); // Compute the trajectory
   original_trajectory.initial_longitudinal_velocity = std::max(req.vehicle_state.longitudinal_vel, config_.minimum_speed);
 
+  // Set the planning plugin field name
+  for (auto& p : original_trajectory.trajectory_points) {
+    p.planner_plugin_name = plugin_discovery_msg_.name;
+  }
+  
+  
   if (config_.enable_object_avoidance)
   {
     ROS_DEBUG_STREAM("Activate Object Avoidance");
@@ -151,582 +173,6 @@ bool InLaneCruisingPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& r
   return true;
 }
 
-std::vector<double> InLaneCruisingPlugin::apply_speed_limits(const std::vector<double> speeds,
-                                                             const std::vector<double> speed_limits)
-{
-  ROS_DEBUG_STREAM("Speeds list size: " << speeds.size());
-  ROS_DEBUG_STREAM("SpeedLimits list size: " << speed_limits.size());
-
-  if (speeds.size() != speed_limits.size())
-  {
-    throw std::invalid_argument("Speeds and speed limit lists not same size");
-  }
-  std::vector<double> out;
-  for (size_t i = 0; i < speeds.size(); i++)
-  {
-    out.push_back(std::min(speeds[i], speed_limits[i]));
-  }
-
-  return out;
-}
-
-Eigen::Isometry2d InLaneCruisingPlugin::compute_heading_frame(const lanelet::BasicPoint2d& p1,
-                                                              const lanelet::BasicPoint2d& p2)
-{
-  Eigen::Rotation2Dd yaw(atan2(p2.y() - p1.y(), p2.x() - p1.x()));
-
-  return carma_wm::geometry::build2dEigenTransform(p1, yaw);
-}
-
-
-
-std::pair<double, size_t> InLaneCruisingPlugin::min_with_exclusions(const std::vector<double>& values, const std::unordered_set<size_t>& excluded) const {
-  double min = std::numeric_limits<double>::max();
-  size_t best_idx = -1;
-  for (size_t i = 0; i < values.size(); i++) {
-    if (excluded.find(i) != excluded.end()) {
-      continue;
-    }
-
-    if (values[i] < min) {
-      min = values[i];
-      best_idx = i;
-    }
-  }
-
-  return std::make_pair(min, best_idx);
-}
-
-std::vector<double> InLaneCruisingPlugin::optimize_speed(const std::vector<double>& downtracks, const std::vector<double>& curv_speeds, double accel_limit)
-{
-  if (downtracks.size() != curv_speeds.size())
-  {
-    throw std::invalid_argument("Downtracks and speeds do not have the same size");
-  }
-
-  if (accel_limit <= 0)
-  {
-    throw std::invalid_argument("Accel limits should be positive");
-  }
-
-  bool optimize = true;
-  std::unordered_set<size_t> visited_idx;
-  visited_idx.reserve(curv_speeds.size());
-
-  std::vector<double> output = curv_speeds;
-
-  while (optimize)
-  {
-    auto min_pair = min_with_exclusions(curv_speeds, visited_idx);
-    size_t min_idx = std::get<1>(min_pair);
-    if (min_idx == -1) {
-      break;
-    }
-
-    visited_idx.insert(min_idx); // Mark this point as visited
-
-    double v_i = std::get<0>(min_pair);
-    double x_i = downtracks[min_idx];
-    for (int i = min_idx - 1; i > 0; i--) { // NOTE: Do not use size_t for i type here as -- with > 0 will result in overflow
-                                            //       First point's speed is left unchanged as it is current speed of the vehicle
-      double v_f = curv_speeds[i];
-      double dv = v_f - v_i;
-      
-      double x_f = downtracks[i];
-      double dx = x_f - x_i;
-
-      if(dv > 0) {
-        v_f = std::min(v_f, sqrt(v_i * v_i - 2 * accel_limit * dx)); // inverting accel as we are only visiting deceleration case
-        visited_idx.insert(i);
-      } else if (dv < 0) {
-        break;
-      }
-      output[i] = v_f;
-      v_i = v_f;
-      x_i = x_f;
-    }
-  }
-
-  log::printDoublesPerLineWithPrefix("only_reverse[i]: ", output);
-  
-  output = trajectory_utils::apply_accel_limits_by_distance(downtracks, output, accel_limit, accel_limit);
-  log::printDoublesPerLineWithPrefix("after_forward[i]: ", output);
-
-  return output;
-}
-
-std::vector<PointSpeedPair> InLaneCruisingPlugin::constrain_to_time_boundary(const std::vector<PointSpeedPair>& points,
-                                                                             double time_span)
-{
-  std::vector<lanelet::BasicPoint2d> basic_points;
-  std::vector<double> speeds;
-  basic_autonomy::waypoint_generation::split_point_speed_pairs(points, &basic_points, &speeds);
-
-  std::vector<double> downtracks = carma_wm::geometry::compute_arc_lengths(basic_points);
-
-  size_t time_boundary_exclusive_index =
-      trajectory_utils::time_boundary_index(downtracks, speeds, config_.trajectory_time_length);
-
-  if (time_boundary_exclusive_index == 0)
-  {
-    throw std::invalid_argument("No points to fit in timespan"); 
-  }
-
-  std::vector<PointSpeedPair> time_bound_points;
-  time_bound_points.reserve(time_boundary_exclusive_index);
-
-  if (time_boundary_exclusive_index == points.size())
-  {
-    time_bound_points.insert(time_bound_points.end(), points.begin(),
-                             points.end());  // All points fit within time boundary
-  }
-  else
-  {
-    time_bound_points.insert(time_bound_points.end(), points.begin(),
-                             points.begin() + time_boundary_exclusive_index - 1);  // Limit points by time boundary
-  }
-
-  return time_bound_points;
-}
-
-
-std::vector<cav_msgs::TrajectoryPlanPoint> InLaneCruisingPlugin::compose_trajectory_from_centerline(
-    const std::vector<PointSpeedPair>& points, const cav_msgs::VehicleState& state, const ros::Time& state_time)
-{
- ROS_DEBUG_STREAM("VehicleState: "
-                   << " x: " << state.X_pos_global << " y: " << state.Y_pos_global << " yaw: " << state.orientation
-                   << " speed: " << state.longitudinal_vel);
-
-  log::printDebugPerLine(points, &log::pointSpeedPairToStream);
-
-  int nearest_pt_index = basic_autonomy::waypoint_generation::get_nearest_index_by_downtrack(points, wm_, state);
-
-  ROS_DEBUG_STREAM("NearestPtIndex: " << nearest_pt_index);
-
-  std::vector<PointSpeedPair> future_points(points.begin() + nearest_pt_index + 1, points.end()); // Points in front of current vehicle position
-  auto time_bound_points = constrain_to_time_boundary(future_points, config_.trajectory_time_length);
-  
-  ROS_DEBUG_STREAM("Got time_bound_points with size:" << time_bound_points.size());
-  log::printDebugPerLine(time_bound_points, &log::pointSpeedPairToStream);
-
-
-  std::vector<PointSpeedPair> back_and_future = attach_back_points(points,nearest_pt_index,time_bound_points, config_.back_distance);
-
-  ROS_DEBUG_STREAM("Got back_and_future points with size" <<back_and_future.size());
-  log::printDebugPerLine(back_and_future, &log::pointSpeedPairToStream);
-
-  std::vector<double> speed_limits;
-  std::vector<lanelet::BasicPoint2d> curve_points;
-  basic_autonomy::waypoint_generation::split_point_speed_pairs(back_and_future, &curve_points, &speed_limits);
-  
-  std::unique_ptr<smoothing::SplineI> fit_curve = compute_fit(curve_points); // Compute splines based on curve points
-  if (!fit_curve)
-  {
-    throw std::invalid_argument("Could not fit a spline curve along the given trajectory!");
-  }
-
-  ROS_DEBUG("Got fit");
-
-  ROS_DEBUG_STREAM("speed_limits.size() " << speed_limits.size());
-
-  std::vector<lanelet::BasicPoint2d> all_sampling_points;
-  all_sampling_points.reserve(1 + curve_points.size() * 2);
-
-  std::vector<double> distributed_speed_limits;
-  distributed_speed_limits.reserve(1 + curve_points.size() * 2);
-
-  // compute total length of the trajectory to get correct number of points 
-  // we expect using curve_resample_step_size
-  std::vector<double> downtracks_raw = carma_wm::geometry::compute_arc_lengths(curve_points);
-
-  int total_step_along_curve = static_cast<int>(downtracks_raw.back() / config_.curve_resample_step_size);
-
-  int current_speed_index = 0;
-  size_t total_point_size = curve_points.size();
-
-  double step_threshold_for_next_speed = (double)total_step_along_curve / (double)total_point_size;
-  double scaled_steps_along_curve = 0.0; // from 0 (start) to 1 (end) for the whole trajectory
-  std::vector<double> better_curvature;
-  better_curvature.reserve(1 + curve_points.size() * 2);
-    
-  for (size_t steps_along_curve = 0; steps_along_curve < total_step_along_curve; steps_along_curve++) // Resample curve at tighter resolution
-  {
-    lanelet::BasicPoint2d p = (*fit_curve)(scaled_steps_along_curve);
-    
-    all_sampling_points.push_back(p);
-    double c = compute_curvature_at((*fit_curve), scaled_steps_along_curve);
-    better_curvature.push_back(c);
-    if ((double)steps_along_curve > step_threshold_for_next_speed)
-    {
-      step_threshold_for_next_speed += (double)total_step_along_curve / (double) total_point_size;
-      current_speed_index ++;
-    }
-    distributed_speed_limits.push_back(speed_limits[current_speed_index]); // Identify speed limits for resampled points
-    scaled_steps_along_curve += 1.0/total_step_along_curve; //adding steps_along_curve_step_size
-  }
-
-  ROS_DEBUG_STREAM("Got sampled points with size:" << all_sampling_points.size());
-  log::printDebugPerLine(all_sampling_points, &log::basicPointToStream);
-
-  std::vector<double> final_yaw_values = carma_wm::geometry::compute_tangent_orientations(all_sampling_points);
-
-  log::printDoublesPerLineWithPrefix("raw_curvatures[i]: ", better_curvature);
-
-  std::vector<double> curvatures = smoothing::moving_average_filter(better_curvature, config_.curvature_moving_average_window_size, false);
-
-  std::vector<double> ideal_speeds =
-      trajectory_utils::constrained_speeds_for_curvatures(curvatures, config_.lateral_accel_limit);
-  
-  log::printDoublesPerLineWithPrefix("curvatures[i]: ", curvatures);
-  log::printDoublesPerLineWithPrefix("ideal_speeds: ", ideal_speeds);
-  log::printDoublesPerLineWithPrefix("final_yaw_values[i]: ", final_yaw_values);
-
-  std::vector<double> constrained_speed_limits = apply_speed_limits(ideal_speeds, distributed_speed_limits);
-  log::printDoublesPerLineWithPrefix("constrained_speed_limits: ", constrained_speed_limits);
-
-  ROS_DEBUG("Processed all points in computed fit");
-
-  if (all_sampling_points.size() == 0)
-  {
-    ROS_WARN_STREAM("No trajectory points could be generated");
-    return {};
-  }
-
-  // Add current vehicle point to front of the trajectory
-
-  nearest_pt_index = basic_autonomy::waypoint_generation::get_nearest_index_by_downtrack(all_sampling_points,wm_, state);
-  ROS_DEBUG_STREAM("Current state's nearest_pt_index: " << nearest_pt_index);
-  ROS_DEBUG_STREAM("Curvature right now: " << better_curvature[nearest_pt_index] << ", at state x: " << state.X_pos_global << ", state y: " << state.Y_pos_global);
-  ROS_DEBUG_STREAM("Corresponding to point: x: " << all_sampling_points[nearest_pt_index].x() << ", y:" << all_sampling_points[nearest_pt_index].y());
-
-  int buffer_pt_index = basic_autonomy::waypoint_generation::get_nearest_index_by_downtrack(all_sampling_points,wm_, ending_state_before_buffer);
-  ROS_DEBUG_STREAM("Ending state's index before applying buffer (buffer_pt_index): " << buffer_pt_index);
-  ROS_DEBUG_STREAM("Corresponding to point: x: " << all_sampling_points[buffer_pt_index].x() << ", y:" << all_sampling_points[buffer_pt_index].y());
-  
-  if (nearest_pt_index + 1 >= buffer_pt_index)
-  {
-    ROS_WARN_STREAM("Current state is at or passed the planned end distance. Couldn't generate trajectory");
-    return {};
-  }
-  //drop buffer points here
-  std::vector<lanelet::BasicPoint2d> future_basic_points(all_sampling_points.begin() + nearest_pt_index + 1,
-                                            all_sampling_points.begin()+ buffer_pt_index);  // Points in front of current vehicle position
-
-  std::vector<double> future_speeds(constrained_speed_limits.begin() + nearest_pt_index + 1,
-                                            constrained_speed_limits.begin() + buffer_pt_index);  // Points in front of current vehicle position
-  std::vector<double> future_yaw(final_yaw_values.begin() + nearest_pt_index + 1,
-                                            final_yaw_values.begin() + buffer_pt_index);  // Points in front of current vehicle position
-  std::vector<double>  final_actual_speeds = future_speeds;
-  all_sampling_points = future_basic_points;
-  final_yaw_values = future_yaw;
-  ROS_DEBUG_STREAM("Trimmed future points to size: " << future_basic_points.size() );
-
-  lanelet::BasicPoint2d cur_veh_point(state.X_pos_global, state.Y_pos_global);
-
-  all_sampling_points.insert(all_sampling_points.begin(),
-                             cur_veh_point);  // Add current vehicle position to front of sample points
-
-  final_actual_speeds.insert(final_actual_speeds.begin(), state.longitudinal_vel);
-
-  final_yaw_values.insert(final_yaw_values.begin(), state.orientation);
-
-  // Compute points to local downtracks
-  std::vector<double> downtracks = carma_wm::geometry::compute_arc_lengths(all_sampling_points);
-
-  // Apply accel limits
-  final_actual_speeds = optimize_speed(downtracks, final_actual_speeds, config_.max_accel);
-
-  log::printDoublesPerLineWithPrefix("postAccel[i]: ", final_actual_speeds);
-
-  final_actual_speeds = smoothing::moving_average_filter(final_actual_speeds, config_.speed_moving_average_window_size);
-  log::printDoublesPerLineWithPrefix("post_average[i]: ", final_actual_speeds);
-
-  for (auto& s : final_actual_speeds)  // Limit minimum speed. TODO how to handle stopping?
-  {
-    s = std::max(s, config_.minimum_speed);
-  }
-
-  log::printDoublesPerLineWithPrefix("post_min_speed[i]: ", final_actual_speeds);
-
-  // Convert speeds to times
-  std::vector<double> times;
-  trajectory_utils::conversions::speed_to_time(downtracks, final_actual_speeds, &times);
-
-  log::printDoublesPerLineWithPrefix("times[i]: ", times);
-  
-  // Build trajectory points
-  std::vector<cav_msgs::TrajectoryPlanPoint> traj_points =
-      trajectory_from_points_times_orientations(all_sampling_points, times, final_yaw_values, state_time); 
-
-  if (config_.publish_debug) {
-    carma_debug_msgs::TrajectoryCurvatureSpeeds msg;
-    msg.velocity_profile = final_actual_speeds;
-    msg.relative_downtrack = downtracks;
-    msg.tangent_headings = final_yaw_values;
-    std::vector<double> aligned_speed_limits(constrained_speed_limits.begin() + nearest_pt_index,
-                                            constrained_speed_limits.end());
-    msg.speed_limits = aligned_speed_limits;
-    std::vector<double> aligned_curvatures(curvatures.begin() + nearest_pt_index,
-                                            curvatures.end());
-    msg.curvatures = aligned_curvatures;
-    msg.lat_accel_limit = config_.lateral_accel_limit;
-    msg.lon_accel_limit = config_.max_accel;
-    msg.starting_state = state;
-    debug_msg_ = msg;
-  }
-  return traj_points;
-}
-
-
-std::vector<cav_msgs::TrajectoryPlanPoint> InLaneCruisingPlugin::trajectory_from_points_times_orientations(
-    const std::vector<lanelet::BasicPoint2d>& points, const std::vector<double>& times, const std::vector<double>& yaws,
-    ros::Time startTime)
-{
-  if (points.size() != times.size() || points.size() != yaws.size())
-  {
-    throw std::invalid_argument("All input vectors must have the same size");
-  }
-
-  std::vector<cav_msgs::TrajectoryPlanPoint> traj;
-  traj.reserve(points.size());
-
-  for (int i = 0; i < points.size(); i++)
-  {
-    cav_msgs::TrajectoryPlanPoint tpp;
-    ros::Duration relative_time(times[i]);
-    tpp.target_time = startTime + relative_time;
-    tpp.x = points[i].x();
-    tpp.y = points[i].y();
-    tpp.yaw = yaws[i];
-
-    tpp.controller_plugin_name = "default";
-    tpp.planner_plugin_name = plugin_discovery_msg_.name;
-
-    traj.push_back(tpp);
-  }
-
-  return traj;
-}
-
-std::vector<PointSpeedPair> InLaneCruisingPlugin::attach_back_points(const std::vector<PointSpeedPair>& points, 
-                          const int nearest_pt_index, std::vector<inlanecruising_plugin::PointSpeedPair> future_points, double back_distance) const
-{
-  std::vector<PointSpeedPair> back_and_future;
-  back_and_future.reserve(points.size());
-  double total_dist = 0;
-  int min_i = 0;
-  for (int i = nearest_pt_index; i > 0; --i) { 
-    min_i = i;
-    total_dist += lanelet::geometry::distance2d(points[i].point, points[i-1].point);
-  
-    if (total_dist > back_distance) {
-      break;
-    }
-  }
-
-  back_and_future.insert(back_and_future.end(), points.begin() + min_i, points.begin() + nearest_pt_index + 1);
-  back_and_future.insert(back_and_future.end(), future_points.begin(), future_points.end());
-  return back_and_future;
-}
-
-
-std::vector<PointSpeedPair> InLaneCruisingPlugin::maneuvers_to_points(const std::vector<cav_msgs::Maneuver>& maneuvers,
-                                                                      double max_starting_downtrack,
-                                                                      const carma_wm::WorldModelConstPtr& wm)
-{
-  std::vector<PointSpeedPair> points_and_target_speeds;
-  std::unordered_set<lanelet::Id> visited_lanelets;
-
-  bool first = true;
-  ROS_DEBUG_STREAM("VehDowntrack: " << max_starting_downtrack);
-  for (const auto& manuever : maneuvers)
-  {
-    if (manuever.type != cav_msgs::Maneuver::LANE_FOLLOWING)
-    {
-      throw std::invalid_argument("In-Lane Cruising does not support this maneuver type");
-    }
-
-    cav_msgs::LaneFollowingManeuver lane_following_maneuver = manuever.lane_following_maneuver;
-
-    double starting_downtrack = lane_following_maneuver.start_dist;
-    if (first)
-    {
-      if (starting_downtrack > max_starting_downtrack)
-      {
-        starting_downtrack = max_starting_downtrack;
-      }
-      first = false;
-    }
-
-    ROS_DEBUG_STREAM("Used downtrack: " << starting_downtrack);
-
-    auto lanelets = wm->getLaneletsBetween(starting_downtrack, lane_following_maneuver.end_dist + config_.buffer_ending_downtrack, true, true);
-
-    if (lanelets.empty())
-    {
-      ROS_ERROR_STREAM("Detected no lanelets between starting_downtrack: " << starting_downtrack  << ", and lane_following_maneuver.end_dist: " << lane_following_maneuver.end_dist);
-      throw std::invalid_argument("Detected no lanelets between starting_downtrack and end_dist");
-    }
-
-    ROS_DEBUG_STREAM("Maneuver");
-
-    lanelet::BasicLineString2d downsampled_centerline;
-    downsampled_centerline.reserve(200);
-
-    // getLaneletsBetween is inclusive lanelets between its two boundaries
-    // which may return lanechange lanelets, so 
-    // exclude lanechanges and plan for only the straight part
-    int curr_idx = 0;
-    auto following_lanelets = wm->getMapRoutingGraph()->following(lanelets[curr_idx]);
-    lanelet::ConstLanelets straight_lanelets;
-
-    if (lanelets.size() <= 1) // no lane change anyways if only size 1
-    {
-      ROS_DEBUG_STREAM("Detected one straight lanelet Id: " << lanelets[curr_idx].id());
-      straight_lanelets = lanelets;
-    }
-    else
-    {
-      // skip all lanechanges until lane follow starts
-      while (curr_idx + 1 < lanelets.size() && 
-              std::find(following_lanelets.begin(),following_lanelets.end(), lanelets[curr_idx + 1]) == following_lanelets.end())
-      {
-        ROS_DEBUG_STREAM("As there were no directly following lanelets after this, skipping lanelet id: " << lanelets[curr_idx].id());
-        curr_idx ++;
-        following_lanelets = wm->getMapRoutingGraph()->following(lanelets[curr_idx]);
-      }
-
-      ROS_DEBUG_STREAM("Added lanelet Id for lane follow: " << lanelets[curr_idx].id());
-      // guaranteed to have at least one "straight" lanelet (e.g the last one in the list)
-      straight_lanelets.push_back(lanelets[curr_idx]);
-
-      // add all lanelets on the straight road until next lanechange
-      while (curr_idx + 1 < lanelets.size() && 
-              std::find(following_lanelets.begin(),following_lanelets.end(), lanelets[curr_idx + 1]) != following_lanelets.end())
-      {
-        curr_idx++;
-        ROS_DEBUG_STREAM("Added lanelet Id forlane follow: " << lanelets[curr_idx].id());
-        straight_lanelets.push_back(lanelets[curr_idx]);
-        following_lanelets = wm->getMapRoutingGraph()->following(lanelets[curr_idx]);
-      }
-    }
-    
-    for (auto l : straight_lanelets)
-    {
-      ROS_DEBUG_STREAM("Processing lanelet ID: " << l.id());
-      if (visited_lanelets.find(l.id()) == visited_lanelets.end())
-      {
-
-        bool is_turn = false;
-        if(l.hasAttribute("turn_direction")) {
-          std::string turn_direction = l.attribute("turn_direction").value();
-          is_turn = turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0;
-        }
-        
-        lanelet::BasicLineString2d centerline = l.centerline2d().basicLineString();
-        lanelet::BasicLineString2d downsampled_points;
-        if (is_turn) {
-          downsampled_points = carma_utils::containers::downsample_vector(centerline, config_.turn_downsample_ratio);
-        } else {
-          downsampled_points = carma_utils::containers::downsample_vector(centerline, config_.default_downsample_ratio);
-        }
-        downsampled_centerline = carma_wm::geometry::concatenate_line_strings(downsampled_centerline, downsampled_points);
-        visited_lanelets.insert(l.id());
-      }
-    }
-
-
-    first = true;
-    for (auto p : downsampled_centerline)
-    {
-      if (first && points_and_target_speeds.size() != 0)
-      {
-        first = false;
-        continue;  // Skip the first point if we have already added points from a previous maneuver to avoid duplicates
-      }
-      PointSpeedPair pair;
-      pair.point = p;
-      pair.speed = lane_following_maneuver.end_speed;
-      points_and_target_speeds.push_back(pair);
-    }
-  }
-    // Here we are limiting the trajectory length to the given length by maneuver end dist as opposed to the end of lanelets involved.
-    double starting_route_downtrack = wm_->routeTrackPos(points_and_target_speeds.front().point).downtrack;
-    double ending_downtrack = maneuvers.back().lane_following_maneuver.end_dist;
-
-    if(ending_downtrack + config_.buffer_ending_downtrack < wm_->getRouteEndTrackPos().downtrack){
-      ending_downtrack = ending_downtrack + config_.buffer_ending_downtrack;
-    }
-    else
-    {
-      ending_downtrack = wm_->getRouteEndTrackPos().downtrack;
-    } 
-    
-    size_t max_i = points_and_target_speeds.size() - 1;
-    size_t unbuffered_idx = points_and_target_speeds.size() - 1;
-    bool found_unbuffered_idx = false;
-    double dist_accumulator = starting_route_downtrack;
-    lanelet::BasicPoint2d prev_point;
-
-    for (int i = 0; i < points_and_target_speeds.size(); i ++) {
-      auto current_point = points_and_target_speeds[i].point;
-      if (i == 0) {
-        prev_point = current_point;
-        continue;
-      }
-
-      double delta_d = lanelet::geometry::distance2d(prev_point, current_point);
-      ROS_DEBUG_STREAM("Index i: " << i << ", delta_d: " << delta_d << ", dist_accumulator:" << dist_accumulator <<", current_point.x():" << current_point.x() << 
-        "current_point.y():" << current_point.y());
-      dist_accumulator += delta_d;
-      if (dist_accumulator > maneuvers.back().lane_following_maneuver.end_dist && !found_unbuffered_idx)
-      {
-        unbuffered_idx = i - 1;
-        ROS_DEBUG_STREAM("Found index unbuffered_idx at: " << unbuffered_idx);
-        found_unbuffered_idx = true;
-      }
-      if (dist_accumulator > ending_downtrack) {
-        max_i = i - 1;
-        ROS_DEBUG_STREAM("Max_i breaking at: i: " << i << ", max_i: " << max_i);
-        break;
-      }
-      prev_point = current_point;
-    }
-    ending_state_before_buffer.X_pos_global = points_and_target_speeds[unbuffered_idx].point.x();
-    ending_state_before_buffer.Y_pos_global = points_and_target_speeds[unbuffered_idx].point.y();
-    ROS_DEBUG_STREAM("Here ending_state_before_buffer.X_pos_global: " << ending_state_before_buffer.X_pos_global << 
-      ", and Y_pos_global" << ending_state_before_buffer.Y_pos_global);
-
-    std::vector<PointSpeedPair> constrained_points(points_and_target_speeds.begin(), points_and_target_speeds.begin() + max_i);
-    return constrained_points;
-}
-
-std::unique_ptr<smoothing::SplineI>
-InLaneCruisingPlugin::compute_fit(const std::vector<lanelet::BasicPoint2d>& basic_points)
-{
-  if (basic_points.size() < 4)
-  {
-    ROS_WARN_STREAM("Insufficient Spline Points");
-    return nullptr;
-  }
-
-  std::unique_ptr<smoothing::SplineI> spl = std::make_unique<smoothing::BSpline>();
-  
-  spl->setPoints(basic_points);
-
-  return spl;
-}
-
-double InLaneCruisingPlugin::compute_curvature_at(const inlanecruising_plugin::smoothing::SplineI& fit_curve, double step_along_the_curve) const
-{
-  lanelet::BasicPoint2d f_prime_pt = fit_curve.first_deriv(step_along_the_curve);
-  lanelet::BasicPoint2d f_prime_prime_pt = fit_curve.second_deriv(step_along_the_curve);
-  // Convert to 3d vector to do 3d vector operations like cross.
-  Eigen::Vector3d f_prime = {f_prime_pt.x(), f_prime_pt.y(), 0};
-  Eigen::Vector3d f_prime_prime = {f_prime_prime_pt.x(), f_prime_prime_pt.y(), 0};
-  return (f_prime.cross(f_prime_prime)).norm()/(pow(f_prime.norm(),3));
-}
-
 void InLaneCruisingPlugin::set_yield_client(ros::ServiceClient& client)
 {
   yield_client_ = client;
diff --git a/inlanecruising_plugin/src/main.cpp b/inlanecruising_plugin/src/main.cpp
index 588ee0aa27..000adf945e 100644
--- a/inlanecruising_plugin/src/main.cpp
+++ b/inlanecruising_plugin/src/main.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2019-2020 LEIDOS.
+ * Copyright (C) 2019-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
diff --git a/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp b/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp
index ddb5916f13..413101e82c 100644
--- a/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp
+++ b/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2019-2020 LEIDOS.
+ * Copyright (C) 2019-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
@@ -611,7 +611,7 @@ TEST(InLaneCruisingPluginTest, attach_back_points)
 
   int nearest_pt_index = 2;
 
-  auto result = plugin.attach_back_points(points, nearest_pt_index, future_points, 1.5);
+  auto result = plugin.attach_past_points(points, future_points, nearest_pt_index, 1.5);
 
   ASSERT_EQ(points.size()  -1, result.size());
   ASSERT_NEAR(1.0, result[0].point.x(), 0.0000001);
diff --git a/inlanecruising_plugin/test/test_inlanecruising_plugin_planning.cpp b/inlanecruising_plugin/test/test_inlanecruising_plugin_planning.cpp
index 3b076fc03a..594d972933 100644
--- a/inlanecruising_plugin/test/test_inlanecruising_plugin_planning.cpp
+++ b/inlanecruising_plugin/test/test_inlanecruising_plugin_planning.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2019-2020 LEIDOS.
+ * Copyright (C) 2019-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
@@ -51,13 +51,9 @@
 #include <carma_utils/containers/containers.h>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+
 #include <Eigen/LU>
 #include <Eigen/SVD>
-#include <inlanecruising_plugin/smoothing/SplineI.h>
-#include <inlanecruising_plugin/smoothing/BSpline.h>
-#include <inlanecruising_plugin/inlanecruising_plugin.h>
-#include <inlanecruising_plugin/log/log.h>
-#include <inlanecruising_plugin/smoothing/filters.h>
 #include <unordered_set>
 
 
@@ -277,7 +273,7 @@ TEST(WaypointGeneratorTest, DISABLED_test_compute_fit_full_generation)
     // ROS_INFO_STREAM("Original point: x: " << route_geometry[i].x() << "y: " << route_geometry[i].y());
   }
 
-  std::unique_ptr<smoothing::SplineI> fit_curve = inlc.compute_fit(downsampled_points);
+  std::unique_ptr<basic_autonomy::smoothing::SplineI> fit_curve = basic_autonomy:: waypoint_generation::compute_fit(downsampled_points);
   
   std::vector<lanelet::BasicPoint2d> spline_points;