
#ifdef HAVE_CONFIG_H
#  include "config.h"
#endif

#include <algorithm>

#include <Navaids/routePath.hxx>

#include <simgear/structure/exception.hxx>
#include <simgear/magvar/magvar.hxx>
#include <simgear/timing/sg_time.hxx>

#include <Main/globals.hxx>
#include <Airports/runways.hxx>
#include <Navaids/waypoint.hxx>
#include <Navaids/FlightPlan.hxx>
#include <Navaids/positioned.hxx>

namespace flightgear {

  // implementation of
  // http://williams.best.vwh.net/avform.htm#Intersection
  bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result)
  {
    double crs13 = r1 * SG_DEGREES_TO_RADIANS;
    double crs23 = r2 * SG_DEGREES_TO_RADIANS;
    double dst12 = SGGeodesy::distanceRad(a, b);

    //IF sin(lon2-lon1)<0
    // crs12=acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1)))
    // crs21=2.*pi-acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2)))
    // ELSE
    // crs12=2.*pi-acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1)))
    // crs21=acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2)))
    // ENDIF

    double sinLat1 = sin(a.getLatitudeRad());
    double cosLat1 = cos(a.getLatitudeRad());
    double sinDst12 = sin(dst12);
    double cosDst12 = cos(dst12);

    double crs12 = SGGeodesy::courseRad(a, b),
      crs21 = SGGeodesy::courseRad(b, a);


    // normalise to -pi .. pi range
    double ang1 = SGMiscd::normalizeAngle(crs13-crs12);
    double ang2 = SGMiscd::normalizeAngle(crs21-crs23);

    if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) {
      SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: infinity of intersections");
      return false;
    }

    if ((sin(ang1)*sin(ang2))<0.0) {
     // SG_LOG(SG_INSTR, SG_INFO, "geocRadialIntersection: intersection ambiguous:"
      //       << ang1 << " " << ang2 << " sin1 " << sin(ang1) << " sin2 " << sin(ang2));
      return false;
    }

    ang1 = fabs(ang1);
    ang2 = fabs(ang2);

    //ang3=acos(-cos(ang1)*cos(ang2)+sin(ang1)*sin(ang2)*cos(dst12))
    //dst13=atan2(sin(dst12)*sin(ang1)*sin(ang2),cos(ang2)+cos(ang1)*cos(ang3))
    //lat3=asin(sin(lat1)*cos(dst13)+cos(lat1)*sin(dst13)*cos(crs13))

    //lon3=mod(lon1-dlon+pi,2*pi)-pi

    double ang3 = acos(-cos(ang1) * cos(ang2) + sin(ang1) * sin(ang2) * cosDst12);
    double dst13 = atan2(sinDst12 * sin(ang1) * sin(ang2), cos(ang2) + cos(ang1)*cos(ang3));

    SGGeoc pt3;
    SGGeodesy::advanceRadM(a, crs13, dst13 * SG_RAD_TO_NM * SG_NM_TO_METER, pt3);

    double lat3 = asin(sinLat1 * cos(dst13) + cosLat1 * sin(dst13) * cos(crs13));

    //dlon=atan2(sin(crs13)*sin(dst13)*cos(lat1),cos(dst13)-sin(lat1)*sin(lat3))
    double dlon = atan2(sin(crs13)*sin(dst13)*cosLat1, cos(dst13)- (sinLat1 * sin(lat3)));
    double lon3 = SGMiscd::normalizeAngle(-a.getLongitudeRad()-dlon);

    result = SGGeoc::fromRadM(-lon3, lat3, a.getRadiusM());
    //result = pt3;
    return true;
  }
}

using namespace flightgear;

// implement Point(s) known distance from a great circle

static double sqr(const double x)
{
  return x * x;
}

// http://williams.best.vwh.net/avform.htm#POINTDME
double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
{
  double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
  double bDist = SGGeodesy::distanceRad(a, d);
  
  // r=(cos(b)^2+sin(b)^2*cos(A)^2)^(1/2)
  double r = pow(sqr(cos(bDist)) + sqr(sin(bDist)) * sqr(cos(A)), 0.5); 
  
  double p = atan2(sin(bDist)*cos(A), cos(bDist));
  
  if (sqr(cos(dist)) > sqr(r)) {
    SG_LOG(SG_NAVAID, SG_INFO, "pointsKnownDistanceFromGC, no points exist");
    return -1.0;
  }
  
  double dp1 = p + acos(cos(dist)/r);
  double dp2 = p - acos(cos(dist)/r);
  
  double dp1Nm = fabs(dp1 * SG_RAD_TO_NM);
  double dp2Nm = fabs(dp2 * SG_RAD_TO_NM);
  
  return SGMiscd::min(dp1Nm, dp2Nm);
}

// http://williams.best.vwh.net/avform.htm#Int
double latitudeForGCLongitude(const SGGeoc& a, const SGGeoc& b, double lon)
{
#if 0
Intermediate points {lat,lon} lie on the great circle connecting points 1 and 2 when:

lat=atan((sin(lat1)*cos(lat2)*sin(lon-lon2)
     -sin(lat2)*cos(lat1)*sin(lon-lon1))/(cos(lat1)*cos(lat2)*sin(lon1-lon2)))
#endif
  double lonDiff = a.getLongitudeRad() - b.getLongitudeRad();
  double cosLat1 = cos(a.getLatitudeRad()),
      cosLat2 = cos(b.getLatitudeRad());
  double x = sin(a.getLatitudeRad()) * cosLat2 * sin(lon - b.getLongitudeRad());
  double y = sin(b.getLatitudeRad()) * cosLat1 * sin(lon - a.getLongitudeRad());
  double denom = cosLat1 * cosLat2 * sin(lonDiff);
  double lat = atan((x - y) / denom);
  return lat;
}

static double magVarFor(const SGGeod& geod)
{
  double jd = globals->get_time_params()->getJD();
  return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES;
}

