#![no_std]
#![no_main]

use core::u16::MIN;

use cortex_m::singleton;
use defmt::info;
use defmt::{println, write};

use defmt_rtt as _;
use embedded_hal::pwm::SetDutyCycle;
use panic_probe as _;

// Alias for our HAL crate
use rp2040_hal as hal;

// A shorter alias for the Peripheral Access Crate, which provides low-level
// register access
use hal::fugit::RateExtU32;
use hal::pac;

// Some traits we need
use embedded_hal::digital::OutputPin;
use embedded_hal::digital::{InputPin, StatefulOutputPin};

/// The linker will place this boot block at the start of our program image. We
/// need this to help the ROM bootloader get our code up and running.
/// Note: This boot block is not necessary when using a rp-hal based BSP
/// as the BSPs already perform this step.
#[unsafe(link_section = ".boot_loader")]
#[used]
pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER_GENERIC_03H;

/// External high-speed crystal on the Raspberry Pi Pico board is 12 MHz. Adjust
/// if your board has a different frequency
const XTAL_FREQ_HZ: u32 = 12_000_000u32;

// below this we won't be able to turn on the transistor (0.6v)
const MIN_DUTY: u16 = 6000;

#[rp2040_hal::entry]
fn main() -> ! {
    // Grab our singleton objects
    let mut pac = pac::Peripherals::take().unwrap();
    let core = pac::CorePeripherals::take().unwrap();

    // Set up the watchdog driver - needed by the clock setup code
    let mut watchdog = hal::Watchdog::new(pac.WATCHDOG);

    // Configure the clocks
    let clocks = hal::clocks::init_clocks_and_plls(
        XTAL_FREQ_HZ,
        pac.XOSC,
        pac.CLOCKS,
        pac.PLL_SYS,
        pac.PLL_USB,
        &mut pac.RESETS,
        &mut watchdog,
    )
    .unwrap();

    let mut delay = cortex_m::delay::Delay::new(core.SYST, 133_000_000u32);
    // The single-cycle I/O block controls our GPIO pins
    let sio = hal::Sio::new(pac.SIO);

    // Set the pins to their default state
    let pins = hal::gpio::Pins::new(
        pac.IO_BANK0,
        pac.PADS_BANK0,
        sio.gpio_bank0,
        &mut pac.RESETS,
    );

    let mut adc = hal::Adc::new(pac.ADC, &mut pac.RESETS);
    let mut adc_pin = hal::adc::AdcPin::new(pins.gpio26.into_floating_input()).unwrap();
    let mut adc_fifo = adc
        .build_fifo()
        .set_channel(&mut adc_pin)
        .clock_divider(47999, 0)
        .start_paused();

    let mut pwm_slices = hal::pwm::Slices::new(pac.PWM, &mut pac.RESETS);

    let pwm = &mut pwm_slices.pwm2;
    // info!("PWM Top: {}", pwm.get_top());
    pwm.set_ph_correct();
    pwm.enable();

    let pwm_channel = &mut pwm.channel_a;
    pwm_channel.output_to(pins.gpio20);
    pwm_channel.set_duty_cycle(MIN_DUTY);

    // calibrate
    delay.delay_ms(2000);

    // let mut led_pin = pins.gpio25.into_push_pull_output();

    let mut duty = MIN_DUTY;

    let mut power = 0;

    let mut tick = 19u8;

    let mut peak_v = 0u32;

    loop {
        pwm_channel.set_duty_cycle(duty);

        delay.delay_ms(500);
        adc_fifo.clear();
        adc_fifo.resume();

        while adc_fifo.len() < 7 {
            delay.delay_us(1);
        }

        adc_fifo.pause();

        let mut samps = [0u16; 8];

        for i in 0..8 {
            samps[i] = adc_fifo.read();
        }

        let avg = avg_fifo(&samps, false);
        let voltage = (avg * 3300) / 4095;

        let duty_as_pct = (duty - MIN_DUTY) as u32 * 100;
        let duty_as_pct = duty_as_pct / (0xFFFF - MIN_DUTY) as u32;

        let new_power = (voltage * (1 + voltage / 100)) * duty_as_pct;

        tick = (tick + 1) % 20;

        if tick == 0 {
            info!(
                "{} duty, {}mV, {} power, {} peak",
                duty_as_pct, voltage, new_power, peak_v
            );
        }

        if duty_as_pct == 0 || voltage > peak_v {
            peak_v = voltage;
        }

        duty = duty.wrapping_add(1000);
        duty = MIN_DUTY.max(duty);
        // if voltage > (90 * peak_v) / 100 || new_power >= power {
        //     duty = duty.saturating_add(33);
        // } else {
        //     duty = MIN_DUTY.max(duty - 32);
        // }

        power = new_power;
    }
}

fn avg_fifo(samps: &[u16], log_all: bool) -> u32 {
    let mut avg = 0u32;
    if log_all {
        info!("{}", samps);
    }
    samps.iter().for_each(|n| {
        avg += *n as u32;
    });
    avg / (samps.len() as u32)
}