//! # Firmware for the handcrank project.
//!
//! Implements a voltage boost regulator. Measures the input and output voltages and output current
//! using an external ADC connected via SPI to pins GP2 to GP5. The switching transistor is
//! controlled directly via a PWM signal on pin GP10.

#![no_std]
#![no_main]

use embedded_hal::digital::v2::OutputPin;

// Ensure we halt the program on panic (if we don't mention this crate it won't
// be linked)
use panic_halt 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::pac;

// SPI + DMA
use hal::dma::DMAExt;

// UART related types
use hal::uart::{DataBits, StopBits, UartConfig};

use heapless::String;

// Some traits we need
//use cortex_m::singleton;
use fugit::MicrosDurationU32;
use embedded_hal::PwmPin;
use fugit::RateExtU32;
use hal::clocks::Clock;
use hal::timer::Alarm;
use hal::timer::ScheduleAlarmError;
use pac::interrupt;
use core::cell::RefCell;
use critical_section::Mutex;

mod ext_adc;
mod logger;
mod switch_control;

use logger::UartLogger;
use logger::Logger;

/// 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.
#[link_section = ".boot2"]
#[used]
pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER_GENERIC_03H;

const SYSTICK_INTERVAL_US: MicrosDurationU32 = MicrosDurationU32::millis(1);

static mut SYSTICK_ALARM: Mutex<RefCell<Option<hal::timer::Alarm0>>> = Mutex::new(RefCell::new(None));

// Flag that is set by the alarm interrupt
static mut SYSTICK_FLAG: bool = false;

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

const SWITCH_PWM_MAX: i32 = 512;
const SWITCH_PWM_LIMIT: i32 = 93 * SWITCH_PWM_MAX / 100;

fn convert_adc_measurements(raw: &[u16; 4]) -> (i32, i32, i32)
{
    let iout = (raw[0] as i32 - raw[1] as i32) * 3300 * 20 / 4096 / 28; // *20 = division by 50 mΩ shunt
    let vout = raw[2] as i32 * 3300 * (47 + 10) / 10 / 4096;
    let vin = raw[3] as i32 * 3300 * (10 + 10) / 10 / 4096;

    (vin, vout, iout) /* units: mV, mV, mA */
}

