/** Air Navigation Waypoint Intercept Demonstrator by Robert J Morton 21 Oct 1997*/

/*  Contains data and methods to transact a navigational encounter
    between an aircraft and a waypoint. Each encounter is a transaction
    with a transient lifespan which lasts from when the aircraft enters
    the waypoint's range of influence until it leaves it. In order for an
    encounter to take place, the pilot must select both a waypoint and
    an outgoing radial on which to leave the waypoint. WpEncPG.class uses
    plane geometry. The spherical geometry version is in WpEncSG.class */

class wpencpg {  // Waypoint Encounter (Plane Geometry)

  double
    Pi = Math.PI,       // the circular constant
    TwoPi = Pi + Pi,
    PiBy2 = Pi / 2,
    DegRad = 57.29577,  // number of degrees in a radian
    ISR,                // rationalised inbound selected radial
    OSR,                // rationalised outbound selected radial
    rBrg,               // bearing of waypoint from aircraft
    tBrg,               // bearing of aircraft from waypoint
    RadDev,             // deviation of aircraft from selected radial
    StrOff,             // required steering offset from waypoint
    ReqHdg,             // heading the aircraft must fly
    Dst = 500,          // current distance to waypoint
    PrevDst = 400,      // previously computed distance to waypoint

  boolean
    to = true;  // true if aircraft has not yet passed over the waypoint


  // SET UP FOR APPROACH WITH NEW OUTBOUND RADIAL
  void SetRadial(aircraft ac, waypoint wp, int SR) {
    OSR = SR / DegRad;         // outbound selected radial in radians
    ISR = RatAng(OSR + Pi);    // inbound selected radial in radials
    ac.reset();                // reset aircraft to its starting position
    double dx = wp.X - ac.x;   // x distance between aircraft and waypoint
    double dy = wp.Y - ac.y;   // y distance between aircraft and waypoint

    /* Initialise current and previous distance. Dst = resultant
    distance between aircraft and waypoint. Make the previous
    distance greater than current distance. */

    Dst = Math.sqrt(dx * dx + dy * dy) + 1;
    PrevDst = Dst + 1; 

    to = true;  // indicates that the aircraft is approaching the waypoint
  }


  // return an angle in the range +0 to +360 
  double RatAng(double a) {
    while (a < 0) a += TwoPi;
    while (a > TwoPi) a -= TwoPi;
    return(a);
  }


  // return an angle in the range -180 to + 180
  double BipAng(double a) {
    while (a < -Pi) a += TwoPi;
    while (a > +Pi) a -= TwoPi;
    return(a);
  }


  // COMPUTE THE HEADING ON WHICH THE AIRCRAFT MUST FLY
  double GetReqHdg(aircraft ac) {
    double a, b, c, e;     // see waypoint intercept geometry diagram
    boolean right = true;  // radial deviation is right [false = left]

    /* If the to flag is still set and aircraft is now closer
    than its turning radius and aircraft is now receding from
    the waypoint, then it is deemed to have passed waypoint.*/

    if(to && Dst < ac.r && Dst > PrevDst)
      to = false;

    if(to) {  // IF aircraft is still on its to to the waypoint

      // compute its deviation from the inbound radial
      RadDev = (a = BipAng(tBrg - ISR));

      /* If it is to the left of the inbound
      radial from the waypoint's point of view: */

      if(a < 0) {
        a = -a;         // make 'a' positive
        right = false;  // and set the 'to the left' flag 'true'
      }

      /* The following computations are explained in the Way-
      point Encounter 'Pre-Capture' diagram in AirNav6.html. */

      b = ac.r * Math.cos(a);
      c = Dst - ac.r * Math.sin(a);

      // if aircraft has not yet reached the turning circle
      if((e = b * b + c * c - ac.R) > 0) {

        /* Compute steering offset from waypoint bearing needed
        to keep the aircraft on the tangent to turning circle. */

        StrOff = Math.atan2(ac.r,Math.sqrt(e)) - Math.atan2(b,c);

        if(right)            // if aircraft to the right of the ISR
          StrOff = -StrOff;  // set steering offset negative

        // Set the required heading along the tangent to turning circle.
        ReqHdg = RatAng(rBrg - StrOff);
      }

    } else {  // ELSE if aircraft has already passed the waypoint

      /* Compute the required steering offset a different way.
      The following computations are explained in the Waypoint
      Encounter 'Post-Capture' diagram in AirNav6.html. */

      RadDev = (a = BipAng(tBrg - OSR));
      a = rBrg - a - a - Pi;
      StrOff = BipAng(a - OSR);

      if(StrOff > +1)  // Impose a limit of
        a = OSR + 1;   // plus or minus one radian
      if(StrOff < -1)  // on the steering offset.
        a = OSR - 1;
      ReqHdg = RatAng(a);
    }
    return(ReqHdg);  // return the aircraft's required heading 
  }


  // ADVANCES AIRCRAFT AND UPDATES ALL ENCOUNTER VARIABLES
  boolean outofRange(aircraft ac, waypoint wp) {

    /* If the aircraft still approaching the waypoint and
    it is still more than the turning radius from waypoint
    or it has passed the waypoint and gone out of range: */

    if((Dst < PrevDst &&  Dst > ac.r) ||  Dst < wp.R) {
      double 
        dx = ac.x - wp.X, // longitudinal dist between waypoint & aircraft
        dy = ac.y - wp.Y; // latitudinal dist between waypoint & aircraft
      PrevDst = Dst;      // make last time's dist this time's prev dist

      // compute this time's distance to/from the waypoint
      Dst = Math.sqrt(dx * dx + dy * dy);

      /* compute bearing of waypoint from aircraft. Form 
      bearing of aircraft from waypoint (plane geometry). */

      rBrg = RatAng((tBrg = RatAng(Math.atan2(dx, dy))) + Pi);

      ac.advance(GetReqHdg(ac));  // advance the aircraft along its track
      return false;  // return that the aircraft is still in range
    } 
    return true;  // return that the aircraft has just gone out of range
  }
}