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
  31 // Boost
  32 #include <boost/algorithm/string/case_conv.hpp>
  33 #include <boost/algorithm/string.hpp>
  34 #include <boost/foreach.hpp>
  35
  36 // SimGear
  37 #include <simgear/structure/exception.hxx>
  38 #include <simgear/misc/sg_path.hxx>
  39 #include <simgear/magvar/magvar.hxx>
  40 #include <simgear/timing/sg_time.hxx>
  41 #include <simgear/misc/sgstream.hxx>
  42 #include <simgear/misc/strutils.hxx>
  43 #include <simgear/props/props_io.hxx>
  44
  45 // FlightGear
  46 #include <Main/globals.hxx>
  47 #include "Main/fg_props.hxx"
  48 #include <Navaids/procedure.hxx>
  49 #include <Navaids/waypoint.hxx>
  50
  51 using std::string;
  52 using std::vector;
  53 using std::endl;
  54 using std::fstream;
  55
  56 namespace flightgear {
  57
  58 typedef std::vector<FlightPlan::DelegateFactory*> FPDelegateFactoryVec;
  59 static FPDelegateFactoryVec static_delegateFactories;
  60
  61 FlightPlan::FlightPlan() :
  62   _currentIndex(-1),
  63   _departureRunway(NULL),
  64   _destinationRunway(NULL),
  65   _sid(NULL),
  66   _star(NULL),
  67   _approach(NULL),
  68   _delegate(NULL)
  69 {
  70   BOOST_FOREACH(DelegateFactory* factory, static_delegateFactories) {
  71     Delegate* d = factory->createFlightPlanDelegate(this);
  72     if (d) { // factory might not always create a delegate
  73       d->_deleteWithPlan = true;
  74       addDelegate(d);
  75     }
  76   }
  77 }
  78
  79 FlightPlan::~FlightPlan()
  80 {
  81 // delete all delegates which we own.
  82   Delegate* d = _delegate;
  83   while (d) {
  84     Delegate* cur = d;
  85     d = d->_inner;
  86     if (cur->_deleteWithPlan) {
  87       delete cur;
  88     }
  89   }
  90 }
  91
  92 FlightPlan* FlightPlan::clone(const string& newIdent) const
  93 {
  94   FlightPlan* c = new FlightPlan();
  95   c->_ident = newIdent.empty() ? _ident : newIdent;
  96
  97 // copy destination / departure data.
  98   c->setDeparture(_departure);
  99   c->setDeparture(_departureRunway);
 100
 101   if (_approach) {
 102     c->setApproach(_approach);
 103   } else if (_destinationRunway) {
 104     c->setDestination(_destinationRunway);
 105   } else if (_destination) {
 106     c->setDestination(_destination);
 107   }
 108
 109   c->setSTAR(_star);
 110   c->setSID(_sid);
 111
 112 // copy legs
 113   for (int l=0; l < numLegs(); ++l) {
 114     c->_legs.push_back(_legs[l]->cloneFor(c));
 115   }
 116
 117   return c;
 118 }
 119
 120 void FlightPlan::setIdent(const string& s)
 121 {
 122   _ident = s;
 123 }
 124
 125 string FlightPlan::ident() const
 126 {
 127   return _ident;
 128 }
 129
 130 FlightPlan::Leg* FlightPlan::insertWayptAtIndex(Waypt* aWpt, int aIndex)
 131 {
 132   if (!aWpt) {
 133     return NULL;
 134   }
 135
 136   WayptVec wps;
 137   wps.push_back(aWpt);
 138
 139   int index = aIndex;
 140   if ((aIndex == -1) || (aIndex > (int) _legs.size())) {
 141     index = _legs.size();
 142   }
 143
 144   insertWayptsAtIndex(wps, index);
 145   return legAtIndex(aIndex);
 146 }
 147
 148 void FlightPlan::insertWayptsAtIndex(const WayptVec& wps, int aIndex)
 149 {
 150   if (wps.empty()) {
 151     return;
 152   }
 153
 154   int index = aIndex;
 155   if ((aIndex == -1) || (aIndex > (int) _legs.size())) {
 156     index = _legs.size();
 157   }
 158
 159   LegVec::iterator it = _legs.begin();
 160   it += index;
 161
 162   int endIndex = index + wps.size() - 1;
 163   if (_currentIndex >= endIndex) {
 164     _currentIndex += wps.size();
 165   }
 166
 167   LegVec newLegs;
 168   BOOST_FOREACH(WayptRef wp, wps) {
 169     newLegs.push_back(new Leg(this, wp));
 170   }
 171
 172   _legs.insert(it, newLegs.begin(), newLegs.end());
 173   rebuildLegData();
 174
 175   if (_delegate) {
 176     _delegate->runWaypointsChanged();
 177   }
 178 }
 179
 180 void FlightPlan::deleteIndex(int aIndex)
 181 {
 182   int index = aIndex;
 183   if (aIndex < 0) { // negative indices count the the end
 184     index = _legs.size() + index;
 185   }
 186
 187   if ((index < 0) || (index >= numLegs())) {
 188     SG_LOG(SG_AUTOPILOT, SG_WARN, "removeAtIndex with invalid index:" << aIndex);
 189     return;
 190   }
 191   LegVec::iterator it = _legs.begin();
 192   it += index;
 193   Leg* l = *it;
 194   _legs.erase(it);
 195   delete l;
 196
 197   bool curChanged = false;
 198   if (_currentIndex == index) {
 199     // current waypoint was removed
 200     curChanged = true;
 201   } else if (_currentIndex > index) {
 202     --_currentIndex; // shift current index down if necessary
 203   }
 204
 205   rebuildLegData();
 206   if (_delegate) {
 207     _delegate->runWaypointsChanged();
 208     if (curChanged) {
 209       _delegate->runCurrentWaypointChanged();
 210     }
 211   }
 212 }
 213
 214 void FlightPlan::clear()
 215 {
 216   _currentIndex = -1;
 217   BOOST_FOREACH(Leg* l, _legs) {
 218     delete l;
 219   }
 220   _legs.clear();
 221   rebuildLegData();
 222   if (_delegate) {
 223     _delegate->runDepartureChanged();
 224     _delegate->runArrivalChanged();
 225     _delegate->runWaypointsChanged();
 226     _delegate->runCurrentWaypointChanged();
 227   }
 228 }
 229
 230 int FlightPlan::clearWayptsWithFlag(WayptFlag flag)
 231 {
 232   int count = 0;
 233   for (unsigned int i=0; i<_legs.size(); ++i) {
 234     Leg* l = _legs[i];
 235     if (!l->waypoint()->flag(flag)) {
 236       continue;
 237     }
 238
 239   // okay, we're going to clear this leg
 240     ++count;
 241     if (_currentIndex > (int) i) {
 242       --_currentIndex;
 243     }
 244
 245     delete l;
 246     LegVec::iterator it = _legs.begin();
 247     it += i;
 248     _legs.erase(it);
 249   }
 250
 251   if (count == 0) {
 252     return 0; // nothing was cleared, don't fire the delegate
 253   }
 254
 255   rebuildLegData();
 256   if (_delegate) {
 257     _delegate->runWaypointsChanged();
 258     _delegate->runCurrentWaypointChanged();
 259   }
 260
 261   return count;
 262 }
 263
 264 void FlightPlan::setCurrentIndex(int index)
 265 {
 266   if ((index < -1) || (index >= numLegs())) {
 267     throw sg_range_exception("invalid leg index", "FlightPlan::setCurrentIndex");
 268   }
 269
 270   if (index == _currentIndex) {
 271     return;
 272   }
 273
 274   _currentIndex = index;
 275   if (_delegate) {
 276     _delegate->runCurrentWaypointChanged();
 277   }
 278 }
 279
 280 int FlightPlan::findWayptIndex(const SGGeod& aPos) const
 281 {
 282   for (int i=0; i<numLegs(); ++i) {
 283     if (_legs[i]->waypoint()->matches(aPos)) {
 284       return i;
 285     }
 286   }
 287
 288   return -1;
 289 }
 290
 291 FlightPlan::Leg* FlightPlan::currentLeg() const
 292 {
 293   if ((_currentIndex < 0) || (_currentIndex >= numLegs()))
 294     return NULL;
 295   return legAtIndex(_currentIndex);
 296 }
 297
 298 FlightPlan::Leg* FlightPlan::previousLeg() const
 299 {
 300   if (_currentIndex == 0) {
 301     return NULL;
 302   }
 303
 304   return legAtIndex(_currentIndex - 1);
 305 }
 306
 307 FlightPlan::Leg* FlightPlan::nextLeg() const
 308 {
 309   if ((_currentIndex < 0) || ((_currentIndex + 1) >= numLegs())) {
 310     return NULL;
 311   }
 312
 313   return legAtIndex(_currentIndex + 1);
 314 }
 315
 316 FlightPlan::Leg* FlightPlan::legAtIndex(int index) const
 317 {
 318   if ((index < 0) || (index >= numLegs())) {
 319     throw sg_range_exception("index out of range", "FlightPlan::legAtIndex");
 320   }
 321
 322   return _legs[index];
 323 }
 324
 325 int FlightPlan::findLegIndex(const Leg *l) const
 326 {
 327   for (unsigned int i=0; i<_legs.size(); ++i) {
 328     if (_legs[i] == l) {
 329       return i;
 330     }
 331   }
 332
 333   return -1;
 334 }
 335
 336 void FlightPlan::setDeparture(FGAirport* apt)
 337 {
 338   if (apt == _departure) {
 339     return;
 340   }
 341
 342   _departure = apt;
 343   _departureRunway = NULL;
 344   setSID((SID*)NULL);
 345
 346   if (_delegate) {
 347     _delegate->runDepartureChanged();
 348   }
 349 }
 350
 351 void FlightPlan::setDeparture(FGRunway* rwy)
 352 {
 353   if (_departureRunway == rwy) {
 354     return;
 355   }
 356
 357   _departureRunway = rwy;
 358   if (rwy->airport() != _departure) {
 359     _departure = rwy->airport();
 360     setSID((SID*)NULL);
 361   }
 362
 363   if (_delegate) {
 364     _delegate->runDepartureChanged();
 365   }
 366 }
 367
 368 void FlightPlan::setSID(SID* sid, const std::string& transition)
 369 {
 370   if (sid == _sid) {
 371     return;
 372   }
 373
 374   _sid = sid;
 375   _sidTransition = transition;
 376
 377   if (_delegate) {
 378     _delegate->runDepartureChanged();
 379   }
 380 }
 381
 382 void FlightPlan::setSID(Transition* trans)
 383 {
 384   if (!trans) {
 385     setSID((SID*) NULL);
 386     return;
 387   }
 388
 389   if (trans->parent()->type() != PROCEDURE_SID)
 390     throw sg_exception("FlightPlan::setSID: transition does not belong to a SID");
 391
 392   setSID((SID*) trans->parent(), trans->ident());
 393 }
 394
 395 Transition* FlightPlan::sidTransition() const
 396 {
 397   if (!_sid || _sidTransition.empty()) {
 398     return NULL;
 399   }
 400
 401   return _sid->findTransitionByName(_sidTransition);
 402 }
 403
 404 void FlightPlan::setDestination(FGAirport* apt)
 405 {
 406   if (apt == _destination) {
 407     return;
 408   }
 409
 410   _destination = apt;
 411   _destinationRunway = NULL;
 412   setSTAR((STAR*)NULL);
 413
 414   if (_delegate) {
 415     _delegate->runArrivalChanged();
 416   }
 417 }
 418
 419 void FlightPlan::setDestination(FGRunway* rwy)
 420 {
 421   if (_destinationRunway == rwy) {
 422     return;
 423   }
 424
 425   _destinationRunway = rwy;
 426   if (_destination != rwy->airport()) {
 427     _destination = rwy->airport();
 428     setSTAR((STAR*)NULL);
 429   }
 430
 431   if (_delegate) {
 432     _delegate->runArrivalChanged();
 433   }
 434 }
 435
 436 void FlightPlan::setSTAR(STAR* star, const std::string& transition)
 437 {
 438   if (_star == star) {
 439     return;
 440   }
 441
 442   _star = star;
 443   _starTransition = transition;
 444
 445   if (_delegate) {
 446     _delegate->runArrivalChanged();
 447   }
 448 }
 449
 450 void FlightPlan::setSTAR(Transition* trans)
 451 {
 452   if (!trans) {
 453     setSTAR((STAR*) NULL);
 454     return;
 455   }
 456
 457   if (trans->parent()->type() != PROCEDURE_STAR)
 458     throw sg_exception("FlightPlan::setSTAR: transition does not belong to a STAR");
 459
 460   setSTAR((STAR*) trans->parent(), trans->ident());
 461 }
 462
 463 Transition* FlightPlan::starTransition() const
 464 {
 465   if (!_star || _starTransition.empty()) {
 466     return NULL;
 467   }
 468
 469   return _star->findTransitionByName(_starTransition);
 470 }
 471
 472 void FlightPlan::setApproach(flightgear::Approach *app)
 473 {
 474   if (_approach == app) {
 475     return;
 476   }
 477
 478   _approach = app;
 479   if (app) {
 480     // keep runway + airport in sync
 481     if (_destinationRunway != _approach->runway()) {
 482       _destinationRunway = _approach->runway();
 483     }
 484
 485     if (_destination != _destinationRunway->airport()) {
 486       _destination = _destinationRunway->airport();
 487     }
 488   }
 489
 490   if (_delegate) {
 491     _delegate->runArrivalChanged();
 492   }
 493 }
 494
 495 bool FlightPlan::save(const SGPath& path)
 496 {
 497   SG_LOG(SG_IO, SG_INFO, "Saving route to " << path.str());
 498   try {
 499     SGPropertyNode_ptr d(new SGPropertyNode);
 500     d->setIntValue("version", 2);
 501
 502     if (_departure) {
 503       d->setStringValue("departure/airport", _departure->ident());
 504       if (_sid) {
 505         d->setStringValue("departure/sid", _sid->ident());
 506       }
 507
 508       if (_departureRunway) {
 509         d->setStringValue("departure/runway", _departureRunway->ident());
 510       }
 511     }
 512
 513     if (_destination) {
 514       d->setStringValue("destination/airport", _destination->ident());
 515       if (_star) {
 516         d->setStringValue("destination/star", _star->ident());
 517       }
 518
 519       if (_approach) {
 520         d->setStringValue("destination/approach", _approach->ident());
 521       }
 522
 523       //d->setStringValue("destination/transition", destination->getStringValue("transition"));
 524
 525       if (_destinationRunway) {
 526         d->setStringValue("destination/runway", _destinationRunway->ident());
 527       }
 528     }
 529
 530     // route nodes
 531     SGPropertyNode* routeNode = d->getChild("route", 0, true);
 532     for (unsigned int i=0; i<_legs.size(); ++i) {
 533       Waypt* wpt = _legs[i]->waypoint();
 534       wpt->saveAsNode(routeNode->getChild("wp", i, true));
 535     } // of waypoint iteration
 536     writeProperties(path.str(), d, true /* write-all */);
 537     return true;
 538   } catch (sg_exception& e) {
 539     SG_LOG(SG_IO, SG_ALERT, "Failed to save flight-plan '" << path.str() << "'. " << e.getMessage());
 540     return false;
 541   }
 542 }
 543
 544 bool FlightPlan::load(const SGPath& path)
 545 {
 546   if (!path.exists())
 547   {
 548     SG_LOG(SG_IO, SG_ALERT, "Failed to load flight-plan '" << path.str()
 549            << "'. The file does not exist.");
 550     return false;
 551   }
 552
 553   SGPropertyNode_ptr routeData(new SGPropertyNode);
 554   SG_LOG(SG_IO, SG_INFO, "going to read flight-plan from:" << path.str());
 555
 556   bool Status = false;
 557   try {
 558     readProperties(path.str(), routeData);
 559   } catch (sg_exception& ) {
 560     // if XML parsing fails, the file might be simple textual list of waypoints
 561     Status = loadPlainTextRoute(path);
 562     routeData = 0;
 563   }
 564
 565   if (routeData.valid())
 566   {
 567     try {
 568       int version = routeData->getIntValue("version", 1);
 569       if (version == 1) {
 570         loadVersion1XMLRoute(routeData);
 571       } else if (version == 2) {
 572         loadVersion2XMLRoute(routeData);
 573       } else {
 574         throw sg_io_exception("unsupported XML route version");
 575       }
 576       Status = true;
 577     } catch (sg_exception& e) {
 578       SG_LOG(SG_IO, SG_ALERT, "Failed to load flight-plan '" << e.getOrigin()
 579              << "'. " << e.getMessage());
 580       Status = false;
 581     }
 582   }
 583
 584   rebuildLegData();
 585   if (_delegate) {
 586     _delegate->runWaypointsChanged();
 587   }
 588
 589   return Status;
 590 }
 591
 592 void FlightPlan::loadXMLRouteHeader(SGPropertyNode_ptr routeData)
 593 {
 594   // departure nodes
 595   SGPropertyNode* dep = routeData->getChild("departure");
 596   if (dep) {
 597     string depIdent = dep->getStringValue("airport");
 598     setDeparture((FGAirport*) fgFindAirportID(depIdent));
 599     if (_departure) {
 600       if (dep->hasChild("runway")) {
 601         setDeparture(_departure->getRunwayByIdent(dep->getStringValue("runway")));
 602       }
 603
 604       if (dep->hasChild("sid")) {
 605         setSID(_departure->findSIDWithIdent(dep->getStringValue("sid")));
 606       }
 607    // departure->setStringValue("transition", dep->getStringValue("transition"));
 608     }
 609   }
 610
 611   // destination
 612   SGPropertyNode* dst = routeData->getChild("destination");
 613   if (dst) {
 614     setDestination((FGAirport*) fgFindAirportID(dst->getStringValue("airport")));
 615     if (_destination) {
 616       if (dst->hasChild("runway")) {
 617         setDestination(_destination->getRunwayByIdent(dst->getStringValue("runway")));
 618       }
 619
 620       if (dst->hasChild("star")) {
 621         setSTAR(_destination->findSTARWithIdent(dst->getStringValue("star")));
 622       }
 623
 624       if (dst->hasChild("approach")) {
 625         setApproach(_destination->findApproachWithIdent(dst->getStringValue("approach")));
 626       }
 627     }
 628
 629    // destination->setStringValue("transition", dst->getStringValue("transition"));
 630   }
 631
 632   // alternate
 633   SGPropertyNode* alt = routeData->getChild("alternate");
 634   if (alt) {
 635     //alternate->setStringValue(alt->getStringValue("airport"));
 636   }
 637
 638   // cruise
 639   SGPropertyNode* crs = routeData->getChild("cruise");
 640   if (crs) {
 641  //   cruise->setDoubleValue("speed-kts", crs->getDoubleValue("speed-kts"));
 642    // cruise->setDoubleValue("mach", crs->getDoubleValue("mach"));
 643    // cruise->setDoubleValue("altitude-ft", crs->getDoubleValue("altitude-ft"));
 644   } // of cruise data loading
 645
 646 }
 647
 648 void FlightPlan::loadVersion2XMLRoute(SGPropertyNode_ptr routeData)
 649 {
 650   loadXMLRouteHeader(routeData);
 651
 652   // route nodes
 653   _legs.clear();
 654   SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
 655   for (int i=0; i<routeNode->nChildren(); ++i) {
 656     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
 657     Leg* l = new Leg(this, Waypt::createFromProperties(NULL, wpNode));
 658     _legs.push_back(l);
 659   } // of route iteration
 660 }
 661
 662 void FlightPlan::loadVersion1XMLRoute(SGPropertyNode_ptr routeData)
 663 {
 664   loadXMLRouteHeader(routeData);
 665
 666   // _legs nodes
 667   _legs.clear();
 668   SGPropertyNode_ptr routeNode = routeData->getChild("route", 0);
 669   for (int i=0; i<routeNode->nChildren(); ++i) {
 670     SGPropertyNode_ptr wpNode = routeNode->getChild("wp", i);
 671     Leg* l = new Leg(this, parseVersion1XMLWaypt(wpNode));
 672     _legs.push_back(l);
 673   } // of route iteration
 674
 675 }
 676
 677 WayptRef FlightPlan::parseVersion1XMLWaypt(SGPropertyNode* aWP)
 678 {
 679   SGGeod lastPos;
 680   if (!_legs.empty()) {
 681     lastPos = _legs.back()->waypoint()->position();
 682   } else if (_departure) {
 683     lastPos = _departure->geod();
 684   }
 685
 686   WayptRef w;
 687   string ident(aWP->getStringValue("ident"));
 688   if (aWP->hasChild("longitude-deg")) {
 689     // explicit longitude/latitude
 690     w = new BasicWaypt(SGGeod::fromDeg(aWP->getDoubleValue("longitude-deg"),
 691                                        aWP->getDoubleValue("latitude-deg")), ident, NULL);
 692
 693   } else {
 694     string nid = aWP->getStringValue("navid", ident.c_str());
 695     FGPositionedRef p = FGPositioned::findClosestWithIdent(nid, lastPos);
 696     if (!p) {
 697       throw sg_io_exception("bad route file, unknown navid:" + nid);
 698     }
 699
 700     SGGeod pos(p->geod());
 701     if (aWP->hasChild("offset-nm") && aWP->hasChild("offset-radial")) {
 702       double radialDeg = aWP->getDoubleValue("offset-radial");
 703       // convert magnetic radial to a true radial!
 704       radialDeg += magvarDegAt(pos);
 705       double offsetNm = aWP->getDoubleValue("offset-nm");
 706       double az2;
 707       SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
 708     }
 709
 710     w = new BasicWaypt(pos, ident, NULL);
 711   }
 712
 713   double altFt = aWP->getDoubleValue("altitude-ft", -9999.9);
 714   if (altFt > -9990.0) {
 715     w->setAltitude(altFt, RESTRICT_AT);
 716   }
 717
 718   return w;
 719 }
 720
 721 bool FlightPlan::loadPlainTextRoute(const SGPath& path)
 722 {
 723   try {
 724     sg_gzifstream in(path.str().c_str());
 725     if (!in.is_open()) {
 726       throw sg_io_exception("Cannot open file for reading.");
 727     }
 728
 729     _legs.clear();
 730     while (!in.eof()) {
 731       string line;
 732       getline(in, line, '\n');
 733       // trim CR from end of line, if found
 734       if (line[line.size() - 1] == '\r') {
 735         line.erase(line.size() - 1, 1);
 736       }
 737
 738       line = simgear::strutils::strip(line);
 739       if (line.empty() || (line[0] == '#')) {
 740         continue; // ignore empty/comment lines
 741       }
 742
 743       WayptRef w = waypointFromString(line);
 744       if (!w) {
 745         throw sg_io_exception("Failed to create waypoint from line '" + line + "'.");
 746       }
 747
 748       _legs.push_back(new Leg(this, w));
 749     } // of line iteration
 750   } catch (sg_exception& e) {
 751     SG_LOG(SG_IO, SG_ALERT, "Failed to load route from: '" << path.str() << "'. " << e.getMessage());
 752     _legs.clear();
 753     return false;
 754   }
 755
 756   return true;
 757 }
 758
 759 double FlightPlan::magvarDegAt(const SGGeod& pos) const
 760 {
 761   double jd = globals->get_time_params()->getJD();
 762   return sgGetMagVar(pos, jd) * SG_RADIANS_TO_DEGREES;
 763 }
 764
 765 WayptRef FlightPlan::waypointFromString(const string& tgt )
 766 {
 767   string target(boost::to_upper_copy(tgt));
 768   WayptRef wpt;
 769
 770   // extract altitude
 771   double altFt = 0.0;
 772   RouteRestriction altSetting = RESTRICT_NONE;
 773
 774   size_t pos = target.find( '@' );
 775   if ( pos != string::npos ) {
 776     altFt = atof( target.c_str() + pos + 1 );
 777     target = target.substr( 0, pos );
 778     if ( !strcmp(fgGetString("/sim/startup/units"), "meter") )
 779       altFt *= SG_METER_TO_FEET;
 780     altSetting = RESTRICT_AT;
 781   }
 782
 783   // check for lon,lat
 784   pos = target.find( ',' );
 785   if ( pos != string::npos ) {
 786     double lon = atof( target.substr(0, pos).c_str());
 787     double lat = atof( target.c_str() + pos + 1);
 788     char buf[32];
 789     char ew = (lon < 0.0) ? 'W' : 'E';
 790     char ns = (lat < 0.0) ? 'S' : 'N';
 791     snprintf(buf, 32, "%c%03d%c%03d", ew, (int) fabs(lon), ns, (int)fabs(lat));
 792
 793     wpt = new BasicWaypt(SGGeod::fromDeg(lon, lat), buf, NULL);
 794     if (altSetting != RESTRICT_NONE) {
 795       wpt->setAltitude(altFt, altSetting);
 796     }
 797     return wpt;
 798   }
 799
 800   SGGeod basePosition;
 801   if (_legs.empty()) {
 802     // route is empty, use current position
 803     basePosition = globals->get_aircraft_position();
 804   } else {
 805     basePosition = _legs.back()->waypoint()->position();
 806   }
 807
 808   string_list pieces(simgear::strutils::split(target, "/"));
 809   FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition);
 810   if (!p) {
 811     SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
 812     return NULL;
 813   }
 814
 815   double magvar = magvarDegAt(basePosition);
 816
 817   if (pieces.size() == 1) {
 818     wpt = new NavaidWaypoint(p, NULL);
 819   } else if (pieces.size() == 3) {
 820     // navaid/radial/distance-nm notation
 821     double radial = atof(pieces[1].c_str()),
 822     distanceNm = atof(pieces[2].c_str());
 823     radial += magvar;
 824     wpt = new OffsetNavaidWaypoint(p, NULL, radial, distanceNm);
 825   } else if (pieces.size() == 2) {
 826     FGAirport* apt = dynamic_cast<FGAirport*>(p.ptr());
 827     if (!apt) {
 828       SG_LOG(SG_AUTOPILOT, SG_INFO, "Waypoint is not an airport:" << pieces.front());
 829       return NULL;
 830     }
 831
 832     if (!apt->hasRunwayWithIdent(pieces[1])) {
 833       SG_LOG(SG_AUTOPILOT, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
 834       return NULL;
 835     }
 836
 837     FGRunway* runway = apt->getRunwayByIdent(pieces[1]);
 838     wpt = new NavaidWaypoint(runway, NULL);
 839   } else if (pieces.size() == 4) {
 840     // navid/radial/navid/radial notation
 841     FGPositionedRef p2 = FGPositioned::findClosestWithIdent(pieces[2], basePosition);
 842     if (!p2) {
 843       SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces[2]);
 844       return NULL;
 845     }
 846
 847     double r1 = atof(pieces[1].c_str()),
 848     r2 = atof(pieces[3].c_str());
 849     r1 += magvar;
 850     r2 += magvar;
 851
 852     SGGeod intersection;
 853     bool ok = SGGeodesy::radialIntersection(p->geod(), r1, p2->geod(), r2, intersection);
 854     if (!ok) {
 855       SG_LOG(SG_AUTOPILOT, SG_INFO, "no valid intersection for:" << target);
 856       return NULL;
 857     }
 858
 859     std::string name = p->ident() + "-" + p2->ident();
 860     wpt = new BasicWaypt(intersection, name, NULL);
 861   }
 862
 863   if (!wpt) {
 864     SG_LOG(SG_AUTOPILOT, SG_INFO, "Unable to parse waypoint:" << target);
 865     return NULL;
 866   }
 867
 868   if (altSetting != RESTRICT_NONE) {
 869     wpt->setAltitude(altFt, altSetting);
 870   }
 871   return wpt;
 872 }
 873
 874 FlightPlan::Leg::Leg(FlightPlan* owner, WayptRef wpt) :
 875   _parent(owner),
 876   _speedRestrict(RESTRICT_NONE),
 877   _altRestrict(RESTRICT_NONE),
 878   _waypt(wpt)
 879 {
 880   if (!wpt.valid()) {
 881     throw sg_exception("can't create FlightPlan::Leg without underlying waypoint");
 882   }
 883   _speed = _altitudeFt = 0;
 884 }
 885
 886 FlightPlan::Leg* FlightPlan::Leg::cloneFor(FlightPlan* owner) const
 887 {
 888   Leg* c = new Leg(owner, _waypt);
 889 // clone local data
 890   c->_speed = _speed;
 891   c->_speedRestrict = _speedRestrict;
 892   c->_altitudeFt = _altitudeFt;
 893   c->_altRestrict = _altRestrict;
 894
 895   return c;
 896 }
 897
 898 FlightPlan::Leg* FlightPlan::Leg::nextLeg() const
 899 {
 900   return _parent->legAtIndex(index() + 1);
 901 }
 902
 903 unsigned int FlightPlan::Leg::index() const
 904 {
 905   return _parent->findLegIndex(this);
 906 }
 907
 908 int FlightPlan::Leg::altitudeFt() const
 909 {
 910   if (_altRestrict != RESTRICT_NONE) {
 911     return _altitudeFt;
 912   }
 913
 914   return _waypt->altitudeFt();
 915 }
 916
 917 int FlightPlan::Leg::speed() const
 918 {
 919   if (_speedRestrict != RESTRICT_NONE) {
 920     return _speed;
 921   }
 922
 923   return _waypt->speed();
 924 }
 925
 926 int FlightPlan::Leg::speedKts() const
 927 {
 928   return speed();
 929 }
 930
 931 double FlightPlan::Leg::speedMach() const
 932 {
 933   if (!isMachRestrict(_speedRestrict)) {
 934     return 0.0;
 935   }
 936
 937   return -(_speed / 100.0);
 938 }
 939
 940 RouteRestriction FlightPlan::Leg::altitudeRestriction() const
 941 {
 942   if (_altRestrict != RESTRICT_NONE) {
 943     return _altRestrict;
 944   }
 945
 946   return _waypt->altitudeRestriction();
 947 }
 948
 949 RouteRestriction FlightPlan::Leg::speedRestriction() const
 950 {
 951   if (_speedRestrict != RESTRICT_NONE) {
 952     return _speedRestrict;
 953   }
 954
 955   return _waypt->speedRestriction();
 956 }
 957
 958 void FlightPlan::Leg::setSpeed(RouteRestriction ty, double speed)
 959 {
 960   _speedRestrict = ty;
 961   if (isMachRestrict(ty)) {
 962     _speed = (speed * -100);
 963   } else {
 964     _speed = speed;
 965   }
 966 }
 967
 968 void FlightPlan::Leg::setAltitude(RouteRestriction ty, int altFt)
 969 {
 970   _altRestrict = ty;
 971   _altitudeFt = altFt;
 972 }
 973
 974 double FlightPlan::Leg::courseDeg() const
 975 {
 976   return _courseDeg;
 977 }
 978
 979 double FlightPlan::Leg::distanceNm() const
 980 {
 981   return _pathDistance;
 982 }
 983
 984 double FlightPlan::Leg::distanceAlongRoute() const
 985 {
 986   return _distanceAlongPath;
 987 }
 988
 989 void FlightPlan::rebuildLegData()
 990 {
 991   _totalDistance = 0.0;
 992   int lastLeg = static_cast<int>(_legs.size()) - 1;
 993   for (int l=0; l<lastLeg; ++l) {
 994     Leg* cur = _legs[l];
 995     Leg* next = _legs[l + 1];
 996
 997     std::pair<double, double> crsDist =
 998       next->waypoint()->courseAndDistanceFrom(cur->waypoint()->position());
 999     _legs[l]->_courseDeg = crsDist.first;
1000     _legs[l]->_pathDistance = crsDist.second * SG_METER_TO_NM;
1001     _legs[l]->_distanceAlongPath = _totalDistance;
1002     _totalDistance += crsDist.second * SG_METER_TO_NM;
1003   } // of legs iteration
1004 }
1005
1006 void FlightPlan::registerDelegateFactory(DelegateFactory* df)
1007 {
1008   FPDelegateFactoryVec::iterator it = std::find(static_delegateFactories.begin(),
1009                                                 static_delegateFactories.end(), df);
1010   if (it != static_delegateFactories.end()) {
1011     throw  sg_exception("duplicate delegate factory registration");
1012   }
1013
1014   static_delegateFactories.push_back(df);
1015 }
1016
1017 void FlightPlan::unregisterDelegateFactory(DelegateFactory* df)
1018 {
1019   FPDelegateFactoryVec::iterator it = std::find(static_delegateFactories.begin(),
1020                                                 static_delegateFactories.end(), df);
1021   if (it == static_delegateFactories.end()) {
1022     return;
1023   }
1024
1025   static_delegateFactories.erase(it);
1026 }
1027
1028 void FlightPlan::addDelegate(Delegate* d)
1029 {
1030   // wrap any existing delegate(s) in the new one
1031   d->_inner = _delegate;
1032   _delegate = d;
1033 }
1034
1035 void FlightPlan::removeDelegate(Delegate* d)
1036 {
1037   if (d == _delegate) {
1038     _delegate = _delegate->_inner;
1039   } else if (_delegate) {
1040     _delegate->removeInner(d);
1041   }
1042 }
1043
1044 FlightPlan::Delegate::Delegate() :
1045   _deleteWithPlan(false),
1046   _inner(NULL)
1047 {
1048
1049 }
1050
1051 FlightPlan::Delegate::~Delegate()
1052 {
1053
1054 }
1055
1056 void FlightPlan::Delegate::removeInner(Delegate* d)
1057 {
1058   if (!_inner) {
1059     return;
1060   }
1061
1062   if (_inner == d) {
1063     // replace with grand-child
1064     _inner = d->_inner;
1065   } else { // recurse downwards
1066     _inner->removeInner(d);
1067   }
1068 }
1069
1070 void FlightPlan::Delegate::runDepartureChanged()
1071 {
1072   if (_inner) _inner->runDepartureChanged();
1073   departureChanged();
1074 }
1075
1076 void FlightPlan::Delegate::runArrivalChanged()
1077 {
1078   if (_inner) _inner->runArrivalChanged();
1079   arrivalChanged();
1080 }
1081
1082 void FlightPlan::Delegate::runWaypointsChanged()
1083 {
1084   if (_inner) _inner->runWaypointsChanged();
1085   waypointsChanged();
1086 }
1087
1088 void FlightPlan::Delegate::runCurrentWaypointChanged()
1089 {
1090   if (_inner) _inner->runCurrentWaypointChanged();
1091   currentWaypointChanged();
1092 }
1093
1094 } // of namespace flightgear