/*
Trouble in Terror Town: Community Edition
Copyright (C) 2024 Mikolaj Wojciech Gorski

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License 3.0 as published by
the Free Software Foundation.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU General Public License 3.0 for more details.

You should have received a copy of the GNU General Public License 3.0
along with this program.  If not, see <https://www.gnu.org/licenses/gpl-3.0.html>.
*/

use crate::player_utils::{input_processor::*, player_look::*};
use bevy::{
    prelude::*,
    math::Vec3,
    time::Time,
    ecs::{
        event::EventReader,
        system::*,
        query::*,
    },
    transform::components::Transform,
    core_pipeline::core_3d::Camera3d,
    gizmos::gizmos::Gizmos,
    
};
use bevy_xpbd_3d::{math::*, prelude::*, SubstepSchedule, SubstepSet};

pub struct CharacterControllerPlugin;

impl Plugin for CharacterControllerPlugin {
    fn build(&self, app: &mut App) {
        app.add_systems(Update, move_character);
        app.add_systems(FixedUpdate, (apply_gravity, update_grounded, dampen_movement));
        app.add_systems(SubstepSchedule,  collide_and_slide.in_set(SubstepSet::SolveUserConstraints));
    }
}

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

#[derive(Component)]
pub struct CharacterController {
    pub movement_acceleration: Scalar,
    pub air_control_factor: Scalar,
    pub movement_dampening_factor: Scalar,
    pub jump_impulse: Scalar,
    /// direction in which gravity pulls the character look at [`new_fall_velocity`]
    pub gravity: Vector,
    pub max_slope_angle: Scalar,
    pub axis_speed_limit: f32,
    pub max_acceleration: f32,
    /// value used for holding current calculated friction, do not change on spawn
    pub friction: f32,
}

impl Default for CharacterController {
    fn default() -> Self {
        CharacterController {
            movement_acceleration: 20.0,
            air_control_factor: 0.5,
            movement_dampening_factor: 0.95,
            jump_impulse: 6.0,
            gravity: Vector::NEG_Y * 9.81 * 2.0,
            max_slope_angle: (45.0 as Scalar).to_radians(),
            axis_speed_limit: 6668.5,
            max_acceleration: 857.25,
            friction: 0.0,

        }
    }
}

#[derive(Bundle)]
pub struct CharacterControllerBundle {
    character_controller: CharacterController,
    rigid_body: RigidBody,
    collider: Collider,
    ground_caster: ShapeCaster,
    restitution: Restitution,
    player_look_rotatable: PlayerLookRotatable,
}

impl CharacterControllerBundle {
    pub fn new(controller_collider: Collider, controller: CharacterController) -> Self {
        let mut caster_shape = controller_collider.clone();
        caster_shape.set_scale(Vector::ONE * 0.99, 10);

        Self {
            character_controller: controller,
            rigid_body: RigidBody::Kinematic,
            collider: controller_collider,
            ground_caster: ShapeCaster::new(
                caster_shape,
                Vector::ZERO,
                Quaternion::default(),
                Direction3d::NEG_Y,
            )
            .with_max_time_of_impact(0.2),
            restitution: Restitution::new(0.0).with_combine_rule(CoefficientCombine::Min),
            player_look_rotatable: PlayerLookRotatable,
        }
    }
}

impl Default for CharacterControllerBundle {
    fn default() -> Self {
        let collider = Collider::capsule(1.0, 0.5);
        let mut caster_shape = collider.clone();
        caster_shape.set_scale(Vector::ONE * 0.99, 10);

        CharacterControllerBundle::new(collider, CharacterController::default())
    }
}

fn get_lateral_acceleration(
    controller: &CharacterController,
    delta_time: f32,
    velocity: Vec3,
) -> Vec3 {
    // No acceleration in Y
    let fall_acceleration = Vec3::new(
        controller.gravity.x,
        0.0,
        controller.gravity.z,
    );

    // Bound acceleration, falling object has minimal ability to impact acceleration
    let mut acceleration = fall_acceleration;
    if velocity.length_squared() > 0.0 {
        acceleration *= controller.air_control_factor;
        acceleration = acceleration.clamp_length(0.0, controller.movement_acceleration);
    }

    acceleration
}


/// project a point on a plane
fn point_plane_project(point: Vec3, plane_base: Vec3, plane_normal: Vec3) -> Vec3 {
    point - plane_normal * plane_normal.dot(point - plane_base)
}

