
use rp2040_hal::uart::{Enabled, UartDevice, UartPeripheral, ValidUartPinout};
use embedded_hal::serial::Write;
use heapless::spsc::Queue;

#[derive(Debug)]
pub enum LoggerError {
    OutOfMemory,
    WriteError
}

pub trait Logger
{
    fn log(&mut self, msg: &[u8]) -> Result<(), LoggerError>;
}

pub struct UartLogger<D, P, const QLEN: usize>
where
    D: UartDevice,
    P: ValidUartPinout<D>
{
    uart:        UartPeripheral<Enabled, D, P>,
    queue:       Queue<u8, QLEN>,
}


impl<D, P, const QLEN: usize> Logger for UartLogger<D, P, QLEN>
where
    D: UartDevice,
    P: ValidUartPinout<D>
{
    fn log(&mut self, msg: &[u8]) -> Result<(), LoggerError>
    {
        for c in msg.iter() {
            match self.queue.enqueue(*c) {
                Ok(_) => {},
                Err(_c) => return Err(LoggerError::OutOfMemory)
            }
        }

        self.process().map(|_| ())
    }
}


impl<D, P, const QLEN: usize> UartLogger<D, P, QLEN>
where
    D: UartDevice,
    P: ValidUartPinout<D>
{
    pub fn new(
        uart: UartPeripheral<Enabled, D, P>
        ) -> Self
    {
        UartLogger::<D, P, QLEN> {
            uart,
            queue: Queue::new()
        }
    }

    pub fn log_fatal(&mut self, msg: &[u8])
    {
        // first flush all data in the queue
        self.flush();

        // enqueue the message
        let _ = self.log(msg);

        // block until it is actually handed over to the hardware
        self.flush();
    }

    pub fn flush(&mut self)
    {
        // call process() until no more data remains in the queue
        loop {
            match self.process() {
                Ok(more_data) => if !more_data {
                    break;
                },
                Err(_) => break
            }
        }
    }

    // returns whether data remains in the queue
    pub fn process(&mut self) -> Result<bool, LoggerError>
    {
        while !self.queue.is_empty() && self.uart.uart_is_writable() {
            match self.uart.write(self.queue.dequeue().unwrap()) {
                Ok(_) => {},
                Err(_) => return Err(LoggerError::WriteError)
            }
        }

        Ok(!self.queue.is_empty())
    }
}
