use rapier3d::prelude as rp;
use std::any::Any;

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

pub struct FPVController {
    input: JoystickInput,
    rate_yaw: f32,
    rate_pitch: f32,
    rate_roll: f32,
}

impl FPVController {
    pub fn set_input(&mut self, input: JoystickInput) {
        self.input = input;
    }
}

impl Default for FPVController {
    fn default() -> Self {
        Self {
            input: JoystickInput::default(),
            rate_yaw: 1.0,
            rate_pitch: 1.0,
            rate_roll: 1.0,
        }
    }
}

impl DroneController for FPVController {
    fn set_position(&mut self, _position: rp::Isometry<f32>) {}
    fn set_time(&mut self, _time: f32) {}
    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
    fn get_motor_throttles(&self) -> [f32; 4] {
        let yaw = self.rate_yaw * self.input.yaw_input;
        let roll = self.rate_roll * self.input.roll_input;
        let pitch = self.rate_pitch * self.input.pitch_input;
        let throttle = self.input.throttle_input;

        /*
         * 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,
        ];
        // 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
    }
}