use std::time::{Duration, Instant};

use bevy::{
    prelude::{Component, ReflectResource, Resource, Vec3},
    reflect::Reflect,
};
use bevy_rapier3d::dynamics::Velocity;

#[derive(Debug, Resource)]
pub(crate) struct ActionDebugInstant(pub Instant);

impl Default for ActionDebugInstant {
    fn default() -> Self {
        ActionDebugInstant(Instant::now())
    }
}

impl ActionDebugInstant {
    #[allow(dead_code)]
    pub fn reset(&mut self) {
        self.0 = Instant::now();
    }

    #[allow(dead_code)]
    pub fn elapsed(&self) -> Duration {
        self.0.elapsed()
    }
}

#[derive(Debug, Component, Default)]
pub struct PreviousVelocity(pub Velocity);

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

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

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

impl Default for CatControllerSettings {
    fn default() -> Self {
        Self {
            kp: 80.0,
            kd: 10.0,
            ki: 0.4,
        }
    }
}

#[derive(Component, Debug, Clone, Copy)]
pub struct CatControllerState {
    pub roll_integral: f32,
    pub roll_prev: f32,
    decay_factor: f32,
    roll_limit: f32,
}

impl Default for CatControllerState {
    fn default() -> Self {
        Self {
            roll_integral: Default::default(),
            roll_prev: Default::default(),
            decay_factor: 0.99,
            roll_limit: 1.5,
        }
    }
}

impl CatControllerState {
    pub fn decay(&mut self) {
        if self.roll_integral.abs() > 0.001 {
            self.roll_integral *= self.decay_factor;
        }
    }

    pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) {
        let lim = self.roll_limit;
        self.roll_integral = (self.roll_integral + (error * dt)).min(lim).max(-lim);
        let derivative = (error - self.roll_prev) / dt;
        self.roll_prev = error;
        (derivative, self.roll_integral)
    }

    pub fn set_integral_limits(&mut self, roll: f32) {
        self.roll_limit = roll;
    }
}
