use nalgebra as na;
use rapier3d::prelude as rp;

use crate::engine::World;

mod controller;
use controller::*;

pub struct MotorCharacteristics {
    relative_motor_positions: [na::OPoint<f32, na::Const<3>>; 4],
    max_thrust: f32,
    max_torque: f32,
}

impl Default for MotorCharacteristics {
    fn default() -> Self {
        Self {
            relative_motor_positions: [
                rp::point![5.0, 3.0, 5.0],
                rp::point![-5.0, 3.0, 5.0],
                rp::point![-5.0, 3.0, -5.0],
                rp::point![5.0, 3.0, -5.0],
            ],
            max_thrust: 10.0,
            max_torque: 1.0,
        }
    }
}

struct Drone {
    rb_handle: rp::RigidBodyHandle,
    motor_characteristics: MotorCharacteristics,
    pub controller: Box<dyn DroneController>,
}

impl Drone {
    pub fn new(
        world: &mut World,
        controller: Box<dyn DroneController>,
        motor_characteristics: MotorCharacteristics,
    ) -> Drone {
        let drone_rb_handle = world.register_body(
            rp::RigidBodyBuilder::dynamic()
                .translation(rp::vector![0.0, 10.0, 0.0])
                .rotation(rp::vector![0.6, 0.0, 0.0])
                .linear_damping(0.1)
                .angular_damping(0.1)
                .build(),
        );
        world.register_collider(
            rp::ColliderBuilder::cuboid(5.0, 0.5, 5.0)
                .restitution(0.3)
                .build(),
            drone_rb_handle,
            None,
        );

        controller.set_motor_characteristics(&motor_characteristics);

        return Drone {
            rb_handle: drone_rb_handle,
            controller: controller,
            motor_characteristics: motor_characteristics,
        };
    }
    pub fn update_controller(&mut self, world: &World, tick: u64) {
        let rb = world.bodies.get(self.rb_handle).unwrap();
        self.controller.set_position(*rb.position());
        self.controller.set_time(tick as f32 / 60.0);
    }
}