rkepler 0.4.2

use crate::datetime::mjd::{self, DJCENT, J2000};
use crate::frame::constant::AU;
use crate::frame::iau06::iau06_r;
use crate::frame::iau06::iau06_rv;
use crate::math::angle::{self, ASECRAD, DEGRAD, TURNAS};
use crate::math::matrix::mat_r::{self, mul_p};
use crate::math::matrix::mat_rv;
use crate::math::scalar::scl;
use crate::math::scalar::scl_sv::{self, SclSv};
use crate::math::sphe::sph_p::SphP;
use crate::math::sphe::sph_pv::SphPv;
use crate::math::vector::vec_p::{self, VecP};
use crate::math::vector::vec_pv;
use crate::BodyId;

struct TermVr(i8, i8, i8, i8, f64, f64); // D, Lp, L, F, V amplitude in " and R amplitude in km
const TERMSVR: [TermVr; 75] = [
    TermVr(0, 0, 1, 0, 22639.58578, -20905.35504),
    TermVr(2, 0, -1, 0, 4586.43830, -3699.11092),
    TermVr(2, 0, 0, 0, 2369.91394, -2955.96756),
    TermVr(0, 0, 2, 0, 769.02571, -569.92512),
    TermVr(0, 1, 0, 0, -666.41710, 48.88830),
    TermVr(0, 0, 0, 2, -411.59567, -3.14830),
    TermVr(2, 0, -2, 0, 211.65555, 246.15848),
    TermVr(2, -1, -1, 0, 205.43582, -152.13771),
    TermVr(2, 0, 1, 0, 191.95620, -170.73308),
    TermVr(2, -1, 0, 0, 164.72851, -204.58598),
    TermVr(0, 1, -1, 0, -147.32129, -129.62014),
    TermVr(1, 0, 0, 0, -124.98812, 108.74270),
    TermVr(0, 1, 1, 0, -109.38029, 104.75523),
    TermVr(2, 0, 0, -2, 55.17705, 10.32111),
    TermVr(0, 0, 1, 2, -45.09960, -0.10326),
    TermVr(0, 0, 1, -2, 39.53329, 79.66056),
    TermVr(4, 0, -1, 0, 38.42983, -34.78252),
    TermVr(0, 0, 3, 0, 36.12381, -23.21043),
    TermVr(4, 0, -2, 0, 30.77257, -21.63634),
    TermVr(2, 1, -1, 0, -28.39708, 24.20848),
    TermVr(2, 1, 0, 0, -24.35821, 30.82384),
    TermVr(1, 0, -1, 0, -18.58471, -8.37911),
    TermVr(1, 1, 0, 0, 17.95446, -16.67471),
    TermVr(2, -1, 1, 0, 14.53027, -12.83140),
    TermVr(2, 0, 2, 0, 14.37970, -10.44476),
    TermVr(4, 0, 0, 0, 13.89906, -11.64995),
    TermVr(2, 0, -3, 0, 13.19406, 14.40269),
    TermVr(0, 1, -2, 0, -9.67905, -7.00269),
    TermVr(2, 0, -1, 2, -9.36586, 0.59632),
    TermVr(2, -1, -2, 0, 8.60553, 10.05620),
    TermVr(1, 0, 1, 0, -8.45310, 6.32200),
    TermVr(2, -2, 0, 0, 8.05016, -9.88445),
    TermVr(0, 1, 2, 0, -7.63015, 5.75085),
    TermVr(0, 2, 0, 0, -7.44749, 1.06567),
    TermVr(2, -2, -1, 0, 7.37119, -4.95013),
    TermVr(2, 0, 1, -2, -6.38315, 4.13111),
    TermVr(2, 0, 0, 2, -5.74161, 0.03143),
    TermVr(4, -1, -1, 0, 4.37401, -3.95798),
    TermVr(0, 0, 2, 2, -3.99761, 0.0),
    TermVr(3, 0, -1, 0, -3.20969, 3.25824),
    TermVr(2, 1, 1, 0, -2.91454, 2.61641),
    TermVr(4, -1, -2, 0, 2.73189, -1.89704),
    TermVr(0, 2, -1, 0, -2.56794, -2.11713),
    TermVr(2, 2, -1, 0, -2.52120, 2.35363),
    TermVr(2, 1, -2, 0, 2.48889, 0.14368),
    TermVr(2, -1, 0, -2, 2.14607, 0.65706),
    TermVr(4, 0, 1, 0, 1.97773, -1.42255),
    TermVr(0, 0, 4, 0, 1.93368, -1.11694),
    TermVr(4, -1, 0, 0, 1.87076, -1.57139),
    TermVr(1, 0, -2, 0, -1.75297, -1.73853),
    TermVr(2, 1, 0, -2, -1.43716, -0.13571),
    TermVr(0, 0, 2, -2, -1.37257, -4.42118),
    TermVr(1, 1, 1, 0, 1.26182, -0.93332),
    TermVr(3, 0, -2, 0, -1.22412, 0.86243),
    TermVr(4, 0, -3, 0, 1.18683, -0.51423),
    TermVr(2, -1, 2, 0, 1.17700, -0.84880),
    TermVr(0, 2, 1, 0, -1.16169, 1.16553),
    TermVr(1, 1, -1, 0, 1.07769, 0.85124),
    TermVr(2, 0, 3, 0, 1.05950, -0.66968),
    TermVr(2, 0, 1, 2, -0.99022, 0.0),
    TermVr(2, 0, -4, 0, 0.94828, 0.77854),
    TermVr(2, -2, 1, 0, 0.75168, -0.65753),
    TermVr(0, 1, -3, 0, -0.66938, -0.42241),
    TermVr(4, 1, -1, 0, -0.63521, 0.57879),
    TermVr(1, 0, 2, 0, -0.58399, 0.37852),
    TermVr(1, 0, 0, -2, -0.58331, -0.79563),
    TermVr(6, 0, -2, 0, 0.57156, -0.42250),
    TermVr(2, 0, -2, -2, -0.56064, 0.47262),
    TermVr(1, -1, 0, 0, -0.55692, 0.49755),
    TermVr(0, 1, 3, 0, -0.54592, 0.35508),
    TermVr(2, 0, -2, 2, -0.53571, 0.77404),
    TermVr(2, -1, -3, 0, 0.47840, 0.49504),
    TermVr(3, 0, 0, 0, 0.40423, -1.41893),
    TermVr(2, 0, -1, -2, 0.17903, 8.75156),
    TermVr(4, 0, 0, -2, -0.02391, -0.50792),
];

