#![deny(unsafe_code)]
#![no_main]
#![no_std]

// Silence certain clippy warnings
#![allow(non_upper_case_globals)]
#![allow(clippy::needless_late_init)]
#![allow(clippy::needless_return)]

mod device;

use panic_halt as _;

use cortex_m::asm::delay;
use cortex_m_rt::entry;
use stm32f1xx_hal::{
	adc,
	gpio::{Pin, Input, Analog, PullDown, Output, PushPull},
	pac,
	prelude::*,
	timer::{Channel, Tim2NoRemap, Tim2FullRemap},
	flash::{FlashWriter, FlashSize, SectorSize},
	usb::{Peripheral, UsbBus},
};

// TODO SPI

use usb_device::prelude::*;
use usbd_human_interface_device::prelude::*;

use crate::device::{CustomConfig, CustomInputReport};
use bytemuck::{bytes_of, bytes_of_mut, try_from_bytes, Pod, Zeroable};

// Set layout version
const FLASH_LAYOUT_VERSION: u16 = 0;

macro_rules! define_output_states {
	($bit:literal, $pin:ident, $output:ident, $io_pins:ident) => {
		if $output.leds & $bit == $bit {
			$io_pins.$pin.set_high();
		}
		else {
			$io_pins.$pin.set_low();
		}
	};
}

macro_rules! define_ecam_output_states_row {
	($index:literal, $pin:ident, $ecam_row:ident, $io_pins:ident) => {
					if $ecam_row[$index] == 1 {
						$io_pins.$pin.set_high();
					}
					else {
						$io_pins.$pin.set_low();
					}
	};
}

macro_rules! define_ecam_output_states_col {
	($index:literal, $pin:ident, $ecam_col:ident, $io_pins:ident) => {
					if $ecam_col[$index] == 1 {
						$io_pins.$pin.set_high();
					}
					else {
						$io_pins.$pin.set_low();
					}
	};
}

struct MyPins {
	// Analog Inputs
	pa2: Pin<'A', 2, Analog>, // Speed Brake
	pa3: Pin<'A', 3, Analog>, // Parking Brake
	pa5: Pin<'A', 5, Analog>, // Flaps
	pa6: Pin<'A', 6, Analog>, // Gravity Gear Extension
	pa7: Pin<'A', 7, Analog>, // Flood Light Pedestal
	pb0: Pin<'B', 0, Analog>, // Upper ECAM Brightness
	pb1: Pin<'B', 1, Analog>, // Lower ECAM Brightness
	pc0: Pin<'C', 0, Analog>, // Flood Light MIP
	pc1: Pin<'C', 1, Analog>, // Integ Light
	pc2: Pin<'C', 2, Analog>, // Weather Radar Tilt
	pc3: Pin<'C', 3, Analog>, // Weather Radar Gain

	// Inputs
	pa0: Pin<'A', 0, Input<PullDown>>, // Rudder Trim Reset
	pa1: Pin<'A', 1, Input<PullDown>>, // Rudder Trim Left
	pa4: Pin<'A', 4, Input<PullDown>>, // Spoiler Arm
	pa8: Pin<'A', 8, Input<PullDown>>, // Switching ATT Captain
	pa9: Pin<'A', 9, Input<PullDown>>, // Switching AIR Captain
	pa10: Pin<'A', 10, Input<PullDown>>, // Switching ATT FO
//	pb4: Pin<'B', 4, Input<PullDown>>, // Weather Radar MAP
	pb5: Pin<'B', 5, Input<PullDown>>, // Weather Radar PWS
	pb8: Pin<'B', 8, Input<PullDown>>, // Door Unlock
	pb9: Pin<'B', 9, Input<PullDown>>, // Door Lock
	pc4: Pin<'C', 4, Input<PullDown>>, // DFDR
	pc5: Pin<'C', 5, Input<PullDown>>, // AIDS
	pc6: Pin<'C', 6, Input<PullDown>>, // Switching EIS Captain
	pc9: Pin<'C', 9, Input<PullDown>>, // Switching AIR FO
	pc13: Pin<'C', 13, Input<PullDown>>, // Engine Mode Crank
	pc14: Pin<'C', 14, Input<PullDown>>, // Engine Mode Start
	pc15: Pin<'C', 15, Input<PullDown>>, // Rudder Trim Right
	pd1: Pin<'D', 1, Input<PullDown>>, // Weather Radar System 1
	pd3: Pin<'D', 3, Input<PullDown>>, // Weather Radar System 2
	pd4: Pin<'D', 4, Input<PullDown>>, // Weather Radar Multiscan
	pd5: Pin<'D', 5, Input<PullDown>>, // Weather Radar WX+T
	pd6: Pin<'D', 6, Input<PullDown>>, // Weather Radar CCS
	pd7: Pin<'D', 7, Input<PullDown>>, // Weather Radar TURB
	pd13: Pin<'D', 13, Input<PullDown>>, // Switching ECAM FO
	pd14: Pin<'D', 14, Input<PullDown>>, // Switching ECAM Captain
	pd15: Pin<'D', 15, Input<PullDown>>, // Switching EIS FO
	pe0: Pin<'E', 0, Input<PullDown>>, // Door Video
	pe5: Pin<'E', 5, Input<PullDown>>, // Engine 1 Master
	pe6: Pin<'E', 6, Input<PullDown>>, // Engine 2 Master

