cruisecontrol/interface/src/main.rs

use std::{fmt::format, ops::ControlFlow, path::Path, result, sync::{atomic::{AtomicBool, Ordering}, Arc}, thread::{self, sleep}, time::Duration};

use anyhow::{Context, Ok, Result};
use atomic_float::AtomicF32;
use directories::ProjectDirs;
use interface::{auto::{get_confs, Auto, AutoInterface}, auto_impl, combatlog::{combat_logger, CombatData}, gui::CONFIGS, storage::StorageManager, POWER_THRESHOLD};
use common::{ControlPacket, TelemetryPacket};
use cpal::traits::{DeviceTrait, HostTrait, StreamTrait};
use egui_toast::{Toast, ToastKind};
use interface::gui::{gui, GUIData, DEFAULT_VOLUME_THRESHOLD, GUI};
use pitch_detection::{detector::{mcleod::McLeodDetector, PitchDetector}, utils};
use rust_music_theory::note::{Note, NoteLetter, Pitch, Tuning};
use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter, WriteHalf}, net::{tcp::{OwnedReadHalf, OwnedWriteHalf}, TcpStream}, spawn, sync::{self, broadcast, mpsc, watch, RwLock}, time::timeout};


fn main() -> Result<()> {
    let directories = ProjectDirs::from("dev", "ank", "Cruise Control").context("no home")?;
    let auto = Auto::new("../auto/target/release/libauto.so")?;

    let storage = StorageManager::new(directories.config_dir().to_path_buf().join(Path::new("conf.dat")))?;

    let auto_allowed = Arc::new(AtomicBool::new(false));

    let (auto_telem_sender, auto_telem) = watch::channel(TelemetryPacket::default());
    let (interface, auto_command, auto_enabled) = AutoInterface::new(auto_telem.clone(), storage.clone(), auto_allowed.clone());

    println!("name: {}", auto.name());

    for config in auto.configs().iter() {
        println!("co {}", config.name);
        println!("co {:?}", config.description);
    }

    let (logging_sender, combatlog) = mpsc::channel(64);

    // assumes pulseaudio system with f32 samples and 2204 sample packets
    dbg!(cpal::available_hosts());
    let host = cpal::default_host();
    #[cfg(target_os = "windows")]
    let Some(device) = host.devices().unwrap().filter(|device| device.supported_input_configs().unwrap().count() != 0).next() else {
        panic!("no input devices")
    };
    #[cfg(target_os = "linux")]
    let device = host.devices().unwrap().find(|d|d.name().unwrap() == "pulse").context("no pulse")?;

    let config = device.default_input_config()?;

    dbg!(config.sample_format());
    let rate = config.sample_rate();

    let (sender, notes) = broadcast::channel(2);

    const CLARITY_THRESHOLD: f32 = 0.85;
    #[cfg(target_os = "linux")]
    const PACKET_LEN: usize = 2204;
    #[cfg(target_os = "windows")]
    const PACKET_LEN: usize = 1800;

    let mut packet = Vec::new();

    let stream = device.build_input_stream(&config.into(), 
        move | data: &[f32], _: &_| {
            let mut data = data;

            if packet.len() > PACKET_LEN {
                packet.clear();
            }

            if data.len() <= PACKET_LEN {
                packet.extend_from_slice(data);
                if packet.len() > PACKET_LEN {
                    data = &packet;
                } else {
                    return;
                }
            }
            assert!(data.len() >= PACKET_LEN);
            let data = &data[..PACKET_LEN];

            // reinitialized every packet as it is not thread safe
            let mut detctor = McLeodDetector::new(PACKET_LEN, PACKET_LEN/2);
            let vol = utils::buffer::square_sum(data);
            if let Err(e) = sender.send((detctor.get_pitch(data, rate.0 as usize, POWER_THRESHOLD.load(Ordering::Relaxed), CLARITY_THRESHOLD).map(|p|p.frequency),vol)) {
                println!("channel: {e}");
            }
            
        }, 
        move |err| {eprintln!("{err}")} , 
        Some(Duration::from_secs(1)))?;

    stream.play()?;

    let executor = tokio::runtime::Runtime::new()?;

    //let latest_telem = Arc::new(RwLock::new(None));

    let (toast_sender, toasts) = mpsc::channel(5);
    let (data_sender, gui_data) =watch::channel(GUIData::default());


    let spawner = executor.handle().clone();
    let auto_allowed_ = auto_allowed.clone();
    let interface_ = interface.clone();
    thread::spawn(move || {
        spawner.block_on(async {

            let log_toasts = toast_sender.clone();
            tokio::spawn(async move {
                if let Err(e) = combat_logger(combatlog, directories.data_dir()).await {
                    let _ = log_toasts.clone().send(Toast::new().text(format!("logger crashed: {e:?}")).kind(ToastKind::Error)).await;
                }
            });

            loop {
                if let Err(e) = connect(toast_sender.clone(), notes.resubscribe(), data_sender.clone(), logging_sender.clone(), auto_telem_sender.clone(), auto_allowed_.clone(), interface_.clone()).await {
                    if let Err(_) = toast_sender.send(Toast::new().text(format!("{e:?}")).kind(ToastKind::Error)).await {
                        break;
                    };
                }
            }
        });

    });

    let interface_ = interface.clone();
    let spawner = executor.handle().clone();
    thread::spawn(move || {
        spawner.block_on(async {
            let _stoppie = auto_impl::auto(interface_).await;
            println!("auto ended");
        });
    });

    println!("launching gui");
    let (_conf_sender, conf) = watch::channel(CONFIGS);
    gui(gui_data, toasts, executor, conf, storage.clone(), auto_allowed, auto_enabled).unwrap();

    drop(stream);

    storage.save();

    Ok(())
}

