/*
  File: Phys.java

  University of Applied Science Berne,HTA-Biel/Bienne,
  Computer Science Department.

  Diploma thesis J3D Solar System Simulator
  Originally written by Marcel Portner & Bernhard Hari (c) 2000
  
  CVS - Information :
  
  $Header: /var/cvsreps/projects/c450/2000/sss3d/source_diploma/sss3d/utils/astronomy/Kepler.java,v 1.4 2000/12/13 13:42:48 portm Exp $
  $Author: portm $
  $Date: 2000/12/13 13:42:48 $
  $State: Exp $
  
*/
package sss3d.utils.astronomy;

import sss3d.calculations.constants.AstronomicalConstants;
import sss3d.calculations.*;

import javax.vecmath.Matrix3d;

/**
 * This class handle the two body problem.
 *
 * @author Marcel Portner & Bernhard Hari
 * @version $Revision: 1.4 $
 */
public class Kepler {

   /**
    * Constructor
    */
   public Kepler() {

   }
   
   /**
    * Calculate the eccentric anomaly for elliptical ways.
    *
    * @param m  middle anomaly in [rad].
    * @param e  eccentric way [0,1[.
    *
    * @return eccentric anomaly in [rad].
    */
   public double eccAnom(double m, double e) {

      int    maxit = 15;
      double eps   = 100.0 * AstronomicalConstants.EPSILON;
   
      int i = 0;
      double _E, f;
   
      // Startwert
      double _M = MoreMath.modulo(m, MoreMath.PI2);   

      if(e < 0.8) {
         _E = _M;
      } else {
         _E = StrictMath.PI;
      }
   
      // Iteration
      do {
         f  = _E - e * StrictMath.sin(_E) - _M;
         _E = _E - f / (1.0 - e * StrictMath.cos(_E) );
         i++;
         if(i == maxit) {
            System.out.println("Konvergenzprobleme in eccAnom");
            break;
         }
      }
      while(StrictMath.abs(f) > eps);

      return _E;
   }
   
   
   /**
    * Calculate the eccentric anomaly for hyperbolic ways.
    *
    * @param mh  middle anomaly in [rad].
    * @param e  eccentric way [0,1[.
    *
    * @return eccentric anomaly in [rad].
    */
   public double hypAnom(double mh, double e) {

      int    maxit = 15;
      double eps   = 100.0 * AstronomicalConstants.EPSILON;
    
      int i = 0;
      double _H, f;
      
      // Startwert
      _H = StrictMath.log(2.0 * StrictMath.abs(mh) / e + 1.8); 
      if(mh < 0.0) {
         _H = -_H; 
      }
    
      // Iteration
      do {
         f = e * MoreMath.sinh(_H) - _H - mh; 
         _H = _H - f / (e * MoreMath.cosh(_H) - 1.0);
         ++i;
         if(i == maxit) {
            System.out.println("Konvergenzprobleme in hypAnom");
            break;
         }
      }
      while(StrictMath.abs(f) > eps * (1.0 + StrictMath.abs(_H + mh)));
    
      return _H;
   }
   
   /**
    * Calculate the place- and velocity vector for elliptical ways.
    *
    * @param gm  product of the constant gravitation and the central mass.
    * @param m  middle anomaly in [rad].
    * @param a  half axle of the way in [AE].
    * @param e  eccentric way < 1.
    *
    * @return an array with the place- and velocity vector.<br>
    *         Vec3D[0] = place vector in [AE].<br>
    *         Vec3D[1] = velocity vector in [AE / d].
    */
   public Vec3D[] ellip(double gm, double m, double a, double e) {
    
      double k = StrictMath.sqrt(gm / a);
    
      double _E = eccAnom(m, e);   
    
      double cosE = StrictMath.cos(_E); 
      double sinE = StrictMath.sin(_E);
    
      double fac = StrictMath.sqrt((1.0 - e) * (1.0 + e));  
    
      double rho = 1.0 - e * cosE;

      Vec3D[] vec = new Vec3D[2];
      vec[0] = new Vec3D(a * (cosE - e),
                         a * fac * sinE,
                         0.0);
      vec[1] = new Vec3D(-k * sinE / rho,
                          k * fac * cosE / rho,
                         0.0);
      
      return vec;
   }
   
