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   for (int i=0; i<routeNode->nChildren(); ++i) {
 833     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
 834     Leg* l = new Leg(this, Waypt::createFromProperties(NULL, wpNode));
 835     _legs.push_back(l);
 836   } // of route iteration
 837   _waypointsChanged = true;
 838 }
 839
 840 void FlightPlan::loadVersion1XMLRoute(SGPropertyNode_ptr routeData)
 841 {
 842   loadXMLRouteHeader(routeData);
 843
 844   // _legs nodes
 845   _legs.clear();
 846   SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
 847   for (int i=0; i<routeNode->nChildren(); ++i) {
 848     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
 849     Leg* l = new Leg(this, parseVersion1XMLWaypt(wpNode));
 850     _legs.push_back(l);
 851   } // of route iteration
 852   _waypointsChanged = true;
 853 }
 854
 855 WayptRef FlightPlan::parseVersion1XMLWaypt(SGPropertyNode* aWP)
 856 {
 857   SGGeod lastPos;
 858   if (!_legs.empty()) {
 859     lastPos = _legs.back()->waypoint()->position();
 860   } else if (_departure) {
 861     lastPos = _departure->geod();
 862   }
 863
 864   WayptRef w;
 865   string ident(aWP->getStringValue("ident"));
 866   if (aWP->hasChild("longitude-deg")) {
 867     // explicit longitude/latitude
 868     w = new BasicWaypt(SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"),
 869                                        aWP->getDoubleValue("latitude-deg")), ident, NULL);
 870
 871   } else {
 872     string nid = aWP->getStringValue("navid", ident.c_str());
 873     FGPositionedRef p = FGPositioned::findClosestWithIdent(nid, lastPos);
 874     SGGeod pos;
 875
 876     if (p) {
 877       pos = p->geod();
 878     } else {
 879       SG_LOG(SG_GENERAL, SG_WARN, "unknown navaid in flightplan:" << nid);
 880       pos = SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"),
 881                             aWP->getDoubleValue("latitude-deg"));
 882     }
 883
 884     if (aWP->hasChild("offset-nm") && aWP->hasChild("offset-radial")) {
 885       double radialDeg = aWP->getDoubleValue("offset-radial");
 886       // convert magnetic radial to a true radial!
 887       radialDeg += magvarDegAt(pos);
 888       double offsetNm = aWP->getDoubleValue("offset-nm");
 889       double az2;
 890       SGGeodesy::direct(pos, radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
 891     }
 892
 893     w = new BasicWaypt(pos, ident, NULL);
 894   }
 895
 896   double altFt = aWP->getDoubleValue("altitude-ft", -9999.9);
 897   if (altFt > -9990.0) {
 898     w->setAltitude(altFt, RESTRICT_AT);
 899   }
 900
 901   return w;
 902 }
 903
 904 /** Load a flightplan in FlightGear plain-text format */
 905 bool FlightPlan::loadPlainTextFormat(const SGPath& path)
 906 {
 907   try {
 908     sg_gzifstream in(path.str().c_str());
 909     if (!in.is_open()) {
 910       throw sg_io_exception("Cannot open file for reading.");
 911     }
 912
 913     _legs.clear();
 914     while (!in.eof()) {
 915       string line;
 916       getline(in, line, '\n');
 917       // trim CR from end of line, if found
 918       if (line[line.size() - 1] == '\r') {
 919         line.erase(line.size() - 1, 1);
 920       }
 921
 922       line = simgear::strutils::strip(line);
 923       if (line.empty() || (line[0] == '#')) {
 924         continue; // ignore empty/comment lines
 925       }
 926
 927       WayptRef w = waypointFromString(line);
 928       if (!w) {
 929         throw sg_io_exception("Failed to create waypoint from line '" + line + "'.");
 930       }
 931
 932       _legs.push_back(new Leg(this, w));
 933     } // of line iteration
 934   } catch (sg_exception& e) {
 935     SG_LOG(SG_NAVAID, SG_ALERT, "Failed to load route from: '" << path.str() << "'. " << e.getMessage());
 936     _legs.clear();
 937     return false;
 938   }
 939
 940   return true;
 941 }
 942
 943 double FlightPlan::magvarDegAt(const SGGeod& pos) const
 944 {
 945   double jd = globals->get_time_params()->getJD();
 946   return sgGetMagVar(pos, jd) * SG_RADIANS_TO_DEGREES;
 947 }
 948
 949 WayptRef FlightPlan::waypointFromString(const string& tgt )
 950 {
 951   string target(boost::to_upper_copy(tgt));
 952   WayptRef wpt;
 953
 954   // extract altitude
 955   double altFt = 0.0;
 956   RouteRestriction altSetting = RESTRICT_NONE;
 957
 958   size_t pos = target.find( '@' );
 959   if ( pos != string::npos ) {
 960     altFt = atof( target.c_str() + pos + 1 );
 961     target = target.substr( 0, pos );
 962     if ( !strcmp(fgGetString("/sim/startup/units"), "meter") )
 963       altFt *= SG_METER_TO_FEET;
 964     altSetting = RESTRICT_AT;
 965   }
 966
 967   // check for lon,lat
 968   pos = target.find( ',' );
 969   if ( pos != string::npos ) {
 970     double lon = atof( target.substr(0, pos).c_str());
 971     double lat = atof( target.c_str() + pos + 1);
 972     char buf[32];
 973     char ew = (lon < 0.0) ? 'W' : 'E';
 974     char ns = (lat < 0.0) ? 'S' : 'N';
 975     snprintf(buf, 32, "%c%03d%c%03d", ew, (int) fabs(lon), ns, (int)fabs(lat));
 976
 977     wpt = new BasicWaypt(SGGeod::fromDeg(lon, lat), buf, NULL);
 978     if (altSetting != RESTRICT_NONE) {
 979       wpt->setAltitude(altFt, altSetting);
 980     }
 981     return wpt;
 982   }
 983
 984   SGGeod basePosition;
 985   if (_legs.empty()) {
 986     // route is empty, use current position
 987     basePosition = globals->get_aircraft_position();
 988   } else {
 989     basePosition = _legs.back()->waypoint()->position();
 990   }
 991
 992   string_list pieces(simgear::strutils::split(target, "/"));
 993   FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition);
 994   if (!p) {
 995     SG_LOG( SG_NAVAID, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
 996     return NULL;
 997   }
 998
 999   double magvar = magvarDegAt(basePosition);
1000
1001   if (pieces.size() == 1) {
1002     wpt = new NavaidWaypoint(p, NULL);
1003   } else if (pieces.size() == 3) {
1004     // navaid/radial/distance-nm notation
1005     double radial = atof(pieces[1].c_str()),
1006     distanceNm = atof(pieces[2].c_str());
1007     radial += magvar;
1008     wpt = new OffsetNavaidWaypoint(p, NULL, radial, distanceNm);
1009   } else if (pieces.size() == 2) {
1010     FGAirport* apt = dynamic_cast<FGAirport*>(p.ptr());
1011     if (!apt) {
1012       SG_LOG(SG_NAVAID, SG_INFO, "Waypoint is not an airport:" << pieces.front());
1013       return NULL;
1014     }
1015
1016     if (!apt->hasRunwayWithIdent(pieces[1])) {
1017       SG_LOG(SG_NAVAID, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
1018       return NULL;
1019     }
1020
1021     FGRunway* runway = apt->getRunwayByIdent(pieces[1]);
1022     wpt = new NavaidWaypoint(runway, NULL);
1023   } else if (pieces.size() == 4) {
1024     // navid/radial/navid/radial notation
1025     FGPositionedRef p2 = FGPositioned::findClosestWithIdent(pieces[2], basePosition);
1026     if (!p2) {
1027       SG_LOG( SG_NAVAID, SG_INFO, "Unable to find FGPositioned with ident:" << pieces[2]);
1028       return NULL;
1029     }
1030
1031     double r1 = atof(pieces[1].c_str()),
1032     r2 = atof(pieces[3].c_str());
1033     r1 += magvar;
1034     r2 += magvar;
1035
1036     SGGeod intersection;
1037     bool ok = SGGeodesy::radialIntersection(p->geod(), r1, p2->geod(), r2, intersection);
1038     if (!ok) {
1039       SG_LOG(SG_NAVAID, SG_INFO, "no valid intersection for:" << target);
1040       return NULL;
1041     }
1042
1043     std::string name = p->ident() + "-" + p2->ident();
1044     wpt = new BasicWaypt(intersection, name, NULL);
1045   }
1046
1047   if (!wpt) {
1048     SG_LOG(SG_NAVAID, SG_INFO, "Unable to parse waypoint:" << target);
1049     return NULL;
1050   }
1051
1052   if (altSetting != RESTRICT_NONE) {
1053     wpt->setAltitude(altFt, altSetting);
1054   }
1055   return wpt;
1056 }
1057
1058 FlightPlan::Leg::Leg(FlightPlan* owner, WayptRef wpt) :
1059   _parent(owner),
1060   _speedRestrict(RESTRICT_NONE),
1061   _altRestrict(RESTRICT_NONE),
1062   _waypt(wpt)
1063 {
1064   if (!wpt.valid()) {
1065     throw sg_exception("can't create FlightPlan::Leg without underlying waypoint");
1066   }
1067   _speed = _altitudeFt = 0;
1068 }
1069
1070 FlightPlan::Leg* FlightPlan::Leg::cloneFor(FlightPlan* owner) const
1071 {
1072   Leg* c = new Leg(owner, _waypt);
1073 // clone local data
1074   c->_speed = _speed;
1075   c->_speedRestrict = _speedRestrict;
1076   c->_altitudeFt = _altitudeFt;
1077   c->_altRestrict = _altRestrict;
1078
1079   return c;
1080 }
1081
1082 FlightPlan::Leg* FlightPlan::Leg::nextLeg() const
1083 {
1084   return _parent->legAtIndex(index() + 1);
1085 }
1086
1087 unsigned int FlightPlan::Leg::index() const
1088 {
1089   return _parent->findLegIndex(this);
1090 }
1091
1092 int FlightPlan::Leg::altitudeFt() const
1093 {
1094   if (_altRestrict != RESTRICT_NONE) {
1095     return _altitudeFt;
1096   }
1097
1098   return _waypt->altitudeFt();
1099 }
1100
1101 int FlightPlan::Leg::speed() const
1102 {
1103   if (_speedRestrict != RESTRICT_NONE) {
1104     return _speed;
1105   }
1106
1107   return _waypt->speed();
1108 }
1109
1110 int FlightPlan::Leg::speedKts() const
1111 {
1112   return speed();
1113 }
1114
1115 double FlightPlan::Leg::speedMach() const
1116 {
1117   if (!isMachRestrict(_speedRestrict)) {
1118     return 0.0;
1119   }
1120
1121   return -(_speed / 100.0);
1122 }
1123
1124 RouteRestriction FlightPlan::Leg::altitudeRestriction() const
1125 {
1126   if (_altRestrict != RESTRICT_NONE) {
1127     return _altRestrict;
1128   }
1129
1130   return _waypt->altitudeRestriction();
1131 }
1132
1133 RouteRestriction FlightPlan::Leg::speedRestriction() const
1134 {
1135   if (_speedRestrict != RESTRICT_NONE) {
1136     return _speedRestrict;
1137   }
1138
1139   return _waypt->speedRestriction();
1140 }
1141
1142 void FlightPlan::Leg::setSpeed(RouteRestriction ty, double speed)
1143 {
1144   _speedRestrict = ty;
1145   if (isMachRestrict(ty)) {
1146     _speed = (speed * -100);
1147   } else {
1148     _speed = speed;
1149   }
1150 }
1151
1152 void FlightPlan::Leg::setAltitude(RouteRestriction ty, int altFt)
1153 {
1154   _altRestrict = ty;
1155   _altitudeFt = altFt;
1156 }
1157
1158 double FlightPlan::Leg::courseDeg() const
1159 {
1160   return _courseDeg;
1161 }
1162
1163 double FlightPlan::Leg::distanceNm() const
1164 {
1165   return _pathDistance;
1166 }
1167
1168 double FlightPlan::Leg::distanceAlongRoute() const
1169 {
1170   return _distanceAlongPath;
1171 }
1172
1173 void FlightPlan::rebuildLegData()
1174 {
1175   _totalDistance = 0.0;
1176   int lastLeg = static_cast<int>(_legs.size()) - 1;
1177   for (int l=0; l<lastLeg; ++l) {
1178     Leg* cur = _legs[l];
1179     Leg* next = _legs[l + 1];
1180
1181     std::pair<double, double> crsDist =
1182       next->waypoint()->courseAndDistanceFrom(cur->waypoint()->position());
1183     _legs[l]->_courseDeg = crsDist.first;
1184     _legs[l]->_pathDistance = crsDist.second * SG_METER_TO_NM;
1185     _legs[l]->_distanceAlongPath = _totalDistance;
1186     _totalDistance += crsDist.second * SG_METER_TO_NM;
1187   } // of legs iteration
1188
1189 // set some data on the final leg
1190   if (lastLeg > 0) {
1191     // keep the same course as the final leg, when passing the final
1192     // waypoint
1193     _legs[lastLeg]->_courseDeg = _legs[lastLeg - 1]->_courseDeg;
1194     _legs[lastLeg]->_pathDistance = 0.0;
1195     _legs[lastLeg]->_distanceAlongPath = _totalDistance;
1196   }
1197 }
1198
1199 SGGeod FlightPlan::pointAlongRoute(int aIndex, double aOffsetNm) const
1200 {
1201   if (aIndex >= (int) _legs.size()) {
1202     throw sg_range_exception();
1203   }
1204
1205   const int lastLeg = static_cast<int>(_legs.size()) - 1;
1206 // convert the relative offset and leg index into an absolute, positive
1207 // distance in nm from the route origin. This means we can simply walk
1208 // forwards to find the actual leg.
1209   Leg* leg = _legs[(aIndex >= 0) ? aIndex : lastLeg];
1210   double absolutePathDistance = leg->_distanceAlongPath + aOffsetNm;
1211   if (absolutePathDistance < 0.0) {
1212     return _legs[0]->waypoint()->position(); // begining of route
1213   }
1214
1215   if (absolutePathDistance > _totalDistance) {
1216     return _legs[lastLeg]->waypoint()->position(); // end of route
1217   }
1218
1219 // find the leg containing the absolute distance
1220   for (int l=0; l<lastLeg; ++l) {
1221     leg = _legs[l];
1222     if (absolutePathDistance < leg->_pathDistance) {
1223       break; // found our matching leg
1224     }
1225     absolutePathDistance -= leg->_pathDistance;
1226   } // of forwards walk along route to find leg
1227
1228   return SGGeodesy::direct(leg->waypoint()->position(),
1229                                leg->_courseDeg, absolutePathDistance * SG_NM_TO_METER);
1230 }
1231
1232 void FlightPlan::lockDelegate()
1233 {
1234   if (_delegateLock == 0) {
1235     assert(!_departureChanged && !_arrivalChanged &&
1236            !_waypointsChanged && !_currentWaypointChanged);
1237   }
1238
1239   ++_delegateLock;
1240 }
1241
1242 void FlightPlan::unlockDelegate()
1243 {
1244   assert(_delegateLock > 0);
1245   if (_delegateLock > 1) {
1246     --_delegateLock;
1247     return;
1248   }
1249
1250   if (_departureChanged) {
1251     _departureChanged = false;
1252     if (_delegate) {
1253       _delegate->runDepartureChanged();
1254     }
1255   }
1256
1257   if (_arrivalChanged) {
1258     _arrivalChanged = false;
1259     if (_delegate) {
1260       _delegate->runArrivalChanged();
1261     }
1262   }
1263
1264   if (_waypointsChanged) {
1265     _waypointsChanged = false;
1266     rebuildLegData();
1267     if (_delegate) {
1268       _delegate->runWaypointsChanged();
1269     }
1270   }
1271
1272   if (_currentWaypointChanged) {
1273     _currentWaypointChanged = false;
1274     if (_delegate) {
1275       _delegate->runCurrentWaypointChanged();
1276     }
1277   }
1278
1279   --_delegateLock;
1280 }
1281
1282 void FlightPlan::registerDelegateFactory(DelegateFactory* df)
1283 {
1284   FPDelegateFactoryVec::iterator it = std::find(static_delegateFactories.begin(),
1285                                                 static_delegateFactories.end(), df);
1286   if (it != static_delegateFactories.end()) {
1287     throw  sg_exception("duplicate delegate factory registration");
1288   }
1289
1290   static_delegateFactories.push_back(df);
1291 }
1292
1293 void FlightPlan::unregisterDelegateFactory(DelegateFactory* df)
1294 {
1295   FPDelegateFactoryVec::iterator it = std::find(static_delegateFactories.begin(),
1296                                                 static_delegateFactories.end(), df);
1297   if (it == static_delegateFactories.end()) {
1298     return;
1299   }
1300
1301   static_delegateFactories.erase(it);
1302 }
1303
1304 void FlightPlan::addDelegate(Delegate* d)
1305 {
1306   // wrap any existing delegate(s) in the new one
1307   d->_inner = _delegate;
1308   _delegate = d;
1309 }
1310
1311 void FlightPlan::removeDelegate(Delegate* d)
1312 {
1313   if (d == _delegate) {
1314     _delegate = _delegate->_inner;
1315   } else if (_delegate) {
1316     _delegate->removeInner(d);
1317   }
1318 }
1319
1320 FlightPlan::Delegate::Delegate() :
1321   _deleteWithPlan(false),
1322   _inner(NULL)
1323 {
1324 }
1325
1326 FlightPlan::Delegate::~Delegate()
1327 {
1328 }
1329
1330 void FlightPlan::Delegate::removeInner(Delegate* d)
1331 {
1332   if (!_inner) {
1333       throw sg_exception("FlightPlan delegate not found");
1334   }
1335
1336   if (_inner == d) {
1337     // replace with grand-child
1338     _inner = d->_inner;
1339   } else { // recurse downwards
1340     _inner->removeInner(d);
1341   }
1342 }
1343
1344 void FlightPlan::Delegate::runDepartureChanged()
1345 {
1346   if (_inner) _inner->runDepartureChanged();
1347   departureChanged();
1348 }
1349
1350 void FlightPlan::Delegate::runArrivalChanged()
1351 {
1352   if (_inner) _inner->runArrivalChanged();
1353   arrivalChanged();
1354 }
1355
1356 void FlightPlan::Delegate::runWaypointsChanged()
1357 {
1358   if (_inner) _inner->runWaypointsChanged();
1359   waypointsChanged();
1360 }
1361
1362 void FlightPlan::Delegate::runCurrentWaypointChanged()
1363 {
1364   if (_inner) _inner->runCurrentWaypointChanged();
1365   currentWaypointChanged();
1366 }
1367
1368 void FlightPlan::Delegate::runCleared()
1369 {
1370   if (_inner) _inner->runCleared();
1371   cleared();
1372 }
1373
1374 void FlightPlan::Delegate::runFinished()
1375 {
1376     if (_inner) _inner->runFinished();
1377     endOfFlightPlan();
1378 }
1379
1380 } // of namespace flightgear