use bevy::prelude::*;
use bevy_xpbd_2d::{
    math::{Scalar, Vector, PI},
    prelude::*,
    SubstepSchedule, SubstepSet,
};

use crate::GRAVITY_VECTOR;

#[derive(Component, Reflect)]
#[component(storage = "SparseSet")]
pub struct Grounded;

#[derive(Component, Reflect)]
pub struct JumpImpulse(Scalar);

impl Default for JumpImpulse {
    fn default() -> Self {
        JumpImpulse(7.)
    }
}

#[derive(Component, Reflect)]
pub struct MaxJumpDuration(Scalar);

impl Default for MaxJumpDuration {
    fn default() -> Self {
        MaxJumpDuration(0.1)
    }
}

#[derive(Component, Reflect)]
pub struct MovementAcceleration(Scalar);

impl Default for MovementAcceleration {
    fn default() -> Self {
        MovementAcceleration(75.)
    }
}

#[derive(Component, Reflect)]
pub struct MovementDampingFactor(Scalar);

impl Default for MovementDampingFactor {
    fn default() -> Self {
        MovementDampingFactor(0.9)
    }
}

#[derive(Component, Reflect)]
pub struct MaxSlopeAngle(Scalar);

impl Default for MaxSlopeAngle {
    fn default() -> Self {
        MaxSlopeAngle(PI * 0.45)
    }
}

#[derive(Component, Default, Reflect)]
pub struct CharacterController;

#[derive(Component, Reflect)]
pub struct ControllerGravity(Vector);

impl Default for ControllerGravity {
    fn default() -> Self {
        ControllerGravity(GRAVITY_VECTOR)
    }
}

#[derive(Bundle, Default)]
pub struct MovementBundle {
    acceleration: MovementAcceleration,
    damping: MovementDampingFactor,
    jump_impulse: JumpImpulse,
    max_slope_angle: MaxSlopeAngle,
    direction: MovementDirection,
}

#[derive(Bundle)]
pub struct CharacterControllerBundle {
    character_controller: CharacterController,
    rigidbody: RigidBody,
    collider: Collider,
    ground_caster: ShapeCaster,
    gravity: ControllerGravity,
    movement: MovementBundle,
    locked_axes: LockedAxes,
}

impl Default for CharacterControllerBundle {
    fn default() -> Self {
        let collider = Collider::rectangle(12., 24.);
        let mut caster_shape = collider.clone();
        caster_shape.set_scale(Vector::ONE * 0.5, 1);

        Self {
            locked_axes: LockedAxes::ROTATION_LOCKED,
            character_controller: CharacterController,
            rigidbody: RigidBody::Kinematic,
            collider,
            ground_caster: ShapeCaster::new(caster_shape, Vector::ZERO, 0.0, Direction2d::NEG_Y)
                .with_max_time_of_impact(1.0),
            gravity: ControllerGravity(GRAVITY_VECTOR * 5.),
            movement: MovementBundle {
                acceleration: MovementAcceleration(420.),
                jump_impulse: JumpImpulse(46.),
                ..default()
            },
        }
    }
}

#[derive(Component, Reflect)]
#[component(storage = "SparseSet")]
pub struct Jumping;

#[derive(Component, Default, Reflect)]
pub struct MovementDirection(pub Scalar);

pub struct CharacterControllerPlugin;
impl Plugin for CharacterControllerPlugin {
    fn build(&self, app: &mut App) {
        app.register_type::<Grounded>()
            .register_type::<MovementAcceleration>()
            .register_type::<MovementDampingFactor>()
            .register_type::<JumpImpulse>()
            .register_type::<MaxSlopeAngle>()
            .register_type::<MovementDirection>()
            .register_type::<CharacterController>()
            .register_type::<ControllerGravity>()
            .add_systems(
                Update,
                (
                    update_grounded,
                    apply_gravity,
                    handle_movement,
                    handle_jump,
                    apply_movement_damping,
                )
                    .chain(),
            )
            .add_systems(
                // Run collision handling in substep schedule
                SubstepSchedule,
                kinematic_controller_collisions.in_set(SubstepSet::SolveUserConstraints),
            );
    }
}

