use std::cell::RefCell;

use nalgebra::DMatrix;
use rapier3d::{
    math::Vector,
    na::vector,
    prelude::{
        CCDSolver, Collider, ColliderHandle, ColliderSet, DefaultBroadPhase, ImpulseJointSet,
        IntegrationParameters, IslandManager, MultibodyJointSet, NarrowPhase, PhysicsPipeline, Ray,
        RigidBody, RigidBodyHandle, RigidBodySet,
    },
};

thread_local! {
    static GLOBAL_PHYSICS: RefCell<PhysicsManager> = RefCell::new(PhysicsManager::new());
}

pub struct HeightfieldData
{
    pub heights: DMatrix<f32>,
    pub scale: Vector<f32>,
    pub position: Vector<f32>,
}

pub struct PhysicsManager
{
    rigidbody_set: RigidBodySet,
    collider_set: ColliderSet,
    gravity: Vector<f32>,
    integration_parameters: IntegrationParameters,
    physics_pipeline: PhysicsPipeline,
    island_manager: IslandManager,
    broad_phase: DefaultBroadPhase,
    narrow_phase: NarrowPhase,
    impulse_joint_set: ImpulseJointSet,
    multibody_joint_set: MultibodyJointSet,
    ccd_solver: CCDSolver,
    physics_hooks: (),
    event_handler: (),
    heightfield_data: Option<HeightfieldData>,
}