/// Entry point to our bare-metal application.
///
/// The `#[rp2040_hal::entry]` macro ensures the Cortex-M start-up code calls this function
/// as soon as all global variables and the spinlock are initialised.
///
/// The function configures the RP2040 peripherals, then fades the LED in an
/// infinite loop.
#[rp2040_hal::entry]
fn main() -> ! {
    // Grab our singleton objects
    let mut pac = pac::Peripherals::take().unwrap();

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

    // Configure the clocks
    //
    // The default is to generate a 125 MHz system clock
    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,
    )
    .ok()
    .unwrap();

    // The single-cycle I/O block controls our GPIO pins
    let sio = hal::Sio::new(pac.SIO);

    // Set the pins up according to their function on this particular board
    let pins = hal::gpio::Pins::new(
        pac.IO_BANK0,
        pac.PADS_BANK0,
        sio.gpio_bank0,
        &mut pac.RESETS,
    );

    // The delay object lets us wait for specified amounts of time (in
    // milliseconds)
    //let mut delay = cortex_m::delay::Delay::new(core.SYST, clocks.system_clock.freq().to_Hz());

    // UART
    let uart_pins = (
        // UART TX (characters sent from RP2040) on pin 1 (GPIO0)
        pins.gpio0.into_mode::<hal::gpio::FunctionUart>(),
        // UART RX (characters received by RP2040) on pin 2 (GPIO1)
        pins.gpio1.into_mode::<hal::gpio::FunctionUart>(),
    );
    let uart = hal::uart::UartPeripheral::new(pac.UART0, uart_pins, &mut pac.RESETS)
        .enable(
            UartConfig::new(9600.Hz(), DataBits::Eight, None, StopBits::One),
            clocks.peripheral_clock.freq(),
        )
        .unwrap();

    // initialize the logger
    let mut logger: UartLogger<_, _, 256> = UartLogger::new(uart);
    logger.log(b"Logging initialized.\r\n").unwrap();

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

    // Configure switch PWM
    let pwm5 = &mut pwm_slices.pwm5;
    pwm5.set_ph_correct();
    pwm5.set_div_int(4);
    pwm5.set_div_frac((88 << 4) / 100);
    pwm5.set_top(SWITCH_PWM_MAX as u16);
    pwm5.enable();

    // set up switch PWM output
    // note:
    // duty cycle = 0 => output voltage = input voltage
    // duty cycle = 512 (100%) => input short circuit
    //
    // general: D = duty cycle (0..1)
    // => output voltage = 1/(1-D) * input voltage
    let pwr_switch_ch = &mut pwm5.channel_a;
    pwr_switch_ch.set_duty(0); // no output by default
    pwr_switch_ch.output_to(pins.gpio10);

    // LED pins
    let mut pin_led_r = pins.gpio15.into_push_pull_output();
    let mut pin_led_y = pins.gpio13.into_push_pull_output();
    let mut pin_led_g = pins.gpio14.into_push_pull_output();

    // off by default
    pin_led_r.set_low().unwrap();
    pin_led_y.set_low().unwrap();
    pin_led_g.set_low().unwrap();

    // SPI CS pin is controlled by software
    let mut spi_cs = pins.gpio5.into_push_pull_output();
    spi_cs.set_high().unwrap();

    // These are implicitly used by the spi driver if they are in the correct mode
    let _spi_sclk = pins.gpio2.into_mode::<hal::gpio::FunctionSpi>();
    let _spi_mosi = pins.gpio3.into_mode::<hal::gpio::FunctionSpi>();
    let _spi_miso = pins.gpio4.into_mode::<hal::gpio::FunctionSpi>();

    // Initialize DMA.
    let dma = pac.DMA.split(&mut pac.RESETS);

    let mut adc_ctrl = ext_adc::AdcContext::init(
        pac.SPI0,
        clocks.peripheral_clock.freq(),
        &mut pac.RESETS,
        dma.ch0,
        dma.ch1,
        spi_cs
        );

    logger.log(b"Initialization complete!\r\n").unwrap();

    // initialize the timer
    let mut timer = hal::Timer::new(pac.TIMER, &mut pac.RESETS);

    // set up an alarm that is used for a periodic main loop execution
    let mut next_systick_instant = timer.get_counter() + MicrosDurationU32::millis(1000);

    let mut systick_alarm = timer.alarm_0().unwrap();

    systick_alarm.schedule_at(next_systick_instant).unwrap();
    systick_alarm.enable_interrupt();

    unsafe {
        // move the alarm object so the IRQ can access it
        critical_section::with(|cs| {
            SYSTICK_ALARM.borrow(cs).replace(Some(systick_alarm));
        });

        // Unmask the timer0 IRQ so that it will generate an interrupt
        pac::NVIC::unmask(pac::Interrupt::TIMER_IRQ_0);
    }

    let mut switchctrl = switch_control::SwitchControl::<1>::new(12000, 100, SWITCH_PWM_LIMIT);

    // track nano_joules (== nanoVoltAmpereSeconds) generated, for LED flashing
    let mut nano_joules: u32 = 0;
    let mut loopcnt_led_y_off = 0;

    let mut loopcnt: u64 = 0;

    pin_led_r.set_high().unwrap();

    // main loop
    loop {
        // only execute the main loop if the systick has expired
        let mut systick_received = false;

        critical_section::with(|_cs| unsafe {
            systick_received = core::ptr::read_volatile(&SYSTICK_FLAG);
        });

        if systick_received {
            // re-shedule the systick alarm
            critical_section::with(|_cs| unsafe {
                core::ptr::write_volatile(&mut SYSTICK_FLAG, false);
            });

            next_systick_instant += SYSTICK_INTERVAL_US;

            critical_section::with(|cs| {
                unsafe {
                    if let Some(mut systick_alarm) = SYSTICK_ALARM.borrow(cs).take() {
                        let result = systick_alarm.schedule_at(next_systick_instant);
                        match result {
                            Ok(_) => {}, // just continue
                            Err(e) => {
                                match e {
                                    ScheduleAlarmError::AlarmTooLate => logger.log_fatal(b"Systick: alarm too late.\r\n"),
                                };
                            }
                        }

                        SYSTICK_ALARM.borrow(cs).replace(Some(systick_alarm));
                    } else {
                        logger.log_fatal(b"Systick: object not set when it should be!\r\n");
                    }
                }
            });


            let mut adc_value: [u16; 4] = [0; 4];

            (adc_ctrl, adc_value[0]) = adc_ctrl.sample_adc_channel(0);
            (adc_ctrl, adc_value[1]) = adc_ctrl.sample_adc_channel(1);
            (adc_ctrl, adc_value[2]) = adc_ctrl.sample_adc_channel(2);
            (adc_ctrl, adc_value[3]) = adc_ctrl.sample_adc_channel(3);

            let (vin, vout, iout) = convert_adc_measurements(&adc_value);

            // Update the PWM value for the switch
            let pwmval = switchctrl.update(vin, vout, iout);

            pwr_switch_ch.set_duty(pwmval as u16);

            // Yellow LED flashes for 10 ms whenever 1 Joule has been generated.
            // This means that it flashes once per second when output power is 1 Watt, and 3 times
            // per second if output power is 3 Watt.
            let nano_joules_generated = vout * iout; // * 1 millisecond, implicitely
            if nano_joules_generated > 0 {
                nano_joules += nano_joules_generated as u32;
            }

            // 1000 mV * 1000 mA * 1000 ms = 1 Joule
            if nano_joules >= 1000000000 {
                pin_led_y.set_high().unwrap();
                loopcnt_led_y_off = loopcnt + 10;
                nano_joules -= 1000000000;
            }

            if loopcnt == loopcnt_led_y_off {
                pin_led_y.set_low().unwrap();
            }

            // Green LED is on in constant-voltage mode and off in MPPT mode.
            match switchctrl.get_control_mode() {
                switch_control::ControlMode::ConstantVoltage  => {pin_led_g.set_high().unwrap()},
                switch_control::ControlMode::MPPT             => {pin_led_g.set_low().unwrap()},
                switch_control::ControlMode::ImpedanceControl => {pin_led_g.set_low().unwrap()},
            }

            // do not output status data every loop
            match loopcnt % 500 {
                1 => {
                    let mut data: String<16>;

                    logger.log(b"Vin: ").unwrap();
                    data = String::from(vin);
                    logger.log(data.as_bytes()).unwrap();
                    logger.log(b"mV - Vout: ").unwrap();
                    data = String::from(vout);
                    logger.log(data.as_bytes()).unwrap();
                    logger.log(b"mV - Iout: ").unwrap();
                    data = String::from(iout);
                    logger.log(data.as_bytes()).unwrap();
                    logger.log(b"mA - Pout: ").unwrap();
                    data = String::from(vout * iout / 1000);
                    logger.log(data.as_bytes()).unwrap();

                    logger.log(b"mW - raw: [").unwrap();

                    for i in 0..adc_value.len() {
                        data = String::from(adc_value[i]);
                        logger.log(data.as_bytes()).unwrap();

                        if i < 3 {
                            logger.log(b", ").unwrap();
                        }
                    }

                    logger.log(b"]; PWM: ").unwrap();
                    data = String::from(pwmval);
                    logger.log(data.as_bytes()).unwrap();

                    logger.log(b"\r\n").unwrap();
                },
                2 => {
                    switchctrl.log_status(&mut logger);
                },
                _ => {}
            }

            loopcnt += 1;
        }

        logger.process().unwrap();

        // save some power as long as no interrupt occurs
        // FIXME: not working yet for some reason
        //cortex_m::asm::wfi();
    }
}

#[interrupt]
fn TIMER_IRQ_0() {
    critical_section::with(|cs| {
        unsafe {
            core::ptr::write_volatile(&mut SYSTICK_FLAG, true);

            if let Some(mut systick_alarm) = SYSTICK_ALARM.borrow(cs).take() {
                systick_alarm.clear_interrupt();

                SYSTICK_ALARM.borrow(cs).replace(Some(systick_alarm));
            }
        }
    });
}
