#![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},
	flash::{FlashWriter, FlashSize, SectorSize},
	usb::{Peripheral, UsbBus},
};

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 {
	pa1: Pin<'A', 1, Analog>,
	pa2: Pin<'A', 2, Analog>,
	pb0: Pin<'B', 0, Output<PushPull>>,
	pb12: Pin<'B', 12, Output<PushPull>>,
	pc1: Pin<'C', 1, Input<PullDown>>,
	pc3: Pin<'C', 3, Analog>,
	pc15: Pin<'C', 15, Input<PullDown>>,
}

#[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);

	// ====================== 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("01189998819991197253")
		.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 {
		pa1: gpioa.pa1.into_analog(&mut gpioa.crl),
		pa2: gpioa.pa2.into_analog(&mut gpioa.crl),
		pb0: gpiob.pb0.into_push_pull_output(&mut gpiob.crl),
		pb12: gpiob.pb12.into_push_pull_output(&mut gpiob.crh),
		pc1: gpioc.pc1.into_pull_down_input(&mut gpioc.crl),
		pc3: gpioc.pc3.into_analog(&mut gpioc.crl),
		pc15: gpioc.pc15.into_pull_down_input(&mut gpioc.crh),
	};

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

	// ====================== PWM setup =================
	let mut afio = p.AFIO.constrain();
	let c1 = gpioa.pa0.into_alternate_push_pull(&mut gpioa.crl);
	let mut integ_lt_pwm = p
		.TIM2
		.pwm_hz::<Tim2NoRemap, _, _>(c1, &mut afio.mapr, 1.kHz(), &clocks);
	integ_lt_pwm.enable(Channel::C1);
	//TODO
//	let c3 = gpiob.pb10.into_alternate_push_pull(&mut gpiob.crh);
//	let mut rudder_trim_display_pwm = p
//		.TIM2
//		.pwm_hz::<Tim2NoRemap, _, _>(c3, &mut afio.mapr, 1.kHz(), &clocks);
//	rudder_trim_display_pwm.enable(Channel::C3);
	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(Channel::C1, 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

					// Check generic input field
					// Calibration bit
					if output.generic & 0x1 == 0x1 {
						calibration_active = true;
						if !calibration_min_done && output.generic & 0x2 == 0x2 {
							cal.data[0].min = adc1.read(&mut io_pins.pa1).unwrap();
							cal.data[1].min = adc1.read(&mut io_pins.pa2).unwrap();
							calibration_min_done = true;
						}
					}
					else {
						if calibration_active {
							let mut values: [u16; 2] = [0; 2];
							values[0] = adc1.read(&mut io_pins.pa1).unwrap();
							values[1] = adc1.read(&mut io_pins.pa2).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(Channel::C1, 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];
	values[0] = adc1.read(&mut pins.pa1).unwrap();
	values[1] = adc1.read(&mut pins.pc3).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; }

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