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

pub struct OrbElem {
    pub mu: f64,
    pub mjd0: f64,
    pub a: f64,
    pub l: f64,
    pub k: f64,
    pub h: f64,
    pub q: f64,
    pub p: f64,
}

impl OrbElem {
    pub fn get_pv(&self) -> VecPv {
        let an = GK * (self.mu / self.a.powi(3)).sqrt();
        let mut ee = self.l + self.k * self.l.sin() - self.h * self.l.cos();
        let mut n: i8 = 0;
        loop {
            let ce = ee.cos();
            let se = ee.sin();
            let de = (self.l - ee + self.k * se - self.h * ce) / (1.0 - self.k * ce - self.h * se);
            n += 1;
            ee += de;
            if de.abs() < 1e-12 || n > 50 {
                break;
            };
        }
        let ce = ee.cos();
        let se = ee.sin();
        let dle = self.h * ce - self.k * se;
        let rsam1 = -self.k * ce - self.h * se;
        let asr = 1.0 / (1.0 + rsam1);
        let phi = (1.0 - self.k * self.k - self.h * self.h).sqrt();
        let psi = 1.0 / (1.0 + phi);
        let vxy1 = an * asr * self.a;
        let x1 = self.a * (ce - self.k - psi * self.h * dle);
        let y1 = self.a * (se - self.h + psi * self.k * dle);
        let vx1 = vxy1 * (-se - psi * self.h * rsam1);
        let vy1 = vxy1 * (ce + psi * self.k * rsam1);
        let f2 = 2.0 * (1.0 - self.q * self.q - self.p * self.p).sqrt();
        let p2 = 1.0 - 2.0 * self.p * self.p;
        let q2 = 1.0 - 2.0 * self.q * self.q;
        let pq = 2.0 * self.p * self.q;
        VecPv {
            x: (x1 * p2 + y1 * pq),
            y: (x1 * pq + y1 * q2),
            z: ((self.q * y1 - self.p * x1) * f2),
            vx: (vx1 * p2 + vy1 * pq),
            vy: (vx1 * pq + vy1 * q2),
            vz: ((self.q * vy1 - self.p * vx1) * f2),
        }
    }

    pub fn get_p(&self) -> VecP {
        let mut ee = self.l + self.k * self.l.sin() - self.h * self.l.cos();
        let mut n: i8 = 0;
        loop {
            let ce = ee.cos();
            let se = ee.sin();
            let de = (self.l - ee + self.k * se - self.h * ce) / (1.0 - self.k * ce - self.h * se);
            n += 1;
            ee += de;
            if de.abs() < 1e-12 || n > 50 {
                break;
            };
        }
        let ce = ee.cos();
        let se = ee.sin();
        let dle = self.h * ce - self.k * se;
        let phi = (1.0 - self.k * self.k - self.h * self.h).sqrt();
        let psi = 1.0 / (1.0 + phi);
        let x1 = self.a * (ce - self.k - psi * self.h * dle);
        let y1 = self.a * (se - self.h + psi * self.k * dle);
        let f2 = 2.0 * (1.0 - self.q * self.q - self.p * self.p).sqrt();
        let p2 = 1.0 - 2.0 * self.p * self.p;
        let q2 = 1.0 - 2.0 * self.q * self.q;
        let pq = 2.0 * self.p * self.q;
        VecP {
            x: (x1 * p2 + y1 * pq),
            y: (x1 * pq + y1 * q2),
            z: ((self.q * y1 - self.p * x1) * f2),
        }
    }
}
