use legion::world::SubWorld;
use legion::IntoQuery;
use rapier3d::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
use rapier3d::geometry::{BroadPhase, ColliderSet, NarrowPhase};
use rapier3d::pipeline::PhysicsPipeline;
use legion::*;

use crate::render::{EntityUniforms, Renderer};
use cgmath::Quaternion;
use crate::components::{Collider, Physics, Mesh, Position};
use crate::camera::{CameraController, Camera};
use std::time::Instant;

pub struct PhysicsState {
    gravity: rapier3d::math::Vector<f32>,
    integration_parameters: IntegrationParameters,
    broad_phase: BroadPhase,
    narrow_phase: NarrowPhase,
    bodies: RigidBodySet,
    colliders: ColliderSet,
    joints: JointSet,
}

impl PhysicsState {
    pub fn build(gravity: rapier3d::math::Vector<f32>) -> (PhysicsPipeline, PhysicsState) {
        (
            PhysicsPipeline::new(),
            PhysicsState {
                gravity,
                integration_parameters: IntegrationParameters::default(),
                broad_phase: BroadPhase::new(),
                narrow_phase: NarrowPhase::new(),
                bodies: RigidBodySet::new(),
                colliders: ColliderSet::new(),
                joints: JointSet::new(),
            },
        )
    }
}

#[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);
        }
    }

    // run the physics step
    let event_handler = ();
    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
    );
}

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

}

#[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;

        position.rot.s   = pos.rotation.w;
        position.rot.v.x = pos.rotation.i;
        position.rot.v.y = pos.rotation.j;
        position.rot.v.z = pos.rotation.k;

        // mx.x = pos.rotation.i;
        // mx.y = pos.rotation.j;
        // mx.z = pos.rotation.k;
        // mx.w = pos.rotation.w;

    }

}