use std::error::Error;
use std::io;
use std::thread;
use std::time::Duration;
use embedded_hal::serial::{Read, Write};
use esp_idf_hal::serial::{self, Rx, Tx};

#[derive(Debug)]
pub enum SerialError {
    ReadError(String),
    WriteError(String),
    TimeoutError,
}

impl Error for SerialError {}

impl std::fmt::Display for SerialError {
    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
        write!(f, "{:?}", self)
    }
}

pub type Result<T> = nb::Result<T, SerialError>;

pub struct SerialIO<UART: serial::Uart> {
    pub rx: Rx<UART>,
    pub tx: Tx<UART>,
}

impl<UART: serial::Uart> SerialIO<UART> {
    pub fn new(tx: Tx<UART>, rx: Rx<UART>) -> Self {
        Self { rx, tx }
    }

    pub fn write_bytes(&mut self, payload: &[u8]) -> Result<usize> {
        let mut num_bytes = 0;
        for b in payload.iter() {
            self.tx.write(*b)
                .map_err(|err| SerialError::WriteError(
                        format!("Error writing in serial port ({:?})", err)))?;
            num_bytes += 1;
        }
        if num_bytes == payload.len() {
            Ok(num_bytes)
        }
        else {
            Err(nb::Error::Other(
                SerialError::WriteError(
                    "Written bytes shorter than payload length (write_bytes)".to_string()
                )
            ))
        }
    }

    fn read_bytes(&mut self, buf: &mut [u8]) -> Result<usize> {
        let mut started_reading = false;
        let mut count = 0;
        let mut retries = 0;

        loop {
            match self.rx.read() {
                Ok(b) => {
                    started_reading = true;
                    if count < buf.len() {
                        buf[count] = b;
                        count += 1;
                    }
                    else { break }
                },
                Err(nb::Error::WouldBlock) => {
                    if started_reading || retries > READ_MAX_RETRIES { break }
                    else {
                        thread::sleep(Duration::from_millis(READ_WAIT_TIME));
                        retries += 1;
                    }
                },
                Err(nb::Error::Other(err)) => println!("Serial read error :: {:?}", err),
            }
        };
        Ok(count)
    }
}

const READ_MAX_RETRIES: usize = 5;
const READ_WAIT_TIME: u64 = 50;

impl<UART: serial::Uart> io::Read for SerialIO<UART> {
    fn read(&mut self, buf: &mut [u8]) -> io::Result<usize> {
        let count = nb::block!(self.read_bytes(buf))
            .map_err(|_| io::Error::from(io::ErrorKind::Other))?;
        Ok(count)
    }
}

impl<UART: serial::Uart> io::Write for SerialIO<UART> {
    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
        nb::block!(self.write_bytes(buf))
            .map_err(|_| io::Error::from(io::ErrorKind::Other))
    }

    fn flush(&mut self) -> io::Result<()> {
        self.tx.flush()
            .map_err(|_| io::Error::from(io::ErrorKind::Other))
    }
}