SGGeod turnCenterOverflight(const SGGeod& pt, double inHeadingDeg,
                           double turnAngleDeg, double turnRadiusM)
{
    double p = copysign(90.0, turnAngleDeg);
    return SGGeodesy::direct(pt, inHeadingDeg + p, turnRadiusM);
}

SGGeod turnCenterFlyBy(const SGGeod& pt, double inHeadingDeg,
                           double turnAngleDeg, double turnRadiusM)
{
    double halfAngle = turnAngleDeg * 0.5;
    double turnCenterOffset = turnRadiusM / cos(halfAngle * SG_DEGREES_TO_RADIANS);
    double p = copysign(90.0, turnAngleDeg);
    return SGGeodesy::direct(pt, inHeadingDeg + halfAngle + p, turnCenterOffset);
}

SGGeod turnCenterFromExit(const SGGeod& pt, double outHeadingDeg,
                       double turnAngleDeg, double turnRadiusM)
{
    double p = copysign(90.0, turnAngleDeg);
    return SGGeodesy::direct(pt, outHeadingDeg + p, turnRadiusM);
}

struct TurnInfo
{
    TurnInfo() : valid(false),
    inboundCourseDeg(0.0),
    turnAngleDeg(0.0) { }

    bool valid;
    SGGeod turnCenter;
    double inboundCourseDeg;
    double turnAngleDeg;
};

/**
 * given a turn exit position and heading, and an arbitrary origin position,
 * compute the turn center / angle that matches. Certain configurations may
 * fail, especially if the origin is less than two turn radii from the exit pos.
 */
TurnInfo turnCenterAndAngleFromExit(const SGGeod& pt, double outHeadingDeg,
                                double turnRadiusM, const SGGeod&origin)
{
    double bearingToExit = SGGeodesy::courseDeg(origin, pt);
    // not the final turn angle, but we need to know which side of the
    // exit course we're on, to decide if it's a left-hand or right-hand turn
    double turnAngle = outHeadingDeg - bearingToExit;
    SG_NORMALIZE_RANGE(turnAngle, -180.0, 180.0);
    double p = copysign(90.0, turnAngle);

    TurnInfo r;
    r.turnCenter = SGGeodesy::direct(pt, outHeadingDeg + p, turnRadiusM);

    double courseToTC, distanceToTC, az2;
    SGGeodesy::inverse(origin, r.turnCenter, courseToTC, az2, distanceToTC);
    if (distanceToTC < turnRadiusM) {
        SG_LOG(SG_NAVAID, SG_WARN, "turnCenterAndAngleFromExit: origin point too close to turn center");
        return r;
    }

    // find additional course angle away from the exit pos to intersect
    // the turn circle.
    double theta = asin(turnRadiusM / distanceToTC) * SG_RADIANS_TO_DEGREES;
    // invert angle sign so we increase the turn angle
    theta = -copysign(theta, turnAngle);

    r.inboundCourseDeg = courseToTC + theta;
    SG_NORMALIZE_RANGE(r.inboundCourseDeg, 0.0, 360.0);

    // turn angle must have same direction (sign) as turnAngle above, even if
    // the turn radius means the sign would cross over (happens if origin point
    // is close by
    r.turnAngleDeg = outHeadingDeg - r.inboundCourseDeg;
    if (r.turnAngleDeg > 0.0) {
        if (turnAngle < 0.0) r.turnAngleDeg -= 360.0;
    } else {
        if (turnAngle > 0.0) r.turnAngleDeg += 360.0;
    }

    r.valid = true;
    return r;
}

class WayptData
{
public:
  explicit WayptData(WayptRef w) :
    wpt(w),
    hasEntry(false),
    posValid(false),
    legCourseValid(false),
    skipped(false),
    turnEntryAngle(0.0),
    turnExitAngle(0.0),
    turnRadius(0.0),
    legCourseTrue(0.0),
    pathDistanceM(0.0),
    turnPathDistanceM(0.0),
    overflightCompensationAngle(0.0),
    flyOver(w->flag(WPT_OVERFLIGHT))
  {
  }
  
  void initPass0()
  {
    const std::string& ty(wpt->type());
    if (wpt->flag(WPT_DYNAMIC)) {
      // presumption is that we always overfly such a waypoint
      if (ty == "hdgToAlt") {
        flyOver = true;
      }
    } else if (ty == "discontinuity") {

    } else {
      pos = wpt->position();
      posValid = true;
      
      if (ty == "hold") {
        legCourseTrue = wpt->headingRadialDeg() + magVarFor(pos);
        legCourseValid = true;
      }
    } // of static waypt
  }

  /**
   * test if course of this leg can be adjusted or is contrained to an exact value
   */
  bool isCourseConstrained() const
  {
    const std::string& ty(wpt->type());
    return (ty == "hdgToAlt") || (ty == "radialIntercept") || (ty == "dmeIntercept");
  }

  // compute leg courses for all static legs (both ends are fixed)
  void initPass1(const WayptData& previous, WayptData* next)
  {
    if (wpt->type() == "vectors") {
      // relying on the fact vectors will be followed by a static fix/wpt
      if (next && next->posValid) {
        posValid = true;
        pos = next->pos;
      }
    }

    if (wpt->type() == "via") {
        // even though both ends may be known, we don't
        // want to compute a leg course for a VIA
    } else if (posValid && !legCourseValid && previous.posValid) {
        // check for duplicate points now
      if (previous.wpt->matches(wpt)) {
          skipped = true;
      }

      // we can compute leg course now
        if (previous.wpt->type() == "runway") {
            // use the runway departure end pos
            FGRunway* rwy = static_cast<RunwayWaypt*>(previous.wpt.get())->runway();
            legCourseTrue = SGGeodesy::courseDeg(rwy->end(), pos);
        } else if (wpt->type() != "runway") {
            // need to wait to compute runway leg course
            legCourseTrue = SGGeodesy::courseDeg(previous.pos, pos);
            legCourseValid = true;
        }
    } // of not a VIA
  }

