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

#include <stdint.h>

#define PI 3.14159265358979323846f
#define TWO_PI 6.28318530717958647692f

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

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();
}

volatile uint32_t drill_smo_target_status;
volatile float drill_smo_target_forward_rpm;
volatile float drill_smo_target_reverse_rpm;
volatile float drill_smo_target_angle_error_rad;

static float wrap_pi_test(float x) {
    while (x > PI) x -= TWO_PI;
    while (x < -PI) x += TWO_PI;
    return x;
}

__attribute__((noinline)) static bool run_synthetic(int direction,
                                                    float target_speed,
                                                    float *rpm,
                                                    float *angle_error) {
    const drill_smo_config c={
        .phase_resistance_ohm=0.5f,
        .phase_inductance_h=0.0005f,
        .sliding_gain_v=10.0f,
        .current_error_sat_a=0.25f,
        .bemf_filter_tau_s=0.000012f,
        .pll_kp_per_s=1800.0f,
        .pll_ki_per_s2=80000.0f,
        .min_bemf_v=0.25f,
        .max_phase_voltage_v=7.3f,
        .max_phase_current_a=2.0f,
        .max_mechanical_rpm=50000.0f,
        .max_dt_s=0.000025f,
        .lock_phase_error_rad=0.60f,
        .pole_pairs=4u,
        .lock_samples=16u,
    };
    drill_smo_state state;
    if (!drill_smo_init(&state,&c)) return false;
    const mfoc_config foc_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 foc;
    if (!mfoc_init(&foc,&foc_config)) return false;
    const float dt=0.00001f;
    const float ia=0.20f, ib=-0.10f;
    float theta=0.7f;
    drill_smo_output output={0};
    for (uint32_t n=0; n<150000u; ++n) {
        float sine,cosine;
        drill_smo_sincos(theta,&sine,&cosine);
        const float e_alpha=-target_speed*0.00030f*sine;
        const float e_beta=target_speed*0.00030f*cosine;
        const drill_smo_input input={
            .voltage_alpha_v=c.phase_resistance_ohm*ia+e_alpha,
            .voltage_beta_v=c.phase_resistance_ohm*ib+e_beta,
            .current_alpha_a=ia,
            .current_beta_a=ib,
            .dt_s=dt,
            .commanded_direction=(int8_t)direction,
        };
        if (!drill_smo_step(&state,&input,&output)) return false;
        if (output.angle_valid) {
            const float ia=0.20f;
            const float ib=-0.5f*ia+0.8660254037844386f*(-0.10f);
            const float ic=-0.5f*ia-0.8660254037844386f*(-0.10f);
            const mfoc_input foc_input={
                .phase_a_current_a=ia,
                .phase_b_current_a=ib,
                .phase_c_current_a=ic,
                .sin_electrical_angle=output.sine,
                .cos_electrical_angle=output.cosine,
                .bus_voltage_v=11.1f,
                .d_current_ref_a=0.0f,
                .q_current_ref_a=0.20f,
                .dt_s=dt,
            };
            const mfoc_output foc_output=mfoc_step(&foc,&foc_input);
            if (!foc_output.valid || foc_output.duty_a<0.0f ||
                foc_output.duty_a>1.0f || foc_output.duty_b<0.0f ||
                foc_output.duty_b>1.0f || foc_output.duty_c<0.0f ||
                foc_output.duty_c>1.0f) return false;
        }
        theta=wrap_pi_test(theta+target_speed*dt);
    }
    *rpm=output.mechanical_speed_rpm;
    *angle_error=wrap_pi_test(output.electrical_angle_rad-theta);
    return output.observer_locked && output.angle_valid;
}

int main(void) {
    drill_safe_boot();
    drill_smo_target_status |= 1u;
    float rpm=0.0f, error=0.0f;
    if (run_synthetic(1,20525.0f,&rpm,&error) &&
        rpm>47000.0f && rpm<51000.0f && error>-0.20f && error<0.20f) {
        drill_smo_target_status |= 1u<<1u;
        drill_smo_target_forward_rpm=rpm;
        drill_smo_target_angle_error_rad=error;
    }
    if (run_synthetic(-1,-18850.0f,&rpm,&error) &&
        rpm< -42000.0f && rpm> -48000.0f && error>-0.20f && error<0.20f) {
        drill_smo_target_status |= 1u<<2u;
        drill_smo_target_reverse_rpm=rpm;
    }
    for (;;) __WFI();
}
