#![allow(dead_code)]
use std::any::Any;

use crate::drone::JoystickInput;
use crate::drone::MotorCharacteristics;

impl JoystickInput {
    pub fn clamp(&self) -> JoystickInput {
        return JoystickInput {
            throttle_input: self.throttle_input.clamp(0.0, 1.0),
            yaw_input: self.yaw_input.clamp(-1.0, 1.0),
            pitch_input: self.pitch_input.clamp(-1.0, 1.0),
            roll_input: self.roll_input.clamp(-1.0, 1.0),
        };
    }
}

pub trait DroneController {
    // Allow downcast of trait -> class.
    //
    // This gives us the ability to have a Box<dyn DroneController>
    // And transform it into a pointer to its class.
    fn as_any(&self) -> &dyn Any;
    fn as_mut_any(&mut self) -> &mut dyn Any;

    /*
     * Methods called by Drone, to transmit information to the controller.
     */
    fn set_rotation(&mut self, _rotation: nalgebra::Unit<nalgebra::Quaternion<f32>>) {}
    fn set_angular_velocity(&mut self, _angvel: nalgebra::Vector3<f32>) {}
    fn set_time(&mut self, _time: f32) {}
    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}

    /*
     * Throttle should be between 0 and 1. Values will be by the Drone class.
     */
    fn get_motor_throttles(&mut self) -> [f32; 4];
}

/*
* DefaultController just sets throttle to motor_throttle, a parameter passed to it on its contructor
*/
pub struct DefaultController {
    motor_throttle: f32,
}

impl DefaultController {
    pub fn new(motor_speed: f32) -> DefaultController {
        return DefaultController {
            motor_throttle: motor_speed,
        };
    }
}

impl DroneController for DefaultController {
    fn set_time(&mut self, _time: f32) {}
    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
    fn get_motor_throttles(&mut self) -> [f32; 4] {
        return [self.motor_throttle; 4];
    }

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