use std::f32::consts::PI;


use crate::{config::planet::Planet, ecs::{Me, StarguideCamera, StarguideGizmos}, prelude::*, world_config::WorldConfigResource};

pub fn starguide_orbit_plugin(app: &mut App) {
    app
        .add_systems(Update, update_orbits);
}

fn update_orbits(
    camera: Single<(&Camera, &GlobalTransform, &Projection), With<StarguideCamera>>,
    me: Single<(&Transform, &LinearVelocity), (With<Me>, Without<StarguideCamera>)>,
    mut gizmos: Gizmos<StarguideGizmos>,
    world_config: Res<WorldConfigResource>,
    planets: Query<(&Mass, &Planet, &Transform)>,
) {
    let Some(world_config) = &world_config.config else {
        return;
    };
    let Projection::Orthographic(_) = camera.2.clone() else { return };

    let mut p_mass = None;
    let mut p_transform = None;
    let (sun_mass, _, sun_transform) = planets.iter().find(|planet| planet.1.name == "Sun").unwrap();
    let mut closest = f32::INFINITY;
    for (mass, planet, transform) in planets {
        if planet.name == "Sun" { continue }

        let (other_mass, _, other_transform) = planets.iter().find(|f_planet| f_planet.1.name == planet.orbit.clone().unwrap().orbiting).unwrap();
        let a = other_transform.translation - transform.translation;
        let hill_sphere = a.length()*(mass.0/(3.0*(other_mass.0+mass.0))).powf(1.0/3.0);

        gizmos.circle_2d(transform.translation.truncate(), hill_sphere, Color::linear_rgb(0.02, 0.02, 0.02));

        let rel_dist = (me.0.translation - transform.translation).length();
        if rel_dist < closest && hill_sphere > rel_dist {
            p_mass = Some(mass.0);
            p_transform = Some(*transform);
            closest = rel_dist;
        }
    }
    if p_mass.is_none()  {
        p_mass = Some(sun_mass.0);
        p_transform = Some(*sun_transform);
    }
    let p_mass = p_mass.unwrap();
    let p_transform = p_transform.unwrap();

    // orbit magic
    let rel_pos = me.0.translation - p_transform.translation;
    let u = world_config.world.gravity*p_mass;
    let h = rel_pos.x*me.1.y - rel_pos.y*me.1.x;
    let r = rel_pos.length();
    let a = (u*r) / (2.0*u - r*(me.1.x*me.1.x + me.1.y*me.1.y));
    let e_x = rel_pos.x/r - (h*me.1.y)/u;
    let e_y = rel_pos.y/r + (h*me.1.x)/u;
    let f_x = -2.0*a*e_x;
    let f_y = -2.0*a*e_y;

    // 200 steps in the revolution
    let mut first_pos = None;
    let mut last_pos = None;
    for i in 0..200 {
        let theta = 2.0*PI*(i as f32)/200.0;
        let r = (1.0/2.0) * ((f_x*f_x + f_y*f_y - 4.0*a*a) / (-2.0*a - f_x*theta.cos() - f_y*theta.sin()));

        if r < 0.0 { continue }

        // convert r to image coords
        let pos = Vec2::new(r*theta.cos(), r*theta.sin()) + p_transform.translation.truncate();

        if let Some(last_pos) = last_pos {
            gizmos.line_2d(last_pos, pos, Color::linear_rgb(1.0, 0.0, 0.0));
        }
        if first_pos.is_none() { first_pos = Some(pos) }
        last_pos = Some(pos);
    }
    if let Some(first_pos) = first_pos && let Some(last_pos) = last_pos {
        gizmos.line_2d(first_pos, last_pos, Color::linear_rgb(1.0, 0.0, 0.0));
    }
}