#![no_std]
#![no_main]
#![warn(clippy::pedantic)]

const MAX_ANALOG_VALUE: u32 = 1023;
const R_BIG: u32 = 98700;
const R_MEDIUM: u32 = 9_800;
const R_SMALL: u32 = 1_173;
const R_SMALLEST: u32 = 99;

use arduino_hal::adc::AdcSettings;
use panic_halt as _;
#[cfg(debug_assertions)]
use ufmt::uwrite;
use ufmt::uwriteln;

use heapless::Vec;

#[arduino_hal::entry]
fn main() -> ! {
    let mut stack = Vec::<u32, 10>::new();
    let dp = arduino_hal::Peripherals::take().unwrap();
    let pins = arduino_hal::pins!(dp);
    let mut serial = arduino_hal::default_serial!(dp, pins, 9600);

    let mut adc = arduino_hal::Adc::new(dp.ADC, AdcSettings::default());

    let a0 = pins.a0.into_analog_input(&mut adc);
    let mut relay1 = pins.d2.into_output();
    let mut relay2 = pins.d3.into_output();
    let mut relay3 = pins.d4.into_output();

    #[cfg(debug_assertions)]
    macro_rules! log {
        () => {
            uwwriteln!(&mut serial).unwrap();
        };
        ($($arg:tt)*) => {{
            uwrite!(&mut serial, "INFO: ").unwrap();
            uwriteln!(&mut serial, $($arg)*).unwrap();
        }};
    }

    #[cfg(not(debug_assertions))]
    macro_rules! log {
        () => {
            ()
        };
        ($($arg:tt)*) => {{
            ()
        }};
    }

    let mut r1 = R_BIG;
    loop {
        log!("Start of loop!");
        log!("Read sensor value");
        let sensor = a0.analog_read(&mut adc);
        log!("sensor = {}", sensor);

        let r2;
        if let Some(div) = (r1 * MAX_ANALOG_VALUE).checked_div(u32::from(sensor)) {
            r2 = div - r1;
        } else {
            relay1.set_low();
            relay1.set_low();
            r1 = R_BIG;
            continue;
        }

        stack.push(r2).unwrap();

        if stack.is_full() {
            log!("Stack is full!");

            log!("Add up stack");
            let sum: u32 = stack.iter().sum();
            let average = sum / 10;
            log!("average = {}", average);

            if r1.abs_diff(average) > R_MEDIUM.abs_diff(average) {
                log!("{} > {}", r1.abs_diff(average), R_MEDIUM.abs_diff(average));
                log!("Switching to medium resistance.");
                r1 = R_MEDIUM;
                log!("r1 = {}", r1);
                relay1.set_high();
                arduino_hal::delay_ms(1000);
            } else if r1.abs_diff(average) > R_SMALL.abs_diff(average) {
                log!("{} > {}", r1.abs_diff(average), R_SMALL.abs_diff(average));
                log!("Switching to small resistance.");
                r1 = R_SMALL;
                log!("r1 = {}", r1);
                relay1.set_high();
                relay2.set_high();
                arduino_hal::delay_ms(1000);
            } else if r1.abs_diff(average) > R_SMALLEST.abs_diff(average) {
                log!(
                    "{} > {}",
                    r1.abs_diff(average),
                    R_SMALLEST.abs_diff(average)
                );
                log!("Switching to smallest resistance.");
                r1 = R_SMALLEST;
                log!("r1 = {}", r1);
                relay1.set_high();
                relay2.set_high();
                relay3.set_high();
                arduino_hal::delay_ms(1000);
            } else {
                log!("Already perfect!");
                log!("Printing value!");
                log!("r1 = {}", r1);
                relay1.set_low();
                relay2.set_low();
                relay3.set_low();
                arduino_hal::delay_ms(1000);
                uwriteln!(
                    &mut serial,
                    "RESULT: R2 = {} Ohm. Tested with {} Ohm resistance.\n",
                    average,
                    r1
                )
                .unwrap();
                r1 = R_BIG;
                log!("Resetting to big resistance.");
                log!("r1 = {}", r1);
            }

            stack.clear();
        }
    }
}