use std::time::Duration;
use embedded_hal::{
    digital::v2::*,
};
use embedded_hal::blocking::spi::{Transfer, Write};
use anyhow::Result;
use crate::Error;

use crate::constants::*;

pub(crate) struct Interface<SPI, CS, IRQ> {
    /// SPI
    spi: SPI,
    /// CS for SPI
    cs: CS,
    /// Low for busy
    irq: IRQ,
}

impl<SPI, E, CS, IRQ> Interface<SPI, CS, IRQ>
    where
        SPI: Write<u8, Error=E> + Transfer<u8, Error=E>,
        CS: OutputPin,
        IRQ: InputPin,
{
    pub fn new(spi: SPI, cs: CS, irq: IRQ) -> Self {
        Interface {
            spi,
            cs,
            irq,
        }
    }

    pub fn execute_cmd(&mut self, command: DirectCmd) -> Result<(), Error<E>> {
        // FIXME: Aquire Lock to Protect Comms with ST25R3911
        self.cs.set_low();

        self.spi.write(&[command.addr() | OperationMode::CmdMode as u8])?; //FIXME: make this more generic

        self.cs.set_high();
        // FIXME: Release Lock to Protect Comms with ST25R3911
        Ok(())
    }

    pub fn write_register(&mut self, register: Register, value: u8) -> Result<(), Error<E>> {
        // FIXME: Aquire Lock to Protect Comms with ST25R3911
        self.cs.set_low();

        self.spi.write(&[register.addr() | OperationMode::WriteMode as u8, value])?; //FIXME: make this more generic

        self.cs.set_high();
        // FIXME: Release Lock to Protect Comms with ST25R3911
        Ok(())
    }

    pub fn read_register(&mut self, register: Register) -> Result<u8, Error<E>> {
        // FIXME: Aquire Lock to Protect Comms with ST25R3911
        self.cs.set_low();

        let mut buf = [register.addr() | OperationMode::ReadMode as u8, 0x00];

        let value = self.spi.transfer(&mut buf)?; //FIXME: make this more generic


        self.cs.set_high();
        // FIXME: Release Lock to Protect Comms with ST25R3911
        Ok(value[1])
    }

    pub fn modify_register(&mut self, register: Register, clr_mask: u8, set_mask: u8) -> Result<(), Error<E>> {
        let mut tmp = self.read_register(register)?;
        tmp &= !clr_mask;
        tmp |= set_mask;
        self.write_register(register, tmp)?;
        Ok(())
    }

    pub fn change_register_bits(&mut self, register: Register, mask: u8, value: u8) -> Result<(), Error<E>> {
        self.modify_register(register, mask, value & mask)
    }

    pub fn clr_register_bits(&mut self, register: Register, mask: u8) -> Result<(), Error<E>> {
        let tmp = self.read_register(register)? & !mask;
        self.write_register(register, tmp)
    }

    pub fn set_register_bits(&mut self, register: Register, mask: u8) -> Result<(), Error<E>> {
        let tmp = self.read_register(register)? | mask;
        self.write_register(register, tmp)
    }

    pub fn check_register(&mut self, register: Register, mask: u8, value: u8) -> Result<bool, Error<E>> {
        let tmp = self.read_register(register)?;
        Ok((tmp & mask) == value)
    }

    pub fn read_multiple_registers(&mut self, register: Register, length: u8) -> Result<Box<[u8]>, Error<E>> {
        // FIXME: Aquire Lock to Protect Comms with ST25R3911
        self.cs.set_low();

        let cmd = register.addr() | OperationMode::ReadMode as u8;

        self.spi.write(&[cmd])?; //FIXME: make this more generic

        let mut buf = vec![0; length as usize];
        let value = self.spi.transfer(&mut buf)?; //FIXME: make this more generic


        self.cs.set_high();
        // FIXME: Release Lock to Protect Comms with ST25R3911
        Ok(value.into())
    }
}