#![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 nb::block;

use cortex_m::asm::delay;
use cortex_m_rt::entry;
use stm32f1xx_hal::{
	adc,
	gpio::{Pin, Input, Analog, PullDown, Output, PushPull},
	pac,
	prelude::*,
	timer::{Tim2FullRemap, Timer},
	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
	pb11: Pin<'B', 11, Output<PushPull>>, // ECAM LEDs Column 6
	pb12: Pin<'B', 12, Output<PushPull>>, // ECAM LEDs Row 1
	pc7: Pin<'C', 7, Output<PushPull>>, // ECAM LEDs Column 2
	pc8: Pin<'C', 8, Output<PushPull>>, // ECAM LEDs Column 3
	pd8: Pin<'D', 8, Output<PushPull>>, // ECAM LEDs Column 5
	pd9: Pin<'D', 9, Output<PushPull>>, // ECAM LEDs Column 1
	pd10: Pin<'D', 10, Output<PushPull>>, // ECAM LEDs Column 4
	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
	pe9: Pin<'E', 9, Output<PushPull>>, // ECAM LEDs Row 3
	pe13: Pin<'E', 13, Output<PushPull>>, // ECAM LEDs Row 2

	// ECAM matrix read row
	pe12: Pin<'E', 12, Input<PullDown>>, // ECAM Keys Row 1
	pe15: Pin<'E', 15, Input<PullDown>>, // ECAM Keys Row 2
	pe11: Pin<'E', 11, Input<PullDown>>, // ECAM Keys Row 3
	pe7: Pin<'E', 7, Input<PullDown>>, // ECAM Keys Row 4

	// ECAM matrix column select
	pd11: Pin<'D', 11, Output<PushPull>>, // ECAM Keys Column 1
	pd12: Pin<'D', 12, Output<PushPull>>, // ECAM Keys Column 2
	pe8: Pin<'E', 8, Output<PushPull>>, // ECAM Keys Column 3
	pb2: Pin<'B', 2, Output<PushPull>>, // ECAM Keys Column 4
	pe10: Pin<'E', 10, Output<PushPull>>, // ECAM Keys Column 5
	pe14: Pin<'E', 14, Output<PushPull>>, // ECAM Keys Column 6

}

