use log::{debug, error, info, warn};
use tokio::{
    self,
    io::{AsyncBufReadExt, AsyncWriteExt, BufStream},
    net::{TcpListener, TcpStream},
    sync::{self, mpsc, watch},
    time,
};

use crate::rotctlprotocol::{parse_command, Command};

pub async fn control_rotor(mut rx_cmd: mpsc::Receiver<Command>, pos_tx: watch::Sender<(f32, f32)>) {
    let mut actual_az = 0.0;
    let mut actual_el = 0.0;

    let mut target_az = 0.0;
    let mut target_el = 0.0;

    loop {
        tokio::select! {
            Some(command) = rx_cmd.recv() => {
                match command {
                    Command::SetPos(az, el) => {
                        info!("Received set pos {} {}", az, el);
                        target_az = az;
                        target_el = el;
                    }
                    _ => {}
                }
            },
            _ = time::sleep(time::Duration::from_millis(100)) => {
                if target_az < actual_az {
                    actual_az -= 1.0;
                } else if  target_az > actual_az  {
                    actual_az += 1.0;
                }

                if target_el < actual_el {
                    actual_el -= 1.0;
                } else if  target_el > actual_el  {
                    actual_el += 1.0;
                }

                pos_tx.send((actual_az, actual_el)).unwrap();
            },
            else => return
        };
    }
}