  void computeLegCourse(const WayptData& previous, double radiusM)
  {
      if (legCourseValid) {
          return;
      }

      if ((wpt->type() == "hold") ||
              (wpt->type() == "discontinuity") ||
              (wpt->type()  == "via"))
      {
          // do nothing, we can't compute a valid leg course for these types
          // we'll generate shrap turns in the path but that's no problem.
      } else if (wpt->type() == "runway") {
          FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
          flyOver = true;

          TurnInfo ti = turnCenterAndAngleFromExit(rwy->threshold(),
                                                   rwy->headingDeg(),
                                                   radiusM, previous.pos);
          if (ti.valid) {
              legCourseTrue = ti.inboundCourseDeg;
              turnEntryAngle = ti.turnAngleDeg;
              turnEntryCenter = ti.turnCenter;
              turnRadius = radiusM;
              hasEntry = true;
              turnEntryPos = pointOnEntryTurnFromHeading(ti.inboundCourseDeg);
          } else {
              // couldn't compute entry, never mind
              legCourseTrue = SGGeodesy::courseDeg(previous.pos, rwy->threshold());
          }
          legCourseValid = true;
      } else {
          if (posValid) {
              legCourseTrue = SGGeodesy::courseDeg(previous.pos, pos);
              legCourseValid = true;
          } else if (isCourseConstrained()) {
              double magVar = magVarFor(previous.pos);
              legCourseTrue = wpt->headingRadialDeg() + magVar;
              legCourseValid = true;
          } // of pos not valid
      } // of general case
  }

    SGGeod pointOnEntryTurnFromHeading(double headingDeg) const
    {
        assert(hasEntry);
        double p = copysign(90.0, turnEntryAngle);
        return SGGeodesy::direct(turnEntryCenter, headingDeg - p, turnRadius);
    }

    SGGeod pointOnExitTurnFromHeading(double headingDeg) const
    {
        double p = copysign(90.0, turnExitAngle);
        return SGGeodesy::direct(turnExitCenter, headingDeg - p, turnRadius);
    }

    double pathDistanceForTurnAngle(double angleDeg) const
    {
        return turnRadius * fabs(angleDeg) * SG_DEGREES_TO_RADIANS;
    }

