/**
  * Compute Observed's Latitude and Longitude using Distance 
  * plus Observer's Latitude, Longitude and Forward Bearing.
  * Useful for navigating free routes using VOR/DME type aids.
  * @author Robert John Morton
  * @version 09 April 2010 */

/* Solution to the geodetic direct problem after T. Vincenty.
   Modified Rainsford's Method with Helmert's elliptical terms.
   Effective in any azimuth and at any distance short of antipodal.
   Latitudes and longitudes in radians positive north and east.
   Azimuths in radians clockwise from north. Originally programmed
   for the CDC-6600 by Lcdr L Pfeifer NGS Rockville MD 20FEB75.
   Modified for System 360 by John G Gergen NGS Rockville MD 750608 

   Error Code values:
     0  no error
     1  observer's latitude too close to north pole
     2  observer's latitude too close to south pole
     3  observer's longitude out of range
     4  suspect that observed is at observer's antipode
     5  vincenty iterations excessive (equations didn't converge) */


class forward{

  private static final double
    π = 3.14159265358979323846264338327950288,
    δ = 0.5e-13,  // convergence limit for distance accuracy better than 1mm
    P = 1.57050543858623089763516774317834;  // polar limit (1 minute < pi/2)

  private static forward fw;  // reference to current instance of this class

  private double
    φ2,  // latitude of observed
    λ2,  // longitude of observed
    α2,  // back-azimuth (from observed)
    a,   // semi-major axis of the reference ellipsoid
    f,   // flattening factor of the reference ellipsoid
    r,   // semi-minor axis = r * semi-major axis
    q;   // max permitted distance short of antipodal

  private int ic;  // iteration count for vincenty loop


  forward(double a, double f){  // Constructor
    this.a = a;  // semi-major axis of the reference ellipsoid
    this.f = f;  // flattening factor of the reference ellipsoid
    r = 1 - f;   // semi-minor axis = r * semi-major axis
    q = π * r;   // max permitted distance short of antipodal
    fw = this;   // reference to current instance of this class
  }

  public static forward getCurrent(){return fw;}

  public void setaf(double a, double f) {  // set which geo-ellipsoid to use
    this.a = a;  // semi-major axis of the reference ellipsoid
    this.f = f;  // flattening factor of the reference ellipsoid
    r = 1 - f;   // semi-minor axis = r * semi-major axis
    q = π * r;   // max permitted distance short of antipodal
  }

  public double getObservedLat(){return φ2;} //returned in radians
  public double getObservedLng(){return λ2;} //returned in radians
  public double getBackAzimuth(){return α2;} //bearing: observer from observed
  public int getIterations(){return ic;}     //to check for non-convergence


  public int vincenty(  // VINCENTY FORWARD METHOD
    double φ1,  // latitude of observer
    double λ1,  // longitude of observer
    double α1,  // forward azimuth
    double s    // distance to observed position
  ) {
    if(φ1 > P) return 1;   // observer's latitude > 89°59'00"N
    if(φ1 < -P) return 2;  // observer's latitude > 89°59'00"S
    if(λ1 > π || λ1 < -π)  // observer's longitude > 180° E or W
      return 3;

    /* Convert metres to units of semi-major axis. 
    Return if within a gnat's cock of antipodal point. */
    if((s /= a) > q) return 4;

    double
      tanU = r * Math.tan(φ1),  // tangent of observer's reduced latitude
      sinα1 = Math.sin(α1),     // sine of forward azimuth
      cosα1 = Math.cos(α1);     // cosine of forward azimuth

    /* Zero the back-azimuth, provided the forward
    azimuth is not exactly east or west. */

    α2 = 0;
    if(cosα1 != 0)
      α2 = 2 * Math.atan2(tanU, cosα1);
    double
      cosU = 1/Math.sqrt(tanU * tanU + 1),  // cos of observer's reduced lat
      sinU = tanU * cosU,                   // sin of observer's reduced lat
      sina = cosU * sinα1,
      cos2a = 1 - sina * sina,
      x = Math.sqrt((1 / r / r - 1) * cos2a + 1) + 1;
    x= (x - 2) / x; 
    double 
      x2 = x * x,
      d = (0.375 * x2 - 1) * x;
    tanU = s / (r * a * (x2 / 4 + 1) / (1 - x));
    double y = tanU, siny, cosy, cosz, e, Y;

    do{
      siny = Math.sin(y);
      cosy = Math.cos(y);
      cosz = Math.cos(α2 + y);
      e = cosz * cosz * 2 - 1;
      Y = y;
      int v1 = siny * siny * 4 - 3,
          v2 = e + e - 1,
          v3 = cosz * d / 6 + e * cosy,
          v4 = d / 4 - cosz;
      y = tanU + v1 * v2 * v3 * v4 * siny * d;
    } while(Math.abs((y - Y) > δ && ++ic < 100);

    if(ic >= 100) return 5;  // if y didn't converge sufficiently

    α2 = cosU * cosy * cosα1 - sinU * siny;
    φ2 = Math.atan2(
      sinU * cosy 
      + cosU * siny * cosα1, r * Math.sqrt(sina * sina + α2 * α2)
    );

    x = Math.atan2(siny * sinα1, cosU * cosy - sinU * siny * cosα1);
    double c = ((4 - 3 * cos2a) * f + 4) * cos2a * f / 16;
    λ2 = λ1 + x - (1 - c) * ((e * cosy * c + cosz) * siny * c + y) * sina * f;
    α2 = Math.atan2(sina, α2) + π;
    return 0;  // success
  }
}