use nalgebra_glm::{Quat, Vec3};
use rapier3d::prelude::*;
use serde::{Deserialize, Serialize};

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct RigidBodyComponent {
    pub handle: Option<RigidBodyHandle>,
    pub body_type: RigidBodyType,
    pub translation: [f32; 3],
    pub rotation: [f32; 4],
    pub linvel: [f32; 3],
    pub angvel: [f32; 3],
    pub mass: f32,
    pub locked_axes: LockedAxes,
}

impl Default for RigidBodyComponent {
    fn default() -> Self {
        Self {
            handle: None,
            body_type: RigidBodyType::Dynamic,
            translation: [0.0, 0.0, 0.0],
            rotation: [0.0, 0.0, 0.0, 1.0],
            linvel: [0.0, 0.0, 0.0],
            angvel: [0.0, 0.0, 0.0],
            mass: 1.0,
            locked_axes: LockedAxes::empty(),
        }
    }
}

impl RigidBodyComponent {
    pub fn new_dynamic() -> Self {
        Self {
            body_type: RigidBodyType::Dynamic,
            ..Default::default()
        }
    }

    pub fn new_kinematic() -> Self {
        Self {
            body_type: RigidBodyType::KinematicPositionBased,
            ..Default::default()
        }
    }

    pub fn new_static() -> Self {
        Self {
            body_type: RigidBodyType::Fixed,
            ..Default::default()
        }
    }

    pub fn with_translation(mut self, x: f32, y: f32, z: f32) -> Self {
        self.translation = [x, y, z];
        self
    }

    pub fn with_mass(mut self, mass: f32) -> Self {
        self.mass = mass;
        self
    }

    pub fn with_rotation(mut self, x: f32, y: f32, z: f32, w: f32) -> Self {
        self.rotation = [x, y, z, w];
        self
    }

    pub fn to_rapier_rigid_body(&self) -> RigidBody {
        let mut rb = RigidBodyBuilder::new(self.body_type)
            .translation(vector![
                self.translation[0],
                self.translation[1],
                self.translation[2]
            ])
            .rotation(vector![
                self.rotation[0],
                self.rotation[1],
                self.rotation[2]
            ])
            .linvel(vector![self.linvel[0], self.linvel[1], self.linvel[2]])
            .angvel(vector![self.angvel[0], self.angvel[1], self.angvel[2]])
            .locked_axes(self.locked_axes)
            .build();

        if self.body_type == RigidBodyType::Dynamic {
            rb.set_additional_mass(self.mass, true);
        }

        rb
    }
}

#[derive(Debug, Clone)]
pub struct CharacterControllerComponent {
    pub controller: rapier3d::control::KinematicCharacterController,
    pub grounded: bool,
    pub velocity: Vec3,
    pub shape: ColliderShape,
    pub max_speed: f32,
    pub acceleration: f32,
    pub jump_impulse: f32,
    pub can_jump: bool,
}

impl Default for CharacterControllerComponent {
    fn default() -> Self {
        Self {
            controller: rapier3d::control::KinematicCharacterController::default(),
            grounded: false,
            velocity: Vec3::zeros(),
            shape: ColliderShape::Capsule {
                half_height: 0.9,
                radius: 0.3,
            },
            max_speed: 5.0,
            acceleration: 50.0,
            jump_impulse: 8.0,
            can_jump: false,
        }
    }
}

