use glam::{vec3, EulerRot};
use nalgebra::{self as na, Vector3};
use rapier3d::prelude as rp;
use std::{any::Any, f32};

use crate::drone::controller::DroneController;
pub use crate::drone::controller::JoystickInput;
use crate::drone::MotorCharacteristics;

#[derive(Default)]
pub struct PIDControllerState {
    last_time: f32,
    current_time: f32,
    last_rotation: na::Unit<na::Quaternion<f32>>,
    current_rotation: na::Unit<na::Quaternion<f32>>,
}

impl PIDControllerState {
    pub fn dt(&self) -> f32 {
        return self.last_time - self.current_time;
    }
}

pub struct PIDController {
    input: JoystickInput,
    pub target_rate: f32,
    state: PIDControllerState,
}

impl Default for PIDController {
    fn default() -> Self {
        Self {
            input: JoystickInput::default(),
            target_rate: f32::consts::PI,
            state: Default::default(),
        }
    }
}

impl PIDController {
    pub fn set_input(&mut self, input: JoystickInput) {
        self.input = input;
    }
    pub fn get_angular_velocity(&self) {
        let dt: f32 = self.state.dt();
        let angles =
            (self.state.current_rotation.inverse() * self.state.last_rotation).euler_angles();
        // roll,pitch.yaw
        let vel_roll = angles.0 / dt;
        let vel_pitch = angles.1 / dt;
        let vel_yaw = angles.2 / dt;
        println!("{},{},{}", vel_roll, vel_pitch, vel_yaw);
    }
}

impl DroneController for PIDController {
    fn set_position(&mut self, position: rp::Isometry<f32>) {
        self.state.last_rotation = self.state.current_rotation;
        self.state.current_rotation = position.rotation;
    }
    fn set_time(&mut self, time: f32) {
        self.state.last_time = self.state.current_time;
        self.state.current_time = time;
    }
    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
    fn get_motor_throttles(&self) -> [f32; 4] {
        self.get_angular_velocity();
        return [
            self.input.throttle_input,
            self.input.throttle_input,
            self.input.throttle_input,
            self.input.throttle_input,
        ];
    }

    /*
     * These should be implemented like this for all the classes that implement DroneController
     * Works as an example implementation
     */
    fn as_any(&self) -> &dyn Any {
        self
    }
    fn as_mut_any(&mut self) -> &mut dyn Any {
        self
    }
}