Add unit-tests for Route-manager commands

Fix some bugs in insert-waypt command discovered during the testing
(w00t, tests FTW)

src/Autopilot/route_mgr.cxx

@@ -121,29 +121,40 @@ static bool commandSetActiveWaypt(const SGPropertyNode* arg, SGPropertyNode *)
 
 static bool commandInsertWaypt(const SGPropertyNode* arg, SGPropertyNode *)
 {
-  FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
+    FGRouteMgr* self = globals->get_subsystem<FGRouteMgr>();
-  const bool haveIndex = arg->hasChild("index");
+    const bool haveIndex = arg->hasChild("index");
-  int index = arg->getIntValue("index");
+    int index = arg->getIntValue("index");
 
-  std::string ident(arg->getStringValue("id"));
+    std::string ident(arg->getStringValue("id"));
-  int alt = arg->getIntValue("altitude-ft", -999);
+    int alt = arg->getIntValue("altitude-ft", -999);
-  int ias = arg->getIntValue("speed-knots", -999);
+    int ias = arg->getIntValue("speed-knots", -999);
-  
+
-  WayptRef wp;
+    WayptRef wp;
-// lat/lon may be supplied to narrow down navaid search, or to specify
+    // lat/lon may be supplied to narrow down navaid search, or to specify
-// a raw waypoint
+    // a raw waypoint
-  SGGeod pos;
+    SGGeod pos = SGGeod::invalid();
-  if (arg->hasChild("longitude-deg")) {
+    if (arg->hasChild("longitude-deg")) {
-    pos = SGGeod::fromDeg(arg->getDoubleValue("longitude-deg"),
+        pos = SGGeod::fromDeg(arg->getDoubleValue("longitude-deg"),
-                               arg->getDoubleValue("latitude-deg"));
+                              arg->getDoubleValue("latitude-deg"));
   }
   
   if (arg->hasChild("navaid")) {
-    FGPositionedRef p = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid"), pos);
+      if (!pos.isValid()) {
-    
+          pos = self->flightPlan()->vicinityForInsertIndex(haveIndex ? index : -1 /* append */);
+      }
+
+      FGPositioned::TypeFilter filter({FGPositioned::Type::NDB, FGPositioned::Type::VOR,
+                                       FGPositioned::Type::FIX, FGPositioned::Type::WAYPOINT});
+
+      FGPositionedRef p = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid"), pos, &filter);
+      if (!p) {
+          SG_LOG(SG_AUTOPILOT, SG_WARN, "Unable to find navaid with ident:" << arg->getStringValue("navaid"));
+          return false;
+      }
+
     if (arg->hasChild("navaid", 1)) {
       // intersection of two radials
-      FGPositionedRef p2 = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid[1]"), pos);
+      FGPositionedRef p2 = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid[1]"), pos, &filter);
       if (!p2) {
         SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << arg->getStringValue("navaid[1]"));
         return false;
@@ -1200,24 +1211,7 @@ void FGRouteMgr::setSTAR(const std::string& aIdent)
 
 WayptRef FGRouteMgr::waypointFromString(const std::string& target, int insertPosition)
 {
-    SGGeod vicinity;
+    return _plan->waypointFromString(target, _plan->vicinityForInsertIndex(insertPosition));
-    if (insertPosition < 0) { // appending, not inserting
-        insertPosition = _plan->numLegs();
-        if (insertPosition > 0) {
-            // if we have at least one existing leg, use its position
-            // for the search vicinity
-            vicinity = _plan->pointAlongRoute(insertPosition - 1, 0.0);
-        }
-    } else {
-        // if we're somewhere in the middle of the route compute a search
-        // vicinity halfwya between the previous waypoint and the one we are
-        // inserting at, i.e th emiddle of the leg.
-        // if we're at the beginning, just use zero of course.
-        const double normOffset = (insertPosition > 0) ? -0.5 : 0.0;
-        vicinity = _plan->pointAlongRouteNorm(insertPosition, normOffset);
-    }
-
-    return _plan->waypointFromString(target, vicinity);
 }
 
 double FGRouteMgr::getDepartureFieldElevation() const

src/Navaids/FlightPlan.cxx

@@ -1351,6 +1351,27 @@ double FlightPlan::magvarDegAt(const SGGeod& pos) const
   return sgGetMagVar(pos, jd) * SG_RADIANS_TO_DEGREES;
 }
 
+SGGeod FlightPlan::vicinityForInsertIndex(int aIndex) const
+{
+    if (aIndex < 0) { // appending, not inserting
+        const int n = numLegs();
+        if (n > 0) {
+            // if we have at least one existing leg, use its position
+            // for the search vicinity
+            return pointAlongRoute(n - 1, 0.0);
+        }
+
+        return SGGeod::invalid();
+    }
+
+    // if we're somewhere in the middle of the route compute a search
+    // vicinity halfway between the previous waypoint and the one we are
+    // inserting at, i.e the middle of the leg.
+    // if we're at the beginning, just use zero of course.
+    const double normOffset = (aIndex > 0) ? -0.5 : 0.0;
+    return pointAlongRouteNorm(aIndex, normOffset);
+}
+
 WayptRef FlightPlan::waypointFromString(const string& tgt, const SGGeod& vicinity)
 {
     SGGeod basePosition = vicinity;

src/Navaids/FlightPlan.hxx

@@ -336,6 +336,16 @@ public:
      */
   SGGeod pointAlongRouteNorm(int aIndex, double aOffsetNorm) const;
 
+  /**
+    @brief given an index to insert a waypoint into the plan, find the geographical vicinity.
+        This is used to aid disambiguration searches, etc: see the vicinity paramter to 'waypointFromString'
+        below, for example
+     
+     When aIndex is negative, the vicinity used is the end of the current flight-plan, i.e appending to
+     the waypoints rather than appending.
+     */
+  SGGeod vicinityForInsertIndex(int aIndex) const;
+
   /**
    * Create a WayPoint from a string in the following format:
    *  -

test_suite/unit_tests/Navaids/test_routeManager.cxx

@@ -3,8 +3,9 @@
 #include <memory>
 #include <cstring>
 
-#include <simgear/misc/sg_dir.hxx>
 #include <simgear/io/iostreams/sgstream.hxx>
+#include <simgear/misc/sg_dir.hxx>
+#include <simgear/structure/commands.hxx>
 
 #include "test_suite/FGTestApi/testGlobals.hxx"
 #include "test_suite/FGTestApi/NavDataCache.hxx"
@@ -615,6 +616,59 @@ void RouteManagerTests::loadGPX()
     CPPUNIT_ASSERT_EQUAL(std::string{"KBOS"}, wp2->waypoint()->ident());
 }
 
+const std::string flightPlanXMLData =
+    R"(<?xml version="1.0" encoding="UTF-8"?>
+    <PropertyList>
+      <version type="int">2</version>
+      <departure>
+        <airport type="string">EDDM</airport>
+        <runway type="string">08R</runway>
+      </departure>
+      <destination>
+        <airport type="string">EDDF</airport>
+      </destination>
+      <route>
+        <wp>
+          <type type="string">runway</type>
+          <departure type="bool">true</departure>
+          <ident type="string">08R</ident>
+          <icao type="string">EDDM</icao>
+        </wp>
+        <wp n="1">
+          <type type="string">navaid</type>
+          <ident type="string">GIVMI</ident>
+          <lon type="double">11.364700</lon>
+          <lat type="double">48.701100</lat>
+        </wp>
+        <wp n="2">
+          <type type="string">navaid</type>
+          <ident type="string">ERNAS</ident>
+          <lon type="double">11.219400</lon>
+          <lat type="double">48.844700</lat>
+        </wp>
+        <wp n="3">
+          <type type="string">navaid</type>
+          <ident type="string">TALAL</ident>
+          <lon type="double">11.085300</lon>
+          <lat type="double">49.108300</lat>
+        </wp>
+        <wp n="4">
+          <type type="string">navaid</type>
+          <ident type="string">ERMEL</ident>
+          <lon type="double">11.044700</lon>
+          <lat type="double">49.187800</lat>
+        </wp>
+        <wp n="5">
+          <type type="string">navaid</type>
+          <ident type="string">PSA</ident>
+          <lon type="double">9.348300</lon>
+          <lat type="double">49.862200</lat>
+        </wp>
+      </route>
+    </PropertyList>
+)";
+
+
 // The same test as above, but for a file exported from the route manager or online
 void RouteManagerTests::loadFGFP()
 {
@@ -626,56 +680,7 @@ void RouteManagerTests::loadFGFP()
     SGPath fgfpPath = simgear::Dir::current().path() / "test_fgfp.fgfp";
     {
         sg_ofstream s(fgfpPath);
-        s << R"(<?xml version="1.0" encoding="UTF-8"?>
+        s << flightPlanXMLData;
-            <PropertyList>
-              <version type="int">2</version>
-              <departure>
-                <airport type="string">EDDM</airport>
-                <runway type="string">08R</runway>
-              </departure>
-              <destination>
-                <airport type="string">EDDF</airport>
-              </destination>
-              <route>
-                <wp>
-                  <type type="string">runway</type>
-                  <departure type="bool">true</departure>
-                  <ident type="string">08R</ident>
-                  <icao type="string">EDDM</icao>
-                </wp>
-                <wp n="1">
-                  <type type="string">navaid</type>
-                  <ident type="string">GIVMI</ident>
-                  <lon type="double">11.364700</lon>
-                  <lat type="double">48.701100</lat>
-                </wp>
-                <wp n="2">
-                  <type type="string">navaid</type>
-                  <ident type="string">ERNAS</ident>
-                  <lon type="double">11.219400</lon>
-                  <lat type="double">48.844700</lat>
-                </wp>
-                <wp n="3">
-                  <type type="string">navaid</type>
-                  <ident type="string">TALAL</ident>
-                  <lon type="double">11.085300</lon>
-                  <lat type="double">49.108300</lat>
-                </wp>
-                <wp n="4">
-                  <type type="string">navaid</type>
-                  <ident type="string">ERMEL</ident>
-                  <lon type="double">11.044700</lon>
-                  <lat type="double">49.187800</lat>
-                </wp>
-                <wp n="5">
-                  <type type="string">navaid</type>
-                  <ident type="string">PSA</ident>
-                  <lon type="double">9.348300</lon>
-                  <lat type="double">49.862200</lat>
-                </wp>
-              </route>
-            </PropertyList>
-        )";
     }
     
     CPPUNIT_ASSERT(f->load(fgfpPath));
@@ -861,3 +866,57 @@ void RouteManagerTests::testsSelectNavaid()
     CPPUNIT_ASSERT_EQUAL(wp2->ident(), string{"OD"});
     CPPUNIT_ASSERT_EQUAL(wp2->source()->name(), string{"BRYANSK NDB"});
 }
+
+void RouteManagerTests::testCommandAPI()
+{
+    auto rm = globals->get_subsystem<FGRouteMgr>();
+    SGPath fgfpPath = simgear::Dir::current().path() / "test_fgfp_2.fgfp";
+    {
+        sg_ofstream s(fgfpPath);
+        s << flightPlanXMLData;
+    }
+
+    {
+        SGPropertyNode_ptr args(new SGPropertyNode);
+        args->setStringValue("path", fgfpPath.utf8Str());
+        CPPUNIT_ASSERT(globals->get_commands()->execute("load-flightplan", args));
+    }
+
+    auto f = rm->flightPlan();
+    CPPUNIT_ASSERT_EQUAL(7, f->numLegs());
+
+    CPPUNIT_ASSERT(!f->isActive());
+
+    {
+        SGPropertyNode_ptr args(new SGPropertyNode);
+        CPPUNIT_ASSERT(globals->get_commands()->execute("activate-flightplan", args));
+    }
+
+    CPPUNIT_ASSERT(f->isActive());
+
+    {
+        SGPropertyNode_ptr args(new SGPropertyNode);
+        args->setIntValue("index", 3);
+        CPPUNIT_ASSERT(globals->get_commands()->execute("set-active-waypt", args));
+    }
+
+    CPPUNIT_ASSERT_EQUAL(3, f->currentIndex());
+
+    {
+        SGPropertyNode_ptr args(new SGPropertyNode);
+        args->setIntValue("index", 4);
+        args->setStringValue("navaid", "WLD");
+
+        // let's build an offset waypoint for fun
+        args->setDoubleValue("offset-nm", 10.0);
+        args->setDoubleValue("radial", 30);
+        CPPUNIT_ASSERT(globals->get_commands()->execute("insert-waypt", args));
+    }
+
+    auto waldaWpt = f->legAtIndex(4)->waypoint();
+    auto waldaVOR = waldaWpt->source();
+    CPPUNIT_ASSERT_EQUAL(string{"WALDA VOR-DME"}, waldaVOR->name());
+
+    auto d = SGGeodesy::distanceNm(waldaVOR->geod(), waldaWpt->position());
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, d, 0.1);
+}

test_suite/unit_tests/Navaids/test_routeManager.hxx

@@ -45,6 +45,7 @@ class RouteManagerTests : public CppUnit::TestFixture
     CPPUNIT_TEST(testRouteWithProcedures);
     CPPUNIT_TEST(testRouteWithApproachProcedures);
     CPPUNIT_TEST(testsSelectNavaid);
+    CPPUNIT_TEST(testCommandAPI);
 
     CPPUNIT_TEST_SUITE_END();
 
@@ -72,6 +73,7 @@ public:
     void testRouteWithProcedures();
     void testRouteWithApproachProcedures();
     void testsSelectNavaid();
+    void testCommandAPI();
 
 private:
     GPS* m_gps = nullptr;