use crate::GPIO_ERROR_MSG;
use embedded_hal::digital::OutputPin;
use embedded_hal::spi;
use embedded_hal::spi::SpiBus;

/// End the SPI operation if the result was an error.
/// This function will attempt to flush the SPI bus if the result being inspected was an error.
/// If the flush fails, the flush error will be ignored and the original error will be returned.
#[inline]
pub fn end_spi_if_err<OkT, SpiT: SpiBus>(
    slave_select: &mut impl OutputPin,
    spi: &mut SpiT,
    result: Result<OkT, <SpiT as spi::ErrorType>::Error>,
) -> Result<OkT, <SpiT as spi::ErrorType>::Error> {
    match result {
        Ok(_) => result,
        Err(_) => {
            // Ignore flush error and return the original error
            slave_select.set_high().expect(GPIO_ERROR_MSG);
            let _ = spi.flush();
            result
        },
    }
}

/// End the series of SPI operations and forward the error of the final operation if there was
/// one. Error handling:
/// * If there was an error in the SPI operation and an error flushing the bus, returns the original
///   SPI error, not the bus error.
/// * If there was not an error in the SPI operation but was an error flushing the bus, returns the
///   bus flush error.
/// * If there was an error in the SPI operation and no error flushing the bus, returns the original
///   SPI error.
#[inline]
pub fn end_spi<OkT, SpiT: SpiBus>(
    slave_select: &mut impl OutputPin,
    spi: &mut SpiT,
    result: Result<OkT, <SpiT as spi::ErrorType>::Error>,
) -> Result<OkT, <SpiT as spi::ErrorType>::Error> {
    match result {
        Ok(_) => {
            slave_select.set_high().expect(GPIO_ERROR_MSG);
            spi.flush()?;
            result
        },
        Err(_) => {
            slave_select.set_high().expect(GPIO_ERROR_MSG);
            let _ = spi.flush();
            result
        },
    }
}