fn handle_movement(
    time: Res<Time>,
    mut query: Query<
        (
            &mut LinearVelocity,
            &MovementAcceleration,
            &MovementDirection,
        ),
        With<CharacterController>,
    >,
) {
    let delta = time.delta_seconds();
    for (mut velocity, MovementAcceleration(acceleration), MovementDirection(direction)) in
        query.iter_mut()
    {
        velocity.x += direction * acceleration * delta;
    }
}

fn handle_jump(
    mut query: Query<
        (&mut LinearVelocity, &JumpImpulse),
        (With<CharacterController>, With<Jumping>, With<Grounded>),
    >,
) {
    for (mut velocity, JumpImpulse(impulse)) in query.iter_mut() {
        velocity.y = *impulse;
    }
}

fn apply_movement_damping(mut query: Query<(&MovementDampingFactor, &mut LinearVelocity)>) {
    for (damping_factor, mut linear_velocity) in &mut query {
        // We could use `LinearDamping`, but we don't want to dampen movement along the Y axis
        linear_velocity.x *= damping_factor.0;
    }
}

fn update_grounded(
    mut commands: Commands,
    mut query: Query<
        (Entity, &ShapeHits, &Rotation, Option<&MaxSlopeAngle>),
        With<CharacterController>,
    >,
) {
    for (entity, hits, rotation, max_slope_angle) in &mut query {
        // The character is grounded if the shape caster has a hit with a normal
        // that isn't too steep.
        let is_grounded = hits.iter().any(|hit| {
            if let Some(angle) = max_slope_angle {
                rotation.rotate(-hit.normal2).angle_between(Vector::Y).abs() <= angle.0
            } else {
                true
            }
        });

        if is_grounded {
            commands.entity(entity).insert(Grounded);
        } else {
            commands.entity(entity).remove::<Grounded>();
        }
    }
}

fn apply_gravity(time: Res<Time>, mut query: Query<(&mut LinearVelocity, &ControllerGravity)>) {
    let delta = time.delta_seconds();
    for (mut velocity, ControllerGravity(gravity)) in query.iter_mut() {
        velocity.0 += *gravity * delta;
    }
}

#[allow(clippy::type_complexity)]
fn kinematic_controller_collisions(
    collisions: Res<Collisions>,
    collider_parents: Query<&ColliderParent, Without<Sensor>>,
    mut character_controllers: Query<
        (
            &RigidBody,
            &mut Position,
            &Rotation,
            &mut LinearVelocity,
            Option<&MaxSlopeAngle>,
        ),
        With<CharacterController>,
    >,
) {
    // Iterate through collisions and move the kinematic body to resolve penetration
    for contacts in collisions.iter() {
        // If the collision didn't happen during this substep, skip the collision
        if !contacts.during_current_substep {
            continue;
        }

        // Get the rigid body entities of the colliders (colliders could be children)
        let Ok([collider_parent1, collider_parent2]) =
            collider_parents.get_many([contacts.entity1, contacts.entity2])
        else {
            continue;
        };

        // Get the body of the character controller and whether it is the first
        // or second entity in the collision.
        let is_first: bool;
        let (rb, mut position, rotation, mut linear_velocity, max_slope_angle) =
            if let Ok(character) = character_controllers.get_mut(collider_parent1.get()) {
                is_first = true;
                character
            } else if let Ok(character) = character_controllers.get_mut(collider_parent2.get()) {
                is_first = false;
                character
            } else {
                continue;
            };

        // This system only handles collision response for kinematic character controllers
        if !rb.is_kinematic() {
            continue;
        }

        // Iterate through contact manifolds and their contacts.
        // Each contact in a single manifold shares the same contact normal.
        for manifold in contacts.manifolds.iter() {
            let normal = if is_first {
                -manifold.global_normal1(rotation)
            } else {
                -manifold.global_normal2(rotation)
            };

            // Solve each penetrating contact in the manifold
            for contact in manifold.contacts.iter().filter(|c| c.penetration > 0.0) {
                position.0 += normal * contact.penetration;
            }

            // If the slope isn't too steep to walk on but the character
            // is falling, reset vertical velocity.
            if max_slope_angle.is_some_and(|angle| normal.angle_between(Vector::Y).abs() <= angle.0)
                && linear_velocity.y < 0.0
            {
                linear_velocity.y = linear_velocity.y.max(0.0);
            }
        }
    }
}