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