#include "motor_foc.h"
#include "stm32g4xx.h"
#include "drill_safe_gpio.h"

#include <stdint.h>

static void wait_safe(void) { for (;;) __WFI(); }

/* Exact pin-safe initialization copied from v1.39 target main.c. No timer,
 * ADC, SPI, driver-enable, or charger initialization is added here. */
void drill_safe_boot(void) {
    const uint16_t gpio_a_outputs =
        (1u << 5u) | (1u << 7u) | (1u << 8u) |
        (1u << 9u) | (1u << 10u) | (1u << 12u);
    const uint16_t gpio_b_outputs = (1u << 0u) | (1u << 1u);
    const uint16_t gpio_c_outputs = (1u << 14u);
    const drill_safe_gpio_set safe_pins = {{
        {&GPIOA->MODER, &GPIOA->OTYPER, &GPIOA->OSPEEDR,
         &GPIOA->PUPDR, &GPIOA->BSRR, gpio_a_outputs, (1u << 12u)},
        {&GPIOB->MODER, &GPIOB->OTYPER, &GPIOB->OSPEEDR,
         &GPIOB->PUPDR, &GPIOB->BSRR, gpio_b_outputs, 0u},
        {&GPIOC->MODER, &GPIOC->OTYPER, &GPIOC->OSPEEDR,
         &GPIOC->PUPDR, &GPIOC->BSRR, gpio_c_outputs, 0u}
    }};
    RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN |
                    RCC_AHB2ENR_GPIOBEN |
                    RCC_AHB2ENR_GPIOCEN;
    (void)RCC->AHB2ENR;
    if (!drill_safe_gpio_validate(&safe_pins)) wait_safe();
    if (!drill_safe_gpio_preload(&safe_pins)) wait_safe();
    __DSB();
    if (!drill_safe_gpio_configure_outputs(&safe_pins)) wait_safe();
    __DSB();
}

/* Read by SWD after programming this diagnostic-only image. This image never
 * enables TIM1, DRV_RUN_REQ, DRV_SLEEP_N, charging, or any motor output. */
volatile uint32_t drill_foc_probe_status;
volatile float drill_foc_probe_q_voltage_v;
volatile float drill_foc_probe_duty_a;
volatile float drill_foc_probe_duty_b;
volatile float drill_foc_probe_duty_c;

__attribute__((noinline)) void drill_foc_probe_math(void)
{
    /* Numerical test envelope only. These values are not motor calibration. */
    const mfoc_config config = {
        .min_bus_voltage_v = 9.0f,
        .max_bus_voltage_v = 12.6f,
        .max_phase_current_a = 2.0f,
        .phase_sum_tolerance_a = 0.2f,
        .current_kp_v_per_a = 0.25f,
        .current_ki_v_per_a_s = 0.0f,
        .max_dt_s = 0.001f,
        .modulation_ceiling = 0.90f,
    };
    mfoc_controller controller;
    if (!mfoc_init(&controller, &config)) return;
    drill_foc_probe_status |= (1u << 0u);

    mfoc_input input = {
        .phase_a_current_a = 0.0f,
        .phase_b_current_a = 0.0f,
        .phase_c_current_a = 0.0f,
        .sin_electrical_angle = 0.0f,
        .cos_electrical_angle = 1.0f,
        .bus_voltage_v = 11.1f,
        .d_current_ref_a = 0.0f,
        .q_current_ref_a = 0.0f,
        .dt_s = 0.0001f,
    };
    mfoc_output output = mfoc_step(&controller, &input);
    if (output.valid && output.duty_a > 0.499f && output.duty_a < 0.501f &&
        output.duty_b > 0.499f && output.duty_b < 0.501f &&
        output.duty_c > 0.499f && output.duty_c < 0.501f) {
        drill_foc_probe_status |= (1u << 1u);
    }

    input.q_current_ref_a = 0.5f;
    output = mfoc_step(&controller, &input);
    drill_foc_probe_q_voltage_v = output.q_voltage_v;
    drill_foc_probe_duty_a = output.duty_a;
    drill_foc_probe_duty_b = output.duty_b;
    drill_foc_probe_duty_c = output.duty_c;
    if (output.valid && output.q_voltage_v > 0.0f &&
        output.duty_a >= 0.0f && output.duty_a <= 1.0f &&
        output.duty_b >= 0.0f && output.duty_b <= 1.0f &&
        output.duty_c >= 0.0f && output.duty_c <= 1.0f) {
        drill_foc_probe_status |= (1u << 2u);
    }

    input.phase_a_current_a = 2.1f;
    output = mfoc_step(&controller, &input);
    if (!output.valid) drill_foc_probe_status |= (1u << 3u);

    input.phase_a_current_a = 0.0f;
    input.q_current_ref_a = 0.0f;
    output = mfoc_step(&controller, &input);
    if (output.valid && output.q_voltage_v == 0.0f &&
        output.duty_a > 0.499f && output.duty_a < 0.501f &&
        output.duty_b > 0.499f && output.duty_b < 0.501f &&
        output.duty_c > 0.499f && output.duty_c < 0.501f) {
        drill_foc_probe_status |= (1u << 4u);
    }
}

int main(void)
{
    drill_safe_boot();
    drill_foc_probe_math();
    for (;;) __WFI();
}
