diff --git a/rosplane/include/path_manager_base.hpp b/rosplane/include/path_manager_base.hpp
index ba15170..5bd9e51 100644
--- a/rosplane/include/path_manager_base.hpp
+++ b/rosplane/include/path_manager_base.hpp
@@ -45,6 +45,8 @@ class path_manager_base : public rclcpp::Node
   int num_waypoints_;
   int idx_a_; /** index to the waypoint that was most recently achieved */
 
+  bool temp_waypoint_ = false;
+
   struct input_s
   {
     float pn;  /** position north */
diff --git a/rosplane/include/path_manager_example.hpp b/rosplane/include/path_manager_example.hpp
index 29da5bc..72a2916 100644
--- a/rosplane/include/path_manager_example.hpp
+++ b/rosplane/include/path_manager_example.hpp
@@ -12,6 +12,7 @@ namespace rosplane
 enum class fillet_state
 {
   STRAIGHT,
+  TRANSITION,
   ORBIT
 };
 
@@ -31,14 +32,18 @@ class path_manager_example : public path_manager_base
   path_manager_example();
 
 private:
+  std::chrono::time_point<std::chrono::system_clock> start_time;
   virtual void manage(const struct input_s & input,
                       struct output_s & output);
+  int orbit_direction(float pn, float pe, float chi, float c_n, float c_e);
+  void increment_indices(int & idx_a, int & idx_b, int & idx_c, const struct input_s & input, struct output_s & output);
 
   void manage_line(const struct input_s & input,
                    struct output_s & output);
   void manage_fillet(const struct input_s & input,
                      struct output_s & output);
   fillet_state fil_state_;
+  void construct_straight_path();
   void manage_dubins(const struct input_s & input,
                      struct output_s & output);
   dubin_state dub_state_;
diff --git a/rosplane/params/fixedwing_mission.yaml b/rosplane/params/fixedwing_mission.yaml
index 1cab008..837f363 100644
--- a/rosplane/params/fixedwing_mission.yaml
+++ b/rosplane/params/fixedwing_mission.yaml
@@ -2,20 +2,20 @@
 wp:
   w: [0.0, 0.0, -50.0]
   chi_d: 1.1518
-  use_chi: True
+  use_chi: False
   va_d: 25.0
 wp:
-  w: [200.0, 100.0, -50.0]
+  w: [2000.0, 1000.0, -50.0]
   chi_d: 1.1518
-  use_chi: True
+  use_chi: False
   va_d: 25.0
 wp:
-  w: [200.0, -100.0, -50.0]
+  w: [2000.0, -1000.0, -50.0]
   chi_d: 1.1518
-  use_chi: True
+  use_chi: False
   va_d: 25.0
 wp:
-  w: [0.0, -100.0, -50.0]
+  w: [0.0, -1000.0, -50.0]
   chi_d: 1.1518
-  use_chi: True
-  va_d: 25.0
\ No newline at end of file
+  use_chi: False
+  va_d: 25.0
diff --git a/rosplane/src/path_manager_base.cpp b/rosplane/src/path_manager_base.cpp
index 72e3997..cf9522e 100644
--- a/rosplane/src/path_manager_base.cpp
+++ b/rosplane/src/path_manager_base.cpp
@@ -1,7 +1,9 @@
 #include "path_manager_base.hpp"
+#include "Eigen/src/Core/Matrix.h"
 #include "iostream"
 #include "path_manager_example.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include <limits>
 #include <rclcpp/logging.hpp>
 
 namespace rosplane
@@ -17,9 +19,7 @@ path_manager_base::path_manager_base()
   current_path_pub_ = this->create_publisher<rosplane_msgs::msg::CurrentPath>("current_path", 10);
   update_timer_ =
     this->create_wall_timer(10ms, std::bind(&path_manager_base::current_path_publish, this));
-  // interesting read on wall timer
-  // https://answers.ros.org/question/375561/create-wall-timer-using-callback-with-parameters-ros2-c/
-  //
+
   // Set the parameter callback, for when parameters are changed.
   parameter_callback_handle_ = this->add_on_set_parameters_callback(
     std::bind(&path_manager_base::parametersCallback, this, std::placeholders::_1));
@@ -56,36 +56,59 @@ void path_manager_base::vehicle_state_callback(const rosplane_msgs::msg::State &
 
 void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint & msg)
 {
+  double R_min = params.get_double("R_min");
+
   if (msg.clear_wp_list == true) {
     waypoints_.clear();
     num_waypoints_ = 0;
     idx_a_ = 0;
     return;
   }
-  if (msg.set_current || num_waypoints_ == 0) {
-    waypoint_s currentwp;
-    currentwp.w[0] = vehicle_state_.position[0];
-    currentwp.w[1] = vehicle_state_.position[1];
-    currentwp.w[2] = (vehicle_state_.position[2] > -25 ? msg.w[2] : vehicle_state_.position[2]);
-    currentwp.chi_d = vehicle_state_.chi;
-    currentwp.use_chi = msg.use_chi;
 
-    currentwp.va_d = msg.va_d;
+  if (waypoints_.size() == 0)
+  {
+    waypoint_s temp_waypoint;
 
-    waypoints_.clear();
-    waypoints_.push_back(currentwp);
-    num_waypoints_ = 1;
-    idx_a_ = 0;
+    temp_waypoint.w[0] = vehicle_state_.position[0];
+    temp_waypoint.w[1] = vehicle_state_.position[1];
+    temp_waypoint.w[2] = vehicle_state_.position[2];
+
+    temp_waypoint.chi_d = 0.0; // Doesn't matter, it is never used.
+    temp_waypoint.use_chi = false;
+    temp_waypoint.va_d = msg.va_d; // Use the va_d for the next waypoint.
+
+    waypoints_.push_back(temp_waypoint);
+    num_waypoints_++;
+    temp_waypoint_ = true;
   }
+  
+  // Add a default comparison for the last waypoint for feasiblity check.
   waypoint_s nextwp;
+  Eigen::Vector3f w_existing(std::numeric_limits<double>::infinity(),
+                             std::numeric_limits<double>::infinity(),
+                             std::numeric_limits<double>::infinity());
   nextwp.w[0] = msg.w[0];
   nextwp.w[1] = msg.w[1];
   nextwp.w[2] = msg.w[2];
   nextwp.chi_d = msg.chi_d;
   nextwp.use_chi = msg.use_chi;
   nextwp.va_d = msg.va_d;
+  // Save the last waypoint for comparison.
+  if (waypoints_.size() > 0)
+  {
+    waypoint_s waypoint = waypoints_.back();
+    w_existing << waypoint.w[0], waypoint.w[1], waypoint.w[2];
+  }
   waypoints_.push_back(nextwp);
   num_waypoints_++;
+
+  // Warn if too close to the last waypoint.
+  Eigen::Vector3f w_new(msg.w[0], msg.w[1], msg.w[2]);
+
+  if ((w_new - w_existing).norm() < R_min)
+  {
+    RCLCPP_WARN_STREAM(this->get_logger(), "A waypoint is too close to the next waypoint. Indices: " << waypoints_.size() - 2 << ", " << waypoints_.size() - 1);
+  }
 }
 
 void path_manager_base::current_path_publish() //const rclcpp::TimerEvent &
@@ -119,22 +142,6 @@ void path_manager_base::current_path_publish() //const rclcpp::TimerEvent &
   current_path.rho = output.rho;
   current_path.lamda = output.lamda;
 
-  RCLCPP_DEBUG_STREAM(this->get_logger(), "Publishing Current Path!");
-
-  RCLCPP_DEBUG_STREAM(this->get_logger(), "Path Type: " << current_path.path_type);
-  RCLCPP_DEBUG_STREAM(this->get_logger(), "va_d: " << current_path.va_d);
-  RCLCPP_DEBUG_STREAM(this->get_logger(),
-                      "r: " << current_path.r[0] << ", " << current_path.r[1] << ", "
-                            << current_path.r[2]);
-  RCLCPP_DEBUG_STREAM(this->get_logger(),
-                      "q: " << current_path.q[0] << ", " << current_path.q[1] << ", "
-                            << current_path.q[2]);
-  RCLCPP_DEBUG_STREAM(this->get_logger(),
-                      "c: " << current_path.c[0] << ", " << current_path.c[1] << ", "
-                            << current_path.c[2]);
-  RCLCPP_DEBUG_STREAM(this->get_logger(), "rho: " << current_path.rho);
-  RCLCPP_DEBUG_STREAM(this->get_logger(), "lamda: " << current_path.lamda);
-
   current_path_pub_->publish(current_path);
 }
 
diff --git a/rosplane/src/path_manager_example.cpp b/rosplane/src/path_manager_example.cpp
index 1a1ca27..b3799fd 100644
--- a/rosplane/src/path_manager_example.cpp
+++ b/rosplane/src/path_manager_example.cpp
@@ -1,10 +1,12 @@
 //#include "path_manager_example.h"
 //#include "ros/ros.h"
 #include "path_manager_example.hpp"
+#include "Eigen/src/Core/Matrix.h"
 #include "rclcpp/rclcpp.hpp"
 #include <cmath>
 #include <rclcpp/logging.hpp>
 #include <string>
+#include <chrono>
 
 namespace rosplane
 {
@@ -18,29 +20,50 @@ path_manager_example::path_manager_example()
   // Declare the parameters used in this class with the ROS2 system
   declare_parameters();
   params.set_parameters();
+
+  start_time = std::chrono::system_clock::now();
+
 }
 
 void path_manager_example::manage(const input_s & input, output_s & output)
 {
   // For readability, declare the parameters that will be used in the function here
   double R_min = params.get_double("R_min");
+  double abort_altitude = params.get_double("abort_altitude"); // This is the true altitude not the down position (no need for a negative)
+  double abort_airspeed = params.get_double("abort_airspeed");
 
-  if (num_waypoints_ < 2) {
-    //ROS_WARN_THROTTLE(4, "No waypoints received! Loitering about origin at 50m");
-    //RCLCPP_WARN(this->get_logger(), "No waypoints received! Loitering about origin at 50m",4); !!!
+  if (num_waypoints_ == 0) 
+  {
+    auto now = std::chrono::system_clock::now();
+    if (float(std::chrono::system_clock::to_time_t(now) - std::chrono::system_clock::to_time_t(start_time)) >= 10.0) 
+    {
+      RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "No waypoits received, orbiting origin at " << abort_altitude << " meters.");
+      output.flag = false; // Indicate that the path is an orbit.
+      output.va_d = abort_airspeed; // Set to the abort_airspeed.
+      output.c[0] = 0.0f; // Direcct the center of the orbit to the origin at the abort abort_altitude.
+      output.c[1] = 0.0f;
+      output.c[2] = -abort_altitude;
+      output.rho = R_min; // Make the orbit at the minimum turn radius.
+      output.lamda = 1; // Orbit in a clockwise manner.
+    }
+  }
+  else if (num_waypoints_ == 1) 
+  {
+    // If only a single waypoint is given, orbit it.
     output.flag = false;
-    output.va_d = 20;
-    output.c[0] = 0.0f;
-    output.c[1] = 0.0f;
-    output.c[2] = -50.0f;
+    output.va_d = waypoints_[0].va_d;
+    output.c[0] = waypoints_[0].w[0];
+    output.c[1] = waypoints_[0].w[1];
+    output.c[2] = waypoints_[0].w[2];
     output.rho = R_min;
-    output.lamda = 1;
-  } else {
+    output.lamda = orbit_direction(input.pn, input.pe, input.chi, output.c[0], output.c[1]); // Calculate the most conveinent orbit direction of that point.
+  }
+  else {
     if (waypoints_[idx_a_].use_chi) {
       manage_dubins(input, output);
-    } else {
+    } else { // If the heading through the point does not matter use the default path following.
       /** Switch the following for flying directly to waypoints, or filleting corners */
-      //manage_line(input, output);
+      //manage_line(input, output); // TODO add ROS param for just line following or filleting?
       manage_fillet(input, output);
     }
   }
@@ -51,52 +74,25 @@ void path_manager_example::manage_line(const input_s & input,
 {
   // For readability, declare the parameters that will be used in the function here
   bool orbit_last = params.get_bool("orbit_last");
-  double R_min = params.get_double("R_min");
 
   Eigen::Vector3f p;
   p << input.pn, input.pe, -input.h;
 
   int idx_b;
   int idx_c;
-  if (idx_a_ == num_waypoints_ - 1) {
-
-    if (orbit_last) {
-      output.flag = false;
-      output.va_d = waypoints_[idx_a_].va_d;
-      output.c[0] = waypoints_[idx_a_].w[0];
-      output.c[1] = waypoints_[idx_a_].w[1];
-      output.c[2] = waypoints_[idx_a_].w[2];
-      output.r[0] = 0.0;
-      output.r[1] = 0.0;
-      output.r[2] = 0.0;
-      output.q[0] = 0.0;
-      output.q[1] = 0.0;
-      output.q[2] = 0.0;
-      output.rho = R_min;
-
-      if (waypoints_[idx_a_].chi_d < M_PI) {
-        output.lamda = 1;
-      } else {
-        output.lamda = -1;
-      }
 
-      return;
-    }
+  increment_indices(idx_a_, idx_b, idx_c, input, output);
 
-    idx_b = 0;
-    idx_c = 1;
-  } else if (idx_a_ == num_waypoints_ - 2) {
-    idx_b = num_waypoints_ - 1;
-    idx_c = 0;
-  } else {
-    idx_b = idx_a_ + 1;
-    idx_c = idx_b + 1;
+  if (orbit_last && (idx_a_ == num_waypoints_ - 1 || idx_a_ == num_waypoints_ -2))
+  {
+    return;
   }
 
   Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
   Eigen::Vector3f w_i(waypoints_[idx_b].w);
   Eigen::Vector3f w_ip1(waypoints_[idx_c].w);
-
+  
+  // Fill out data for straight line to the next point.
   output.flag = true;
   output.va_d = waypoints_[idx_a_].va_d;
   output.r[0] = w_im1(0);
@@ -109,6 +105,13 @@ void path_manager_example::manage_line(const input_s & input,
   output.q[2] = q_im1(2);
 
   Eigen::Vector3f n_i = (q_im1 + q_i).normalized();
+
+  // Check if the planes were aligned and then handle the normal vector correctly.
+  if (n_i.isZero()){ 
+    n_i = q_im1;
+  }
+
+  // If the aircraft passes through the plane that bisects the angle between the waypoint lines transition.
   if ((p - w_i).dot(n_i) > 0.0f) {
     if (idx_a_ == num_waypoints_ - 1) {
       idx_a_ = 0;
@@ -125,7 +128,7 @@ void path_manager_example::manage_fillet(const input_s & input,
   bool orbit_last = params.get_bool("orbit_last");
   double R_min = params.get_double("R_min");
 
-  if (num_waypoints_ < 3) //since it fillets don't make sense between just two points
+  if (num_waypoints_ < 3) // Do not attempt to fillet between only 2 points.
   {
     manage_line(input, output);
     return;
@@ -133,97 +136,110 @@ void path_manager_example::manage_fillet(const input_s & input,
 
   Eigen::Vector3f p;
   p << input.pn, input.pe, -input.h;
-
-  int idx_b;
-  int idx_c;
-  if (idx_a_ == num_waypoints_ - 1) {
-
-    if (orbit_last) {
-      output.flag = false;
-      output.va_d = waypoints_[idx_a_].va_d;
-      output.c[0] = waypoints_[idx_a_].w[0];
-      output.c[1] = waypoints_[idx_a_].w[1];
-      output.c[2] = waypoints_[idx_a_].w[2];
-      output.r[0] = 0.0;
-      output.r[1] = 0.0;
-      output.r[2] = 0.0;
-      output.q[0] = 0.0;
-      output.q[1] = 0.0;
-      output.q[2] = 0.0;
-      output.rho = R_min;
-
-      if (waypoints_[idx_a_].chi_d < M_PI) {
-        output.lamda = 1;
-      } else {
-        output.lamda = -1;
-      }
-
-      return;
-    }
-
-    idx_b = 0;
-    idx_c = 1;
-  } else if (idx_a_ == num_waypoints_ - 2) {
-    idx_b = num_waypoints_ - 1;
-
-    if (orbit_last) {
-      idx_c = 0;
-      idx_a_ = idx_b;
-      return;
-    } else {
-      idx_c = 0;
-    }
-  } else {
-    idx_b = idx_a_ + 1;
-    idx_c = idx_b + 1;
+  
+  // idx_a is the waypoint you are coming from.
+  int idx_b; // Next waypoint.
+  int idx_c; // Waypoint after next.
+  
+  increment_indices(idx_a_, idx_b, idx_c, input, output);
+
+  if (orbit_last && idx_a_ == num_waypoints_ - 1)
+  {
+    return;
   }
 
-  Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
-  Eigen::Vector3f w_i(waypoints_[idx_b].w);
-  Eigen::Vector3f w_ip1(waypoints_[idx_c].w);
+  Eigen::Vector3f w_im1(waypoints_[idx_a_].w); // Previous waypoint NED im1 means i-1
+  Eigen::Vector3f w_i(waypoints_[idx_b].w); // Waypoint the aircraft is headed towards.
+  Eigen::Vector3f w_ip1(waypoints_[idx_c].w); // Waypoint after leaving waypoint idx_b.
 
-  output.va_d = waypoints_[idx_a_].va_d;
-  output.r[0] = w_im1(0);
-  output.r[1] = w_im1(1);
+  output.va_d = waypoints_[idx_a_].va_d; // Desired airspeed of this leg of the waypoints.
+  output.r[0] = w_im1(0); // See chapter 11 of the UAV book for more information.
+  output.r[1] = w_im1(1); // This is the point that is a point along the commanded path.
   output.r[2] = w_im1(2);
-  Eigen::Vector3f q_im1 = (w_i - w_im1).normalized();
-  Eigen::Vector3f q_i = (w_ip1 - w_i).normalized();
-  float beta = acosf(-q_im1.dot(q_i));
+  // The vector pointing into the turn (vector pointing from previous waypoint to the next).
+  Eigen::Vector3f q_im1 = (w_i - w_im1); 
+  float dist_w_im1 = q_im1.norm();
+  q_im1 = q_im1.normalized();
+
+  // The vector pointing out of the turn (vector points from next waypoint to the next next waypoint).
+  Eigen::Vector3f q_i = (w_ip1 - w_i); 
+  float dist_w_ip1 = q_i.norm();
+  q_i = q_i.normalized();
+
+  float varrho = acosf(-q_im1.dot(q_i)); // This is var_rho in the book. Angle of the turn.
+  
+  // Check to see if filleting is possible for given waypoints.
+  // Find closest dist to w_i
+  float min_dist = std::min(dist_w_ip1, dist_w_im1);
+
+  // Use varrho to find the distance to bisector from closest waypoint.
+  float max_r = min_dist * sinf(varrho/2.0);
+
+  // If max_r (maximum radius possible for angle) is smaller than R_min, do line management.
+  if (R_min > max_r)
+  {
+    // While in the too acute region, publish notice every 10 seconds.
+    RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Too acute an angle, using line management. Values, max_r: " << max_r << " R_min: " << R_min);
+    manage_line(input, output);
+    return;
+  }
 
   Eigen::Vector3f z;
   switch (fil_state_) {
     case fillet_state::STRAIGHT:
-      output.flag = true;
-      output.q[0] = q_im1(0);
+    {
+      output.flag = true; // Indicate flying a straight path.
+      output.q[0] = q_im1(0); // Fly along vector into the turn the origin of the vector is r (set as previous waypoint above).
       output.q[1] = q_im1(1);
       output.q[2] = q_im1(2);
-      output.c[0] = 1;
+      output.c[0] = 1; // Fill rest of the data though it is not used.
       output.c[1] = 1;
       output.c[2] = 1;
       output.rho = 1;
       output.lamda = 1;
-      z = w_i - q_im1 * (R_min / tanf(beta / 2.0));
-      if ((p - z).dot(q_im1) > 0) fil_state_ = fillet_state::ORBIT;
+      z = w_i - q_im1 * (R_min / tanf(varrho / 2.0)); // Point in plane where after passing through the aircraft should begin the turn.
+      if ((p - z).dot(q_im1) > 0) fil_state_ = fillet_state::TRANSITION; // Check to see if passed through the plane.
       break;
+    }
+    case fillet_state::TRANSITION:
+    {
+      output.flag = false; // Indicate that aircraft is following an orbit.
+      output.q[0] = q_i(0); // Load the message with the vector that will be follwed after the orbit.
+      output.q[1] = q_i(1);
+      output.q[2] = q_i(2);
+      Eigen::Vector3f c = w_i - (q_im1 - q_i).normalized() * (R_min / sinf(varrho / 2.0)); // Calculate the center of the orbit.
+      output.c[0] = c(0); // Load message with the center of the orbit.
+      output.c[1] = c(1);
+      output.c[2] = c(2);
+      output.rho = R_min; // Command the orbit radius to be the minimum acheivable.
+      output.lamda = ((q_im1(0) * q_i(1) - q_im1(1) * q_i(0)) > 0 ? 1 : -1); // Find the direction to orbit the point. TODO change this to the orbit_direction.
+      z = w_i + q_i * (R_min / tanf(varrho / 2.0)); // Find the point in the plane that once you pass through you should increment the indexes and follow a straight line.
+      if ((p - z).dot(q_i) < 0) { // Check to see if passed through plane.
+        fil_state_ = fillet_state::ORBIT;
+      }
+      break;
+    }
     case fillet_state::ORBIT:
-      output.flag = false;
-      output.q[0] = q_i(0);
+    {
+      output.flag = false; // Indicate that aircraft is following an orbit.
+      output.q[0] = q_i(0); // Load the message with the vector that will be follwed after the orbit.
       output.q[1] = q_i(1);
       output.q[2] = q_i(2);
-      Eigen::Vector3f c = w_i - (q_im1 - q_i).normalized() * (R_min / sinf(beta / 2.0));
-      output.c[0] = c(0);
+      Eigen::Vector3f c = w_i - (q_im1 - q_i).normalized() * (R_min / sinf(varrho / 2.0)); // Calculate the center of the orbit.
+      output.c[0] = c(0); // Load message with the center of the orbit.
       output.c[1] = c(1);
       output.c[2] = c(2);
-      output.rho = R_min;
-      output.lamda = ((q_im1(0) * q_i(1) - q_im1(1) * q_i(0)) > 0 ? 1 : -1);
-      z = w_i + q_i * (R_min / tanf(beta / 2.0));
-      if ((p - z).dot(q_i) > 0) {
+      output.rho = R_min; // Command the orbit radius to be the minimum acheivable.
+      output.lamda = ((q_im1(0) * q_i(1) - q_im1(1) * q_i(0)) > 0 ? 1 : -1); // Find the direction to orbit the point. TODO change this to the orbit_direction.
+      z = w_i + q_i * (R_min / tanf(varrho / 2.0)); // Find the point in the plane that once you pass through you should increment the indexes and follow a straight line.
+      if ((p - z).dot(q_i) > 0) { // Check to see if passed through plane.
         if (idx_a_ == num_waypoints_ - 1) idx_a_ = 0;
         else
           idx_a_++;
         fil_state_ = fillet_state::STRAIGHT;
       }
       break;
+    }
   }
 }
 
@@ -466,8 +482,7 @@ void path_manager_example::dubinsParameters(const waypoint_s start_node, const w
       dubinspath_.L = L3;
       idx = 3;
     }
-    if (L4 < dubinspath_.L) {
-      dubinspath_.L = L4;
+    if (L4 < dubinspath_.L) { dubinspath_.L = L4;
       idx = 4;
     }
 
@@ -530,6 +545,77 @@ void path_manager_example::declare_parameters()
 {
   params.declare_param("R_min", 25.0);
   params.declare_param("orbit_last", false);
+  params.declare_param("abort_altitude", 50.0);
+  params.declare_param("abort_airspeed", 15.0);
+}
+
+int path_manager_example::orbit_direction(float pn, float pe, float chi, float c_n, float c_e)
+{
+  Eigen::Vector3f pos;
+  pos << pn, pe, 0.0;
+
+  Eigen::Vector3f center;
+  center << c_n, c_e, 0.0;
+
+  Eigen::Vector3f d = pos - center;
+
+  Eigen::Vector3f course;
+  course << sinf(chi), cosf(chi), 0.0;
+
+  if (d.cross(course)(2) >= 0 ) 
+  {
+    return -1;
+  }
+
+  return 1;
+}
+
+void path_manager_example::increment_indices(int & idx_a, int & idx_b, int & idx_c, const struct input_s & input, struct output_s & output)
+{
+
+  bool orbit_last = params.get_bool("orbit_last");
+  double R_min = params.get_double("R_min");
+
+  if (temp_waypoint_ && idx_a_ == 1)
+  {
+    waypoints_.erase(waypoints_.begin());
+    num_waypoints_--;
+    idx_a_ = 0;
+    idx_b = 1;
+    idx_c = 2;
+    temp_waypoint_ = false;
+    return;
+  }
+
+  if (idx_a == num_waypoints_ - 1) { // The logic for if it is the last waypoint.
+     
+    // If it is the last waypoint, and we orbit the last waypoint, construct the command.
+    if (orbit_last) {
+      output.flag = false;
+      output.va_d = waypoints_[idx_a_].va_d;
+      output.c[0] = waypoints_[idx_a_].w[0];
+      output.c[1] = waypoints_[idx_a_].w[1];
+      output.c[2] = waypoints_[idx_a_].w[2];
+      output.r[0] = 0.0;
+      output.r[1] = 0.0;
+      output.r[2] = 0.0;
+      output.q[0] = 0.0;
+      output.q[1] = 0.0;
+      output.q[2] = 0.0;
+      output.rho = R_min;
+      output.lamda = orbit_direction(input.pn, input.pe, input.chi, output.c[0], output.c[1]); // Calculate the most conveinent orbit direction of that point.
+      return;
+    }
+
+    idx_b = 0; // reset the path and loop the waypoints again.
+    idx_c = 1;
+  } else if (idx_a == num_waypoints_ - 2) { // If the second to last waypoint, appropriately handle the wrapping of waypoints.
+    idx_b = num_waypoints_ - 1;
+    idx_c = 0;
+  } else { // Increment the indices of the waypoints.
+    idx_b = idx_a + 1;
+    idx_c = idx_b + 1;
+  }
 }
 
 } // namespace rosplane