use embedded_hal::blocking::spi::Transfer;
use cortex_m::asm::nop;
use cortex_m_semihosting::hprintln;
use core::assert;

use crate::Error;

pub struct Attenuator<SPI> {
	spi: SPI,
	data: [u8; 4],
}

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

	pub fn set_attenuation(&mut self, att: [f32; 4]) -> Result<[u8; 4], Error<E>> {
		for i in 0..4 {
			let mut atten = att[i];
			if att[i] > 31.5 {
				atten = 31.5;
			}
			if att[i] < 0.0 {
				atten = 0.0;
			}
			self.data[i] = (atten * 2.0) as u8;
			self.data[i] = self.data[i] << 2;
		}
		let mut clone = self.data.clone();
		match self.spi.transfer(&mut clone).map_err(Error::SPI) {
			Ok(arr) => {
				Ok(self.data.clone())
			},
			err => Err(Error::AttenuatorError)
		}
	}

	pub fn set_channel_attenuation(&mut self, channel: u8, attenuation: f32) -> Result<u8, Error<E>> {
		assert!((channel < 4) && (channel >= 0));
		let mut arr: [f32; 4] = self.get_attenuation();
		arr[channel as usize] = attenuation;
		match self.set_attenuation(arr) {
			Ok(v) => Ok(v[channel as usize]),
			Err(e) => Err(e)
		}
	}

	pub fn get_channel_attenuation(&mut self, channel: u8) -> f32 {
		assert!((channel < 4) && (channel >= 0));
		(self.data[channel as usize] as f32)/8.0
	}

	pub fn get_attenuation(&mut self) -> [f32; 4] {
		[
			self.get_channel_attenuation(0),
			self.get_channel_attenuation(1),
			self.get_channel_attenuation(2),
			self.get_channel_attenuation(3),
		]
	}
}

impl<SPI, E> Transfer<u8> for Attenuator<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)
	}
}