use raylib::prelude::*;

use physics::algebra::{Point, Scalar, Vector, N};
use physics::force::{drag::Drag, gravity::Gravity, spring::Spring};
use physics::nalgebra::Point as PointBase;
use physics::particle_system::{Particle, ParticleSystem};
use physics::renderer::Camera;
use physics::solver::Solver;

const SCALE: Scalar = 10.0;
fn screen_space_to_raylib(p: PointBase<Scalar, 2>, d: &RaylibDrawHandle) -> PointBase<i32, 2> {
    PointBase::<i32, 2>::new(
        ((d.get_screen_width() / 2) as Scalar + p.x * SCALE) as i32,
        ((d.get_screen_height() / 2) as Scalar - p.y * SCALE) as i32,
    )
}
fn raylib_to_screen_space(p: PointBase<i32, 2>, d: &RaylibDrawHandle) -> PointBase<Scalar, 2> {
    PointBase::<Scalar, 2>::new(
        (p.x - d.get_screen_width() / 2) as Scalar / SCALE,
        (p.y - d.get_screen_height() / 2) as Scalar / -SCALE,
    )
}

fn main() {
    let dt = 0.0001;
    let mut system = ParticleSystem {
        particles: vec![
            Particle::new(Point::origin(), 4.0),
            Particle::new(Point::new(-30.0, 0.0), 15.0),
            Particle::new(Point::new(20.0, 0.0), 30.0),
            Particle::new(Point::new(5.0, 20.0), 50.0),
            Particle::new(Point::origin(), 25.0),
            Particle::new(Point::new(50.0, 0.0), 10.0),
            Particle::new(Point::new(-30.0, -30.0), 10.0),
        ],
        constraints: vec![],
        forces: vec![
            Box::new(Gravity {
                vector: Vector::y() * -9.8,
            }),
            Box::new(Drag { coefficient: 0.2 }),
            Box::new(Spring {
                particle_ids: [4, 2],
                spring_constant: 0.75,
                damping_constant: 0.3,
                rest_length: 20.0,
            }),
        ],
        t: 0.0,
    };

    system.add_anchor_constraint(0, Point::origin());
    system.add_beam_constraint([0, 2]);
    system.add_beam_constraint([1, 2]);
    system.add_beam_constraint([1, 3]);
    system.add_beam_constraint([2, 3]);
    system.add_slider_constraint(5, Vector::x());
    system.add_beam_constraint([5, 4]);

    let mut selected_particle_id = None;
    let mut watch_particle_id = None;
    let mouse_particle_id = system.particles.len() - 1;

    let (mut rl, thread) = raylib::init()
        .size(640, 480)
        .title("Physics simulation")
        .resizable()
        .build();

    while !rl.window_should_close() {
        let mut d = rl.begin_drawing(&thread);

        d.clear_background(Color::WHITE);
        d.draw_text(
            format!(
                "Time: {:03.3}\n\nFPS: {}\n\nKinetic energy: {:03.0}",
                system.t,
                d.get_fps(),
                system.get_kinetic_energy()
            )
            .as_str(),
            12,
            12,
            20,
            Color::BLACK,
        );

        let camera = Camera::new(
            match watch_particle_id {
                Some(id) => {
                    let particle: &Particle = &system.particles[id];
                    particle.position
                },
                None => Point::origin(),
            },
            Vector::y(),
            Vector::x(),
        );

        match selected_particle_id {
            Some(particle_id) => {
                // let p: &Particle = &system.particles[particle_id];

                let mouse_point = PointBase::<i32, 2>::new(d.get_mouse_x(), d.get_mouse_y());
                let screen_space = raylib_to_screen_space(mouse_point, &d);
                let world_mouse = camera.screen_space_to_world(screen_space);

                system.particles[mouse_particle_id].position = world_mouse;
                system.particles[mouse_particle_id].velocity = Vector::zeros();

                system.forces.push(Box::new(Spring {
                    particle_ids: [mouse_particle_id, particle_id],
                    spring_constant: 25.0,
                    damping_constant: 10.00,
                    rest_length: 1.0,
                }));
            }
            None => (),
        }

        for _ in 0..10 {
            system.apply_forces();
            system.enforce_constraints();
            system.step(dt);
        }

        match selected_particle_id {
            Some(_) => {
                system.forces.pop();
            }
            None => (),
        }

        if d.is_mouse_button_released(MouseButton::MOUSE_BUTTON_LEFT) {
            selected_particle_id = None
        }

        for (particle_id, particle) in system.particles.iter().enumerate() {
            let position =
                screen_space_to_raylib(camera.world_to_screen_space(particle.position), &d);
            let radius = particle.mass.powf(1.0 / (N as f64)) as f32 * SCALE as f32;

            if d.is_mouse_button_down(MouseButton::MOUSE_BUTTON_LEFT)
                && selected_particle_id.is_none()
            {
                let mouse_point =
                    PointBase::<f32, 2>::new(d.get_mouse_x() as f32, d.get_mouse_y() as f32);
                let position = PointBase::<f32, 2>::new(position.x as f32, position.y as f32);
                if (position - mouse_point).norm() < radius {
                    selected_particle_id = Some(particle_id)
                }
            }

            if d.is_mouse_button_pressed(MouseButton::MOUSE_BUTTON_RIGHT) {
                let mouse_point =
                    PointBase::<f32, 2>::new(d.get_mouse_x() as f32, d.get_mouse_y() as f32);
                let position = PointBase::<f32, 2>::new(position.x as f32, position.y as f32);
                if (position - mouse_point).norm() < radius {
                    watch_particle_id = Some(particle_id)
                }
            }

            d.draw_circle(
                position.x,
                position.y,
                radius,
                if selected_particle_id.is_some_and(|v| v == particle_id) {
                    Color::RED
                } else if watch_particle_id.is_some_and(|v| v == particle_id) {
                    Color::YELLOW
                } else {
                    Color::BLACK
                },
            );
        }
        for c in &system.constraints {
            let particle_ids = c.get_particles();
            if particle_ids.len() == 2 {
                let a = screen_space_to_raylib(
                    camera.world_to_screen_space(system.particles[particle_ids[0]].position),
                    &d,
                );
                let b = screen_space_to_raylib(
                    camera.world_to_screen_space(system.particles[particle_ids[1]].position),
                    &d,
                );

                d.draw_line(a.x, a.y, b.x, b.y, Color::GRAY);
            }
        }

        {
            // Hard-coded spring
            let particle_ids = [4, 2];
            let a = screen_space_to_raylib(
                camera.world_to_screen_space(system.particles[particle_ids[0]].position),
                &d,
            );
            let b = screen_space_to_raylib(
                camera.world_to_screen_space(system.particles[particle_ids[1]].position),
                &d,
            );
            d.draw_line(a.x, a.y, b.x, b.y, Color::GREEN);
        }
        {
            // Hard-coded spring
            match selected_particle_id {
                Some(id) => {
                    let mouse_particle_id = system.particles.len() - 1;
                    let a = screen_space_to_raylib(
                        camera.world_to_screen_space(system.particles[id].position),
                        &d,
                    );
                    let b = screen_space_to_raylib(
                        camera.world_to_screen_space(system.particles[mouse_particle_id].position),
                        &d,
                    );

                    d.draw_line(a.x, a.y, b.x, b.y, Color::GREEN);
                }
                None => (),
            }
        }
    }
}