use bevy::prelude::{
    info, Color, Gizmos, GlobalTransform, Plugin, Quat, Query, Res, Transform, Update, Vec2, Vec3,
    With, Without,
};

use crate::{
    input::XrInput,
    resources::{XrFrameState, XrInstance, XrSession},
};

use crate::xr_input::{
    oculus_touch::{OculusController, OculusControllerRef},
    Hand,
};

use super::{
    handtracking::{HandTrackingRef, HandTrackingTracker},
    trackers::{OpenXRLeftController, OpenXRRightController, OpenXRTrackingRoot},
    QuatConv,
};

/// add debug renderer for controllers
#[derive(Default)]
pub struct OpenXrDebugRenderer;

impl Plugin for OpenXrDebugRenderer {
    fn build(&self, app: &mut bevy::prelude::App) {
        app.add_systems(Update, draw_gizmos);
    }
}

pub fn draw_gizmos(
    mut gizmos: Gizmos,
    oculus_controller: Res<OculusController>,
    frame_state: Res<XrFrameState>,
    xr_input: Res<XrInput>,
    instance: Res<XrInstance>,
    session: Res<XrSession>,
    tracking_root_query: Query<(
        &mut Transform,
        With<OpenXRTrackingRoot>,
        Without<OpenXRLeftController>,
        Without<OpenXRRightController>,
    )>,
    left_controller_query: Query<(
        &GlobalTransform,
        With<OpenXRLeftController>,
        Without<OpenXRRightController>,
        Without<OpenXRTrackingRoot>,
    )>,
    right_controller_query: Query<(
        &GlobalTransform,
        With<OpenXRRightController>,
        Without<OpenXRLeftController>,
        Without<OpenXRTrackingRoot>,
    )>,
    hand_tracking: Res<HandTrackingTracker>,
) {
    let handtracking_ref = hand_tracking.get_ref(&xr_input, &frame_state);
    if let Some(joints) = handtracking_ref.get_left_poses() {
        for joint in joints {
            let p = joint.pose.position;
            let r = joint.pose.orientation;
            let quat = r.to_quat();
            let trans = Transform::from_rotation(quat);
            gizmos.circle(
                (p.x, p.y, p.z).into(),
                trans.forward(),
                joint.radius,
                Color::ORANGE_RED,
            );
        }
    } else {
        info!("left_hand_poses returned None");
    }
    if let Some(joints) = handtracking_ref.get_right_poses() {
        for joint in joints {
            let p = joint.pose.position;
            let r = joint.pose.orientation;
            let quat = r.to_quat();
            let trans = Transform::from_rotation(quat);
            gizmos.circle(
                (p.x, p.y, p.z).into(),
                trans.forward(),
                joint.radius,
                Color::LIME_GREEN,
            );
        }
        return;
    }
    //lock frame
    let frame_state = *frame_state.lock().unwrap();
    //get controller
    let controller = oculus_controller.get_ref(&instance, &session, &frame_state, &xr_input);
    //tracking root?
    let mut tracking_transform = &Transform::IDENTITY;
    let root = tracking_root_query.get_single();
    match root {
        Ok(position) => {
            gizmos.circle(
                position.0.translation
                    + Vec3 {
                        x: 0.0,
                        y: 0.01,
                        z: 0.0,
                    },
                Vec3::Y,
                0.2,
                Color::RED,
            );
            tracking_transform = position.0;
        }
        Err(_) => info!("too many tracking roots"),
    }
    //draw the hands
    let left_transform = left_controller_query.get_single().unwrap().0;
    let right_transform = right_controller_query.get_single().unwrap().0;

    draw_hand_gizmo(&mut gizmos, &controller, Hand::Right, right_transform);
    draw_hand_gizmo(&mut gizmos, &controller, Hand::Left, left_transform);
}

