use embedded_hal::blocking::spi::Transfer;
use cortex_m_semihosting::hprintln;
use crate::Error;
use core::mem::size_of;

// Bitmasks for CFG
construct_bitmask!(CFGMask; u32;
	RF_SW,		0,	4,
	LED,		4,	4,
	PROFILE,	8,	3,
	IO_UPDATE,	12,	1,
	MASK_NU,	13,	4,
	CLK_SEL0,	17,	1,
	SYNC_SEL,	18,	1,
	RST,		19,	1,
	IO_RST,		20,	1,
	CLK_SEL1,	21,	1,
	DIV,		22,	2
);

// BitMasks for CFG read
construct_bitmask!(StatusMask; u32;
	RF_SW,		0,	4,
	SMP_ERR,	4,	4,
	PLL_LOCK,	8,	4,
	IFC_MODE,	12,	4,
	PROTO_KEY,	16,	7	
);

pub struct ConfigRegister<SPI> {
	spi: SPI,
	data: u32,
}

impl<SPI, E> ConfigRegister<SPI>
where
	SPI: Transfer<u8, Error = E>
{
	pub fn new(spi: SPI) -> Self {
		ConfigRegister {
			spi,
			data: 0,
		}
	}

	/*
	 * Set configuration bits according to data field
	 * Return status
	 */
	fn set_all_configurations(&mut self) -> Result<u32, Error<E>> {
		match self.spi.transfer(&mut [
			((self.data & 0x00FF0000) >> 16) as u8,
			((self.data & 0x0000FF00) >>  8) as u8,
			((self.data & 0x000000FF) >>  0) as u8,
		]).map_err(Error::SPI) {
			Ok(arr) => Ok(
				((arr[0] as u32) << 16) |
				((arr[1] as u32) <<  8) |
				arr[2] as u32
			),
			Err(e) => Err(e),
		}
	}

	/*
	 * Set configuration bits according to supplied configs
	 * Return status
	 */	
	pub fn set_configurations(&mut self, configs: &mut[(CFGMask, u32)]) -> Result<u32, Error<E>> {
		for config in configs.into_iter() {
			config.0.set_data_by_arg(&mut self.data, config.1)
		}
		// Write all configurations at the same time
		self.set_all_configurations()
	}

	/*
	 * Return selected configuration field
	 */
	pub fn get_configuration(&mut self, config_type: CFGMask) -> u8 {
		(config_type.get_filtered_content(self.data) as u8)
	}

	/*
	 * Return status using mask
	 */
	pub fn get_status(&mut self, status_type: StatusMask) -> Result<u8, Error<E>> {
		match self.set_all_configurations() {
			Ok(val) => Ok(status_type.get_filtered_content(val) as u8),
			Err(e) => Err(e),
		}
	}

	/*
	 * Return entire status register
	 */
	pub fn get_all_status(&mut self) -> Result<u32, Error<E>> {
		return self.set_all_configurations();
	}
	
}

impl<SPI, E> Transfer<u8> for ConfigRegister<SPI>
where
	SPI: Transfer<u8, Error = E>
{
	type Error = Error<E>;

	fn transfer<'w>(&mut self, words: &'w mut [u8]) -> Result<&'w [u8], Self::Error> {
		self.spi.transfer(words).map_err(Error::SPI)
	}
}