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

use bevy::{
    prelude::{
        info, Gizmos, Local, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without,
    },
    render::color::Color,
};
use bevy_rapier3d::{
    geometry::Friction,
    prelude::{
        ExternalForce, QueryFilter, RapierConfiguration, RapierContext, ReadMassProperties,
        TimestepMode, Velocity,
    },
};

#[cfg(feature = "inspector")]
use super::ActionDebugInstant;
use super::{
    CatControllerSettings, CatControllerState, CyberLean, MovementSettings, PreviousVelocity,
};
use crate::{
    bike::{CyberBikeBody, CyberSteering, CyberWheel, WheelConfig},
    input::InputState,
};

pub(super) fn physics_init(
    mut config: ResMut<RapierConfiguration>,
    settings: Res<MovementSettings>,
) {
    config.timestep_mode = TimestepMode::Variable {
        max_dt: 1.0 / 60.0,
        time_scale: 1.0,
        substeps: 3,
    };

    config.gravity = Vec3::NEG_Y * settings.gravity;
}

pub(super) fn reset_external_forces(
    mut query: Query<&mut ExternalForce>,
    #[cfg(feature = "inspector")] mut debug_instant: ResMut<ActionDebugInstant>,
) {
    for mut force in query.iter_mut() {
        #[cfg(feature = "inspector")]
        if debug_instant.elapsed().as_millis() > 1000 {
            info!("{force:?}");
        }

        force.force = Vec3::ZERO;
        force.torque = Vec3::ZERO;
    }
    #[cfg(feature = "inspector")]
    if debug_instant.elapsed().as_millis() > 1000 {
        debug_instant.reset();
    }
}

/// The desired lean angle, given steering input and speed.
pub(super) fn cyber_lean(
    bike_state: Query<(&Velocity, &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.linvel.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 falling_cat(
    mut bike_query: Query<(
        &Transform,
        &Velocity,
        &mut ExternalForce,
        &mut CatControllerState,
    )>,
    time: Res<Time>,
    settings: Res<CatControllerSettings>,
    lean: Res<CyberLean>,
    mut count: Local<usize>,
) {
    let (xform, vel, mut forces, mut control_vars) = bike_query.single_mut();
    let world_up = Vec3::Y; //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());

    if *count < 30 {
        if *count == 0 {
            //dbg!(&xform.translation);
            //dbg!(&vel.linvel);
        }
        *count += 1;
    } else {
        *count = 0;
    }

    // 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() {
            forces.torque += xform.back() * mag;
        }
    }
}

pub(super) fn wheel_forces(
    mut wheels_query: Query<
        (
            &Transform,
            &Velocity,
            &mut ExternalForce,
            &mut Friction,
            Option<&CyberSteering>,
        ),
        With<CyberWheel>,
    >,
    wheel_config: Res<WheelConfig>,
    context: Res<RapierContext>,
    settings: Res<MovementSettings>,
    input: Res<InputState>,
    mut gizmos: Gizmos,
) {
    // wheel stuff
    let radius = wheel_config.radius;

    // inputs
    let yaw = input.yaw;
    let throttle = input.throttle * settings.accel;

    for (xform, velocity, mut external_force, mut friction, steering) in wheels_query.iter_mut() {
        let pos = xform.translation;
        if let Some((_, projected)) = context.project_point(pos, false, QueryFilter::only_fixed()) {
            let mut force = Vec3::ZERO;

            gizmos.sphere(projected.point, Quat::IDENTITY, radius, Color::RED);

            let dir = (projected.point - pos).normalize();

            if let Some((_, intersection)) = context.cast_ray_and_get_normal(
                pos,
                dir,
                radius * 1.05,
                false,
                QueryFilter::only_fixed(),
            ) {
                let norm = intersection.normal;
                let point = intersection.point;

                // do thrust's share
                let thrust = norm.cross(xform.right().into()) * throttle;
                force += thrust;

                // do steering's share
                if steering.is_some() {
                    let thrust = norm.cross(xform.back().into());
                    force += 5.0 * yaw * thrust;
                }

                let forward = if steering.is_some() {
                    let angle = yaw_to_angle(yaw);
                    let rot = Quat::from_axis_angle(Vec3::Y, angle);
                    Transform::from_rotation(rot).forward()
                } else {
                    xform.forward()
                };

                // do the friction's share
                //
                // first the brakes
                if input.brake {
                    friction.coefficient = 2.0;
                } else {
                    friction.coefficient = 0.0;
                }

                // now static simulated friction

                let alignment = forward.dot(velocity.linvel);
                let alignment = if alignment.is_normal() || alignment == 0.0 {
                    alignment
                } else {
                    1.0
                };
                dbg!(alignment);
                let sign = alignment.signum();
                let slide = sign * (1.0 - alignment.abs());
                let perp = forward.cross(norm).normalize();
                force += slide * perp;

                // show the force
                #[cfg(feature = "inspector")]
                gizmos.arrow(pos, pos + force, Color::RED);

                // ok apply the force
                *external_force = ExternalForce::at_point(force, point, pos);
            }
        }
    }
}

/// General velocity-based drag-force calculation; does not take orientation
/// into account.
pub(super) fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With<CyberBikeBody>>) {
    let (vels, mut forces) = query.single_mut();

    if let Some(vel) = vels.linvel.try_normalize() {
        let v2 = vels.linvel.length_squared();
        forces.force -= vel * (v2 * 0.02);
    }
}

// helpers
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])
}