   /**
    * Calculate the place- and velocity vector for hyperbolic ways.
    *
    * @param gm  product of the constant gravitation and the central mass.
    * @param t0  time of perihel transit in julianish century.
    * @param t  calculation time in julianish century.
    * @param a  half axle of the way in [AE].
    * @param e  eccentric way > 1.
    *
    * @return an array with the place- and velocity vector.<br>
    *         Vec3D[0] = place vector in [AE].<br>
    *         Vec3D[1] = velocity vector in [AE / d].
    */
   public Vec3D[] hyperb(double gm, double t0, double t, double a, double e) {
    
      double aa = StrictMath.abs(a);
      double k  = StrictMath.sqrt(gm / aa);
    
      double mh = k * (t - t0) / aa;
      double _H = hypAnom(mh, e);
    
      double coshH = MoreMath.cosh(_H);
      double sinhH = MoreMath.sinh(_H);
    
      double fac = StrictMath.sqrt((e + 1.0) * (e - 1.0));  
    
      double rho = e * coshH - 1.0;
    
      Vec3D[] vec = new Vec3D[2];
      vec[0] = new Vec3D(aa * (e - coshH),
                         aa * fac * sinhH,
                         0.0);
      vec[1] = new Vec3D(-k * sinhH / rho,
                          k * fac * coshH / rho,
                         0.0);
      
      return vec;
   }
   
   /**
    * Get the difference between the ephemeriden time and the world time.
    * This method can only calculate the difference of this time from 1825 to 2005.
    *
    * @param e2  eccentric anomaly squared (e2 = e * e) in [rad^2].
    *
    * @return an array with the values:<br>
    *         double[0] = sin(e) / e.<br>
    *         double[1] = (1 - cos(e)) / e^2.<br>
    *         double[2] = (e - sin(e)) / e^3.
    */
   public double[] stumpff(double e2) {

      double eps = 100.0 * AstronomicalConstants.EPSILON;
      double[] c = new double[3];
    
      c[0] = 0.0;
      c[1] = 0.0;
      c[2] = 0.0;
    
      double add = 1.0;
      double n = 1.0;
    
      do {
         c[0] += add;
         add /= (2.0 * n);
         c[1] += add;
         add /= (2.0 * n + 1.0);
         c[2] += add;
         add *= -e2; 
         n += 1.0;
      } while(StrictMath.abs(add) >= eps);
      
      return c;
   }
   
   /**
    * Calculate the place- and velocity vector for parabolically ways.
    *
    * @param gm  product of the constant gravitation and the central mass.
    * @param t0  time of perihel transit in julianish century.
    * @param t  calculation time in julianish century.
    * @param q  perihel distance in [AE].
    * @param e  eccentric way ~ 1.
    *
    * @return an array with the place- and velocity vector.<br>
    *         Vec3D[0] = place vector in [AE].<br>
    *         Vec3D[1] = velocity vector in [AE / d].
    */
   public Vec3D[] parab(double gm, double t0, double t, double q, double e) {

      int    maxit = 15;
      double eps   = 100.0 * AstronomicalConstants.EPSILON;

      int     i = 0;
      double  e2 = 0.0;
      double  e20, u, u2;
      double[] c;
    
      double fac = 0.5 * e;
    
      double k   = StrictMath.sqrt(gm / (q * (1.0 + e))); 
      double tau = StrictMath.sqrt(gm) * (t - t0);
    
      do {
         i++;
     
         e20 = e2;
     
         double a = 1.5 * StrictMath.sqrt(fac / (q * q * q)) * tau;  
         double b = StrictMath.pow(StrictMath.sqrt(a * a + 1.0) + a, 1.0 / 3.0 );    
     
         u  = b - 1.0 / b;
         u2 = u * u;  
         e2 = u2 * (1.0 - e) / fac;
     
         c = stumpff(e2); 
     
         fac = 3.0 * e * c[2];
     
         if(i == maxit) {
            System.out.println("Konvergenzprobleme in parab");
            break;
         }
      } while(StrictMath.abs(e2 - e20) >= eps);
    
      double _R = q * (1.0 + u2 * c[1] * e / fac); 
    
      Vec3D[] vec = new Vec3D[2];
      vec[0] = new Vec3D(q * (1.0 - u2 * c[1] / fac),
                         q * StrictMath.sqrt((1.0 + e) / fac) * u * c[0],
                         0.0);
      vec[1] = new Vec3D(-k * vec[0].y / _R,
                          k * (vec[0].x / _R + e),
                         0.0);
      
      return vec;
   }
   