impl PhysicsManager
{
    pub fn new() -> Self
    {
        Self {
            rigidbody_set: RigidBodySet::new(),
            collider_set: ColliderSet::new(),
            gravity: vector![0.0, -9.81, 0.0],
            integration_parameters: IntegrationParameters::default(),
            physics_pipeline: PhysicsPipeline::new(),
            island_manager: IslandManager::new(),
            broad_phase: DefaultBroadPhase::new(),
            narrow_phase: NarrowPhase::new(),
            impulse_joint_set: ImpulseJointSet::new(),
            multibody_joint_set: MultibodyJointSet::new(),
            ccd_solver: CCDSolver::new(),
            physics_hooks: (),
            event_handler: (),
            heightfield_data: None,
        }
    }

    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.rigidbody_set,
            &mut self.collider_set,
            &mut self.impulse_joint_set,
            &mut self.multibody_joint_set,
            &mut self.ccd_solver,
            &self.physics_hooks,
            &self.event_handler,
        );
    }

    fn add_collider_internal(
        &mut self,
        collider: Collider,
        parent: Option<RigidBodyHandle>,
    ) -> ColliderHandle
    {
        if let Some(parent) = parent
        {
            self.collider_set
                .insert_with_parent(collider, parent, &mut self.rigidbody_set)
        }
        else
        {
            self.collider_set.insert(collider)
        }
    }

    pub fn physics_step()
    {
        GLOBAL_PHYSICS.with(|manager| manager.borrow_mut().step());
    }

    pub fn add_rigidbody(rigidbody: RigidBody) -> RigidBodyHandle
    {
        GLOBAL_PHYSICS.with(|manager| manager.borrow_mut().rigidbody_set.insert(rigidbody))
    }

    pub fn add_collider(collider: Collider, parent: Option<RigidBodyHandle>) -> ColliderHandle
    {
        GLOBAL_PHYSICS.with(|manager| manager.borrow_mut().add_collider_internal(collider, parent))
    }

    pub fn with_rigidbody_mut<F, R>(handle: RigidBodyHandle, f: F) -> Option<R>
    where
        F: FnOnce(&mut RigidBody) -> R,
    {
        GLOBAL_PHYSICS.with(|manager| manager.borrow_mut().rigidbody_set.get_mut(handle).map(f))
    }

    pub fn with_collider_mut<F, R>(handle: ColliderHandle, f: F) -> Option<R>
    where
        F: FnOnce(&mut Collider) -> R,
    {
        GLOBAL_PHYSICS.with(|manager| manager.borrow_mut().collider_set.get_mut(handle).map(f))
    }

    pub fn get_rigidbody_position(handle: RigidBodyHandle) -> Option<rapier3d::na::Isometry3<f32>>
    {
        GLOBAL_PHYSICS.with(|manager| {
            manager
                .borrow()
                .rigidbody_set
                .get(handle)
                .map(|rb| *rb.position())
        })
    }

    pub fn get_all_collider_aabbs() -> Vec<([f32; 3], [f32; 3])>
    {
        GLOBAL_PHYSICS.with(|manager| {
            let manager = manager.borrow();
            manager
                .collider_set
                .iter()
                .map(|(_, collider)| {
                    let aabb = collider.compute_aabb();
                    (
                        [aabb.mins.x, aabb.mins.y, aabb.mins.z],
                        [aabb.maxs.x, aabb.maxs.y, aabb.maxs.z],
                    )
                })
                .collect()
        })
    }

    pub fn raycast(
        origin: Vector<f32>,
        direction: Vector<f32>,
        max_distance: f32,
        exclude_rigidbody: Option<RigidBodyHandle>,
    ) -> Option<(ColliderHandle, f32)>
    {
        GLOBAL_PHYSICS.with(|manager| {
            let manager = manager.borrow();
            let ray = Ray::new(origin.into(), direction);

            let mut closest_hit: Option<(ColliderHandle, f32)> = None;

            for (handle, collider) in manager.collider_set.iter()
            {
                if let Some(exclude_rb) = exclude_rigidbody
                {
                    if collider.parent() == Some(exclude_rb)
                    {
                        continue;
                    }
                }

                if let Some(toi) =
                    collider
                        .shape()
                        .cast_ray(collider.position(), &ray, max_distance, true)
                {
                    if let Some((_, closest_toi)) = closest_hit
                    {
                        if toi < closest_toi
                        {
                            closest_hit = Some((handle, toi));
                        }
                    }
                    else
                    {
                        closest_hit = Some((handle, toi));
                    }
                }
            }

            closest_hit
        })
    }

    pub fn set_heightfield_data(heights: DMatrix<f32>, scale: Vector<f32>, position: Vector<f32>)
    {
        GLOBAL_PHYSICS.with(|manager| {
            manager.borrow_mut().heightfield_data = Some(HeightfieldData {
                heights,
                scale,
                position,
            });
        });
    }

    pub fn get_terrain_height_at(x: f32, z: f32) -> Option<f32>
    {
        GLOBAL_PHYSICS.with(|manager| {
            let manager = manager.borrow();
            let data = manager.heightfield_data.as_ref()?;

            let local_x = x - data.position.x;
            let local_z = z - data.position.z;

            let normalized_x = (local_x / data.scale.x) + 0.5;
            let normalized_z = (local_z / data.scale.z) + 0.5;

            let nrows = data.heights.nrows();
            let ncols = data.heights.ncols();

            let row_f = normalized_z * (nrows - 1) as f32;
            let col_f = normalized_x * (ncols - 1) as f32;

            if row_f < 0.0
                || row_f >= (nrows - 1) as f32
                || col_f < 0.0
                || col_f >= (ncols - 1) as f32
            {
                return None;
            }

            let row0 = row_f.floor() as usize;
            let row1 = (row0 + 1).min(nrows - 1);
            let col0 = col_f.floor() as usize;
            let col1 = (col0 + 1).min(ncols - 1);

            let frac_row = row_f - row0 as f32;
            let frac_col = col_f - col0 as f32;

            let h00 = data.heights[(row0, col0)];
            let h01 = data.heights[(row0, col1)];
            let h10 = data.heights[(row1, col0)];
            let h11 = data.heights[(row1, col1)];

            let h0 = h00 * (1.0 - frac_col) + h01 * frac_col;
            let h1 = h10 * (1.0 - frac_col) + h11 * frac_col;
            let height = h0 * (1.0 - frac_row) + h1 * frac_row;

            Some(height * data.scale.y + data.position.y)
        })
    }

    pub fn get_terrain_slope_in_direction(x: f32, z: f32, direction_x: f32, direction_z: f32)
        -> f32
    {
        const SAMPLE_DISTANCE: f32 = 0.5;

        let dir_len = (direction_x * direction_x + direction_z * direction_z).sqrt();
        if dir_len < 0.001
        {
            return 0.0;
        }

        let norm_dir_x = direction_x / dir_len;
        let norm_dir_z = direction_z / dir_len;

        let height_current = Self::get_terrain_height_at(x, z).unwrap_or(0.0);
        let height_forward = Self::get_terrain_height_at(
            x + norm_dir_x * SAMPLE_DISTANCE,
            z + norm_dir_z * SAMPLE_DISTANCE,
        )
        .unwrap_or(height_current);

        let height_diff = height_forward - height_current;
        let slope_angle = (height_diff / SAMPLE_DISTANCE).atan();

        slope_angle
    }
}
