#![no_std]
#![no_main]

mod keyboard;

use calc_math::calc::StackCalc;
use calc_math::Decimal;
use core::ops::{Index, IndexMut};
use keyboard::{Debounce, KeyPress, KeyReadout, Keyboard};

use arduino_hal::{
    adc::channel::{ADC6, ADC7},
    hal::port::{
        PB0, PB1, PB2, PB3, PB4, PC0, PC1, PC2, PC3, PC4, PC5, PD2, PD3, PD4, PD5, PD6, PD7,
    },
    port::{mode::Output, Pin},
    prelude::*,
    Adc,
};
use ufmt::derive::uDebug;

// NOTE: 115200 @ 16MHz is 3.5% off, try 9600 or 1M if it causes issues (https://wormfood.net/avrbaudcalc.php)
const SERIAL_BAUD: u32 = 115200;
// Analog threshold level of key press on ADC read from KN or KO
pub const KEYBOARD_ADC_THRESHOLD: u16 = 500;
// Number of key presses to record for debounce
pub const DEBOUNCE_DEPTH: usize = 4;

#[cfg(not(doc))]
#[panic_handler]
fn panic(info: &core::panic::PanicInfo) -> ! {
    // disable interrupts - firmware has panicked so no ISRs should continue running
    // avr_device::interrupt::disable();

    // SAFETY: Main code aborted.
    let dp = unsafe { arduino_hal::Peripherals::steal() };
    let pins = arduino_hal::pins!(dp);
    let mut serial = arduino_hal::default_serial!(dp, pins, SERIAL_BAUD);

    ufmt::uwriteln!(&mut serial, "Firmware panic!\r").unwrap_infallible();
    if let Some(loc) = info.location() {
        ufmt::uwriteln!(
            &mut serial,
            "  At {}:{}:{} - {}\r",
            loc.file(),
            loc.line(),
            loc.column(),
            info.message().as_str().unwrap_or("<no message avaliable>")
        )
        .unwrap_infallible();
    }

    let mut led = pins.d13.into_output();
    loop {
        led.toggle();
        arduino_hal::delay_ms(100);
    }
}

struct SegmentPins {
    kd_seg_a: Pin<Output, PD5>,
    kd_seg_b: Pin<Output, PB0>,
    kd_seg_c: Pin<Output, PB2>,
    kd_seg_d: Pin<Output, PC5>,
    kd_seg_e: Pin<Output, PB1>,
    kd_seg_f: Pin<Output, PD6>,
    kd_seg_g: Pin<Output, PC0>,
    kd_seg_dp: Pin<Output, PC4>,
}

impl SegmentPins {
    fn new(
        kd_seg_a: Pin<Output, PD5>,
        kd_seg_b: Pin<Output, PB0>,
        kd_seg_c: Pin<Output, PB2>,
        kd_seg_d: Pin<Output, PC5>,
        kd_seg_e: Pin<Output, PB1>,
        kd_seg_f: Pin<Output, PD6>,
        kd_seg_g: Pin<Output, PC0>,
        kd_seg_dp: Pin<Output, PC4>,
    ) -> SegmentPins {
        let mut out = SegmentPins {
            kd_seg_a,
            kd_seg_b,
            kd_seg_c,
            kd_seg_d,
            kd_seg_e,
            kd_seg_f,
            kd_seg_g,
            kd_seg_dp,
        };

        out.set_off();
        out
    }

    fn set_off(&mut self) {
        self.kd_seg_a.set_high();
        self.kd_seg_b.set_high();
        self.kd_seg_c.set_high();
        self.kd_seg_d.set_high();
        self.kd_seg_e.set_high();
        self.kd_seg_f.set_high();
        self.kd_seg_g.set_high();
        self.kd_seg_dp.set_high();
    }

    fn set_a(&mut self) {
        self.kd_seg_a.set_low();
    }
    fn set_b(&mut self) {
        self.kd_seg_b.set_low();
    }
    fn set_c(&mut self) {
        self.kd_seg_c.set_low();
    }
    fn set_d(&mut self) {
        self.kd_seg_d.set_low();
    }
    fn set_e(&mut self) {
        self.kd_seg_e.set_low();
    }
    fn set_f(&mut self) {
        self.kd_seg_f.set_low();
    }
    fn set_g(&mut self) {
        self.kd_seg_g.set_low();
    }
    fn set_dp(&mut self) {
        self.kd_seg_dp.set_low();
    }
}