/// calculating gravity, later you'll have to pass custom terminal velocity
fn new_fall_velocity(initial_velocity: LinearVelocity, gravity: Vec3, delta_time: f32, speed_limit: f32)
-> Vec3 {

	let mut result: Vec3 = Vec3 { x: initial_velocity.x, y: initial_velocity.y, z: initial_velocity.z };
    let terminal_velocity: f32 = 50.0;

    // Apply gravity.
    result += gravity * delta_time;

    // Don't exceed terminal velocity.
    let terminal_limit: f32 = f32::abs(terminal_velocity); //FMath::Abs(GetPhysicsVolume()->TerminalVelocity);
    if result.length_squared() > (terminal_limit * terminal_limit)
    {
        let gravity_dir: Vec3 = gravity.normalize();

        if result.y > terminal_limit || gravity_dir.y > terminal_limit
        {
            result = point_plane_project(result, Vec3::ZERO, gravity_dir) + gravity_dir * terminal_limit;
        }
    }

    result.z = f32::clamp(result.z, -speed_limit, speed_limit);

	return result;
}

<<<<<<< HEAD
// fn move_character(
//     time: Res<Time>,
//     mut movement_event_reader: EventReader<MovementAction>,
//     mut query: Query<(
//         &mut LinearVelocity,
//         &CharacterController,
//         Has<Grounded>,
//         &Rotation,
//         &ShapeHits,
//         &Transform,
//     )>,
// ) {
//     let delta_time = time.delta_seconds_f64().adjust_precision();

//     for event in movement_event_reader.read() {
//         for (
//             mut velocity,
//             character_controller,
//             is_grounded,
//             rotation,
//             shape_hits,
//             character_transform,
//         ) in &mut query
//         {
//             character_controller.friction = f32::max(0.0f, character_controller.friction);
//             let forward = character_transform.forward().xyz().normalize();

//             match event {
//                 MovementAction::Move(direction) => {
//                     if is_grounded {
//                         velocity.x = f32::clamp(velocity.x, -character_controller.axis_speed_limit, character_controller.axis_speed_limit);
//                         velocity.y = f32::clamp(velocity.y, -character_controller.axis_speed_limit, character_controller.axis_speed_limit);
//                     } else {
                        
//                     }
//                 }
//                 MovementAction::Jump => {
//                     if is_grounded {

//                     }
//                 }
//             }
//         }
//     }
// }


fn apply_gravity(
    time: Res<Time>,
    mut controllers: Query<(&CharacterController, &mut LinearVelocity)>,
) {
    let delta_time = time.delta_seconds_f64().adjust_precision();

    for (character_controller, mut linear_velocity) in &mut controllers {
        let new_velocity = new_fall_velocity(
            *linear_velocity,
            character_controller.gravity,
            delta_time,
            character_controller.axis_speed_limit,
        );
        linear_velocity.0 = new_velocity;
    }
}

