use crate::solar_system::{Angle, BodyId, Kilometers, OrbitalBody, Percentage};
use crate::timeman::{Second};

pub struct StaticOrbit
{
    pub parent: Option<BodyId>,
    pub eccentricity: Percentage,
    pub inclination: Angle,
    pub long_asc_node: Angle,
    pub long_periapsis: Angle,
    pub mean_long: Angle,

    pub semi_major_axis: Kilometers,
}

impl StaticOrbit
{
    pub fn period(
        &self,
        this_body: &OrbitalBody)
        -> Second
    {
        ((self.semi_major_axis.powf(3.0) / this_body.sgp).sqrt() * std::f64::consts::TAU) as Second 
    }

    pub fn parent(&self) -> Option<BodyId> {
        self.parent
    }

    pub fn calculate_position_at(
        &self,
        this_body: &OrbitalBody,
        time: Second)
    -> cgmath::Vector3<f64>
    {
        if self.parent.is_none() {
            return cgmath::Vector3::<f64>::new(0.0, 0.0, 0.0);
        }

        let eccentricity = self.eccentricity;

        let long_asc_node = self.long_asc_node;
        let arg_periaps = self.long_periapsis - long_asc_node;

        let sma_cubed = self.semi_major_axis.powf(3.0);   
        let mean_motion = (this_body.sgp / sma_cubed).sqrt();

        let mean_anomaly_epoch = self.mean_long - self.long_periapsis;
        let mean_anomaly = mean_anomaly_epoch + (mean_motion * time as f64);

        //Find the eccentric anomaly via newton's method
        let mut eccentric_anomaly = mean_anomaly;
        for _ in 0..100 {
            let (e_sin, e_cos) = eccentric_anomaly.sin_cos();
            let new_anomaly = eccentric_anomaly -
                (
                    (eccentric_anomaly - eccentricity * e_sin - mean_anomaly) /
                    (1.0 - eccentricity * e_cos)
                );

            let diff = eccentric_anomaly - new_anomaly;
            eccentric_anomaly = new_anomaly;
            if diff.abs() < 1e-6 {
                break;
            }
        }

        let true_anomaly = 2.0 * (
            ((1.0 + eccentricity) / (1.0 - eccentricity)).sqrt() 
            * (eccentric_anomaly / 2.0).tan()
        ).atan();

        let vw = true_anomaly + arg_periaps;
        let (vw_sin, vw_cos) = vw.sin_cos();
        let (omega_sin, omega_cos) = long_asc_node.sin_cos();
        let (i_sin, i_cos) = self.inclination.sin_cos();

        let r = self.semi_major_axis * (1.0 - eccentricity * eccentric_anomaly.cos());
        let x = r * (omega_cos * vw_cos - omega_sin * vw_sin * i_cos);
        let y = r * i_sin * vw_sin;
        let z = r * (omega_sin * vw_cos + omega_cos * vw_sin * i_cos);

        cgmath::Vector3::<f64>::new(x, y, z)
    }
}