use atmega_hal::Peripherals;
use avr_device::interrupt;
use core::convert::Infallible;
use nb::Error::WouldBlock;

pub fn read_byte(p_addr: *const u8) -> nb::Result<u8, Infallible> {
    let dp = unsafe { Peripherals::steal() };
    let eeprom = dp.EEPROM;

    // Wait for completion of previous access
    if eeprom.eecr.read().eepe().bit_is_set() {
        return Err(WouldBlock);
    }

    interrupt::free(|_cs| {
        // Write address into EEPROM address register
        eeprom.eear.write(|w| unsafe { w.bits(p_addr as u16) });
        // Start to read from EEPROM by setting EERE
        eeprom.eecr.write(|w| w.eere().set_bit());
    });

    // Return data from EEPROM data register
    Ok(eeprom.eedr.read().bits())
}

pub fn write_byte(p_addr: *const u8, data: u8) -> nb::Result<(), Infallible> {
    let dp = unsafe { Peripherals::steal() };
    let eeprom = dp.EEPROM;

    // Wait for completion of previous access
    if eeprom.eecr.read().eepe().bit_is_set() {
        return Err(WouldBlock);
    }

    interrupt::free(|_cs| {
        // Write address into EEPROM address register
        eeprom.eear.write(|w| unsafe { w.bits(p_addr as u16) });
        // Write data into EEPROM data register
        eeprom.eedr.write(|w| unsafe { w.bits(data) });
        // Enable writing to the EEPROM by setting EEMPE
        eeprom.eecr.write(|w| w.eempe().set_bit());
        // Start to write to EEPROM by setting EEPE
        eeprom.eecr.write(|w| w.eepe().set_bit());
    });

    // Return data from EEPROM data register
    Ok(())
}