src/Navaids/FlightPlan.cxx

   1 // FlightPlan.cxx - flight plan object
   2
   3 // Written by James Turner, started 2012.
   4 //
   5 // Copyright (C) 2012  Curtis L. Olson
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20
  21 #ifdef HAVE_CONFIG_H
  22 # include "config.h"
  23 #endif
  24
  25 #include "FlightPlan.hxx"
  26
  27 // std
  28 #include <map>
  29 #include <fstream>
  30 #include <cassert>
  31
  32 // Boost
  33 #include <boost/algorithm/string/case_conv.hpp>
  34 #include <boost/algorithm/string.hpp>
  35 #include <boost/foreach.hpp>
  36
  37 // SimGear
  38 #include <simgear/structure/exception.hxx>
  39 #include <simgear/misc/sg_path.hxx>
  40 #include <simgear/magvar/magvar.hxx>
  41 #include <simgear/timing/sg_time.hxx>
  42 #include <simgear/misc/sgstream.hxx>
  43 #include <simgear/misc/strutils.hxx>
  44 #include <simgear/props/props_io.hxx>
  45
  46 // FlightGear
  47 #include <Main/globals.hxx>
  48 #include "Main/fg_props.hxx"
  49 #include <Navaids/procedure.hxx>
  50 #include <Navaids/waypoint.hxx>
  51
  52 using std::string;
  53 using std::vector;
  54 using std::endl;
  55 using std::fstream;
  56
  57 namespace flightgear {
  58
  59 typedef std::vector<FlightPlan::DelegateFactory*> FPDelegateFactoryVec;
  60 static FPDelegateFactoryVec static_delegateFactories;
  61
  62 FlightPlan::FlightPlan() :
  63   _delegateLock(0),
  64   _currentIndex(-1),
  65   _departureRunway(NULL),
  66   _destinationRunway(NULL),
  67   _sid(NULL),
  68   _star(NULL),
  69   _approach(NULL),
  70   _delegate(NULL)
  71 {
  72   _departureChanged = _arrivalChanged = _waypointsChanged = _currentWaypointChanged = false;
  73
  74   BOOST_FOREACH(DelegateFactory* factory, static_delegateFactories) {
  75     Delegate* d = factory->createFlightPlanDelegate(this);
  76     if (d) { // factory might not always create a delegate
  77       d->_deleteWithPlan = true;
  78       addDelegate(d);
  79     }
  80   }
  81 }
  82
  83 FlightPlan::~FlightPlan()
  84 {
  85 // delete all delegates which we own.
  86   Delegate* d = _delegate;
  87   while (d) {
  88     Delegate* cur = d;
  89     d = d->_inner;
  90     if (cur->_deleteWithPlan) {
  91       delete cur;
  92     }
  93   }
  94 }
  95
  96 FlightPlan* FlightPlan::clone(const string& newIdent) const
  97 {
  98   FlightPlan* c = new FlightPlan();
  99   c->_ident = newIdent.empty() ? _ident : newIdent;
 100   c->lockDelegate();
 101
 102 // copy destination / departure data.
 103   c->setDeparture(_departure);
 104   c->setDeparture(_departureRunway);
 105
 106   if (_approach) {
 107     c->setApproach(_approach);
 108   } else if (_destinationRunway) {
 109     c->setDestination(_destinationRunway);
 110   } else if (_destination) {
 111     c->setDestination(_destination);
 112   }
 113
 114   c->setSTAR(_star);
 115   c->setSID(_sid);
 116
 117 // copy legs
 118   c->_waypointsChanged = true;
 119   for (int l=0; l < numLegs(); ++l) {
 120     c->_legs.push_back(_legs[l]->cloneFor(c));
 121   }
 122   c->unlockDelegate();
 123   return c;
 124 }
 125
 126 void FlightPlan::setIdent(const string& s)
 127 {
 128   _ident = s;
 129 }
 130
 131 string FlightPlan::ident() const
 132 {
 133   return _ident;
 134 }
 135
 136 FlightPlan::Leg* FlightPlan::insertWayptAtIndex(Waypt* aWpt, int aIndex)
 137 {
 138   if (!aWpt) {
 139     return NULL;
 140   }
 141
 142   WayptVec wps;
 143   wps.push_back(aWpt);
 144
 145   int index = aIndex;
 146   if ((aIndex == -1) || (aIndex > (int) _legs.size())) {
 147     index = _legs.size();
 148   }
 149
 150   insertWayptsAtIndex(wps, index);
 151   return legAtIndex(index);
 152 }
 153
 154 void FlightPlan::insertWayptsAtIndex(const WayptVec& wps, int aIndex)
 155 {
 156   if (wps.empty()) {
 157     return;
 158   }
 159
 160   int index = aIndex;
 161   if ((aIndex == -1) || (aIndex > (int) _legs.size())) {
 162     index = _legs.size();
 163   }
 164
 165   LegVec::iterator it = _legs.begin();
 166   it += index;
 167
 168   int endIndex = index + wps.size() - 1;
 169   if (_currentIndex >= endIndex) {
 170     _currentIndex += wps.size();
 171   }
 172
 173   LegVec newLegs;
 174   BOOST_FOREACH(WayptRef wp, wps) {
 175     newLegs.push_back(new Leg(this, wp));
 176   }
 177
 178   lockDelegate();
 179   _waypointsChanged = true;
 180   _legs.insert(it, newLegs.begin(), newLegs.end());
 181   unlockDelegate();
 182 }
 183
 184 void FlightPlan::deleteIndex(int aIndex)
 185 {
 186   int index = aIndex;
 187   if (aIndex < 0) { // negative indices count the the end
 188     index = _legs.size() + index;
 189   }
 190
 191   if ((index < 0) || (index >= numLegs())) {
 192     SG_LOG(SG_NAVAID, SG_WARN, "removeAtIndex with invalid index:" << aIndex);
 193     return;
 194   }
 195
 196   lockDelegate();
 197   _waypointsChanged = true;
 198
 199   LegVec::iterator it = _legs.begin();
 200   it += index;
 201   Leg* l = *it;
 202   _legs.erase(it);
 203   delete l;
 204
 205   if (_currentIndex == index) {
 206     // current waypoint was removed
 207     _currentWaypointChanged = true;
 208   } else if (_currentIndex > index) {
 209     --_currentIndex; // shift current index down if necessary
 210   }
 211
 212   unlockDelegate();
 213 }
 214
 215 void FlightPlan::clear()
 216 {
 217   lockDelegate();
 218   _waypointsChanged = true;
 219   _currentWaypointChanged = true;
 220   _arrivalChanged = true;
 221   _departureChanged = true;
 222
 223   _currentIndex = -1;
 224   BOOST_FOREACH(Leg* l, _legs) {
 225     delete l;
 226   }
 227   _legs.clear();
 228
 229   unlockDelegate();
 230 }
 231
 232 class RemoveWithFlag
 233 {
 234 public:
 235   RemoveWithFlag(WayptFlag f) : flag(f), delCount(0) { }
 236
 237   int numDeleted() const { return delCount; }
 238
 239   bool operator()(FlightPlan::Leg* leg) const
 240   {
 241     if (leg->waypoint()->flag(flag)) {
 242       delete leg;
 243       ++delCount;
 244       return true;
 245     }
 246
 247     return false;
 248   }
 249 private:
 250   WayptFlag flag;
 251   mutable int delCount;
 252 };
 253
 254 int FlightPlan::clearWayptsWithFlag(WayptFlag flag)
 255 {
 256   int count = 0;
 257 // first pass, fix up currentIndex
 258   for (int i=0; i<_currentIndex; ++i) {
 259     Leg* l = _legs[i];
 260     if (l->waypoint()->flag(flag)) {
 261       ++count;
 262     }
 263   }
 264
 265   _currentIndex -= count;
 266
 267 // now delete and remove
 268   RemoveWithFlag rf(flag);
 269   LegVec::iterator it = std::remove_if(_legs.begin(), _legs.end(), rf);
 270   if (it == _legs.end()) {
 271     return 0; // nothing was cleared, don't fire the delegate
 272   }
 273
 274   lockDelegate();
 275   _waypointsChanged = true;
 276   if (count > 0) {
 277     _currentWaypointChanged = true;
 278   }
 279
 280   _legs.erase(it, _legs.end());
 281   unlockDelegate();
 282   return rf.numDeleted();
 283 }
 284
 285 void FlightPlan::setCurrentIndex(int index)
 286 {
 287   if ((index < -1) || (index >= numLegs())) {
 288     throw sg_range_exception("invalid leg index", "FlightPlan::setCurrentIndex");
 289   }
 290
 291   if (index == _currentIndex) {
 292     return;
 293   }
 294
 295   lockDelegate();
 296   _currentIndex = index;
 297   _currentWaypointChanged = true;
 298   unlockDelegate();
 299 }
 300
 301 int FlightPlan::findWayptIndex(const SGGeod& aPos) const
 302 {
 303   for (int i=0; i<numLegs(); ++i) {
 304     if (_legs[i]->waypoint()->matches(aPos)) {
 305       return i;
 306     }
 307   }
 308
 309   return -1;
 310 }
 311
 312 FlightPlan::Leg* FlightPlan::currentLeg() const
 313 {
 314   if ((_currentIndex < 0) || (_currentIndex >= numLegs()))
 315     return NULL;
 316   return legAtIndex(_currentIndex);
 317 }
 318
 319 FlightPlan::Leg* FlightPlan::previousLeg() const
 320 {
 321   if (_currentIndex <= 0) {
 322     return NULL;
 323   }
 324
 325   return legAtIndex(_currentIndex - 1);
 326 }
 327
 328 FlightPlan::Leg* FlightPlan::nextLeg() const
 329 {
 330   if ((_currentIndex < 0) || ((_currentIndex + 1) >= numLegs())) {
 331     return NULL;
 332   }
 333
 334   return legAtIndex(_currentIndex + 1);
 335 }
 336
 337 FlightPlan::Leg* FlightPlan::legAtIndex(int index) const
 338 {
 339   if ((index < 0) || (index >= numLegs())) {
 340     throw sg_range_exception("index out of range", "FlightPlan::legAtIndex");
 341   }
 342
 343   return _legs[index];
 344 }
 345
 346 int FlightPlan::findLegIndex(const Leg *l) const
 347 {
 348   for (unsigned int i=0; i<_legs.size(); ++i) {
 349     if (_legs[i] == l) {
 350       return i;
 351     }
 352   }
 353
 354   return -1;
 355 }
 356
 357 void FlightPlan::setDeparture(FGAirport* apt)
 358 {
 359   if (apt == _departure) {
 360     return;
 361   }
 362
 363   lockDelegate();
 364   _departureChanged = true;
 365   _departure = apt;
 366   _departureRunway = NULL;
 367   setSID((SID*)NULL);
 368   unlockDelegate();
 369 }
 370
 371 void FlightPlan::setDeparture(FGRunway* rwy)
 372 {
 373   if (_departureRunway == rwy) {
 374     return;
 375   }
 376
 377   lockDelegate();
 378   _departureChanged = true;
 379
 380   _departureRunway = rwy;
 381   if (rwy->airport() != _departure) {
 382     _departure = rwy->airport();
 383     setSID((SID*)NULL);
 384   }
 385   unlockDelegate();
 386 }
 387
 388 void FlightPlan::setSID(SID* sid, const std::string& transition)
 389 {
 390   if (sid == _sid) {
 391     return;
 392   }
 393
 394   lockDelegate();
 395   _departureChanged = true;
 396   _sid = sid;
 397   _sidTransition = transition;
 398   unlockDelegate();
 399 }
 400
 401 void FlightPlan::setSID(Transition* trans)
 402 {
 403   if (!trans) {
 404     setSID((SID*) NULL);
 405     return;
 406   }
 407
 408   if (trans->parent()->type() != PROCEDURE_SID)
 409     throw sg_exception("FlightPlan::setSID: transition does not belong to a SID");
 410
 411   setSID((SID*) trans->parent(), trans->ident());
 412 }
 413
 414 Transition* FlightPlan::sidTransition() const
 415 {
 416   if (!_sid || _sidTransition.empty()) {
 417     return NULL;
 418   }
 419
 420   return _sid->findTransitionByName(_sidTransition);
 421 }
 422
 423 void FlightPlan::setDestination(FGAirport* apt)
 424 {
 425   if (apt == _destination) {
 426     return;
 427   }
 428
 429   lockDelegate();
 430   _arrivalChanged = true;
 431   _destination = apt;
 432   _destinationRunway = NULL;
 433   setSTAR((STAR*)NULL);
 434   setApproach(NULL);
 435   unlockDelegate();
 436 }
 437
 438 void FlightPlan::setDestination(FGRunway* rwy)
 439 {
 440   if (_destinationRunway == rwy) {
 441     return;
 442   }
 443
 444   lockDelegate();
 445   _arrivalChanged = true;
 446   _destinationRunway = rwy;
 447   if (_destination != rwy->airport()) {
 448     _destination = rwy->airport();
 449     setSTAR((STAR*)NULL);
 450   }
 451
 452   unlockDelegate();
 453 }
 454
 455 void FlightPlan::setSTAR(STAR* star, const std::string& transition)
 456 {
 457   if (_star == star) {
 458     return;
 459   }
 460
 461   lockDelegate();
 462   _arrivalChanged = true;
 463   _star = star;
 464   _starTransition = transition;
 465   unlockDelegate();
 466 }
 467
 468 void FlightPlan::setSTAR(Transition* trans)
 469 {
 470   if (!trans) {
 471     setSTAR((STAR*) NULL);
 472     return;
 473   }
 474
 475   if (trans->parent()->type() != PROCEDURE_STAR)
 476     throw sg_exception("FlightPlan::setSTAR: transition does not belong to a STAR");
 477
 478   setSTAR((STAR*) trans->parent(), trans->ident());
 479 }
 480
 481 Transition* FlightPlan::starTransition() const
 482 {
 483   if (!_star || _starTransition.empty()) {
 484     return NULL;
 485   }
 486
 487   return _star->findTransitionByName(_starTransition);
 488 }
 489
 490 void FlightPlan::setApproach(flightgear::Approach *app)
 491 {
 492   if (_approach == app) {
 493     return;
 494   }
 495
 496   lockDelegate();
 497   _arrivalChanged = true;
 498   _approach = app;
 499   if (app) {
 500     // keep runway + airport in sync
 501     if (_destinationRunway != _approach->runway()) {
 502       _destinationRunway = _approach->runway();
 503     }
 504
 505     if (_destination != _destinationRunway->airport()) {
 506       _destination = _destinationRunway->airport();
 507     }
 508   }
 509   unlockDelegate();
 510 }
 511
 512 bool FlightPlan::save(const SGPath& path)
 513 {
 514   SG_LOG(SG_NAVAID, SG_INFO, "Saving route to " << path.str());
 515   try {
 516     SGPropertyNode_ptr d(new SGPropertyNode);
 517     d->setIntValue("version", 2);
 518
 519     if (_departure) {
 520       d->setStringValue("departure/airport", _departure->ident());
 521       if (_sid) {
 522         d->setStringValue("departure/sid", _sid->ident());
 523       }
 524
 525       if (_departureRunway) {
 526         d->setStringValue("departure/runway", _departureRunway->ident());
 527       }
 528     }
 529
 530     if (_destination) {
 531       d->setStringValue("destination/airport", _destination->ident());
 532       if (_star) {
 533         d->setStringValue("destination/star", _star->ident());
 534       }
 535
 536       if (_approach) {
 537         d->setStringValue("destination/approach", _approach->ident());
 538       }
 539
 540       //d->setStringValue("destination/transition", destination->getStringValue("transition"));
 541
 542       if (_destinationRunway) {
 543         d->setStringValue("destination/runway", _destinationRunway->ident());
 544       }
 545     }
 546
 547     // route nodes
 548     SGPropertyNode* routeNode = d->getChild("route", 0, true);
 549     for (unsigned int i=0; i<_legs.size(); ++i) {
 550       Waypt* wpt = _legs[i]->waypoint();
 551       wpt->saveAsNode(routeNode->getChild("wp", i, true));
 552     } // of waypoint iteration
 553     writeProperties(path.str(), d, true /* write-all */);
 554     return true;
 555   } catch (sg_exception& e) {
 556     SG_LOG(SG_NAVAID, SG_ALERT, "Failed to save flight-plan '" << path.str() << "'. " << e.getMessage());
 557     return false;
 558   }
 559 }
 560
 561 bool FlightPlan::load(const SGPath& path)
 562 {
 563   if (!path.exists())
 564   {
 565     SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load flight-plan '" << path.str()
 566            << "'. The file does not exist.");
 567     return false;
 568   }
 569
 570   SGPropertyNode_ptr routeData(new SGPropertyNode);
 571   SG_LOG(SG_NAVAID, SG_INFO, "going to read flight-plan from:" << path.str());
 572
 573   bool Status = false;
 574   lockDelegate();
 575   try {
 576     readProperties(path.str(), routeData);
 577   } catch (sg_exception& ) {
 578     // if XML parsing fails, the file might be simple textual list of waypoints
 579     Status = loadPlainTextRoute(path);
 580     routeData = 0;
 581     _waypointsChanged = true;
 582   }
 583
 584   if (routeData.valid())
 585   {
 586     try {
 587       int version = routeData->getIntValue("version", 1);
 588       if (version == 1) {
 589         loadVersion1XMLRoute(routeData);
 590       } else if (version == 2) {
 591         loadVersion2XMLRoute(routeData);
 592       } else {
 593         throw sg_io_exception("unsupported XML route version");
 594       }
 595       Status = true;
 596     } catch (sg_exception& e) {
 597       SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load flight-plan '" << e.getOrigin()
 598              << "'. " << e.getMessage());
 599       Status = false;
 600     }
 601   }
 602
 603   unlockDelegate();
 604   return Status;
 605 }
 606
 607 void FlightPlan::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
 608 {
 609   // departure nodes
 610   SGPropertyNode* dep = routeData->getChild("departure");
 611   if (dep) {
 612     string depIdent = dep->getStringValue("airport");
 613     setDeparture((FGAirport*) fgFindAirportID(depIdent));
 614     if (_departure) {
 615       string rwy(dep->getStringValue("runway"));
 616       if (_departure->hasRunwayWithIdent(rwy)) {
 617         setDeparture(_departure->getRunwayByIdent(rwy));
 618       }
 619
 620       if (dep->hasChild("sid")) {
 621         setSID(_departure->findSIDWithIdent(dep->getStringValue("sid")));
 622       }
 623    // departure->setStringValue("transition", dep->getStringValue("transition"));
 624     }
 625   }
 626
 627   // destination
 628   SGPropertyNode* dst = routeData->getChild("destination");
 629   if (dst) {
 630     setDestination((FGAirport*) fgFindAirportID(dst->getStringValue("airport")));
 631     if (_destination) {
 632       string rwy(dst->getStringValue("runway"));
 633       if (_destination->hasRunwayWithIdent(rwy)) {
 634         setDestination(_destination->getRunwayByIdent(rwy));
 635       }
 636
 637       if (dst->hasChild("star")) {
 638         setSTAR(_destination->findSTARWithIdent(dst->getStringValue("star")));
 639       }
 640
 641       if (dst->hasChild("approach")) {
 642         setApproach(_destination->findApproachWithIdent(dst->getStringValue("approach")));
 643       }
 644     }
 645
 646    // destination->setStringValue("transition", dst->getStringValue("transition"));
 647   }
 648
 649   // alternate
 650   SGPropertyNode* alt = routeData->getChild("alternate");
 651   if (alt) {
 652     //alternate->setStringValue(alt->getStringValue("airport"));
 653   }
 654
 655   // cruise
 656   SGPropertyNode* crs = routeData->getChild("cruise");
 657   if (crs) {
 658  //   cruise->setDoubleValue("speed-kts", crs->getDoubleValue("speed-kts"));
 659    // cruise->setDoubleValue("mach", crs->getDoubleValue("mach"));
 660    // cruise->setDoubleValue("altitude-ft", crs->getDoubleValue("altitude-ft"));
 661   } // of cruise data loading
 662
 663 }
 664
 665 void FlightPlan::loadVersion2XMLRoute(SGPropertyNode_ptr routeData)
 666 {
 667   loadXMLRouteHeader(routeData);
 668
 669   // route nodes
 670   _legs.clear();
 671   SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
 672   for (int i=0; i<routeNode->nChildren(); ++i) {
 673     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
 674     Leg* l = new Leg(this, Waypt::createFromProperties(NULL, wpNode));
 675     _legs.push_back(l);
 676   } // of route iteration
 677   _waypointsChanged = true;
 678 }
 679
 680 void FlightPlan::loadVersion1XMLRoute(SGPropertyNode_ptr routeData)
 681 {
 682   loadXMLRouteHeader(routeData);
 683
 684   // _legs nodes
 685   _legs.clear();
 686   SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
 687   for (int i=0; i<routeNode->nChildren(); ++i) {
 688     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
 689     Leg* l = new Leg(this, parseVersion1XMLWaypt(wpNode));
 690     _legs.push_back(l);
 691   } // of route iteration
 692   _waypointsChanged = true;
 693 }
 694
 695 WayptRef FlightPlan::parseVersion1XMLWaypt(SGPropertyNode* aWP)
 696 {
 697   SGGeod lastPos;
 698   if (!_legs.empty()) {
 699     lastPos = _legs.back()->waypoint()->position();
 700   } else if (_departure) {
 701     lastPos = _departure->geod();
 702   }
 703
 704   WayptRef w;
 705   string ident(aWP->getStringValue("ident"));
 706   if (aWP->hasChild("longitude-deg")) {
 707     // explicit longitude/latitude
 708     w = new BasicWaypt(SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"),
 709                                        aWP->getDoubleValue("latitude-deg")), ident, NULL);
 710
 711   } else {
 712     string nid = aWP->getStringValue("navid", ident.c_str());
 713     FGPositionedRef p = FGPositioned::findClosestWithIdent(nid, lastPos);
 714     SGGeod pos;
 715
 716     if (p) {
 717       pos = p->geod();
 718     } else {
 719       SG_LOG(SG_GENERAL, SG_WARN, "unknown navaid in flightplan:" << nid);
 720       pos = SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"),
 721                             aWP->getDoubleValue("latitude-deg"));
 722     }
 723
 724     if (aWP->hasChild("offset-nm") && aWP->hasChild("offset-radial")) {
 725       double radialDeg = aWP->getDoubleValue("offset-radial");
 726       // convert magnetic radial to a true radial!
 727       radialDeg += magvarDegAt(pos);
 728       double offsetNm = aWP->getDoubleValue("offset-nm");
 729       double az2;
 730       SGGeodesy::direct(pos, radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
 731     }
 732
 733     w = new BasicWaypt(pos, ident, NULL);
 734   }
 735
 736   double altFt = aWP->getDoubleValue("altitude-ft", -9999.9);
 737   if (altFt > -9990.0) {
 738     w->setAltitude(altFt, RESTRICT_AT);
 739   }
 740
 741   return w;
 742 }
 743
 744 bool FlightPlan::loadPlainTextRoute(const SGPath& path)
 745 {
 746   try {
 747     sg_gzifstream in(path.str().c_str());
 748     if (!in.is_open()) {
 749       throw sg_io_exception("Cannot open file for reading.");
 750     }
 751
 752     _legs.clear();
 753     while (!in.eof()) {
 754       string line;
 755       getline(in, line, '\n');
 756       // trim CR from end of line, if found
 757       if (line[line.size() - 1] == '\r') {
 758         line.erase(line.size() - 1, 1);
 759       }
 760
 761       line = simgear::strutils::strip(line);
 762       if (line.empty() || (line[0] == '#')) {
 763         continue; // ignore empty/comment lines
 764       }
 765
 766       WayptRef w = waypointFromString(line);
 767       if (!w) {
 768         throw sg_io_exception("Failed to create waypoint from line '" + line + "'.");
 769       }
 770
 771       _legs.push_back(new Leg(this, w));
 772     } // of line iteration
 773   } catch (sg_exception& e) {
 774     SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load route from: '" << path.str() << "'. " << e.getMessage());
 775     _legs.clear();
 776     return false;
 777   }
 778
 779   return true;
 780 }
 781
 782 double FlightPlan::magvarDegAt(const SGGeod& pos) const
 783 {
 784   double jd = globals->get_time_params()->getJD();
 785   return sgGetMagVar(pos, jd) * SG_RADIANS_TO_DEGREES;
 786 }
 787
 788 WayptRef FlightPlan::waypointFromString(const string& tgt )
 789 {
 790   string target(boost::to_upper_copy(tgt));
 791   WayptRef wpt;
 792
 793   // extract altitude
 794   double altFt = 0.0;
 795   RouteRestriction altSetting = RESTRICT_NONE;
 796
 797   size_t pos = target.find( '@' );
 798   if ( pos != string::npos ) {
 799     altFt = atof( target.c_str() + pos + 1 );
 800     target = target.substr( 0, pos );
 801     if ( !strcmp(fgGetString("/sim/startup/units"), "meter") )
 802       altFt *= SG_METER_TO_FEET;
 803     altSetting = RESTRICT_AT;
 804   }
 805
 806   // check for lon,lat
 807   pos = target.find( ',' );
 808   if ( pos != string::npos ) {
 809     double lon = atof( target.substr(0, pos).c_str());
 810     double lat = atof( target.c_str() + pos + 1);
 811     char buf[32];
 812     char ew = (lon < 0.0) ? 'W' : 'E';
 813     char ns = (lat < 0.0) ? 'S' : 'N';
 814     snprintf(buf, 32, "%c%03d%c%03d", ew, (int) fabs(lon), ns, (int)fabs(lat));
 815
 816     wpt = new BasicWaypt(SGGeod::fromDeg(lon, lat), buf, NULL);
 817     if (altSetting != RESTRICT_NONE) {
 818       wpt->setAltitude(altFt, altSetting);
 819     }
 820     return wpt;
 821   }
 822
 823   SGGeod basePosition;
 824   if (_legs.empty()) {
 825     // route is empty, use current position
 826     basePosition = globals->get_aircraft_position();
 827   } else {
 828     basePosition = _legs.back()->waypoint()->position();
 829   }
 830
 831   string_list pieces(simgear::strutils::split(target, "/"));
 832   FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition);
 833   if (!p) {
 834     SG_LOG( SG_NAVAID, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
 835     return NULL;
 836   }
 837
 838   double magvar = magvarDegAt(basePosition);
 839
 840   if (pieces.size() == 1) {
 841     wpt = new NavaidWaypoint(p, NULL);
 842   } else if (pieces.size() == 3) {
 843     // navaid/radial/distance-nm notation
 844     double radial = atof(pieces[1].c_str()),
 845     distanceNm = atof(pieces[2].c_str());
 846     radial += magvar;
 847     wpt = new OffsetNavaidWaypoint(p, NULL, radial, distanceNm);
 848   } else if (pieces.size() == 2) {
 849     FGAirport* apt = dynamic_cast<FGAirport*>(p.ptr());
 850     if (!apt) {
 851       SG_LOG(SG_NAVAID, SG_INFO, "Waypoint is not an airport:" << pieces.front());
 852       return NULL;
 853     }
 854
 855     if (!apt->hasRunwayWithIdent(pieces[1])) {
 856       SG_LOG(SG_NAVAID, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
 857       return NULL;
 858     }
 859
 860     FGRunway* runway = apt->getRunwayByIdent(pieces[1]);
 861     wpt = new NavaidWaypoint(runway, NULL);
 862   } else if (pieces.size() == 4) {
 863     // navid/radial/navid/radial notation
 864     FGPositionedRef p2 = FGPositioned::findClosestWithIdent(pieces[2], basePosition);
 865     if (!p2) {
 866       SG_LOG( SG_NAVAID, SG_INFO, "Unable to find FGPositioned with ident:" << pieces[2]);
 867       return NULL;
 868     }
 869
 870     double r1 = atof(pieces[1].c_str()),
 871     r2 = atof(pieces[3].c_str());
 872     r1 += magvar;
 873     r2 += magvar;
 874
 875     SGGeod intersection;
 876     bool ok = SGGeodesy::radialIntersection(p->geod(), r1, p2->geod(), r2, intersection);
 877     if (!ok) {
 878       SG_LOG(SG_NAVAID, SG_INFO, "no valid intersection for:" << target);
 879       return NULL;
 880     }
 881
 882     std::string name = p->ident() + "-" + p2->ident();
 883     wpt = new BasicWaypt(intersection, name, NULL);
 884   }
 885
 886   if (!wpt) {
 887     SG_LOG(SG_NAVAID, SG_INFO, "Unable to parse waypoint:" << target);
 888     return NULL;
 889   }
 890
 891   if (altSetting != RESTRICT_NONE) {
 892     wpt->setAltitude(altFt, altSetting);
 893   }
 894   return wpt;
 895 }
 896
 897 FlightPlan::Leg::Leg(FlightPlan* owner, WayptRef wpt) :
 898   _parent(owner),
 899   _speedRestrict(RESTRICT_NONE),
 900   _altRestrict(RESTRICT_NONE),
 901   _waypt(wpt)
 902 {
 903   if (!wpt.valid()) {
 904     throw sg_exception("can't create FlightPlan::Leg without underlying waypoint");
 905   }
 906   _speed = _altitudeFt = 0;
 907 }
 908
 909 FlightPlan::Leg* FlightPlan::Leg::cloneFor(FlightPlan* owner) const
 910 {
 911   Leg* c = new Leg(owner, _waypt);
 912 // clone local data
 913   c->_speed = _speed;
 914   c->_speedRestrict = _speedRestrict;
 915   c->_altitudeFt = _altitudeFt;
 916   c->_altRestrict = _altRestrict;
 917
 918   return c;
 919 }
 920
 921 FlightPlan::Leg* FlightPlan::Leg::nextLeg() const
 922 {
 923   return _parent->legAtIndex(index() + 1);
 924 }
 925
 926 unsigned int FlightPlan::Leg::index() const
 927 {
 928   return _parent->findLegIndex(this);
 929 }
 930
 931 int FlightPlan::Leg::altitudeFt() const
 932 {
 933   if (_altRestrict != RESTRICT_NONE) {
 934     return _altitudeFt;
 935   }
 936
 937   return _waypt->altitudeFt();
 938 }
 939
 940 int FlightPlan::Leg::speed() const
 941 {
 942   if (_speedRestrict != RESTRICT_NONE) {
 943     return _speed;
 944   }
 945
 946   return _waypt->speed();
 947 }
 948
 949 int FlightPlan::Leg::speedKts() const
 950 {
 951   return speed();
 952 }
 953
 954 double FlightPlan::Leg::speedMach() const
 955 {
 956   if (!isMachRestrict(_speedRestrict)) {
 957     return 0.0;
 958   }
 959
 960   return -(_speed / 100.0);
 961 }
 962
 963 RouteRestriction FlightPlan::Leg::altitudeRestriction() const
 964 {
 965   if (_altRestrict != RESTRICT_NONE) {
 966     return _altRestrict;
 967   }
 968
 969   return _waypt->altitudeRestriction();
 970 }
 971
 972 RouteRestriction FlightPlan::Leg::speedRestriction() const
 973 {
 974   if (_speedRestrict != RESTRICT_NONE) {
 975     return _speedRestrict;
 976   }
 977
 978   return _waypt->speedRestriction();
 979 }
 980
 981 void FlightPlan::Leg::setSpeed(RouteRestriction ty, double speed)
 982 {
 983   _speedRestrict = ty;
 984   if (isMachRestrict(ty)) {
 985     _speed = (speed * -100);
 986   } else {
 987     _speed = speed;
 988   }
 989 }
 990
 991 void FlightPlan::Leg::setAltitude(RouteRestriction ty, int altFt)
 992 {
 993   _altRestrict = ty;
 994   _altitudeFt = altFt;
 995 }
 996
 997 double FlightPlan::Leg::courseDeg() const
 998 {
 999   return _courseDeg;
1000 }
1001
1002 double FlightPlan::Leg::distanceNm() const
1003 {
1004   return _pathDistance;
1005 }
1006
1007 double FlightPlan::Leg::distanceAlongRoute() const
1008 {
1009   return _distanceAlongPath;
1010 }
1011
1012 void FlightPlan::rebuildLegData()
1013 {
1014   _totalDistance = 0.0;
1015   int lastLeg = static_cast<int>(_legs.size()) - 1;
1016   for (int l=0; l<lastLeg; ++l) {
1017     Leg* cur = _legs[l];
1018     Leg* next = _legs[l + 1];
1019
1020     std::pair<double, double> crsDist =
1021       next->waypoint()->courseAndDistanceFrom(cur->waypoint()->position());
1022     _legs[l]->_courseDeg = crsDist.first;
1023     _legs[l]->_pathDistance = crsDist.second * SG_METER_TO_NM;
1024     _legs[l]->_distanceAlongPath = _totalDistance;
1025     _totalDistance += crsDist.second * SG_METER_TO_NM;
1026   } // of legs iteration
1027
1028 // set some data on the final leg
1029   if (lastLeg > 0) {
1030     // keep the same course as the final leg, when passing the final
1031     // waypoint
1032     _legs[lastLeg]->_courseDeg = _legs[lastLeg - 1]->_courseDeg;
1033     _legs[lastLeg]->_pathDistance = 0.0;
1034     _legs[lastLeg]->_distanceAlongPath = _totalDistance;
1035   }
1036 }
1037
1038 void FlightPlan::lockDelegate()
1039 {
1040   if (_delegateLock == 0) {
1041     assert(!_departureChanged && !_arrivalChanged &&
1042            !_waypointsChanged && !_currentWaypointChanged);
1043   }
1044
1045   ++_delegateLock;
1046 }
1047
1048 void FlightPlan::unlockDelegate()
1049 {
1050   assert(_delegateLock > 0);
1051   if (_delegateLock > 1) {
1052     --_delegateLock;
1053     return;
1054   }
1055
1056   if (_departureChanged) {
1057     _departureChanged = false;
1058     if (_delegate) {
1059       _delegate->runDepartureChanged();
1060     }
1061   }
1062
1063   if (_arrivalChanged) {
1064     _arrivalChanged = false;
1065     if (_delegate) {
1066       _delegate->runArrivalChanged();
1067     }
1068   }
1069
1070   if (_waypointsChanged) {
1071     _waypointsChanged = false;
1072     rebuildLegData();
1073     if (_delegate) {
1074       _delegate->runWaypointsChanged();
1075     }
1076   }
1077
1078   if (_currentWaypointChanged) {
1079     _currentWaypointChanged = false;
1080     if (_delegate) {
1081       _delegate->runCurrentWaypointChanged();
1082     }
1083   }
1084
1085   --_delegateLock;
1086 }
1087
1088 void FlightPlan::registerDelegateFactory(DelegateFactory* df)
1089 {
1090   FPDelegateFactoryVec::iterator it = std::find(static_delegateFactories.begin(),
1091                                                 static_delegateFactories.end(), df);
1092   if (it != static_delegateFactories.end()) {
1093     throw  sg_exception("duplicate delegate factory registration");
1094   }
1095
1096   static_delegateFactories.push_back(df);
1097 }
1098
1099 void FlightPlan::unregisterDelegateFactory(DelegateFactory* df)
1100 {
1101   FPDelegateFactoryVec::iterator it = std::find(static_delegateFactories.begin(),
1102                                                 static_delegateFactories.end(), df);
1103   if (it == static_delegateFactories.end()) {
1104     return;
1105   }
1106
1107   static_delegateFactories.erase(it);
1108 }
1109
1110 void FlightPlan::addDelegate(Delegate* d)
1111 {
1112   // wrap any existing delegate(s) in the new one
1113   d->_inner = _delegate;
1114   _delegate = d;
1115 }
1116
1117 void FlightPlan::removeDelegate(Delegate* d)
1118 {
1119   if (d == _delegate) {
1120     _delegate = _delegate->_inner;
1121   } else if (_delegate) {
1122     _delegate->removeInner(d);
1123   }
1124 }
1125
1126 FlightPlan::Delegate::Delegate() :
1127   _deleteWithPlan(false),
1128   _inner(NULL)
1129 {
1130
1131 }
1132
1133 FlightPlan::Delegate::~Delegate()
1134 {
1135
1136 }
1137
1138 void FlightPlan::Delegate::removeInner(Delegate* d)
1139 {
1140   if (!_inner) {
1141     return;
1142   }
1143
1144   if (_inner == d) {
1145     // replace with grand-child
1146     _inner = d->_inner;
1147   } else { // recurse downwards
1148     _inner->removeInner(d);
1149   }
1150 }
1151
1152 void FlightPlan::Delegate::runDepartureChanged()
1153 {
1154   if (_inner) _inner->runDepartureChanged();
1155   departureChanged();
1156 }
1157
1158 void FlightPlan::Delegate::runArrivalChanged()
1159 {
1160   if (_inner) _inner->runArrivalChanged();
1161   arrivalChanged();
1162 }
1163
1164 void FlightPlan::Delegate::runWaypointsChanged()
1165 {
1166   if (_inner) _inner->runWaypointsChanged();
1167   waypointsChanged();
1168 }
1169
1170 void FlightPlan::Delegate::runCurrentWaypointChanged()
1171 {
1172   if (_inner) _inner->runCurrentWaypointChanged();
1173   currentWaypointChanged();
1174 }
1175
1176 } // of namespace flightgear