diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx
index 523fd5e78..4bbd87c28 100644
--- a/src/AIModel/AIFlightPlan.cxx
+++ b/src/AIModel/AIFlightPlan.cxx
@@ -6,11 +6,11 @@
  */
 
 #ifdef HAVE_CONFIG_H
-#  include <config.h>
+#include <config.h>
 #endif
 
-#include <iterator>
 #include <algorithm>
+#include <iterator>
 
 #include <simgear/constants.h>
 #include <simgear/debug/logstream.hxx>
@@ -22,37 +22,39 @@
 #include <simgear/structure/exception.hxx>
 #include <simgear/timing/sg_time.hxx>
 
-#include <Main/globals.hxx>
-#include <Main/fg_props.hxx>
-#include <Main/fg_init.hxx>
 #include <Airports/airport.hxx>
 #include <Airports/dynamics.hxx>
-#include <Airports/runways.hxx>
 #include <Airports/groundnetwork.hxx>
+#include <Airports/runways.hxx>
+#include <Main/fg_init.hxx>
+#include <Main/fg_props.hxx>
+#include <Main/globals.hxx>
 
-#include <Environment/environment_mgr.hxx>
 #include <Environment/environment.hxx>
+#include <Environment/environment_mgr.hxx>
 
 #include <Traffic/Schedule.hxx>
 
-#include "AIFlightPlan.hxx"
 #include "AIAircraft.hxx"
+#include "AIFlightPlan.hxx"
 
 using std::string;
 