	// Outputs
	pb6: Pin<'B', 6, Output<PushPull>>, // Door Fault Light
	pb7: Pin<'B', 7, Output<PushPull>>, // Door Open Light
	pe1: Pin<'E', 1, Output<PushPull>>, // Engine 1 Fire
	pe2: Pin<'E', 2, Output<PushPull>>, // Engine 1 Fault
	pe3: Pin<'E', 3, Output<PushPull>>, // Engine 2 Fault
	pe4: Pin<'E', 4, Output<PushPull>>, // Engine 2 Fire

	// TODO ECAM matrix pins
}

#[derive(Pod)]
#[repr(C)]
#[derive(Copy)]
#[derive(Clone)]
#[derive(Zeroable)]
struct CalibrationData {
	min: u16,
	max: u16,
	factor: f32,
}

impl CalibrationData {
	const ADC_MAX: u16 = 4095;
	fn new (min: u16, max: u16) -> CalibrationData {
		return CalibrationData {min, max, factor: calculate_factor(min, max)};
	}
}

#[derive(Pod)]
#[repr(C)]
#[derive(Copy)]
#[derive(Clone)]
#[derive(Zeroable)]
struct Calibration {
	data: [CalibrationData; 11],
}

impl Calibration {
	const _dummy: () = {
		let size = core::mem::size_of::<Calibration>();
		assert!(size <= 1021, "Calibration too big for flash size!");
	};
	fn new () -> Calibration {
		return Calibration {
			data: [CalibrationData::new(0, CalibrationData::ADC_MAX); 11],
		};
	}
}