  void computeTurn(double radiusM, bool constrainLegCourse, WayptData& next)
  {
    assert(!skipped);
    assert(next.legCourseValid);
      bool isRunway = (wpt->type() == "runway");

      if (legCourseValid) {
          if (isRunway) {
              FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
              turnExitAngle = next.legCourseTrue - rwy->headingDeg();
          } else {
              turnExitAngle = next.legCourseTrue - legCourseTrue;
          }
      } else {
          // happens for first leg
          if (isRunway) {
              legCourseValid = true;
              FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
              turnExitAngle = next.legCourseTrue - rwy->headingDeg();
              legCourseTrue = rwy->headingDeg();
              flyOver = true;
          } else {
              // don't set legCourseValid
              turnExitAngle = 0.0;
          }
      }

    SG_NORMALIZE_RANGE(turnExitAngle, -180.0, 180.0);
    turnRadius = radiusM;

    if (!flyOver && fabs(turnExitAngle) > 120.0) {
        // flyBy logic blows up for sharp turns - due to the tan() term
        // heading towards infinity. By converting to flyOver we do something
        // closer to what was requested.
        flyOver = true;
    }

    if (flyOver) {
        if (isRunway) {
            FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
            turnExitCenter = turnCenterOverflight(rwy->end(), rwy->headingDeg(), turnExitAngle, turnRadius);
        } else {
            turnEntryPos = pos;
            turnExitCenter = turnCenterOverflight(pos, legCourseTrue, turnExitAngle, turnRadius);

        }
      turnExitPos = pointOnExitTurnFromHeading(next.legCourseTrue);
      
      if (!next.wpt->flag(WPT_DYNAMIC)) {
        // distance perpendicular to next leg course, after turning
        // through turnAngle
        double xtk = turnRadius * (1 - cos(turnExitAngle * SG_DEGREES_TO_RADIANS));

        if (constrainLegCourse || next.isCourseConstrained()) {
          // next leg course is constrained. We need to swing back onto the
          // desired course, using a compensation turn
          
          // compensation angle to turn back on course
          double theta = acos((turnRadius - (xtk * 0.5)) / turnRadius) * SG_RADIANS_TO_DEGREES;
          theta = copysign(theta, turnExitAngle);
          turnExitAngle += theta;
        
          // move by the distance to compensate
          double d = turnRadius * 2.0 * sin(theta * SG_DEGREES_TO_RADIANS);
          turnExitPos = SGGeodesy::direct(turnExitPos, next.legCourseTrue, d);
          overflightCompensationAngle = -theta;

            // sign of angles will differ, so compute distances seperately
            turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle) +
                pathDistanceForTurnAngle(overflightCompensationAngle);
        } else {
          // next leg course can be adjusted. increase the turn angle
          // and modify the next leg's course accordingly.

          // hypotenuse of triangle, opposite edge has length turnRadius
          double distAlongPath = std::min(1.0, sin(fabs(turnExitAngle) * SG_DEGREES_TO_RADIANS)) * turnRadius;
          double nextLegDistance = SGGeodesy::distanceM(pos, next.pos) - distAlongPath;
          double increaseAngle = atan2(xtk, nextLegDistance) * SG_RADIANS_TO_DEGREES;
          increaseAngle = copysign(increaseAngle, turnExitAngle);

          turnExitAngle += increaseAngle;
          turnExitPos = pointOnExitTurnFromHeading(legCourseTrue + turnExitAngle);
          // modify next leg course
          next.legCourseTrue = SGGeodesy::courseDeg(turnExitPos, next.pos);
          turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle);
        } // of next leg isn't course constrained
      } else {
          // next point is dynamic
          // no compensation needed
          turnPathDistanceM = pathDistanceForTurnAngle(turnExitAngle);
      }
    } else {
      hasEntry = true;
        turnEntryCenter = turnCenterFlyBy(pos, legCourseTrue, turnExitAngle, turnRadius);

        turnExitAngle = turnExitAngle * 0.5;
        turnEntryAngle = turnExitAngle;
        turnExitCenter = turnEntryCenter; // important that these match

      turnEntryPos = pointOnEntryTurnFromHeading(legCourseTrue);
      turnExitPos = pointOnExitTurnFromHeading(next.legCourseTrue);
      turnPathDistanceM = pathDistanceForTurnAngle(turnEntryAngle);
    }
  }
  
  double turnDistanceM() const
  {
      return turnPathDistanceM;
  }
  
  void turnEntryPath(SGGeodVec& path) const
  {
    if (!hasEntry || fabs(turnEntryAngle) < 0.5 ) {
      path.push_back(pos);
      return;
    }

    int steps = std::max(SGMiscd::roundToInt(fabs(turnEntryAngle) / 3.0), 1);
    double stepIncrement = turnEntryAngle / steps;
    double h = legCourseTrue;
    for (int s=0; s<steps; ++s) {
        path.push_back(pointOnEntryTurnFromHeading(h));
        h += stepIncrement;
    }
  }
  
  void turnExitPath(SGGeodVec& path) const
  {
    if (fabs(turnExitAngle) < 0.5) {
      path.push_back(turnExitPos);
      return;
    }

    int steps = std::max(SGMiscd::roundToInt(fabs(turnExitAngle) / 3.0), 1);
    double stepIncrement = turnExitAngle / steps;
    
    // initial exit heading
      double h = legCourseTrue + (flyOver ? 0.0 : turnEntryAngle);
      if (wpt->type() == "runway") {
          FGRunway* rwy = static_cast<RunwayWaypt*>(wpt.get())->runway();
          h = rwy->headingDeg();
      }

    for (int s=0; s<steps; ++s) {
        path.push_back(pointOnExitTurnFromHeading(h));
        h += stepIncrement;
    }

    // we can use an exact check on the compensation angle, because we
    // initialise it directly. Depending on the next element we might be
    // doing compensation or adjusting the next leg's course; if we did
    // adjust the course everything 'just works' above.

    if (flyOver && (overflightCompensationAngle != 0.0)) {
      // skew by compensation angle back
      steps = std::max(SGMiscd::roundToInt(fabs(overflightCompensationAngle) / 3.0), 1);
      
      // step in opposite direction to the turn angle to swing back onto
      // the next leg course
      stepIncrement = overflightCompensationAngle / steps;

        SGGeod p = path.back();
        double stepDist = (fabs(stepIncrement) / 360.0) * SGMiscd::twopi() * turnRadius;

      for (int s=0; s<steps; ++s) {
          h += stepIncrement;
        p = SGGeodesy::direct(p, h, stepDist);
          path.push_back(p);

      }
    } // of overflight compensation turn
  }

  SGGeod pointAlongExitPath(double distanceM) const
  {
      double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
      double p = copysign(90, turnExitAngle);

      if (flyOver && (overflightCompensationAngle != 0.0)) {
          // figure out if we're in the compensation section
          if (theta > turnExitAngle) {
              // compute the compensation turn center - twice the turn radius
              // from turnCenter
              SGGeod tc2 = SGGeodesy::direct(turnExitCenter,
                                             legCourseTrue - overflightCompensationAngle - p,
                                             turnRadius * 2.0);
              theta = copysign(theta - turnExitAngle, overflightCompensationAngle);
              return SGGeodesy::direct(tc2,
                                       legCourseTrue - overflightCompensationAngle + theta + p, turnRadius);
          }
      }

      theta = copysign(theta, turnExitAngle);
      double inboundCourse = legCourseTrue + (flyOver ? 0.0 : turnExitAngle);
      return pointOnExitTurnFromHeading(inboundCourse + theta);
  }

  SGGeod pointAlongEntryPath(double distanceM) const
  {
    assert(hasEntry);
    double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
      theta = copysign(theta, turnEntryAngle);
      return pointOnEntryTurnFromHeading(legCourseTrue + theta);
  }
  
  WayptRef wpt;
  bool hasEntry, posValid, legCourseValid, skipped;
  SGGeod pos, turnEntryPos, turnExitPos, turnEntryCenter, turnExitCenter;
  double turnEntryAngle, turnExitAngle, turnRadius, legCourseTrue;
  double pathDistanceM;
  double turnPathDistanceM; // for flyBy, this is half the distance; for flyOver it's the complete distance
  double overflightCompensationAngle;
  bool flyOver;
};

typedef std::vector<WayptData> WayptDataVec;

class PerformanceBracket
{
public:
  PerformanceBracket(double atOrBelow, double climb, double descent, double speed, bool isMach = false) :
    atOrBelowAltitudeFt(atOrBelow),
    climbRateFPM(climb),
    descentRateFPM(descent),
    speedIASOrMach(speed),
    speedIsMach(isMach)
  { }
  
  double atOrBelowAltitudeFt;
  double climbRateFPM;
  double descentRateFPM;
  double speedIASOrMach;
  bool speedIsMach;
};

typedef std::vector<PerformanceBracket> PerformanceBracketVec;

bool isDescentWaypoint(const WayptRef& wpt)
{
  return (wpt->flag(WPT_APPROACH) && !wpt->flag(WPT_MISS)) || wpt->flag(WPT_ARRIVAL);
}

class RoutePath::RoutePathPrivate
{
public:
    WayptDataVec waypoints;

    char aircraftCategory;
    PerformanceBracketVec perf;
    double pathTurnRate;
    bool constrainLegCourses;
  
