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