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

use bevy::prelude::{
    Commands, Entity, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without,
};
use bevy_rapier3d::prelude::{
    CollisionGroups, ExternalForce, Group, MultibodyJoint, QueryFilter, RapierConfiguration,
    RapierContext, ReadMassProperties, Velocity,
};

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

const PITCH_SCALE: f32 = 0.2;

fn yaw_to_angle(yaw: f32) -> f32 {
    yaw.powi(3) * FRAC_PI_4
}

/// The gravity vector points from the cyberbike to the center of the planet.
pub(super) fn gravity(
    mut query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
    settings: Res<MovementSettings>,
    mut rapier_config: ResMut<RapierConfiguration>,
) {
    let (xform, mut forces) = query.single_mut();
    rapier_config.gravity = xform.translation.normalize() * -settings.gravity;
    forces.force = Vec3::ZERO;
    forces.torque = Vec3::ZERO;
}

/// The desired lean angle, given steering input and speed.
pub(super) fn cyber_lean(
    mut bike_state: Query<
        (
            &Velocity,
            &mut ExternalForce,
            &Transform,
            &ReadMassProperties,
        ),
        With<CyberBikeBody>,
    >,
    wheels: Query<&Transform, With<CyberWheel>>,
    input: Res<InputState>,
    gravity_settings: Res<MovementSettings>,
    mut lean: ResMut<CyberLean>,
) {
    let (velocity, mut forces, xform, mass_props) = bike_state.single_mut();
    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);

    let up = xform.translation.normalize();
    let right = up.cross(xform.back()).normalize();

    let centripetal = -right * (mass_props.0.mass * v2_r);

    if v2_r.is_normal() {
        forces.force += centripetal;
        lean.lean = -tan_theta.atan().clamp(-FRAC_PI_4, FRAC_PI_4);
    }
}

/// PID-based controller for stabilizing attitude; keeps the cyberbike upright.
pub(super) fn falling_cat(
    mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>,
    time: Res<Time>,
    settings: Res<CatControllerSettings>,
    lean: Res<CyberLean>,
    #[cfg(feature = "inspector")] mut debug_instant: ResMut<ActionDebugInstant>,
) {
    let (xform, mut forces, mut control_vars) = bike_query.single_mut();
    let wup = xform.translation.normalize(); //Transform::from_translation(xform.translation.normalize());
    let rot = Quat::from_axis_angle(xform.forward(), lean.lean);
    let [x, y, z] = wup.to_array();
    let qup = Quat::from_xyzw(x, y, z, 0.0); // turn our "world up" point into a Quat

    // p' = rot^-1 * qup * rot
    let wup = rot.inverse() * qup * rot;
    let wup = Vec3::from_array([wup.x, wup.y, wup.z]).normalize();
    let bike_up = xform.up();

    let wright = xform
        .forward()
        .cross(xform.translation.normalize())
        .normalize();

    let roll_error = wright.dot(wup);
    let pitch_error = xform.translation.normalize().dot(xform.forward());

    // 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;
        }
        #[cfg(feature = "inspector")]
        {
            if debug_instant.elapsed().as_millis() > 5000 {
                dbg!(&control_vars, mag, &wup);
                debug_instant.reset();
            }
        }
    }

    // we can do pitch correction any time, it's not coupled to roll
    let scaled_pitch_error = pitch_error * PITCH_SCALE;
    let (derivative, integral) =
        control_vars.update_pitch(scaled_pitch_error, time.delta_seconds());
    let mag =
        (settings.kp * scaled_pitch_error) + (settings.ki * integral) + (settings.kd * derivative);
    if mag.is_finite() {
        //forces.torque += wright * mag;
    }

    //control_vars.decay();
}

/// Apply forces to the cyberbike as a result of input.
pub(super) fn input_forces(
    settings: Res<MovementSettings>,
    input: Res<InputState>,
    mut braking_query: Query<&mut MultibodyJoint, (Without<CyberSteering>, With<CyberWheel>)>,
    mut body_query: Query<
        (&Transform, &mut ExternalForce),
        (With<CyberBikeBody>, Without<CyberSteering>),
    >,
    mut steering_query: Query<&mut MultibodyJoint, With<CyberSteering>>,
) {
    let (xform, mut forces) = body_query.single_mut();

    // thrust
    let thrust = xform.forward() * input.throttle * settings.accel;
    let point = xform.translation + xform.back();
    let force = ExternalForce::at_point(thrust, point, xform.translation);
    *forces += force;

    // brake
    for mut motor in braking_query.iter_mut() {
        let factor = if input.brake { 500.00 } else { 0.0 };
        motor.data = (*motor
            .data
            .as_revolute_mut()
            .unwrap()
            .set_motor_max_force(factor)
            .set_motor_velocity(0.0, factor))
        .into();
    }

    // steering
    let angle = yaw_to_angle(input.yaw);
    let mut steering = steering_query.single_mut();
    steering.data = (*steering
        .data
        .as_revolute_mut()
        .unwrap()
        .set_motor_position(-angle, 100.0, 0.5))
    .into();
}

/// Don't let the wheels get stuck underneat the planet
pub(super) fn surface_fix(
    mut commands: Commands,
    mut wheel_query: Query<
        (Entity, &Transform, &mut CollisionGroups),
        (With<CyberWheel>, Without<Tunneling>),
    >,
    context: Res<RapierContext>,
) {
    // assume the body is not below the planet surface
    for (entity, xform, mut cgroups) in wheel_query.iter_mut() {
        let ray_dir = xform.translation.normalize();
        if let Some(hit) = context.cast_ray_and_get_normal(
            xform.translation,
            ray_dir,
            10.0,
            false,
            QueryFilter::only_fixed(),
        ) {
            cgroups.memberships = Group::NONE;
            cgroups.filters = Group::NONE;
            commands.entity(entity).insert(Tunneling {
                frames: 10,
                dir: -hit.1.normal,
            });
        }
    }
}

pub(super) fn tunnel_out(
    mut commands: Commands,
    mut wheel_query: Query<
        (
            Entity,
            &mut Tunneling,
            &mut ExternalForce,
            &mut CollisionGroups,
        ),
        With<CyberWheel>,
    >,
    mprops: Query<&ReadMassProperties, With<CyberBikeBody>>,
    settings: Res<MovementSettings>,
) {
    let mprops = mprops.single();
    for (entity, mut tunneling, mut force, mut cgroups) in wheel_query.iter_mut() {
        if tunneling.frames == 0 {
            commands.entity(entity).remove::<Tunneling>();
            force.force = Vec3::ZERO;
            (cgroups.memberships, cgroups.filters) = BIKE_WHEEL_COLLISION_GROUP;
            continue;
        }
        tunneling.frames -= 1;
        force.force = tunneling.dir * settings.gravity * 1.5 * mprops.0.mass;
        //#[cfg(feature = "inspector")]
        //dbg!(&tunneling);
    }
}

/// 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);
    }
}
