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

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();
		hprintln!("Before Attenuation: {:?}", clone).unwrap();
		match self.spi.transfer(&mut clone).map_err(Error::SPI) {
			Ok(arr) => {
				hprintln!("Attenuation array: {:?}", arr).unwrap()
			},
			err => nop()
		};
		Ok(clone.clone())
	}
}

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