//! Focused physical-route diagnostics for supported F722 boards.

use super::peripherals::{Usart2Tx, configure_usart2_pins_pa2_pa3, select_pll216_profile};
use cortex_m_rt as _;
use panic_halt as _;
use stm32f7::stm32f722;

const UART2_CLOCK_HZ: u32 = 54_000_000;
const UART2_BAUD: u32 = 115_200;
const SQUARE_WAVE_BYTE: u8 = 0x55;
const LED_TOGGLE_INTERVAL_BYTES: u16 = 11_520;

/// Continuously transmits `0x55` on the GEP-F722-HD V2's T2 pad.
///
/// This intentionally initializes only the clock tree, PA2/PA3, and USART2.
/// It does not enable RTIC, DMA, MAVLink, sensors, timers, or motor outputs.
/// The PA13 blue LED also changes state roughly once per second, proving the
/// test passed clock and UART initialization. A host at 115200 8N1 must
/// receive a continuous sequence of `55` bytes.
pub fn geprc_uart2_wire_test() -> ! {
    // SAFETY: this diagnostic installs the sole reset entry point and takes no
    // interrupt or RTIC path, so no other code can own these peripherals.
    let device = unsafe { stm32f722::Peripherals::steal() };

    select_pll216_profile(&device.RCC, &device.PWR, &device.FLASH)
        .expect("could not select F722 PLL216 clock profile");
    device
        .RCC
        .ahb1enr()
        .modify(|_, write| write.gpioaen().set_bit());
    configure_usart2_pins_pa2_pa3(&device.GPIOA);
    device
        .GPIOA
        .moder()
        .modify(|_, write| write.moder13().output());
    // The V2's blue LED is active low. Start it off so each following toggle
    // is visible; PA13 is otherwise unused by this standalone diagnostic.
    device.GPIOA.bsrr().write(|write| write.bs13().set_bit());
    let uart = Usart2Tx::new(&device.RCC, device.USART2, UART2_CLOCK_HZ, UART2_BAUD)
        .expect("could not initialize USART2");

    let mut bytes_since_led_toggle = 0;
    let mut led_is_on = false;
    loop {
        uart.write_blocking(SQUARE_WAVE_BYTE);
        bytes_since_led_toggle += 1;
        if bytes_since_led_toggle == LED_TOGGLE_INTERVAL_BYTES {
            bytes_since_led_toggle = 0;
            led_is_on = !led_is_on;
            device.GPIOA.bsrr().write(|write| {
                if led_is_on {
                    write.br13().set_bit()
                } else {
                    write.bs13().set_bit()
                }
            });
        }
    }
}