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

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

    semi_major_axis: Kilometers,
}

impl StaticOrbit
{
    pub fn new(
        parent: BodyId,
        eccentricity: Percentage,
        inclination: Angle,
        long_asc_node: Angle,
        long_periapsis: Angle,
        mean_long: Angle,
        semi_major_axis: Kilometers)
    -> Self {
        Self {
            parent,
            eccentricity,
            inclination,
            long_asc_node,
            long_periapsis,
            mean_long,
            semi_major_axis
        }
    }

    pub fn new_circular(
        parent: &OrbitalBody,
        radius: Kilometers)
    -> Self
    {
        Self {
            parent: parent.id(),
            eccentricity: 1e-6,
            inclination: 0.0,
            long_asc_node: 0.0,
            long_periapsis: 0.0,
            mean_long: 0.0,
            semi_major_axis: radius
        }
    }

    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) -> BodyId {
        self.parent
    }

    pub fn calculate_position_at(
        &self,
        sgp: f64,
        time: Second)
    -> cgmath::Vector3<f64>
    {
        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 = (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)
    }
}