From 12076bce0e9fb66286f14931e8f4eb12fc0b9b82 Mon Sep 17 00:00:00 2001
From: James Turner <jmt@Bishop.local>
Date: Wed, 26 Dec 2012 23:48:19 +0000
Subject: [PATCH] Expose position along the flight-path to Nasal.

Useful to query a point '100nm before wpt X' from Nasal, especially for VNAV calculations (T/C, T/D).
---
 src/Navaids/FlightPlan.cxx        | 33 +++++++++++++++++++++++++++++++
 src/Navaids/FlightPlan.hxx        |  7 +++++++
 src/Scripting/NasalPositioned.cxx | 28 +++++++++++++++++++++++++-
 3 files changed, 67 insertions(+), 1 deletion(-)

diff --git a/src/Navaids/FlightPlan.cxx b/src/Navaids/FlightPlan.cxx
index cb7e41c0d..b66108fbb 100644
--- a/src/Navaids/FlightPlan.cxx
+++ b/src/Navaids/FlightPlan.cxx
@@ -1035,6 +1035,39 @@ void FlightPlan::rebuildLegData()
   }
 }
   
+SGGeod FlightPlan::pointAlongRoute(int aIndex, double aOffsetNm) const
+{
+  if (aIndex >= (int) _legs.size()) {
+    throw sg_range_exception();
+  }
+  
+  const int lastLeg = static_cast<int>(_legs.size()) - 1;
+// convert the relative offset and leg index into an absolute, positive
+// distance in nm from the route origin. This means we can simply walk
+// forwards to find the actual leg.
+  Leg* leg = _legs[(aIndex >= 0) ? aIndex : lastLeg];
+  double absolutePathDistance = leg->_distanceAlongPath + aOffsetNm;
+  if (absolutePathDistance < 0.0) {
+    return _legs[0]->waypoint()->position(); // begining of route
+  }
+  
+  if (absolutePathDistance > _totalDistance) {
+    return _legs[lastLeg]->waypoint()->position(); // end of route
+  }
+  
+// find the leg containing the absolute distance
+  for (int l=0; l<lastLeg; ++l) {
+    leg = _legs[l];
+    if (absolutePathDistance < leg->_pathDistance) {
+      break; // found our matching leg
+    }
+    absolutePathDistance -= leg->_pathDistance;
+  } // of forwards walk along route to find leg
+  
+  return SGGeodesy::direct(leg->waypoint()->position(),
+                               leg->_courseDeg, absolutePathDistance * SG_NM_TO_METER);
+}
+    
 void FlightPlan::lockDelegate()
 {
   if (_delegateLock == 0) {
diff --git a/src/Navaids/FlightPlan.hxx b/src/Navaids/FlightPlan.hxx
index bdd022830..3eeb02cf6 100644
--- a/src/Navaids/FlightPlan.hxx
+++ b/src/Navaids/FlightPlan.hxx
@@ -199,6 +199,13 @@ public:
   double totalDistanceNm() const
   { return _totalDistance; }
   
+  /**
+   * given a waypoint index, and an offset in NM, find the geodetic
+   * position on the route path. I.e the point 10nm before or after
+   * a particular waypoint.
+   */
+  SGGeod pointAlongRoute(int aIndex, double aOffsetNm) const;
+    
   /**
    * Create a WayPoint from a string in the following format:
    *  - simple identifier
diff --git a/src/Scripting/NasalPositioned.cxx b/src/Scripting/NasalPositioned.cxx
index 9deebee89..66bfb5826 100644
--- a/src/Scripting/NasalPositioned.cxx
+++ b/src/Scripting/NasalPositioned.cxx
@@ -2125,6 +2125,31 @@ static naRef f_flightplan_clone(naContext c, naRef me, int argc, naRef* args)
   return ghostForFlightPlan(c, fp->clone());
 }
 
+static naRef f_flightplan_pathGeod(naContext c, naRef me, int argc, naRef* args)
+{
+  FlightPlan* fp = flightplanGhost(me);
+  if (!fp) {
+    naRuntimeError(c, "flightplan.clone called on non-flightplan object");
+  }
+
+  if ((argc < 1) || !naIsNum(args[0])) {
+    naRuntimeError(c, "bad argument to flightplan.pathGeod");
+  }
+
+  if ((argc > 1) && !naIsNum(args[1])) {
+    naRuntimeError(c, "bad argument to flightplan.pathGeod");
+  }
+
+  int index = (int) args[0].num;
+  double offset = (argc > 1) ? args[1].num : 0.0;
+  naRef result = naNewHash(c);
+  SGGeod g = fp->pointAlongRoute(index, offset);
+  hashset(c, result, "lat", naNum(g.getLatitudeDeg()));
+  hashset(c, result, "lon", naNum(g.getLongitudeDeg()));
+  return result;
+}
+
+
 static naRef f_leg_setSpeed(naContext c, naRef me, int argc, naRef* args)
 {
   FlightPlan::Leg* leg = fpLegGhost(me);
@@ -2365,7 +2390,8 @@ naRef initNasalPositioned(naRef globals, naContext c, naRef gcSave)
     hashset(c, flightplanPrototype, "cleanPlan", naNewFunc(c, naNewCCode(c, f_flightplan_clearPlan))); 
     hashset(c, flightplanPrototype, "clearWPType", naNewFunc(c, naNewCCode(c, f_flightplan_clearWPType))); 
     hashset(c, flightplanPrototype, "clone", naNewFunc(c, naNewCCode(c, f_flightplan_clone))); 
-  
+    hashset(c, flightplanPrototype, "pathGeod", naNewFunc(c, naNewCCode(c, f_flightplan_pathGeod)));
+    
     waypointPrototype = naNewHash(c);
     hashset(c, gcSave, "wayptProto", waypointPrototype);
     
-- 
2.39.5