use ::core::f64;

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, Rotation3, Unit};
use physics::particle_system::{Particle, ParticleSystem};
use physics::renderer::Camera;
use physics::solver::Solver;

const SCALE: Scalar = 25.0;
const CAMERA_DISTANCE: Scalar = 20.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 = 1e-5;
    let mut system = ParticleSystem {
        particles: vec![
            Particle::new(Point::new(3.0, 0.0, 0.0), 1.0),
            Particle::new(Point::new(0.0, 0.0, 0.0), 1.0),
            Particle::new(Point::new(0.0, 4.0, 0.0), 1.0),
            Particle::new(Point::new(-10.0, 00.0, 0.0), 1.0),
            // Particle::new(Point::new(0.0, -100.0, -100.0), 1.0),
        ],
        constraints: vec![],
        forces: vec![
            Box::new(Gravity {
                vector: Vector::y() * -9.8,
            }),
            Box::new(Drag { coefficient: 0.2 }),
            // Box::new(Spring {
            //     particle_ids: [0, 3],
            //     spring_constant: 1000.00,
            //     damping_constant: 36.1,
            //     rest_length: 13.0,
            // }),
        ],
        t: 0.0,
    };

    system.add_beam_constraint([0, 1]);
    system.add_beam_constraint([1, 2]);
    system.add_beam_constraint([2, 0]);

    system.add_slider_constraint(3, Vector::x());
    system.add_beam_constraint([3, 0]);

    // system.add_beam_constraint([3, 4]);
    // system.add_distsance_constraint(3, Line::new(Point::origin(), [Vector::x()]), 0.0);

    // system.add_slider_constraint(5, Vector::x());
    // system.add_anchor_constraint(2, Point::origin());
    //
    // system.add_beam_constraint([1, 2]);
    // system.add_beam_constraint([1, 3]);
    // system.add_beam_constraint([2, 3]);
    // 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 camera_right = Vector::x();

    let trace_particle_id = 3;
    let mut trace_particle_positions: Vec<PointBase<i32, 2>> = vec![];

    let mut max_kinetic: Scalar = 0.0;

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

    while !rl.window_should_close() {
        // sleep(Duration::from_millis(1));
        // dbg!(&system.particles[3]);

        let mut d = rl.begin_drawing(&thread);

        let kinetic = system.get_kinetic_energy();
        max_kinetic = max_kinetic.max(kinetic);

        d.clear_background(Color::WHITE);
        let fps = d.get_fps();
        let mut text = format!(
                "Time: {:03.3}\n\nFPS: {}\n\nKinetic energy: {:03.0}\n\nMax kinetic energy: {:03.0}\n\n",
                system.t,
                fps,
                kinetic,
                max_kinetic,
            );

        {
            let (q, _) = system.collect_q();
            for (id, constraint) in system.constraints.iter().enumerate() {
                let c = constraint.c(&q);
                text += &format!("Constraint {}: {:.05}\n\n", id, c);
            }
        }

        d.draw_text(text.as_str(), 12, 12, 20, Color::BLACK);

        if d.is_key_down(KeyboardKey::KEY_L) {
            camera_right = Rotation3::from_axis_angle(
                &Unit::new_normalize(Vector::y()),
                (f64::consts::PI / 180.0 / 10.0) as Scalar,
            ) * camera_right;
        }
        if d.is_key_down(KeyboardKey::KEY_H) {
            camera_right = Rotation3::from_axis_angle(
                &Unit::new_normalize(Vector::y()),
                (-f64::consts::PI / 180.0 / 10.0) as Scalar,
            ) * camera_right;
        }

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

        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..500 as usize {
            system.apply_forces();
            if d.is_key_down(KeyboardKey::KEY_Z) {
                for particle in &mut system.particles {
                    particle.apply_force(Vector::z() * 20.0);
                }
            }
            system.enforce_constraints();
            system.step(dt);
        }
        // panic!("HALT");

        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_factor = (CAMERA_DISTANCE - particle.position.z) / CAMERA_DISTANCE;
            let radius = particle.mass.powf(1.0 / (N as Scalar)) as f32
                * SCALE as f32
                * radius_factor 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::new(
                        0,
                        0,
                        (particle.position.z / CAMERA_DISTANCE * 255.0) as u8,
                        255,
                    )
                },
            );

            let force_end = screen_space_to_raylib(camera.world_to_screen_space(particle.position + particle.velocity), &d);
            d.draw_line(position.x, position.y, force_end.x, force_end.y, Color::BLUE);

            if particle_id == trace_particle_id {
                trace_particle_positions.push(position);
                if trace_particle_positions.len() > 500 {
                    trace_particle_positions.remove(0);
                }
            }
        }
        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);
            }
        }
        for (i, pos) in trace_particle_positions.iter().enumerate() {
            d.draw_circle(
                pos.x,
                pos.y,
                1.0,
                Color::new(
                    0,
                    255,
                    0,
                    (i as f32 / trace_particle_positions.len() as f32 * 255.0) as u8,
                ),
            );
        }

        {
            // 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 => (),
            }
        }
    }
}