struct TermU(i8, i8, i8, i8, f64); // D, Lp, L, F, U amplitude in "
const TERMSU: [TermU; 59] = [
    TermU(0, 0, 0, 1, 18461.23868),
    TermU(0, 0, 1, 1, 1010.16707),
    TermU(0, 0, 1, -1, 999.69358),
    TermU(2, 0, 0, -1, 623.65243),
    TermU(2, 0, -1, 1, 199.48374),
    TermU(2, 0, -1, -1, 166.57410),
    TermU(2, 0, 0, 1, 117.26069),
    TermU(0, 0, 2, 1, 61.91195),
    TermU(2, 0, 1, -1, 33.35720),
    TermU(0, 0, 2, -1, 31.75967),
    TermU(2, -1, 0, -1, 29.57658),
    TermU(2, 0, -2, -1, 15.56626),
    TermU(2, 0, 1, 1, 15.12155),
    TermU(2, 1, 0, -1, -12.09414),
    TermU(2, -1, -1, 1, 8.86814),
    TermU(2, -1, 0, 1, 7.95855),
    TermU(2, -1, -1, -1, 7.43455),
    TermU(0, 1, -1, -1, -6.73143),
    TermU(4, 0, -1, -1, 6.57957),
    TermU(0, 1, 0, 1, -6.46007),
    TermU(0, 0, 0, 3, -6.29648),
    TermU(0, 1, -1, 1, -5.63235),
    TermU(1, 0, 0, 1, -5.36840),
    TermU(0, 1, 1, 1, -5.31127),
    TermU(0, 1, 1, -1, -5.07591),
    TermU(0, 1, 0, -1, -4.83961),
    TermU(1, 0, 0, -1, -4.80574),
    TermU(0, 0, 3, 1, 3.98405),
    TermU(4, 0, 0, -1, 3.67446),
    TermU(4, 0, -1, 1, 2.99848),
    TermU(0, 0, 1, -3, 2.79864),
    TermU(4, 0, -2, 1, 2.41388),
    TermU(2, 0, 0, -3, 2.18631),
    TermU(2, 0, 2, -1, 2.14617),
    TermU(2, -1, 1, -1, 1.76598),
    TermU(2, 0, -2, 1, -1.62442),
    TermU(0, 0, 3, -1, 1.58130),
    TermU(2, 0, 2, 1, 1.51975),
    TermU(2, 0, -3, -1, 1.51563),
    TermU(2, 1, -1, 1, -1.31782),
    TermU(2, 1, 0, 1, -1.26427),
    TermU(4, 0, 0, 1, 1.19187),
    TermU(2, -1, 1, 1, 1.13461),
    TermU(2, -2, 0, -1, 1.08578),
    TermU(0, 0, 1, 3, -1.01938),
    TermU(2, 1, 1, -1, -0.82271),
    TermU(1, 1, 0, -1, 0.80422),
    TermU(1, 1, 0, 1, 0.80259),
    TermU(0, 1, -2, -1, -0.79319),
    TermU(2, 1, -1, -1, -0.79101),
    TermU(1, 0, 1, 1, -0.66741),
    TermU(2, -1, -2, -1, 0.65022),
    TermU(0, 1, 2, 1, -0.63881),
    TermU(4, 0, -2, -1, 0.63371),
    TermU(4, -1, -1, -1, 0.59577),
    TermU(1, 0, 1, -1, -0.58893),
    //additional terms
    TermU(4, 0, 1, -1, 0.47338),
    TermU(1, 0, -1, -1, -0.42989),
    TermU(4, -1, 0, -1, 0.41494),
];

struct TermP(i8, i8, i8, i8, i8, i8, i8, i8, i8, f64, f64); //Ve, Te, Ma, Ju, Lm, D, Lp, L, F, phase, ampl in " or km
const TERMSPV0: [TermP; 10] = [
    TermP(18, -16, 0, 0, 0, 0, 0, -1, 0, 26.54261 * DEGRAD, 14.24883),
    TermP(0, 0, 0, 0, 1, 0, 0, 0, -1, 0.00094 * DEGRAD, 7.06304),
    TermP(0, 2, 0, -2, 0, 2, 0, -1, 0, 180.11977 * DEGRAD, 1.14307),
    TermP(0, 4, -8, 3, 0, 0, 0, 0, 0, 285.98707 * DEGRAD, 0.90114),
    TermP(1, -1, 0, 0, 0, 0, 0, 0, 0, 180.00988 * DEGRAD, 0.82155),
    TermP(18, -16, 0, 0, 0, 0, 0, -2, 0, 26.54324 * DEGRAD, 0.78811),
    TermP(18, -16, 0, 0, 0, 0, 0, 0, 0, 26.54560 * DEGRAD, 0.73930),
    TermP(3, -3, 0, 0, 0, 2, 0, -1, 0, 179.98144 * DEGRAD, 0.64371),
    TermP(0, 1, 0, -1, 0, 0, 0, 0, 0, 1.22890 * DEGRAD, 0.63880),
    TermP(10, -3, 0, 0, 0, 0, 0, -1, 0, 333.30551 * DEGRAD, 0.56341),
];
const TERMSPV1: [TermP; 16] = [
    TermP(0, 0, 0, 0, 0, 0, 1, 0, 0, 0.00000 * DEGRAD, 1.67680),
    TermP(0, 0, 0, 0, 0, 2, -1, -1, 0, 180.00000 * DEGRAD, 0.51642),
    TermP(0, 0, 0, 0, 0, 2, -1, 0, 0, 180.00000 * DEGRAD, 0.41383),
    TermP(0, 0, 0, 0, 0, 0, 1, -1, 0, 0.00000 * DEGRAD, 0.37115),
    TermP(0, 0, 0, 0, 0, 0, 1, 1, 0, 0.00000 * DEGRAD, 0.27560),
    TermP(18, -16, 0, 0, 0, 0, 0, -1, 0, 114.56550 * DEGRAD, 0.25425),
    TermP(0, 0, 0, 0, 0, 2, 1, -1, 0, 0.00000 * DEGRAD, 0.07118),
    TermP(0, 0, 0, 0, 0, 2, 1, 0, 0, 0.00000 * DEGRAD, 0.06128),
    TermP(0, 0, 0, 0, 0, 1, 1, 0, 0, 180.00000 * DEGRAD, 0.04516),
    TermP(0, 0, 0, 0, 0, 2, -2, 0, 0, 180.00000 * DEGRAD, 0.04048),
    TermP(0, 0, 0, 0, 0, 0, 2, 0, 0, 0.00000 * DEGRAD, 0.03747),
    TermP(0, 0, 0, 0, 0, 2, -2, -1, 0, 180.00000 * DEGRAD, 0.03707),
    TermP(0, 0, 0, 0, 0, 2, -1, 1, 0, 180.00000 * DEGRAD, 0.03649),
    TermP(0, 0, 0, 0, 0, 0, 1, -2, 0, 0.00000 * DEGRAD, 0.02438),
    TermP(0, 0, 0, 0, 0, 2, -1, -2, 0, 180.00000 * DEGRAD, 0.02165),
    TermP(0, 0, 0, 0, 0, 0, 1, 2, 0, 0.00000 * DEGRAD, 0.01923),
];