#[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 cp = cortex_m::Peripherals::take().unwrap();
	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: 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),
		pb11: gpiob.pb11.into_push_pull_output(&mut gpiob.crh),
		pb12: gpiob.pb12.into_push_pull_output(&mut gpiob.crh),
		pc7: gpioc.pc7.into_push_pull_output(&mut gpioc.crl),
		pc8: gpioc.pc8.into_push_pull_output(&mut gpioc.crh),
		pd8: gpiod.pd8.into_push_pull_output(&mut gpiod.crh),
		pd9: gpiod.pd9.into_push_pull_output(&mut gpiod.crh),
		pd10: gpiod.pd10.into_push_pull_output(&mut gpiod.crh),
		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),
		pe9: gpioe.pe9.into_push_pull_output(&mut gpioe.crh),
		pe13: gpioe.pe13.into_push_pull_output(&mut gpioe.crh),

		// ECAM matrix read row
		pe12: gpioe.pe12.into_pull_down_input(&mut gpioe.crh),
		pe15: gpioe.pe15.into_pull_down_input(&mut gpioe.crh),
		pe11: gpioe.pe11.into_pull_down_input(&mut gpioe.crh),
		pe7: gpioe.pe7.into_pull_down_input(&mut gpioe.crl),


		// ECAM matrix column select
		pd11: gpiod.pd11.into_push_pull_output(&mut gpiod.crh),
		pd12: gpiod.pd12.into_push_pull_output(&mut gpiod.crh),
		pe8: gpioe.pe8.into_push_pull_output(&mut gpioe.crh),
		pb2: gpiob.pb2.into_push_pull_output(&mut gpiob.crl),
		pe10: gpioe.pe10.into_push_pull_output(&mut gpioe.crh),
		pe14: gpioe.pe14.into_push_pull_output(&mut gpioe.crh),
	};

	// ====================== 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();
	rudder_trim_display_pwm.enable();
	let pwm_max = integ_lt_pwm.get_max_duty(); //48000 in our case

	// ====================== Timer setup ===============
	let mut timer = Timer::syst(cp.SYST, &clocks).counter_hz();
	timer.start(1.kHz()).unwrap();

	// ====================== Setup other vars ==========
	let mut ecam_col_select: u8 = 0;
	let mut ecam_buttons: u32 = 0; // stores the values of the keyboard matrix we can not read out right now

	// ====================== Main loop =================
	loop {
		block!(timer.wait()).unwrap();
		// Generate report
		// Read axis
		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();
		values[2] = adc1.read(&mut io_pins.pa5).unwrap();
		values[3] = adc1.read(&mut io_pins.pa6).unwrap();
		values[4] = adc1.read(&mut io_pins.pa7).unwrap();
		values[5] = adc1.read(&mut io_pins.pb0).unwrap();
		values[6] = adc1.read(&mut io_pins.pb1).unwrap();
		values[7] = adc1.read(&mut io_pins.pc0).unwrap();
		values[8] = adc1.read(&mut io_pins.pc1).unwrap();
		values[9] = adc1.read(&mut io_pins.pc2).unwrap();
		values[10] = adc1.read(&mut io_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: u32 = 0;
		if io_pins.pa0.is_high() { buttons += 0x1; }
		if io_pins.pa1.is_high() { buttons += 0x2; }
		if io_pins.pa4.is_high() { buttons += 0x4; }
		if io_pins.pa8.is_high() { buttons += 0x8; }
		if io_pins.pa9.is_high() { buttons += 0x10; }
		if io_pins.pa10.is_high() { buttons += 0x20; }
		if io_pins.pb4.is_high() { buttons += 0x40; }
		if io_pins.pb5.is_high() { buttons += 0x80; }
		if io_pins.pb8.is_high() { buttons += 0x100; }
		if io_pins.pb9.is_high() { buttons += 0x200; }
		if io_pins.pc4.is_high() { buttons += 0x400; }
		if io_pins.pc5.is_high() { buttons += 0x800; }
		if io_pins.pc6.is_high() { buttons += 0x1000; }
		if io_pins.pc9.is_high() { buttons += 0x0200; }
		if io_pins.pc13.is_high() { buttons += 0x4000; }
		if io_pins.pc14.is_high() { buttons += 0x8000; }
		if io_pins.pc15.is_high() { buttons += 0x10000; }
		if io_pins.pd1.is_high() { buttons += 0x20000; }
		if io_pins.pd3.is_high() { buttons += 0x40000; }
		if io_pins.pd4.is_high() { buttons += 0x80000; }
		if io_pins.pd5.is_high() { buttons += 0x100000; }
		if io_pins.pd6.is_high() { buttons += 0x200000; }
		if io_pins.pd7.is_high() { buttons += 0x400000; }
		if io_pins.pd13.is_high() { buttons += 0x800000; }
		if io_pins.pd14.is_high() { buttons += 0x1000000; }
		if io_pins.pd15.is_high() { buttons += 0x2000000; }
		if io_pins.pe0.is_high() { buttons += 0x4000000; }
		if io_pins.pe5.is_high() { buttons += 0x8000000; }
		if io_pins.pe6.is_high() { buttons += 0x10000000; }

		// ECAM Keyboard matrix
		let mut ecam_buttons_read: u32 = 0;

		if io_pins.pe12.is_high() { ecam_buttons_read += 0x1; }
		if io_pins.pe15.is_high() { ecam_buttons_read += 0x2; }
		if io_pins.pe11.is_high() { ecam_buttons_read += 0x4; }
		if io_pins.pe7.is_high() { ecam_buttons_read += 0x8; }

		ecam_buttons = ecam_buttons & (0xFFFFFFFF - (0xF << (ecam_col_select * 4)));
		ecam_buttons += ecam_buttons_read << (ecam_col_select * 4);

		// Pull the next column high for next time coming through in the main loop ~1ms
		if ecam_col_select == 0 {
			io_pins.pd11.set_high();
			io_pins.pe14.set_low();
		}
		else if ecam_col_select == 1 {
			io_pins.pd12.set_high();
			io_pins.pd11.set_low();
		}
		else if ecam_col_select == 2 {
			io_pins.pe8.set_high();
			io_pins.pd12.set_low();
		}
		else if ecam_col_select == 3 {
			io_pins.pb2.set_high();
			io_pins.pe8.set_low();
		}
		else if ecam_col_select == 4 {
			io_pins.pe10.set_high();
			io_pins.pb2.set_low();
		}
		else if ecam_col_select == 5 {
			io_pins.pe14.set_high();
			io_pins.pe10.set_low();
		}

		ecam_col_select += 1;
		if ecam_col_select >= 6 {
			ecam_col_select = 0
		}

		let report = CustomInputReport {
			report_id: 1,
			axis: values_norm,
			buttons,
			ecam_buttons,
		};
		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(_) => {
			}
			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_col, io_pins); // Col 1
					define_ecam_output_states_col!(1, pc7, ecam_col, io_pins); // Col 2
					define_ecam_output_states_col!(2, pc8, ecam_col, io_pins); // Col 3
					define_ecam_output_states_col!(3, pd10, ecam_col, io_pins); // Col 4
					define_ecam_output_states_col!(4, pd8, ecam_col, io_pins); // Col 5
					define_ecam_output_states_col!(5, pb11, ecam_col, 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 {
							cal.data[0].min = adc1.read(&mut io_pins.pa2).unwrap();
							cal.data[1].min = adc1.read(&mut io_pins.pa3).unwrap();
							cal.data[2].min = adc1.read(&mut io_pins.pa5).unwrap();
							cal.data[3].min = adc1.read(&mut io_pins.pa6).unwrap();
							cal.data[4].min = adc1.read(&mut io_pins.pa7).unwrap();
							cal.data[5].min = adc1.read(&mut io_pins.pb0).unwrap();
							cal.data[6].min = adc1.read(&mut io_pins.pb1).unwrap();
							cal.data[7].min = adc1.read(&mut io_pins.pc0).unwrap();
							cal.data[8].min = adc1.read(&mut io_pins.pc1).unwrap();
							cal.data[9].min = adc1.read(&mut io_pins.pc2).unwrap();
							cal.data[10].min = adc1.read(&mut io_pins.pc3).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();
							values[2] = adc1.read(&mut io_pins.pa5).unwrap();
							values[3] = adc1.read(&mut io_pins.pa6).unwrap();
							values[4] = adc1.read(&mut io_pins.pa7).unwrap();
							values[5] = adc1.read(&mut io_pins.pb0).unwrap();
							values[6] = adc1.read(&mut io_pins.pb1).unwrap();
							values[7] = adc1.read(&mut io_pins.pc0).unwrap();
							values[8] = adc1.read(&mut io_pins.pc1).unwrap();
							values[9] = adc1.read(&mut io_pins.pc2).unwrap();
							values[10] = adc1.read(&mut io_pins.pc3).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; 
}

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