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