const TERMSPV2: [TermP; 5] = [
    TermP(0, 0, 0, 0, 0, 0, 1, 0, 0, 0.00000 * DEGRAD, 0.00487),
    TermP(0, 0, 0, 0, 0, 2, -1, -1, 0, 180.00000 * DEGRAD, 0.00150),
    TermP(0, 0, 0, 0, 0, 2, -1, 0, 0, 180.00000 * DEGRAD, 0.00120),
    TermP(0, 0, 0, 0, 0, 0, 1, -1, 0, 0.00000 * DEGRAD, 0.00108),
    TermP(0, 0, 0, 0, 0, 0, 1, 1, 0, 0.00000 * DEGRAD, 0.00080),
];

const TERMSPU0: [TermP; 5] = [
    TermP(0, 0, 0, 0, 1, 0, 0, 0, 0, 180.00071 * DEGRAD, 8.04508),
    TermP(0, 1, 0, 0, 0, 1, 0, 0, 0, 276.68007 * DEGRAD, 1.51021),
    TermP(18, -16, 0, 0, 0, 0, 0, -1, 1, 26.54287 * DEGRAD, 0.63037),
    TermP(18, -16, 0, 0, 0, 0, 0, -1, -1, 26.54272 * DEGRAD, 0.63014),
    //additional terms
    TermP(0, 0, 0, 0, 1, 0, 0, -1, 0, 0.00075 * DEGRAD, 0.45586),
];

const TERMSPU1: [TermP; 7] = [
    TermP(0, 0, 0, 0, 0, 2, -1, 0, -1, 180.00000 * DEGRAD, 0.07430),
    TermP(0, 0, 0, 0, 0, 2, 1, 0, -1, 0.00000 * DEGRAD, 0.03043),
    TermP(0, 0, 0, 0, 0, 2, -1, -1, 1, 180.00000 * DEGRAD, 0.02229),
    TermP(0, 0, 0, 0, 0, 2, -1, 0, 1, 180.00000 * DEGRAD, 0.01999),
    TermP(0, 0, 0, 0, 0, 2, -1, -1, -1, 180.00000 * DEGRAD, 0.01869),
    TermP(0, 0, 0, 0, 0, 0, 1, -1, -1, 0.00000 * DEGRAD, 0.01696),
    TermP(0, 0, 0, 0, 0, 0, 1, 0, 1, 0.00000 * DEGRAD, 0.01623),
];

const TERMSPR0: [TermP; 6] = [
    TermP(0, 2, 0, -2, 0, 2, 0, -1, 0, 90.11969 * DEGRAD, 1.05870),
    //additional terms
    TermP(18, -16, 0, 0, 0, 0, 0, -2, 0, 116.54311 * DEGRAD, 0.72783),
    TermP(18, -16, 0, 0, 0, 0, 0, 0, 0, 296.54574 * DEGRAD, 0.68256),
    TermP(3, -3, 0, 0, 0, 2, 0, -1, 0, 89.98187 * DEGRAD, 0.59827),
    TermP(0, 0, 0, 0, 1, 0, 0, 1, -1, 270.00126 * DEGRAD, 0.45648),
    TermP(0, 0, 0, 0, 1, 0, 0, -1, -1, 90.00128 * DEGRAD, 0.45276),
];

const TERMSPR1: [TermP; 10] = [
    TermP(0, 0, 0, 0, 0, 2, -1, 0, 0, 90.00000 * DEGRAD, 0.51395),
    TermP(0, 0, 0, 0, 0, 2, -1, -1, 0, 90.00000 * DEGRAD, 0.38245),
    TermP(0, 0, 0, 0, 0, 0, 1, -1, 0, 90.00000 * DEGRAD, 0.32654),
    TermP(0, 0, 0, 0, 0, 0, 1, 1, 0, 270.00000 * DEGRAD, 0.26396),
    TermP(0, 0, 0, 0, 0, 0, 1, 0, 0, 270.00000 * DEGRAD, 0.12302),
    TermP(0, 0, 0, 0, 0, 2, 1, 0, 0, 270.00000 * DEGRAD, 0.07754),
    TermP(0, 0, 0, 0, 0, 2, 1, -1, 0, 270.00000 * DEGRAD, 0.06068),
    TermP(0, 0, 0, 0, 0, 2, -2, 0, 0, 90.00000 * DEGRAD, 0.04970),
    TermP(0, 0, 0, 0, 0, 1, 1, 0, 0, 90.00000 * DEGRAD, 0.04194),
    TermP(0, 0, 0, 0, 0, 2, -1, 1, 0, 90.00000 * DEGRAD, 0.03222),
];