  PerformanceBracketVec::const_iterator
  findPerformanceBracket(double altFt) const
  {
    PerformanceBracketVec::const_iterator r;
    PerformanceBracketVec::const_iterator result = perf.begin();

    for (r = perf.begin(); r != perf.end(); ++r) {
      if (r->atOrBelowAltitudeFt > altFt) {
        break;
      }
      
      result = r;
    } // of brackets iteration
    
    return result;
  }
  
  void computeDynamicPosition(int index)
  {
    auto previous(previousValidWaypoint(index));
    WayptRef wpt = waypoints[index].wpt;
    assert(previous != waypoints.end());
    assert(previous->posValid);

    const std::string& ty(wpt->type());
    if (ty == "hdgToAlt") {
      HeadingToAltitude* h = (HeadingToAltitude*) wpt.get();
      
      double altFt = computeVNAVAltitudeFt(index - 1);
      double altChange = h->altitudeFt() - altFt;
      PerformanceBracketVec::const_iterator it = findPerformanceBracket(altFt);
      double speedMSec = groundSpeedForAltitude(altFt) * SG_KT_TO_MPS;
      double timeToChangeSec;
      
      if (isDescentWaypoint(wpt)) {
        timeToChangeSec = (altChange / it->descentRateFPM) * 60.0;
      } else {
        timeToChangeSec = (altChange / it->climbRateFPM) * 60.0;
      }

      double distanceM = timeToChangeSec * speedMSec;
      double hdg = h->headingDegMagnetic() + magVarFor(previous->pos);
      waypoints[index].pos = SGGeodesy::direct(previous->turnExitPos, hdg, distanceM);
      waypoints[index].posValid = true;
    } else if (ty == "radialIntercept") {
      // start from previous.turnExit
      RadialIntercept* i = (RadialIntercept*) wpt.get();
      
      SGGeoc prevGc = SGGeoc::fromGeod(previous->turnExitPos);
      SGGeoc navid = SGGeoc::fromGeod(wpt->position());
      SGGeoc rGc;
      double magVar = magVarFor(previous->pos);
      
      double radial = i->radialDegMagnetic() + magVar;
      double track = i->courseDegMagnetic() + magVar;
      bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
      if (!ok) {
        SGGeoc navidAdjusted;
        // try pulling backward along the radial in case we're too close.
        // suggests bad procedure construction if this is happening!
        SGGeodesy::advanceRadM(navid, radial, SG_NM_TO_METER * -10, navidAdjusted);

        // try again
        ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc);
        if (!ok) {
          SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:"
               << previous->turnExitPos << " / " << track << "/" << wpt->position()
               << "/" << radial);
          waypoints[index].pos = wpt->position(); // horrible fallback

        } else {
          waypoints[index].pos = SGGeod::fromGeoc(rGc);
        }
      } else {
        waypoints[index].pos = SGGeod::fromGeoc(rGc);
      }
      