fn update_grounded(
    mut commands: Commands,
    mut query: Query<(Entity, &CharacterController, &Rotation, &ShapeHits)>,
) {
    for (entity, character_controller, rotation, hits) in &mut query {
        let is_grounded = hits.iter().any(|hit| {
            let angle = rotation.rotate(-hit.normal2).angle_between(Vector::Y).abs();
            angle <= character_controller.max_slope_angle
        });

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

#[allow(clippy::type_complexity)]
fn collide_and_slide(
    collisions: Res<Collisions>,
    collider_parents: Query<&ColliderParent, Without<Sensor>>,
    mut character_controllers: Query<
        (
            &RigidBody,
            &mut Position,
            &Rotation,
            &mut LinearVelocity,
            &CharacterController,
        )
    >,
) {
    for contacts in collisions.iter() {
        if !contacts.during_current_substep {
            continue;
        }

        let Ok([collider_parent1, collider_parent2]) =
            collider_parents.get_many([contacts.entity1, contacts.entity2])
        else {
            continue;
        };

        let is_first: bool;
        let (rb, mut position, rotation, mut linear_velocity, character_controller) =
            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() {
            // Determine the normal of the collision based on which entity the character is
            let normal = if is_first {
                -manifold.global_normal1(rotation)
            } else {
                -manifold.global_normal2(rotation)
            };

            // Resolve penetration by moving the position out of the collision
            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 normal.angle_between(Vector::Y).abs() <= character_controller.max_slope_angle
                && linear_velocity.y < 0.0
            {
                // Stop downward velocity to prevent sliding down walkable slopes
                linear_velocity.y = linear_velocity.y.max(0.0);
            }

        }
    }
}


fn move_character(
    time: Res<Time>,
    mut movement_event_reader: EventReader<MovementAction>,
    mut query: Query<(
        &mut LinearVelocity,
        &CharacterController,
        Has<Grounded>,
        &Rotation,
        &ShapeHits,
        &Transform,
    )>,
) {
    let delta_time = time.delta_seconds_f64().adjust_precision();

    for event in movement_event_reader.read() {
        for (
            mut velocity,
            character_controller,
            is_grounded,
            rotation,
            shape_hits,
            character_transform,
        ) in &mut query
        {
            match event {
                MovementAction::Move(direction) => {
                    // Get the forward direction of the character in the local coordinate system
                    let forward = character_transform.forward().xyz().normalize();

                    // Calculate the movement in the local coordinate system
                    let local_movement = forward
                        * direction.y
                        * character_controller.movement_acceleration
                        * delta_time
                        + character_transform
                            .rotation
                            .mul_vec3(Vec3::new(direction.x, 0.0, 0.0))
                            * character_controller.movement_acceleration
                            * delta_time;

                    if is_grounded {
                        let mut slope_factor = 1.0;
                        for hit in shape_hits.iter() {
                            let angle =
                                rotation.rotate(-hit.normal2).angle_between(Vector::Y).abs();
                            if angle <= character_controller.max_slope_angle {
                                slope_factor *=
                                    (angle / character_controller.max_slope_angle).cos();

                                // Update linear velocity
                                velocity.x += local_movement.x * slope_factor;
                                velocity.z += local_movement.z * slope_factor;
                            } else {
                                velocity.x = 0.0;
                                velocity.z = 0.0;
                            }
                        }
                    } else {
                        velocity.x += local_movement.x * character_controller.air_control_factor;
                        velocity.z += local_movement.z * character_controller.air_control_factor;
                    }
                }
                MovementAction::Jump => {
                    if is_grounded {
                        velocity.y = character_controller.jump_impulse;
                    }
                }
=======
fn apply_gravity(
    time: Res<Time>,
    mut controllers: Query<(&CharacterController, &mut LinearVelocity)>,
) {
    // Precision is adjusted so that the example works with
    // both the `f32` and `f64` features. Otherwise you don't need this.
    let delta_time = time.delta_seconds_f64().adjust_precision();

    for (character_controller, mut linear_velocity) in &mut controllers {        

        let mut fall_acceleration: Vec3 = get_lateral_acceleration(character_controller, delta_time, linear_velocity.0);
        fall_acceleration.y = 0.0;
        let has_limited_air_control: bool = false;
        

        // Ugly fix for mismatch types, find a proper solution later
        let new_velocity: Vec3 = new_fall_velocity(linear_velocity.clone(), character_controller.gravity, delta_time, character_controller.axis_speed_limit);
        linear_velocity.x = new_velocity.x;
        linear_velocity.y = new_velocity.y;
        linear_velocity.z = new_velocity.z;
    }
}

fn move_character(
    time: Res<Time>,
    mut movement_event_reader: EventReader<MovementAction>,
    mut query: Query<(
        &mut LinearVelocity,
        &CharacterController,
        Has<Grounded>,
        &Rotation,
        &ShapeHits,
        &Transform,
    )>,
) {
    let delta_time = time.delta_seconds_f64().adjust_precision();

    for event in movement_event_reader.read() {
        for (
            mut velocity,
            character_controller,
            is_grounded,
            rotation,
            shape_hits,
            character_transform,
        ) in &mut query
        {
            //character_controller.friction = f32::max(0.0, character_controller.friction);
            let forward = character_transform.forward().xyz().normalize();

            match event {
                MovementAction::Move(direction) => {
                    if is_grounded {
                        velocity.x = f32::clamp(velocity.x, -character_controller.axis_speed_limit, character_controller.axis_speed_limit);
                        velocity.y = f32::clamp(velocity.y, -character_controller.axis_speed_limit, character_controller.axis_speed_limit);
                    } else {
                        
                    }
                }
                MovementAction::Jump => {
                    if is_grounded {

                    }
                }
            }
        }
    }
}

fn move_character(
    time: Res<Time>,
    mut movement_event_reader: EventReader<MovementAction>,
    mut query: Query<(
        &mut LinearVelocity,
        &CharacterController,
        Has<Grounded>,
        &Rotation,
        &ShapeHits,
        &Transform,
    )>,
) {
    let delta_time = time.delta_seconds_f64().adjust_precision();

    for event in movement_event_reader.read() {
        for (
            mut velocity,
            character_controller,
            is_grounded,
            rotation,
            shape_hits,
            character_transform,
        ) in &mut query
        {
            match event {
                MovementAction::Move(direction) => {
                    // Get the forward direction of the character in the local coordinate system
                    let forward = character_transform.forward().xyz().normalize();

                    // Calculate the movement in the local coordinate system
                    let local_movement = forward
                        * direction.y
                        * character_controller.movement_acceleration
                        * delta_time
                        + character_transform
                            .rotation
                            .mul_vec3(Vec3::new(direction.x, 0.0, 0.0))
                            * character_controller.movement_acceleration
                            * delta_time;

                    if is_grounded {
                        let mut slope_factor = 1.0;
                        for hit in shape_hits.iter() {
                            let angle =
                                rotation.rotate(-hit.normal2).angle_between(Vector::Y).abs();
                            if angle <= character_controller.max_slope_angle {
                                slope_factor *=
                                    (angle / character_controller.max_slope_angle).cos();

                                // Update linear velocity
                                velocity.x += local_movement.x * slope_factor;
                                velocity.z += local_movement.z * slope_factor;
                            } else {
                                velocity.x = 0.0;
                                velocity.z = 0.0;
                            }
                        }
                    } else {
                        velocity.x += local_movement.x * character_controller.air_control_factor;
                        velocity.z += local_movement.z * character_controller.air_control_factor;
                    }
                }
                MovementAction::Jump => {
                    if is_grounded {
                        velocity.y = character_controller.jump_impulse;
                    }
                }
            }
        }
    }
}

fn update_grounded(
    mut commands: Commands,
    mut query: Query<(Entity, &CharacterController, &Rotation, &ShapeHits)>,
) {
    for (entity, character_controller, rotation, hits) in &mut query {
        let is_grounded = hits.iter().any(|hit| {
            let angle = rotation.rotate(-hit.normal2).angle_between(Vector::Y).abs();

            println!(
                "slope angle: {} / {}",
                angle, character_controller.max_slope_angle
            );

            angle <= character_controller.max_slope_angle
        });

        //println!("is grounded: {}", is_grounded);

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

#[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,
            &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, character_controller) =
            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 normal.angle_between(Vector::Y).abs() <= character_controller.max_slope_angle
                && linear_velocity.y < 0.0
            {
                linear_velocity.y = linear_velocity.y.max(0.0);
>>>>>>> branch 'dev-kinematic' of ssh://git@129.151.217.187:23/MikolajG/ttt-ce.git
            }
        }
    }
}

fn dampen_movement(mut qeury: Query<(&mut LinearVelocity, &CharacterController)>) {
    for (mut velocity, character_controller) in &mut qeury {
        // We could use `LinearDamping`, but we don't want to dampen movement along the Y axis
        velocity.x *= character_controller.movement_dampening_factor;
        velocity.z *= character_controller.movement_dampening_factor;
    }
}

pub fn spawn_player(
    mut commands: Commands,
    mut meshes: ResMut<Assets<Mesh>>,
    mut materials: ResMut<Assets<StandardMaterial>>,
    camera_hight: f32,
    character_bundle: CharacterControllerBundle,
) {
    commands
        .spawn((
            character_bundle,
            PbrBundle {
                mesh: meshes.add(Capsule3d::new(0.5, 1.0)),
                material: materials.add(Color::rgb_u8(124, 144, 255)),
                transform: Transform::from_xyz(0.0, 5.0, 0.0),
                ..default()
            },
        ))
        .with_children(|parent| {
            parent.spawn((
                PlayerCamera::default(),
                Camera3dBundle {
                    projection: PerspectiveProjection {
                        // We must specify the FOV in radians.
                        // Rust can convert degrees to radians for us.
                        fov: 90.0_f32.to_radians(),
                        ..default()
                    }.into(),
                    transform: Transform::from_xyz(0., camera_hight, 0.),
                    ..default()
                },
            ));
        });
}