const TERMSPR2: [TermP; 2] = [
    TermP(0, 0, 0, 0, 0, 2, -1, 0, 0, 90.00000 * DEGRAD, 0.00149),
    TermP(0, 0, 0, 0, 0, 2, -1, -1, 0, 90.00000 * DEGRAD, 0.00111),
];

const AL: [f64; 5] = [
    134.0 * 3600.0 + 57.0 * 60.0 + 48.28096,
    1717915923.4728,
    32.3893,
    0.051651,
    -0.0002447,
];
const ALP: [f64; 4] = [
    357.0 * 3600.0 + 31.0 * 60.0 + 44.79306,
    129596581.0474,
    -0.5529,
    0.000147,
];
const AD: [f64; 5] = [
    297.0 * 3600.0 + 51.0 * 60.0 + 0.73512,
    1602961601.4603,
    -5.8681,
    0.006595,
    -0.00003184,
];
const AF: [f64; 5] = [
    93.0 * 3600.0 + 16.0 * 60.0 + 19.55755,
    1739527263.0983,
    -12.2505,
    -0.001021,
    0.00000417,
];

const ALM: [f64; 5] = [
    218.31664563 * 3600.0,
    1732564372.30470,
    -5.2790,
    0.006665,
    -0.00005522,
];

fn elp85_main(dtc: f64) -> SphP {
    let al = scl::polynom(dtc, &AL) % TURNAS * ASECRAD;
    let alp = scl::polynom(dtc, &ALP) % TURNAS * ASECRAD;
    let ad = scl::polynom(dtc, &AD) % TURNAS * ASECRAD;
    let af = scl::polynom(dtc, &AF) % TURNAS * ASECRAD;

    let mut v0 = 0.0;
    let mut u0 = 0.0;
    let mut r0 = 385000.52899;

    for j in 0..TERMSVR.len() {
        let fr = TERMSVR[j].0 as f64 * ad
            + TERMSVR[j].1 as f64 * alp
            + TERMSVR[j].2 as f64 * al
            + TERMSVR[j].3 as f64 * af;
        v0 += TERMSVR[j].4 * fr.sin();
        r0 += TERMSVR[j].5 * fr.cos();
    }
    for j in 0..TERMSU.len() {
        u0 += TERMSU[j].4
            * (TERMSU[j].0 as f64 * ad
                + TERMSU[j].1 as f64 * alp
                + TERMSU[j].2 as f64 * al
                + TERMSU[j].3 as f64 * af)
                .sin();
    }
    SphP {
        lon: v0 * ASECRAD,
        lat: u0 * ASECRAD,
        rad: r0 / AU,
    }
}

fn elp85_main_pv(dtc: f64) -> SphPv {
    let al = scl_sv::scale(ASECRAD, &scl_sv::poly_norm(dtc, &AL, TURNAS));
    let alp = scl_sv::scale(ASECRAD, &scl_sv::poly_norm(dtc, &ALP, TURNAS));
    let ad = scl_sv::scale(ASECRAD, &scl_sv::poly_norm(dtc, &AD, TURNAS));
    let af = scl_sv::scale(ASECRAD, &scl_sv::poly_norm(dtc, &AF, TURNAS));

    let mut v0 = 0.0;
    let mut u0 = 0.0;
    let mut r0 = 385000.52899;
    let mut vv0 = 0.0;
    let mut vu0 = 0.0;
    let mut vr0 = 0.0;

    for j in 0..TERMSVR.len() {
        let fr = TERMSVR[j].0 as f64 * ad.s
            + TERMSVR[j].1 as f64 * alp.s
            + TERMSVR[j].2 as f64 * al.s
            + TERMSVR[j].3 as f64 * af.s;
        let vfr = TERMSVR[j].0 as f64 * ad.v
            + TERMSVR[j].1 as f64 * alp.v
            + TERMSVR[j].2 as f64 * al.v
            + TERMSVR[j].3 as f64 * af.v;
        v0 += TERMSVR[j].4 * fr.sin();
        r0 += TERMSVR[j].5 * fr.cos();
        vv0 += TERMSVR[j].4 * fr.cos() * vfr;
        vr0 += -TERMSVR[j].5 * fr.sin() * vfr;
    }
    for j in 0..TERMSU.len() {
        let fr = TERMSU[j].0 as f64 * ad.s
            + TERMSU[j].1 as f64 * alp.s
            + TERMSU[j].2 as f64 * al.s
            + TERMSU[j].3 as f64 * af.s;
        let vfr = TERMSU[j].0 as f64 * ad.v
            + TERMSU[j].1 as f64 * alp.v
            + TERMSU[j].2 as f64 * al.v
            + TERMSU[j].3 as f64 * af.v;
        u0 += TERMSU[j].4 * fr.sin();
        vu0 += TERMSU[j].4 * fr.cos() * vfr;
    }

    SphPv {
        lon: v0 * ASECRAD,
        lat: u0 * ASECRAD,
        rad: r0 / AU,
        vlon: vv0 * ASECRAD / DJCENT,
        vlat: vu0 * ASECRAD / DJCENT,
        vrad: vr0 / AU / DJCENT,
    }
}

