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