struct IOSelect<'p> {
    display_no: usize,
    pin: &'p mut Pin<Output>,
}

impl IOSelect<'_> {
    fn display_no(&self) -> usize {
        self.display_no
    }

    fn set_on(&mut self) {
        self.pin.set_high()
    }
    fn set_off(&mut self) {
        self.pin.set_low()
    }
}

struct IOPins([Pin<Output>; 9]);

impl IOPins {
    fn new(
        kd_d1_1c: Pin<Output, PD2>,
        kd_d2_5div: Pin<Output, PD3>,
        kd_d3_6mul: Pin<Output, PD4>,
        kd_d4_7up: Pin<Output, PC3>,
        kd_d5_8e: Pin<Output, PC2>,
        kd_d6_90: Pin<Output, PD7>,
        kd_d7_2down: Pin<Output, PC1>,
        kd_d8_3plus: Pin<Output, PB3>,
        kd_d9_4min: Pin<Output, PB4>,
    ) -> IOPins {
        IOPins([
            kd_d1_1c.downgrade(),
            kd_d2_5div.downgrade(),
            kd_d3_6mul.downgrade(),
            kd_d4_7up.downgrade(),
            kd_d5_8e.downgrade(),
            kd_d6_90.downgrade(),
            kd_d7_2down.downgrade(),
            kd_d8_3plus.downgrade(),
            kd_d9_4min.downgrade(),
        ])
    }

    fn iter_mut(&mut self) -> impl Iterator<Item = IOSelect> {
        self.0
            .iter_mut()
            .enumerate()
            .map(|(display_no, pin)| IOSelect { display_no, pin })
    }
}

#[derive(Clone, Copy, PartialEq, Eq)]
enum SegmentState {
    Num(u8, bool),
    Minus,
    Dot,
    Off,
}

impl SegmentState {
    fn apply(&self, seg: &mut SegmentPins) {
        seg.set_off();
        match self {
            SegmentState::Num(no, dp) => {
                match no {
                    0 => {
                        seg.set_a();
                        seg.set_b();
                        seg.set_c();
                        seg.set_d();
                        seg.set_e();
                        seg.set_f();
                    }
                    1 => {
                        seg.set_b();
                        seg.set_c();
                    }
                    2 => {
                        seg.set_a();
                        seg.set_b();
                        seg.set_g();
                        seg.set_e();
                        seg.set_d();
                    }
                    3 => {
                        seg.set_a();
                        seg.set_b();
                        seg.set_g();
                        seg.set_c();
                        seg.set_d();
                    }
                    4 => {
                        seg.set_f();
                        seg.set_g();
                        seg.set_b();
                        seg.set_c();
                    }
                    5 => {
                        seg.set_a();
                        seg.set_f();
                        seg.set_g();
                        seg.set_c();
                        seg.set_d();
                    }
                    6 => {
                        seg.set_a();
                        seg.set_f();
                        seg.set_g();
                        seg.set_c();
                        seg.set_d();
                        seg.set_e();
                    }
                    7 => {
                        seg.set_a();
                        seg.set_b();
                        seg.set_c();
                    }
                    8 => {
                        seg.set_a();
                        seg.set_b();
                        seg.set_c();
                        seg.set_d();
                        seg.set_e();
                        seg.set_f();
                        seg.set_g();
                    }
                    9 => {
                        seg.set_a();
                        seg.set_b();
                        seg.set_c();
                        seg.set_d();
                        seg.set_f();
                        seg.set_g();
                    }
                    _ => panic!("Can't dispaly {}", no),
                }
                if *dp {
                    seg.set_dp()
                }
            }
            SegmentState::Minus => seg.set_g(),
            SegmentState::Dot => seg.set_dp(),
            SegmentState::Off => (),
        }
    }
}

struct DispalyState([SegmentState; 9]);

impl Default for DispalyState {
    fn default() -> DispalyState {
        DispalyState([
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
        ])
    }
}

impl Index<usize> for DispalyState {
    type Output = SegmentState;

    fn index(&self, index: usize) -> &Self::Output {
        &self.0[index]
    }
}

impl IndexMut<usize> for DispalyState {
    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
        &mut self.0[index]
    }
}

impl DispalyState {
    fn set(&mut self, seg: [SegmentState; 9]) {
        self.0 = seg;
    }

