#include "stm32g4xx.h"
#include "drill_safe_gpio.h"
#include "drill_clock.h"
#include "drill_smo.h"
#include "motor_foc.h"
#include <stdint.h>
_Noreturn static void wait_safe(void) {
    for (;;) __WFI();
}

void drill_safe_boot(void) {
    /* PF1 is OSC_OUT: this image requires cold-reset HSI operation.
     * Reject HSE-enabled entry before changing the shared pad function. */
    if (RCC->CR & (RCC_CR_HSEON | RCC_CR_HSEBYP)) wait_safe();
    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_f_outputs = (1u << 1u);
    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},
        {&GPIOF->MODER, &GPIOF->OTYPER, &GPIOF->OSPEEDR,
         &GPIOF->PUPDR, &GPIOF->BSRR, gpio_f_outputs, 0u}
    }};

    RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN |
                    RCC_AHB2ENR_GPIOBEN |
                    RCC_AHB2ENR_GPIOFEN;
    (void)RCC->AHB2ENR;
    if (!drill_safe_gpio_validate(&safe_pins)) wait_safe();

    /* BSRR is written before MODER so no active pulse occurs on mode change. */
    if (!drill_safe_gpio_preload(&safe_pins)) wait_safe();
    __DSB();
    if (!drill_safe_gpio_configure_outputs(&safe_pins)) wait_safe();
    __DSB();
}


/* Isolated, default hard-off calculation diagnostic. No sample comes from
 * ADC and no mathematical duty is written to TIM1 or GPIO. */
