use std::time::Instant;

use cgmath::{Euler, Quaternion};
use legion::world::SubWorld;
use legion::IntoQuery;
use legion::*;
use nalgebra::Quaternion as naQuaternion;
use rapier3d::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
use rapier3d::geometry::{BroadPhase, ColliderSet, NarrowPhase};
use rapier3d::pipeline::{PhysicsPipeline, ChannelEventCollector};

use crate::camera::{Camera, CameraController};
use crate::components::{Collider, LoopState, Mesh, Physics, Position};
use imgui::FontSource;
use crate::physics::state::PhysicsState;


#[system]
#[write_component(Collider)]
#[write_component(Physics)]
#[write_component(Mesh)]
pub fn run_physics(
    world: &mut SubWorld,
    #[resource] physics_state: &mut PhysicsState,
    #[resource] physics_pipeline: &mut PhysicsPipeline,
) {
    // Make sure all the entities we care about are added to the system
    let mut query = <(&mut Collider, &mut Physics, &mut Mesh)>::query();
    for (collider, physics, mesh) in query.iter_mut(world) {
        let rigid_body_handle = match physics.rigid_body_handle {
            None => {
                let handle = physics_state.bodies.insert(physics.rigid_body.clone());
                physics.rigid_body_handle = Some(handle);
                physics.rigid_body_handle.unwrap()
            }
            Some(handle) => handle,
        };
        if collider.collider_handle == None {
            let handle = physics_state.colliders.insert(
                collider.collider.clone(),
                rigid_body_handle,
                &mut physics_state.bodies,
            );
            collider.collider_handle = Some(handle);
        }
    }

    let (contact_send, contact_recv) = crossbeam::channel::unbounded();
    let (intersection_send, intersection_recv) = crossbeam::channel::unbounded();
    let event_handler = ChannelEventCollector::new(intersection_send, contact_send);


    // run the physics step
    physics_pipeline.step(
        &physics_state.gravity,
        &physics_state.integration_parameters,
        &mut physics_state.broad_phase,
        &mut physics_state.narrow_phase,
        &mut physics_state.bodies,
        &mut physics_state.colliders,
        &mut physics_state.joints,
        None,
        None,
        &event_handler,
    );

    while let Ok(intersection_event) = intersection_recv.try_recv() {
        println!("{:?}", intersection_event)
    }

    while let Ok(contact_event) = contact_recv.try_recv() {
        println!("{:?}", contact_event)
    }
}


#[system]
#[write_component(Camera)]
#[write_component(CameraController)]
pub fn update_camera(world: &mut SubWorld, #[resource] loop_state: &mut LoopState) {
    let mut query = <(&mut Camera, &mut CameraController)>::query();
    for (mut camera, controller) in query.iter_mut(world) {
        controller.update_camera(&mut camera, loop_state.step_size)
    }
}

#[system]
#[write_component(Collider)]
#[write_component(Physics)]
#[write_component(Mesh)]
#[write_component(Position)]
pub fn update_models(
    world: &mut SubWorld,
    #[resource] physics_state: &mut PhysicsState,
    #[resource] physics_pipeline: &mut PhysicsPipeline,
) {
    // Make sure all the entities we care about are added to the system
    let mut query = <(&mut Collider, &mut Physics, &mut Mesh, &mut Position)>::query();
    for (collider, physics, mesh, position) in query.iter_mut(world) {
        let pos = physics_state
            .bodies
            .get(physics.rigid_body_handle.unwrap())
            .unwrap()
            .position();
        position.x = pos.translation.x;
        position.y = pos.translation.y;
        position.z = pos.translation.z;

        let q = Quaternion::new(
            pos.rotation.w,
            pos.rotation.i,
            pos.rotation.j,
            pos.rotation.k,
        );
        let rotation = Euler::from(q);
        position.rot.x = rotation.x.into();
        position.rot.y = rotation.y.into();
        position.rot.z = rotation.z.into();
    }
}