use glam::{Quat, Vec3};
use rapier3d::na::{Quaternion, UnitQuaternion};
use rapier3d::prelude::*;
use raidillon_ecs::Transform;

/// Tiny wrapper around rapier3d.
pub struct Physics {
    pub rigid_body_set: RigidBodySet,
    pub collider_set: ColliderSet,
    physics_pipeline: PhysicsPipeline,
    island_manager: IslandManager,
    pub broad_phase: DefaultBroadPhase,
    pub narrow_phase: NarrowPhase,
    impulse_joint_set: ImpulseJointSet,
    multibody_joint_set: MultibodyJointSet,
    ccd_solver: CCDSolver,
    gravity: Vector<f32>,
    integration_parameters: IntegrationParameters,
}

impl Default for Physics {
    fn default() -> Self {
        Self {
            gravity: vector![0.0, -9.81, 0.0],
            rigid_body_set: Default::default(),
            collider_set: Default::default(),
            physics_pipeline: Default::default(),
            island_manager: Default::default(),
            broad_phase: Default::default(),
            narrow_phase: Default::default(),
            impulse_joint_set: Default::default(),
            multibody_joint_set: Default::default(),
            ccd_solver: Default::default(),
            integration_parameters: Default::default(),
        }
    }
}

impl Physics {
    pub fn step(&mut self, dt: f32) {
        self.integration_parameters.dt = dt;
        self.physics_pipeline.step(
            &self.gravity,
            &self.integration_parameters,
            &mut self.island_manager,
            &mut self.broad_phase,
            &mut self.narrow_phase,
            &mut self.rigid_body_set,
            &mut self.collider_set,
            &mut self.impulse_joint_set,
            &mut self.multibody_joint_set,
            &mut self.ccd_solver,
            &(),
            &(),
        );
    }

    pub fn add_rigid_body(&mut self, kind: RigidBodyType, transform: Transform, collider: Collider) -> RigidBodyHandle {
        let rb = RigidBodyBuilder::new(kind)
            .translation(vector![transform.translation.x, transform.translation.y, transform.translation.z])
            .build();
        let rb_handle = self.rigid_body_set.insert(rb);
        self.collider_set.insert_with_parent(collider, rb_handle, &mut self.rigid_body_set);
        rb_handle
    }

    pub fn get_rigid_body(&self, handle: RigidBodyHandle) -> Option<&RigidBody> {
        self.rigid_body_set.get(handle)
    }

    pub fn get_rigid_body_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
        self.rigid_body_set.get_mut(handle)
    }

    pub fn rapier_translation_to_glam(v: &Vector<f32>) -> Vec3 {
        Vec3::new(v.x, v.y, v.z)
    }

    pub fn rapier_rotation_to_glam(r: &UnitQuaternion<f32>) -> Quat {
        Quat::from_xyzw(r.i, r.j, r.k, r.w)
    }
}