src/Autopilot/route_mgr.cxx

   1 // route_mgr.cxx - manage a route (i.e. a collection of waypoints)
   2 //
   3 // Written by Curtis Olson, started January 2004.
   4 //            Norman Vine
   5 //            Melchior FRANZ
   6 //
   7 // Copyright (C) 2004  Curtis L. Olson  - http://www.flightgear.org/~curt
   8 //
   9 // This program is free software; you can redistribute it and/or
  10 // modify it under the terms of the GNU General Public License as
  11 // published by the Free Software Foundation; either version 2 of the
  12 // License, or (at your option) any later version.
  13 //
  14 // This program is distributed in the hope that it will be useful, but
  15 // WITHOUT ANY WARRANTY; without even the implied warranty of
  16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17 // General Public License for more details.
  18 //
  19 // You should have received a copy of the GNU General Public License
  20 // along with this program; if not, write to the Free Software
  21 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  22 //
  23 // $Id$
  24
  25
  26 #ifdef HAVE_CONFIG_H
  27 #  include <config.h>
  28 #endif
  29
  30 #ifdef HAVE_WINDOWS_H
  31 #include <time.h>
  32 #endif
  33
  34 #include <simgear/compiler.h>
  35
  36 #include "route_mgr.hxx"
  37
  38 #include <boost/algorithm/string/case_conv.hpp>
  39 #include <boost/tuple/tuple.hpp>
  40
  41 #include <simgear/misc/strutils.hxx>
  42 #include <simgear/structure/exception.hxx>
  43 #include <simgear/structure/commands.hxx>
  44 #include <simgear/misc/sgstream.hxx>
  45
  46 #include <simgear/props/props_io.hxx>
  47 #include <simgear/misc/sg_path.hxx>
  48 #include <simgear/route/route.hxx>
  49 #include <simgear/sg_inlines.h>
  50
  51 #include "Main/fg_props.hxx"
  52 #include "Navaids/positioned.hxx"
  53 #include <Navaids/waypoint.hxx>
  54 #include <Navaids/airways.hxx>
  55 #include <Navaids/procedure.hxx>
  56 #include "Airports/simple.hxx"
  57 #include "Airports/runways.hxx"
  58
  59 #define RM "/autopilot/route-manager/"
  60
  61 #include <GUI/new_gui.hxx>
  62 #include <GUI/dialog.hxx>
  63
  64 using namespace flightgear;
  65
  66 class PropertyWatcher : public SGPropertyChangeListener
  67 {
  68 public:
  69   void watch(SGPropertyNode* p)
  70   {
  71     p->addChangeListener(this, false);
  72   }
  73
  74   virtual void valueChanged(SGPropertyNode*)
  75   {
  76     fire();
  77   }
  78 protected:
  79   virtual void fire() = 0;
  80 };
  81
  82 /**
  83  * Template adapter, created by convenience helper below
  84  */
  85 template <class T>
  86 class MethodPropertyWatcher : public PropertyWatcher
  87 {
  88 public:
  89   typedef void (T::*fire_method)();
  90
  91   MethodPropertyWatcher(T* obj, fire_method m) :
  92     _object(obj),
  93     _method(m)
  94   { ; }
  95
  96 protected:
  97   virtual void fire()
  98   { // dispatch to the object method we're helping
  99     (_object->*_method)();
 100   }
 101
 102 private:
 103   T* _object;
 104   fire_method _method;
 105 };
 106
 107 template <class T>
 108 PropertyWatcher* createWatcher(T* obj, void (T::*m)())
 109 {
 110   return new MethodPropertyWatcher<T>(obj, m);
 111 }
 112
 113 static bool commandLoadFlightPlan(const SGPropertyNode* arg)
 114 {
 115   FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
 116   SGPath path(arg->getStringValue("path"));
 117   return self->loadRoute(path);
 118 }
 119
 120 static bool commandSaveFlightPlan(const SGPropertyNode* arg)
 121 {
 122   FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
 123   SGPath path(arg->getStringValue("path"));
 124   return self->saveRoute(path);
 125 }
 126
 127 static bool commandActivateFlightPlan(const SGPropertyNode* arg)
 128 {
 129   FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
 130   bool activate = arg->getBoolValue("activate", true);
 131   if (activate) {
 132     self->activate();
 133   } else {
 134
 135   }
 136
 137   return true;
 138 }
 139
 140 static bool commandClearFlightPlan(const SGPropertyNode*)
 141 {
 142   FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
 143   self->clearRoute();
 144   return true;
 145 }
 146
 147 static bool commandSetActiveWaypt(const SGPropertyNode* arg)
 148 {
 149   FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
 150   int index = arg->getIntValue("index");
 151   if ((index < 0) || (index >= self->numWaypts())) {
 152     return false;
 153   }
 154
 155   self->jumpToIndex(index);
 156   return true;
 157 }
 158
 159 static bool commandInsertWaypt(const SGPropertyNode* arg)
 160 {
 161   FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
 162   int index = arg->getIntValue("index");
 163   std::string ident(arg->getStringValue("id"));
 164   int alt = arg->getIntValue("altitude-ft", -999);
 165   int ias = arg->getIntValue("speed-knots", -999);
 166
 167   WayptRef wp;
 168 // lat/lon may be supplied to narrow down navaid search, or to specify
 169 // a raw waypoint
 170   SGGeod pos;
 171   if (arg->hasChild("longitude-deg")) {
 172     pos = SGGeod::fromDeg(arg->getDoubleValue("longitude-deg"),
 173                                arg->getDoubleValue("latitude-deg"));
 174   }
 175
 176   if (arg->hasChild("navaid")) {
 177     FGPositionedRef p = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid"), pos);
 178
 179     if (arg->hasChild("navaid", 1)) {
 180       // intersection of two radials
 181       FGPositionedRef p2 = FGPositioned::findClosestWithIdent(arg->getStringValue("navaid[1]"), pos);
 182       if (!p2) {
 183         SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << arg->getStringValue("navaid[1]"));
 184         return false;
 185       }
 186
 187       double r1 = arg->getDoubleValue("radial"),
 188         r2 = arg->getDoubleValue("radial[1]");
 189
 190       SGGeod intersection;
 191       bool ok = SGGeodesy::radialIntersection(p->geod(), r1, p2->geod(), r2, intersection);
 192       if (!ok) {
 193         SG_LOG(SG_AUTOPILOT, SG_INFO, "no valid intersection for:" << p->ident()
 194                 << "," << p2->ident());
 195         return false;
 196       }
 197
 198       std::string name = p->ident() + "-" + p2->ident();
 199       wp = new BasicWaypt(intersection, name, NULL);
 200     } else if (arg->hasChild("offset-nm") && arg->hasChild("radial")) {
 201       // offset radial from navaid
 202       double radial = arg->getDoubleValue("radial");
 203       double distanceNm = arg->getDoubleValue("offset-nm");
 204       //radial += magvar->getDoubleValue(); // convert to true bearing
 205       wp = new OffsetNavaidWaypoint(p, NULL, radial, distanceNm);
 206     } else {
 207       wp = new NavaidWaypoint(p, NULL);
 208     }
 209   } else if (arg->hasChild("airport")) {
 210     const FGAirport* apt = fgFindAirportID(arg->getStringValue("airport"));
 211     if (!apt) {
 212       SG_LOG(SG_AUTOPILOT, SG_INFO, "no such airport" << arg->getStringValue("airport"));
 213       return false;
 214     }
 215
 216     if (arg->hasChild("runway")) {
 217       if (!apt->hasRunwayWithIdent(arg->getStringValue("runway"))) {
 218         SG_LOG(SG_AUTOPILOT, SG_INFO, "No runway: " << arg->getStringValue("runway") << " at " << apt->ident());
 219         return false;
 220       }
 221
 222       FGRunway* runway = apt->getRunwayByIdent(arg->getStringValue("runway"));
 223       wp = new RunwayWaypt(runway, NULL);
 224     } else {
 225       wp = new NavaidWaypoint((FGAirport*) apt, NULL);
 226     }
 227   } else if (arg->hasChild("text")) {
 228     wp = self->waypointFromString(arg->getStringValue("text"));
 229   } else if (!(pos == SGGeod())) {
 230     // just a raw lat/lon
 231     wp = new BasicWaypt(pos, ident, NULL);
 232   } else {
 233     return false; // failed to build waypoint
 234   }
 235
 236   if (alt >= 0) {
 237     wp->setAltitude(alt, flightgear::RESTRICT_AT);
 238   }
 239
 240   if (ias > 0) {
 241     wp->setSpeed(ias, flightgear::RESTRICT_AT);
 242   }
 243
 244   self->insertWayptAtIndex(wp, index);
 245   return true;
 246 }
 247
 248 static bool commandDeleteWaypt(const SGPropertyNode* arg)
 249 {
 250   FGRouteMgr* self = (FGRouteMgr*) globals->get_subsystem("route-manager");
 251   int index = arg->getIntValue("index");
 252   self->removeWayptAtIndex(index);
 253   return true;
 254 }
 255
 256 /////////////////////////////////////////////////////////////////////////////
 257
 258 FGRouteMgr::FGRouteMgr() :
 259   _currentIndex(0),
 260   input(fgGetNode( RM "input", true )),
 261   mirror(fgGetNode( RM "route", true )),
 262   _departureWatcher(NULL),
 263   _arrivalWatcher(NULL)
 264 {
 265   listener = new InputListener(this);
 266   input->setStringValue("");
 267   input->addChangeListener(listener);
 268
 269   SGCommandMgr::instance()->addCommand("load-flightplan", commandLoadFlightPlan);
 270   SGCommandMgr::instance()->addCommand("save-flightplan", commandSaveFlightPlan);
 271   SGCommandMgr::instance()->addCommand("activate-flightplan", commandActivateFlightPlan);
 272   SGCommandMgr::instance()->addCommand("clear-flightplan", commandClearFlightPlan);
 273   SGCommandMgr::instance()->addCommand("set-active-waypt", commandSetActiveWaypt);
 274   SGCommandMgr::instance()->addCommand("insert-waypt", commandInsertWaypt);
 275   SGCommandMgr::instance()->addCommand("delete-waypt", commandDeleteWaypt);
 276 }
 277
 278
 279 FGRouteMgr::~FGRouteMgr()
 280 {
 281   input->removeChangeListener(listener);
 282   delete listener;
 283   delete _departureWatcher;
 284   delete _arrivalWatcher;
 285 }
 286
 287
 288 void FGRouteMgr::init() {
 289   SGPropertyNode_ptr rm(fgGetNode(RM));
 290
 291   lon = fgGetNode( "/position/longitude-deg", true );
 292   lat = fgGetNode( "/position/latitude-deg", true );
 293   alt = fgGetNode( "/position/altitude-ft", true );
 294   magvar = fgGetNode("/environment/magnetic-variation-deg", true);
 295
 296   departure = fgGetNode(RM "departure", true);
 297   departure->tie("airport", SGRawValueMethods<FGRouteMgr, const char*>(*this,
 298     &FGRouteMgr::getDepartureICAO, &FGRouteMgr::setDepartureICAO));
 299   departure->tie("name", SGRawValueMethods<FGRouteMgr, const char*>(*this,
 300     &FGRouteMgr::getDepartureName, NULL));
 301   departure->setStringValue("runway", "");
 302
 303   delete _departureWatcher;
 304   _departureWatcher = createWatcher(this, &FGRouteMgr::departureChanged);
 305   _departureWatcher->watch(departure->getChild("runway"));
 306
 307   departure->getChild("etd", 0, true);
 308   _departureWatcher->watch(departure->getChild("sid", 0, true));
 309   departure->getChild("takeoff-time", 0, true);
 310
 311   destination = fgGetNode(RM "destination", true);
 312   destination->getChild("airport", 0, true);
 313
 314   destination->tie("airport", SGRawValueMethods<FGRouteMgr, const char*>(*this,
 315     &FGRouteMgr::getDestinationICAO, &FGRouteMgr::setDestinationICAO));
 316   destination->tie("name", SGRawValueMethods<FGRouteMgr, const char*>(*this,
 317     &FGRouteMgr::getDestinationName, NULL));
 318
 319   delete _arrivalWatcher;
 320   _arrivalWatcher = createWatcher(this, &FGRouteMgr::arrivalChanged);
 321   _arrivalWatcher->watch(destination->getChild("runway", 0, true));
 322
 323   destination->getChild("eta", 0, true);
 324   _arrivalWatcher->watch(destination->getChild("star", 0, true));
 325   _arrivalWatcher->watch(destination->getChild("transition", 0, true));
 326   destination->getChild("touchdown-time", 0, true);
 327
 328   alternate = fgGetNode(RM "alternate", true);
 329   alternate->getChild("airport", 0, true);
 330   alternate->getChild("runway", 0, true);
 331
 332   cruise = fgGetNode(RM "cruise", true);
 333   cruise->getChild("altitude-ft", 0, true);
 334   cruise->setDoubleValue("altitude-ft", 10000.0);
 335   cruise->getChild("flight-level", 0, true);
 336   cruise->getChild("speed-kts", 0, true);
 337   cruise->setDoubleValue("speed-kts", 160.0);
 338
 339   _routingType = cruise->getChild("routing", 0, true);
 340   _routingType->setIntValue(ROUTE_HIGH_AIRWAYS);
 341
 342   totalDistance = fgGetNode(RM "total-distance", true);
 343   totalDistance->setDoubleValue(0.0);
 344   distanceToGo = fgGetNode(RM "distance-remaining-nm", true);
 345   distanceToGo->setDoubleValue(0.0);
 346
 347   ete = fgGetNode(RM "ete", true);
 348   ete->setDoubleValue(0.0);
 349
 350   elapsedFlightTime = fgGetNode(RM "flight-time", true);
 351   elapsedFlightTime->setDoubleValue(0.0);
 352
 353   active = fgGetNode(RM "active", true);
 354   active->setBoolValue(false);
 355
 356   airborne = fgGetNode(RM "airborne", true);
 357   airborne->setBoolValue(false);
 358
 359   _edited = fgGetNode(RM "signals/edited", true);
 360   _finished = fgGetNode(RM "signals/finished", true);
 361
 362   _currentWpt = fgGetNode(RM "current-wp", true);
 363   _currentWpt->tie(SGRawValueMethods<FGRouteMgr, int>
 364     (*this, &FGRouteMgr::currentIndex, &FGRouteMgr::jumpToIndex));
 365
 366   // temporary distance / eta calculations, for backward-compatability
 367   wp0 = fgGetNode(RM "wp", 0, true);
 368   wp0->getChild("id", 0, true);
 369   wp0->getChild("dist", 0, true);
 370   wp0->getChild("eta", 0, true);
 371   wp0->getChild("bearing-deg", 0, true);
 372
 373   wp1 = fgGetNode(RM "wp", 1, true);
 374   wp1->getChild("id", 0, true);
 375   wp1->getChild("dist", 0, true);
 376   wp1->getChild("eta", 0, true);
 377
 378   wpn = fgGetNode(RM "wp-last", 0, true);
 379   wpn->getChild("dist", 0, true);
 380   wpn->getChild("eta", 0, true);
 381
 382   update_mirror();
 383   _pathNode = fgGetNode(RM "file-path", 0, true);
 384 }
 385
 386
 387 void FGRouteMgr::postinit()
 388 {
 389   SGPath path(_pathNode->getStringValue());
 390   if (!path.isNull()) {
 391     SG_LOG(SG_AUTOPILOT, SG_INFO, "loading flight-plan from: " << path.str());
 392     loadRoute(path);
 393   }
 394
 395 // this code only matters for the --wp option now - perhaps the option
 396 // should be deprecated in favour of an explicit flight-plan file?
 397 // then the global initial waypoint list could die.
 398   string_list *waypoints = globals->get_initial_waypoints();
 399   if (waypoints) {
 400     string_list::iterator it;
 401     for (it = waypoints->begin(); it != waypoints->end(); ++it) {
 402       WayptRef w = waypointFromString(*it);
 403       if (w) {
 404         _route.push_back(w);
 405       }
 406     }
 407
 408     SG_LOG(SG_AUTOPILOT, SG_INFO, "loaded initial waypoints:" << _route.size());
 409     update_mirror();
 410   }
 411
 412   weightOnWheels = fgGetNode("/gear/gear[0]/wow", true);
 413   // check airbone flag agrees with presets
 414 }
 415
 416 void FGRouteMgr::bind() { }
 417 void FGRouteMgr::unbind() { }
 418
 419 bool FGRouteMgr::isRouteActive() const
 420 {
 421   return active->getBoolValue();
 422 }
 423
 424 void FGRouteMgr::update( double dt )
 425 {
 426   if (dt <= 0.0) {
 427     return; // paused, nothing to do here
 428   }
 429
 430   double groundSpeed = fgGetDouble("/velocities/groundspeed-kt", 0.0);
 431   if (airborne->getBoolValue()) {
 432     time_t now = time(NULL);
 433     elapsedFlightTime->setDoubleValue(difftime(now, _takeoffTime));
 434   } else { // not airborne
 435     if (weightOnWheels->getBoolValue() || (groundSpeed < 40)) {
 436       return;
 437     }
 438
 439     airborne->setBoolValue(true);
 440     _takeoffTime = time(NULL); // start the clock
 441     departure->setIntValue("takeoff-time", _takeoffTime);
 442   }
 443
 444   if (!active->getBoolValue()) {
 445     return;
 446   }
 447
 448 // basic course/distance information
 449   SGGeod currentPos = SGGeod::fromDegFt(lon->getDoubleValue(),
 450     lat->getDoubleValue(),alt->getDoubleValue());
 451
 452   Waypt* curWpt = currentWaypt();
 453   if (!curWpt) {
 454     return;
 455   }
 456
 457   double courseDeg;
 458   double distanceM;
 459   boost::tie(courseDeg, distanceM) = curWpt->courseAndDistanceFrom(currentPos);
 460
 461 // update wp0 / wp1 / wp-last
 462   wp0->setDoubleValue("dist", distanceM * SG_METER_TO_NM);
 463   wp0->setDoubleValue("true-bearing-deg", courseDeg);
 464   courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing
 465   wp0->setDoubleValue("bearing-deg", courseDeg);
 466   setETAPropertyFromDistance(wp0->getChild("eta"), distanceM);
 467
 468   double totalPathDistance = totalDistance->getDoubleValue() * SG_NM_TO_METER;
 469   double totalDistanceRemaining = distanceM; // distance to current waypoint
 470   double pathDistance = cachedWaypointPathTotalDistance(_currentIndex);
 471
 472 // total distance to go, is direct distance to wp0, plus the remaining
 473 // path distance from wp0
 474   totalDistanceRemaining += (totalPathDistance - pathDistance);
 475
 476   wp0->setDoubleValue("distance-along-route-nm",
 477                       pathDistance * SG_METER_TO_NM);
 478   wp0->setDoubleValue("remaining-distance-nm",
 479                       (totalPathDistance - pathDistance) * SG_METER_TO_NM);
 480
 481   Waypt* nextWpt = nextWaypt();
 482   if (nextWpt) {
 483     boost::tie(courseDeg, distanceM) = nextWpt->courseAndDistanceFrom(currentPos);
 484
 485     wp1->setDoubleValue("dist", distanceM * SG_METER_TO_NM);
 486     wp1->setDoubleValue("true-bearing-deg", courseDeg);
 487     courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing
 488     wp1->setDoubleValue("bearing-deg", courseDeg);
 489     setETAPropertyFromDistance(wp1->getChild("eta"), distanceM);
 490
 491     double pathDistance = cachedWaypointPathTotalDistance(_currentIndex + 1);
 492     wp1->setDoubleValue("distance-along-route-nm",
 493                         pathDistance * SG_METER_TO_NM);
 494     wp1->setDoubleValue("remaining-distance-nm",
 495                         (totalPathDistance - pathDistance) * SG_METER_TO_NM);
 496   }
 497
 498   distanceToGo->setDoubleValue(totalDistanceRemaining * SG_METER_TO_NM);
 499   wpn->setDoubleValue("dist", totalDistanceRemaining * SG_METER_TO_NM);
 500   ete->setDoubleValue(totalDistanceRemaining * SG_METER_TO_NM / groundSpeed * 3600.0);
 501   setETAPropertyFromDistance(wpn->getChild("eta"), totalDistanceRemaining);
 502 }
 503
 504 void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance)
 505 {
 506   double speed = fgGetDouble("/velocities/groundspeed-kt", 0.0);
 507   if (speed < 1.0) {
 508     aProp->setStringValue("--:--");
 509     return;
 510   }
 511
 512   char eta_str[64];
 513   double eta = aDistance * SG_METER_TO_NM / speed;
 514   if ( eta >= 100.0 ) {
 515       eta = 99.999; // clamp
 516   }
 517
 518   if ( eta < (1.0/6.0) ) {
 519     eta *= 60.0; // within 10 minutes, bump up to min/secs
 520   }
 521
 522   int major = (int)eta,
 523       minor = (int)((eta - (int)eta) * 60.0);
 524   snprintf( eta_str, 64, "%d:%02d", major, minor );
 525   aProp->setStringValue( eta_str );
 526 }
 527
 528 flightgear::WayptRef FGRouteMgr::removeWayptAtIndex(int aIndex)
 529 {
 530   int index = aIndex;
 531   if (aIndex < 0) { // negative indices count the the end
 532     index = _route.size() + index;
 533   }
 534
 535   if ((index < 0) || (index >= numWaypts())) {
 536     SG_LOG(SG_AUTOPILOT, SG_WARN, "removeWayptAtIndex with invalid index:" << aIndex);
 537     return NULL;
 538   }
 539   WayptVec::iterator it = _route.begin();
 540   it += index;
 541
 542   WayptRef w = *it; // hold a ref now, in case _route is the only other owner
 543   _route.erase(it);
 544
 545   update_mirror();
 546
 547   if (_currentIndex == index) {
 548     currentWaypointChanged(); // current waypoint was removed
 549   }
 550   else
 551   if (_currentIndex > index) {
 552     --_currentIndex; // shift current index down if necessary
 553   }
 554
 555   _edited->fireValueChanged();
 556   checkFinished();
 557
 558   return w;
 559 }
 560
 561 struct NotGeneratedWayptPredicate : public std::unary_function<const Waypt*, bool>
 562 {
 563   bool operator() (const Waypt* w) const
 564   {
 565     return (w->flag(WPT_GENERATED) == false);
 566   }
 567 };
 568
 569
 570 void FGRouteMgr::clearRoute()
 571 {
 572 // erase all non-generated waypoints
 573   WayptVec::iterator r =
 574     std::remove_if(_route.begin(), _route.end(), NotGeneratedWayptPredicate());
 575   _route.erase(r, _route.end());
 576
 577   _currentIndex = -1;
 578
 579   update_mirror();
 580   active->setBoolValue(false);
 581   _edited->fireValueChanged();
 582 }
 583
 584 /**
 585  * route between index-1 and index, using airways.
 586  */
 587 bool FGRouteMgr::routeToIndex(int index, RouteType aRouteType)
 588 {
 589   WayptRef wp1;
 590   WayptRef wp2;
 591
 592   if (index == -1) {
 593     index = _route.size(); // can still be zero, of course
 594   }
 595
 596   if (index == 0) {
 597     if (!_departure) {
 598       SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no departure set");
 599       return false;
 600     }
 601
 602     wp1 = new NavaidWaypoint(_departure.get(), NULL);
 603   } else {
 604     wp1 = wayptAtIndex(index - 1);
 605   }
 606
 607   if (index >= numWaypts()) {
 608     if (!_destination) {
 609       SG_LOG(SG_AUTOPILOT, SG_WARN, "routeToIndex: no destination set");
 610       return false;
 611     }
 612
 613     wp2 = new NavaidWaypoint(_destination.get(), NULL);
 614   } else {
 615     wp2 = wayptAtIndex(index);
 616   }
 617
 618   double distNm = SGGeodesy::distanceNm(wp1->position(), wp2->position());
 619   if (distNm < 100.0) {
 620     SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: existing waypoints are nearby, direct route");
 621     return true;
 622   }
 623
 624   WayptVec r;
 625   switch (aRouteType) {
 626   case ROUTE_HIGH_AIRWAYS:
 627     Airway::highLevel()->route(wp1, wp2, r);
 628     break;
 629
 630   case ROUTE_LOW_AIRWAYS:
 631     Airway::lowLevel()->route(wp1, wp2, r);
 632     break;
 633
 634   case ROUTE_VOR:
 635     throw sg_exception("VOR routing not supported yet");
 636   }
 637
 638   if (r.empty()) {
 639     SG_LOG(SG_AUTOPILOT, SG_INFO, "routeToIndex: no route found");
 640     return false;
 641   }
 642
 643   WayptVec::iterator it = _route.begin();
 644   it += index;
 645   _route.insert(it, r.begin(), r.end());
 646
 647   update_mirror();
 648   _edited->fireValueChanged();
 649   return true;
 650 }
 651
 652 void FGRouteMgr::autoRoute()
 653 {
 654   if (!_departure || !_destination) {
 655     return;
 656   }
 657
 658   string runwayId(departure->getStringValue("runway"));
 659   FGRunway* runway = NULL;
 660   if (_departure->hasRunwayWithIdent(runwayId)) {
 661     runway = _departure->getRunwayByIdent(runwayId);
 662   }
 663
 664   FGRunway* dstRunway = NULL;
 665   runwayId = destination->getStringValue("runway");
 666   if (_destination->hasRunwayWithIdent(runwayId)) {
 667     dstRunway = _destination->getRunwayByIdent(runwayId);
 668   }
 669
 670   _route.clear(); // clear out the existing, first
 671 // SID
 672   flightgear::SID* sid;
 673   WayptRef sidTrans;
 674
 675   boost::tie(sid, sidTrans) = _departure->selectSID(_destination->geod(), runway);
 676   if (sid) {
 677     SG_LOG(SG_AUTOPILOT, SG_INFO, "selected SID " << sid->ident());
 678     if (sidTrans) {
 679       SG_LOG(SG_AUTOPILOT, SG_INFO, "\tvia " << sidTrans->ident() << " transition");
 680     }
 681
 682     sid->route(runway, sidTrans, _route);
 683     departure->setStringValue("sid", sid->ident());
 684   } else {
 685     // use airport location for airway search
 686     sidTrans = new NavaidWaypoint(_departure.get(), NULL);
 687     departure->setStringValue("sid", "");
 688   }
 689
 690 // STAR
 691   destination->setStringValue("transition", "");
 692   destination->setStringValue("star", "");
 693
 694   STAR* star;
 695   WayptRef starTrans;
 696   boost::tie(star, starTrans) = _destination->selectSTAR(_departure->geod(), dstRunway);
 697   if (star) {
 698     SG_LOG(SG_AUTOPILOT, SG_INFO, "selected STAR " << star->ident());
 699     if (starTrans) {
 700       SG_LOG(SG_AUTOPILOT, SG_INFO, "\tvia " << starTrans->ident() << " transition");
 701       destination->setStringValue("transition", starTrans->ident());
 702     }
 703     destination->setStringValue("star", star->ident());
 704   } else {
 705     // use airport location for search
 706     starTrans = new NavaidWaypoint(_destination.get(), NULL);
 707   }
 708
 709 // route between them
 710   WayptVec airwayRoute;
 711   if (Airway::highLevel()->route(sidTrans, starTrans, airwayRoute)) {
 712     _route.insert(_route.end(), airwayRoute.begin(), airwayRoute.end());
 713   }
 714
 715 // add the STAR if we have one
 716   if (star) {
 717     _destination->buildApproach(starTrans, star, dstRunway, _route);
 718   }
 719
 720   update_mirror();
 721   _edited->fireValueChanged();
 722 }
 723
 724 void FGRouteMgr::departureChanged()
 725 {
 726 // remove existing departure waypoints
 727   WayptVec::iterator it = _route.begin();
 728   for (; it != _route.end(); ++it) {
 729     if (!(*it)->flag(WPT_DEPARTURE)) {
 730       break;
 731     }
 732   }
 733
 734   // erase() invalidates iterators, so grab now
 735   WayptRef enroute;
 736   if (it == _route.end()) {
 737     if (_destination) {
 738       enroute = new NavaidWaypoint(_destination.get(), NULL);
 739     }
 740   } else {
 741     enroute = *it;
 742   }
 743
 744   _route.erase(_route.begin(), it);
 745   if (!_departure) {
 746     waypointsChanged();
 747     return;
 748   }
 749
 750   WayptVec wps;
 751   buildDeparture(enroute, wps);
 752   for (it = wps.begin(); it != wps.end(); ++it) {
 753     (*it)->setFlag(WPT_DEPARTURE);
 754     (*it)->setFlag(WPT_GENERATED);
 755   }
 756   _route.insert(_route.begin(), wps.begin(), wps.end());
 757
 758   update_mirror();
 759   waypointsChanged();
 760 }
 761
 762 void FGRouteMgr::buildDeparture(WayptRef enroute, WayptVec& wps)
 763 {
 764   string runwayId(departure->getStringValue("runway"));
 765   if (!_departure->hasRunwayWithIdent(runwayId)) {
 766 // valid airport, but no runway selected, so just the airport noide itself
 767     wps.push_back(new NavaidWaypoint(_departure.get(), NULL));
 768     return;
 769   }
 770
 771   FGRunway* r = _departure->getRunwayByIdent(runwayId);
 772   string sidId = departure->getStringValue("sid");
 773   flightgear::SID* sid = _departure->findSIDWithIdent(sidId);
 774   if (!sid) {
 775 // valid runway, but no SID selected/found, so just the runway node for now
 776     if (!sidId.empty() && (sidId != "(none)")) {
 777       SG_LOG(SG_AUTOPILOT, SG_INFO, "SID not found:" << sidId);
 778     }
 779
 780     wps.push_back(new RunwayWaypt(r, NULL));
 781     return;
 782   }
 783
 784 // we have a valid SID, awesome
 785   string trans(departure->getStringValue("transition"));
 786   WayptRef t = sid->findTransitionByName(trans);
 787   if (!t && enroute) {
 788     t = sid->findBestTransition(enroute->position());
 789   }
 790
 791   sid->route(r, t, wps);
 792   if (!wps.empty() && wps.front()->flag(WPT_DYNAMIC)) {
 793     // ensure first waypoint is static, to simplify other computations
 794     wps.insert(wps.begin(), new RunwayWaypt(r, NULL));
 795   }
 796 }
 797
 798 void FGRouteMgr::arrivalChanged()
 799 {
 800   // remove existing arrival waypoints
 801   WayptVec::reverse_iterator rit = _route.rbegin();
 802   for (; rit != _route.rend(); ++rit) {
 803     if (!(*rit)->flag(WPT_ARRIVAL)) {
 804       break;
 805     }
 806   }
 807
 808   // erase() invalidates iterators, so grab now
 809   WayptRef enroute;
 810   WayptVec::iterator it;
 811
 812   if (rit != _route.rend()) {
 813     enroute = *rit;
 814     it = rit.base(); // convert to fwd iterator
 815   } else {
 816     it = _route.begin();
 817   }
 818
 819   _route.erase(it, _route.end());
 820
 821   WayptVec wps;
 822   buildArrival(enroute, wps);
 823   for (it = wps.begin(); it != wps.end(); ++it) {
 824     (*it)->setFlag(WPT_ARRIVAL);
 825     (*it)->setFlag(WPT_GENERATED);
 826   }
 827   _route.insert(_route.end(), wps.begin(), wps.end());
 828
 829   update_mirror();
 830   waypointsChanged();
 831 }
 832
 833 void FGRouteMgr::buildArrival(WayptRef enroute, WayptVec& wps)
 834 {
 835   if (!_destination) {
 836     return;
 837   }
 838
 839   string runwayId(destination->getStringValue("runway"));
 840   if (!_destination->hasRunwayWithIdent(runwayId)) {
 841 // valid airport, but no runway selected, so just the airport node itself
 842     wps.push_back(new NavaidWaypoint(_destination.get(), NULL));
 843     return;
 844   }
 845
 846   FGRunway* r = _destination->getRunwayByIdent(runwayId);
 847   string starId = destination->getStringValue("star");
 848   STAR* star = _destination->findSTARWithIdent(starId);
 849   if (!star) {
 850 // valid runway, but no STAR selected/found, so just the runway node for now
 851     wps.push_back(new RunwayWaypt(r, NULL));
 852     return;
 853   }
 854
 855 // we have a valid STAR
 856   string trans(destination->getStringValue("transition"));
 857   WayptRef t = star->findTransitionByName(trans);
 858   if (!t && enroute) {
 859     t = star->findBestTransition(enroute->position());
 860   }
 861
 862   _destination->buildApproach(t, star, r, wps);
 863 }
 864
 865 void FGRouteMgr::waypointsChanged()
 866 {
 867
 868 }
 869
 870 void FGRouteMgr::insertWayptAtIndex(Waypt* aWpt, int aIndex)
 871 {
 872   if (!aWpt) {
 873     return;
 874   }
 875
 876   int index = aIndex;
 877   if ((aIndex == -1) || (aIndex > (int) _route.size())) {
 878     index = _route.size();
 879   }
 880
 881   WayptVec::iterator it = _route.begin();
 882   it += index;
 883
 884   if (_currentIndex >= index) {
 885     ++_currentIndex;
 886   }
 887
 888   _route.insert(it, aWpt);
 889
 890   update_mirror();
 891   _edited->fireValueChanged();
 892 }
 893
 894 WayptRef FGRouteMgr::waypointFromString(const string& tgt )
 895 {
 896   string target(boost::to_upper_copy(tgt));
 897   WayptRef wpt;
 898
 899 // extract altitude
 900   double altFt = cruise->getDoubleValue("altitude-ft");
 901   RouteRestriction altSetting = RESTRICT_NONE;
 902
 903   size_t pos = target.find( '@' );
 904   if ( pos != string::npos ) {
 905     altFt = atof( target.c_str() + pos + 1 );
 906     target = target.substr( 0, pos );
 907     if ( !strcmp(fgGetString("/sim/startup/units"), "meter") )
 908       altFt *= SG_METER_TO_FEET;
 909     altSetting = RESTRICT_AT;
 910   }
 911
 912 // check for lon,lat
 913   pos = target.find( ',' );
 914   if ( pos != string::npos ) {
 915     double lon = atof( target.substr(0, pos).c_str());
 916     double lat = atof( target.c_str() + pos + 1);
 917     char buf[32];
 918     char ew = (lon < 0.0) ? 'W' : 'E';
 919     char ns = (lat < 0.0) ? 'S' : 'N';
 920     snprintf(buf, 32, "%c%03d%c%03d", ew, (int) fabs(lon), ns, (int)fabs(lat));
 921
 922     wpt = new BasicWaypt(SGGeod::fromDeg(lon, lat), buf, NULL);
 923     if (altSetting != RESTRICT_NONE) {
 924       wpt->setAltitude(altFt, altSetting);
 925     }
 926     return wpt;
 927   }
 928
 929   SGGeod basePosition;
 930   if (_route.empty()) {
 931     // route is empty, use current position
 932     basePosition = SGGeod::fromDeg(lon->getDoubleValue(), lat->getDoubleValue());
 933   } else {
 934     basePosition = _route.back()->position();
 935   }
 936
 937   string_list pieces(simgear::strutils::split(target, "/"));
 938   FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition);
 939   if (!p) {
 940     SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
 941     return NULL;
 942   }
 943
 944   if (pieces.size() == 1) {
 945     wpt = new NavaidWaypoint(p, NULL);
 946   } else if (pieces.size() == 3) {
 947     // navaid/radial/distance-nm notation
 948     double radial = atof(pieces[1].c_str()),
 949       distanceNm = atof(pieces[2].c_str());
 950     radial += magvar->getDoubleValue(); // convert to true bearing
 951     wpt = new OffsetNavaidWaypoint(p, NULL, radial, distanceNm);
 952   } else if (pieces.size() == 2) {
 953     FGAirport* apt = dynamic_cast<FGAirport*>(p.ptr());
 954     if (!apt) {
 955       SG_LOG(SG_AUTOPILOT, SG_INFO, "Waypoint is not an airport:" << pieces.front());
 956       return NULL;
 957     }
 958
 959     if (!apt->hasRunwayWithIdent(pieces[1])) {
 960       SG_LOG(SG_AUTOPILOT, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
 961       return NULL;
 962     }
 963
 964     FGRunway* runway = apt->getRunwayByIdent(pieces[1]);
 965     wpt = new NavaidWaypoint(runway, NULL);
 966   } else if (pieces.size() == 4) {
 967     // navid/radial/navid/radial notation
 968     FGPositionedRef p2 = FGPositioned::findClosestWithIdent(pieces[2], basePosition);
 969     if (!p2) {
 970       SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces[2]);
 971       return NULL;
 972     }
 973
 974     double r1 = atof(pieces[1].c_str()),
 975       r2 = atof(pieces[3].c_str());
 976     r1 += magvar->getDoubleValue();
 977     r2 += magvar->getDoubleValue();
 978
 979     SGGeod intersection;
 980     bool ok = SGGeodesy::radialIntersection(p->geod(), r1, p2->geod(), r2, intersection);
 981     if (!ok) {
 982       SG_LOG(SG_AUTOPILOT, SG_INFO, "no valid intersection for:" << target);
 983       return NULL;
 984     }
 985
 986     std::string name = p->ident() + "-" + p2->ident();
 987     wpt = new BasicWaypt(intersection, name, NULL);
 988   }
 989
 990   if (!wpt) {
 991     SG_LOG(SG_AUTOPILOT, SG_INFO, "Unable to parse waypoint:" << target);
 992     return NULL;
 993   }
 994
 995   if (altSetting != RESTRICT_NONE) {
 996     wpt->setAltitude(altFt, altSetting);
 997   }
 998   return wpt;
 999 }
