//package hehl.gl;

/**
This code is the Java version of C++ code released in conjunction with
the publication

Hehl(2005): C++ and Java code for recursion formulas in mathematical geodesy.
GPS Solutions, Springer, Vol. 9, pp. 51-58.

author    hehl@tfh-berlin.de
version   17-May-2005
*/
public class GL  {

  public GL(Ellipsoid ell, double h, int n) throws Msg {
                init(ell,h,n);
        }

  private void init(Ellipsoid ell, double h, int n) throws Msg {
    _ell = ell;

    if(Math.abs(h()) > 1.0)
      throw new Msg("GL","invalid parameter h="+h);
    _h = h;

    if(n < 2 || n > MAX)
      throw new Msg("GL","invalid order n="+n);

    if(n < 2) n = 2; if(n > MAX) n = MAX;
    _n = n;
    _k2 = new double[order()];
    _k4 = new double[order()];
    koeff();
  }

/** compute meridian on Bessel ellipsoid, max. accuracy */
  public GL() {
                try {
        init(Ellipsoid.BESSEL,0.0,MAX);
    }
    catch(Msg m) { }
  }

/**
compute meridian on ellipsoid 'ell', max. accuracy.
@param ell the reference ellipsoid
*/
  public GL(Ellipsoid ell) {
                try {
        init(ell,0.0,MAX);
                }
                catch(Msg m) { }
  }

/**
compute GL with parameter 'h' on ellipsoid 'ell', max. accuracy.
@param ell the reference ellipsoid
@param h the parameter of GL
*/
  public GL(Ellipsoid ell, double h) throws Msg {
        init(ell,h,MAX);
  }

/**
compute GL from two different points on GL.
@param ell the reference ellipsoid
@param phi1rad ellipsoidal latitude (in [rad]) of first point
@param phi2rad ellipsoidal latitude (in [rad]) of second point
@param dlam12rad difference in ellipsoidal longitudes of the two points
(lam2 - lam1 in [rad])
*/
  public GL(Ellipsoid ell, double phi1rad, double phi2rad, double dlam12rad)
        throws Msg {
    _ell = ell;
    _n = MAX;
    _k2 = new double[order()];
    _k4 = new double[order()];
    _h = 0.0;
    double h_old = 1.E30;
    double beta1 = _ell.phi2beta(phi1rad);
    double beta2 = _ell.phi2beta(phi2rad);
    int iter = 0;
    while(++iter < MAXITER && Math.abs(h()-h_old) > 1.E-15) {
      koeff();
      double v1 = beta2v(beta1);
      double v2 = beta2v(beta2);
      double dLam = dlam12rad - h() * (koeff3()*(v2-v1)
         - 0.5 * Math.sin(2.*v2)*koeff4(v2)+
           0.5 * Math.sin(2.*v1)*koeff4(v1));
      h_old = h();
      _h = 1./Math.sqrt(1.+Math.tan(beta1)*Math.tan(beta1)+
          (Math.pow(Math.tan(beta2)-Math.tan(beta1)*Math.cos(dLam),2) /
          Math.sin(dLam)/Math.sin(dLam)));
    }
    if(iter == MAXITER)
     throw new Msg("lat","could not converge, check input data");
    koeff();
  }

/**
compute GL from two different points on GL.
@param ell the reference ellipsoid
@param phirad ellipsoidal latitude (in [rad]) of first point
@param dlamrad difference in ellipsoidal longitudes of the two points
(lam2 - lam1 in [rad])
*/
  public GL(Ellipsoid ell, double phirad, double dlamrad) throws Msg {
    _ell = ell;
    _n = MAX;
    _k2 = new double[order()];
    _k4 = new double[order()];
    _h = 0.0;
    double h_old = 1.E30;
    double beta = _ell.phi2beta(phirad);
    int iter = 0;
    while(++iter < MAXITER && Math.abs(h()-h_old) > 1.E-15) {
      koeff();
      double v = beta2v(beta);
      double dLam = dlamrad - h() * (koeff3()*(Math.PI/2.-v)+0.5*Math.sin(2.*v)*koeff4(v));
      h_old = h();
      double betaF = Math.atan(Math.tan(beta)/Math.cos(dLam));
      _h = Math.cos(betaF);
    }
    if(iter == MAXITER)
     throw new Msg("lat","could not converge, check input data");
    koeff();
  }

// here the coefficients are computed, private method
  private void koeff() {
    double h2 = 1.-h()*h();
    for(int i=0;i<order();i++) _k2[i] = _k4[i] = 0.0;
    _k4[order()-1] = 1.0;
    double p[] = new double[order()+2];
    double c = 1.0;
    for(int i=0;i<=order()+1;i++) p[i] = 0.0;
    if(!_ell.isSphere()) for(int n=1;n<=order();n++) {
        c *= (2.*n-3.)/2./n*_ell.e2();
        double c1 = c*(2.*n-1.)/(2.*n+2.)*_ell.e2();
        _k4[order()-1] += c;
        double s = 1, t = p[1];
        p[0] = 1.;
        double d = 1.0;
        if(0.0 != h2) for(int i=1;i<=n;i++) {
          d *= (1.-2.*i)/2./i*h2;
          p[i] = s+t; s = t; t = p[i+1];
          for(int j=0;j<i;j++) {
            _k2[j] += d * p[i] * c;
            if(n != order()) _k4[j] += d * p[i] * c1;
          }
        } // end for(i)
      } // end for(n)
  } // end koeff()

/**
@return name of the ellipsoid in use
*/
  public String ellname() { return(_ell.name()); }

/**
@return starting azimuth on equator (in radians)
*/
  public double alpha0() { return(Math.asin(h())); }

/** @return starting azimuth on equator (as an 'Angle') */
//public Angle alpha0() { return(new Angle(alpha0()*Angle.RHOGON)); }

