use bevy::math::FloatPow;
use crate::config::planet::Planet;
use crate::ecs::Part;
use crate::prelude::*;
use crate::server::system_sets::WorldUpdateSet;
use crate::world_config::WorldConfigResource;

pub fn newtonian_gravity_plugin(app: &mut App) {
    app.add_systems(Update, update_gravity.in_set(WorldUpdateSet));
}


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

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

        let part_mass = part_mass.0;
        let part_translation = part_transform.translation;

        for (planet_transform, planet_mass) in &planet_query {
            let planet_mass = planet_mass.0;
            let planet_translation = planet_transform.translation;

            let distance = planet_translation.distance(part_translation);

            const GRAV_ITERS: u32 = 8;
            let mut x = 0.0;
            let mut total_f = 0.0;
            let mut v = part_velocity.0;
            let dt = time.delta_secs() / GRAV_ITERS as f32;
            for i in 0..GRAV_ITERS {
                let f =
                    world_config.world.gravity * ((part_mass * planet_mass) / distance.squared()-x);
                let dx = dt*(v.project_onto((planet_translation-part_translation).truncate())).length()
                    - 1.0/2.0*(f/part_mass)*dt*dt;
                x += dx;
                v += f/part_mass*dt;
                if i == 0 || i == GRAV_ITERS-1 {
                    total_f += f;
                } else {
                    total_f += 2.0*f;
                }
            }
            let force = (dt/2.0*(total_f))/time.delta_secs();
            let direction = (planet_translation - part_translation).normalize() * force;
            forces.x += direction.x;
            forces.y += direction.y;
        }
    }
}