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,7 +121,7 @@ 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");
     int index = arg->getIntValue("index");
 
@@ -132,18 +132,29 @@ static bool commandInsertWaypt(const SGPropertyNode* arg, SGPropertyNode *)
     WayptRef wp;
     // lat/lon may be supplied to narrow down navaid search, or to specify
     // a raw waypoint
-  SGGeod pos;
+    SGGeod pos = SGGeod::invalid();
     if (arg->hasChild("longitude-deg")) {
         pos = SGGeod::fromDeg(arg->getDoubleValue("longitude-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;
-    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);
+    return _plan->waypointFromString(target, _plan->vicinityForInsertIndex(insertPosition));
 }
 
 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,18 +616,8 @@ void RouteManagerTests::loadGPX()
     CPPUNIT_ASSERT_EQUAL(std::string{"KBOS"}, wp2->waypoint()->ident());
 }
 
-// The same test as above, but for a file exported from the route manager or online
-void RouteManagerTests::loadFGFP()
-{
-    auto rm = globals->get_subsystem<FGRouteMgr>();
-    
-    FlightPlanRef f = new FlightPlan;
-    rm->setFlightPlan(f);
-    
-    SGPath fgfpPath = simgear::Dir::current().path() / "test_fgfp.fgfp";
-    {
-        sg_ofstream s(fgfpPath);
-        s << R"(<?xml version="1.0" encoding="UTF-8"?>
+const std::string flightPlanXMLData =
+    R"(<?xml version="1.0" encoding="UTF-8"?>
     <PropertyList>
       <version type="int">2</version>
       <departure>
@@ -676,6 +667,20 @@ void RouteManagerTests::loadFGFP()
       </route>
     </PropertyList>
 )";
+
+
+// The same test as above, but for a file exported from the route manager or online
+void RouteManagerTests::loadFGFP()
+{
+    auto rm = globals->get_subsystem<FGRouteMgr>();
+    
+    FlightPlanRef f = new FlightPlan;
+    rm->setFlightPlan(f);
+    
+    SGPath fgfpPath = simgear::Dir::current().path() / "test_fgfp.fgfp";
+    {
+        sg_ofstream s(fgfpPath);
+        s << flightPlanXMLData;
     }
     
     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;