use nalgebra::{self as na, Quaternion, Unit};

pub mod config;
pub mod controller;
pub mod input;
pub mod stacked;

pub use config::*;
pub use controller::*;
pub use input::*;
pub use stacked::*;

#[unsafe(no_mangle)]
pub extern "C" fn create(c: config::ControllerConfig) -> *mut StackedController {
    return Box::into_raw(Box::new(StackedController::new(c)));
}


#[unsafe(no_mangle)]
pub unsafe extern "C" fn destroy(ptr: *mut StackedController) {
    assert!(!ptr.is_null());
    let _x = unsafe { Box::from_raw(ptr) };
}

#[repr(C)]
pub struct MotorThrottles {
    pub values: [f32; 4],
}

#[repr(C)]
pub struct Vec3C {
    pub x: f32,
    pub y: f32,
    pub z: f32,
}

impl Into<na::Vector3<f32>> for Vec3C {
    fn into(self) -> na::Vector3<f32> {
        na::vector![self.x, self.y, self.z]
    }
}

#[unsafe(no_mangle)]
pub extern "C" fn get_throttles(ptr: *mut StackedController) -> MotorThrottles {
    let controller = unsafe {
        assert!(!ptr.is_null());
        &mut *ptr
    };

    return MotorThrottles {
        values: controller.get_motor_throttles(),
    };
}

#[unsafe(no_mangle)]
pub extern "C" fn set_cur_time(ptr: *mut StackedController, time: f32) {
    let controller = unsafe {
        assert!(!ptr.is_null());
        &mut *ptr
    };

    controller.set_time(time);
}

#[unsafe(no_mangle)]
pub extern "C" fn set_input(ptr: *mut StackedController, inp: Input) {
    let controller = unsafe {
        assert!(!ptr.is_null());
        &mut *ptr
    };

    controller.set_input(inp);
}

#[unsafe(no_mangle)]
pub extern "C" fn set_cur_rot(ptr: *mut StackedController, parts: Vec3C, scalar: f32) {
    let controller = unsafe {
        assert!(!ptr.is_null());
        &mut *ptr
    };

    let quat: na::UnitQuaternion<f32> =
        na::UnitQuaternion::from_quaternion(Quaternion::from_parts(scalar, parts.into()));
    controller.set_rotation(quat);
}

#[unsafe(no_mangle)]
pub extern "C" fn set_cur_angvel(ptr: *mut StackedController, angvel: Vec3C) {
    let controller = unsafe {
        assert!(!ptr.is_null());
        &mut *ptr
    };

    controller.set_angular_velocity(angvel.into());
}

#[unsafe(no_mangle)]
pub extern "C" fn set_cur_linvel(ptr: *mut StackedController, vel: Vec3C) {
    let controller = unsafe {
        assert!(!ptr.is_null());
        &mut *ptr
    };

    controller.set_linvel(vel.into());
}

#[unsafe(no_mangle)]
pub extern "C" fn set_cur_pos(ptr: *mut StackedController, pos: Vec3C) {
    let controller = unsafe {
        assert!(!ptr.is_null());
        &mut *ptr
    };

    controller.set_pos(pos.into());
}