/// frequency, volume
type Detection = (Option<f32>, f32);

async fn connect(toast_sender: mpsc::Sender<Toast>, notes: broadcast::Receiver<Detection>, data_sender: watch::Sender<GUIData>, logging_sender: mpsc::Sender<CombatData>, auto_sender: watch::Sender<TelemetryPacket>, auto_allowed: Arc<AtomicBool>, interface: AutoInterface) -> Result<()>{
    toast_sender.send(Toast::new().text("connecting to bot").kind(ToastKind::Info)).await?;
    let cruisecontrol = TcpStream::connect("192.168.1.2:1234").await?;
    println!("connected");
    toast_sender.send(Toast::new().text("connected").kind(ToastKind::Success)).await?;
    cruisecontrol.set_nodelay(true)?;
    let (telem, control) = cruisecontrol.into_split();

    tokio::spawn(telemetry_handler(telem, data_sender.clone(), logging_sender.clone(), auto_sender.clone()));

    controller(notes, control, data_sender.clone(), logging_sender.clone(), auto_allowed, interface).await?;
    Ok(())
}

async fn telemetry_handler(mut telem: OwnedReadHalf, gui: watch::Sender<GUIData>, logging_sender: mpsc::Sender<CombatData>, auto_sender: watch::Sender<TelemetryPacket>) -> Result<()> {
    let mut buf = vec![0; 2048];
    loop {
        let len = telem.read_u32().await.context("bad length")? as usize;
        let data = &mut buf[0..len];
        telem.read_exact(data).await?;
        let telem: TelemetryPacket = postcard::from_bytes(&data)?;

        //println!("telem: {telem:?}");

        auto_sender.send(telem.clone())?;
        let _ = logging_sender.send(CombatData::Telemetry(telem.clone())).await;

        gui.send_modify(|gui| {
            gui.telemetry = Some(telem);
        });
        GUI.get().map(|c| c.request_repaint());
    }
}

async fn controller(mut notes: broadcast::Receiver<Detection>, controller: OwnedWriteHalf, gui: watch::Sender<GUIData>, logging_sender: mpsc::Sender<CombatData>, enable: Arc<AtomicBool>, interface: AutoInterface) -> Result<()> {
    let mut controller = BufWriter::new(controller);
    //send_packet(&mut controller, ControlPacket::Arm(true)).await?;
    //println!("armed flipper");

    //let (auto_tx, mut auto_rx) = mpsc::channel(1);
    //let mut auto = None;

    //auto = Some(spawn(seek))

    let mut auto_data = interface.subscribe();

    loop {
        let mut control = ControlPacket::Stop;

        let result::Result::Ok(note) = timeout(Duration::from_millis(250), notes.recv()).await else {
            println!("timeout");
            send_packet(&mut controller, ControlPacket::Stop).await?;
            continue;
        };

        let (note,vol) = note.context("channel closed")?;
        if let Some(note) = note {
            dbg!(note);
            if let ControlFlow::Break(_) = sax_control(&mut control, vol, note, enable.clone()) {
                if let ControlFlow::Break(_) = recorder_control(&mut control, vol, note, enable.clone()) {
                    continue;
                }
            }

            if enable.load(Ordering::Acquire) && interface.enabled() {
                control = auto_data.borrow_and_update().clone();
            }

            send_packet(&mut controller, control.clone()).await?;
            let _ = logging_sender.send(CombatData::Control(control.clone())).await;
            gui.send_modify(|gui| gui.last_command = Some(control));
        } else {
            send_packet(&mut controller, ControlPacket::Twist(0.0, 0.0)).await?;
            gui.send_modify(|gui| gui.last_command = Some(ControlPacket::Stop));
        }
        GUI.get().map(|c| c.request_repaint());
    }
}

