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

use crate::drone::MotorCharacteristics;

pub trait DroneController {
    fn as_any(&self) -> &dyn Any;

    fn as_mut_any(&mut self) -> &mut dyn Any;

    fn set_position(&self, position: rp::Isometry<f32>);
    fn set_time(&self, time: f32);
    fn set_motor_characteristics(&self, motor_characteristics: &MotorCharacteristics);

    fn get_motor_speeds(&self) -> [f32; 4];
}

struct DefaultController {
    motor_speed: f32,
}

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

impl DroneController for DefaultController {
    fn set_position(&self, _position: rp::Isometry<f32>) {}
    fn set_time(&self, _time: f32) {}
    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
    fn get_motor_speeds(&self) -> [f32; 4] {
        return [self.motor_speed; 4];
    }
    fn as_any(&self) -> &dyn Any {
        self
    }
    fn as_mut_any(&mut self) -> &mut dyn Any {
        self
    }
}