fn elp85_pert(dtc: f64) -> SphP {
    let dtc2 = dtc * dtc;

    let al = scl::polynom(dtc, &AL) % TURNAS * ASECRAD;
    let alp = scl::polynom(dtc, &ALP) % TURNAS * ASECRAD;
    let ad = scl::polynom(dtc, &AD) % TURNAS * ASECRAD;
    let af = scl::polynom(dtc, &AF) % TURNAS * ASECRAD;
    // let alm = (218.0 * 3600.0 + 18.0 * 60.0 + 59.95571 + 1732564372.83264 * dtc - 4.7763 * dtc2
    //     + 0.006681 * dtc3
    //     - 0.00005522 * dtc4)
    //     % angle::TURNAS
    //     * ASECRAD;
    let alm = scl::polynom(dtc, &ALM) % TURNAS * ASECRAD;
    let ave = (181.0 * 3600.0 + 58.0 * 60.0 + 47.28305 + 210664136.43355 * dtc) % TURNAS * ASECRAD;
    let ate = (100.0 * 3600.0 + 27.0 * 60.0 + 59.22059 + 129597742.2758 * dtc) % TURNAS * ASECRAD;
    let ama = (355.0 * 3600.0 + 25.0 * 60.0 + 59.78866 + 68905077.59284 * dtc) % TURNAS * ASECRAD;
    let aju = (34.0 * 3600.0 + 21.0 * 60.0 + 5.34212 + 10925660.42861 * dtc) % TURNAS * ASECRAD;

    let mut pv0 = 0.0;
    let mut pv1 = 0.0;
    let mut pv2 = 0.0;
    let mut pu0 = 0.0;
    let mut pu1 = 0.0;
    let mut pr0 = 0.0;
    let mut pr1 = 0.0;
    let mut pr2 = 0.0;

    for j in 0..TERMSPV0.len() {
        let fr = TERMSPV0[j].0 as f64 * ave
            + TERMSPV0[j].1 as f64 * ate
            + TERMSPV0[j].2 as f64 * ama
            + TERMSPV0[j].3 as f64 * aju
            + TERMSPV0[j].4 as f64 * alm
            + TERMSPV0[j].5 as f64 * ad
            + TERMSPV0[j].6 as f64 * alp
            + TERMSPV0[j].7 as f64 * al
            + TERMSPV0[j].8 as f64 * af;
        pv0 += TERMSPV0[j].10 * (TERMSPV0[j].9 + fr).sin();
    }
    for j in 0..TERMSPV1.len() {
        let fr = TERMSPV1[j].0 as f64 * ave
            + TERMSPV1[j].1 as f64 * ate
            + TERMSPV1[j].2 as f64 * ama
            + TERMSPV1[j].3 as f64 * aju
            + TERMSPV1[j].4 as f64 * alm
            + TERMSPV1[j].5 as f64 * ad
            + TERMSPV1[j].6 as f64 * alp
            + TERMSPV1[j].7 as f64 * al
            + TERMSPV1[j].8 as f64 * af;
        pv1 += TERMSPV1[j].10 * (TERMSPV1[j].9 + fr).sin();
    }
    for j in 0..TERMSPV2.len() {
        let fr = TERMSPV2[j].0 as f64 * ave
            + TERMSPV2[j].1 as f64 * ate
            + TERMSPV2[j].2 as f64 * ama
            + TERMSPV2[j].3 as f64 * aju
            + TERMSPV2[j].4 as f64 * alm
            + TERMSPV2[j].5 as f64 * ad
            + TERMSPV2[j].6 as f64 * alp
            + TERMSPV2[j].7 as f64 * al
            + TERMSPV2[j].8 as f64 * af;
        pv2 += TERMSPV2[j].10 * (TERMSPV2[j].9 + fr).sin();
    }

    for j in 0..TERMSPU0.len() {
        let fr = TERMSPU0[j].0 as f64 * ave
            + TERMSPU0[j].1 as f64 * ate
            + TERMSPU0[j].2 as f64 * ama
            + TERMSPU0[j].3 as f64 * aju
            + TERMSPU0[j].4 as f64 * alm
            + TERMSPU0[j].5 as f64 * ad
            + TERMSPU0[j].6 as f64 * alp
            + TERMSPU0[j].7 as f64 * al
            + TERMSPU0[j].8 as f64 * af;
        pu0 += TERMSPU0[j].10 * (TERMSPU0[j].9 + fr).sin();
    }
    for j in 0..TERMSPU1.len() {
        let fr = TERMSPU1[j].0 as f64 * ave
            + TERMSPU1[j].1 as f64 * ate
            + TERMSPU1[j].2 as f64 * ama
            + TERMSPU1[j].3 as f64 * aju
            + TERMSPU1[j].4 as f64 * alm
            + TERMSPU1[j].5 as f64 * ad
            + TERMSPU1[j].6 as f64 * alp
            + TERMSPU1[j].7 as f64 * al
            + TERMSPU1[j].8 as f64 * af;
        pu1 += TERMSPU1[j].10 * (TERMSPU1[j].9 + fr).sin();
    }

    for j in 0..TERMSPR0.len() {
        let fr = TERMSPR0[j].0 as f64 * ave
            + TERMSPR0[j].1 as f64 * ate
            + TERMSPR0[j].2 as f64 * ama
            + TERMSPR0[j].3 as f64 * aju
            + TERMSPR0[j].4 as f64 * alm
            + TERMSPR0[j].5 as f64 * ad
            + TERMSPR0[j].6 as f64 * alp
            + TERMSPR0[j].7 as f64 * al
            + TERMSPR0[j].8 as f64 * af;
        pr0 += TERMSPR0[j].10 * (TERMSPR0[j].9 + fr).sin();
    }
    for j in 0..TERMSPR1.len() {
        let fr = TERMSPR1[j].0 as f64 * ave
            + TERMSPR1[j].1 as f64 * ate
            + TERMSPR1[j].2 as f64 * ama
            + TERMSPR1[j].3 as f64 * aju
            + TERMSPR1[j].4 as f64 * alm
            + TERMSPR1[j].5 as f64 * ad
            + TERMSPR1[j].6 as f64 * alp
            + TERMSPR1[j].7 as f64 * al
            + TERMSPR1[j].8 as f64 * af;
        pr1 += TERMSPR1[j].10 * (TERMSPR1[j].9 + fr).sin();
    }
    for j in 0..TERMSPR2.len() {
        let fr = TERMSPR2[j].0 as f64 * ave
            + TERMSPR2[j].1 as f64 * ate
            + TERMSPR2[j].2 as f64 * ama
            + TERMSPR2[j].3 as f64 * aju
            + TERMSPR2[j].4 as f64 * alm
            + TERMSPR2[j].5 as f64 * ad
            + TERMSPR2[j].6 as f64 * alp
            + TERMSPR2[j].7 as f64 * al
            + TERMSPR2[j].8 as f64 * af;
        pr2 += TERMSPR2[j].10 * (TERMSPR2[j].9 + fr).sin();
    }
    SphP {
        lon: (pv0 + pv1 * dtc + pv2 * dtc2) * ASECRAD,
        lat: (pu0 + pu1 * dtc) * ASECRAD,
        rad: (pr0 + pr1 * dtc + pr2 * dtc2) / AU,
    }
}