1000
1001 // mirror internal route to the property system for inspection by other subsystems
1002 void FGRouteMgr::update_mirror()
1003 {
1004   mirror->removeChildren("wp");
1005
1006   int num = numWaypts();
1007   double totalDistanceEnroute = 0.0;
1008
1009   for (int i = 0; i < num; i++) {
1010     Waypt* wp = _route[i];
1011     SGPropertyNode *prop = mirror->getChild("wp", i, 1);
1012
1013     const SGGeod& pos(wp->position());
1014     prop->setStringValue("id", wp->ident().c_str());
1015     prop->setDoubleValue("longitude-deg", pos.getLongitudeDeg());
1016     prop->setDoubleValue("latitude-deg",pos.getLatitudeDeg());
1017
1018     // leg course+distance
1019     if (i < (num - 1)) {
1020       Waypt* next = _route[i+1];
1021       std::pair<double, double> crsDist =
1022         next->courseAndDistanceFrom(pos);
1023       prop->setDoubleValue("leg-bearing-true-deg", crsDist.first);
1024       prop->setDoubleValue("leg-distance-nm", crsDist.second * SG_METER_TO_NM);
1025       prop->setDoubleValue("distance-along-route-nm", totalDistanceEnroute);
1026       totalDistanceEnroute += crsDist.second * SG_METER_TO_NM;
1027     }
1028
1029     if (wp->altitudeRestriction() != RESTRICT_NONE) {
1030       double ft = wp->altitudeFt();
1031       prop->setDoubleValue("altitude-m", ft * SG_FEET_TO_METER);
1032       prop->setDoubleValue("altitude-ft", ft);
1033       prop->setIntValue("flight-level", static_cast<int>(ft / 1000) * 10);
1034     } else {
1035       prop->setDoubleValue("altitude-m", -9999.9);
1036       prop->setDoubleValue("altitude-ft", -9999.9);
1037     }
1038
1039     if (wp->speedRestriction() == SPEED_RESTRICT_MACH) {
1040       prop->setDoubleValue("speed-mach", wp->speedMach());
1041     } else if (wp->speedRestriction() != RESTRICT_NONE) {
1042       prop->setDoubleValue("speed-kts", wp->speedKts());
1043     }
1044
1045     if (wp->flag(WPT_ARRIVAL)) {
1046       prop->setBoolValue("arrival", true);
1047     }
1048
1049     if (wp->flag(WPT_DEPARTURE)) {
1050       prop->setBoolValue("departure", true);
1051     }
1052
1053     if (wp->flag(WPT_MISS)) {
1054       prop->setBoolValue("missed-approach", true);
1055     }
1056
1057     prop->setBoolValue("generated", wp->flag(WPT_GENERATED));
1058   } // of waypoint iteration
1059
1060   // set number as listener attachment point
1061   mirror->setIntValue("num", _route.size());
1062
1063   NewGUI * gui = (NewGUI *)globals->get_subsystem("gui");
1064   FGDialog* rmDlg = gui->getDialog("route-manager");
1065   if (rmDlg) {
1066     rmDlg->updateValues();
1067   }
1068
1069   if (_departure) {
1070     departure->setDoubleValue("field-elevation-ft", _departure->getElevation());
1071   }
1072
1073   if (_destination) {
1074     destination->setDoubleValue("field-elevation-ft", _destination->getElevation());
1075   }
1076
1077   totalDistance->setDoubleValue(totalDistanceEnroute);
1078 }
1079
1080 double FGRouteMgr::cachedLegPathDistanceM(int index) const
1081 {
1082   SGPropertyNode *prop = mirror->getChild("wp", index, 1);
1083   return prop->getDoubleValue("leg-distance-nm") * SG_NM_TO_METER;
1084 }
1085
1086 double FGRouteMgr::cachedWaypointPathTotalDistance(int index) const
1087 {
1088   SGPropertyNode *prop = mirror->getChild("wp", index, 1);
1089   return prop->getDoubleValue("distance-along-route-nm") * SG_NM_TO_METER;
1090 }
1091
1092 // command interface /autopilot/route-manager/input:
1093 //
1094 //   @CLEAR             ... clear route
1095 //   @POP               ... remove first entry
1096 //   @DELETE3           ... delete 4th entry
1097 //   @INSERT2:KSFO@900  ... insert "KSFO@900" as 3rd entry
1098 //   KSFO@900           ... append "KSFO@900"
1099 //
1100 void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop)
1101 {
1102     const char *s = prop->getStringValue();
1103     if (strlen(s) == 0) {
1104       return;
1105     }
1106
1107     if (!strcmp(s, "@CLEAR"))
1108         mgr->clearRoute();
1109     else if (!strcmp(s, "@ACTIVATE"))
1110         mgr->activate();
1111     else if (!strcmp(s, "@LOAD")) {
1112       SGPath path(mgr->_pathNode->getStringValue());
1113       mgr->loadRoute(path);
1114     } else if (!strcmp(s, "@SAVE")) {
1115       SGPath path(mgr->_pathNode->getStringValue());
1116       mgr->saveRoute(path);
1117     } else if (!strcmp(s, "@NEXT")) {
1118       mgr->jumpToIndex(mgr->_currentIndex + 1);
1119     } else if (!strcmp(s, "@PREVIOUS")) {
1120       mgr->jumpToIndex(mgr->_currentIndex - 1);
1121     } else if (!strncmp(s, "@JUMP", 5)) {
1122       mgr->jumpToIndex(atoi(s + 5));
1123     } else if (!strncmp(s, "@DELETE", 7))
1124         mgr->removeWayptAtIndex(atoi(s + 7));
1125     else if (!strncmp(s, "@INSERT", 7)) {
1126         char *r;
1127         int pos = strtol(s + 7, &r, 10);
1128         if (*r++ != ':')
1129             return;
1130         while (isspace(*r))
1131             r++;
1132         if (*r)
1133             mgr->insertWayptAtIndex(mgr->waypointFromString(r), pos);
1134     } else if (!strncmp(s, "@ROUTE", 6)) {
1135       char* r;
1136       int endIndex = strtol(s + 6, &r, 10);
1137       RouteType rt = (RouteType) mgr->_routingType->getIntValue();
1138       mgr->routeToIndex(endIndex, rt);
1139     } else if (!strcmp(s, "@AUTOROUTE")) {
1140       mgr->autoRoute();
1141     } else if (!strcmp(s, "@POSINIT")) {
1142       mgr->initAtPosition();
1143     } else
1144       mgr->insertWayptAtIndex(mgr->waypointFromString(s), -1);
1145 }
1146
1147 void FGRouteMgr::initAtPosition()
1148 {
1149   if (isRouteActive()) {
1150     return; // don't mess with the active route
1151   }
1152
1153   if (haveUserWaypoints()) {
1154     // user has already defined, loaded or entered a route, again
1155     // don't interfere with it
1156     return;
1157   }
1158
1159   if (airborne->getBoolValue()) {
1160     SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: airborne, clearing departure info");
1161     _departure = NULL;
1162     departure->setStringValue("runway", "");
1163     return;
1164   }
1165
1166 // on the ground
1167   SGGeod pos = SGGeod::fromDegFt(lon->getDoubleValue(),
1168     lat->getDoubleValue(), alt->getDoubleValue());
1169   if (!_departure) {
1170     _departure = FGAirport::findClosest(pos, 20.0);
1171     if (!_departure) {
1172       SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: couldn't find an airport within 20nm");
1173       departure->setStringValue("runway", "");
1174       return;
1175     }
1176   }
1177
1178   std::string rwy = departure->getStringValue("runway");
1179   if (!rwy.empty()) {
1180     // runway already set, fine
1181     return;
1182   }
1183
1184   FGRunway* r = _departure->findBestRunwayForPos(pos);
1185   if (!r) {
1186     return;
1187   }
1188
1189   departure->setStringValue("runway", r->ident().c_str());
1190   SG_LOG(SG_AUTOPILOT, SG_INFO, "initAtPosition: starting at "
1191     << _departure->ident() << " on runway " << r->ident());
1192 }
1193
1194 bool FGRouteMgr::haveUserWaypoints() const
1195 {
1196   return std::find_if(_route.begin(), _route.end(), NotGeneratedWayptPredicate()) != _route.end();
1197 }
1198
1199 bool FGRouteMgr::activate()
1200 {
1201   if (isRouteActive()) {
1202     SG_LOG(SG_AUTOPILOT, SG_WARN, "duplicate route-activation, no-op");
1203     return false;
1204   }
1205
1206   _currentIndex = 0;
1207   currentWaypointChanged();
1208
1209  /* double routeDistanceNm = _route->total_distance() * SG_METER_TO_NM;
1210   totalDistance->setDoubleValue(routeDistanceNm);
1211   double cruiseSpeedKts = cruise->getDoubleValue("speed", 0.0);
1212   if (cruiseSpeedKts > 1.0) {
1213     // very very crude approximation, doesn't allow for climb / descent
1214     // performance or anything else at all
1215     ete->setDoubleValue(routeDistanceNm / cruiseSpeedKts * (60.0 * 60.0));
1216   }
1217   */
1218   active->setBoolValue(true);
1219   SG_LOG(SG_AUTOPILOT, SG_INFO, "route-manager, activate route ok");
1220   return true;
1221 }
1222
1223
1224 void FGRouteMgr::sequence()
1225 {
1226   if (!active->getBoolValue()) {
1227     SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route");
1228     return;
1229   }
1230
1231   if (checkFinished()) {
1232     return;
1233   }
1234
1235   _currentIndex++;
1236   currentWaypointChanged();
1237 }
1238
1239 bool FGRouteMgr::checkFinished()
1240 {
1241   if (_currentIndex < (int) _route.size()) {
1242     return false;
1243   }
1244
1245   SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
1246   _finished->fireValueChanged();
1247   active->setBoolValue(false);
1248   return true;
1249 }
1250
1251 void FGRouteMgr::jumpToIndex(int index)
1252 {
1253   if ((index < 0) || (index >= (int) _route.size())) {
1254     SG_LOG(SG_AUTOPILOT, SG_ALERT, "passed invalid index (" <<
1255       index << ") to FGRouteMgr::jumpToIndex");
1256     return;
1257   }
1258
1259   if (_currentIndex == index) {
1260     return; // no-op
1261   }
1262
1263 // all the checks out the way, go ahead and update state
1264   _currentIndex = index;
1265   currentWaypointChanged();
1266   _currentWpt->fireValueChanged();
1267 }
1268
1269 void FGRouteMgr::currentWaypointChanged()
1270 {
1271   Waypt* cur = currentWaypt();
1272   Waypt* next = nextWaypt();
1273
1274   wp0->getChild("id")->setStringValue(cur ? cur->ident() : "");
1275   wp1->getChild("id")->setStringValue(next ? next->ident() : "");
1276
1277   _currentWpt->fireValueChanged();
1278   SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _currentIndex);
1279 }
1280
1281 int FGRouteMgr::findWayptIndex(const SGGeod& aPos) const
1282 {
1283   for (int i=0; i<numWaypts(); ++i) {
1284     if (_route[i]->matches(aPos)) {
1285       return i;
1286     }
1287   }
1288
1289   return -1;
1290 }
1291
1292 Waypt* FGRouteMgr::currentWaypt() const
1293 {
1294   if ((_currentIndex < 0) || (_currentIndex >= numWaypts()))
1295       return NULL;
1296   return wayptAtIndex(_currentIndex);
1297 }
1298
1299 Waypt* FGRouteMgr::previousWaypt() const
1300 {
1301   if (_currentIndex == 0) {
1302     return NULL;
1303   }
1304
1305   return wayptAtIndex(_currentIndex - 1);
1306 }
1307
1308 Waypt* FGRouteMgr::nextWaypt() const
1309 {
1310   if ((_currentIndex < 0) || ((_currentIndex + 1) >= numWaypts())) {
1311     return NULL;
1312   }
1313
1314   return wayptAtIndex(_currentIndex + 1);
1315 }
1316
1317 Waypt* FGRouteMgr::wayptAtIndex(int index) const
1318 {
1319   if ((index < 0) || (index >= numWaypts())) {
1320     throw sg_range_exception("waypt index out of range", "FGRouteMgr::wayptAtIndex");
1321   }
1322
1323   return _route[index];
1324 }
1325
1326 SGPropertyNode_ptr FGRouteMgr::wayptNodeAtIndex(int index) const
1327 {
1328     if ((index < 0) || (index >= numWaypts())) {
1329         throw sg_range_exception("waypt index out of range", "FGRouteMgr::wayptAtIndex");
1330     }
1331
1332     return mirror->getChild("wp", index);
1333 }
1334
1335 bool FGRouteMgr::saveRoute(const SGPath& path)
1336 {
1337   SG_LOG(SG_IO, SG_INFO, "Saving route to " << path.str());
1338   try {
1339     SGPropertyNode_ptr d(new SGPropertyNode);
1340     SGPath path(_pathNode->getStringValue());
1341     d->setIntValue("version", 2);
1342
1343     if (_departure) {
1344       d->setStringValue("departure/airport", _departure->ident());
1345       d->setStringValue("departure/sid", departure->getStringValue("sid"));
1346       d->setStringValue("departure/runway", departure->getStringValue("runway"));
1347     }
1348
1349     if (_destination) {
1350       d->setStringValue("destination/airport", _destination->ident());
1351       d->setStringValue("destination/star", destination->getStringValue("star"));
1352       d->setStringValue("destination/transition", destination->getStringValue("transition"));
1353       d->setStringValue("destination/runway", destination->getStringValue("runway"));
1354     }
1355
1356   // route nodes
1357     SGPropertyNode* routeNode = d->getChild("route", 0, true);
1358     for (unsigned int i=0; i<_route.size(); ++i) {
1359       Waypt* wpt = _route[i];
1360       wpt->saveAsNode(routeNode->getChild("wp", i, true));
1361     } // of waypoint iteration
1362     writeProperties(path.str(), d, true /* write-all */);
1363     return true;
1364   } catch (sg_exception& e) {
1365     SG_LOG(SG_IO, SG_ALERT, "Failed to save flight-plan '" << path.str() << "'. " << e.getMessage());
1366     return false;
1367   }
1368 }
1369
1370 bool FGRouteMgr::loadRoute(const SGPath& path)
1371 {
1372   if (!path.exists())
1373   {
1374       SG_LOG(SG_IO, SG_ALERT, "Failed to load flight-plan '" << path.str()
1375               << "'. The file does not exist.");
1376       return false;
1377   }
1378
1379   // deactivate route first
1380   active->setBoolValue(false);
1381
1382   SGPropertyNode_ptr routeData(new SGPropertyNode);
1383
1384   SG_LOG(SG_IO, SG_INFO, "going to read flight-plan from:" << path.str());
1385
1386   bool Status = false;
1387   try {
1388     readProperties(path.str(), routeData);
1389   } catch (sg_exception& ) {
1390     // if XML parsing fails, the file might be simple textual list of waypoints
1391     Status = loadPlainTextRoute(path);
1392     routeData = 0;
1393   }
1394
1395   if (routeData.valid())
1396   {
1397       try {
1398         int version = routeData->getIntValue("version", 1);
1399         if (version == 1) {
1400           loadVersion1XMLRoute(routeData);
1401         } else if (version == 2) {
1402           loadVersion2XMLRoute(routeData);
1403         } else {
1404           throw sg_io_exception("unsupported XML route version");
1405         }
1406         Status = true;
1407       } catch (sg_exception& e) {
1408         SG_LOG(SG_IO, SG_ALERT, "Failed to load flight-plan '" << e.getOrigin()
1409           << "'. " << e.getMessage());
1410         Status = false;
1411       }
1412   }
1413
1414   update_mirror();
1415
1416   return Status;
1417 }
1418
1419 void FGRouteMgr::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
1420 {
1421   // departure nodes
1422   SGPropertyNode* dep = routeData->getChild("departure");
1423   if (dep) {
1424     string depIdent = dep->getStringValue("airport");
1425     _departure = (FGAirport*) fgFindAirportID(depIdent);
1426     departure->setStringValue("runway", dep->getStringValue("runway"));
1427     departure->setStringValue("sid", dep->getStringValue("sid"));
1428     departure->setStringValue("transition", dep->getStringValue("transition"));
1429   }
1430
1431 // destination
1432   SGPropertyNode* dst = routeData->getChild("destination");
1433   if (dst) {
1434     _destination = (FGAirport*) fgFindAirportID(dst->getStringValue("airport"));
1435     destination->setStringValue("runway", dst->getStringValue("runway"));
1436     destination->setStringValue("star", dst->getStringValue("star"));
1437     destination->setStringValue("transition", dst->getStringValue("transition"));
1438   }
1439
1440 // alternate
1441   SGPropertyNode* alt = routeData->getChild("alternate");
1442   if (alt) {
1443     alternate->setStringValue(alt->getStringValue("airport"));
1444   } // of cruise data loading
1445
1446 // cruise
1447   SGPropertyNode* crs = routeData->getChild("cruise");
1448   if (crs) {
1449     cruise->setDoubleValue("speed-kts", crs->getDoubleValue("speed-kts"));
1450     cruise->setDoubleValue("mach", crs->getDoubleValue("mach"));
1451     cruise->setDoubleValue("altitude-ft", crs->getDoubleValue("altitude-ft"));
1452   } // of cruise data loading
1453
1454 }
1455
1456 void FGRouteMgr::loadVersion2XMLRoute(SGPropertyNode_ptr routeData)
1457 {
1458   loadXMLRouteHeader(routeData);
1459
1460 // route nodes
1461   WayptVec wpts;
1462   SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
1463   for (int i=0; i<routeNode->nChildren(); ++i) {
1464     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
1465     WayptRef wpt = Waypt::createFromProperties(NULL, wpNode);
1466     wpts.push_back(wpt);
1467   } // of route iteration
1468
1469   _route = wpts;
1470 }
1471
1472 void FGRouteMgr::loadVersion1XMLRoute(SGPropertyNode_ptr routeData)
1473 {
1474   loadXMLRouteHeader(routeData);
1475
1476 // route nodes
1477   WayptVec wpts;
1478   SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
1479   for (int i=0; i<routeNode->nChildren(); ++i) {
1480     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
1481     WayptRef wpt = parseVersion1XMLWaypt(wpNode);
1482     wpts.push_back(wpt);
1483   } // of route iteration
1484
1485   _route = wpts;
1486 }
1487
1488 WayptRef FGRouteMgr::parseVersion1XMLWaypt(SGPropertyNode* aWP)
1489 {
1490   SGGeod lastPos;
1491   if (!_route.empty()) {
1492     lastPos = _route.back()->position();
1493   } else if (_departure) {
1494     lastPos = _departure->geod();
1495   }
1496
1497   WayptRef w;
1498   string ident(aWP->getStringValue("ident"));
1499   if (aWP->hasChild("longitude-deg")) {
1500     // explicit longitude/latitude
1501     w = new BasicWaypt(SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"),
1502       aWP->getDoubleValue("latitude-deg")), ident, NULL);
1503
1504   } else {
1505     string nid = aWP->getStringValue("navid", ident.c_str());
1506     FGPositionedRef p = FGPositioned::findClosestWithIdent(nid, lastPos);
1507     if (!p) {
1508       throw sg_io_exception("bad route file, unknown navid:" + nid);
1509     }
1510
1511     SGGeod pos(p->geod());
1512     if (aWP->hasChild("offset-nm") && aWP->hasChild("offset-radial")) {
1513       double radialDeg = aWP->getDoubleValue("offset-radial");
1514       // convert magnetic radial to a true radial!
1515       radialDeg += magvar->getDoubleValue();
1516       double offsetNm = aWP->getDoubleValue("offset-nm");
1517       double az2;
1518       SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
1519     }
1520
1521     w = new BasicWaypt(pos, ident, NULL);
1522   }
1523
1524   double altFt = aWP->getDoubleValue("altitude-ft", -9999.9);
1525   if (altFt > -9990.0) {
1526     w->setAltitude(altFt, RESTRICT_AT);
1527   }
1528
1529   return w;
1530 }
1531
1532 bool FGRouteMgr::loadPlainTextRoute(const SGPath& path)
1533 {
1534   try {
1535     sg_gzifstream in(path.str().c_str());
1536     if (!in.is_open()) {
1537         throw sg_io_exception("Cannot open file for reading.");
1538     }
1539
1540     WayptVec wpts;
1541     while (!in.eof()) {
1542       string line;
1543       getline(in, line, '\n');
1544     // trim CR from end of line, if found
1545       if (line[line.size() - 1] == '\r') {
1546         line.erase(line.size() - 1, 1);
1547       }
1548
1549       line = simgear::strutils::strip(line);
1550       if (line.empty() || (line[0] == '#')) {
1551         continue; // ignore empty/comment lines
1552       }
1553
1554       WayptRef w = waypointFromString(line);
1555       if (!w) {
1556         throw sg_io_exception("Failed to create waypoint from line '" + line + "'.");
1557       }
1558
1559       wpts.push_back(w);
1560     } // of line iteration
1561
1562     _route = wpts;
1563     return true;
1564   } catch (sg_exception& e) {
1565     SG_LOG(SG_IO, SG_ALERT, "Failed to load route from: '" << path.str() << "'. " << e.getMessage());
1566     return false;
1567   }
1568 }
1569
1570 const char* FGRouteMgr::getDepartureICAO() const
1571 {
1572   if (!_departure) {
1573     return "";
1574   }
1575
1576   return _departure->ident().c_str();
1577 }
1578
1579 const char* FGRouteMgr::getDepartureName() const
1580 {
1581   if (!_departure) {
1582     return "";
1583   }
1584
1585   return _departure->name().c_str();
1586 }
1587
1588 void FGRouteMgr::setDepartureICAO(const char* aIdent)
1589 {
1590   if ((aIdent == NULL) || (strlen(aIdent) < 4)) {
1591     _departure = NULL;
1592   } else {
1593     _departure = FGAirport::findByIdent(aIdent);
1594   }
1595
1596   departureChanged();
1597 }
1598
1599 const char* FGRouteMgr::getDestinationICAO() const
1600 {
1601   if (!_destination) {
1602     return "";
1603   }
1604
1605   return _destination->ident().c_str();
1606 }
1607
1608 const char* FGRouteMgr::getDestinationName() const
1609 {
1610   if (!_destination) {
1611     return "";
1612   }
1613
1614   return _destination->name().c_str();
1615 }
1616
1617 void FGRouteMgr::setDestinationICAO(const char* aIdent)
1618 {
1619   if ((aIdent == NULL) || (strlen(aIdent) < 4)) {
1620     _destination = NULL;
1621   } else {
1622     _destination = FGAirport::findByIdent(aIdent);
1623   }
1624
1625   arrivalChanged();
1626 }
1627
1628 FGAirportRef FGRouteMgr::departureAirport() const
1629 {
1630     return _departure;
1631 }
1632
1633 FGAirportRef FGRouteMgr::destinationAirport() const
1634 {
1635     return _destination;
1636 }
1637
1638 FGRunway* FGRouteMgr::departureRunway() const
1639 {
1640     if (!_departure) {
1641         return NULL;
1642     }
1643
1644     string runwayId(departure->getStringValue("runway"));
1645     if (!_departure->hasRunwayWithIdent(runwayId)) {
1646         return NULL;
1647     }
1648
1649     return _departure->getRunwayByIdent(runwayId);
1650 }
1651
1652 FGRunway* FGRouteMgr::destinationRunway() const
1653 {
1654     if (!_destination) {
1655         return NULL;
1656     }
1657
1658     string runwayId(destination->getStringValue("runway"));
1659     if (!_destination->hasRunwayWithIdent(runwayId)) {
1660         return NULL;
1661     }
1662
1663     return _destination->getRunwayByIdent(runwayId);
1664 }
1665