use crate::gamepad::Gamepad;
use crate::printer::gcode::{G0Command, G91Command};
use crate::printer::{Printer};
use euclid::{vec3, Vector3D};
use futures::never::Never;
use std::sync::Arc;
use std::time::Duration;

/// Time that a single movement command should take
///
/// For a given GCode buffer size on the machine, we can assume
/// a maximum delay of `TIME_PER_MOVEMENT * BUFFER_COUNT`
const TIME_PER_MOVEMENT: Duration = Duration::from_millis(50);
/// Movement speed of gantry when going full throttle in x/y-direction
/// in units/min (mm/min)
const FULL_SCALE_SPEED_XY: f64 = 1000.0;
/// Movement speed of gantry when going full throttle in z-direction
/// in units/min (mm/min)
const FULL_SCALE_SPEED_Z: f64 = 10.0;

/// Should be mm on most machine including the Leapfrog
pub struct PrinterUnits;
pub type PrinterVec = Vector3D<f64, PrinterUnits>;

/// Jogging the gantry by pumping loads of gcode into the printer board
pub async fn jog(gamepad: Arc<Gamepad>, mut printer: Printer) -> Never {
    printer.send_gcode(Box::new(G91Command)).await.unwrap();
    println!("Sent G91Command");
    loop {
        let (setpoint_x, setpoint_y, setpoint_z) = gamepad.speed_setpoint();
        // We're bound by lower speed on z and have no way to go at separate speeds per axis
        let full_scale_speed = if setpoint_z == 0.0 {
            FULL_SCALE_SPEED_XY
        } else {
            FULL_SCALE_SPEED_Z
        };
        let distance: PrinterVec = vec3(
            full_scale_speed * (TIME_PER_MOVEMENT.as_secs_f64() / 60.0) * (setpoint_x as f64),
            full_scale_speed * (TIME_PER_MOVEMENT.as_secs_f64() / 60.0) * (setpoint_y as f64),
            full_scale_speed * (TIME_PER_MOVEMENT.as_secs_f64() / 60.0) * (setpoint_z as f64),
        );
        if distance.length() == 0.0 {
            continue;
        }
        let velocity = distance.length() / (TIME_PER_MOVEMENT.as_secs_f64() / 60.0);
        let command = G0Command {
            x: distance.x.into(),
            y: distance.y.into(),
            z: distance.z.into(),
            e: None,
            velocity: velocity.into(),
        };
        printer.send_gcode(Box::new(command)).await;
    }
}