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