use nalgebra as na;

pub struct MotorMixer {
    pub motor_map: [[f32; 3]; 4], // roll, yaw, pitch
    pub min_throttle: f32,
    pub max_throttle: f32,
}

impl MotorMixer {
    pub fn mix(&self, throttle: f32, torque: na::Vector3<f32>) -> ([f32; 4], bool) {
        let mut delta = [0.0f32; 4];

        // 1. Torque-only contribution
        for i in 0..4 {
            delta[i] = self.motor_map[i][0] * torque.x
                + self.motor_map[i][1] * torque.y
                + self.motor_map[i][2] * torque.z;
        }

        // 2. Compute allowable scaling
        let mut scale = 1.0f32;

        for i in 0..4 {
            if delta[i] > 0.0 {
                scale = scale.min((self.max_throttle - throttle) / delta[i]);
            } else if delta[i] < 0.0 {
                scale = scale.min((self.min_throttle - throttle) / delta[i]);
            }
        }

        scale = scale.clamp(0.0, 1.0);

        // 3. Apply
        let mut motors = [0.0f32; 4];
        for i in 0..4 {
            motors[i] = throttle + scale * delta[i];
        }

        let saturated = scale < 1.0;
        (motors, saturated)
    }
}