#define A100_MAGIC 0x41313030u
#define A100_ABI 1u
#define A100_CYCLES_50US 6400u
#define A100_CORE_HZ 128000000u
typedef struct {
 uint32_t magic,abi,epoch,status,core_hz,cpacr,dwt_ctrl;
 uint32_t steps,locked_steps,observer_max_cycles,foc_max_cycles,combined_max_cycles;
 uint32_t calculation_budget_exceeded,synthetic_source,motor_run_permission;
 float last_rpm,last_angle_error,last_duty_a,last_duty_b,last_duty_c;
} a100_record;
_Static_assert(sizeof(a100_record)==80u,"A100 record ABI");
volatile a100_record drill_compute_diagnostic;
void SysTick_Handler(void) { wait_safe(); }
static float wrap_angle(float a){
 const float pi=3.14159265358979323846f,two_pi=6.28318530717958647692f;
 while(a>pi){a-=two_pi;}
 while(a < -pi){a+=two_pi;}
 return a;
}
static bool counter_init(void){
 CoreDebug->DEMCR|=CoreDebug_DEMCR_TRCENA_Msk;
 if(DWT->CTRL&DWT_CTRL_NOCYCCNT_Msk)return false;
 DWT->CYCCNT=0;DWT->CTRL|=DWT_CTRL_CYCCNTENA_Msk;__DSB();__ISB();
 const uint32_t before=DWT->CYCCNT;
 for(volatile uint32_t i=0;i<16u;++i)__NOP();
 return DWT->CYCCNT!=before && (DWT->CTRL&DWT_CTRL_CYCCNTENA_Msk)!=0;
}
static void maximum(volatile uint32_t *dst,uint32_t value){if(value>*dst)*dst=value;}
__attribute__((noinline)) static bool run_synthetic(int direction,float speed){
 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=400000.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.00005f,.lock_phase_error_rad=0.60f,.pole_pairs=4,.lock_samples=16};
 const mfoc_config fc={.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,.max_dt_s=0.00005f,.modulation_ceiling=0.90f};
 drill_smo_state s;mfoc_controller f;
 if(!drill_smo_init(&s,&c)||!mfoc_init(&f,&fc))return false;
 float theta=0.7f;const float dt=0.00005f;drill_smo_output o={0};
 for(uint32_t n=0;n<40000u;++n){
  float sn,cs;drill_smo_sincos(theta,&sn,&cs);
  drill_smo_input i={.voltage_alpha_v=0.1f-speed*0.0003f*sn,.voltage_beta_v=-0.05f+speed*0.0003f*cs,.current_alpha_a=0.2f,.current_beta_a=-0.1f,.dt_s=dt,.commanded_direction=(int8_t)direction};
  ++drill_compute_diagnostic.epoch;__DMB();
  __DSB();const uint32_t begin=DWT->CYCCNT;
  const bool valid=drill_smo_step(&s,&i,&o);
  __DSB();const uint32_t after_observer=DWT->CYCCNT;
  maximum(&drill_compute_diagnostic.observer_max_cycles,after_observer-begin);
  if(!valid){__DMB();++drill_compute_diagnostic.epoch;return false;}
  uint32_t end=after_observer;
  if(o.angle_valid&&o.observer_locked){
   mfoc_input fi={.phase_a_current_a=0.2f,.phase_b_current_a=-0.18660254f,.phase_c_current_a=-0.01339746f,.sin_electrical_angle=o.sine,.cos_electrical_angle=o.cosine,.bus_voltage_v=11.1f,.d_current_ref_a=0,.q_current_ref_a=(float)direction*0.2f,.dt_s=dt};
   __DSB();const uint32_t foc_begin=DWT->CYCCNT;
   const mfoc_output fo=mfoc_step(&f,&fi);__DSB();end=DWT->CYCCNT;
   maximum(&drill_compute_diagnostic.foc_max_cycles,end-foc_begin);
   if(!fo.valid){__DMB();++drill_compute_diagnostic.epoch;return false;}
   ++drill_compute_diagnostic.locked_steps;
   drill_compute_diagnostic.last_duty_a=fo.duty_a;drill_compute_diagnostic.last_duty_b=fo.duty_b;drill_compute_diagnostic.last_duty_c=fo.duty_c;
  }
  const uint32_t cycles=end-begin;maximum(&drill_compute_diagnostic.combined_max_cycles,cycles);
  if(cycles>A100_CYCLES_50US)drill_compute_diagnostic.calculation_budget_exceeded=1;
  ++drill_compute_diagnostic.steps;drill_compute_diagnostic.last_rpm=o.mechanical_speed_rpm;
  theta=wrap_angle(theta+speed*dt);drill_compute_diagnostic.last_angle_error=wrap_angle(o.electrical_angle_rad-theta);
  __DMB();++drill_compute_diagnostic.epoch;
 }
 return o.observer_locked&&o.angle_valid;
}
int main(void){
 drill_safe_boot();
 drill_compute_diagnostic.magic=A100_MAGIC;drill_compute_diagnostic.abi=A100_ABI;
 drill_compute_diagnostic.synthetic_source=1;drill_compute_diagnostic.motor_run_permission=0;
 if(!drill_clock_init()){drill_compute_diagnostic.status=0x80000001u;wait_safe();}
 drill_compute_diagnostic.core_hz=SystemCoreClock;drill_compute_diagnostic.cpacr=SCB->CPACR;
 if((SCB->CPACR&0x00f00000u)!=0x00f00000u||SystemCoreClock!=A100_CORE_HZ){drill_compute_diagnostic.status=0x80000002u;wait_safe();}
 __DSB();__ISB();
 if(!counter_init()){drill_compute_diagnostic.status=0x80000003u;wait_safe();}
 drill_compute_diagnostic.dwt_ctrl=DWT->CTRL;
 drill_compute_diagnostic.status=1;
 if(!run_synthetic(1,20943.0f)||!run_synthetic(-1,-20943.0f)){drill_compute_diagnostic.status=0x80000004u;wait_safe();}
 ++drill_compute_diagnostic.epoch;__DMB();
 drill_compute_diagnostic.status=2;__DMB();
 ++drill_compute_diagnostic.epoch;wait_safe();
}