   /**
    * Calculate the place- and velocity vector for the kepler ways based on ecliptic.
    *
    * @param gm  product of the constant gravitation and the central mass.
    * @param t0  time of perihel transit in julianish century.
    * @param t  calculation time in julianish century.
    * @param q  perihel distance in [AE].
    * @param e  eccentric way.
    * @param pqr  Transform matrix of the plane way -> ecliptic (gauss vector).
    *
    * @return an array with the place- and velocity vector.<br>
    *         Vec3D[0] = place vector in [AE].<br>
    *         Vec3D[1] = velocity vector in [AE / d].
    */
   public Vec3D[] kepler(double gm, double t0, double t, 
                         double q, double e, Matrix3d pqr) {

      double m0  = 0.1; // [rad]
      double eps = 0.1;

      Vec3D[] vec;
    
      double delta = StrictMath.abs(1.0 - e);
    
      double invax = delta / q;
    
      double tau   = StrictMath.sqrt(gm) * (t - t0);
      double m     = tau * StrictMath.sqrt(invax * invax * invax);
    
      if((m < m0) && (delta < eps)) {
         vec = parab(gm, t0, t, q, e);
      } else if(e < 1.0) {
         vec = ellip(gm, m, 1.0 / invax, e);
      } else {
         vec = hyperb(gm, t0, t, 1.0 / invax, e);
      }
      
      vec[0].mul(pqr);
      vec[1].mul(pqr);
      
      return vec;
   }
   
   /**
    * Calculate the transform matrix of the coordinate system in the plane way
    * to ecliptical coordinate.
    *
    * @param h  the length of ascending nodes of the way in [rad].
    * @param i  inclination of the way to the ecliptic in [rad].
    * @param j  argument of the perihel in [rad].
    *
    * @return a transform matrix with the gauss vectors p, q and r.
    */
   public Matrix3d gaussVec(double h, double i, double j) {

      Matrix3d mat1 = new Matrix3d();
      Matrix3d mat2 = new Matrix3d();
      Matrix3d mat3 = new Matrix3d();
      
      mat1.setIdentity();
      mat2.setIdentity();
      mat3.setIdentity();
      
      // R_z(-h) * R_x(-i) * R_z(-j)
      mat1.rotZ(-h);
      mat2.rotX(-i);
      mat3.rotZ(-j);

      mat3.mul(mat2);
      mat3.mul(mat1);

      return mat3;
   }
   
   /**
    * Calculate the elements of a elliptical way of place- and velocity vector.
    *
    * @param gm  product of the constant gravitation and the central mass.
    * @param r  heliocentrically ecliptical place in [AE].
    * @param v  heliocentrically ecliptical velocity in [AE / d].
    *
    * @return a double array with the follow values:<br>
    *         double[0] = big half axle of the way in [AE].<br>
    *         double[1] = eccentric way.<br>
    *         double[2] = inclination of the way to the ecliptic in [rad].<br>
    *         double[3] = the length of ascending nodes of the way in [rad].<br>
    *         double[4] = argument of the perihel in [rad].<br>
    *         double[5] = middle anomaly in [rad].<br>
    */
   public double[] elements(double gm, Vec3D r, Vec3D v) {

      double[] result = new double[6];
    
      Vec3D vec = new Vec3D();
      vec.cross(r, v);                                           // Flaechengeschwindigkeit
      double h = vec.norm();
    
      result[3] = StrictMath.atan2(vec.x, -vec.y);               // Laenge aufst. Knoten

      result[2] = StrictMath.atan2(StrictMath.sqrt(vec.x * vec.x + vec.y * vec.y),
                                   vec.z);                       // Bahnneigung

      double u  = StrictMath.atan2( r.z * h,
                                   -r.x * vec.y + r.y * vec.x);  // Argument der Breite
    
      double _R = r.norm();                                      // Entfernung
      double v2 = v.dot(v);                                      // Quadrat der Geschw.
    
      result[0] = 1.0 / (2.0 / _R - v2 / gm);                    // Grosse Halbachse
    
      double eCosE = 1.0 - _R / result[0];                       // e*cos(E)           
      double eSinE = r.dot(v) / StrictMath.sqrt(gm * result[0]); // e*sin(E)           
    
      double e2 = eCosE * eCosE + eSinE * eSinE;
      result[1] = StrictMath.sqrt(e2);                           // Exzentrizitaet
      double e  = StrictMath.atan2(eSinE, eCosE);                // Exzentrische Anomalie
    
      result[5] = e - eSinE;                                     // Mittlere Anomalie
    
      double nu = StrictMath.atan2(StrictMath.sqrt(1.0 - e2) * eSinE,
                                   eCosE - e2);                  // Wahre Anomalie
    
      result[4] = u - nu;                                        // Argument des Perihels
     
      if(result[3] < 0.0) {
         result[3] += MoreMath.PI2;
      }
      if(result[4] < 0.0) {
         result[4] += MoreMath.PI2;
      }
      if(result[5] < 0.0) {
         result[5] += MoreMath.PI2;
      }
      
      return result;
   }
    