impl CharacterControllerComponent {
    pub fn new_capsule(half_height: f32, radius: f32) -> Self {
        let controller = rapier3d::control::KinematicCharacterController {
            offset: rapier3d::control::CharacterLength::Absolute(0.01),
            up: nalgebra::Vector3::y_axis(),
            max_slope_climb_angle: 45_f32.to_radians(),
            min_slope_slide_angle: 30_f32.to_radians(),
            autostep: Some(rapier3d::control::CharacterAutostep {
                max_height: rapier3d::control::CharacterLength::Absolute(0.3),
                min_width: rapier3d::control::CharacterLength::Absolute(0.2),
                include_dynamic_bodies: false,
            }),
            snap_to_ground: Some(rapier3d::control::CharacterLength::Absolute(0.2)),
            ..Default::default()
        };

        Self {
            controller,
            grounded: false,
            velocity: Vec3::zeros(),
            shape: ColliderShape::Capsule {
                half_height,
                radius,
            },
            max_speed: 5.0,
            acceleration: 50.0,
            jump_impulse: 8.0,
            can_jump: false,
        }
    }
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct ColliderComponent {
    pub handle: Option<ColliderHandle>,
    pub shape: ColliderShape,
    pub friction: f32,
    pub restitution: f32,
    pub density: f32,
    pub is_sensor: bool,
    pub collision_groups: InteractionGroups,
    pub solver_groups: InteractionGroups,
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub enum ColliderShape {
    Ball {
        radius: f32,
    },
    Cuboid {
        hx: f32,
        hy: f32,
        hz: f32,
    },
    Capsule {
        half_height: f32,
        radius: f32,
    },
    Cylinder {
        half_height: f32,
        radius: f32,
    },
    Cone {
        half_height: f32,
        radius: f32,
    },
    ConvexMesh {
        vertices: Vec<[f32; 3]>,
    },
    TriMesh {
        vertices: Vec<[f32; 3]>,
        indices: Vec<[u32; 3]>,
    },
    HeightField {
        nrows: usize,
        ncols: usize,
        heights: Vec<f32>,
        scale: [f32; 3],
    },
}

impl Default for ColliderComponent {
    fn default() -> Self {
        Self {
            handle: None,
            shape: ColliderShape::Cuboid {
                hx: 0.5,
                hy: 0.5,
                hz: 0.5,
            },
            friction: 0.5,
            restitution: 0.0,
            density: 1.0,
            is_sensor: false,
            collision_groups: InteractionGroups::all(),
            solver_groups: InteractionGroups::all(),
        }
    }
}

impl ColliderComponent {
    pub fn new_ball(radius: f32) -> Self {
        Self {
            shape: ColliderShape::Ball { radius },
            ..Default::default()
        }
    }

    pub fn new_cuboid(hx: f32, hy: f32, hz: f32) -> Self {
        Self {
            shape: ColliderShape::Cuboid { hx, hy, hz },
            ..Default::default()
        }
    }

    pub fn new_capsule(half_height: f32, radius: f32) -> Self {
        Self {
            shape: ColliderShape::Capsule {
                half_height,
                radius,
            },
            ..Default::default()
        }
    }

    pub fn new_cylinder(half_height: f32, radius: f32) -> Self {
        Self {
            shape: ColliderShape::Cylinder {
                half_height,
                radius,
            },
            ..Default::default()
        }
    }

    pub fn new_cone(half_height: f32, radius: f32) -> Self {
        Self {
            shape: ColliderShape::Cone {
                half_height,
                radius,
            },
            ..Default::default()
        }
    }

    pub fn with_friction(mut self, friction: f32) -> Self {
        self.friction = friction;
        self
    }

    pub fn with_restitution(mut self, restitution: f32) -> Self {
        self.restitution = restitution;
        self
    }

    pub fn with_density(mut self, density: f32) -> Self {
        self.density = density;
        self
    }

    pub fn as_sensor(mut self) -> Self {
        self.is_sensor = true;
        self
    }

    pub fn to_rapier_collider(&self) -> Collider {
        let shape: SharedShape = match &self.shape {
            ColliderShape::Ball { radius } => SharedShape::ball(*radius),
            ColliderShape::Cuboid { hx, hy, hz } => SharedShape::cuboid(*hx, *hy, *hz),
            ColliderShape::Capsule {
                half_height,
                radius,
            } => SharedShape::capsule_y(*half_height, *radius),
            ColliderShape::Cylinder {
                half_height,
                radius,
            } => SharedShape::cylinder(*half_height, *radius),
            ColliderShape::Cone {
                half_height,
                radius,
            } => SharedShape::cone(*half_height, *radius),
            ColliderShape::ConvexMesh { vertices } => {
                let points: Vec<Point<Real>> = vertices
                    .iter()
                    .map(|v| Point::new(v[0], v[1], v[2]))
                    .collect();
                SharedShape::convex_hull(&points).unwrap_or_else(|| SharedShape::ball(0.1))
            }
            ColliderShape::TriMesh { vertices, indices } => {
                let points: Vec<Point<Real>> = vertices
                    .iter()
                    .map(|v| Point::new(v[0], v[1], v[2]))
                    .collect();
                SharedShape::trimesh(points, indices.clone())
            }
            ColliderShape::HeightField {
                nrows,
                ncols,
                heights,
                scale,
            } => SharedShape::heightfield(
                DMatrix::from_vec(*nrows, *ncols, heights.clone()),
                vector![scale[0], scale[1], scale[2]],
            ),
        };

        ColliderBuilder::new(shape)
            .friction(self.friction)
            .restitution(self.restitution)
            .density(self.density)
            .sensor(self.is_sensor)
            .collision_groups(self.collision_groups)
            .solver_groups(self.solver_groups)
            .build()
    }
}

#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize)]
pub struct PhysicsInterpolation {
    pub previous_translation: Vec3,
    pub previous_rotation: Quat,
    pub current_translation: Vec3,
    pub current_rotation: Quat,
    pub enabled: bool,
}

impl PhysicsInterpolation {
    pub fn new() -> Self {
        Self {
            previous_translation: Vec3::zeros(),
            previous_rotation: Quat::identity(),
            current_translation: Vec3::zeros(),
            current_rotation: Quat::identity(),
            enabled: true,
        }
    }

    pub fn interpolate(&self, alpha: f32) -> (Vec3, Quat) {
        if !self.enabled {
            return (self.current_translation, self.current_rotation);
        }

        let translation =
            nalgebra_glm::lerp(&self.previous_translation, &self.current_translation, alpha);
        let rotation =
            nalgebra_glm::quat_slerp(&self.previous_rotation, &self.current_rotation, alpha);
        (translation, rotation)
    }

    pub fn update_previous(&mut self) {
        self.previous_translation = self.current_translation;
        self.previous_rotation = self.current_rotation;
    }

    pub fn update_current(&mut self, translation: Vec3, rotation: Quat) {
        self.current_translation = translation;
        self.current_rotation = rotation;
    }
}
