// routePath.hxx - compute data about planned route
//
// Copyright (C) 2018  James Turner  <james@flightgear.org>
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

#include "config.h"

#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>
#include <Aircraft/AircraftPerformance.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
{
    SGGeod turnCenter;
    double inboundCourseDeg = 0.0;
    double turnAngleDeg = 0.0;
    bool valid = false;
};

/**
 * 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;
using WayptDataVec = std::vector<WayptData>;
using WpDataIt =  WayptDataVec::iterator;

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;
    } // 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)
  {
    const auto ty = wpt->type();
    if ((ty == "vectors") || (ty == "discontinuity")) {
      // relying on the fact vectors will be followed by a static fix/wpt
      if (next && next->posValid) {
        posValid = true;
        pos = next->pos;
      }
    }

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

      // we can compute leg course now
        if (prevWP->type() == "runway") {
            // use the runway departure end pos
            FGRunway* rwy = static_cast<RunwayWaypt*>(prevWP.get())->runway();
            legCourseTrue = SGGeodesy::courseDeg(rwy->end(), pos);
        } else if (ty == "discontinuity") {
            // we want to keep the path across DISCONs sharp (no turn entry/exit)
            // so we keep the leg course invalid, even though we actually
            // could compute it
        } else if (ty != "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() == "via") || (wpt->type() == "discontinuty"))
    {
      // do nothing, we can't compute a valid leg course for these types
      // we'll generate sharp turns in the path but that's no problem.
      return;
    }
    
    if (!previous || !previous->posValid) {
      // all the cases below assume previous pas exists and has a valid position
      SG_LOG(SG_NAVAID, SG_WARN, "RoutePath: Asked to compute leg course, but, previous is invalid");
      return;
    }
    
    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;
      return;
    }
      
      SGGeod previousPos = previous->pos;
      // use correct location for runway, otherwise use threshold
      // and get weird courses
      if (previous->wpt->type() == "runway") {
          FGRunway* rwy = static_cast<RunwayWaypt*>(previous->wpt.get())->runway();
          previousPos = rwy->end();
      }
    
    if (posValid) {
      legCourseTrue = SGGeodesy::courseDeg(previousPos, pos);
      legCourseValid = true;
    } else if (isCourseConstrained()) {
      double magVar = magVarFor(previousPos);
      legCourseTrue = wpt->headingRadialDeg() + magVar;
      legCourseValid = true;
    } // of pos not valid
  }

    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, double maxFlyByTurnAngleDeg, 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;
                // fall through
            } else {
                legCourseValid = true;
                legCourseTrue = next.legCourseTrue;
                turnExitAngle = 0.0;
                turnExitPos = pos;
                flyOver = true;
                return;
            }
        }

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

        if (!flyOver && fabs(turnExitAngle) > maxFlyByTurnAngleDeg) {
            // 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. This matches logic in the RNAV
            // Leg controller
            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;

                    double p = copysign(90, turnExitAngle);
                    const double offsetAngle = legCourseTrue + turnExitAngle - p;
                    SGGeod tc2 = SGGeodesy::direct(turnExitCenter,
                                                   offsetAngle,
                                                   turnRadius * 2.0);
                    
       
                    turnExitPos = SGGeodesy::direct(tc2, next.legCourseTrue + p , turnRadius);
                    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 basicTurnDistance = pathDistanceForTurnAngle(turnExitAngle);
      
      if (distanceM > basicTurnDistance) {
          assert(flyOver);
          assert(overflightCompensationAngle != 0.0);
          double p = copysign(90, turnExitAngle);

          double distanceAlongCompensationTurn = distanceM - basicTurnDistance;
          double theta = (distanceAlongCompensationTurn / turnRadius) * SG_RADIANS_TO_DEGREES;

          // compute the compensation turn center - twice the turn radius
          // from turnCenter
          SGGeod tc2 = SGGeodesy::direct(turnExitCenter,
                                         legCourseTrue + turnExitAngle - p,
                                         turnRadius * 2.0);

          theta = copysign(theta, overflightCompensationAngle);
          return SGGeodesy::direct(tc2,
                                   legCourseTrue + turnExitAngle + theta + p, turnRadius);
      }
      
      double theta = (distanceM / turnRadius) * SG_RADIANS_TO_DEGREES;
      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;
};

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

class RoutePath::RoutePathPrivate
{
public:
    WayptDataVec waypoints;

    AircraftPerformance perf;
    bool constrainLegCourses;
    double maxFlyByTurnAngleDeg = 90.0;

    void computeDynamicPosition(int index)
    {
        auto previous(previousValidWaypoint(index));
        if ((previous == waypoints.end()) || !previous->posValid) {
            SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute position for dynamic waypoint: no preceeding valid waypoint");
            return;
        }

        WayptRef wpt = waypoints[index].wpt;

        const std::string& ty(wpt->type());
        if (ty == "hdgToAlt") {
            HeadingToAltitude* h = (HeadingToAltitude*)wpt.get();

            double altFt = computeVNAVAltitudeFt(index - 1);
            double distanceM = perf.distanceNmBetween(altFt, h->altitudeFt()) * SG_NM_TO_METER;
            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) {
                // try pulling backward along the radial in case we're too close.
                // suggests bad procedure construction if this is happening!
                SGGeoc navidAdjusted = SGGeodesy::advanceDegM(navid, radial, -10 * SG_NM_TO_METER);

                // 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;

            double crs = di->courseDegMagnetic() + magVarFor(wpt->position());
            SGGeoc bPt = SGGeodesy::advanceDegM(prevGc, crs, 1e5);

            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);
      return perf.computePreviousAltitude(distanceM, fixedAlt);
    } else {
      // climb
      int prev = findPreceedingKnownAltitude(index);
      if (prev < 0) {
        return 0.0;
      }
      
      double fixedAlt = altitudeForIndex(prev);
      double distanceM = distanceBetweenIndices(prev, index);
      return perf.computeNextAltitude(distanceM, fixedAlt);
    }
  }
  
  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 distanceBetweenIndices(int from, int to) const
  {
    double total = 0.0;
    
    for (int i=from+1; i<= to; ++i) {
      total += waypoints[i].pathDistanceM;
    }
    
    return total;
  }
  
    WayptDataVec::iterator previousValidWaypoint(unsigned int index)
    {
        do {
            if (index == 0) {
                return waypoints.end();
            }
            
            --index;
        } while (waypoints.at(index).skipped || (waypoints.at(index).wpt->type() == "discontinuity"));

        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->wpt->type() == "discontinuity"))) {
            ++it;
        }
        return it;
    }
}; // of RoutePathPrivate class

RoutePath::RoutePath(const flightgear::FlightPlan* fp) :
  d(new RoutePathPrivate)
{
    for (int l=0; l<fp->numLegs(); ++l) {
        WayptRef 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->constrainLegCourses = fp->followLegTrackToFixes();
    commonInit();
}


RoutePath::RoutePath(const RoutePath& other)
{
    d.reset(new RoutePathPrivate(*other.d));
}

RoutePath& RoutePath::operator=(const RoutePath& other)
{
    d.reset(new RoutePathPrivate(*other.d));
    return *this;
}

RoutePath::~RoutePath()
{
}

void RoutePath::commonInit()
{
  for (auto& w : d->waypoints) {
    w.initPass0();
  }
  
  for (unsigned int i=1; i<d->waypoints.size(); ++i) {
    WayptData* nextPtr = ((i + 1) < d->waypoints.size()) ? &d->waypoints[i+1] : nullptr;
    auto prev = d->previousValidWaypoint(i);
    WayptData* prevPtr = (prev == d->waypoints.end()) ? nullptr : &(*prev);
    d->waypoints[i].initPass1(prevPtr, nextPtr);
  }

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

      double alt = 0.0; // FIXME
      double radiusM = d->perf.turnRadiusMForAltitude(alt);

      if (i > 0) {
          auto prevIt = d->previousValidWaypoint(i);
          WayptData* prevPtr = (prevIt == d->waypoints.end()) ? nullptr : &(*prevIt);

          d->waypoints[i].computeLegCourse(prevPtr, 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, d->maxFlyByTurnAngleDeg, *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
{
  if ((index < 0) || (index >= static_cast<int>(d->waypoints.size()))) {
    return {};
  }

  const WayptData& w(d->waypoints[index]);
  const std::string& ty(w.wpt->type());
  SGGeodVec r;
  
  if (d->waypoints[index].skipped) {
    return {};
  }
  
  // don't show any path
  if (w.wpt->flag(WPT_HIDDEN)) {
    return {};
  }

  // also hide the path if the prev WP is hidden
  auto prevIt = d->previousValidWaypoint(index);
  if (prevIt != d->waypoints.end()) {
      if (prevIt->wpt->flag(WPT_HIDDEN)) {
          return {};
      }
  }

  if (ty == "vectors") {
    // ideally we'd show a stippled line to connect the route?
    return {};
  }
  
  if (ty == "discontinuity") {
    return {}; // no points for a discontinuity of course
  }
  
  if (ty == "via") {
    return pathForVia(static_cast<Via*>(d->waypoints[index].wpt.get()), 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);
  
  // hold is the normal leg and then the hold waypoints as well
  if (ty== "hold") {
    const auto h = static_cast<Hold*>(d->waypoints[index].wpt.get());
    const auto holdPath = pathForHold(h);
    r.insert(r.end(), holdPath.begin(), holdPath.end());
  }
  
  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
{
  if ((index < 0) || (index >= static_cast<int>(d->waypoints.size()))) {
    return {};
  }

  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->perf.groundSpeedForAltitudeKnots(altFt);

    SGGeodVec r;
    double az2;
    double stepTime = turnDelta / 3.0; // 3.0 is degrees/sec for standard rate turn
    double stepDist = gsKts * SG_KT_TO_MPS * stepTime;
    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 >= static_cast<int>(d->waypoints.size()))) {
      return 0.0;
    }

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

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

  auto prevIt = d->previousValidWaypoint(index);
  if (prevIt == d->waypoints.end()) {
    return 0.0;
  }

    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;

    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
{
    const auto sz = static_cast<int>(d->waypoints.size());
    if ((index < 0) || (index >= sz)) {
        return 0.0;
    }

    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
{
    const auto sz = static_cast<int>(d->waypoints.size());
    if ((index < 0) || (index >= sz)) {
        return 0.0;
    }

  return d->waypoints[index].pathDistanceM;
}

double RoutePath::distanceBetweenIndices(int from, int to) const
{
    if (from < 0) {
        from = 0;
    }

    const auto sz = static_cast<int>(d->waypoints.size());
    if (to >= sz) {
        to = sz - 1;
    }

    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.
      
      // note pathDistanceM is 0 for skipped waypoints, so this works out
      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++;
    }
  }
  
  auto nextIt = d->nextValidWaypoint(index);
  if (nextIt == d->waypoints.end()) {
    // past route end, just return final position
    return d->waypoints[sz - 1].pos;
  }
  
  // this is important so we start from a valid WP if we're
  // working either side of a DISCON
  auto curIt = d->previousValidWaypoint(nextIt);
  if (curIt == d->waypoints.end()) {
    SG_LOG(SG_NAVAID, SG_WARN, "Couldn't find valid preceeding waypoint " << index);
    return nextIt->pos;
  }
  
  const WayptData& wpt = *curIt;
  const WayptData& next = *nextIt;
  
  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();
}