/** Air Navigation Waypoint Intercept Demonstrator
    by Robert J Morton UK-YE572246C 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 way-
    point'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
    π = Math.PI, two_π = π + π,  // the circular constant

    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
    approach = true;  // true if aircraft has not yet passed over 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 + π);  // 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
      dy = wp.Y - ac.y;  // y distance between aircraft and waypoint

    /* Initialise the previous distance to the current distance between the
    aircraft and waypoint (+1 to make it greater than current distance). */

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

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


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


  double BipAng(double a) {  // return an angle in the range -180 to + 180
    while(a < -π)
      a += two_π;
    while(a > +π)
      a -= two_π;
    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;  // indicates whether radial deviation is right or left

    /* If the approach flag is still set AND the aircraft is now
    closer than its turning radius AND the aircraft is now reced-
    ing from the waypoint THEN it is deemed to have passed waypoint. */

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

    if(approach) {  // If aircraft is still on its approach to 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, make 'a' positive
      and set the 'to the left' flag 'true'. */

      if(a < 0) {
        a = -a;
        right = false;
      }

      /* Now is a good time to refer to the Waypoint
      Encounter 'Pre-Capture' diagram in AirNav6.htm. */

      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 the steering offset from the waypoint bearing
        needed to keep the aircraft on tangent to turning circle. */

        StrOff = Math.atan2(ac.r,Math.sqrt(e)) - Math.atan2(b,c);
        
        if(right)            // if the aircraft is to the right of ISR
          StrOff = -StrOff;  // set steering offset negative 

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

      }  // end of 'if aircraft has not yet reached the turning circle'

    } else {  // Else the aircraft has already passed the waypoint, so:

      // Compute the required steering offset a different way
      RadDev = (a = BipAng(tBrg - OSR));

      /* Now is a good time to refer to the Waypoint
      Encounter 'Post-Capture' diagram in AirNav6.htm. */

      a = rBrg - a - a - π;
      StrOff = BipAng(a - OSR);

      if (StrOff > +1)  // Impose a limit of plus or minus
        a = OSR + 1;    // one radian on the steering offset.
      if (StrOff < -1)
        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 is 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) {

      /* Compute the 'longitudinal' and 'latitudinal'
      distances between the waypoint and the aircraft. */

      double
        dx = ac.x - wp.X,
        dy = ac.y - wp.Y;

      PrevDst = Dst;  // Make last time's distance
                      // this time's previous distance.

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

      /* Compute bearing of the waypoint from the aircraft and form the
      bearing of the aircraft from the waypoint (plane geometry). */

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

      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
  }
}