-FGAIWaypoint::FGAIWaypoint() {
-  speed       = 0;
-  crossat     = 0;
-  finished    = 0;
-  gear_down   = 0;
-  flaps       = 0;
-  on_ground   = 0;
-  routeIndex  = 0;
-  time_sec    = 0;
-  trackLength = 0;
+FGAIWaypoint::FGAIWaypoint()
+{
+    speed = 0;
+    crossat = 0;
+    finished = 0;
+    gear_down = 0;
+    flaps = 0;
+    on_ground = 0;
+    routeIndex = 0;
+    time_sec = 0;
+    trackLength = 0;
 }
 
-bool FGAIWaypoint::contains(const string& target) {
+bool FGAIWaypoint::contains(const string& target)
+{
     size_t found = name.find(target);
     if (found == string::npos)
         return false;
@@ -60,62 +62,60 @@ bool FGAIWaypoint::contains(const string& target) {
         return true;
 }
 
-double FGAIWaypoint::getLatitude()
+double FGAIWaypoint::getLatitude() const
 {
-  return pos.getLatitudeDeg();
+    return pos.getLatitudeDeg();
 }
 
-double FGAIWaypoint::getLongitude()
+double FGAIWaypoint::getLongitude() const
 {
-  return pos.getLongitudeDeg();
+    return pos.getLongitudeDeg();
 }
 
-double FGAIWaypoint::getAltitude()
+double FGAIWaypoint::getAltitude() const
 {
-  return pos.getElevationFt();
+    return pos.getElevationFt();
 }
 
 void FGAIWaypoint::setLatitude(double lat)
 {
-  pos.setLatitudeDeg(lat);
+    pos.setLatitudeDeg(lat);
 }
 
 void FGAIWaypoint::setLongitude(double lon)
 {
-  pos.setLongitudeDeg(lon);
+    pos.setLongitudeDeg(lon);
 }
 
 void FGAIWaypoint::setAltitude(double alt)
 {
-  pos.setElevationFt(alt);
+    pos.setElevationFt(alt);
 }
 
-FGAIFlightPlan::FGAIFlightPlan() :
-    sid(NULL),
-    repeat(false),
-    distance_to_go(0),
-    lead_distance_ft(0),
-    leadInAngle(0),
-    start_time(0),
-    arrivalTime(0),
-    leg(0),
-    lastNodeVisited(0),
-    isValid(true)
+FGAIFlightPlan::FGAIFlightPlan() : sid(NULL),
+                                   repeat(false),
+                                   distance_to_go(0),
+                                   lead_distance_ft(0),
+                                   leadInAngle(0),
+                                   start_time(0),
+                                   arrivalTime(0),
+                                   leg(0),
+                                   lastNodeVisited(0),
+                                   isValid(true)
 {
-    wpt_iterator    = waypoints.begin();
+    wpt_iterator = waypoints.begin();
 }
 
-FGAIFlightPlan::FGAIFlightPlan(const string& filename) :
-    sid(NULL),
-    repeat(false),
-    distance_to_go(0),
-    lead_distance_ft(0),
-    leadInAngle(0),
-    start_time(0),
-    arrivalTime(0),
-    leg(10),
-    lastNodeVisited(0),
-    isValid(parseProperties(filename))
+FGAIFlightPlan::FGAIFlightPlan(const string& filename) : sid(NULL),
+                                                         repeat(false),
+                                                         distance_to_go(0),
+                                                         lead_distance_ft(0),
+                                                         leadInAngle(0),
+                                                         start_time(0),
+                                                         arrivalTime(0),
+                                                         leg(10),
+                                                         lastNodeVisited(0),
+                                                         isValid(parseProperties(filename))
 {
 }
 
@@ -128,13 +128,13 @@ FGAIFlightPlan::FGAIFlightPlan(const string& filename) :
  * as setting speeds and altitude computed by the
  * traffic manager.
  */
-FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
+FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft* ac,
                                const std::string& p,
                                double course,
                                time_t start,
                                time_t remainingTime,
-                               FGAirport *dep,
-                               FGAirport *arr,
+                               FGAirport* dep,
+                               FGAirport* arr,
                                bool firstLeg,
                                double radius,
                                double alt,
@@ -143,40 +143,39 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
                                double speed,
                                const string& fltType,
                                const string& acType,
-                               const string& airline) :
-    sid(NULL),
-    repeat(false),
-    distance_to_go(0),
-    lead_distance_ft(0),
-    leadInAngle(0),
-    start_time(start),
-    arrivalTime(0),
-    leg(10),
-    lastNodeVisited(0),
-    isValid(false),
-    departure(dep),
-    arrival(arr)
+                               const string& airline) : sid(NULL),
+                                                        repeat(false),
+                                                        distance_to_go(0),
+                                                        lead_distance_ft(0),
+                                                        leadInAngle(0),
+                                                        start_time(start),
+                                                        arrivalTime(0),
+                                                        leg(10),
+                                                        lastNodeVisited(0),
+                                                        isValid(false),
+                                                        departure(dep),
+                                                        arrival(arr)
 {
-  if (parseProperties(p)) {
-    isValid = true;
-  } else {
-    createWaypoints(ac, course, start, remainingTime, dep, arr, firstLeg, radius,
-                    alt, lat, lon, speed, fltType, acType, airline);
-  }
+    if (parseProperties(p)) {
+        isValid = true;
+    } else {
+        createWaypoints(ac, course, start, remainingTime, dep, arr, firstLeg, radius,
+                        alt, lat, lon, speed, fltType, acType, airline);
+    }
 }
 
 FGAIFlightPlan::~FGAIFlightPlan()
 {
-  deleteWaypoints();
-  //delete taxiRoute;
+    deleteWaypoints();
+    //delete taxiRoute;
 }
 
-void FGAIFlightPlan::createWaypoints(FGAIAircraft *ac,
+void FGAIFlightPlan::createWaypoints(FGAIAircraft* ac,
                                      double course,
                                      time_t start,
                                      time_t remainingTime,
-                                     FGAirport *dep,
-                                     FGAirport *arr,
+                                     FGAirport* dep,
+                                     FGAirport* arr,
                                      bool firstLeg,
                                      double radius,
                                      double alt,
@@ -187,30 +186,29 @@ void FGAIFlightPlan::createWaypoints(FGAIAircraft *ac,
                                      const string& acType,
                                      const string& airline)
 {
-  time_t now = globals->get_time_params()->get_cur_time();
-  time_t timeDiff = now-start;
-  leg = AILeg::STARTUP_PUSHBACK;
+    time_t now = globals->get_time_params()->get_cur_time();
+    time_t timeDiff = now - start;
+    leg = AILeg::STARTUP_PUSHBACK;
 
-  if ((timeDiff > 60) && (timeDiff < 1500))
-    leg = AILeg::TAXI;
-  else if ((timeDiff >= 1500) && (timeDiff < 2000))
-    leg = AILeg::TAKEOFF;
-  else if (timeDiff >= 2000) {
-    if (remainingTime > 2000) {
-      leg = AILeg::CRUISE;
-    } else {
-      leg = AILeg::APPROACH;
+    if ((timeDiff > 60) && (timeDiff < 1500))
+        leg = AILeg::TAXI;
+    else if ((timeDiff >= 1500) && (timeDiff < 2000))
+        leg = AILeg::TAKEOFF;
+    else if (timeDiff >= 2000) {
+        if (remainingTime > 2000) {
+            leg = AILeg::CRUISE;
+        } else {
+            leg = AILeg::APPROACH;
+        }
     }
-  }
 
-  SG_LOG(SG_AI, SG_DEBUG, ac->getTrafficRef()->getCallSign() << "|Route from " << dep->getId() << " to " << arr->getId() <<
-           ". Set leg to : " << leg << " " << remainingTime);
+    SG_LOG(SG_AI, SG_DEBUG, ac->getTrafficRef()->getCallSign() << "|Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg << " " << remainingTime);
 
-  wpt_iterator = waypoints.begin();
-  bool dist = 0;
-  isValid = create(ac, dep, arr, leg, alt, speed, lat, lon,
-                   firstLeg, radius, fltType, acType, airline, dist);
-  wpt_iterator = waypoints.begin();
+    wpt_iterator = waypoints.begin();
+    bool dist = 0;
+    isValid = create(ac, dep, arr, leg, alt, speed, lat, lon,
+                     firstLeg, radius, fltType, acType, airline, dist);
+    wpt_iterator = waypoints.begin();
 }
 
 bool FGAIFlightPlan::parseProperties(const std::string& filename)
@@ -307,7 +305,7 @@ bool FGAIFlightPlan::readFlightplan(std::istream& stream, const sg_location& loc
     return true;
 }
 
-FGAIWaypoint* FGAIFlightPlan::getLastWaypoint()
+FGAIWaypoint* FGAIFlightPlan::getLastWaypoint() const
 {
     if (waypoints.empty())
         return nullptr;
@@ -315,7 +313,7 @@ FGAIWaypoint* FGAIFlightPlan::getLastWaypoint()
     return waypoints.back();
 };
 
-FGAIWaypoint* FGAIFlightPlan::getPreviousWaypoint( void ) const
+FGAIWaypoint* FGAIFlightPlan::getPreviousWaypoint() const
 {
     if (empty())
         return nullptr;
@@ -328,14 +326,14 @@ FGAIWaypoint* FGAIFlightPlan::getPreviousWaypoint( void ) const
     }
 }
 
-FGAIWaypoint* FGAIFlightPlan::getCurrentWaypoint( void ) const
+FGAIWaypoint* FGAIFlightPlan::getCurrentWaypoint() const
 {
-  if (wpt_iterator == waypoints.end())
-      return nullptr;
-  return *wpt_iterator;
+    if (wpt_iterator == waypoints.end())
+        return nullptr;
+    return *wpt_iterator;
 }
 
-FGAIWaypoint* FGAIFlightPlan::getNextWaypoint( void ) const
+FGAIWaypoint* FGAIFlightPlan::getNextWaypoint() const
 {
     if (wpt_iterator == waypoints.end())
         return nullptr;
@@ -348,43 +346,39 @@ FGAIWaypoint* FGAIFlightPlan::getNextWaypoint( void ) const
     }
 }
 
-int FGAIFlightPlan::getNextTurnAngle( void ) const
+int FGAIFlightPlan::getNextTurnAngle() const
 {
     return nextTurnAngle;
 }
 
 
-void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints )
+void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints)
 {
     if (empty())
         return;
 
-    if (eraseWaypoints)
-    {
+    if (eraseWaypoints) {
         if (wpt_iterator == waypoints.begin())
             ++wpt_iterator;
-        else
-        if (!waypoints.empty())
-        {
+        else if (!waypoints.empty()) {
             delete *(waypoints.begin());
             waypoints.erase(waypoints.begin());
             wpt_iterator = waypoints.begin();
             ++wpt_iterator;
         }
-    }
-    else {
-      ++wpt_iterator;
+    } else {
+        ++wpt_iterator;
     }
     // Calculate the angle of the next turn.
     if (wpt_iterator == waypoints.end())
         return;
     if (wpt_iterator == waypoints.begin())
         return;
-    if (wpt_iterator+1 == waypoints.end())
+    if (wpt_iterator + 1 == waypoints.end())
         return;
-    if (waypoints.size()<3)
+    if (waypoints.size() < 3)
         return;
-    FGAIWaypoint* previousWP = *(wpt_iterator -1);
+    FGAIWaypoint* previousWP = *(wpt_iterator - 1);
     FGAIWaypoint* currentWP = *(wpt_iterator);
     FGAIWaypoint* nextWP = *(wpt_iterator + 1);
     int currentBearing = this->getBearing(previousWP, currentWP);
@@ -393,8 +387,8 @@ void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints )
     nextTurnAngle = SGMiscd::normalizePeriodic(-180, 180, nextBearing - currentBearing);
     if ((previousWP->getSpeed() > 0 && nextWP->getSpeed() < 0) ||
         (previousWP->getSpeed() < 0 && nextWP->getSpeed() > 0)) {
-      nextTurnAngle += 180;
-      SG_LOG(SG_AI, SG_BULK, "Add 180 to turn angle pushback end");
+        nextTurnAngle += 180;
+        SG_LOG(SG_AI, SG_BULK, "Add 180 to turn angle pushback end");
     }
     SG_LOG(SG_AI, SG_BULK, "Calculated next turn angle " << nextTurnAngle << " " << previousWP->getName() << " " << currentWP->getName() << " Previous Speed " << previousWP->getSpeed() << " Next Speed " << nextWP->getSpeed());
 }
@@ -413,14 +407,15 @@ void FGAIFlightPlan::eraseLastWaypoint()
         return;
 
     delete (waypoints.back());
-    waypoints.pop_back();;
+    waypoints.pop_back();
     wpt_iterator = waypoints.begin();
     ++wpt_iterator;
 }
 
 // gives distance in meters from a position to a waypoint
-double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const{
-  return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat), wp->getPos());
+double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const
+{
+    return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat), wp->getPos());
 }
 
 /**
@@ -430,55 +425,56 @@ double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp)
 void FGAIFlightPlan::setLeadDistance(double speed,
                                      double bearing,
                                      FGAIWaypoint* current,
-                                     FGAIWaypoint* next){
-  double turn_radius_m;
-  // Handle Ground steering
-  // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn
-  // So, to get an estimate of the turn radius, calculate the circumference of the circle
-  // we travel on. Get the turn radius by dividing by PI (*2).
-  // FIXME Why when going backwards? No fabs
-  if (speed < 0.5) {
-    setLeadDistance(0.5);
-    return;
-  }
-  if (speed > 0 && speed < 0.5) {
-    setLeadDistance(5 * SG_METER_TO_FEET);
-    SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance fixed " << (lead_distance_ft*SG_FEET_TO_METER));
-    return;
-  }
-
-  double speed_mps = speed * SG_KT_TO_MPS;
-  if (speed < 25) {
-    turn_radius_m = ((360/30)*fabs(speed_mps)) / (2*M_PI);
-  } else {
-    turn_radius_m = 0.1911 * speed * speed; // an estimate for 25 degrees bank
-  }
-
-  double inbound = bearing;
-  double outbound = getBearing(current, next);
-  leadInAngle = fabs(inbound - outbound);
-  if (leadInAngle > 180.0) leadInAngle = 360.0 - leadInAngle;
-  //if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped
-  //  leadInAngle = 30.0;
-
-  //lead_distance_ft = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS);
-
-  if ((int)leadInAngle==0) {
-    double lead_distance_m = fabs(2*speed) * SG_FEET_TO_METER;
-    setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
-    if (lead_distance_ft > 1000) {
-        SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance leadin 0 " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound);
+                                     FGAIWaypoint* next)
+{
+    double turn_radius_m;
+    // Handle Ground steering
+    // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn
+    // So, to get an estimate of the turn radius, calculate the circumference of the circle
+    // we travel on. Get the turn radius by dividing by PI (*2).
+    // FIXME Why when going backwards? No fabs
+    if (speed < 0.5) {
+        setLeadDistance(0.5);
+        return;
     }
-  } else {
-    double lead_distance_m = turn_radius_m * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2);
-    setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
-    SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance " << (lead_distance_ft*SG_FEET_TO_METER) << " Turnradius " << turn_radius_m << " Speed " << speed_mps << " Half turn Angle " << (leadInAngle)/2);
-    if (lead_distance_ft > 1000) {
-        SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance possible direction change " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound << " at " << current->getName());
+    if (speed > 0 && speed < 0.5) {
+        setLeadDistance(5 * SG_METER_TO_FEET);
+        SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance fixed " << (lead_distance_ft * SG_FEET_TO_METER));
+        return;
     }
-  }
 
-  /*
+    double speed_mps = speed * SG_KT_TO_MPS;
+    if (speed < 25) {
+        turn_radius_m = ((360 / 30) * fabs(speed_mps)) / (2 * M_PI);
+    } else {
+        turn_radius_m = 0.1911 * speed * speed; // an estimate for 25 degrees bank
+    }
+
+    double inbound = bearing;
+    double outbound = getBearing(current, next);
+    leadInAngle = fabs(inbound - outbound);
+    if (leadInAngle > 180.0) leadInAngle = 360.0 - leadInAngle;
+    //if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped
+    //  leadInAngle = 30.0;
+
+    //lead_distance_ft = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS);
+
+    if ((int)leadInAngle == 0) {
+        double lead_distance_m = fabs(2 * speed) * SG_FEET_TO_METER;
+        setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
+        if (lead_distance_ft > 1000) {
+            SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance leadin 0 " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound);
+        }
+    } else {
+        double lead_distance_m = turn_radius_m * tan((leadInAngle * SG_DEGREES_TO_RADIANS) / 2);
+        setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
+        SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance " << (lead_distance_ft * SG_FEET_TO_METER) << " Turnradius " << turn_radius_m << " Speed " << speed_mps << " Half turn Angle " << (leadInAngle) / 2);
+        if (lead_distance_ft > 1000) {
+            SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance possible direction change " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound << " at " << current->getName());
+        }
+    }
+
+    /*
   if ((lead_distance_ft > (3*turn_radius)) && (current->on_ground == false)) {
       SG_LOG(SG_AI, SG_ALERT, "Warning: Lead-in distance is large. Inbound = " << inbound
             << ". Outbound = " << outbound << ". Lead in angle = " << leadInAngle  << ". Turn radius = " << turn_radius);
@@ -491,60 +487,61 @@ void FGAIFlightPlan::setLeadDistance(double speed,
   }*/
 }
 
