use bevy::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,
        }
    }
}

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 = xform.translation.normalize() * -settings.gravity;
    forces.force = grav;
}

fn falling_cat_pid(
    mut bike_query: Query<(
        &Transform,
        &Velocity,
        &mut ExternalForce,
        &mut CyberBikeControl,
    )>,
) {
    let (xform, vels, mut forces, mut control_vars) = bike_query.single_mut();
    let up = xform.translation.normalize();
    let cam_forward = xform.forward();
    let cam_right = xform.right();
    let pitch_cos = up.dot(cam_forward);
    let roll_cos = up.dot(cam_right);

    let mut torque = Vec3::ZERO;

    // roll
    {
        let roll_error = if roll_cos.is_normal() { -roll_cos } else { 0.0 };
        let derivative = roll_error - control_vars.prev_roll_error;
        control_vars.prev_roll_error = roll_error;

        let integral = (control_vars.roll_sum * 0.8) + roll_error;
        control_vars.roll_sum = integral;

        //dbg!(integral);

        let proportional = roll_error;

        let kp = 7.1;
        let ki = 0.8;
        let kd = 5.1;
        let mag = (kp * proportional) + (ki * integral) + (kd * derivative);

        let angvel = vels.angvel.as_ref();
        let roll_rate = cam_forward.dot(Vec3::from_slice(angvel)).abs();
        //dbg!(roll_rate);
        if pitch_cos.abs() < 0.85 && roll_rate < 1.0 {
            torque += xform.back() * mag;
        }
    }

    // pitch
    {
        let pitch_error = if pitch_cos.is_normal() {
            -pitch_cos
        } else {
            0.0
        };

        let derivative = pitch_error - control_vars.prev_pitch_error;
        control_vars.prev_pitch_error = pitch_error;

        let integral = (control_vars.pitch_sum * 0.7) + pitch_error;
        control_vars.pitch_sum = integral;

        let proportional = pitch_error;

        let kp = 7.1;
        let ki = 0.3;
        let kd = 4.1;
        let mag = (kp * proportional) + (ki * integral) + (kd * derivative);

        let angvel = vels.angvel.as_ref();
        let pitch_rate = cam_right.dot(Vec3::from_slice(angvel)).abs();

        if roll_cos.abs() < 0.85 && pitch_rate < 2.0 {
            torque += xform.left() * mag;
        }
    }

    forces.torque = torque;
}

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.up() * input.yaw * settings.sensitivity;
    forces.torque += torque;

    //dbg!(&input);
}

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

    //dbg!(&forces);
}

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