use std::collections::HashMap;
use std::f64::consts::PI;
use avian2d::math::TAU;
use avian2d::prelude::{LinearVelocity, Mass};
use bevy::prelude::{Plugin, Transform};
use bevy::time::Time;
use crate::shared::config::planet::{Planet, PlanetSpring};
use crate::prelude::{App, Query, Res, Update, Without};
use crate::shared::world_config::WorldConfigResource;

const KEPLER_MAX_ITERATIONS: u32 = 100;
const KEPLER_CONVERGENCE_THRESHOLD: f64 = 1e-10;

pub struct OrbitPlugin;
impl Plugin for OrbitPlugin {
    fn build(&self, app: &mut App) {
        app.add_systems(Update, update_orbits);
    }
}

fn update_orbits(
    mut planets: Query<(&Planet, &Transform, &mut LinearVelocity), Without<PlanetSpring>>,
    planets_2: Query<(&Planet, &Transform, &Mass), Without<PlanetSpring>>,
    mut planet_springs: Query<(&PlanetSpring, &mut Transform, &mut LinearVelocity), Without<Planet>>,
    world_config: Res<WorldConfigResource>,
    time: Res<Time>
) {
    let Some(ref world_config) = world_config.config else {
        return;
    };
    let parent_velocities = planets.iter().map(|u| (u.0.name.clone(), u.2.clone())).collect::<HashMap<String, LinearVelocity>>();
    for (planet, _, _vel) in planets.iter_mut() {
        let Some(orbit_data) = &planet.orbit else { continue; };
        // find parent
        let Some(parent) = planets_2.iter().find(|u| u.0.name == orbit_data.orbiting) else { continue; };

        let (parent_data, parent_transform, parent_mass) = parent;
        // Orbital elements:
        //   a   = semi-major axis
        //   e   = eccentricity
        //   t   = orbital period
        //   m   = mean anomaly
        //   e_k = eccentric anomaly (solution to Kepler's equation)
        //   nu  = true anomaly
        //   r   = radial distance
        let a = (planet.default_position.x as f64 - parent_data.default_position.x as f64) / (1.0 - orbit_data.eccentricity);
        let e = orbit_data.eccentricity;
        let t = 2.0*PI*((a*a*a)/(world_config.world.gravity*(**parent_mass as f64))).sqrt();

        let time = time.elapsed_secs_f64();

        // calculate position of the planet
        let m = (TAU / t) * time;
        let e_k = iterative_kepler(m, e);
        let nu = 2.0_f64 * ((1.0 + e).sqrt() * (e_k / 2.0).sin())
            .atan2((1.0 - e).sqrt() * (e_k / 2.0).cos());
        let r = a * (1.0 - e * e_k.cos());

        let x = r * nu.cos();
        let y = r * nu.sin();

        // find the spring
        let Some(mut planet_spring) = planet_springs.iter_mut().find(|u| u.0.name == planet.name) else { continue; };
        planet_spring.1.translation.x = x as f32 + parent_transform.translation.x;
        planet_spring.1.translation.y = y as f32 + parent_transform.translation.y;

        let Some(parent_velocity) = parent_velocities.get(&orbit_data.orbiting) else { continue; };
        let de_dt = (TAU / t) / (1.0 - e * e_k.cos());
        let b_factor = (1.0 - e * e).sqrt();
        let vx = -a * e_k.sin() * de_dt;
        let vy = a * b_factor * e_k.cos() * de_dt;
        planet_spring.2.x = vx + parent_velocity.x;
        planet_spring.2.y = vy + parent_velocity.y;
    }
}

fn iterative_kepler(m: f64, e: f64) -> f64 {
    let mut output = m;
    for _ in 0..KEPLER_MAX_ITERATIONS {
        let d = (m - output + e * output.sin()) / (1.0 - e * output.cos());
        output += d;
        if d.abs() < KEPLER_CONVERGENCE_THRESHOLD {
            break;
        }
    }
    output
}