fn elp85_pert_pv(dtc: f64) -> SphPv {
    let dtc2 = dtc * dtc;

    let al = scl_sv::scale(ASECRAD, &scl_sv::poly_norm(dtc, &AL, TURNAS));
    let alp = scl_sv::scale(ASECRAD, &scl_sv::poly_norm(dtc, &ALP, TURNAS));
    let ad = scl_sv::scale(ASECRAD, &scl_sv::poly_norm(dtc, &AD, TURNAS));
    let af = scl_sv::scale(ASECRAD, &scl_sv::poly_norm(dtc, &AF, TURNAS));
    // let alm = (218.0 * 3600.0 + 18.0 * 60.0 + 59.95571 + 1732564372.83264 * dtc - 4.7763 * dtc2
    //     + 0.006681 * dtc3
    //     - 0.00005522 * dtc4)
    //     % angle::TURNAS
    //     * ASECRAD;
    let alm = scl_sv::scale(ASECRAD, &scl_sv::poly_norm(dtc, &ALM, TURNAS));
    let ave = SclSv {
        s: (181.0 * 3600.0 + 58.0 * 60.0 + 47.28305 + 210664136.43355 * dtc) % TURNAS * ASECRAD,
        v: 210664136.43355 * ASECRAD,
    };
    let ate = SclSv {
        s: (100.0 * 3600.0 + 27.0 * 60.0 + 59.22059 + 129597742.2758 * dtc) % TURNAS * ASECRAD,
        v: 129597742.2758 * ASECRAD,
    };
    let ama = SclSv {
        s: (355.0 * 3600.0 + 25.0 * 60.0 + 59.78866 + 68905077.59284 * dtc) % TURNAS * ASECRAD,
        v: 68905077.59284 * ASECRAD,
    };
    let aju = SclSv {
        s: (34.0 * 3600.0 + 21.0 * 60.0 + 5.34212 + 10925660.42861 * dtc) % TURNAS * ASECRAD,
        v: 10925660.42861 * ASECRAD,
    };

    let mut pv0 = 0.0;
    let mut vpv0 = 0.0;
    let mut pv1 = 0.0;
    let mut vpv1 = 0.0;
    let mut pv2 = 0.0;
    let mut vpv2 = 0.0;
    let mut pu0 = 0.0;
    let mut vpu0 = 0.0;
    let mut pu1 = 0.0;
    let mut vpu1 = 0.0;
    let mut pr0 = 0.0;
    let mut vpr0 = 0.0;
    let mut pr1 = 0.0;
    let mut vpr1 = 0.0;
    let mut pr2 = 0.0;
    let mut vpr2 = 0.0;

    for j in 0..TERMSPV0.len() {
        let fr = TERMSPV0[j].0 as f64 * ave.s
            + TERMSPV0[j].1 as f64 * ate.s
            + TERMSPV0[j].2 as f64 * ama.s
            + TERMSPV0[j].3 as f64 * aju.s
            + TERMSPV0[j].4 as f64 * alm.s
            + TERMSPV0[j].5 as f64 * ad.s
            + TERMSPV0[j].6 as f64 * alp.s
            + TERMSPV0[j].7 as f64 * al.s
            + TERMSPV0[j].8 as f64 * af.s;
        let vfr = TERMSPV0[j].0 as f64 * ave.v
            + TERMSPV0[j].1 as f64 * ate.v
            + TERMSPV0[j].2 as f64 * ama.v
            + TERMSPV0[j].3 as f64 * aju.v
            + TERMSPV0[j].4 as f64 * alm.v
            + TERMSPV0[j].5 as f64 * ad.v
            + TERMSPV0[j].6 as f64 * alp.v
            + TERMSPV0[j].7 as f64 * al.v
            + TERMSPV0[j].8 as f64 * af.v;
        pv0 += TERMSPV0[j].10 * (TERMSPV0[j].9 + fr).sin();
        vpv0 += TERMSPV0[j].10 * (TERMSPV0[j].9 + fr).cos() * vfr;
    }
    for j in 0..TERMSPV1.len() {
        let fr = TERMSPV1[j].0 as f64 * ave.s
            + TERMSPV1[j].1 as f64 * ate.s
            + TERMSPV1[j].2 as f64 * ama.s
            + TERMSPV1[j].3 as f64 * aju.s
            + TERMSPV1[j].4 as f64 * alm.s
            + TERMSPV1[j].5 as f64 * ad.s
            + TERMSPV1[j].6 as f64 * alp.s
            + TERMSPV1[j].7 as f64 * al.s
            + TERMSPV1[j].8 as f64 * af.s;
        let vfr = TERMSPV1[j].0 as f64 * ave.v
            + TERMSPV1[j].1 as f64 * ate.v
            + TERMSPV1[j].2 as f64 * ama.v
            + TERMSPV1[j].3 as f64 * aju.v
            + TERMSPV1[j].4 as f64 * alm.v
            + TERMSPV1[j].5 as f64 * ad.v
            + TERMSPV1[j].6 as f64 * alp.v
            + TERMSPV1[j].7 as f64 * al.v
            + TERMSPV1[j].8 as f64 * af.v;
        pv1 += TERMSPV1[j].10 * (TERMSPV1[j].9 + fr).sin();
        vpv1 += TERMSPV1[j].10 * (TERMSPV1[j].9 + fr).cos() * vfr;
    }
    for j in 0..TERMSPV2.len() {
        let fr = TERMSPV2[j].0 as f64 * ave.s
            + TERMSPV2[j].1 as f64 * ate.s
            + TERMSPV2[j].2 as f64 * ama.s
            + TERMSPV2[j].3 as f64 * aju.s
            + TERMSPV2[j].4 as f64 * alm.s
            + TERMSPV2[j].5 as f64 * ad.s
            + TERMSPV2[j].6 as f64 * alp.s
            + TERMSPV2[j].7 as f64 * al.s
            + TERMSPV2[j].8 as f64 * af.s;
        let vfr = TERMSPV2[j].0 as f64 * ave.v
            + TERMSPV2[j].1 as f64 * ate.v
            + TERMSPV2[j].2 as f64 * ama.v
            + TERMSPV2[j].3 as f64 * aju.v
            + TERMSPV2[j].4 as f64 * alm.v
            + TERMSPV2[j].5 as f64 * ad.v
            + TERMSPV2[j].6 as f64 * alp.v
            + TERMSPV2[j].7 as f64 * al.v
            + TERMSPV2[j].8 as f64 * af.v;
        pv2 += TERMSPV2[j].10 * (TERMSPV2[j].9 + fr).sin();
        vpv2 += TERMSPV2[j].10 * (TERMSPV2[j].9 + fr).cos() * vfr;
    }

    for j in 0..TERMSPU0.len() {
        let fr = TERMSPU0[j].0 as f64 * ave.s
            + TERMSPU0[j].1 as f64 * ate.s
            + TERMSPU0[j].2 as f64 * ama.s
            + TERMSPU0[j].3 as f64 * aju.s
            + TERMSPU0[j].4 as f64 * alm.s
            + TERMSPU0[j].5 as f64 * ad.s
            + TERMSPU0[j].6 as f64 * alp.s
            + TERMSPU0[j].7 as f64 * al.s
            + TERMSPU0[j].8 as f64 * af.s;
        let vfr = TERMSPU0[j].0 as f64 * ave.v
            + TERMSPU0[j].1 as f64 * ate.v
            + TERMSPU0[j].2 as f64 * ama.v
            + TERMSPU0[j].3 as f64 * aju.v
            + TERMSPU0[j].4 as f64 * alm.v
            + TERMSPU0[j].5 as f64 * ad.v
            + TERMSPU0[j].6 as f64 * alp.v
            + TERMSPU0[j].7 as f64 * al.v
            + TERMSPU0[j].8 as f64 * af.v;
        pu0 += TERMSPU0[j].10 * (TERMSPU0[j].9 + fr).sin();
        vpu0 += TERMSPU0[j].10 * (TERMSPU0[j].9 + fr).cos() * vfr;
    }
    for j in 0..TERMSPU1.len() {
        let fr = TERMSPU1[j].0 as f64 * ave.s
            + TERMSPU1[j].1 as f64 * ate.s
            + TERMSPU1[j].2 as f64 * ama.s
            + TERMSPU1[j].3 as f64 * aju.s
            + TERMSPU1[j].4 as f64 * alm.s
            + TERMSPU1[j].5 as f64 * ad.s
            + TERMSPU1[j].6 as f64 * alp.s
            + TERMSPU1[j].7 as f64 * al.s
            + TERMSPU1[j].8 as f64 * af.s;
        let vfr = TERMSPU1[j].0 as f64 * ave.v
            + TERMSPU1[j].1 as f64 * ate.v
            + TERMSPU1[j].2 as f64 * ama.v
            + TERMSPU1[j].3 as f64 * aju.v
            + TERMSPU1[j].4 as f64 * alm.v
            + TERMSPU1[j].5 as f64 * ad.v
            + TERMSPU1[j].6 as f64 * alp.v
            + TERMSPU1[j].7 as f64 * al.v
            + TERMSPU1[j].8 as f64 * af.v;
        pu1 += TERMSPU1[j].10 * (TERMSPU1[j].9 + fr).sin();
        vpu1 += TERMSPU1[j].10 * (TERMSPU1[j].9 + fr).cos() * vfr;
    }

    for j in 0..TERMSPR0.len() {
        let fr = TERMSPR0[j].0 as f64 * ave.s
            + TERMSPR0[j].1 as f64 * ate.s
            + TERMSPR0[j].2 as f64 * ama.s
            + TERMSPR0[j].3 as f64 * aju.s
            + TERMSPR0[j].4 as f64 * alm.s
            + TERMSPR0[j].5 as f64 * ad.s
            + TERMSPR0[j].6 as f64 * alp.s
            + TERMSPR0[j].7 as f64 * al.s
            + TERMSPR0[j].8 as f64 * af.s;
        let vfr = TERMSPR0[j].0 as f64 * ave.v
            + TERMSPR0[j].1 as f64 * ate.v
            + TERMSPR0[j].2 as f64 * ama.v
            + TERMSPR0[j].3 as f64 * aju.v
            + TERMSPR0[j].4 as f64 * alm.v
            + TERMSPR0[j].5 as f64 * ad.v
            + TERMSPR0[j].6 as f64 * alp.v
            + TERMSPR0[j].7 as f64 * al.v
            + TERMSPR0[j].8 as f64 * af.v;
        pr0 += TERMSPR0[j].10 * (TERMSPR0[j].9 + fr).sin();
        vpr0 += TERMSPR0[j].10 * (TERMSPR0[j].9 + fr).cos() * vfr;
    }
    for j in 0..TERMSPR1.len() {
        let fr = TERMSPR1[j].0 as f64 * ave.s
            + TERMSPR1[j].1 as f64 * ate.s
            + TERMSPR1[j].2 as f64 * ama.s
            + TERMSPR1[j].3 as f64 * aju.s
            + TERMSPR1[j].4 as f64 * alm.s
            + TERMSPR1[j].5 as f64 * ad.s
            + TERMSPR1[j].6 as f64 * alp.s
            + TERMSPR1[j].7 as f64 * al.s
            + TERMSPR1[j].8 as f64 * af.s;
        let vfr = TERMSPR1[j].0 as f64 * ave.v
            + TERMSPR1[j].1 as f64 * ate.v
            + TERMSPR1[j].2 as f64 * ama.v
            + TERMSPR1[j].3 as f64 * aju.v
            + TERMSPR1[j].4 as f64 * alm.v
            + TERMSPR1[j].5 as f64 * ad.v
            + TERMSPR1[j].6 as f64 * alp.v
            + TERMSPR1[j].7 as f64 * al.v
            + TERMSPR1[j].8 as f64 * af.v;
        pr1 += TERMSPR1[j].10 * (TERMSPR1[j].9 + fr).sin();
        vpr1 += TERMSPR1[j].10 * (TERMSPR1[j].9 + fr).cos() * vfr;
    }
    for j in 0..TERMSPR2.len() {
        let fr = TERMSPR2[j].0 as f64 * ave.s
            + TERMSPR2[j].1 as f64 * ate.s
            + TERMSPR2[j].2 as f64 * ama.s
            + TERMSPR2[j].3 as f64 * aju.s
            + TERMSPR2[j].4 as f64 * alm.s
            + TERMSPR2[j].5 as f64 * ad.s
            + TERMSPR2[j].6 as f64 * alp.s
            + TERMSPR2[j].7 as f64 * al.s
            + TERMSPR2[j].8 as f64 * af.s;
        let vfr = TERMSPR2[j].0 as f64 * ave.v
            + TERMSPR2[j].1 as f64 * ate.v
            + TERMSPR2[j].2 as f64 * ama.v
            + TERMSPR2[j].3 as f64 * aju.v
            + TERMSPR2[j].4 as f64 * alm.v
            + TERMSPR2[j].5 as f64 * ad.v
            + TERMSPR2[j].6 as f64 * alp.v
            + TERMSPR2[j].7 as f64 * al.v
            + TERMSPR2[j].8 as f64 * af.v;
        pr2 += TERMSPR2[j].10 * (TERMSPR2[j].9 + fr).sin();
        vpr2 += TERMSPR2[j].10 * (TERMSPR2[j].9 + fr).cos() * vfr;
    }
    SphPv {
        lon: (pv0 + pv1 * dtc + pv2 * dtc2) * ASECRAD,
        lat: (pu0 + pu1 * dtc) * ASECRAD,
        rad: (pr0 + pr1 * dtc + pr2 * dtc2) / AU,
        vlon: (pv1 + 2.0 * pv2 * dtc + vpv0 + vpv1 * dtc + vpv2 * dtc2) * ASECRAD / DJCENT,
        vlat: (pu1 + vpu0 + vpu1 * dtc) * ASECRAD / DJCENT,
        vrad: (pr1 + 2.0 * pr2 * dtc + vpr0 + vpr1 * dtc + vpr2 * dtc2) / AU / DJCENT,
    }
}

