use bevy::{
    diagnostic::{Diagnostics, FrameTimeDiagnosticsPlugin},
    prelude::*,
};
use bevy_rapier3d::prelude::{
    ExternalForce, Friction, NoUserData, RapierConfiguration, RapierPhysicsPlugin, Velocity,
};

use crate::{
    bike::{CyberBikeBody, CyberBikeCollider, CyberBikeControl},
    input::InputState,
};

#[derive(Resource)]
pub struct MovementSettings {
    pub accel: f32,
    pub gravity: f32,
    pub sensitivity: f32,
}

impl Default for MovementSettings {
    fn default() -> Self {
        Self {
            sensitivity: 10.0,
            accel: 20.0,
            gravity: 9.8,
        }
    }
}

#[derive(Debug, Resource, Reflect)]
#[reflect(Resource)]
struct CatControllerSettings {
    pub kp: f32,
    pub kd: f32,
    pub kws: f32,
}

impl Default for CatControllerSettings {
    fn default() -> Self {
        Self {
            kp: 10.0,
            kd: 4.0,
            kws: 0.85,
        }
    }
}

fn zero_gravity(mut config: ResMut<RapierConfiguration>) {
    config.gravity = Vec3::ZERO;
}

fn gravity(
    mut query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
    settings: Res<MovementSettings>,
) {
    let (_xform, mut forces) = query.single_mut();
    let grav = Vec3::Y * -settings.gravity;
    forces.force = grav;
}

fn falling_cat(
    mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CyberBikeControl)>,
    _diagnostics: Res<Diagnostics>,
    settings: Res<CatControllerSettings>,
) {
    let (xform, mut forces, mut control_vars) = bike_query.single_mut();
    let up = Vec3::Y;
    let bike_up = xform.up();

    let torque = bike_up.cross(up).normalize_or_zero();
    let cos = up.dot(bike_up);
    let cos = if cos.is_finite() { cos } else { 1.0 };

    let error = 1.0 - cos;

    let derivative = error - control_vars.prev_error;
    control_vars.prev_error = error;
    // this integral term is not an integral, it's more like a weighted moving sum
    let weighted_sum = control_vars.error_sum + error;
    control_vars.error_sum = weighted_sum * 0.8;

    let mag = (settings.kp * error) + (settings.kws * weighted_sum) + (settings.kd * derivative);

    #[cfg(feature = "inspector")]
    if let Some(count) = _diagnostics
        .get(FrameTimeDiagnosticsPlugin::FRAME_COUNT)
        .and_then(|d| d.smoothed())
        .map(|x| x as u64)
    {
        if count % 30 == 0 {
            dbg!(&control_vars, mag, cos, derivative);
        }
    }

    forces.torque = torque * mag;
}

fn input_forces(
    settings: Res<MovementSettings>,
    input: Res<InputState>,
    mut cquery: Query<&mut Friction, With<CyberBikeCollider>>,
    mut bquery: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
) {
    let (xform, mut forces) = bquery.single_mut();
    let mut friction = cquery.single_mut();

    // thrust
    let thrust = xform.forward() * input.throttle * settings.accel;
    forces.force += thrust;

    // brake
    friction.coefficient = if input.brake { 2.0 } else { 0.0 };

    // steering
    let torque = xform.down() * input.yaw * settings.sensitivity;
    forces.torque += torque;
}

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

pub struct CyberActionPlugin;
impl Plugin for CyberActionPlugin {
    fn build(&self, app: &mut App) {
        app.init_resource::<MovementSettings>()
            .init_resource::<CatControllerSettings>()
            .register_type::<CatControllerSettings>()
            .add_plugin(RapierPhysicsPlugin::<NoUserData>::default())
            .add_plugin(FrameTimeDiagnosticsPlugin::default())
            .add_startup_system(zero_gravity)
            .add_system(gravity.before("cat"))
            .add_system(falling_cat.label("cat"))
            .add_system(input_forces.label("iforces").after("cat"))
            .add_system(drag.label("drag").after("iforces"));
    }
}