rkepler 0.4.1

/*
Copyright (c) 2022 Peter Hristozov / Петър Христозов

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

//! # References
//! LASKAR J., JACOBSON, R.: 1987, GUST86 - An analytical ephemeris of
//! the Uranian satellites, Astron. Astrophys. 188, 212-224
//! # Link
//! <https://ftp.imcce.fr/pub/ephem/satel/gust86/>

use crate::{
    datetime::mjd::{self, DJYEAR},
    frame::constant::AU,
    math::{
        angle::{norm_dblpi, DEGRAD},
        matrix::mat_r::{self, MatR},
        vector::vec_p,
        vector::vec_pv,
    },
    pos_eph::equ_orbit,
    BodyId,
};

const FQN: [f64; 5] = [
    4445190.550E-06,
    2492952.519E-06,
    1516148.111E-06,
    721718.509E-06,
    466692.120E-06,
];
const FQE: [f64; 5] = [
    20.082E0 * DEGRAD / DJYEAR,
    6.217E0 * DEGRAD / DJYEAR,
    2.865E0 * DEGRAD / DJYEAR,
    2.078E0 * DEGRAD / DJYEAR,
    0.386E0 * DEGRAD / DJYEAR,
];
const FQI: [f64; 5] = [
    -20.309E0 * DEGRAD / DJYEAR,
    -6.288E0 * DEGRAD / DJYEAR,
    -2.836E0 * DEGRAD / DJYEAR,
    -1.843E0 * DEGRAD / DJYEAR,
    -0.259E0 * DEGRAD / DJYEAR,
];
const PHN: [f64; 5] = [
    -238051E-06,
    3098046E-06,
    2285402E-06,
    856359E-06,
    -915592E-06,
];
const PHE: [f64; 5] = [0.611392E0, 2.408974E0, 2.067774E0, 0.735131E0, 0.426767E0];
const PHI: [f64; 5] = [5.702313E0, 0.395757E0, 0.589326E0, 1.746237E0, 4.206896E0];
const GMS: [f64; 5] = [4.4E0, 86.1E0, 84.0E0, 230.0E0, 200.0E0];
const GMU: f64 = 5794554.5 - (4.4e0 + 86.1e0 + 84.0e0 + 230.0e0 + 200.0e0);
const GMSUN: f64 = 132712440017.987;
const T0: f64 = 2444239.5;
const SEJ2: f64 = 86400.0 * 86400.0;
//const ALF: f64 = (90.0 - 76.60666666666667) * DEGRAD;
//const DEL: f64 = (90.0 - 15.03222222222222) * DEGRAD;
//&mat_r::mul(&mat_r::rot_z(ALF), &mat_r::rot_x(DEL))
const UME50_EME50: MatR = MatR {
    r00: 0.9728028367173511,
    r01: 0.0600772949490795,
    r02: 0.22370820169648678,
    r10: -0.23163471431258895,
    r11: 0.25230830845976193,
    r12: 0.939513638329694,
    r20: 0.0,
    r21: -0.9657801178911145,
    r22: 0.25936222524921604,
};
const B1950_J2000: MatR = MatR {
    r00: 0.9999257079523629,
    r01: -0.0111789381377700,
    r02: -0.0048590038153592,
    r10: 0.0111789381264276,
    r11: 0.9999375133499888,
    r12: -0.0000271625947142,
    r20: 0.0048590038414544,
    r21: -0.0000271579262585,
    r22: 0.9999881946023742,
};

fn ariel(mjd_tdb: f64) -> equ_orbit::OrbElem {
    let dt = mjd_tdb - mjd::jd_mjd(T0);
    let an1 = FQN[0] * dt + PHN[0];
    let an2 = FQN[1] * dt + PHN[1];
    let an3 = FQN[2] * dt + PHN[2];
    let an4 = FQN[3] * dt + PHN[3];
    let ae1 = FQE[0] * dt + PHE[0];
    let ae2 = FQE[1] * dt + PHE[1];
    let ae3 = FQE[2] * dt + PHE[2];
    let ae4 = FQE[3] * dt + PHE[3];
    let ae5 = FQE[4] * dt + PHE[4];
    let ai1 = FQI[0] * dt + PHI[0];
    let ai2 = FQI[1] * dt + PHI[1];
    let ai3 = FQI[2] * dt + PHI[2];
    let ai4 = FQI[3] * dt + PHI[3];
    let ai5 = FQI[4] * dt + PHI[4];

    let rn = 2492542.57e-06 + 2.55e-06 * (an1 - 3.0 * an2 + 2.0 * an3).cos()
        - 42.16e-06 * (an2 - an3).cos()
        - 102.56e-06 * (2.0 * an2 - 2.0 * an3).cos();
    let rl = 3098046.41e-06 + 2492952.52e-06 * dt
        - 1860.50e-06 * (an1 - 3.0 * an2 + 2.0 * an3).sin()
        + 219.99e-06 * (2.0 * an1 - 6.0 * an2 + 4.0 * an3).sin()
        + 23.10e-06 * (3.0 * an1 - 9.0 * an2 + 6.0 * an3).sin()
        + 4.30e-06 * (4.0 * an1 - 12.0 * an2 + 8.0 * an3).sin()
        - 90.11e-06 * (an2 - an3).sin()
        - 91.07e-06 * (2.0 * an2 - 2.0 * an3).sin()
        - 42.75e-06 * (3.0 * an2 - 3.0 * an3).sin()
        - 16.49e-06 * (2.0 * an2 - 2.0 * an4).sin();
    let rk = -3.35e-06 * (ae1).cos()
        + 1187.63e-06 * (ae2).cos()
        + 861.59e-06 * (ae3).cos()
        + 71.50e-06 * (ae4).cos()
        + 55.59e-06 * (ae5).cos()
        - 84.60e-06 * (-an2 + 2.0 * an3).cos()
        + 91.81e-06 * (-2.0 * an2 + 3.0 * an3).cos()
        + 20.03e-06 * (-an2 + 2.0 * an4).cos()
        + 89.77e-06 * (an2).cos();
    let rh = -3.35e-06 * (ae1).sin()
        + 1187.63e-06 * (ae2).sin()
        + 861.59e-06 * (ae3).sin()
        + 71.50e-06 * (ae4).sin()
        + 55.59e-06 * (ae5).sin()
        - 84.60e-06 * (-an2 + 2.0 * an3).sin()
        + 91.81e-06 * (-2.0 * an2 + 3.0 * an3).sin()
        + 20.03e-06 * (-an2 + 2.0 * an4).sin()
        + 89.77e-06 * (an2).sin();
    let rq = -121.75e-06 * (ai1).cos()
        + 358.25e-06 * (ai2).cos()
        + 290.08e-06 * (ai3).cos()
        + 97.78e-06 * (ai4).cos()
        + 33.97e-06 * (ai5).cos();
    let rp = -121.75e-06 * (ai1).sin()
        + 358.25e-06 * (ai2).sin()
        + 290.08e-06 * (ai3).sin()
        + 97.78e-06 * (ai4).sin()
        + 33.97e-06 * (ai5).sin();
    let rmu = GMS[1] + GMU;
    let ra = (rmu * SEJ2 / rn / rn).powf(1.0 / 3.0);

    equ_orbit::OrbElem {
        mu: rmu / GMSUN,
        mjd0: mjd_tdb,
        a: ra / AU,
        l: norm_dblpi(rl),
        k: rk,
        h: rh,
        q: rq,
        p: rp,
    }
}

fn umbriel(mjd_tdb: f64) -> equ_orbit::OrbElem {
    let dt = mjd_tdb - mjd::jd_mjd(T0);
    let an1 = FQN[0] * dt + PHN[0];
    let an2 = FQN[1] * dt + PHN[1];
    let an3 = FQN[2] * dt + PHN[2];
    let an4 = FQN[3] * dt + PHN[3];
    let an5 = FQN[4] * dt + PHN[4];
    let ae1 = FQE[0] * dt + PHE[0];
    let ae2 = FQE[1] * dt + PHE[1];
    let ae3 = FQE[2] * dt + PHE[2];
    let ae4 = FQE[3] * dt + PHE[3];
    let ae5 = FQE[4] * dt + PHE[4];
    let ai1 = FQI[0] * dt + PHI[0];
    let ai2 = FQI[1] * dt + PHI[1];
    let ai3 = FQI[2] * dt + PHI[2];
    let ai4 = FQI[3] * dt + PHI[3];
    let ai5 = FQI[4] * dt + PHI[4];

    let rn = 1515954.90e-06 + 9.74e-06 * (an3 - 2.0 * an4 + ae3).cos()
        - 106.00e-06 * (an2 - an3).cos()
        + 54.16e-06 * (2.0 * an2 - 2.0 * an3).cos()
        - 23.59e-06 * (an3 - an4).cos()
        - 70.70e-06 * (2.0 * an3 - 2.0 * an4).cos()
        - 36.28e-06 * (3.0 * an3 - 3.0 * an4).cos();
    let rl =
        2285401.69e-06 + 1516148.11e-06 * dt + 660.57e-06 * (an1 - 3.0 * an2 + 2.0 * an3).sin()
            - 76.51e-06 * (2.0 * an1 - 6.0 * an2 + 4.0 * an3).sin()
            - 8.96e-06 * (3.0 * an1 - 9.0 * an2 + 6.0 * an3).sin()
            - 2.53e-06 * (4.0 * an1 - 12.0 * an2 + 8.0 * an3).sin()
            - 52.91e-06 * (an3 - 4.0 * an4 + 3.0 * an5).sin()
            - 7.34e-06 * (an3 - 2.0 * an4 + ae5).sin()
            - 1.83e-06 * (an3 - 2.0 * an4 + ae4).sin()
            + 147.91e-06 * (an3 - 2.0 * an4 + ae3).sin()
            - 7.77e-06 * (an3 - 2.0 * an4 + ae2).sin()
            + 97.76e-06 * (an2 - an3).sin()
            + 73.13e-06 * (2.0 * an2 - 2.0 * an3).sin()
            + 34.71e-06 * (3.0 * an2 - 3.0 * an3).sin()
            + 18.89e-06 * (4.0 * an2 - 4.0 * an3).sin()
            - 67.89e-06 * (an3 - an4).sin()
            - 82.86e-06 * (2.0 * an3 - 2.0 * an4).sin()
            - 33.81e-06 * (3.0 * an3 - 3.0 * an4).sin()
            - 15.79e-06 * (4.0 * an3 - 4.0 * an4).sin()
            - 10.21e-06 * (an3 - an5).sin()
            - 17.08e-06 * (2.0 * an3 - 2.0 * an5).sin();
    let rk = -0.21e-06 * (ae1).cos() - 227.95e-06 * (ae2).cos()
        + 3904.69e-06 * (ae3).cos()
        + 309.17e-06 * (ae4).cos()
        + 221.92e-06 * (ae5).cos()
        + 29.34e-06 * (an2).cos()
        + 26.20e-06 * (an3).cos()
        + 51.19e-06 * (-an2 + 2.0 * an3).cos()
        - 103.86e-06 * (-2.0 * an2 + 3.0 * an3).cos()
        - 27.16e-06 * (-3.0 * an2 + 4.0 * an3).cos()
        - 16.22e-06 * (an4).cos()
        + 549.23e-06 * (-an3 + 2.0 * an4).cos()
        + 34.70e-06 * (-2.0 * an3 + 3.0 * an4).cos()
        + 12.81e-06 * (-3.0 * an3 + 4.0 * an4).cos()
        + 21.81e-06 * (-an3 + 2.0 * an5).cos()
        + 46.25e-06 * (an3).cos();
    let rh = -0.21e-06 * (ae1).sin() - 227.95e-06 * (ae2).sin()
        + 3904.69e-06 * (ae3).sin()
        + 309.17e-06 * (ae4).sin()
        + 221.92e-06 * (ae5).sin()
        + 29.34e-06 * (an2).sin()
        + 26.20e-06 * (an3).sin()
        + 51.19e-06 * (-an2 + 2.0 * an3).sin()
        - 103.86e-06 * (-2.0 * an2 + 3.0 * an3).sin()
        - 27.16e-06 * (-3.0 * an2 + 4.0 * an3).sin()
        - 16.22e-06 * (an4).sin()
        + 549.23e-06 * (-an3 + 2.0 * an4).sin()
        + 34.70e-06 * (-2.0 * an3 + 3.0 * an4).sin()
        + 12.81e-06 * (-3.0 * an3 + 4.0 * an4).sin()
        + 21.81e-06 * (-an3 + 2.0 * an5).sin()
        + 46.25e-06 * (an3).sin();
    let rq = -10.86e-06 * (ai1).cos() - 81.51e-06 * (ai2).cos()
        + 1113.36e-06 * (ai3).cos()
        + 350.14e-06 * (ai4).cos()
        + 106.50e-06 * (ai5).cos();
    let rp = -10.86e-06 * (ai1).sin() - 81.51e-06 * (ai2).sin()
        + 1113.36e-06 * (ai3).sin()
        + 350.14e-06 * (ai4).sin()
        + 106.50e-06 * (ai5).sin();
    let rmu = GMS[2] + GMU;
    let ra = (rmu * SEJ2 / rn / rn).powf(1.0 / 3.0);

    equ_orbit::OrbElem {
        mu: rmu / GMSUN,
        mjd0: mjd_tdb,
        a: ra / AU,
        l: norm_dblpi(rl),
        k: rk,
        h: rh,
        q: rq,
        p: rp,
    }
}

fn titania(mjd_tdb: f64) -> equ_orbit::OrbElem {
    let dt = mjd_tdb - mjd::jd_mjd(T0);
    let an2 = FQN[1] * dt + PHN[1];
    let an3 = FQN[2] * dt + PHN[2];
    let an4 = FQN[3] * dt + PHN[3];
    let an5 = FQN[4] * dt + PHN[4];
    let ae1 = FQE[0] * dt + PHE[0];
    let ae2 = FQE[1] * dt + PHE[1];
    let ae3 = FQE[2] * dt + PHE[2];
    let ae4 = FQE[3] * dt + PHE[3];
    let ae5 = FQE[4] * dt + PHE[4];
    let ai1 = FQI[0] * dt + PHI[0];
    let ai2 = FQI[1] * dt + PHI[1];
    let ai3 = FQI[2] * dt + PHI[2];
    let ai4 = FQI[3] * dt + PHI[3];
    let ai5 = FQI[4] * dt + PHI[4];

    let rn = 721663.16e-06
        - 2.64e-06 * (an3 - 2.0 * an4 + ae3).cos()
        - 2.16e-06 * (2.0 * an4 - 3.0 * an5 + ae5).cos()
        + 6.45e-06 * (2.0 * an4 - 3.0 * an5 + ae4).cos()
        - 1.11e-06 * (2.0 * an4 - 3.0 * an5 + ae3).cos()
        - 62.23e-06 * (an2 - an4).cos()
        - 56.13e-06 * (an3 - an4).cos()
        - 39.94e-06 * (an4 - an5).cos()
        - 91.85e-06 * (2.0 * an4 - 2.0 * an5).cos()
        - 58.31e-06 * (3.0 * an4 - 3.0 * an5).cos()
        - 38.60e-06 * (4.0 * an4 - 4.0 * an5).cos()
        - 26.18e-06 * (5.0 * an4 - 5.0 * an5).cos()
        - 18.06e-06 * (6.0 * an4 - 6.0 * an5).cos();
    let rl = 856358.79e-06 + 721718.51e-06 * dt + 20.61e-06 * (an3 - 4.0 * an4 + 3.0 * an5).sin()
        - 2.07e-06 * (an3 - 2.0 * an4 + ae5).sin()
        - 2.88e-06 * (an3 - 2.0 * an4 + ae4).sin()
        - 40.79e-06 * (an3 - 2.0 * an4 + ae3).sin()
        + 2.11e-06 * (an3 - 2.0 * an4 + ae2).sin()
        - 51.83e-06 * (2.0 * an4 - 3.0 * an5 + ae5).sin()
        + 159.87e-06 * (2.0 * an4 - 3.0 * an5 + ae4).sin()
        - 35.05e-06 * (2.0 * an4 - 3.0 * an5 + ae3).sin()
        - 1.56e-06 * (3.0 * an4 - 4.0 * an5 + ae5).sin()
        + 40.54e-06 * (an2 - an4).sin()
        + 46.17e-06 * (an3 - an4).sin()
        - 317.76e-06 * (an4 - an5).sin()
        - 305.59e-06 * (2.0 * an4 - 2.0 * an5).sin()
        - 148.36e-06 * (3.0 * an4 - 3.0 * an5).sin()
        - 82.92e-06 * (4.0 * an4 - 4.0 * an5).sin()
        - 49.98e-06 * (5.0 * an4 - 5.0 * an5).sin()
        - 31.56e-06 * (6.0 * an4 - 6.0 * an5).sin()
        - 20.56e-06 * (7.0 * an4 - 7.0 * an5).sin()
        - 13.69e-06 * (8.0 * an4 - 8.0 * an5).sin();
    let rk = -0.02e-06 * (ae1).cos() - 1.29e-06 * (ae2).cos() - 324.51e-06 * (ae3).cos()
        + 932.81e-06 * (ae4).cos()
        + 1120.89e-06 * (ae5).cos()
        + 33.86e-06 * (an2).cos()
        + 17.46e-06 * (an4).cos()
        + 16.58e-06 * (-an2 + 2.0 * an4).cos()
        + 28.89e-06 * (an3).cos()
        - 35.86e-06 * (-an3 + 2.0 * an4).cos()
        - 17.86e-06 * (an4).cos()
        - 32.10e-06 * (an5).cos()
        - 177.83e-06 * (-an4 + 2.0 * an5).cos()
        + 793.43e-06 * (-2.0 * an4 + 3.0 * an5).cos()
        + 99.48e-06 * (-3.0 * an4 + 4.0 * an5).cos()
        + 44.83e-06 * (-4.0 * an4 + 5.0 * an5).cos()
        + 25.13e-06 * (-5.0 * an4 + 6.0 * an5).cos()
        + 15.43e-06 * (-6.0 * an4 + 7.0 * an5).cos();
    let rh = -0.02e-06 * (ae1).sin() - 1.29e-06 * (ae2).sin() - 324.51e-06 * (ae3).sin()
        + 932.81e-06 * (ae4).sin()
        + 1120.89e-06 * (ae5).sin()
        + 33.86e-06 * (an2).sin()
        + 17.46e-06 * (an4).sin()
        + 16.58e-06 * (-an2 + 2.0 * an4).sin()
        + 28.89e-06 * (an3).sin()
        - 35.86e-06 * (-an3 + 2.0 * an4).sin()
        - 17.86e-06 * (an4).sin()
        - 32.10e-06 * (an5).sin()
        - 177.83e-06 * (-an4 + 2.0 * an5).sin()
        + 793.43e-06 * (-2.0 * an4 + 3.0 * an5).sin()
        + 99.48e-06 * (-3.0 * an4 + 4.0 * an5).sin()
        + 44.83e-06 * (-4.0 * an4 + 5.0 * an5).sin()
        + 25.13e-06 * (-5.0 * an4 + 6.0 * an5).sin()
        + 15.43e-06 * (-6.0 * an4 + 7.0 * an5).sin();
    let rq = -1.43e-06 * (ai1).cos() - 1.06e-06 * (ai2).cos() - 140.13e-06 * (ai3).cos()
        + 685.72e-06 * (ai4).cos()
        + 378.32e-06 * (ai5).cos();
    let rp = -1.43e-06 * (ai1).sin() - 1.06e-06 * (ai2).sin() - 140.13e-06 * (ai3).sin()
        + 685.72e-06 * (ai4).sin()
        + 378.32e-06 * (ai5).sin();
    let rmu = GMS[3] + GMU;
    let ra = (rmu * SEJ2 / rn / rn).powf(1.0 / 3.0);

    equ_orbit::OrbElem {
        mu: rmu / GMSUN,
        mjd0: mjd_tdb,
        a: ra / AU,
        l: norm_dblpi(rl),
        k: rk,
        h: rh,
        q: rq,
        p: rp,
    }
}

fn oberon(mjd_tdb: f64) -> equ_orbit::OrbElem {
    let dt = mjd_tdb - mjd::jd_mjd(T0);
    let an2 = FQN[1] * dt + PHN[1];
    let an3 = FQN[2] * dt + PHN[2];
    let an4 = FQN[3] * dt + PHN[3];
    let an5 = FQN[4] * dt + PHN[4];
    let ae1 = FQE[0] * dt + PHE[0];
    let ae2 = FQE[1] * dt + PHE[1];
    let ae3 = FQE[2] * dt + PHE[2];
    let ae4 = FQE[3] * dt + PHE[3];
    let ae5 = FQE[4] * dt + PHE[4];
    let ai1 = FQI[0] * dt + PHI[0];
    let ai2 = FQI[1] * dt + PHI[1];
    let ai3 = FQI[2] * dt + PHI[2];
    let ai4 = FQI[3] * dt + PHI[3];
    let ai5 = FQI[4] * dt + PHI[4];

    let rn = 466580.54e-06 + 2.08e-06 * (2.0 * an4 - 3.0 * an5 + ae5).cos()
        - 6.22e-06 * (2.0 * an4 - 3.0 * an5 + ae4).cos()
        + 1.07e-06 * (2.0 * an4 - 3.0 * an5 + ae3).cos()
        - 43.10e-06 * (an2 - an5).cos()
        - 38.94e-06 * (an3 - an5).cos()
        - 80.11e-06 * (an4 - an5).cos()
        + 59.06e-06 * (2.0 * an4 - 2.0 * an5).cos()
        + 37.49e-06 * (3.0 * an4 - 3.0 * an5).cos()
        + 24.82e-06 * (4.0 * an4 - 4.0 * an5).cos()
        + 16.84e-06 * (5.0 * an4 - 5.0 * an5).cos();
    let rl = -915591.80e-06 + 466692.12e-06 * dt - 7.82e-06 * (an3 - 4.0 * an4 + 3.0 * an5).sin()
        + 51.29e-06 * (2.0 * an4 - 3.0 * an5 + ae5).sin()
        - 158.24e-06 * (2.0 * an4 - 3.0 * an5 + ae4).sin()
        + 34.51e-06 * (2.0 * an4 - 3.0 * an5 + ae3).sin()
        + 47.51e-06 * (an2 - an5).sin()
        + 38.96e-06 * (an3 - an5).sin()
        + 359.73e-06 * (an4 - an5).sin()
        + 282.78e-06 * (2.0 * an4 - 2.0 * an5).sin()
        + 138.60e-06 * (3.0 * an4 - 3.0 * an5).sin()
        + 78.03e-06 * (4.0 * an4 - 4.0 * an5).sin()
        + 47.29e-06 * (5.0 * an4 - 5.0 * an5).sin()
        + 30.00e-06 * (6.0 * an4 - 6.0 * an5).sin()
        + 19.62e-06 * (7.0 * an4 - 7.0 * an5).sin()
        + 13.11e-06 * (8.0 * an4 - 8.0 * an5).sin();
    let rk = 0.00e-06 * (ae1).cos() - 0.35e-06 * (ae2).cos() + 74.53e-06 * (ae3).cos()
        - 758.68e-06 * (ae4).cos()
        + 1397.34e-06 * (ae5).cos()
        + 39.00e-06 * (an2).cos()
        + 17.66e-06 * (-an2 + 2.0 * an5).cos()
        + 32.42e-06 * (an3).cos()
        + 79.75e-06 * (an4).cos()
        + 75.66e-06 * (an5).cos()
        + 134.04e-06 * (-an4 + 2.0 * an5).cos()
        - 987.26e-06 * (-2.0 * an4 + 3.0 * an5).cos()
        - 126.09e-06 * (-3.0 * an4 + 4.0 * an5).cos()
        - 57.42e-06 * (-4.0 * an4 + 5.0 * an5).cos()
        - 32.41e-06 * (-5.0 * an4 + 6.0 * an5).cos()
        - 19.99e-06 * (-6.0 * an4 + 7.0 * an5).cos()
        - 12.94e-06 * (-7.0 * an4 + 8.0 * an5).cos();
    let rh = 0.00e-06 * (ae1).sin() - 0.35e-06 * (ae2).sin() + 74.53e-06 * (ae3).sin()
        - 758.68e-06 * (ae4).sin()
        + 1397.34e-06 * (ae5).sin()
        + 39.00e-06 * (an2).sin()
        + 17.66e-06 * (-an2 + 2.0 * an5).sin()
        + 32.42e-06 * (an3).sin()
        + 79.75e-06 * (an4).sin()
        + 75.66e-06 * (an5).sin()
        + 134.04e-06 * (-an4 + 2.0 * an5).sin()
        - 987.26e-06 * (-2.0 * an4 + 3.0 * an5).sin()
        - 126.09e-06 * (-3.0 * an4 + 4.0 * an5).sin()
        - 57.42e-06 * (-4.0 * an4 + 5.0 * an5).sin()
        - 32.41e-06 * (-5.0 * an4 + 6.0 * an5).sin()
        - 19.99e-06 * (-6.0 * an4 + 7.0 * an5).sin()
        - 12.94e-06 * (-7.0 * an4 + 8.0 * an5).sin();
    let rq = -0.44e-06 * (ai1).cos() - 0.31e-06 * (ai2).cos() + 36.89e-06 * (ai3).cos()
        - 596.33e-06 * (ai4).cos()
        + 451.69e-06 * (ai5).cos();
    let rp = -0.44e-06 * (ai1).sin() - 0.31e-06 * (ai2).sin() + 36.89e-06 * (ai3).sin()
        - 596.33e-06 * (ai4).sin()
        + 451.69e-06 * (ai5).sin();
    let rmu = GMS[4] + GMU;
    let ra = (rmu * SEJ2 / rn / rn).powf(1.0 / 3.0);

    equ_orbit::OrbElem {
        mu: rmu / GMSUN,
        mjd0: mjd_tdb,
        a: ra / AU,
        l: norm_dblpi(rl),
        k: rk,
        h: rh,
        q: rq,
        p: rp,
    }
}

fn miranda(mjd_tdb: f64) -> equ_orbit::OrbElem {
    let dt = mjd_tdb - mjd::jd_mjd(T0);
    let an1 = FQN[0] * dt + PHN[0];
    let an2 = FQN[1] * dt + PHN[1];
    let an3 = FQN[2] * dt + PHN[2];
    let ae1 = FQE[0] * dt + PHE[0];
    let ae2 = FQE[1] * dt + PHE[1];
    let ae3 = FQE[2] * dt + PHE[2];
    let ae4 = FQE[3] * dt + PHE[3];
    let ae5 = FQE[4] * dt + PHE[4];
    let ai1 = FQI[0] * dt + PHI[0];
    let ai2 = FQI[1] * dt + PHI[1];
    let ai3 = FQI[2] * dt + PHI[2];
    let ai4 = FQI[3] * dt + PHI[3];
    let ai5 = FQI[4] * dt + PHI[4];

    let rn = 4443522.67e-06 - 34.92e-06 * (an1 - 3.0 * an2 + 2.0 * an3).cos()
        + 8.47e-06 * (2.0 * an1 - 6.0 * an2 + 4.0 * an3).cos()
        + 1.31e-06 * (3.0 * an1 - 9.0 * an2 + 6.0 * an3).cos()
        - 52.28e-06 * (an1 - an2).cos()
        - 136.65e-06 * (2.0 * an1 - 2.0 * an2).cos();
    let rl =
        -238051.58e-06 + 4445190.55e-06 * dt + 25472.17e-06 * (an1 - 3.0 * an2 + 2.0 * an3).sin()
            - 3088.31e-06 * (2.0 * an1 - 6.0 * an2 + 4.0 * an3).sin()
            - 318.10e-06 * (3.0 * an1 - 9.0 * an2 + 6.0 * an3).sin()
            - 37.49e-06 * (4.0 * an1 - 12.0 * an2 + 8.0 * an3).sin()
            - 57.85e-06 * (an1 - an2).sin()
            - 62.32e-06 * (2.0 * an1 - 2.0 * an2).sin()
            - 27.95e-06 * (3.0 * an1 - 3.0 * an2).sin();
    let rk = 1312.38e-06 * ae1.cos()
        + 71.81e-06 * ae2.cos()
        + 69.77e-06 * ae3.cos()
        + 6.75e-06 * ae4.cos()
        + 6.27e-06 * ae5.cos()
        - 123.31e-06 * (-an1 + 2.0 * an2).cos()
        + 39.52e-06 * (-2.0 * an1 + 3.0 * an2).cos()
        + 194.10e-06 * an1.cos();
    let rh = 1312.38e-06 * ae1.sin()
        + 71.81e-06 * ae2.sin()
        + 69.77e-06 * ae3.sin()
        + 6.75e-06 * ae4.sin()
        + 6.27e-06 * ae5.sin()
        - 123.31e-06 * (-an1 + 2.0 * an2).sin()
        + 39.52e-06 * (-2.0 * an1 + 3.0 * an2).sin()
        + 194.10e-06 * an1.sin();
    let rq = 37871.71e-06 * ai1.cos()
        + 27.01e-06 * ai2.cos()
        + 30.76e-06 * ai3.cos()
        + 12.18e-06 * ai4.cos()
        + 5.37e-06 * ai5.cos();
    let rp = 37871.71e-06 * ai1.sin()
        + 27.01e-06 * ai2.sin()
        + 30.76e-06 * ai3.sin()
        + 12.18e-06 * ai4.sin()
        + 5.37e-06 * ai5.sin();
    let rmu = GMS[0] + GMU;
    let ra = (rmu * SEJ2 / rn / rn).powf(1.0 / 3.0);

    equ_orbit::OrbElem {
        mu: rmu / GMSUN,
        mjd0: mjd_tdb,
        a: ra / AU,
        l: norm_dblpi(rl),
        k: rk,
        h: rh,
        q: rq,
        p: rp,
    }
}

///
/// # Arguments
/// # Returns
///
pub fn get_orb_elm(mjd_tdb: f64, body: &BodyId) -> equ_orbit::OrbElem {
    match body {
        BodyId::ARIEL => ariel(mjd_tdb),
        BodyId::UMBRIEL => umbriel(mjd_tdb),
        BodyId::TITANIA => titania(mjd_tdb),
        BodyId::OBERON => oberon(mjd_tdb),
        BodyId::MIRANDA => miranda(mjd_tdb),
        _ => equ_orbit::OrbElem {
            mu: f64::NAN,
            mjd0: mjd_tdb,
            a: f64::NAN,
            l: f64::NAN,
            k: f64::NAN,
            h: f64::NAN,
            q: f64::NAN,
            p: f64::NAN,
        },
    }
}

///
/// # Arguments
/// # Returns
///
pub fn get_icrs_pv(mjd_tdb: f64, body: &BodyId) -> vec_pv::VecPv {
    let orb_el = get_orb_elm(mjd_tdb, body);
    let out = orb_el.get_pv();
    let eqv_icrs = mat_r::mul(&B1950_J2000, &UME50_EME50);
    mat_r::mul_pv(&eqv_icrs, &out)
}

///
/// # Arguments
/// # Returns
///
pub fn get_icrs_p(mjd_tdb: f64, body: &BodyId) -> vec_p::VecP {
    let orb_el = get_orb_elm(mjd_tdb, body);
    let out = orb_el.get_p();
    let eqv_icrs = mat_r::mul(&B1950_J2000, &UME50_EME50);
    mat_r::mul_p(&eqv_icrs, &out)
}