  public double betamax() { return(Math.acos(h())); }
  public double phimax() { return(_ell.beta2phi(betamax())); }
  public boolean isMeridian() { return(1.0E-12 > Math.abs(h())); }
  public double h() { return(_h); }
  public int order() { return(_n); }

  public double alpha(double phirad) throws Msg {
                if(phirad > phimax() || phirad < -phimax())
                        throw new Msg("alpha","Azimut undefined for phi="+phirad+" rad");
    return(Math.asin(h()/Math.cos(_ell.phi2beta(phirad))));
  }

  public double smax() { return(_ell.a()*koeff1()*Math.PI/2.); }
  public double Lammax() { return(Math.PI/2.*(1.+h()*koeff3())); }
  public double koeff1() { return(_k2[0]+_k4[order()-1]); }
  public double koeff3() { return(_k4[0]+_k4[order()-1]-1.); }

  private double k24(int ID, double v) {
    double sin2 = Math.sin(v)*Math.sin(v);
    double result = (ID == 2 ? _k2[0] : _k4[0]);
    double k = 1.0;
    int nmax = (ID == 2 ? order() : order()-1);
    for(int n=1;n<nmax;n++) {
      k *= (2.*n)/(2.*n+1.)*sin2;
      result += k * (ID == 2 ? _k2[n] : _k4[n]);
    }
    return(result);
  }
  public double koeff2(double v) {
    return(k24(2,v));
  }
  public double koeff4(double v) {
    return(k24(4,v));
  }
  public double arc(double phirad) throws Msg {
    double v = phi2v(phirad);
    return(_ell.a()*(v*koeff1() - 0.5*Math.sin(2.*v)*koeff2(v)));
  }

/**
compute ellipsoidal latitude that corresponds to arc length s.
@param s the arclength in [m]
@return the corresponding ellipsoidal latitude in radians.
*/
  public double lat(double s) throws Msg {
    if(s > smax() || s < -smax())
     throw new Msg("lat","has to be implemented...");
    double v0 = s/_ell.a()/koeff1();
    double v = v0,vold = 1.E30;
    int iter = 0;
    while(++iter < MAXITER && Math.abs(v-vold) > 1.E-15) {
      vold = v;
      v = v0 + koeff2(v)/koeff1()/2.*Math.sin(2.*v);
    }
    if(iter == MAXITER)
     throw new Msg("lat","could not converge, check input data");
    return v2phi(v);
  }

//public Angle lat(double s) throws Msg { return new Angle(lat(s)*Angle.RHOGON); }

/**
computes GL-longitude (capital Lambda)
for a point on GL with latitude phi (in [rad]).
@param phirad latitude in [rad]
*/
  public double Lambda(double phirad) throws Msg {
    double beta = _ell.phi2beta(phirad);
    double v = beta2v(beta);
    return Math.asin(h()/Math.cos(beta)*Math.sin(v)) +
        h() * v * koeff3() - h()/2. * Math.sin(2.*v)*koeff4(v);
  }

/**
converts reduced latitude to latitude v.
(maps latitude range [-betamax..+betamax] to [-pi/2..+pi/2])
@param beta reduced latitude in radians
@return the mapping latitude v in radians
*/
  public double beta2v(double beta) throws Msg {
                if(beta > betamax() || beta < -betamax())
                        throw new Msg("beta2v","invalid beta="+beta+" rad");
    return(Math.asin(Math.sin(beta)/Math.sqrt(1.-h()*h())));
  }

  public double v2beta(double v) {
    return(Math.asin(Math.sin(v)*Math.sqrt(1.-h()*h())));
  }

  public double phi2v(double phirad) throws Msg {
    return(beta2v(_ell.phi2beta(phirad)));
  }

  public double v2phi(double v) {
    return(_ell.beta2phi(v2beta(v)));
  }

/** short description of GL */
  public String toString() {
    String info = "Geodesic\n "+_ell+
           "\n order N="+order()+
           "\n h="+h();
    if(isMeridian())
      info += " (Meridian)\n";
     else info += "\n  starting azimuth alpha0="+deg2dms(alpha0()*RHODEG)+
           "\n  min/max latitude phi="+
             deg2dms(phimax()*RHODEG)+"\n";
    return(info);
  }