      waypoints[index].posValid = true;
    } else if (ty == "dmeIntercept") {
      DMEIntercept* di = (DMEIntercept*) wpt.get();
      
      SGGeoc prevGc = SGGeoc::fromGeod(previous->turnExitPos);
      SGGeoc navid = SGGeoc::fromGeod(wpt->position());
      double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
      SGGeoc rGc;
      
      SGGeoc bPt;
      double crs = di->courseDegMagnetic() + magVarFor(wpt->position());
      SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
      
      double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
      if (dNm < 0.0) {
        SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed");
        waypoints[index].pos = wpt->position(); // horrible fallback
      } else {
        waypoints[index].pos = SGGeodesy::direct(previous->turnExitPos, crs, dNm * SG_NM_TO_METER);
      }
      
      waypoints[index].posValid = true;
    } else if (ty == "vectors") {
      waypoints[index].legCourseTrue = SGGeodesy::courseDeg(previous->turnExitPos, waypoints[index].pos);
      waypoints[index].legCourseValid = true;
      // no turn data
    }
  }
  
  double computeVNAVAltitudeFt(int index)
  {
    WayptRef w = waypoints[index].wpt;
    if ((w->flag(WPT_APPROACH) && !w->flag(WPT_MISS)) || w->flag(WPT_ARRIVAL)) {
      // descent
      int next = findNextKnownAltitude(index);
      if (next < 0) {
        return 0.0;
      }
      
      double fixedAlt = altitudeForIndex(next);
      double distanceM = distanceBetweenIndices(index, next);
      double speedMSec = groundSpeedForAltitude(fixedAlt) * SG_KT_TO_MPS;
      double minutes = (distanceM / speedMSec) / 60.0;
      
      PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
      return fixedAlt + (it->descentRateFPM * minutes);

    } else {
      // climb
      int prev = findPreceedingKnownAltitude(index);
      if (prev < 0) {
        return 0.0;
      }
      
      double fixedAlt = altitudeForIndex(prev);
      double distanceM = distanceBetweenIndices(prev, index);
      double speedMSec = groundSpeedForAltitude(fixedAlt) * SG_KT_TO_MPS;
      double minutes = (distanceM / speedMSec) / 60.0;
      
      PerformanceBracketVec::const_iterator it = findPerformanceBracket(fixedAlt);
      return fixedAlt + (it->climbRateFPM * minutes);
    }
    
  }
  
  int findPreceedingKnownAltitude(int index) const
  {
    const WayptData& w(waypoints[index]);
    if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
      return index;
    }
    
    // principal base case is runways.
    const std::string& ty(w.wpt->type());
    if (ty == "runway") {
      return index; // runway always has a known elevation
    }
    
    if (index == 0) {
      SG_LOG(SG_NAVAID, SG_WARN, "findPreceedingKnownAltitude: no preceding altitude value found");
      return -1;
    }
    
    // recurse earlier in the route
    return findPreceedingKnownAltitude(index - 1);
  }
  
  int findNextKnownAltitude(unsigned int index) const
  {
    if (index >= waypoints.size()) {
      SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
      return -1;
    }
    
    const WayptData& w(waypoints[index]);
    if (w.wpt->altitudeRestriction() == RESTRICT_AT) {
      return index;
    }
    
    // principal base case is runways.
    const std::string& ty(w.wpt->type());
    if (ty == "runway") {
      return index; // runway always has a known elevation
    }
    
    if (index == waypoints.size() - 1) {
      SG_LOG(SG_NAVAID, SG_WARN, "findNextKnownAltitude: no next altitude value found");
      return -1;
    }
    
    return findNextKnownAltitude(index + 1);
  }
  
  double altitudeForIndex(int index) const
  {
    const WayptData& w(waypoints[index]);
    if (w.wpt->altitudeRestriction() != RESTRICT_NONE) {
      return w.wpt->altitudeFt();
    }
    
    const std::string& ty(w.wpt->type());
    if (ty == "runway") {
      FGRunway* rwy = static_cast<RunwayWaypt*>(w.wpt.get())->runway();
      return rwy->threshold().getElevationFt();
    }
    
    SG_LOG(SG_NAVAID, SG_WARN, "altitudeForIndex: waypoint has no explicit altitude");
    return 0.0;
  }
  
  double groundSpeedForAltitude(double altitude) const
  {
      PerformanceBracketVec::const_iterator it = findPerformanceBracket(altitude);
      if (it->speedIsMach) {
          return 300.0;
      } else {
          // FIXME - convert IAS to ground-speed, using standard atmosphere / temperature model
          return it->speedIASOrMach;
      }

#if 0
    if (0) {
      double mach;
      
      if (it->speedIsMach) {
        mach = it->speedIASOrMach; // easy
      } else {
        const double Cs_0 = 661.4786; // speed of sound at sea level, knots
        const double P_0 = 29.92126;
        const double P = P_0 * pow(, );
        // convert IAS (which we will treat as CAS) to Mach based on altitude
      }
      
      double oatK;
      double Cs = sqrt(SG_gamma * SG_R_m2_p_s2_p_K * oatK);
      
      double tas = mach * Cs;
      
/*
      P_0= 29.92126 "Hg = 1013.25 mB = 2116.2166 lbs/ft^2
      P= P_0*(1-6.8755856*10^-6*PA)^5.2558797, pressure altitude, PA<36,089.24ft
      CS= 38.967854*sqrt(T+273.15)  where T is the (static/true) OAT in Celsius.
      
      DP=P_0*((1 + 0.2*(IAS/CS_0)^2)^3.5 -1)
      M=(5*( (DP/P + 1)^(2/7) -1) )^0.5   (*)
      TAS= M*CS
*/
    }
#endif
  }

  double distanceBetweenIndices(int from, int to) const
  {
    double total = 0.0;
    
    for (int i=from+1; i<= to; ++i) {
      total += waypoints[i].pathDistanceM;
    }
    
    return total;
  }
  
  void initPerfData()
  {
      pathTurnRate = 3.0; // 3 deg/sec = 180deg/min = standard rate turn
      switch (aircraftCategory) {
      case ICAO_AIRCRAFT_CATEGORY_A:
          perf.push_back(PerformanceBracket(4000, 600, 1200, 75));
          perf.push_back(PerformanceBracket(10000, 600, 1200, 140));
          break;

      case ICAO_AIRCRAFT_CATEGORY_B:
          perf.push_back(PerformanceBracket(4000, 100, 1200, 100));
          perf.push_back(PerformanceBracket(10000, 800, 1200, 160));
          perf.push_back(PerformanceBracket(18000, 600, 1800, 200));
          break;

      case ICAO_AIRCRAFT_CATEGORY_C:
          perf.push_back(PerformanceBracket(4000, 1800, 1800, 150));
          perf.push_back(PerformanceBracket(10000, 1800, 1800, 200));
          perf.push_back(PerformanceBracket(18000, 1200, 1800, 270));
          perf.push_back(PerformanceBracket(60000, 800, 1200, 0.80, true /* is Mach */));
          break;

      case ICAO_AIRCRAFT_CATEGORY_D:
      case ICAO_AIRCRAFT_CATEGORY_E:
      default:

          perf.push_back(PerformanceBracket(4000, 1800, 1800, 180));
          perf.push_back(PerformanceBracket(10000, 1800, 1800, 230));
          perf.push_back(PerformanceBracket(18000, 1200, 1800, 270));
          perf.push_back(PerformanceBracket(60000, 800, 1200, 0.87, true /* is Mach */));
          break;
      }
  }

    WayptDataVec::iterator previousValidWaypoint(unsigned int index)
    {
        if (index == 0) {
            return waypoints.end();
        }

        while (waypoints[--index].skipped) {
            // waypoint zero should be unskippable, this assert verified that
            assert(index > 0);
        }

        return waypoints.begin() + index;
    }

    WayptDataVec::iterator previousValidWaypoint(WayptDataVec::iterator it)
    {
        return previousValidWaypoint(std::distance(waypoints.begin(), it));
    }


    WayptDataVec::iterator nextValidWaypoint(int index)
    {
        return nextValidWaypoint(waypoints.begin() + index);
    }

    WayptDataVec::iterator nextValidWaypoint(WayptDataVec::iterator it)
    {
        if (it == waypoints.end()) {
            return it;
        }

        ++it;
        while ((it != waypoints.end()) && it->skipped) {
            ++it;
        }
        return it;
    }
}; // of RoutePathPrivate class

RoutePath::RoutePath(const flightgear::FlightPlan* fp) :
  d(new RoutePathPrivate)
{
    for (int l=0; l<fp->numLegs(); ++l) {
        Waypt *wpt = fp->legAtIndex(l)->waypoint();
        if (!wpt) {
            SG_LOG(SG_NAVAID, SG_DEV_ALERT, "Waypoint " << l << " of " << fp->numLegs() << "is NULL");
            break;
        }
        d->waypoints.push_back(WayptData(wpt));
    }

    d->aircraftCategory = fp->icaoAircraftCategory()[0];
    d->constrainLegCourses = fp->followLegTrackToFixes();
    commonInit();
}

