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