rfa 0.5.9

use crate::{rfam::*, utils::*};
use crate::vector_matrix::{angle_ops::anpm::*};
///  Approximate heliocentric position and velocity of a nominated major
///  planet:  Mercury, Venus, EMB, Mars, Jupiter, Saturn, Uranus or
///  Neptune (but not the Earth itself).
///
///  n.b. Not IAU-endorsed and without canonical status.
///
///  Given:
///     date1  double       TDB date part A (Note 1)
///     date2  double       TDB date part B (Note 1)
///     np     int          planet (1=Mercury, 2=Venus, 3=EMB, 4=Mars,
///                             5=Jupiter, 6=Saturn, 7=Uranus, 8=Neptune)
///
///  Returned (argument):
///     pv     double[2][3] planet p,v (heliocentric, J2000.0, au,au/d)
///
///  Returned (function value):
///            int          status: -1 = illegal NP (outside 1-8)
///                                  0 = OK
///                                 +1 = warning: year outside 1000-3000
///                                 +2 = warning: failed to converge
///
///  Notes:
///
///  1) The date date1+date2 is in the TDB time scale (in practice TT can
///     be used) and is a Julian Date, apportioned in any convenient way
///     between the two arguments.  For example, JD(TDB)=2450123.7 could
///     be expressed in any of these ways, among others:
///
///            date1          date2
///
///         2450123.7           0.0       (JD method)
///         2451545.0       -1421.3       (J2000 method)
///         2400000.5       50123.2       (MJD method)
///         2450123.5           0.2       (date & time method)
///
///     The JD method is the most natural and convenient to use in cases
///     where the loss of several decimal digits of resolution is
///     acceptable.  The J2000 method is best matched to the way the
///     argument is handled internally and will deliver the optimum
///     resolution.  The MJD method and the date & time methods are both
///     good compromises between resolution and convenience.  The limited
///     accuracy of the present algorithm is such that any of the methods
///     is satisfactory.
///
///  2) If an np value outside the range 1-8 is supplied, an error status
///     (function value -1) is returned and the pv vector set to zeroes.
///
///  3) For np=3 the result is for the Earth-Moon Barycenter.  To obtain
///     the heliocentric position and velocity of the Earth, use instead
///     the ERFA function eraEpv00.
///
///  4) On successful return, the array pv contains the following:
///
///        pv[0][0]   x      }
///        pv[0][1]   y      } heliocentric position, au
///        pv[0][2]   z      }
///
///        pv[1][0]   xdot   }
///        pv[1][1]   ydot   } heliocentric velocity, au/d
///        pv[1][2]   zdot   }
///
///     The reference frame is equatorial and is with respect to the
///     mean equator and equinox of epoch J2000.0.
///
///  5) The algorithm is due to J.L. Simon, P. Bretagnon, J. Chapront,
///     M. Chapront-Touze, G. Francou and J. Laskar (Bureau des
///     Longitudes, Paris, France).  From comparisons with JPL
///     ephemeris DE102, they quote the following maximum errors
///     over the interval 1800-2050:
///
///                     L (arcsec)    B (arcsec)      R (km)
///
///        Mercury          4             1             300
///        Venus            5             1             800
///        EMB              6             1            1000
///        Mars            17             1            7700
///        Jupiter         71             5           76000
///        Saturn          81            13          267000
///        Uranus          86             7          712000
///        Neptune         11             1          253000
///
///     Over the interval 1000-3000, they report that the accuracy is no
///     worse than 1.5 times that over 1800-2050.  Outside 1000-3000 the
///     accuracy declines.
///
///     Comparisons of the present function with the JPL DE200 ephemeris
///     give the following RMS errors over the interval 1960-2025:
///
///                      position (km)     velocity (m/s)
///
///        Mercury            334               0.437
///        Venus             1060               0.855
///        EMB               2010               0.815
///        Mars              7690               1.98
///        Jupiter          71700               7.70
///        Saturn          199000              19.4
///        Uranus          564000              16.4
///        Neptune         158000              14.4
///
///     Comparisons against DE200 over the interval 1800-2100 gave the
///     following maximum absolute differences.  (The results using
///     DE406 were essentially the same.)
///
///                   L (arcsec)   B (arcsec)     R (km)   Rdot (m/s)
///
///        Mercury        7            1            500       0.7
///        Venus          7            1           1100       0.9
///        EMB            9            1           1300       1.0
///        Mars          26            1           9000       2.5
///        Jupiter       78            6          82000       8.2
///        Saturn        87           14         263000      24.6
///        Uranus        86            7         661000      27.4
///        Neptune       11            2         248000      21.4
///
///  6) The present RFA re-implementation of the original Simon et al.
///     Fortran code differs from the original in the following respects:
///
///       *  RUST instead of Fortran.
///
///       *  The date is supplied in two parts.
///
///       *  The result is returned only in equatorial Cartesian form;
///          the ecliptic longitude, latitude and radius vector are not
///          returned.
///
///       *  The result is in the J2000.0 equatorial frame, not ecliptic.
///
///       *  More is done in-line: there are fewer calls to subroutines.
///
///       *  Different error/warning status values are used.
///
///       *  A different Kepler's-equation-solver is used (avoiding
///          use of double precision complex).
///
///       *  Polynomials in t are nested to minimize rounding errors.
///
///       *  Explicit double constants are used to avoid mixed-mode
///          expressions.
///
///     None of the above changes affects the result significantly.
///
///  7) The returned status indicates the most serious condition
///     encountered during execution of the function.  Illegal np is
///     considered the most serious, overriding failure to converge,
///     which in turn takes precedence over the remote date warning.
///
///  Called:
///     eraAnpm      normalize angle into range +/- pi
///
///  Reference:  Simon, J.L, Bretagnon, P., Chapront, J.,
///              Chapront-Touze, M., Francou, G., and Laskar, J.,
///              Astron.Astrophys., 282, 663 (1994).
///
///  This revision:  2021 May 11
pub fn plan94(date1: f64, date2: f64, nplanet: i32, pv: &mut [[f64; 3]; 2])->i32
{
    /* Gaussian constant */
       const GK: f64 = 0.017202098950;
    
    /* Sin and cos of J2000.0 mean obliquity (IAU 1976) */
       const SINEPS: f64 = 0.3977771559319137;
       const COSEPS: f64 = 0.9174820620691818;
    
    /* Maximum number of iterations allowed to solve Kepler's equation */
       const KMAX: u8 = 10;
    
    /* Planetary inverse masses */
        const AMAS: [f64; 8]      = [ 6023600.0,       /* Mercury */
                                       408523.5,       /* Venus   */
                                       328900.5,       /* EMB     */
                                      3098710.0,       /* Mars    */
                                         1047.355,     /* Jupiter */
                                         3498.5,       /* Saturn  */
                                        22869.0,       /* Uranus  */
                                        19314.0 ];     /* Neptune */
    
    /*
    /// Tables giving the mean Keplerian elements, limited to t^2 terms:
    ///
    ///   a       semi-major axis (au)
    ///   dlm     mean longitude (degree and arcsecond)
    ///   e       eccentricity
    ///   pi      longitude of the perihelion (degree and arcsecond)
    ///   dinc    inclination (degree and arcsecond)
    ///   omega   longitude of the ascending node (degree and arcsecond)
    */
    
        const A: [[f64;3]; 8] = [
           [  0.3870983098,           0.0,     0.0 ],  /* Mercury */
           [  0.7233298200,           0.0,     0.0 ],  /* Venus   */
           [  1.0000010178,           0.0,     0.0 ],  /* EMB     */
           [  1.5236793419,         3e-10,     0.0 ],  /* Mars    */
           [  5.2026032092,     19132e-10, -39e-10 ],  /* Jupiter */
           [  9.5549091915, -0.0000213896, 444e-10 ],  /* Saturn  */
           [ 19.2184460618,     -3716e-10, 979e-10 ],  /* Uranus  */
           [ 30.1103868694,    -16635e-10, 686e-10 ]   /* Neptune */
       ];
    
        const DLM: [[f64; 3];8] = [
           [ 252.25090552, 5381016286.88982,  -1.92789 ],
           [ 181.97980085, 2106641364.33548,   0.59381 ],
           [ 100.46645683, 1295977422.83429,  -2.04411 ],
           [ 355.43299958,  689050774.93988,   0.94264 ],
           [  34.35151874,  109256603.77991, -30.60378 ],
           [  50.07744430,   43996098.55732,  75.61614 ],
           [ 314.05500511,   15424811.93933,  -1.75083 ],
           [ 304.34866548,    7865503.20744,   0.21103 ]
        ];
    
        const E: [[f64;3]; 8] = [
           [ 0.2056317526,  0.0002040653,    -28349e-10 ],
           [ 0.0067719164, -0.0004776521,     98127e-10 ],
           [ 0.0167086342, -0.0004203654, -0.0000126734 ],
           [ 0.0934006477,  0.0009048438,    -80641e-10 ],
           [ 0.0484979255,  0.0016322542, -0.0000471366 ],
           [ 0.0555481426, -0.0034664062, -0.0000643639 ],
           [ 0.0463812221, -0.0002729293,  0.0000078913 ],
           [ 0.0094557470,  0.0000603263,           0.0 ]
        ];
    
        const PI: [[f64; 3]; 8] = [
           [  77.45611904,  5719.11590,   -4.83016 ],
           [ 131.56370300,   175.48640, -498.48184 ],
           [ 102.93734808, 11612.35290,   53.27577 ],
           [ 336.06023395, 15980.45908,  -62.32800 ],
           [  14.33120687,  7758.75163,  259.95938 ],
           [  93.05723748, 20395.49439,  190.25952 ],
           [ 173.00529106,  3215.56238,  -34.09288 ],
           [  48.12027554,  1050.71912,   27.39717 ]
        ];
    
        const DINC: [[f64;3];8] = [
           [ 7.00498625, -214.25629,   0.28977 ],
           [ 3.39466189,  -30.84437, -11.67836 ],
           [        0.0,  469.97289,  -3.35053 ],
           [ 1.84972648, -293.31722,  -8.11830 ],
           [ 1.30326698,  -71.55890,  11.95297 ],
           [ 2.48887878,   91.85195, -17.66225 ],
           [ 0.77319689,  -60.72723,   1.25759 ],
           [ 1.76995259,    8.12333,   0.08135 ]
        ];
    
        const OMEGA: [[f64;3];8] = [
           [  48.33089304,  -4515.21727,  -31.79892 ],
           [  76.67992019, -10008.48154,  -51.32614 ],
           [ 174.87317577,  -8679.27034,   15.34191 ],
           [  49.55809321, -10620.90088, -230.57416 ],
           [ 100.46440702,   6362.03561,  326.52178 ],
           [ 113.66550252,  -9240.19942,  -66.23743 ],
           [  74.00595701,   2669.15033,  145.93964 ],
           [ 131.78405702,   -221.94322,   -0.78728 ]
        ];
    
    /* Tables for trigonometric terms to be added to the mean elements of */
    /* the semi-major axes */
    
    const KP: [[i32;9]; 8] = [
        [   69613, 75645, 88306, 59899, 15746, 71087, 142173,  3086,    0 ],
        [   21863, 32794, 26934, 10931, 26250, 43725,  53867, 28939,    0 ],
        [   16002, 21863, 32004, 10931, 14529, 16368,  15318, 32794,    0 ],
        [    6345,  7818, 15636,  7077,  8184, 14163,   1107,  4872,    0 ],
        [    1760,  1454,  1167,   880,   287,  2640,     19,  2047, 1454 ],
        [     574,     0,   880,   287,    19,  1760,   1167,   306,  574 ],
        [     204,     0,   177,  1265,     4,   385,    200,   208,  204 ],
        [       0,   102,   106,     4,    98,  1367,    487,   204,    0 ]
       ];
    
    const CA: [[i32;9];8] = [
        [       4,    -13,    11,   -9,    -9,   -3,     -1,     4,     0 ],
        [    -156,     59,   -42,    6,    19,  -20,    -10,   -12,     0 ],
        [      64,   -152,    62,   -8,    32,  -41,     19,   -11,     0 ],
        [     124,    621,  -145,  208,    54,  -57,     30,    15,     0 ],
        [  -23437,  -2634,  6601, 6259, -1507,-1821,   2620, -2115, -1489 ],
        [   62911,-119919, 79336,17814,-24241,12068,   8306, -4893,  8902 ],
        [  389061,-262125,-44088, 8387,-22976,-2093,   -615, -9720,  6633 ],
        [ -412235,-157046,-31430,37817, -9740,  -13,  -7449,  9644,     0 ]
       ];
    
       const SA: [[i32;9];8] = [
        [     -29,    -1,     9,     6,    -6,     5,     4,     0,     0 ],
        [     -48,  -125,   -26,   -37,    18,   -13,   -20,    -2,     0 ],
        [    -150,   -46,    68,    54,    14,    24,   -28,    22,     0 ],
        [    -621,   532,  -694,   -20,   192,   -94,    71,   -73,     0 ],
        [  -14614,-19828, -5869,  1881, -4372, -2255,   782,   930,   913 ],
        [  139737,     0, 24667, 51123, -5102,  7429, -4095, -1976, -9566 ],
        [ -138081,     0, 37205,-49039,-41901,-33872,-27037,-12474, 18797 ],
        [       0, 28492,133236, 69654, 52322,-49577,-26430, -3593,     0 ]
       ];
    
    /* Tables giving the trigonometric terms to be added to the mean */
    /* elements of the mean longitudes */
    
       const KQ: [[i32;10];8] = [
        [   3086,15746,69613,59899,75645,88306, 12661,  2658,    0,     0 ],
        [  21863,32794,10931,   73, 4387,26934,  1473,  2157,    0,     0 ],
        [     10,16002,21863,10931, 1473,32004,  4387,    73,    0,     0 ],
        [     10, 6345, 7818, 1107,15636, 7077,  8184,   532,   10,     0 ],
        [     19, 1760, 1454,  287, 1167,  880,   574,  2640,   19,  1454 ],
        [     19,  574,  287,  306, 1760,   12,    31,    38,   19,   574 ],
        [      4,  204,  177,    8,   31,  200,  1265,   102,    4,   204 ],
        [      4,  102,  106,    8,   98, 1367,   487,   204,    4,   102 ]
       ];
    
       const CL: [[i32;10];8] = [
        [      21,   -95, -157,   41,   -5,   42,  23,  30,      0,     0 ],
        [    -160,  -313, -235,   60,  -74,  -76, -27,  34,      0,     0 ],
        [    -325,  -322,  -79,  232,  -52,   97,  55, -41,      0,     0 ],
        [    2268,  -979,  802,  602, -668,  -33, 345, 201,    -55,     0 ],
        [    7610, -4997,-7689,-5841,-2617, 1115,-748,-607,   6074,   354 ],
        [  -18549, 30125,20012, -730,  824,   23,1289,-352, -14767, -2062 ],
        [ -135245,-14594, 4197,-4030,-5630,-2898,2540,-306,   2939,  1986 ],
        [   89948,  2103, 8963, 2695, 3682, 1648, 866,-154,  -1963,  -283 ]
       ];
    
       const SL: [[i32;10];8] = [
        [   -342,   136,  -23,   62,   66,  -52, -33,    17,     0,     0 ],
        [    524,  -149,  -35,  117,  151,  122, -71,   -62,     0,     0 ],
        [   -105,  -137,  258,   35, -116,  -88,-112,   -80,     0,     0 ],
        [    854,  -205, -936, -240,  140, -341, -97,  -232,   536,     0 ],
        [ -56980,  8016, 1012, 1448,-3024,-3710, 318,   503,  3767,   577 ],
        [ 138606,-13478,-4964, 1441,-1319,-1482, 427,  1236, -9167, -1918 ],
        [  71234,-41116, 5334,-4935,-1848,   66, 434, -1748,  3780,  -701 ],
        [ -47645, 11647, 2166, 3194,  679,    0,-244,  -419, -2531,    48 ]
       ];
    
    /* ------------------------------------------------------------------ */
    
    /* Validate the planet number. */
       if (nplanet < 1) || (nplanet > 8) {
    
       /* Reset the result in case of failure. */
          for k in 0..2 {
             for i in 0..3 {
                pv[k][i] = 0.0;
             }
          }
          return -1;
       } 
    
       /* Decrement the planet number to start at zero. */
          let np = (nplanet-1) as usize;
    
       /* Time: Julian millennia since J2000.0. */
          let t = ((date1 - URSA_DJ00) + date2) / URSA_DJM;
    
       /* OK status unless remote date. */
          let mut jstat = if fabs(t) <= 1.0 {0} else{1};
    
       /* Compute the mean elements. */
          let mut da = A[np][0] +
              (A[np][1] +
               A[np][2] * t) * t;
          let mut dl = (3600.0 * DLM[np][0] +
                        (DLM[np][1] +
                         DLM[np][2] * t) * t) * URSA_DAS2R;
          let de = E[np][0] +
             ( E[np][1] +
               E[np][2] * t) * t;
          let dp = anpm((3600.0 * PI[np][0] +
                                (PI[np][1] +
                                 PI[np][2] * t) * t) * URSA_DAS2R);
          let di = (3600.0 * DINC[np][0] +
                        (DINC[np][1] +
                         DINC[np][2] * t) * t) * URSA_DAS2R;
          let dom = anpm((3600.0 * OMEGA[np][0] +
                                 (OMEGA[np][1] +
                                  OMEGA[np][2] * t) * t) * URSA_DAS2R);
    
       /* Apply the trigonometric terms. */
          let dmu = 0.35953620 * t;
          for k in 0..8 {
             let arga = KP[np][k] as f64 * dmu;
             let argl = KQ[np][k] as f64 * dmu;
             da += (CA[np][k] as f64 * cos(arga) +
                    SA[np][k] as f64 * sin(arga)) * 1e-7;
             dl += (CL[np][k] as f64 * cos(argl) +
                    SL[np][k] as f64 * sin(argl)) * 1e-7;
          }
          let arga = KP[np][8] as f64 * dmu;
          da += t * (CA[np][8] as f64 * cos(arga) +
                     SA[np][8] as f64 * sin(arga)) * 1e-7;
          for k in 8..10 {
             let argl = KQ[np][k] as f64 * dmu;
             dl += t * (CL[np][k] as f64 * cos(argl) +
                        SL[np][k] as f64 * sin(argl)) * 1e-7;
          }
          dl = fmod(dl, URSA_D2PI);
    
       /* Iterative soln. of Kepler's equation to get eccentric anomaly. */
          let am = dl - dp;
          let mut ae = am + de * sin(am);
          let mut k = 0;
          while k < KMAX  {
             let dae = (am - ae + de * sin(ae)) / (1.0 - de * cos(ae));
             ae += dae;
             k+=1;
             if k == KMAX-1{ jstat = 2;}
             if fabs(dae) < 1e-12 {break;}
          }
    
       /* True anomaly. */
          let ae2 = ae / 2.0;
          let at = 2.0 * atan2(sqrt((1.0 + de) / (1.0 - de)) * sin(ae2),
                                                           cos(ae2));
    
       /* Distance (au) and speed (radians per day). */
          let r = da * (1.0 - de * cos(ae));
          let v = GK * sqrt((1.0 + 1.0 / AMAS[np]) / (da * da * da));
    
          let si2 = sin(di / 2.0);
          let xq = si2 * cos(dom);
          let xp = si2 * sin(dom);
          let tl = at + dp;
          let xsw = sin(tl);
          let xcw = cos(tl);
          let xm2 = 2.0 * (xp * xcw - xq * xsw);
          let xf = da / sqrt(1.0  -  de * de);
          let ci2 = cos(di / 2.0);
          let xms = (de * sin(dp) + xsw) * xf;
          let xmc = (de * cos(dp) + xcw) * xf;
          let xpxq2 = 2.0 * xp * xq;
    
       /* Position (J2000.0 ecliptic x,y,z in au). */
          let x = r * (xcw - xm2 * xp);
          let y = r * (xsw + xm2 * xq);
          let z = r * (-xm2 * ci2);
    
       /* Rotate to equatorial. */
          pv[0][0] = x;
          pv[0][1] = y * COSEPS - z * SINEPS;
          pv[0][2] = y * SINEPS + z * COSEPS;
    
       /* Velocity (J2000.0 ecliptic xdot,ydot,zdot in au/d). */
          let x = v * (( -1.0 + 2.0 * xp * xp) * xms + xpxq2 * xmc);
          let y = v * ((  1.0 - 2.0 * xq * xq) * xmc - xpxq2 * xms);
          let z = v * (2.0 * ci2 * (xp * xms + xq * xmc));
    
       /* Rotate to equatorial. */
          pv[1][0] = x;
          pv[1][1] = y * COSEPS - z * SINEPS;
          pv[1][2] = y * SINEPS + z * COSEPS;

    /* Return the status. */
       return jstat;
    
    /* Finished. */
    
}