#[entry]
fn main() -> ! {
	// ====================== general setup =================
	// Acquire peripherals
	let p = pac::Peripherals::take().unwrap();
	let mut flash = p.FLASH.constrain();
	let rcc = p.RCC.constrain();

	// Setup GPIOA
	let mut gpioa = p.GPIOA.split();
	let mut gpiob = p.GPIOB.split();
	let mut gpioc = p.GPIOC.split();
	let mut gpiod = p.GPIOD.split();
	let mut gpioe = p.GPIOE.split();

	// configure clock
	let clocks = rcc
		.cfgr
		.use_hse(16.MHz())
		.sysclk(48.MHz())
		.pclk1(24.MHz())
		.freeze(&mut flash.acr);

	let mut afio = p.AFIO.constrain();
	let (pa15, pb3, pb4) = afio.mapr.disable_jtag(gpioa.pa15, gpiob.pb3, gpiob.pb4); // To allow us to use pa15 and pb4

	// ====================== USB setup =================
	assert!(clocks.usbclk_valid());
	let mut usb_dp = gpioa.pa12.into_push_pull_output(&mut gpioa.crh);
	usb_dp.set_low();
	delay(clocks.sysclk().raw() / 100);

	let usb = Peripheral {
		usb: p.USB,
		pin_dm: gpioa.pa11,
		pin_dp: usb_dp.into_floating_input(&mut gpioa.crh),
	};
	let usb_bus = UsbBus::new(usb);

	let mut consumer = UsbHidClassBuilder::new()
		.add_device(CustomConfig::default())
		.build(&usb_bus);

	let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x16c0, 0x27dd))
		.manufacturer("FLC Meow")
		.product("Pedestal box")
		.serial_number("01189998819991197250")
		.build();

	// ====================== ADC setup =================
	let mut adc1 = adc::Adc::adc1(p.ADC1, clocks);

	// ====================== Calibration ===============
	let mut calibration_active = false;
	let mut calibration_min_done = false;
	let mut flash_writer = flash.writer(SectorSize::Sz1K, FlashSize::Sz64K);
	let mut cal = load_calibration(&mut flash_writer);

	// ====================== Pin setup =================
	let mut io_pins = MyPins {
		// Analog Inputs
		pa2: gpioa.pa2.into_analog(&mut gpioa.crl),
		pa3: gpioa.pa3.into_analog(&mut gpioa.crl),
		pa5: gpioa.pa5.into_analog(&mut gpioa.crl),
		pa6: gpioa.pa6.into_analog(&mut gpioa.crl),
		pa7: gpioa.pa7.into_analog(&mut gpioa.crl),
		pb0: gpiob.pb0.into_analog(&mut gpiob.crl),
		pb1: gpiob.pb1.into_analog(&mut gpiob.crl),
		pc0: gpioc.pc0.into_analog(&mut gpioc.crl),
		pc1: gpioc.pc1.into_analog(&mut gpioc.crl),
		pc2: gpioc.pc2.into_analog(&mut gpioc.crl),
		pc3: gpioc.pc3.into_analog(&mut gpioc.crl),

		// Inputs
		pa0: gpioa.pa0.into_pull_down_input(&mut gpioa.crl),
		pa1: gpioa.pa1.into_pull_down_input(&mut gpioa.crl),
		pa4: gpioa.pa4.into_pull_down_input(&mut gpioa.crl),
		pa8: gpioa.pa8.into_pull_down_input(&mut gpioa.crh),
		pa9: gpioa.pa9.into_pull_down_input(&mut gpioa.crh),
		pa10: gpioa.pa10.into_pull_down_input(&mut gpioa.crh),
//		pb4: gpiob.pb4.into_pull_down_input(&mut gpiob.crl),
		pb5: gpiob.pb5.into_pull_down_input(&mut gpiob.crl),
		pb8: gpiob.pb8.into_pull_down_input(&mut gpiob.crh),
		pb9: gpiob.pb9.into_pull_down_input(&mut gpiob.crh),
		pc4: gpioc.pc4.into_pull_down_input(&mut gpioc.crl),
		pc5: gpioc.pc5.into_pull_down_input(&mut gpioc.crl),
		pc6: gpioc.pc6.into_pull_down_input(&mut gpioc.crl),
		pc9: gpioc.pc9.into_pull_down_input(&mut gpioc.crh),
		pc13: gpioc.pc13.into_pull_down_input(&mut gpioc.crh),
		pc14: gpioc.pc14.into_pull_down_input(&mut gpioc.crh),
		pc15: gpioc.pc15.into_pull_down_input(&mut gpioc.crh),
		pd1: gpiod.pd1.into_pull_down_input(&mut gpiod.crl),
		pd3: gpiod.pd3.into_pull_down_input(&mut gpiod.crl),
		pd4: gpiod.pd4.into_pull_down_input(&mut gpiod.crl),
		pd5: gpiod.pd5.into_pull_down_input(&mut gpiod.crl),
		pd6: gpiod.pd6.into_pull_down_input(&mut gpiod.crl),
		pd7: gpiod.pd7.into_pull_down_input(&mut gpiod.crl),
		pd13: gpiod.pd13.into_pull_down_input(&mut gpiod.crh),
		pd14: gpiod.pd14.into_pull_down_input(&mut gpiod.crh),
		pd15: gpiod.pd15.into_pull_down_input(&mut gpiod.crh),
		pe0: gpioe.pe0.into_pull_down_input(&mut gpioe.crl),
		pe5: gpioe.pe5.into_pull_down_input(&mut gpioe.crl),
		pe6: gpioe.pe6.into_pull_down_input(&mut gpioe.crl),

		// Outputs
		pb6: gpiob.pb6.into_push_pull_output(&mut gpiob.crl),
		pb7: gpiob.pb7.into_push_pull_output(&mut gpiob.crl),
		pe1: gpioe.pe1.into_push_pull_output(&mut gpioe.crl),
		pe2: gpioe.pe2.into_push_pull_output(&mut gpioe.crl),
		pe3: gpioe.pe3.into_push_pull_output(&mut gpioe.crl),
		pe4: gpioe.pe4.into_push_pull_output(&mut gpioe.crl),
	};

