use defmt::Format;

use core::fmt::Write;
use embassy_stm32::interrupt;
use embassy_stm32::peripherals;
use embassy_stm32::usb::Driver;
use embassy_usb::Builder;
use embassy_usb_serial::{CdcAcmClass, State};
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_util::channel::mpmc::{Receiver, Sender};

use futures::future::join;
use heapless::String;

use crate::usb::Gs232Cmd;
use crate::{AzElPair, RotorState};

#[embassy_executor::task]
pub async fn movement_task(
    cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
    pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>,
    state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>,
) {
    let mut rotor_state = RotorState {
        actual_pos: AzElPair { az: 0, el: 0 },
        setpoint_pos: AzElPair { az: 0, el: 0 },
        stopped: false,
    };

    loop {
        let cmd = cmd_receiver.recv().await;

        match cmd {
            Gs232Cmd::MoveTo(pair) => {
                rotor_state.setpoint_pos = pair;
                rotor_state.stopped = false;
            }
            Gs232Cmd::Stop => {
                rotor_state.stopped = true;
            }
            _ => {}
        };

        join(
            pos_sender.send(rotor_state.actual_pos),
            state_sender.send(rotor_state),
        )
        .await;
    }
}