RoutePath::~RoutePath()
{
}

void RoutePath::commonInit()
{
  d->initPerfData();
  
  WayptDataVec::iterator it;
  for (it = d->waypoints.begin(); it != d->waypoints.end(); ++it) {
    it->initPass0();
  }
  
  for (unsigned int i=1; i<d->waypoints.size(); ++i) {
    WayptData* nextPtr = ((i + 1) < d->waypoints.size()) ? &d->waypoints[i+1] : 0;
    d->waypoints[i].initPass1(d->waypoints[i-1], nextPtr);
  }

  for (unsigned int i=0; i<d->waypoints.size(); ++i) {
      if (d->waypoints[i].skipped) {
          continue;
      }

      double alt = 0.0; // FIXME
      double gs = d->groundSpeedForAltitude(alt);
      double radiusM = ((360.0 / d->pathTurnRate) * gs * SG_KT_TO_MPS) / SGMiscd::twopi();

      if (i > 0) {
          auto prevIt = d->previousValidWaypoint(i);
          assert(prevIt != d->waypoints.end());
          d->waypoints[i].computeLegCourse(*prevIt, radiusM);
          d->computeDynamicPosition(i);
      }

      auto nextIt = d->nextValidWaypoint(i);
      if (nextIt != d->waypoints.end()) {
          nextIt->computeLegCourse(d->waypoints[i], radiusM);

          if (nextIt->legCourseValid) {
              d->waypoints[i].computeTurn(radiusM, d->constrainLegCourses, *nextIt);
          } else {
            // next waypoint has indeterminate course. Let's create a sharp turn
            // this can happen when the following point is ATC vectors, for example.
            d->waypoints[i].turnEntryPos = d->waypoints[i].pos;
            d->waypoints[i].turnExitPos = d->waypoints[i].pos;
          }
    } else {
      // final waypt, fix up some data
      d->waypoints[i].turnExitPos = d->waypoints[i].pos;
      d->waypoints[i].turnEntryPos = d->waypoints[i].pos;
    }
    
    // now turn is computed, can resolve distances
    d->waypoints[i].pathDistanceM = computeDistanceForIndex(i);
  }
}

SGGeodVec RoutePath::pathForIndex(int index) const
{
  const WayptData& w(d->waypoints[index]);
  const std::string& ty(w.wpt->type());
  SGGeodVec r;

    if (d->waypoints[index].skipped) {
        return SGGeodVec();
    }

  if (ty == "vectors") {
      // ideally we'd show a stippled line to connect the route?
    return SGGeodVec();
  }

  if (ty == "discontinuity") {
    return SGGeodVec(); // no points for a discontinuity of course
  }

  if (ty == "via") {
      return pathForVia(static_cast<Via*>(d->waypoints[index].wpt.get()), index);
  }
  
  if (ty== "hold") {
    return pathForHold((Hold*) d->waypoints[index].wpt.get());
  }

    auto prevIt = d->previousValidWaypoint(index);
    if (prevIt != d->waypoints.end()) {
        prevIt->turnExitPath(r);
      
        SGGeod from = prevIt->turnExitPos,
            to = w.turnEntryPos;
      
      // compute rounding offset, we want to round towards the direction of travel
      // which depends on the east/west sign of the longitude change
      double lonDelta = to.getLongitudeDeg() - from.getLongitudeDeg();
      if (fabs(lonDelta) > 0.5) {
        interpolateGreatCircle(from, to, r);
      }
    } // of have previous waypoint

    w.turnEntryPath(r);
  
  if (ty == "runway") {
    // runways get an extra point, at the end. this is particularly
    // important so missed approach segments draw correctly
    FGRunway* rwy = static_cast<RunwayWaypt*>(w.wpt.get())->runway();
    r.push_back(rwy->end());
  }
  
  return r;
}

void RoutePath::interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, SGGeodVec& r) const
{
  SGGeoc gcFrom = SGGeoc::fromGeod(aFrom),
    gcTo = SGGeoc::fromGeod(aTo);
  
  double lonDelta = gcTo.getLongitudeRad() - gcFrom.getLongitudeRad();
  if (fabs(lonDelta) < 1e-3) {
    return;
  }
  
  lonDelta = SGMiscd::normalizeAngle(lonDelta);    
  int steps = static_cast<int>(fabs(lonDelta) * SG_RADIANS_TO_DEGREES * 2);
  double lonStep = (lonDelta / steps);
  
  double lon = gcFrom.getLongitudeRad() + lonStep;
  for (int s=0; s < (steps - 1); ++s) {
    lon = SGMiscd::normalizeAngle(lon);
    double lat = latitudeForGCLongitude(gcFrom, gcTo, lon);
    r.push_back(SGGeod::fromGeoc(SGGeoc::fromRadM(lon, lat, SGGeodesy::EQURAD)));
    //SG_LOG(SG_GENERAL, SG_INFO, "lon:" << lon * SG_RADIANS_TO_DEGREES << " gives lat " << lat * SG_RADIANS_TO_DEGREES);
    lon += lonStep;
  }
}

SGGeod RoutePath::positionForIndex(int index) const
{
  return d->waypoints[index].pos;
}

SGGeodVec RoutePath::pathForVia(Via* via, int index) const
{
    // previous waypoint must be valid for a VIA
    auto prevIt = d->previousValidWaypoint(index);
    if (prevIt == d->waypoints.end()) {
        return SGGeodVec();

    }

    WayptVec enrouteWaypoints = via->expandToWaypoints(prevIt->wpt);
    SGGeodVec r;

    WayptVec::const_iterator it;
    SGGeod legStart = prevIt->wpt->position();
    for (it = enrouteWaypoints.begin(); it != enrouteWaypoints.end(); ++it) {
        // interpolate directly into the result vector
        interpolateGreatCircle(legStart, (*it)->position(), r);
        legStart = (*it)->position();
    }

    return r;
}