//	let mut last = get_report(&mut io_pins, &mut adc1, &cal);

	// ====================== PWM setup =================
	let c1 = pa15.into_alternate_push_pull(&mut gpioa.crh);
	let c3 = gpiob.pb10.into_alternate_push_pull(&mut gpiob.crh);
	let (mut integ_lt_pwm, mut rudder_trim_display_pwm) = p
		.TIM2
		.pwm_hz::<Tim2FullRemap, _, _>((c1, c3), &mut afio.mapr, 1.kHz(), &clocks).split();
	integ_lt_pwm.enable();
//	let mut rudder_trim_display_pwm = p
//		.TIM2
//		.pwm_hz::<Tim2FullRemap, _, _>(c3, &mut afio.mapr, 1.kHz(), &clocks);
	rudder_trim_display_pwm.enable();
	let pwm_max = integ_lt_pwm.get_max_duty(); //48000 in our case

	// ====================== Timer setup ===============
//	let timer = Instant;
//	let mut last_report_sent = timer.elapsed();

	// ====================== Main loop =================
	loop {
		let report = get_report(&mut io_pins, &mut adc1, &cal);
// TODO figure out timer and only send in like 1ms intervals or on change
//		if report != last {
			match consumer.device().write_report(&report) {
				Err(UsbHidError::WouldBlock) => {}
				Err(UsbHidError::UsbError(usb_device::UsbError::BufferOverflow)) => {
					core::panic!("Failed to write consumer report, report is too big")
				}
				Ok(_) => {
//					last = report;
				}
				Err(e) => {
					// set as indicator that this has happened
//					io_pins.pe1.set_high();
//					io_pins.pe4.set_high();
					core::panic!("Failed to write consumer report: {:?}", e)
				}
			}
//		}

		if usb_dev.poll(&mut [&mut consumer]) {
			match consumer.device().read_report() {
				Err(UsbHidError::WouldBlock) => {}
				Ok(output) => {
					// Set backlight brightness
					let pwm_val: u16;
					if output.integ_lt > pwm_max {
						pwm_val = pwm_max;
					}
					else {
						pwm_val = output.integ_lt;
					}
					integ_lt_pwm.set_duty(pwm_val);

					// LED outputs
					// ECAM
					let mut ecam_row = [0; 3];
					let mut ecam_col = [0; 6];
					// Match row and col
					if output.leds & 0x54A != 0 { ecam_row[0] = 1; }
					if output.leds & 0x1A94 != 0 { ecam_row[1] = 1; }
					if output.leds & 0x21 != 0 { ecam_row[2] = 1; }
					if output.leds & 0x6 != 0 { ecam_col[0] = 1; }
					if output.leds & 0x18 != 0 { ecam_col[1] = 1; }
					if output.leds & 0xE0 != 0 { ecam_col[2] = 1; }
					if output.leds & 0x300 != 0 { ecam_col[3] = 1; }
					if output.leds & 0xC00 != 0 { ecam_col[4] = 1; }
					if output.leds & 0x1001 != 0 { ecam_col[5] = 1; }
					// Set ECAM Out
//					define_ecam_output_states_row!(0, pb12, ecam_row, io_pins); // Row 1
//					define_ecam_output_states_row!(1, pe13, ecam_row, io_pins); // Row 2
//					define_ecam_output_states_row!(2, pe9, ecam_row, io_pins); // Row 3
//					define_ecam_output_states_col!(0, pd9, ecam_row, io_pins); // Col 1
//					define_ecam_output_states_col!(1, pc7, ecam_row, io_pins); // Col 2
//					define_ecam_output_states_col!(2, pc8, ecam_row, io_pins); // Col 3
//					define_ecam_output_states_col!(3, pd10, ecam_row, io_pins); // Col 4
//					define_ecam_output_states_col!(4, pd8, ecam_row, io_pins); // Col 5
//					define_ecam_output_states_col!(5, pb11, ecam_row, io_pins); // Col 6
					// Other Indicators
//					define_output_states!(0x2000, pb7, output, io_pins); // DOOR OPEN
//					define_output_states!(0x4000, pb6, output, io_pins); // DOOR FAULT
//					define_output_states!(0x8000, pe2, output, io_pins); // ENG 1 FAULT
//					define_output_states!(0x10000, pe1, output, io_pins); // ENG 1 FIRE
//					define_output_states!(0x20000, pe3, output, io_pins); // ENG 2 FAULT
//					define_output_states!(0x40000, pe4, output, io_pins); // ENG 2 FIRE

					// Check generic input field
					// Calibration bit
					if output.generic & 0x1 == 0x1 {
						calibration_active = true;
						if !calibration_min_done && output.generic & 0x2 == 0x2 {
							// TODO all pins
							cal.data[0].min = adc1.read(&mut io_pins.pa2).unwrap();
							cal.data[1].min = adc1.read(&mut io_pins.pa3).unwrap();
							calibration_min_done = true;
						}
					}
					else {
						if calibration_active {
							let mut values: [u16; 11] = [0; 11];
							values[0] = adc1.read(&mut io_pins.pa2).unwrap();
							values[1] = adc1.read(&mut io_pins.pa3).unwrap();
							let mut i = 0;
							loop {
								if values[i] > cal.data[i].min {
									cal.data[i].max = values[i];
								}
								else {
									cal.data[i].max = CalibrationData::ADC_MAX;
								}
								cal.data[i].factor = calculate_factor(cal.data[i].min, cal.data[i].max);
								i += 1;
								if i == values.len() {
									break;
								}
							}
							let save_success = save_calibration(&mut flash_writer, &cal);
							if save_success {
								integ_lt_pwm.set_duty(pwm_max);
							}
						}
						calibration_active = false;
						calibration_min_done = false;
					}
				}
				Err(e) => {
					core::panic!("Failed to write consumer report: {:?}", e)
				}
			}
		}

	}
}

