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   if (_delegate) {
 230     _delegate->runCleared();
 231   }
 232   unlockDelegate();
 233 }
 234
 235 class RemoveWithFlag
 236 {
 237 public:
 238   RemoveWithFlag(WayptFlag f) : flag(f), delCount(0) { }
 239
 240   int numDeleted() const { return delCount; }
 241
 242   bool operator()(FlightPlan::Leg* leg) const
 243   {
 244     if (leg->waypoint()->flag(flag)) {
 245       delete leg;
 246       ++delCount;
 247       return true;
 248     }
 249
 250     return false;
 251   }
 252 private:
 253   WayptFlag flag;
 254   mutable int delCount;
 255 };
 256
 257 int FlightPlan::clearWayptsWithFlag(WayptFlag flag)
 258 {
 259   int count = 0;
 260 // first pass, fix up currentIndex
 261   for (int i=0; i<_currentIndex; ++i) {
 262     Leg* l = _legs[i];
 263     if (l->waypoint()->flag(flag)) {
 264       ++count;
 265     }
 266   }
 267
 268   _currentIndex -= count;
 269
 270 // now delete and remove
 271   RemoveWithFlag rf(flag);
 272   LegVec::iterator it = std::remove_if(_legs.begin(), _legs.end(), rf);
 273   if (it == _legs.end()) {
 274     return 0; // nothing was cleared, don't fire the delegate
 275   }
 276
 277   lockDelegate();
 278   _waypointsChanged = true;
 279   if (count > 0) {
 280     _currentWaypointChanged = true;
 281   }
 282
 283   _legs.erase(it, _legs.end());
 284   unlockDelegate();
 285   return rf.numDeleted();
 286 }
 287
 288 void FlightPlan::setCurrentIndex(int index)
 289 {
 290   if ((index < -1) || (index >= numLegs())) {
 291     throw sg_range_exception("invalid leg index", "FlightPlan::setCurrentIndex");
 292   }
 293
 294   if (index == _currentIndex) {
 295     return;
 296   }
 297
 298   lockDelegate();
 299   _currentIndex = index;
 300   _currentWaypointChanged = true;
 301   unlockDelegate();
 302 }
 303
 304 int FlightPlan::findWayptIndex(const SGGeod& aPos) const
 305 {
 306   for (int i=0; i<numLegs(); ++i) {
 307     if (_legs[i]->waypoint()->matches(aPos)) {
 308       return i;
 309     }
 310   }
 311
 312   return -1;
 313 }
 314
 315 FlightPlan::Leg* FlightPlan::currentLeg() const
 316 {
 317   if ((_currentIndex < 0) || (_currentIndex >= numLegs()))
 318     return NULL;
 319   return legAtIndex(_currentIndex);
 320 }
 321
 322 FlightPlan::Leg* FlightPlan::previousLeg() const
 323 {
 324   if (_currentIndex <= 0) {
 325     return NULL;
 326   }
 327
 328   return legAtIndex(_currentIndex - 1);
 329 }
 330
 331 FlightPlan::Leg* FlightPlan::nextLeg() const
 332 {
 333   if ((_currentIndex < 0) || ((_currentIndex + 1) >= numLegs())) {
 334     return NULL;
 335   }
 336
 337   return legAtIndex(_currentIndex + 1);
 338 }
 339
 340 FlightPlan::Leg* FlightPlan::legAtIndex(int index) const
 341 {
 342   if ((index < 0) || (index >= numLegs())) {
 343     throw sg_range_exception("index out of range", "FlightPlan::legAtIndex");
 344   }
 345
 346   return _legs[index];
 347 }
 348
 349 int FlightPlan::findLegIndex(const Leg *l) const
 350 {
 351   for (unsigned int i=0; i<_legs.size(); ++i) {
 352     if (_legs[i] == l) {
 353       return i;
 354     }
 355   }
 356
 357   return -1;
 358 }
 359
 360 void FlightPlan::setDeparture(FGAirport* apt)
 361 {
 362   if (apt == _departure) {
 363     return;
 364   }
 365
 366   lockDelegate();
 367   _departureChanged = true;
 368   _departure = apt;
 369   _departureRunway = NULL;
 370   setSID((SID*)NULL);
 371   unlockDelegate();
 372 }
 373
 374 void FlightPlan::setDeparture(FGRunway* rwy)
 375 {
 376   if (_departureRunway == rwy) {
 377     return;
 378   }
 379
 380   lockDelegate();
 381   _departureChanged = true;
 382
 383   _departureRunway = rwy;
 384   if (rwy->airport() != _departure) {
 385     _departure = rwy->airport();
 386     setSID((SID*)NULL);
 387   }
 388   unlockDelegate();
 389 }
 390
 391 void FlightPlan::setSID(SID* sid, const std::string& transition)
 392 {
 393   if (sid == _sid) {
 394     return;
 395   }
 396
 397   lockDelegate();
 398   _departureChanged = true;
 399   _sid = sid;
 400   _sidTransition = transition;
 401   unlockDelegate();
 402 }
 403
 404 void FlightPlan::setSID(Transition* trans)
 405 {
 406   if (!trans) {
 407     setSID((SID*) NULL);
 408     return;
 409   }
 410
 411   if (trans->parent()->type() != PROCEDURE_SID)
 412     throw sg_exception("FlightPlan::setSID: transition does not belong to a SID");
 413
 414   setSID((SID*) trans->parent(), trans->ident());
 415 }
 416
 417 Transition* FlightPlan::sidTransition() const
 418 {
 419   if (!_sid || _sidTransition.empty()) {
 420     return NULL;
 421   }
 422
 423   return _sid->findTransitionByName(_sidTransition);
 424 }
 425
 426 void FlightPlan::setDestination(FGAirport* apt)
 427 {
 428   if (apt == _destination) {
 429     return;
 430   }
 431
 432   lockDelegate();
 433   _arrivalChanged = true;
 434   _destination = apt;
 435   _destinationRunway = NULL;
 436   setSTAR((STAR*)NULL);
 437   setApproach(NULL);
 438   unlockDelegate();
 439 }
 440
 441 void FlightPlan::setDestination(FGRunway* rwy)
 442 {
 443   if (_destinationRunway == rwy) {
 444     return;
 445   }
 446
 447   lockDelegate();
 448   _arrivalChanged = true;
 449   _destinationRunway = rwy;
 450   if (_destination != rwy->airport()) {
 451     _destination = rwy->airport();
 452     setSTAR((STAR*)NULL);
 453   }
 454
 455   unlockDelegate();
 456 }
 457
 458 void FlightPlan::setSTAR(STAR* star, const std::string& transition)
 459 {
 460   if (_star == star) {
 461     return;
 462   }
 463
 464   lockDelegate();
 465   _arrivalChanged = true;
 466   _star = star;
 467   _starTransition = transition;
 468   unlockDelegate();
 469 }
 470
 471 void FlightPlan::setSTAR(Transition* trans)
 472 {
 473   if (!trans) {
 474     setSTAR((STAR*) NULL);
 475     return;
 476   }
 477
 478   if (trans->parent()->type() != PROCEDURE_STAR)
 479     throw sg_exception("FlightPlan::setSTAR: transition does not belong to a STAR");
 480
 481   setSTAR((STAR*) trans->parent(), trans->ident());
 482 }
 483
 484 Transition* FlightPlan::starTransition() const
 485 {
 486   if (!_star || _starTransition.empty()) {
 487     return NULL;
 488   }
 489
 490   return _star->findTransitionByName(_starTransition);
 491 }
 492
 493 void FlightPlan::setApproach(flightgear::Approach *app)
 494 {
 495   if (_approach == app) {
 496     return;
 497   }
 498
 499   lockDelegate();
 500   _arrivalChanged = true;
 501   _approach = app;
 502   if (app) {
 503     // keep runway + airport in sync
 504     if (_destinationRunway != _approach->runway()) {
 505       _destinationRunway = _approach->runway();
 506     }
 507
 508     if (_destination != _destinationRunway->airport()) {
 509       _destination = _destinationRunway->airport();
 510     }
 511   }
 512   unlockDelegate();
 513 }
 514
 515 bool FlightPlan::save(const SGPath& path)
 516 {
 517   SG_LOG(SG_NAVAID, SG_INFO, "Saving route to " << path.str());
 518   try {
 519     SGPropertyNode_ptr d(new SGPropertyNode);
 520     d->setIntValue("version", 2);
 521
 522     if (_departure) {
 523       d->setStringValue("departure/airport", _departure->ident());
 524       if (_sid) {
 525         d->setStringValue("departure/sid", _sid->ident());
 526       }
 527
 528       if (_departureRunway) {
 529         d->setStringValue("departure/runway", _departureRunway->ident());
 530       }
 531     }
 532
 533     if (_destination) {
 534       d->setStringValue("destination/airport", _destination->ident());
 535       if (_star) {
 536         d->setStringValue("destination/star", _star->ident());
 537       }
 538
 539       if (_approach) {
 540         d->setStringValue("destination/approach", _approach->ident());
 541       }
 542
 543       //d->setStringValue("destination/transition", destination->getStringValue("transition"));
 544
 545       if (_destinationRunway) {
 546         d->setStringValue("destination/runway", _destinationRunway->ident());
 547       }
 548     }
 549
 550     // route nodes
 551     SGPropertyNode* routeNode = d->getChild("route", 0, true);
 552     for (unsigned int i=0; i<_legs.size(); ++i) {
 553       Waypt* wpt = _legs[i]->waypoint();
 554       wpt->saveAsNode(routeNode->getChild("wp", i, true));
 555     } // of waypoint iteration
 556     writeProperties(path.str(), d, true /* write-all */);
 557     return true;
 558   } catch (sg_exception& e) {
 559     SG_LOG(SG_NAVAID, SG_ALERT, "Failed to save flight-plan '" << path.str() << "'. " << e.getMessage());
 560     return false;
 561   }
 562 }
 563
 564 bool FlightPlan::load(const SGPath& path)
 565 {
 566   if (!path.exists())
 567   {
 568     SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load flight-plan '" << path.str()
 569            << "'. The file does not exist.");
 570     return false;
 571   }
 572
 573   SGPropertyNode_ptr routeData(new SGPropertyNode);
 574   SG_LOG(SG_NAVAID, SG_INFO, "going to read flight-plan from:" << path.str());
 575
 576   bool Status = false;
 577   lockDelegate();
 578   try {
 579     readProperties(path.str(), routeData);
 580   } catch (sg_exception& ) {
 581     // if XML parsing fails, the file might be simple textual list of waypoints
 582     Status = loadPlainTextRoute(path);
 583     routeData = 0;
 584     _waypointsChanged = true;
 585   }
 586
 587   if (routeData.valid())
 588   {
 589     try {
 590       int version = routeData->getIntValue("version", 1);
 591       if (version == 1) {
 592         loadVersion1XMLRoute(routeData);
 593       } else if (version == 2) {
 594         loadVersion2XMLRoute(routeData);
 595       } else {
 596         throw sg_io_exception("unsupported XML route version");
 597       }
 598       Status = true;
 599     } catch (sg_exception& e) {
 600       SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load flight-plan '" << e.getOrigin()
 601              << "'. " << e.getMessage());
 602       Status = false;
 603     }
 604   }
 605
 606   unlockDelegate();
 607   return Status;
 608 }
 609
 610 void FlightPlan::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
 611 {
 612   // departure nodes
 613   SGPropertyNode* dep = routeData->getChild("departure");
 614   if (dep) {
 615     string depIdent = dep->getStringValue("airport");
 616     setDeparture((FGAirport*) fgFindAirportID(depIdent));
 617     if (_departure) {
 618       string rwy(dep->getStringValue("runway"));
 619       if (_departure->hasRunwayWithIdent(rwy)) {
 620         setDeparture(_departure->getRunwayByIdent(rwy));
 621       }
 622
 623       if (dep->hasChild("sid")) {
 624         setSID(_departure->findSIDWithIdent(dep->getStringValue("sid")));
 625       }
 626    // departure->setStringValue("transition", dep->getStringValue("transition"));
 627     }
 628   }
 629
 630   // destination
 631   SGPropertyNode* dst = routeData->getChild("destination");
 632   if (dst) {
 633     setDestination((FGAirport*) fgFindAirportID(dst->getStringValue("airport")));
 634     if (_destination) {
 635       string rwy(dst->getStringValue("runway"));
 636       if (_destination->hasRunwayWithIdent(rwy)) {
 637         setDestination(_destination->getRunwayByIdent(rwy));
 638       }
 639
 640       if (dst->hasChild("star")) {
 641         setSTAR(_destination->findSTARWithIdent(dst->getStringValue("star")));
 642       }
 643
 644       if (dst->hasChild("approach")) {
 645         setApproach(_destination->findApproachWithIdent(dst->getStringValue("approach")));
 646       }
 647     }
 648
 649    // destination->setStringValue("transition", dst->getStringValue("transition"));
 650   }
 651
 652   // alternate
 653   SGPropertyNode* alt = routeData->getChild("alternate");
 654   if (alt) {
 655     //alternate->setStringValue(alt->getStringValue("airport"));
 656   }
 657
 658   // cruise
 659   SGPropertyNode* crs = routeData->getChild("cruise");
 660   if (crs) {
 661  //   cruise->setDoubleValue("speed-kts", crs->getDoubleValue("speed-kts"));
 662    // cruise->setDoubleValue("mach", crs->getDoubleValue("mach"));
 663    // cruise->setDoubleValue("altitude-ft", crs->getDoubleValue("altitude-ft"));
 664   } // of cruise data loading
 665
 666 }
 667
 668 void FlightPlan::loadVersion2XMLRoute(SGPropertyNode_ptr routeData)
 669 {
 670   loadXMLRouteHeader(routeData);
 671
 672   // route nodes
 673   _legs.clear();
 674   SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
 675   for (int i=0; i<routeNode->nChildren(); ++i) {
 676     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
 677     Leg* l = new Leg(this, Waypt::createFromProperties(NULL, wpNode));
 678     _legs.push_back(l);
 679   } // of route iteration
 680   _waypointsChanged = true;
 681 }
 682
 683 void FlightPlan::loadVersion1XMLRoute(SGPropertyNode_ptr routeData)
 684 {
 685   loadXMLRouteHeader(routeData);
 686
 687   // _legs nodes
 688   _legs.clear();
 689   SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
 690   for (int i=0; i<routeNode->nChildren(); ++i) {
 691     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
 692     Leg* l = new Leg(this, parseVersion1XMLWaypt(wpNode));
 693     _legs.push_back(l);
 694   } // of route iteration
 695   _waypointsChanged = true;
 696 }
 697
 698 WayptRef FlightPlan::parseVersion1XMLWaypt(SGPropertyNode* aWP)
 699 {
 700   SGGeod lastPos;
 701   if (!_legs.empty()) {
 702     lastPos = _legs.back()->waypoint()->position();
 703   } else if (_departure) {
 704     lastPos = _departure->geod();
 705   }
 706
 707   WayptRef w;
 708   string ident(aWP->getStringValue("ident"));
 709   if (aWP->hasChild("longitude-deg")) {
 710     // explicit longitude/latitude
 711     w = new BasicWaypt(SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"),
 712                                        aWP->getDoubleValue("latitude-deg")), ident, NULL);
 713
 714   } else {
 715     string nid = aWP->getStringValue("navid", ident.c_str());
 716     FGPositionedRef p = FGPositioned::findClosestWithIdent(nid, lastPos);
 717     SGGeod pos;
 718
 719     if (p) {
 720       pos = p->geod();
 721     } else {
 722       SG_LOG(SG_GENERAL, SG_WARN, "unknown navaid in flightplan:" << nid);
 723       pos = SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"),
 724                             aWP->getDoubleValue("latitude-deg"));
 725     }
 726
 727     if (aWP->hasChild("offset-nm") && aWP->hasChild("offset-radial")) {
 728       double radialDeg = aWP->getDoubleValue("offset-radial");
 729       // convert magnetic radial to a true radial!
 730       radialDeg += magvarDegAt(pos);
 731       double offsetNm = aWP->getDoubleValue("offset-nm");
 732       double az2;
 733       SGGeodesy::direct(pos, radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
 734     }
 735
 736     w = new BasicWaypt(pos, ident, NULL);
 737   }
 738
 739   double altFt = aWP->getDoubleValue("altitude-ft", -9999.9);
 740   if (altFt > -9990.0) {
 741     w->setAltitude(altFt, RESTRICT_AT);
 742   }
 743
 744   return w;
 745 }
 746
 747 bool FlightPlan::loadPlainTextRoute(const SGPath& path)
 748 {
 749   try {
 750     sg_gzifstream in(path.str().c_str());
 751     if (!in.is_open()) {
 752       throw sg_io_exception("Cannot open file for reading.");
 753     }
 754
 755     _legs.clear();
 756     while (!in.eof()) {
 757       string line;
 758       getline(in, line, '\n');
 759       // trim CR from end of line, if found
 760       if (line[line.size() - 1] == '\r') {
 761         line.erase(line.size() - 1, 1);
 762       }
 763
 764       line = simgear::strutils::strip(line);
 765       if (line.empty() || (line[0] == '#')) {
 766         continue; // ignore empty/comment lines
 767       }
 768
 769       WayptRef w = waypointFromString(line);
 770       if (!w) {
 771         throw sg_io_exception("Failed to create waypoint from line '" + line + "'.");
 772       }
 773
 774       _legs.push_back(new Leg(this, w));
 775     } // of line iteration
 776   } catch (sg_exception& e) {
 777     SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load route from: '" << path.str() << "'. " << e.getMessage());
 778     _legs.clear();
 779     return false;
 780   }
 781
 782   return true;
 783 }
 784
 785 double FlightPlan::magvarDegAt(const SGGeod& pos) const
 786 {
 787   double jd = globals->get_time_params()->getJD();
 788   return sgGetMagVar(pos, jd) * SG_RADIANS_TO_DEGREES;
 789 }
 790
 791 WayptRef FlightPlan::waypointFromString(const string& tgt )
 792 {
 793   string target(boost::to_upper_copy(tgt));
 794   WayptRef wpt;
 795
 796   // extract altitude
 797   double altFt = 0.0;
 798   RouteRestriction altSetting = RESTRICT_NONE;
 799
 800   size_t pos = target.find( '@' );
 801   if ( pos != string::npos ) {
 802     altFt = atof( target.c_str() + pos + 1 );
 803     target = target.substr( 0, pos );
 804     if ( !strcmp(fgGetString("/sim/startup/units"), "meter") )
 805       altFt *= SG_METER_TO_FEET;
 806     altSetting = RESTRICT_AT;
 807   }
 808
 809   // check for lon,lat
 810   pos = target.find( ',' );
 811   if ( pos != string::npos ) {
 812     double lon = atof( target.substr(0, pos).c_str());
 813     double lat = atof( target.c_str() + pos + 1);
 814     char buf[32];
 815     char ew = (lon < 0.0) ? 'W' : 'E';
 816     char ns = (lat < 0.0) ? 'S' : 'N';
 817     snprintf(buf, 32, "%c%03d%c%03d", ew, (int) fabs(lon), ns, (int)fabs(lat));
 818
 819     wpt = new BasicWaypt(SGGeod::fromDeg(lon, lat), buf, NULL);
 820     if (altSetting != RESTRICT_NONE) {
 821       wpt->setAltitude(altFt, altSetting);
 822     }
 823     return wpt;
 824   }
 825
 826   SGGeod basePosition;
 827   if (_legs.empty()) {
 828     // route is empty, use current position
 829     basePosition = globals->get_aircraft_position();
 830   } else {
 831     basePosition = _legs.back()->waypoint()->position();
 832   }
 833
 834   string_list pieces(simgear::strutils::split(target, "/"));
 835   FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition);
 836   if (!p) {
 837     SG_LOG( SG_NAVAID, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
 838     return NULL;
 839   }
 840
 841   double magvar = magvarDegAt(basePosition);
 842
 843   if (pieces.size() == 1) {
 844     wpt = new NavaidWaypoint(p, NULL);
 845   } else if (pieces.size() == 3) {
 846     // navaid/radial/distance-nm notation
 847     double radial = atof(pieces[1].c_str()),
 848     distanceNm = atof(pieces[2].c_str());
 849     radial += magvar;
 850     wpt = new OffsetNavaidWaypoint(p, NULL, radial, distanceNm);
 851   } else if (pieces.size() == 2) {
 852     FGAirport* apt = dynamic_cast<FGAirport*>(p.ptr());
 853     if (!apt) {
 854       SG_LOG(SG_NAVAID, SG_INFO, "Waypoint is not an airport:" << pieces.front());
 855       return NULL;
 856     }
 857
 858     if (!apt->hasRunwayWithIdent(pieces[1])) {
 859       SG_LOG(SG_NAVAID, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
 860       return NULL;
 861     }
 862
 863     FGRunway* runway = apt->getRunwayByIdent(pieces[1]);
 864     wpt = new NavaidWaypoint(runway, NULL);
 865   } else if (pieces.size() == 4) {
 866     // navid/radial/navid/radial notation
 867     FGPositionedRef p2 = FGPositioned::findClosestWithIdent(pieces[2], basePosition);
 868     if (!p2) {
 869       SG_LOG( SG_NAVAID, SG_INFO, "Unable to find FGPositioned with ident:" << pieces[2]);
 870       return NULL;
 871     }
 872
 873     double r1 = atof(pieces[1].c_str()),
 874     r2 = atof(pieces[3].c_str());
 875     r1 += magvar;
 876     r2 += magvar;
 877
 878     SGGeod intersection;
 879     bool ok = SGGeodesy::radialIntersection(p->geod(), r1, p2->geod(), r2, intersection);
 880     if (!ok) {
 881       SG_LOG(SG_NAVAID, SG_INFO, "no valid intersection for:" << target);
 882       return NULL;
 883     }
 884
 885     std::string name = p->ident() + "-" + p2->ident();
 886     wpt = new BasicWaypt(intersection, name, NULL);
 887   }
 888
 889   if (!wpt) {
 890     SG_LOG(SG_NAVAID, SG_INFO, "Unable to parse waypoint:" << target);
 891     return NULL;
 892   }
 893
 894   if (altSetting != RESTRICT_NONE) {
 895     wpt->setAltitude(altFt, altSetting);
 896   }
 897   return wpt;
 898 }
 899
 900 FlightPlan::Leg::Leg(FlightPlan* owner, WayptRef wpt) :
 901   _parent(owner),
 902   _speedRestrict(RESTRICT_NONE),
 903   _altRestrict(RESTRICT_NONE),
 904   _waypt(wpt)
 905 {
 906   if (!wpt.valid()) {
 907     throw sg_exception("can't create FlightPlan::Leg without underlying waypoint");
 908   }
 909   _speed = _altitudeFt = 0;
 910 }
 911
 912 FlightPlan::Leg* FlightPlan::Leg::cloneFor(FlightPlan* owner) const
 913 {
 914   Leg* c = new Leg(owner, _waypt);
 915 // clone local data
 916   c->_speed = _speed;
 917   c->_speedRestrict = _speedRestrict;
 918   c->_altitudeFt = _altitudeFt;
 919   c->_altRestrict = _altRestrict;
 920
 921   return c;
 922 }
 923
 924 FlightPlan::Leg* FlightPlan::Leg::nextLeg() const
 925 {
 926   return _parent->legAtIndex(index() + 1);
 927 }
 928
 929 unsigned int FlightPlan::Leg::index() const
 930 {
 931   return _parent->findLegIndex(this);
 932 }
 933
 934 int FlightPlan::Leg::altitudeFt() const
 935 {
 936   if (_altRestrict != RESTRICT_NONE) {
 937     return _altitudeFt;
 938   }
 939
 940   return _waypt->altitudeFt();
 941 }
 942
 943 int FlightPlan::Leg::speed() const
 944 {
 945   if (_speedRestrict != RESTRICT_NONE) {
 946     return _speed;
 947   }
 948
 949   return _waypt->speed();
 950 }
 951
 952 int FlightPlan::Leg::speedKts() const
 953 {
 954   return speed();
 955 }
 956
 957 double FlightPlan::Leg::speedMach() const
 958 {
 959   if (!isMachRestrict(_speedRestrict)) {
 960     return 0.0;
 961   }
 962
 963   return -(_speed / 100.0);
 964 }
 965
 966 RouteRestriction FlightPlan::Leg::altitudeRestriction() const
 967 {
 968   if (_altRestrict != RESTRICT_NONE) {
 969     return _altRestrict;
 970   }
 971
 972   return _waypt->altitudeRestriction();
 973 }
 974
 975 RouteRestriction FlightPlan::Leg::speedRestriction() const
 976 {
 977   if (_speedRestrict != RESTRICT_NONE) {
 978     return _speedRestrict;
 979   }
 980
 981   return _waypt->speedRestriction();
 982 }
 983
 984 void FlightPlan::Leg::setSpeed(RouteRestriction ty, double speed)
 985 {
 986   _speedRestrict = ty;
 987   if (isMachRestrict(ty)) {
 988     _speed = (speed * -100);
 989   } else {
 990     _speed = speed;
 991   }
 992 }
 993
 994 void FlightPlan::Leg::setAltitude(RouteRestriction ty, int altFt)
 995 {
 996   _altRestrict = ty;
 997   _altitudeFt = altFt;
 998 }
 999
1000 double FlightPlan::Leg::courseDeg() const
1001 {
1002   return _courseDeg;
1003 }
1004
1005 double FlightPlan::Leg::distanceNm() const
1006 {
1007   return _pathDistance;
1008 }
1009
1010 double FlightPlan::Leg::distanceAlongRoute() const
1011 {
1012   return _distanceAlongPath;
1013 }
1014
1015 void FlightPlan::rebuildLegData()
1016 {
1017   _totalDistance = 0.0;
1018   int lastLeg = static_cast<int>(_legs.size()) - 1;
1019   for (int l=0; l<lastLeg; ++l) {
1020     Leg* cur = _legs[l];
1021     Leg* next = _legs[l + 1];
1022
1023     std::pair<double, double> crsDist =
1024       next->waypoint()->courseAndDistanceFrom(cur->waypoint()->position());
1025     _legs[l]->_courseDeg = crsDist.first;
1026     _legs[l]->_pathDistance = crsDist.second * SG_METER_TO_NM;
1027     _legs[l]->_distanceAlongPath = _totalDistance;
1028     _totalDistance += crsDist.second * SG_METER_TO_NM;
1029   } // of legs iteration
1030
1031 // set some data on the final leg
1032   if (lastLeg > 0) {
1033     // keep the same course as the final leg, when passing the final
1034     // waypoint
1035     _legs[lastLeg]->_courseDeg = _legs[lastLeg - 1]->_courseDeg;
1036     _legs[lastLeg]->_pathDistance = 0.0;
1037     _legs[lastLeg]->_distanceAlongPath = _totalDistance;
1038   }
1039 }
1040
1041 SGGeod FlightPlan::pointAlongRoute(int aIndex, double aOffsetNm) const
1042 {
1043   if (aIndex >= (int) _legs.size()) {
1044     throw sg_range_exception();
1045   }
1046
1047   const int lastLeg = static_cast<int>(_legs.size()) - 1;
1048 // convert the relative offset and leg index into an absolute, positive
1049 // distance in nm from the route origin. This means we can simply walk
1050 // forwards to find the actual leg.
1051   Leg* leg = _legs[(aIndex >= 0) ? aIndex : lastLeg];
1052   double absolutePathDistance = leg->_distanceAlongPath + aOffsetNm;
1053   if (absolutePathDistance < 0.0) {
1054     return _legs[0]->waypoint()->position(); // begining of route
1055   }
1056
1057   if (absolutePathDistance > _totalDistance) {
1058     return _legs[lastLeg]->waypoint()->position(); // end of route
1059   }
1060
1061 // find the leg containing the absolute distance
1062   for (int l=0; l<lastLeg; ++l) {
1063     leg = _legs[l];
1064     if (absolutePathDistance < leg->_pathDistance) {
1065       break; // found our matching leg
1066     }
1067     absolutePathDistance -= leg->_pathDistance;
1068   } // of forwards walk along route to find leg
1069
1070   return SGGeodesy::direct(leg->waypoint()->position(),
1071                                leg->_courseDeg, absolutePathDistance * SG_NM_TO_METER);
1072 }
1073
1074 void FlightPlan::lockDelegate()
1075 {
1076   if (_delegateLock == 0) {
1077     assert(!_departureChanged && !_arrivalChanged &&
1078            !_waypointsChanged && !_currentWaypointChanged);
1079   }
1080
1081   ++_delegateLock;
1082 }
1083
1084 void FlightPlan::unlockDelegate()
1085 {
1086   assert(_delegateLock > 0);
1087   if (_delegateLock > 1) {
1088     --_delegateLock;
1089     return;
1090   }
1091
1092   if (_departureChanged) {
1093     _departureChanged = false;
1094     if (_delegate) {
1095       _delegate->runDepartureChanged();
1096     }
1097   }
1098
1099   if (_arrivalChanged) {
1100     _arrivalChanged = false;
1101     if (_delegate) {
1102       _delegate->runArrivalChanged();
1103     }
1104   }
1105
1106   if (_waypointsChanged) {
1107     _waypointsChanged = false;
1108     rebuildLegData();
1109     if (_delegate) {
1110       _delegate->runWaypointsChanged();
1111     }
1112   }
1113
1114   if (_currentWaypointChanged) {
1115     _currentWaypointChanged = false;
1116     if (_delegate) {
1117       _delegate->runCurrentWaypointChanged();
1118     }
1119   }
1120
1121   --_delegateLock;
1122 }
1123
1124 void FlightPlan::registerDelegateFactory(DelegateFactory* df)
1125 {
1126   FPDelegateFactoryVec::iterator it = std::find(static_delegateFactories.begin(),
1127                                                 static_delegateFactories.end(), df);
1128   if (it != static_delegateFactories.end()) {
1129     throw  sg_exception("duplicate delegate factory registration");
1130   }
1131
1132   static_delegateFactories.push_back(df);
1133 }
1134
1135 void FlightPlan::unregisterDelegateFactory(DelegateFactory* df)
1136 {
1137   FPDelegateFactoryVec::iterator it = std::find(static_delegateFactories.begin(),
1138                                                 static_delegateFactories.end(), df);
1139   if (it == static_delegateFactories.end()) {
1140     return;
1141   }
1142
1143   static_delegateFactories.erase(it);
1144 }
1145
1146 void FlightPlan::addDelegate(Delegate* d)
1147 {
1148   // wrap any existing delegate(s) in the new one
1149   d->_inner = _delegate;
1150   _delegate = d;
1151 }
1152
1153 void FlightPlan::removeDelegate(Delegate* d)
1154 {
1155   if (d == _delegate) {
1156     _delegate = _delegate->_inner;
1157   } else if (_delegate) {
1158     _delegate->removeInner(d);
1159   }
1160 }
1161
1162 FlightPlan::Delegate::Delegate() :
1163   _deleteWithPlan(false),
1164   _inner(NULL)
1165 {
1166
1167 }
1168
1169 FlightPlan::Delegate::~Delegate()
1170 {
1171
1172 }
1173
1174 void FlightPlan::Delegate::removeInner(Delegate* d)
1175 {
1176   if (!_inner) {
1177     return;
1178   }
1179
1180   if (_inner == d) {
1181     // replace with grand-child
1182     _inner = d->_inner;
1183   } else { // recurse downwards
1184     _inner->removeInner(d);
1185   }
1186 }
1187
1188 void FlightPlan::Delegate::runDepartureChanged()
1189 {
1190   if (_inner) _inner->runDepartureChanged();
1191   departureChanged();
1192 }
1193
1194 void FlightPlan::Delegate::runArrivalChanged()
1195 {
1196   if (_inner) _inner->runArrivalChanged();
1197   arrivalChanged();
1198 }
1199
1200 void FlightPlan::Delegate::runWaypointsChanged()
1201 {
1202   if (_inner) _inner->runWaypointsChanged();
1203   waypointsChanged();
1204 }
1205
1206 void FlightPlan::Delegate::runCurrentWaypointChanged()
1207 {
1208   if (_inner) _inner->runCurrentWaypointChanged();
1209   currentWaypointChanged();
1210 }
1211
1212 void FlightPlan::Delegate::runCleared()
1213 {
1214   if (_inner) _inner->runCleared();
1215   cleared();
1216 }
1217
1218 } // of namespace flightgear