   /**
    * Calculate the elements of a kepler way from two given places.
    *
    * @param gm  product of the constant gravitation and the central mass.
    * @param mjd_a  time of the passage at place A [mjd].
    * @param mjd_b  time of the passage at place B [mjd].
    * @param r_a  place A in heliocentrically ecliptical coordinate in [AU].
    * @param r_b  place B in heliocentrically ecliptical coordinate in [AU].
    *
    * @return a double array with the follow values:<br>
    *         double[0] = time of the perihel transit.<br>
    *         double[1] = perihel distance in [AE].<br>
    *         double[2] = inclination of the way to the ecliptic in [rad].<br>
    *         double[3] = the length of ascending nodes of the way in [rad].<br>
    *         double[4] = argument of the perihel in [rad].<br>
    *         double[5] = middle anomaly in [rad].<br>
    */
   public double[] elements(double gm, double mjd_a, double mjd_b, 
                            Vec3D r_a, Vec3D r_b) {

      double[] result = new double[6];
    
      // Berechnen Vektor r_0 (Anteil von r_b, der senkrecht auf r_a steht)
      // und die Betraege der Vektoren r_a,r_b und r_0
      double s_a = r_a.norm();
      Vec3D e_a = new Vec3D();
      e_a.scale(1.0 / s_a, r_a);
      double s_b = r_b.norm(); 
      double fac = r_b.dot(e_a);
      
      //r_0 = r_b - fac * e_a;
      Vec3D r_0 = new Vec3D();
      r_0.scale(fac, e_a);
      r_0.sub(r_b, r_0);
      double s_0 = r_0.norm();
      Vec3D e_0 = new Vec3D();
      e_0.scale(1.0 / s_0, r_0);
      
      // Bahnneigung und aufsteigender Knoten
      Vec3D r = new Vec3D();
      r.cross(e_a, e_0);
      result[2] = StrictMath.PI / 2.0 - r.getElevation();
      result[3] = MoreMath.modulo(StrictMath.PI / 2.0 + r.getAzimut(),
                                  MoreMath.PI2);

      double u;
      if(result[2] == 0.0) {
        u = StrictMath.atan2(r_a.y, r_a.x);
      } else {
        u = StrictMath.atan2( e_0.x * r.y - e_0.y * r.x,
                             -e_a.x * r.y + e_a.y * r.x);
      }
      
      // Semilatus rectum
      double tau = StrictMath.sqrt(gm) * StrictMath.abs(mjd_b - mjd_a);   
      double eta = findEta(r_a, r_b, tau);
      double p   = StrictMath.pow(s_a * s_0 * eta / tau, 2);   
    
    
      // Exzentrizitaet, wahre Anomalie und Argument des Perihels
      double cos_dnu = fac / s_b;    
      double sin_dnu = s_0 / s_b;
    
      double ecos_nu = p / s_a - 1.0;  
      double esin_nu = (ecos_nu * cos_dnu - (p / s_b - 1.0)) / sin_dnu;
    
      result[5] = StrictMath.sqrt(ecos_nu * ecos_nu + esin_nu * esin_nu);
      double nu = StrictMath.atan2(esin_nu, ecos_nu);
    
      result[4] = MoreMath.modulo(u - nu, MoreMath.PI2);
    
      // Periheldistanz, grosse Halbachse und mittlere Bewegung
      result[1] = p / (1.0 + result[5]);
      double a  = result[1] / (1.0 - result[5]);
      double n  = StrictMath.sqrt(gm / StrictMath.abs(a * a * a) );
    
      double m;
      // Mittlere Anomalie und Zeitpunkt des Periheldurchgangs
      if (result[5] < 1.0) {
        double e = StrictMath.atan2(StrictMath.sqrt((1.0 - result[5]) *
                                    (1.0 + result[5])) * esin_nu,
                                    ecos_nu + result[5] * result[5]);
        m = e - result[5] * StrictMath.sin(e);
      } else {
        double sinhH = StrictMath.sqrt((result[5] - 1.0) * (result[5] + 1.0)) *
                       esin_nu / (result[5] + result[5] * ecos_nu);
        m = result[5] * sinhH -
            StrictMath.log(sinhH + StrictMath.sqrt(1.0 + sinhH * sinhH));
      }
      result[0] = mjd_a - m / n;
      
      return result;
   }