    fn busy(&mut self) {
        self.set([
            SegmentState::Off,
            SegmentState::Dot,
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
            SegmentState::Off,
        ])
    }
}

#[derive(Default)]
struct NumberInput {
    minus: bool,
    significant: [u8; 5],
    minus_exponent: bool,
    exponent: [u8; 2],
    enter_exponent: bool,
    significant_pos: usize,
    exponent_pos: usize,
}

impl NumberInput {
    fn reset(&mut self) {
        *self = Self::default();
    }
    fn input_significant(&mut self, val: u8) -> Result<(), ()> {
        if val > 9 {
            panic!("Bad significatn val");
        }
        if self.significant_pos >= 5 {
            return Err(());
        }
        self.significant[self.significant_pos] = val;
        self.significant_pos += 1;
        return Ok(());
    }

    fn input_exponent(&mut self, val: u8) -> Result<(), ()> {
        if val > 9 {
            panic!("Bad exponent val");
        }
        if self.exponent_pos >= 2 {
            return Err(());
        }
        self.exponent[self.exponent_pos] = val;
        self.exponent_pos += 1;
        Ok(())
    }

    fn input(&mut self, val: u8) -> Result<(), ()> {
        if self.enter_exponent {
            self.input_exponent(val)
        } else {
            self.input_significant(val)
        }
    }

    fn toggle_enter_exponent(&mut self) {
        self.significant_pos = 0;
        self.exponent_pos = 0;
        self.enter_exponent = !self.enter_exponent;
    }

    fn toggle_minus(&mut self) {
        if self.enter_exponent {
            self.minus_exponent = !self.minus_exponent;
        } else {
            self.minus = !self.minus;
        }
    }

    fn to_decimal(&self) -> Decimal<5, u8> {
        Decimal::new(
            self.minus,
            self.significant,
            self.minus_exponent,
            self.exponent[0] * 10 + self.exponent[1],
        )
    }

    fn set_decimal(&mut self, dec: Decimal<5, u8>) {
        let (minus, significant, minus_exponent, exponent) = dec.into_parts();
        self.minus = minus;
        self.significant = significant;
        self.minus_exponent = minus_exponent;
        self.exponent[0] = exponent / 10;
        self.exponent[1] = exponent - exponent / 10;
    }
}

// Show data on segment display
trait Show {
    fn show(&self, display: &mut DispalyState);
}

impl Show for NumberInput {
    fn show(&self, display: &mut DispalyState) {
        display[0] = if self.minus {
            SegmentState::Minus
        } else {
            SegmentState::Off
        };
        display[1] = SegmentState::Num(self.significant[0], true);
        display[2] = SegmentState::Num(self.significant[1], false);
        display[3] = SegmentState::Num(self.significant[2], false);
        display[4] = SegmentState::Num(self.significant[3], false);
        display[5] = SegmentState::Num(self.significant[4], false);
        display[6] = if self.minus_exponent {
            SegmentState::Minus
        } else {
            SegmentState::Off
        };
        display[7] = SegmentState::Num(self.exponent[0], false);
        display[8] = SegmentState::Num(self.exponent[1], false);
    }
}

#[derive(uDebug)]
enum State {
    EnterSignificant,
    EnterExponent,
    EnterOperation,
    Err,
}