-void FGAIFlightPlan::setLeadDistance(double distance_ft){
-  lead_distance_ft = distance_ft;
-  if (lead_distance_ft>10000) {
-    SG_LOG(SG_AI, SG_BULK, "Excessive Leaddistance " << distance_ft);
-  }
+void FGAIFlightPlan::setLeadDistance(double distance_ft)
+{
+    lead_distance_ft = distance_ft;
+    if (lead_distance_ft > 10000) {
+        SG_LOG(SG_AI, SG_BULK, "Excessive Leaddistance " << distance_ft);
+    }
 }
 
 
 double FGAIFlightPlan::getBearing(FGAIWaypoint* first, FGAIWaypoint* second) const
 {
-  return SGGeodesy::courseDeg(first->getPos(), second->getPos());
+    return SGGeodesy::courseDeg(first->getPos(), second->getPos());
 }
 
 double FGAIFlightPlan::getBearing(const SGGeod& aPos, FGAIWaypoint* wp) const
 {
-  return SGGeodesy::courseDeg(aPos, wp->getPos());
+    return SGGeodesy::courseDeg(aPos, wp->getPos());
 }
 
 void FGAIFlightPlan::deleteWaypoints()
 {
-  for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end(); ++i)
-    delete (*i);
-  waypoints.clear();
-  wpt_iterator = waypoints.begin();
+    for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end(); ++i)
+        delete (*i);
+    waypoints.clear();
+    wpt_iterator = waypoints.begin();
 }
 
 // Delete all waypoints except the last,
 // which we will recycle as the first waypoint in the next leg;
 void FGAIFlightPlan::resetWaypoints()
 {
-  if (waypoints.begin() == waypoints.end())
-    return;
+    if (waypoints.begin() == waypoints.end())
+        return;
 
 
-  FGAIWaypoint* wpt = new FGAIWaypoint;
-  wpt_vector_iterator i = waypoints.end();
-  --i;
-  wpt->setName((*i)->getName());
-  wpt->setPos((*i)->getPos());
-  wpt->setCrossat((*i)->getCrossat());
-  wpt->setGear_down((*i)->getGear_down());
-  wpt->setFlaps((*i)->getFlaps());
-  wpt->setSpoilers((*i)->getSpoilers());
-  wpt->setSpeedBrakes((*i)->getSpeedBrakes());
-  wpt->setBeaconLight((*i)->getBeaconLight());
-  wpt->setLandingLight((*i)->getLandingLight());
-  wpt->setNavLight((*i)->getNavLight());
-  wpt->setStrobeLight((*i)->getStrobeLight());
-  wpt->setTaxiLight((*i)->getTaxiLight());
-  wpt->setFinished(false);
-  wpt->setOn_ground((*i)->getOn_ground());
-  SG_LOG(SG_AI, SG_DEBUG, "Recycling waypoint " << wpt->getName());
-  deleteWaypoints();
-  pushBackWaypoint(wpt);
+    FGAIWaypoint* wpt = new FGAIWaypoint;
+    wpt_vector_iterator i = waypoints.end();
+    --i;
+    wpt->setName((*i)->getName());
+    wpt->setPos((*i)->getPos());
+    wpt->setCrossat((*i)->getCrossat());
+    wpt->setGear_down((*i)->getGear_down());
+    wpt->setFlaps((*i)->getFlaps());
+    wpt->setSpoilers((*i)->getSpoilers());
+    wpt->setSpeedBrakes((*i)->getSpeedBrakes());
+    wpt->setBeaconLight((*i)->getBeaconLight());
+    wpt->setLandingLight((*i)->getLandingLight());
+    wpt->setNavLight((*i)->getNavLight());
+    wpt->setStrobeLight((*i)->getStrobeLight());
+    wpt->setTaxiLight((*i)->getTaxiLight());
+    wpt->setFinished(false);
+    wpt->setOn_ground((*i)->getOn_ground());
+    SG_LOG(SG_AI, SG_DEBUG, "Recycling waypoint " << wpt->getName());
+    deleteWaypoints();
+    pushBackWaypoint(wpt);
 }
 
 void FGAIFlightPlan::addWaypoint(FGAIWaypoint* wpt)
