use glam::{Quat, Vec3};
use rapier3d::prelude::{nalgebra, ColliderBuilder, QueryFilter, RigidBodyBuilder, RigidBodyType};
use rapier3d::prelude::vector;
use rapier3d::control::{CharacterCollision, KinematicCharacterController as RapierKinematicCharacterController};
use rapier3d::na::{Isometry3, Vector3};
use winit::event::DeviceEvent::MouseMotion;
use winit::event::Event;
use winit::keyboard::KeyCode;
use raidillon_app::prelude::*;

use crate::systems::common::{camera_front, is_camera_mode_valid, is_mouse_look_enabled};
use crate::systems::menu::MenuState;

#[derive(Default)]
pub struct KinematicCharacterController {
    character_controller: RapierKinematicCharacterController,
    character_collider: ColliderBuilder,

    desired_movement: Vec3,
    last_position: Vector3<f32>,
    yaw:   f32,
    pitch: f32,
    speed: f32,
    sensitivity: f32,
    mouse_delta: (f64, f64),

    vertical_velocity: f32,
    gravity: f32,
    max_fall_speed: f32,
}

impl System for KinematicCharacterController {
    fn load_world(&mut self, res: &mut EngineResources, scene: &mut Scene) {
        // create the rigid body, add it to the body set
        let p = scene.resources.get_mut::<Physics>().expect("Physics missing");
        let rb = RigidBodyBuilder::kinematic_position_based().build();
        let rb_handle = p.rigid_body_set.insert(rb);
        self.character_collider = ColliderBuilder::capsule_y(1.0, 1.0);
        p.collider_set.insert_with_parent(self.character_collider.build(), rb_handle, &mut p.rigid_body_set);
        let tr = Transform {
            translation: Vec3::new(0.0, 2.0, 3.0),
            rotation: Quat::IDENTITY,
            scale: Vec3::new(1.0, 1.0, 1.0),
        };
        self.last_position = vector![
            tr.translation.x,
            tr.translation.y,
            tr.translation.z,
        ];
        scene.world.spawn((
            tr,
            CharacterBodyComponent(rb_handle),
        ));

        self.speed = 5.0;
        self.sensitivity = 0.05;
        self.gravity = -9.81;
        self.max_fall_speed = -50.0;
        self.vertical_velocity = 0.0;
    }

    fn handle_event(&mut self, res: &mut EngineResources, scene: &mut Scene) {
        if !is_camera_mode_valid(scene, CameraMode::Kinematic) {
            return
        }

        let pctx = res.get::<PlatformContext>().unwrap();
        let event2 = pctx.current_event.clone();
        match event2 {
            Event::DeviceEvent { device_id, event } => {
                match event {
                    MouseMotion { delta } => {
                        self.mouse_delta.0 += delta.0;
                        self.mouse_delta.1 += delta.1;
                    },
                    _ => {}
                }
            },
            _ => {},
        }
    }

    fn frame_update(&mut self, res: &mut EngineResources, scene: &mut Scene) {
        let front = camera_front(self.yaw, self.pitch);
        let right_vec = front.cross(Vec3::Y).normalize();

        if is_camera_mode_valid(scene, CameraMode::Kinematic) && is_mouse_look_enabled(scene) {
            let (pctx, input) = res.get_many::<(PlatformContext, InputState)>().unwrap();

            self.yaw   += (self.mouse_delta.0 as f32) * self.sensitivity;
            self.pitch -= (self.mouse_delta.1 as f32) * self.sensitivity;
            self.pitch = self.pitch.clamp(-89.0, 89.0);

            if input.key_held(KeyCode::KeyW) {
                self.desired_movement += front * pctx.time_ctx.frame_dt * self.speed;
            }
            if input.key_held(KeyCode::KeyS) {
                self.desired_movement -= front * pctx.time_ctx.frame_dt * self.speed;
            }
            if input.key_held(KeyCode::KeyA) {
                self.desired_movement -= right_vec * pctx.time_ctx.frame_dt * self.speed;
            }
            if input.key_held(KeyCode::KeyD) {
                self.desired_movement += right_vec * pctx.time_ctx.frame_dt * self.speed;
            }
        }

        if is_camera_mode_valid(scene, CameraMode::Kinematic) {
            let pos = Physics::rapier_translation_to_glam(&self.last_position);

            scene.world.query_mut::<&mut Camera>().into_iter().for_each(|(_, camera)| {
                // INTERPOLATION NEEDED.
                camera.eye = pos;
                camera.center = pos + front;
            });
        }

        self.mouse_delta = (0.0, 0.0);
    }

    fn fixed_update(&mut self, res: &mut EngineResources, scene: &mut Scene) {
        let p = scene.resources.get_mut::<Physics>().unwrap();
        let (pctx, input) = res.get_many::<(PlatformContext, InputState)>().unwrap();

        let (ch_ent, (ch_tr, ch_component)) = scene
            .world
            .query_mut::<(&mut Transform, &mut CharacterBodyComponent)>()
            .into_iter()
            .next()
            .expect("no character entity in world");

        let query_pipeline = p.broad_phase.as_query_pipeline(
            p.narrow_phase.query_dispatcher(),
            &p.rigid_body_set,
            &p.collider_set,
            QueryFilter::default().exclude_rigid_body(ch_component.0),
        );

        self.vertical_velocity = (self.vertical_velocity + self.gravity * pctx.time_ctx.fixed_dt)
            .max(self.max_fall_speed);
        let mut total_displacement = self.desired_movement;
        total_displacement.y += self.vertical_velocity * pctx.time_ctx.fixed_dt;

        let mut collisions: Vec<CharacterCollision> = Vec::new();
        let corrected_movement = self.character_controller.move_shape(
            pctx.time_ctx.fixed_dt,
            &query_pipeline,
            &*self.character_collider.shape,
            &Isometry3::from(self.last_position),
            vector![total_displacement.x, total_displacement.y, total_displacement.z],
            |collision| collisions.push(collision),
        );

        // update character rigid body with the new translation.
        if let Some(body) = p.get_rigid_body_mut(ch_component.0) {
            self.last_position = vector![
                self.last_position.x + corrected_movement.translation.x,
                self.last_position.y + corrected_movement.translation.y,
                self.last_position.z + corrected_movement.translation.z,
            ];
            body.set_next_kinematic_position(Isometry3::from(self.last_position));
            ch_tr.translation = Physics::rapier_translation_to_glam(&self.last_position);
            // reset vertical velocity if grounded
            if corrected_movement.grounded {
                self.vertical_velocity = 0.0;
            }
        }

        // apply impulses to dynamic bodies the character collided with
        let character_push_force = 50.0;
        for collision in collisions {
            if let Some(collider) = p.collider_set.get(collision.handle) {
                if let Some(rb_handle) = collider.parent() {
                    if let Some(rb) = p.rigid_body_set.get_mut(rb_handle) {
                        if rb.body_type() == RigidBodyType::Dynamic {
                            let push_direction = -collision.hit.normal1.into_inner();
                            let impulse = push_direction * character_push_force * pctx.time_ctx.fixed_dt;
                            rb.apply_impulse(impulse, true);
                        }
                    }
                }
            }
        }

        self.desired_movement = Vec3::ZERO;
    }
}