use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};

use avian3d::{
    collision::Collisions,
    dynamics::solver::xpbd::AngularConstraint,
    prelude::{
        ColliderMassProperties, Collision, ExternalForce, ExternalTorque, Gravity, LinearVelocity,
        PrismaticJoint, RigidBodyQuery,
    },
};
use bevy::prelude::{EventReader, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without};

use super::{CatControllerSettings, CatControllerState, CyberLean, MovementSettings};
use crate::{
    bike::{CyberBikeBody, CyberFork, CyberSpring, CyberSteering, CyberWheel, WheelConfig},
    input::InputState,
};

fn yaw_to_angle(yaw: f32) -> f32 {
    let v = yaw.powi(5) * FRAC_PI_4;
    if v.is_normal() {
        v
    } else {
        0.0
    }
}

fn rotate_point(pt: &Vec3, rot: &Quat) -> Vec3 {
    // thanks to https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html
    let [x, y, z] = pt.normalize().to_array();
    let qpt = Quat::from_xyzw(x, y, z, 0.0);
    // p' = rot^-1 * qpt * rot
    let rot_qpt = rot.inverse() * qpt * *rot;

    // why does this need to be inverted???
    -Vec3::from_array([rot_qpt.x, rot_qpt.y, rot_qpt.z])
}

/// The gravity vector points from the cyberbike to the center of the planet.
pub(super) fn set_gravity(
    xform: Query<&Transform, With<CyberBikeBody>>,

    settings: Res<MovementSettings>,
    mut gravity: ResMut<Gravity>,
) {
    let xform = xform.single();
    *gravity = Gravity(xform.translation.normalize() * -settings.gravity);
}

pub(super) fn clear_forces(
    mut forces: Query<(Option<&mut ExternalForce>, Option<&mut ExternalTorque>)>,
) {
    for (force, torque) in forces.iter_mut() {
        if let Some(mut force) = force {
            force.clear();
        }
        if let Some(mut torque) = torque {
            torque.clear();
        }
    }
}

pub(super) fn suspension(
    movment_settings: Res<MovementSettings>,
    wheel_config: Res<WheelConfig>,
    time: Res<Time>,
    mass: Query<&ColliderMassProperties, With<CyberBikeBody>>,
    mut axels: Query<(&mut ExternalForce, &CyberSpring, &Transform)>,
) {
    let mass = if let Ok(mass) = mass.get_single() {
        mass.mass.0
    } else {
        1.0
    };
    let dt = time.delta_seconds();
    let gravity = movment_settings.gravity;
    let mag = -wheel_config.stiffness * mass * gravity * dt;
    for (mut force, spring, xform) in axels.iter_mut() {
        let spring = mag * spring.0;
        let spring = xform.translation + spring;
        bevy::log::info!("suspension force: {spring:?}");
        let _ = force.apply_force(spring);
    }
}

/// The desired lean angle, given steering input and speed.
pub(super) fn calculate_lean(
    bike_state: Query<(&LinearVelocity, &Transform), With<CyberBikeBody>>,
    wheels: Query<&Transform, With<CyberWheel>>,
    input: Res<InputState>,
    gravity_settings: Res<MovementSettings>,
    mut lean: ResMut<CyberLean>,
) {
    let (velocity, xform) = bike_state.single();
    let vel = velocity.dot(*xform.forward());
    let v_squared = vel.powi(2);
    let steering_angle = yaw_to_angle(input.yaw);
    let wheels: Vec<_> = wheels.iter().map(|w| w.translation).collect();
    let wheel_base = (wheels[0] - wheels[1]).length();
    let radius = wheel_base / steering_angle.tan();
    let gravity = gravity_settings.gravity;
    let v2_r = v_squared / radius;
    let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);

    if tan_theta.is_finite() && !tan_theta.is_subnormal() {
        lean.lean = tan_theta.atan().clamp(-FRAC_PI_4, FRAC_PI_4);
    } else {
        lean.lean = 0.0;
    }
}