@@ -552,56 +549,57 @@ void FGAIFlightPlan::addWaypoint(FGAIWaypoint* wpt)
     pushBackWaypoint(wpt);
 }
 
-void FGAIFlightPlan::pushBackWaypoint(FGAIWaypoint *wpt)
+void FGAIFlightPlan::pushBackWaypoint(FGAIWaypoint* wpt)
 {
-  if (!wpt) {
-    SG_LOG(SG_AI, SG_WARN, "Null WPT added");
-  }
-  size_t pos = wpt_iterator - waypoints.begin();
-  if (waypoints.size()>0) {
-      double dist = SGGeodesy::distanceM( waypoints.back()->getPos(), wpt->getPos());
-      if( dist == 0 ) {
-        SG_LOG(SG_AI, SG_DEBUG, "Double WP : \t" << wpt->getName() << " not added ");
-      } else {
+    if (!wpt) {
+        SG_LOG(SG_AI, SG_WARN, "Null WPT added");
+    }
+    size_t pos = wpt_iterator - waypoints.begin();
+    if (waypoints.size() > 0) {
+        double dist = SGGeodesy::distanceM(waypoints.back()->getPos(), wpt->getPos());
+        if (dist == 0) {
+            SG_LOG(SG_AI, SG_DEBUG, "Double WP : \t" << wpt->getName() << " not added ");
+        } else {
+            waypoints.push_back(wpt);
+            SG_LOG(SG_AI, SG_BULK, "Added WP : \t" << std::setprecision(12) << wpt->getName() << "\t" << wpt->getPos() << "\t" << wpt->getSpeed());
+        }
+    } else {
         waypoints.push_back(wpt);
         SG_LOG(SG_AI, SG_BULK, "Added WP : \t" << std::setprecision(12) << wpt->getName() << "\t" << wpt->getPos() << "\t" << wpt->getSpeed());
-      }
-  } else {
-    waypoints.push_back(wpt);
-    SG_LOG(SG_AI, SG_BULK, "Added WP : \t" << std::setprecision(12) << wpt->getName() << "\t" << wpt->getPos() << "\t" << wpt->getSpeed());
-  }
-  // std::vector::push_back invalidates waypoints
-  //  so we should restore wpt_iterator after push_back
-  //  (or it could be an index in the vector)
-  wpt_iterator = waypoints.begin() + pos;
+    }
+    // std::vector::push_back invalidates waypoints
+    //  so we should restore wpt_iterator after push_back
+    //  (or it could be an index in the vector)
+    wpt_iterator = waypoints.begin() + pos;
 }
 
 // Start flightplan over from the beginning
 void FGAIFlightPlan::restart()
 {
-  wpt_iterator = waypoints.begin();
+    wpt_iterator = waypoints.begin();
 }
 
