use embedded_hal::blocking::spi::Transfer;
use cortex_m_semihosting::hprintln;
use crate::Error;

/*
 * Macro builder for bit masks
 */
macro_rules! construct_bitmask {
	($collection: ident; $($name: ident, $shift: expr, $width: expr),+) => {
		#[derive(Debug, Copy, Clone)]
		pub enum $collection {
			$(
				$name,
			)*
		}

		impl $collection {
			pub(crate) fn get_width(self) -> u8 {
				match self {
					$(
						$collection::$name => $width,
					)*
				}
			}
			pub(crate) fn get_shift(self) -> u8 {
				match self {
					$(
						$collection::$name => $shift,
					)*
				}
			}
			pub(crate) fn get_bitmask(self) -> u32 {
				match self {
					$(
						$collection::$name => {
							let mut mask: u32 = 0;
							for bit in 0..$width {
								mask |= (1 << ($shift + bit));
							}
							mask
						},
					)*
				}
			}
		}
	}
}

construct_bitmask!(CFGMask;
	// Bitmasks for CFG write
	RF_SW,		0,	4,	// Reuse RF_SW for Status register
	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
	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,
		}
	}

	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),
		}
	}

	pub fn set_configurations(&mut self, configs: &mut[(CFGMask, u32)]) -> Result<u32, Error<E>> {
		for config in configs.into_iter() {
			// Erase the bits in the configuration region
			self.data &= (!config.0.get_bitmask());
			// Check validity of config data
			let shifted_config: u32 = config.1 << config.0.get_shift();
			assert_eq!(shifted_config | config.0.get_bitmask(), config.0.get_bitmask());
			// Write the configuration onto local data
			self.data |= shifted_config;
		}
		// Write all configurations at the same time
		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)
	}
}