#![feature(iter_collect_into)]
use std::{ops::ControlFlow, result, sync::Arc, thread::sleep, time::Duration};

use anyhow::{Context, Ok, Result};
use common::{ControlPacket, TelemetryPacket};
use cpal::traits::{DeviceTrait, HostTrait, StreamTrait};
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, mpsc, RwLock}, time::timeout};

fn main() -> Result<()> {
    // assumes pulseaudio system with f32 samples and 2204 sample packets
    dbg!(cpal::available_hosts());
    let host = cpal::default_host();
    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) = mpsc::channel(2);

    const POWER_THRESHOLD: f32 = 5.0;
    const CLARITY_THRESHOLD: f32 = 0.85;
    const PACKET_LEN: usize = 2204;

    let stream = device.build_input_stream(&config.into(), 
        move | data: &[f32], _: &_| {
            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);
            sender.blocking_send((detctor.get_pitch(data, rate.0 as usize, POWER_THRESHOLD, CLARITY_THRESHOLD),vol)).unwrap();
            
        }, 
        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 control = executor.block_on(async {
        let cruisecontrol = TcpStream::connect("169.254.1.1:1234").await?;
        println!("connected");
        cruisecontrol.set_nodelay(true)?;
        let (telem, control) = cruisecontrol.into_split();

        tokio::spawn(telemetry_handler(telem));

        Ok(control)
    })?;


    executor.block_on(controller(notes, control)).unwrap();

    drop(stream);

    Ok(())
}

async fn telemetry_handler(mut telem: OwnedReadHalf) -> 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:?}");

    }
}

async fn controller(mut notes: mpsc::Receiver<(Option<pitch_detection::Pitch<f32>>, f32)>, controller: OwnedWriteHalf) -> 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))

    loop {
        let mut control = ControlPacket::Stop;

        let result::Result::Ok(note) = timeout(Duration::from_millis(95), 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.frequency);
            if let ControlFlow::Break(_) = sax_control(&mut control, vol, &note) {
                if let ControlFlow::Break(_) = recorder_control(&mut control, vol, note) {
                    continue;
                }
            }

            send_packet(&mut controller, control).await?;
        } else {
            send_packet(&mut controller, ControlPacket::Twist(0.0, 0.0)).await?;
        }
    }
}

fn sax_control(control: &mut ControlPacket, vol: f32, note: &pitch_detection::Pitch<f32>) -> ControlFlow<()> {
    if note.frequency < 150. {
        println!("too low");
        return ControlFlow::Break(());
    }
    if note.frequency > 270. {
        println!("too high");
        return ControlFlow::Break(());
    }
    let note = Note::from_freq(note.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}  => {
            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, note: pitch_detection::Pitch<f32>) -> ControlFlow<()> {
    if note.frequency < 300. {
        println!("too low");
        return ControlFlow::Break(());
    }
    if note.frequency > 600. {
        println!("too high");
        return ControlFlow::Break(());
    }
    let note = Note::from_freq(note.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}  => {
            println!("fire");
            *control = ControlPacket::Fire;
        }
        Pitch { letter: NoteLetter::F, accidental: 1}  => {
            println!("stop flat");
            *control = ControlPacket::Stop;
        }
        Pitch { letter: NoteLetter::F, accidental: 0}  => {
            println!("stop");
            *control = ControlPacket::Stop;
        }
        Pitch { letter: NoteLetter::E, accidental: 0}  => {
            println!("stop");
            *control = ControlPacket::Stop;
            //if let result::Result::Ok(command) = auto_rx.try_recv() {
            //    control = command;
            //}
        }
        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;
//
//
//}