src/Navaids/route.cxx

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