///
/// # Arguments
/// # Returns
///
pub fn get_ecl_p(mjd_tdb: f64) -> SphP {
    let dtc = (mjd_tdb - mjd::J2000) / mjd::DJCENT;

    let alm = scl::polynom(dtc, &ALM) % TURNAS * ASECRAD;

    let main = elp85_main(dtc);
    let pert = elp85_pert(dtc);

    SphP {
        lon: angle::norm_dblpi(main.lon + pert.lon + alm),
        lat: main.lat + pert.lat,
        rad: main.rad + pert.rad,
    }
}

///
/// # Arguments
/// # Returns
///
pub fn get_ecl_pv(mjd_tdb: f64) -> SphPv {
    let dtc = (mjd_tdb - mjd::J2000) / mjd::DJCENT;

    let alm = scl_sv::scale2(
        ASECRAD,
        ASECRAD / DJCENT,
        &scl_sv::poly_norm(dtc, &ALM, TURNAS),
    );

    let main = elp85_main_pv(dtc);
    let pert = elp85_pert_pv(dtc);

    SphPv {
        lon: angle::norm_dblpi(main.lon + pert.lon + alm.s),
        lat: main.lat + pert.lat,
        rad: main.rad + pert.rad,
        vlon: main.vlon + pert.vlon + alm.v,
        vlat: main.vlat + pert.vlat,
        vrad: main.vrad + pert.vrad,
    }
}

