satellitesfactory 0.0.2

use crate::config::parameters::*;

#[derive(Debug, Clone)]
pub struct OrbitalElements {
    pub a: f64,
    pub e: f64,
    pub i: f64,
    pub omega_big: f64,
    pub omega_small: f64,
    pub mean_anomaly: f64,
}

impl OrbitalElements {
    pub fn new(
        a: f64,
        e: f64,
        i: f64,
        omega_big: f64,
        omega_small: f64,
        mean_anomaly: f64,
    ) -> Self {
        Self {
            a,
            e,
            i,
            omega_big,
            omega_small,
            mean_anomaly,
        }
    }

    pub fn from_km_deg(
        a_km: f64,
        e: f64,
        i_deg: f64,
        ob_deg: f64,
        os_deg: f64,
        m_deg: f64,
    ) -> Self {
        Self {
            a: a_km * 1.0e3,
            e,
            i: i_deg * DEG,
            omega_big: ob_deg * DEG,
            omega_small: os_deg * DEG,
            mean_anomaly: m_deg * DEG,
        }
    }

    pub fn from_au_deg(
        a_au: f64,
        e: f64,
        i_deg: f64,
        ob_deg: f64,
        os_deg: f64,
        m_deg: f64,
    ) -> Self {
        Self {
            a: a_au * AU,
            e,
            i: i_deg * DEG,
            omega_big: ob_deg * DEG,
            omega_small: os_deg * DEG,
            mean_anomaly: m_deg * DEG,
        }
    }

    pub fn zero() -> Self {
        Self {
            a: 0.0,
            e: 0.0,
            i: 0.0,
            omega_big: 0.0,
            omega_small: 0.0,
            mean_anomaly: 0.0,
        }
    }

    pub fn period(&self, mu: f64) -> f64 {
        2.0 * std::f64::consts::PI * (self.a.powi(3) / mu).sqrt()
    }

    pub fn mean_motion(&self, mu: f64) -> f64 {
        (mu / self.a.powi(3)).sqrt()
    }

    pub fn periapsis(&self) -> f64 {
        self.a * (1.0 - self.e)
    }

    pub fn apoapsis(&self) -> f64 {
        self.a * (1.0 + self.e)
    }

    pub fn semi_latus_rectum(&self) -> f64 {
        self.a * (1.0 - self.e * self.e)
    }

    pub fn specific_energy(&self, mu: f64) -> f64 {
        -mu / (2.0 * self.a)
    }

    pub fn specific_angular_momentum(&self, mu: f64) -> f64 {
        (mu * self.semi_latus_rectum()).sqrt()
    }
}

pub fn solve_kepler(mean_anomaly: f64, e: f64, tol: f64) -> f64 {
    let mut ea = if e < 0.8 {
        mean_anomaly
    } else {
        std::f64::consts::PI
    };
    for _ in 0..50 {
        let f = ea - e * ea.sin() - mean_anomaly;
        let fp = 1.0 - e * ea.cos();
        let delta = f / fp;
        ea -= delta;
        if delta.abs() < tol {
            break;
        }
    }
    ea
}

pub fn true_from_eccentric(ea: f64, e: f64) -> f64 {
    2.0 * ((1.0 + e).sqrt() * (ea / 2.0).sin()).atan2((1.0 - e).sqrt() * (ea / 2.0).cos())
}

pub fn elements_to_state(elem: &OrbitalElements, total_mass: f64) -> ([f64; 3], [f64; 3]) {
    let mu = G * total_mass;
    let ea = solve_kepler(elem.mean_anomaly, elem.e, 1e-14);
    let nu = true_from_eccentric(ea, elem.e);

    let r = elem.a * (1.0 - elem.e * ea.cos());
    let p = elem.semi_latus_rectum();
    let h = (mu * p).sqrt();

    let x_pf = r * nu.cos();
    let y_pf = r * nu.sin();
    let vx_pf = -(mu / h) * nu.sin();
    let vy_pf = (mu / h) * (elem.e + nu.cos());

    let (co, so) = (elem.omega_big.cos(), elem.omega_big.sin());
    let (cw, sw) = (elem.omega_small.cos(), elem.omega_small.sin());
    let (ci, si) = (elem.i.cos(), elem.i.sin());

    let px = co * cw - so * sw * ci;
    let py = so * cw + co * sw * ci;
    let pz = sw * si;
    let qx = -co * sw - so * cw * ci;
    let qy = -so * sw + co * cw * ci;
    let qz = cw * si;

    let pos = [
        x_pf * px + y_pf * qx,
        x_pf * py + y_pf * qy,
        x_pf * pz + y_pf * qz,
    ];
    let vel = [
        vx_pf * px + vy_pf * qx,
        vx_pf * py + vy_pf * qy,
        vx_pf * pz + vy_pf * qz,
    ];

    (pos, vel)
}

pub fn vis_viva(mu: f64, r: f64, a: f64) -> f64 {
    (mu * (2.0 / r - 1.0 / a)).sqrt()
}

pub fn hohmann_delta_v(mu: f64, r1: f64, r2: f64) -> (f64, f64) {
    let a_t = (r1 + r2) / 2.0;
    let v1_circ = (mu / r1).sqrt();
    let v1_trans = (mu * (2.0 / r1 - 1.0 / a_t)).sqrt();
    let v2_circ = (mu / r2).sqrt();
    let v2_trans = (mu * (2.0 / r2 - 1.0 / a_t)).sqrt();
    ((v1_trans - v1_circ).abs(), (v2_circ - v2_trans).abs())
}

pub fn laplace_resonance_check(p1: f64, p2: f64, p3: f64) -> (f64, f64) {
    (p2 / p1, p3 / p2)
}