use crate::frame::constant::GK;
use crate::math::vector::vec_pv::VecPv;

pub struct OrbElem {
    pub mu: f64,
    pub mjd0: f64,
    pub a: f64,
    pub man: f64,
    pub ecc: f64,
    pub inc: f64,
    pub om: f64,
    pub omega: f64,
}

impl OrbElem {
    pub fn get_pv(&self) -> VecPv {
        let an = GK * (self.mu / self.a.powi(3)).sqrt();
        let mut ean = self.man + self.ecc * self.man.sin();
        let mut n = 0;
        loop {
            let dean = (self.man - ean + self.ecc * ean.sin()) / (1.0 - self.ecc * ean.cos());
            ean += dean;
            n += 1;
            if dean.abs() < 1e-12 || n > 50 {
                break;
            };
        }

        let cean = ean.cos();
        let sean = ean.sin();
        let de2 = (1.0 - self.ecc * self.ecc).sqrt();
        let r = self.a * (1.0 - self.ecc * cean);
        let v = an * self.a * self.a / r;
        let xorb = self.a * (cean - self.ecc);
        let yorb = self.a * de2 * sean;
        let vxorb = -v * sean;
        let vyorb = v * de2 * cean;

        let cinc = self.inc.cos();
        let sinc = self.inc.sin();
        let com = self.om.cos();
        let som = self.om.sin();
        let comega = self.omega.cos();
        let somega = self.omega.sin();
        let p1x = com * comega - som * somega * cinc;
        let p2x = -som * comega - com * somega * cinc;
        let p1y = com * somega + som * comega * cinc;
        let p2y = -som * somega + com * comega * cinc;
        let p1z = som * sinc;
        let p2z = com * sinc;
        VecPv {
            x: (p1x * xorb + p2x * yorb),
            y: (p1y * xorb + p2y * yorb),
            z: (p1z * xorb + p2z * yorb),
            vx: (p1x * vxorb + p2x * vyorb),
            vy: (p1y * vxorb + p2y * vyorb),
            vz: (p1z * vxorb + p2z * vyorb),
        }
    }
}