   /**
    * Calculate the sector to triangle ratio of two given places and the meantime.
    *
    * @param r_a  place at time A in [AE].
    * @param r_b  place at time B in [AE].
    * @param tau  time between the passage from A and B (kGauss * dT in [d]).
    *
    * @return the sector to triangle ratio.
    */
   public double findEta(Vec3D r_a, Vec3D r_b, double tau) {

      int     maxit = 30;
      double  delta = 100.0 * AstronomicalConstants.EPSILON;  

      double s_a = r_a.norm();  
      double s_b = r_b.norm();  
    
      double kappa = StrictMath.sqrt(2.0 * (s_a * s_b + r_a.dot(r_b)) );
    
      double m = tau * tau / StrictMath.pow(kappa, 3);   
      double l = (s_a + s_b) / (2.0 * kappa) - 0.5;
    
      double eta_min = StrictMath.sqrt(m / (l + 1.0));
    
      // Start mit der  Hansen'schen Naeherung
      double eta2 = (12.0 + 10.0 *
                     StrictMath.sqrt(1.0 + (44.0 / 9.0) * m / (l + 5.0 / 6.0))) / 22.0;
      double eta1 = eta2 + 0.1;   
    
      // Sekantenverfahren
      double f1 = f(eta1, m, l);   
      double f2 = f(eta2, m, l);  
    
      int    i = 0;
    
      while(Math.abs(f2 - f1) > delta) {
         double d_eta = -f2 * (eta2 - eta1) / (f2 - f1);  
         eta1 = eta2;
         f1 = f2; 
         while(eta2 + d_eta <= eta_min) {
            d_eta *= 0.5;
         }
         eta2 += d_eta;  
         f2 = f(eta2, m, l);
         ++i;
       
         if(i == maxit) {
           System.out.println("Konvergenzprobleme in findEta");;
           break;
         }
      }
      return eta2;
   }

   /**
    * Locale: used from the findEta method.
    *
    * @param eta  a double value.
    * @param m  a double value.
    * @param l  a double value.
    *
    * @return the result of this arithmetic:<br>
    *         f = 1 - eta + (m / eta^2) * w(m / eta^2-l).
    */
   private double f(double eta, double m, double l) {

      double _W = 4.0 / 3.0;
      double eps = 100.0 * AstronomicalConstants.EPSILON;
      double w = m / (eta * eta) - l;
 
      if(StrictMath.abs(w) < 0.1) { // Reihenentwicklung
         double a = _W;
         double n = 0.0;
         do {
            n += 1.0;
            a *= w * (n + 2.0) / (n + 1.5);
            _W += a; 
         } while(StrictMath.abs(a) >= eps);
      } else {
         double g;
         if(w > 0.0) {
            g = 2.0 * StrictMath.asin(StrictMath.sqrt(w));  
            _W = (2.0 * g - StrictMath.sin(2.0 * g)) /
                 StrictMath.pow(StrictMath.sin(g), 3);
         } else {
            g = 2.0 * StrictMath.log(StrictMath.sqrt(-w) +
                                     StrictMath.sqrt(1.0 - w));  // =2.0*arsinh(sqrt(-w))
            _W = (MoreMath.sinh(2.0 * g) - 2.0 * g) /
                 StrictMath.pow(MoreMath.sinh(g), 3);
         }
      }
      return 1.0 - eta + (w + l) * _W;
   }
}