// Calculate factor from min and max
fn calculate_factor(min: u16, max: u16) -> f32 {
	return CalibrationData::ADC_MAX as f32 / (CalibrationData::ADC_MAX - min - (CalibrationData::ADC_MAX - max)) as f32; 
}

// Returns a CustomInputReport from the inputs given
fn get_report(pins: &mut MyPins, adc1: &mut adc::Adc<pac::ADC1>, cal: &Calibration) -> CustomInputReport {
	// Read axis
	let mut values: [u16; 11] = [0; 11];
	// TODO all pins
	values[0] = adc1.read(&mut pins.pa2).unwrap();
	values[1] = adc1.read(&mut pins.pa3).unwrap();

	// Apply calibration to axis data
	let mut values_norm: [u16; 11] = [0; 11];
	let mut i = 0;
	loop {
		if values[i] < cal.data[i].min {
			values_norm[i] = 0;
		}
		else if values[i] > cal.data[i].max {
			values_norm[i] = CalibrationData::ADC_MAX;
		}
		else {
			values_norm[i] = ((values[i] - cal.data[i].min) as f32 * cal.data[i].factor) as u16;
		}
		i += 1;
		if i == values_norm.len() {
			break;
		}
	}

	// Buttons
	let mut buttons: u16 = 0;
//	if pins.pb0.is_high() { buttons += 0x1; }

	// ECAM Keyboard matrix

	CustomInputReport {
		report_id: 1,
		axis: values_norm,
		buttons,
	}
}

// Save calibration to flash
fn save_calibration(flash: &mut FlashWriter, cal: &Calibration) -> bool {
	let mut data: [u8; 1024] = [0; 1024];

	let encoded_layout_version = bytes_of(&FLASH_LAYOUT_VERSION);
	data[0..2].copy_from_slice(encoded_layout_version);

	data[2] = 1; // Calibration available bit

	let encoded_calibration_data = bytes_of(cal);
	data[3..][..encoded_calibration_data.len()].copy_from_slice(encoded_calibration_data);

	// Verify deactivation due to bug, see https://github.com/stm32-rs/stm32f1xx-hal/issues/330
	flash.change_verification(false);
	flash.erase(64512, 1024).unwrap();
	flash.change_verification(true);
	match flash.write(64512, &data) {
		Ok(_ret) => return true,
		Err(_e) => return false,
	};
}

// Load calibration to flash
fn load_calibration(flash: &mut FlashWriter) -> Calibration {
	let mut cal = Calibration::new();
	match flash.read(64512, 1023) {
		Ok(data) => {
			// Check if data is available and return early if not suitable
			if data[2] != 1 {
				return cal;
			}
			// Check if data is in compatible version and return early if not suitable
			match try_from_bytes::<u16>(&data[0..2]) {
				Ok(flash_version) => {
					if flash_version != &FLASH_LAYOUT_VERSION {
						return cal
					}
				},
				Err(_e) => {
					return cal
				},
			}
			// Load calibration data
			let dummy = bytes_of(&cal);
			let dummy2 = &data[3..][..dummy.len()];
			let dummy3 = bytes_of_mut(&mut cal);
			dummy3.copy_from_slice(dummy2);
			return cal;
		},
		Err(_e) => {
			return cal
		},
	};
}