use crate::shared::config::planet::Planet;
use crate::shared::ecs::Part;
use crate::prelude::*;
use crate::shared::world_config::WorldConfigResource;

pub fn update_gravity(
    mut part_query: Query<(&Position, &LinearVelocity, &Mass, &mut ConstantForce), With<Part>>,
    planet_query: Query<(&Position, &Mass), With<Planet>>,
    world_config: Res<WorldConfigResource>,
    time: Res<Time>,
) {
    let Some(world_config) = &world_config.config else {
        return;
    };

    for (part_position, part_velocity, part_mass, mut forces) in &mut part_query {
        *forces = ConstantForce::new(0.0, 0.0);

        let part_mass = part_mass.0 as f64;
        let part_pos = part_position.0;

        for (planet_position, planet_mass) in &planet_query {
            let planet_mass = planet_mass.0 as f64;
            let planet_pos = planet_position.0;

            let distance = planet_pos.distance(part_pos);

            let mut x = 0.0;
            let mut total_f = 0.0;
            let mut v = part_velocity.0;
            let dt = time.delta_secs() as f64 / world_config.world.gravity_iterations as f64;
            for i in 0..world_config.world.gravity_iterations {
                let f =
                    world_config.world.gravity * ((part_mass * planet_mass) / ((distance - x)*(distance - x)));
                let dx = dt*(v.project_onto(planet_pos - part_pos)).length()
                    - 1.0/2.0*(f/part_mass)*(dt*dt);
                x += dx;
                v += f/part_mass*dt;
                if i == 0 || i == world_config.world.gravity_iterations-1 {
                    total_f += f;
                } else {
                    total_f += 2.0*f;
                }
            }
            let force = (dt/2.0*(total_f))/time.delta_secs() as f64;
            let direction = (planet_pos - part_pos).normalize() * force;
            forces.x += direction.x;
            forces.y += direction.y;
        }
    }
}