use std::collections::HashMap;

use macroquad::prelude as mq;
use nalgebra as na;
use rapier3d::prelude::{self as rp, ColliderHandle};

fn random_color() -> mq::Color {
    return mq::Color::new(
        rand::random_range(0.0..1.0),
        rand::random_range(0.0..1.0),
        rand::random_range(0.0..1.0),
        1.0,
    );
}

pub struct World {
    physics_pipeline: rp::PhysicsPipeline,
    gravity: na::Vector3<f32>,
    integration_parameters: rp::IntegrationParameters,
    island_manager: rp::IslandManager,
    broad_phase: rp::BroadPhaseBvh,
    narrow_phase: rp::NarrowPhase,
    impulse_joint_set: rp::ImpulseJointSet,
    multibody_joint_set: rp::MultibodyJointSet,
    ccd_solver: rp::CCDSolver,
    hooks: Box<dyn rp::PhysicsHooks>,
    events: Box<dyn rp::EventHandler>,

    pub bodies: rp::RigidBodySet,
    pub colliders: rp::ColliderSet,
    pub collider_data: HashMap<rp::ColliderHandle, ColliderExtraData>,
    pub tick: u64,
}

pub struct ColliderExtraData {
    pub color: mq::Color,
}

impl World {
    pub fn step(&mut self) {
        self.physics_pipeline.step(
            &self.gravity,
            &self.integration_parameters,
            &mut self.island_manager,
            &mut self.broad_phase,
            &mut self.narrow_phase,
            &mut self.bodies,
            &mut self.colliders,
            &mut self.impulse_joint_set,
            &mut self.multibody_joint_set,
            &mut self.ccd_solver,
            self.hooks.as_ref(),
            self.events.as_ref(),
        );
        self.tick += 1;
    }

    pub fn get_time(&self) -> f32 {
        return self.integration_parameters.dt * (self.tick as f32);
    }

    pub fn clear_ofb(&mut self) {
        let mut coll_to_del: Vec<ColliderHandle> = Vec::new();
        for (handle, col) in self.colliders.iter() {
            if col.translation().y < -30.0 {
                coll_to_del.push(handle);
            }
        }
        for handle in coll_to_del {
            self.colliders
                .remove(handle, &mut self.island_manager, &mut self.bodies, false);
        }
    }

    pub fn position_of_collider(
        &self,
        collider_handle: rp::ColliderHandle,
    ) -> Option<na::Vector3<f32>> {
        let coll = self.colliders.get(collider_handle)?;
        return Some(*coll.translation());
    }

    pub fn register_body(&mut self, rb: rp::RigidBody) -> rp::RigidBodyHandle {
        return self.bodies.insert(rb);
    }

    pub fn register_collider(
        &mut self,
        collider: rp::Collider,
        rigid_body_handle: rp::RigidBodyHandle,
        extra_options: Option<ColliderExtraData>,
    ) -> rp::ColliderHandle {
        let handle =
            self.colliders
                .insert_with_parent(collider, rigid_body_handle, &mut self.bodies);
        self.collider_data.insert(
            handle,
            extra_options.unwrap_or(ColliderExtraData {
                color: random_color(),
            }),
        );
        return handle;
    }

    pub fn register_free_collider(
        &mut self,
        collider: rp::Collider,
        extra_options: Option<ColliderExtraData>,
    ) -> rp::ColliderHandle {
        let handle = self.colliders.insert(collider);
        self.collider_data.insert(
            handle,
            extra_options.unwrap_or(ColliderExtraData {
                color: random_color(),
            }),
        );
        return handle;
    }
}

impl Default for World {
    fn default() -> Self {
        let gravity: na::Vector3<f32> = rp::vector![0.0, -9.81, 0.0];
        let mut integration_parameters = rp::IntegrationParameters::default();
        integration_parameters.set_inv_dt(600.0);
        let physics_pipeline = rp::PhysicsPipeline::new();
        let island_manager = rp::IslandManager::new();
        let broad_phase = rp::DefaultBroadPhase::new();
        let narrow_phase = rp::NarrowPhase::new();
        let impulse_joint_set = rp::ImpulseJointSet::new();
        let multibody_joint_set = rp::MultibodyJointSet::new();
        let ccd_solver = rp::CCDSolver::new();
        let physics_hooks = ();
        let event_handler = ();
        return Self {
            physics_pipeline,
            gravity: gravity,
            integration_parameters,
            island_manager,
            broad_phase,
            narrow_phase,
            impulse_joint_set,
            multibody_joint_set,
            ccd_solver,
            hooks: Box::new(physics_hooks),
            events: Box::new(event_handler),
            bodies: rp::RigidBodySet::new(),
            colliders: rp::ColliderSet::new(),
            collider_data: HashMap::new(),
            tick: 0,
        };
    }
}

pub fn collider_world_aabb(world: &World, handle: rp::ColliderHandle) -> (mq::Vec3, mq::Vec3) {
    use rp::ShapeType;
    let pos: glam::Vec3 = world.position_of_collider(handle).unwrap().into();
    let shape = world.colliders.get(handle).unwrap().shape();

    match shape.shape_type() {
        ShapeType::Cuboid => {
            let he: glam::Vec3 = (shape.as_cuboid().unwrap().half_extents * 2.0).into();
            let min = mq::vec3(pos.x - he.x * 0.5, pos.y - he.y * 0.5, pos.z - he.z * 0.5);
            let max = mq::vec3(pos.x + he.x * 0.5, pos.y + he.y * 0.5, pos.z + he.z * 0.5);
            (min, max)
        }

        ShapeType::Ball => {
            let r = shape.as_ball().unwrap().radius;
            let min = pos - mq::vec3(r, r, r);
            let max = pos + mq::vec3(r, r, r);
            (min, max)
        }

        _ => {
            // Default fallback
            (pos - mq::vec3(1.0, 1.0, 1.0), pos + mq::vec3(1.0, 1.0, 1.0))
        }
    }
}