  static public String deg2dms(double x) {
    int sign = (x < 0.0 ? -1 : 1);
    x = Math.abs(x);
    int d = (int)Math.floor(x);
    int m = (int)Math.floor((x-d)*60.+5.E-15);
    double s = ((x-d)*60.-m)*60.;
    d *= sign;
    return d+"d "+m+"m "+s+"s";
  }

// private data
  static final int    MAX = 8; // max. order of evaluation
  static final int    MAXITER = 15; // max. number of iterations
  static final double RHODEG = 180./Math.PI;
  private Ellipsoid   _ell = Ellipsoid.BESSEL; // ellipsoid in use
  private double      _h; // parameter of GL
  private double      _k2[],_k4[]; // vectors of coefficients
  private int         _n; // order of series expansion

// test routine --
  static public void demo() {
                System.out.println("**************************");
                System.out.println("*** Test of Class 'GL' ***");
                System.out.println("**************************");
                System.out.println();

                try {
    System.out.println("Computation of Geodesics  (c) hehl@tfh-berlin.de\n"+
                       "------------------------------------------------\n");
    Ellipsoid ell = Ellipsoid.BESSEL;

//new GL(ell,.5,10); // test of exeptions

// meridian arcs
    GL g = new GL(ell);
    System.out.println("*** calculate arc length of meridian (example due to Hofmann-Wellenhof)\n"+g);
    System.out.println("circumference="+(4.*g.smax()/1000.0)+" km");
    double phi = 47. + 7./60. + 10.1955/3600.;
    double G = g.arc(phi/RHODEG);
    System.out.println("arc("+deg2dms(phi)+")="+G+" m");
    double phineu = g.lat(G);
    System.out.println("lat("+G+" m)="+deg2dms(phineu*RHODEG)+
        "\n error="+deg2dms(phi-phineu*RHODEG));

// direct problem
    double phi1 = 53.+50./60.+2.8809/3600.,
           lam1 = 10.+12./60.+4.1772/3600.,
           alpha1 = 25.+16./60.+31.96/3600.,
           s12 = 47652.597;
    double h = Math.sin(alpha1/RHODEG)*Math.cos(ell.phi2beta(phi1/RHODEG));
    GL ha1 = new GL(ell,h);
    System.out.print("\n*** direct problem (due to Grossmann)\n"+ha1);
    double s2 = ha1.arc(phi1/RHODEG)+s12;
    System.out.print("s2="+s2+" m");
    double phi2 = ha1.lat(s2);
    System.out.println(" --> phi2="+deg2dms(phi2*RHODEG));
    double alpha2 = ha1.alpha(phi2);
    System.out.println("alpha2="+deg2dms(alpha2*RHODEG));
    double lam2 = lam1/RHODEG + ha1.Lambda(phi2)-ha1.Lambda(phi1/RHODEG);
    System.out.println("lambda2="+deg2dms(lam2*RHODEG));

// indirect problem
    GL ha2 = new GL(ell,phi1/RHODEG,phi2,lam2-lam1/RHODEG);
    System.out.println("\n*** indirect problem\n"+ha2);
    System.out.println("h-h_new="+(ha1.h()-ha2.h()));
    double s12_neu = ha2.arc(phi2)-ha2.arc(phi1/RHODEG);
    System.out.println("s12(computed)="+s12_neu+" m; (error="+(s12-s12_neu)+" m)");
    System.out.println("alpha2(computed)="+deg2dms(ha2.alpha(phi2)*RHODEG));
    System.out.println();

// Soldner
    phi = 48.+24./60.+37.91590/3600.;
    double dlam = 1.+43./60.+16.98120/3600.;
    GL soldner_ordinate = new GL(ell,phi/RHODEG,dlam/RHODEG);
    System.out.println("*** Soldner coordinates (ex due to Hofm.-Wellenhof)\n"
      +soldner_ordinate);
    double y = soldner_ordinate.smax()-soldner_ordinate.arc(phi/RHODEG);
    GL soldner_abszisse = new GL(ell);
    double x = soldner_abszisse.arc(soldner_ordinate.phimax());
    double betaF = soldner_ordinate.betamax();
    double gamma = Math.acos(Math.cos(betaF)/Math.cos(ell.phi2beta(phi/RHODEG)));
    System.out.println("y="+y+"m, x="+x+"m, gamma="+deg2dms(gamma*RHODEG));

    GL neu = new GL(ell);
    double phiF = neu.lat(x);
    betaF = ell.phi2beta(phiF);
    neu = new GL(ell,Math.cos(betaF));
    double y_neu = neu.smax()-y;
    double phi_neu = neu.lat(y_neu);
    System.out.println("phi_new="+deg2dms(phi_neu*RHODEG)+", (error="+
       deg2dms(phi-phi_neu*RHODEG)+")");
    double dlam_neu = neu.Lammax()-neu.Lambda(phi_neu);
    System.out.println("lam_new="+deg2dms(dlam_neu*RHODEG)+", (error="+
       deg2dms(dlam-dlam_neu*RHODEG)+")");
    System.out.println();
                } // end try
                catch(Msg m) {
                        System.err.println(m);
                }
                catch(Exception ex) {
                        System.err.println("\n"+ex);
                }
  } // end demo()

//public static void main(String [] args) {  GL.demo(); }

} // end class GL