use alloc::vec::Vec;
use sachy_fmt::{info, unwrap};

use embassy_futures::select::select4;
use embassy_rp::peripherals::DMA_CH1;
use embassy_strike_driver::{
    DetectorDriver, ZeroCopyChannel,
    drivers::rp::{AdcDriver, PwmDriver},
    traits::{AdcSource, PwmSource, TimeSource},
};
use embassy_sync::{blocking_mutex::raw::NoopRawMutex, zerocopy_channel::Channel as ZChannel};
use embassy_time::Timer;

use crate::{
    constants::BLOCK_SIZE,
    rtc::GlobalRtc,
    updates::{DetectorConfigurationHandle, UpdateConnection},
    utils::{static_alloc, try_buffer, try_static_timestamped_block_vecs},
};

#[embassy_executor::task]
pub async fn detector_task(
    adc: AdcDriver<'static, NoopRawMutex, DMA_CH1>,
    pwm: PwmDriver<'static>,
    rtc: GlobalRtc<'static>,
) {
    info!("Waiting for RTC to start running");
    while !rtc.is_ready().await {
        Timer::after_secs(2).await;
    }

    info!("Allocating detector resources");

    let blocks = unwrap!(
        try_static_timestamped_block_vecs::<BLOCK_SIZE, u16>(2),
        "Couldn't allocate block buffers"
    );

    let buf_channel: &mut ZeroCopyChannel = static_alloc(ZChannel::new(blocks));

    let (mut sender, mut receiver) = buf_channel.split();

    let config = DetectorConfigurationHandle::get_config();
    let mut detector = DetectorDriver::new(config, rtc.track_time().await, pwm, adc);
    let mut samples = unwrap!(try_buffer(64), "Failed to allocate sample buffer");
    let mut peaks = Vec::new();

    unwrap!(
        peaks.try_reserve_exact(BLOCK_SIZE),
        "Failed to allocate peaks buffer"
    );

    loop {
        detector.tune(samples.as_mut_slice()).await;

        select4(
            update_detector_config(&detector),
            detector.sample_with_zerocopy(&mut sender),
            detector.detect_with_zerocopy(
                &mut receiver,
                &mut peaks,
                UpdateConnection::transmit_update,
            ),
            detector.tick(UpdateConnection::transmit_update),
        )
        .await;

        detector.reset_timer_source(rtc).await;
    }
}

async fn update_detector_config<T, P, A>(detector: &DetectorDriver<T, P, A>)
where
    T: TimeSource,
    P: PwmSource,
    A: AdcSource,
{
    loop {
        let update = DetectorConfigurationHandle::get_updated_config().await;

        detector.set_config(update);
    }
}