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