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

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.current_time - self.last_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) -> na::Vector3<f32> {
        let dt: f32 = self.state.dt();

        let angles =
            (self.state.last_rotation.inverse() * self.state.current_rotation).euler_angles();
        println!("dt: {}", dt);
        return na::Vector3::new(angles.0, angles.1, angles.2) / dt;
    }

    pub fn get_desired_angular_velocity(&self) -> na::Vector3<f32> {
        return na::Vector3::new(
            self.input.pitch_input,
            self.input.yaw_input,
            self.input.roll_input,
        ) * self.target_rate;
    }
}

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(&mut self) -> [f32; 4] {
        let target = self.get_desired_angular_velocity();
        let current = self.get_angular_velocity();
        let diff = target - current;
        println!("Target: {:}", target);
        println!("Current: {:}", current);
        println!("Diff: {:}", diff);

        let throttle = self.input.throttle_input;

        let roll = diff.z * 0.01;

        let pitch = diff.x * 0.01;

        let yaw = diff.y;

        /*
         * Motor position indices
         *    ^ - Front
         *    |
         *    |
         * 1 --- 0
         * |     |
         * |     |
         * 2 --- 3
         */
        let mut motors = [
            throttle - pitch + roll + yaw,
            throttle - pitch - roll - yaw,
            throttle + pitch - roll + yaw,
            throttle + pitch + roll - yaw,
        ];

        // let mut motors = [
        //     throttle + yaw + pitch,
        //     throttle - yaw + pitch,
        //     throttle + yaw - pitch,
        //     throttle - yaw - pitch,
        // ];
        // let mut motors = [
        //     throttle - pitch + roll + yaw,
        //     throttle - pitch - roll - yaw,
        //     throttle + pitch - roll + yaw,
        //     throttle + pitch + roll - yaw,
        // ];

        // motors.max
        let max = motors
            .iter()
            .copied()
            .max_by(|a, b| a.total_cmp(b))
            .unwrap_or(0.0);
        if max > 1.0 {
            for v in motors.iter_mut() {
                *v /= max;
            }
        }
        return motors;
    }

    /*
     * 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
    }
}