///
/// # Arguments
/// # Returns
///
pub fn get_icrs_p(mjd_tdb: f64, body: &BodyId) -> vec_p::VecP {
    match body {
        BodyId::MOON => {
            let dtc = (mjd_tdb - J2000) / DJCENT;
            let sph = get_ecl_p(mjd_tdb);
            let out = sph.to_cart();
            let mat = mat_r::mul(
                &mat_r::rot_z(-iau06_r::gamb(dtc)),
                &mat_r::mul(
                    &mat_r::rot_x(-iau06_r::phib(dtc)),
                    &mat_r::rot_z(iau06_r::psib(dtc)),
                ),
            );
            mul_p(&mat, &out)
        }
        _ => VecP {
            x: f64::NAN,
            y: f64::NAN,
            z: f64::NAN,
        },
    }
}

///
/// # Arguments
/// # Returns
///
pub fn get_icrs_pv(mjd_tdb: f64, body: &BodyId) -> vec_pv::VecPv {
    match body {
        BodyId::MOON => {
            let dtc = (mjd_tdb - J2000) / DJCENT;
            let sph = get_ecl_pv(mjd_tdb);
            let out = sph.to_cart();
            let mat = mat_rv::mul(
                &mat_rv::rot_z(&scl_sv::scale(-1.0, &iau06_rv::gamb(dtc))),
                &mat_rv::mul(
                    &mat_rv::rot_x(&scl_sv::scale(-1.0, &iau06_rv::phib(dtc))),
                    &mat_rv::rot_z(&iau06_rv::psib(dtc)),
                ),
            );
            mat_rv::mul_pv(&mat, &out)
        }
        _ => vec_pv::VecPv {
            x: f64::NAN,
            y: f64::NAN,
            z: f64::NAN,
            vx: f64::NAN,
            vy: f64::NAN,
            vz: f64::NAN,
        },
    }
}