fn draw_hand_gizmo(
    gizmos: &mut Gizmos,
    controller: &OculusControllerRef<'_>,
    hand: Hand,
    hand_transform: &GlobalTransform,
) {
    match hand {
        Hand::Left => {
            let left_color = Color::YELLOW_GREEN;
            let off_color = Color::BLUE;
            let touch_color = Color::GREEN;
            let pressed_color = Color::RED;

            let grip_quat_offset = Quat::from_rotation_x(-1.4);
            let face_quat_offset = Quat::from_rotation_x(1.05);
            let trans = hand_transform.compute_transform();
            let controller_vec3 = trans.translation;
            let controller_quat = trans.rotation;
            let face_quat = controller_quat.mul_quat(face_quat_offset);
            let face_quat_normal = face_quat.mul_vec3(Vec3::Z);

            //draw grip
            gizmos.rect(
                controller_vec3,
                controller_quat * grip_quat_offset,
                Vec2::new(0.05, 0.1),
                left_color,
            );

            let face_translation_offset = Quat::from_rotation_x(-1.7); //direction to move the face from the controller tracking point
            let face_translation_vec3 = controller_vec3
                + controller_quat
                    .mul_quat(face_translation_offset)
                    .mul_vec3(Vec3::Y * 0.075); //distance to move face by

            //draw face
            gizmos.circle(
                face_translation_vec3,
                face_quat_normal,
                0.04,
                Color::YELLOW_GREEN,
            );

            //button b
            let mut b_color = off_color;
            if controller.y_button_touched() {
                b_color = touch_color;
            }
            if controller.y_button() {
                b_color = pressed_color;
            }

            let b_offset_quat = face_quat;
            let b_translation_vec3 =
                face_translation_vec3 + b_offset_quat.mul_vec3(Vec3::new(0.025, -0.01, 0.0));
            gizmos.circle(b_translation_vec3, face_quat_normal, 0.0075, b_color);

            //button a
            let mut a_color = off_color;
            if controller.x_button_touched() {
                a_color = touch_color;
            }
            if controller.x_button() {
                a_color = pressed_color;
            }

            let a_offset_quat = face_quat;
            let a_translation_vec3 =
                face_translation_vec3 + a_offset_quat.mul_vec3(Vec3::new(0.025, 0.01, 0.0));
            gizmos.circle(a_translation_vec3, face_quat_normal, 0.0075, a_color);

            //joystick
            let joystick_offset_quat = face_quat;
            let joystick_base_vec =
                face_translation_vec3 + joystick_offset_quat.mul_vec3(Vec3::new(-0.02, 0.0, 0.0));
            let mut joystick_color = off_color;
            if controller.thumbstick_touch(Hand::Left) {
                joystick_color = touch_color;
            }

            //base
            gizmos.circle(joystick_base_vec, face_quat_normal, 0.014, joystick_color);

            let stick = controller.thumbstick(Hand::Left);
            let input = Vec3::new(stick.x, -stick.y, 0.0);
            let joystick_top_vec = face_translation_vec3
                + joystick_offset_quat.mul_vec3(Vec3::new(-0.02, 0.0, -0.01))
                + joystick_offset_quat.mul_vec3(input * 0.01);
            //top
            gizmos.circle(joystick_top_vec, face_quat_normal, 0.005, joystick_color);

            //trigger
            let trigger_state = controller.trigger(Hand::Left);
            let trigger_rotation = Quat::from_rotation_x(-0.75 * trigger_state);
            let mut trigger_color = off_color;
            if controller.trigger_touched(Hand::Left) {
                trigger_color = touch_color;
            }
            let trigger_transform = Transform {
                translation: face_translation_vec3
                    + face_quat
                        .mul_quat(trigger_rotation)
                        .mul_vec3(Vec3::new(0.0, 0.0, 0.02)),
                rotation: face_quat.mul_quat(trigger_rotation),
                scale: Vec3 {
                    x: 0.01,
                    y: 0.02,
                    z: 0.03,
                },
            };
            gizmos.cuboid(trigger_transform, trigger_color);
        }
        Hand::Right => {
            //get right controller
            let right_color = Color::YELLOW_GREEN;
            let off_color = Color::BLUE;
            let touch_color = Color::GREEN;
            let pressed_color = Color::RED;

            let grip_quat_offset = Quat::from_rotation_x(-1.4);
            let face_quat_offset = Quat::from_rotation_x(1.05);

            let trans = hand_transform.compute_transform();
            let controller_vec3 = trans.translation;
            let controller_quat = trans.rotation;
            let face_quat = controller_quat.mul_quat(face_quat_offset);
            let face_quat_normal = face_quat.mul_vec3(Vec3::Z);

            let _squeeze = controller.squeeze(Hand::Right);
            //info!("{:?}", squeeze);
            //grip
            gizmos.rect(
                controller_vec3,
                controller_quat * grip_quat_offset,
                Vec2::new(0.05, 0.1),
                right_color,
            );

            let face_translation_offset = Quat::from_rotation_x(-1.7); //direction to move the face from the controller tracking point
            let face_translation_vec3 = controller_vec3
                + controller_quat
                    .mul_quat(face_translation_offset)
                    .mul_vec3(Vec3::Y * 0.075); //distance to move face by

            //draw face
            gizmos.circle(
                face_translation_vec3,
                face_quat_normal,
                0.04,
                Color::YELLOW_GREEN,
            );

            //button b
            let mut b_color = off_color;
            if controller.b_button_touched() {
                b_color = touch_color;
            }
            if controller.b_button() {
                b_color = pressed_color;
            }

            let b_offset_quat = face_quat;
            let b_translation_vec3 =
                face_translation_vec3 + b_offset_quat.mul_vec3(Vec3::new(-0.025, -0.01, 0.0));
            gizmos.circle(b_translation_vec3, face_quat_normal, 0.0075, b_color);

            //button a
            let mut a_color = off_color;
            if controller.a_button_touched() {
                a_color = touch_color;
            }
            if controller.a_button() {
                a_color = pressed_color;
            }

            let a_offset_quat = face_quat;
            let a_translation_vec3 =
                face_translation_vec3 + a_offset_quat.mul_vec3(Vec3::new(-0.025, 0.01, 0.0));
            gizmos.circle(a_translation_vec3, face_quat_normal, 0.0075, a_color);

            //joystick time
            let joystick_offset_quat = face_quat;
            let joystick_base_vec =
                face_translation_vec3 + joystick_offset_quat.mul_vec3(Vec3::new(0.02, 0.0, 0.0));
            let mut joystick_color = off_color;
            if controller.thumbstick_touch(Hand::Right) {
                joystick_color = touch_color;
            }

            //base
            gizmos.circle(joystick_base_vec, face_quat_normal, 0.014, joystick_color);

            let stick = controller.thumbstick(Hand::Right);
            let input = Vec3::new(stick.x, -stick.y, 0.0);
            let joystick_top_vec = face_translation_vec3
                + joystick_offset_quat.mul_vec3(Vec3::new(0.02, 0.0, -0.01))
                + joystick_offset_quat.mul_vec3(input * 0.01);
            //top
            gizmos.circle(joystick_top_vec, face_quat_normal, 0.005, joystick_color);

            //trigger
            let trigger_state = controller.trigger(Hand::Right);
            let trigger_rotation = Quat::from_rotation_x(-0.75 * trigger_state);
            let mut trigger_color = off_color;
            if controller.trigger_touched(Hand::Right) {
                trigger_color = touch_color;
            }
            let trigger_transform = Transform {
                translation: face_translation_vec3
                    + face_quat
                        .mul_quat(trigger_rotation)
                        .mul_vec3(Vec3::new(0.0, 0.0, 0.02)),
                rotation: face_quat.mul_quat(trigger_rotation),
                scale: Vec3 {
                    x: 0.01,
                    y: 0.02,
                    z: 0.03,
                },
            };
            gizmos.cuboid(trigger_transform, trigger_color);
        }
    }
}