#[arduino_hal::entry]
fn main() -> ! {
    let dp = arduino_hal::Peripherals::take().unwrap();
    let pins = arduino_hal::pins!(dp);

    let mut serial = arduino_hal::default_serial!(dp, pins, SERIAL_BAUD);

    ufmt::uwriteln!(&mut serial, "Hello from Arduino!").unwrap_infallible();

    let mut io = IOPins::new(
        pins.d2.into_output(),
        pins.d3.into_output(),
        pins.d4.into_output(),
        pins.a3.into_output(),
        pins.a2.into_output(),
        pins.d7.into_output(),
        pins.a1.into_output(),
        pins.d11.into_output(),
        pins.d12.into_output(),
    );

    let mut seg = SegmentPins::new(
        pins.d5.into_output(),
        pins.d8.into_output(),
        pins.d10.into_output(),
        pins.a5.into_output(),
        pins.d9.into_output(),
        pins.d6.into_output(),
        pins.a0.into_output(),
        pins.a4.into_output(),
    );

    let mut adc = Adc::new(dp.ADC, Default::default());
    let input = Keyboard::new(ADC7, ADC6);
    let mut debounce = Debounce::default();
    let mut number_input = NumberInput::default();

    let mut display = DispalyState::default();
    number_input.show(&mut display);

    let mut calc = StackCalc::<f32, 2, 5, u8>::default();
    let mut state = State::EnterSignificant;

    loop {
        let mut last_key_readout: Option<(usize, KeyReadout)> = None;
        for (mut io_select, ss) in io.iter_mut().zip(display.0.iter()) {
            ss.apply(&mut seg);
            io_select.set_on();
            arduino_hal::delay_ms(1);
            io_select.set_off();
            seg.set_off();
            io_select.set_on();
            if let Some(key_readout) = input.read(&mut adc) {
                last_key_readout = Some((io_select.display_no(), key_readout));
            }
            io_select.set_off();
        }

        let key_press = last_key_readout
            .and_then(|(display_no, key_readout)| key_readout.to_keypress(display_no));
        if let Some(key) = debounce.input(key_press) {
            ufmt::uwriteln!(&mut serial, "key: {:?} state: {:?}", key, state).unwrap_infallible();
            let res = match state {
                State::EnterSignificant => match key {
                    KeyPress::C => {
                        number_input.reset();
                        calc.reset();
                        Ok(())
                    }
                    KeyPress::Num(val) => number_input.input(val),
                    KeyPress::Minus => Ok(number_input.toggle_minus()),
                    KeyPress::E => {
                        number_input.toggle_enter_exponent();
                        state = State::EnterExponent;
                        Ok(())
                    }
                    _ => Ok(()),
                },
                State::EnterExponent => match key {
                    KeyPress::C => {
                        number_input.reset();
                        calc.reset();
                        state = State::EnterSignificant;
                        Ok(())
                    }
                    KeyPress::Num(val) => number_input.input(val),
                    KeyPress::Minus => Ok(number_input.toggle_minus()),
                    KeyPress::E => {
                        number_input.toggle_enter_exponent();
                        match calc.push(number_input.to_decimal()) {
                            Ok(()) => {
                                number_input.reset();
                                state = if calc.is_full() {
                                    State::EnterOperation
                                } else {
                                    State::EnterSignificant
                                };
                                Ok(())
                            }
                            Err(_) => Err(()),
                        }
                    }
                    _ => Ok(()),
                },
                State::EnterOperation => match key {
                    KeyPress::Up => todo!(),
                    KeyPress::C => {
                        number_input.reset();
                        calc.reset();
                        state = State::EnterSignificant;
                        Ok(())
                    }
                    KeyPress::Num(_) => todo!(),
                    KeyPress::Mul => match calc.mul() {
                        Ok(dec) => {
                            number_input.set_decimal(dec);
                            state = State::EnterSignificant;
                            Ok(())
                        }
                        Err(_) => Err(()),
                    },
                    KeyPress::Div => match calc.div() {
                        Ok(dec) => {
                            number_input.set_decimal(dec);
                            state = State::EnterSignificant;
                            Ok(())
                        }
                        Err(_) => Err(()),
                    },
                    KeyPress::Plus => match calc.add() {
                        Ok(dec) => {
                            number_input.set_decimal(dec);
                            state = State::EnterSignificant;
                            Ok(())
                        }
                        Err(_) => Err(()),
                    },
                    KeyPress::Minus => match calc.sub() {
                        Ok(dec) => {
                            number_input.set_decimal(dec);
                            state = State::EnterSignificant;
                            Ok(())
                        }
                        Err(_) => Err(()),
                    },
                    KeyPress::Down => todo!(),
                    KeyPress::E => todo!(),
                },
                State::Err => match key {
                    KeyPress::C => {
                        number_input.reset();
                        calc.reset();
                        state = State::EnterSignificant;
                        Ok(())
                    }
                    _ => Ok(()),
                },
            };
            if res.is_err() {
                state = State::Err
            }

            ufmt::uwriteln!(&mut serial, "state: {:?} stack: {}", state, calc.len())
                .unwrap_infallible();
            match state {
                State::EnterSignificant | State::EnterExponent | State::EnterOperation => {
                    number_input.show(&mut display)
                }
                State::Err => todo!("display error"),
            }
        }
        arduino_hal::delay_ms(1);
    }

    // loop {
    //     led.toggle();
    //     arduino_hal::delay_ms(1000);
    //     ufmt::uwrite!(&mut serial, ".").unwrap_infallible();
    // }
}