SGGeodVec RoutePath::pathForHold(Hold* hold) const
{
    int turnSteps = 16;
    double hdg = hold->inboundRadial();
    double turnDelta = 180.0 / turnSteps;
    double altFt = 0.0; // FIXME
    double gsKts = d->groundSpeedForAltitude(altFt);

    SGGeodVec r;
    double az2;
    double stepTime = turnDelta / d->pathTurnRate; // in seconds
    double stepDist = gsKts * (stepTime / 3600.0) * SG_NM_TO_METER;
    double legDist = hold->isDistance() ?
    hold->timeOrDistance()
    : gsKts * (hold->timeOrDistance() / 3600.0);
    legDist *= SG_NM_TO_METER;

    if (hold->isLeftHanded()) {
        turnDelta = -turnDelta;
    }
    SGGeod pos = hold->position();
    r.push_back(pos);

    // turn+leg sides are a mirror
    for (int j=0; j < 2; ++j) {
        // turn
        for (int i=0;i<turnSteps; ++i) {
            hdg += turnDelta;
            SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
            r.push_back(pos);
        }

        // leg
        SGGeodesy::direct(pos, hdg, legDist, pos, az2);
        r.push_back(pos);
    } // of leg+turn duplication
    
    return r;
}

double RoutePath::computeDistanceForIndex(int index) const
{
    if ((index < 0) || (index >= (int) d->waypoints.size())) {
        throw sg_range_exception("waypt index out of range",
                                 "RoutePath::computeDistanceForIndex");
    }

    auto it = d->waypoints.begin() + index;
    if ((index == 0) || it->skipped) {
        // first waypoint, distance is 0
        return 0.0;
    }

    if (it->wpt->type() == "via") {
        return distanceForVia(static_cast<Via*>(it->wpt.get()), index);
    }

    auto prevIt = d->previousValidWaypoint(index);
    assert(prevIt != d->waypoints.end());

    double dist = SGGeodesy::distanceM(prevIt->turnExitPos, it->turnEntryPos);
    dist += prevIt->turnDistanceM();

    if (!it->flyOver) {
        // add entry distance
        dist += it->turnDistanceM();
    }
    
    return dist;
}

double RoutePath::distanceForVia(Via* via, int index) const
{
    auto prevIt = d->previousValidWaypoint(index);
    if (prevIt == d->waypoints.end()) {
        return 0.0;
    }

    WayptVec enrouteWaypoints = via->expandToWaypoints(prevIt->wpt);
    double dist = 0.0;

    WayptVec::const_iterator it;
    SGGeod legStart = prevIt->wpt->position();
    for (auto wp : enrouteWaypoints) {
        dist += SGGeodesy::distanceM(legStart, wp->position());
        legStart = wp->position();
    }

    return dist;
}

double RoutePath::trackForIndex(int index) const
{
    if (d->waypoints[index].skipped)
        return trackForIndex(index - 1);

    const WayptData& wd(d->waypoints[index]);
    if (!wd.legCourseValid)
        return 0.0;

    return wd.legCourseTrue;
}

double RoutePath::distanceForIndex(int index) const
{
  return d->waypoints[index].pathDistanceM;
}

double RoutePath::distanceBetweenIndices(int from, int to) const
{
  return d->distanceBetweenIndices(from, to);
}

SGGeod RoutePath::positionForDistanceFrom(int index, double distanceM) const
{
    int sz = (int) d->waypoints.size();
    if (index < 0) {
        index = sz - 1; // map negative values to end of the route
    }

    if ((index < 0) || (index >= sz)) {
        throw sg_range_exception("waypt index out of range",
                                 "RoutePath::positionForDistanceFrom");
    }

    // find the actual leg we're within
    if (distanceM < 0.0) {
        // scan backwards
        while ((index > 0) && (distanceM < 0.0)) {
            // we are looking at index n, say 4, but with a negative distance.
            // we want to look at index n-1 (so, 3), and see if this makes
            // distance positive. We need to offset by distance from 3 -> 4,
            // which is waypoint 4's path distance.
            distanceM += d->waypoints[index].pathDistanceM;
            --index;
        }

        if (distanceM < 0.0) {
            // still negative, return route start
            return d->waypoints[0].pos;
        }

    } else {
        // scan forwards
        int nextIndex = index + 1;
        while ((nextIndex < sz) && (d->waypoints[nextIndex].pathDistanceM < distanceM)) {
            distanceM -= d->waypoints[nextIndex].pathDistanceM;
            index = nextIndex++;
        }
    }

    if ((index + 1) >= sz) {
        // past route end, just return final position
        return d->waypoints[sz - 1].pos;
    }


    const WayptData& wpt(d->waypoints[index]);
    const WayptData& next(d->waypoints[index+1]);
    if (next.wpt->type() == "via") {
        return positionAlongVia(static_cast<Via*>(next.wpt.get()), index, distanceM);
    }

    if (wpt.turnPathDistanceM > distanceM) {
        // on the exit path of current wpt
        return wpt.pointAlongExitPath(distanceM);
    } else {
        distanceM -= wpt.turnPathDistanceM;
    }

    double corePathDistance = next.pathDistanceM - next.turnPathDistanceM;
    if (next.hasEntry && (distanceM > corePathDistance)) {
        // on the entry path of next waypoint
        return next.pointAlongEntryPath(distanceM - corePathDistance);
    }

    // linear between turn exit and turn entry points
    return SGGeodesy::direct(wpt.turnExitPos, next.legCourseTrue, distanceM);
}

SGGeod RoutePath::positionAlongVia(Via* via, int previousIndex, double distanceM) const
{
    SG_LOG(SG_NAVAID, SG_ALERT, "RoutePath::positionAlongVia not implemented");
    return SGGeod();
}
