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