-int FGAIFlightPlan::getRouteIndex(int i) {
-  if ((i > 0) && (i < (int)waypoints.size())) {
-    return waypoints[i]->getRouteIndex();
-  }
-  else
-    return 0;
+int FGAIFlightPlan::getRouteIndex(int i) const
+{
+    if ((i > 0) && (i < (int)waypoints.size())) {
+        return waypoints[i]->getRouteIndex();
+    } else
+        return 0;
 }
 
-double FGAIFlightPlan::checkTrackLength(const string& wptName) const {
+double FGAIFlightPlan::checkTrackLength(const string& wptName) const
+{
     // skip the first two waypoints: first one is behind, second one is partially done;
     double trackDistance = 0;
     wpt_vector_iterator wptvec = waypoints.begin();
     ++wptvec;
     ++wptvec;
     while ((wptvec != waypoints.end())) {
-      if (*wptvec!=nullptr && (!((*wptvec)->contains(wptName)))) {
-        break;
-      }
-      trackDistance += (*wptvec)->getTrackLength();
-      ++wptvec;
+        if (*wptvec != nullptr && (!((*wptvec)->contains(wptName)))) {
+            break;
+        }
+        trackDistance += (*wptvec)->getTrackLength();
+        ++wptvec;
     }
     if (wptvec == waypoints.end()) {
         trackDistance = 0; // name not found
@@ -609,7 +607,7 @@ double FGAIFlightPlan::checkTrackLength(const string& wptName) const {
     return trackDistance;
 }
 
-void FGAIFlightPlan::shortenToFirst(unsigned int number, string name)
+void FGAIFlightPlan::shortenToFirst(unsigned int number, const std::string& name)
 {
     while (waypoints.size() > number + 3) {
         eraseLastWaypoint();
@@ -619,12 +617,12 @@ void FGAIFlightPlan::shortenToFirst(unsigned int number, string name)
 
 void FGAIFlightPlan::setGate(const ParkingAssignment& pka)
 {
-  gate = pka;
+    gate = pka;
 }
 
-FGParking* FGAIFlightPlan::getParkingGate()
+FGParking* FGAIFlightPlan::getParkingGate() const
 {
-  return gate.parking();
+    return gate.parking();
 }
 
 FGAirportRef FGAIFlightPlan::departureAirport() const
diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx
index d25a5a5cf..183787346 100644
--- a/src/AIModel/AIFlightPlan.hxx
+++ b/src/AIModel/AIFlightPlan.hxx
@@ -93,9 +93,9 @@ public:
 
     const std::string& getName() { return name; };
     const SGGeod& getPos() { return pos; };
-    double getLatitude();
-    double getLongitude();
-    double getAltitude();
+    double getLatitude() const;
+    double getLongitude() const;
+    double getAltitude() const;
     double getSpeed() { return speed; };
 
     double getCrossat() { return crossat; };
@@ -198,7 +198,7 @@ public:
     void restart(void);
     int getNrOfWayPoints() { return waypoints.size(); }
 
-    int getRouteIndex(int i); // returns the AI related index of this current routes.
+    int getRouteIndex(int i) const; // returns the AI related index of this current routes.
 
     const std::string& getRunway() { return activeRunway; }
     bool isActive(time_t time) { return time >= this->getStartTime(); }
@@ -220,12 +220,12 @@ public:
     void setSID(FGAIFlightPlan* fp) { sid = fp; };
     FGAIFlightPlan* getSID() { return sid; };
     FGAIWaypoint* getWayPoint(int i) { return waypoints[i]; };
-    FGAIWaypoint* getLastWaypoint();
+    FGAIWaypoint* getLastWaypoint() const;
 
-    void shortenToFirst(unsigned int number, std::string name);
+    void shortenToFirst(unsigned int number, const std::string& name);
 
     void setGate(const ParkingAssignment& pka);
-    FGParking* getParkingGate();
+    FGParking* getParkingGate() const;
 
     FGAirportRef departureAirport() const;
     FGAirportRef arrivalAirport() const;