#![no_main]
#![no_std]

use panic_semihosting as _;

use stm32h7xx_hal::hal::digital::v2::{
	InputPin,
	OutputPin,
};
use stm32h7xx_hal::{pac, prelude::*, spi};

use cortex_m;
use cortex_m::asm::nop;
use cortex_m_rt::entry;
use cortex_m_semihosting::hprintln;

use core::ptr;
use nb::block;

use firmware;
use firmware::{
	CPLD,
	attenuator::Attenuator,
	config_register::{
		ConfigRegister,
		CFGMask,
		StatusMask,
	},
	dds::DDS,
};

#[entry]
fn main() -> ! {

	let cp = cortex_m::Peripherals::take().unwrap();
	let dp = pac::Peripherals::take().unwrap();

	let pwr = dp.PWR.constrain();
	let vos = pwr.freeze();

	let rcc = dp.RCC.constrain();
	let ccdr = rcc
		.sys_ck(400.mhz())
		.pll1_q_ck(48.mhz())
		.freeze(vos, &dp.SYSCFG);
	
	let mut delay = cp.SYST.delay(ccdr.clocks);

	let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA);
	let gpiob = dp.GPIOB.split(ccdr.peripheral.GPIOB);
	let gpioc = dp.GPIOC.split(ccdr.peripheral.GPIOC);
	let gpiod = dp.GPIOD.split(ccdr.peripheral.GPIOD);
	let gpioe = dp.GPIOE.split(ccdr.peripheral.GPIOE);
	let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF);

	// Setup CDONE for checking
	let fpga_cdone = gpiod.pd15.into_pull_up_input();

	match fpga_cdone.is_high() {
		Ok(true)	=> hprintln!("FPGA is ready."),
		Ok(_)		=> hprintln!("FPGA is in reset state."),
		Err(_)		=> hprintln!("Error: Cannot read C_DONE"),
	}.unwrap();

	/*
	 *	Using SPI1, AF5
	 *  SCLK -> PA5
	 *	MOSI -> PB5
	 *	MISO -> PA6
	 *	CS -> 0: PB12, 1: PA15, 2: PC7
	 */

	let sclk = gpioa.pa5.into_alternate_af5();
	let mosi = gpiob.pb5.into_alternate_af5();
	let miso = gpioa.pa6.into_alternate_af5();


	let (mut cs0, mut cs1, mut cs2) = (
		gpiob.pb12.into_push_pull_output(),
		gpioa.pa15.into_push_pull_output(),
		gpioc.pc7.into_push_pull_output(),
	);

	/*
	 *	I/O_Update -> PB13
	 */
	let mut io_update = gpiob.pb15.into_push_pull_output();

	let mut spi = dp.SPI1.spi(
		(sclk, miso, mosi),
		spi::MODE_0,
		3.mhz(),
		ccdr.peripheral.SPI1,
		&ccdr.clocks,
	);

	// debug led
	let mut yellow = gpioe.pe1.into_push_pull_output();
	yellow.set_high().unwrap();
	
/*
	let mut switch = CPLD::new(spi, (cs0, cs1, cs2));
	let parts = switch.split();

	let mut config = ConfigRegister::new(parts.spi1);
	let mut att = Attenuator::new(parts.spi2);
	let mut dds0 = DDS::new(parts.spi4);

	loop {
		let mut counter = config.get_status(StatusMask::RF_SW).unwrap();
		hprintln!("{}", counter);
		config.set_configurations(&mut [
			(CFGMask::RF_SW, ((counter + 1)%16) as u32)
		]).unwrap();
	}
*/

	cs0.set_low().unwrap();
	cs1.set_low().unwrap();
	cs2.set_low().unwrap();

	io_update.set_low().unwrap();

	let mut dummy :[u8;1] = [0];
	spi.transfer(&mut dummy);

	// Master reset DDS_0 through CPLD, with LED at 3
	cs0.set_high().unwrap();
	spi.transfer(&mut [
		0x08, 0x00, 0x03
	]).unwrap();
	cs0.set_low().unwrap();

	// Perform I/O Reset through CPLD, with LED at 4
	cs0.set_high().unwrap();
	spi.transfer(&mut [
		0x10, 0x00, 0x04
	]).unwrap();
	cs0.set_low().unwrap();

	// Release reset, control I/O Update through EEM
	cs0.set_high().unwrap();
	spi.transfer(&mut [
		0x00, 0x00, 0x0A
	]).unwrap();
	cs0.set_low().unwrap();

	cs0.set_low().unwrap();
	cs1.set_low().unwrap();
	cs2.set_high().unwrap();
	
	hprintln!("{:?}", spi.transfer(&mut [
		0x00, 0x00, 0x00, 0x00, 0x02
	]).unwrap()).unwrap();

	io_update.set_high().unwrap();
	delay.delay_ms(1_u16);
	io_update.set_low().unwrap();

	hprintln!("{:?}", spi.transfer(&mut [
		0x80, 0x00, 0x00, 0x00, 0x00
	]).unwrap()).unwrap();
	

	loop {}
}