/// PID-based controller for stabilizing attitude; keeps the cyberbike upright.
pub(super) fn apply_lean(
    mut bike_query: Query<(&Transform, &mut ExternalTorque, &mut CatControllerState)>,
    time: Res<Time>,
    settings: Res<CatControllerSettings>,
    lean: Res<CyberLean>,
) {
    let (xform, mut torque, mut control_vars) = bike_query.single_mut();
    let world_up = xform.translation.normalize();
    let rot = Quat::from_axis_angle(*xform.back(), lean.lean);
    let target_up = rotate_point(&world_up, &rot).normalize();

    let bike_right = xform.right();

    let roll_error = bike_right.dot(target_up);
    let pitch_error = world_up.dot(*xform.back());

    // only try to correct roll if we're not totally vertical
    if pitch_error.abs() < 0.95 {
        let (derivative, integral) = control_vars.update_roll(roll_error, time.delta_seconds());
        let mag =
            (settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative);
        if mag.is_finite() {
            torque.apply_torque(*xform.back() * mag);
        }
    }
}

/// Apply forces to the cyberbike as a result of input.
pub(super) fn apply_inputs(
    settings: Res<MovementSettings>,
    input: Res<InputState>,
    time: Res<Time>,
    fork: Query<&PrismaticJoint, With<CyberFork>>,
    mut axle: Query<(RigidBodyQuery, &Transform), With<CyberSteering>>,
    mut braking_query: Query<&mut ExternalTorque, With<CyberWheel>>,
    mut body_query: Query<
        (
            RigidBodyQuery,
            &Transform,
            &ColliderMassProperties,
            &mut ExternalForce,
        ),
        (With<CyberBikeBody>, Without<CyberSteering>),
    >,
) {
    let Ok((mut bike, xform, mass, mut forces)) = body_query.get_single_mut() else {
        bevy::log::debug!("no bike body found");
        return;
    };
    let dt = time.delta_seconds();

    // thrust
    let thrust = xform.forward() * input.throttle * settings.accel;
    let point = xform.translation + *xform.back();

    //let _ = *forces.apply_force_at_point(dt * thrust, point,
    // mass.center_of_mass.0);

    // brake + thrust
    for mut motor in braking_query.iter_mut() {
        let factor = if input.brake {
            500.00
        } else {
            input.throttle * settings.accel
        };
        let speed = if input.brake { 0.0 } else { -70.0 };
        let target = dt * factor * speed;
        let torque = target * Vec3::X;
        motor.apply_torque(torque);
    }

    // steering
    let _angle = yaw_to_angle(input.yaw);
    let (mut axle, xform) = axle.single_mut();

    let cur_rot = xform.rotation;
    let fork = fork.single();
    let axis = fork.free_axis.normalize();
    let new = Quat::from_axis_angle(axis, _angle);
    let diff = (new - cur_rot).to_scaled_axis();
    let mut compliance = 1.0;

    fork.align_orientation(&mut axle, &mut bike, diff, &mut compliance, 1.0, dt);
}

//#[cfg(feature = "inspector")]
mod inspector {
    use bevy::prelude::Entity;

    use super::*;
    pub(crate) fn debug_bodies(
        bodies: Query<(Entity, RigidBodyQuery)>,
        mut collisions: EventReader<Collision>,
    ) {
        let bodies: Vec<_> = bodies.iter().collect();
        for i in 0..(bodies.len()) {
            let (ent, ref rb) = bodies[i];
            let cur_pos = rb.current_position();
            let mut max = 0.0f32;
            let mut min = f32::MAX;
            for j in 0..(bodies.len()) {
                if j == i {
                    continue;
                }
                let (_, ref other) = bodies[j];
                let dist = (cur_pos - other.current_position()).length();
                max = max.max(dist);
                min = min.min(dist);
            }
            let line = format!("{ent:?} at {cur_pos:?} -- min: {min}, max: {max}");
            bevy::log::info!(line);
        }
        for Collision(contacts) in collisions.read() {
            bevy::log::info!(
                "Entities {:?} and {:?} are colliding",
                contacts.entity1,
                contacts.entity2,
            );
        }
    }
}
//#[cfg(feature = "inspector")]
pub(super) use inspector::debug_bodies;