/// Weapon enabled
const ARMED: bool = true;

fn sax_control(control: &mut ControlPacket, vol: f32, frequency: f32, enable: Arc<AtomicBool>) -> ControlFlow<()> {
    if frequency < 150. {
        println!("too low");
        return ControlFlow::Break(());
    }
    if frequency > 270. {
        println!("too high");
        return ControlFlow::Break(());
    }
    let note = Note::from_freq(frequency, Tuning::EqualTemperament);
    //dbg!(note.clarity);
    //dbg!(vol);
    match note.pitch {
        Pitch { letter: NoteLetter::C, accidental: 1}  => {
            println!("forward");
            *control = ControlPacket::Twist(1.0, 0.0);
        }
        //Pitch { letter: NoteLetter::A, accidental: 1}  => {
        //    println!("backward");
        //    *control = ControlPacket::Twist(-1.0, 0.0);
        //}
        Pitch { letter: NoteLetter::B, accidental: 0}  => {
            println!("right");
            *control = ControlPacket::Twist(0.0, 1.0);
        }
        Pitch { letter: NoteLetter::A, accidental: 0}  => {
            println!("left");
            *control = ControlPacket::Twist(0.0, -1.0);
        }
        Pitch { letter: NoteLetter::G, accidental: 1}  => {
            if ARMED {
                println!("fire");
                *control = ControlPacket::Fire;
            }
        }
        Pitch { letter: NoteLetter::F, accidental: 1}  => {
            println!("stop flat");
            *control = ControlPacket::Stop;
        }
        pitch => {
            if vol > 3000. {
                println!("rly loud");
                //control = ControlPacket::FireOverride(1.0);
                dbg!(pitch);
            } else {
                dbg!(pitch);
            }
        }
    }
    ControlFlow::Continue(())
}


fn recorder_control(control: &mut ControlPacket, vol: f32, frequency: f32, enable: Arc<AtomicBool>) -> ControlFlow<()> {
    if frequency < 300. {
        println!("too low");
        return ControlFlow::Break(());
    }
    if frequency > 600. {
        println!("too high");
        return ControlFlow::Break(());
    }
    let note = Note::from_freq(frequency, Tuning::EqualTemperament);
    //dbg!(note.clarity);
    //dbg!(vol);
    match note.pitch {
        Pitch { letter: NoteLetter::A, accidental: 0}  => {
            println!("forward");
            *control = ControlPacket::Twist(1.0, 0.0);
        }
        Pitch { letter: NoteLetter::A, accidental: 1}  => {
            println!("backward");
            *control = ControlPacket::Twist(-1.0, 0.0);
        }
        Pitch { letter: NoteLetter::C, accidental: 0}  => {
            println!("right");
            *control = ControlPacket::Twist(0.0, 1.0);
        }
        Pitch { letter: NoteLetter::C, accidental: 1}  => {
            println!("left");
            *control = ControlPacket::Twist(0.0, -1.0);
        }
        Pitch { letter: NoteLetter::G, accidental: 0|1}  => {
            if ARMED {
                println!("fire");
                *control = ControlPacket::Fire;
            }
        }
        Pitch { letter: NoteLetter::F, accidental: 1}  => {
            println!("stop flat");
            *control = ControlPacket::Stop;
        }
        Pitch { letter: NoteLetter::F, accidental: 0}  => {
            println!("auto en");
            enable.store(true, Ordering::Release);
        }
        Pitch { letter: NoteLetter::E, accidental: 0}  => {
            println!("auto disable");
            enable.store(false, Ordering::Release);
        }
        pitch => {
            if vol > 3000. {
                println!("rly loud");
                //control = ControlPacket::FireOverride(1.0);
            } else {
                dbg!(pitch);
            }
        }
    }
    ControlFlow::Continue(())
}

async fn send_packet(controller: &mut BufWriter<OwnedWriteHalf>, control: ControlPacket) -> Result<(), anyhow::Error> {
    let control: heapless::Vec<u8, 2048> = postcard::to_vec(&control)?;
    controller.write_u32(control.len() as u32).await?;
    controller.write_all(&control).await?;
    controller.flush().await?;
    Ok(())
}

//async fn seek(data: TelemetryPacket) -> Result<()>{
//
//    let left_tof = data.sensors.tof1.context("missing tof1")?;
//    let right_tof = data.sensors.tof2.context("missing tof2")?;
//
//    /// Distance (mm) where both tofs point at the same thing
//    const CONVERGANCE_DISTANCE: u16 = 80;
//    const DETECT_DISTANCE: u16 = 380;
//
//
//}