#define main historical_tests_main
#include "test_drill_smo.c"
#undef main
int main(void){
 const float dts[]={0.00001f,0.00005f,0.0001f};unsigned cases=0;double maxerr=0;
 for(unsigned d=0;d<3;++d)for(int sign=-1;sign<=1;sign+=2)for(unsigned k=1;k<=10;++k){
  drill_smo_config c=config();c.max_dt_s=0.0001f;
  drill_smo_state s;assert(drill_smo_init(&s,&c));drill_smo_output o={0};
  const float omega_sample=(float)sign*(float)k*0.25f;float theta=0.7f;
  for(unsigned n=0;n<2000;++n){
   drill_smo_input i={.voltage_alpha_v=0.1f*cosf(theta),.voltage_beta_v=0.1f*sinf(theta),.dt_s=dts[d],.commanded_direction=(int8_t)sign};
   assert(drill_smo_step(&s,&i,&o));
   if(n<1999)theta=wrap_pi_test(theta+omega_sample);
  }
  /* Low-amplitude injection stays in the linear band. Reference phase is
   * independently computed in double from two transfer-function poles. */
  const double h=(double)dts[d]/(double)c.phase_inductance_h;
  const double a=1/(1+h*((double)c.phase_resistance_ohm+(double)c.sliding_gain_v/(double)c.current_error_sat_a));
  const double q=(double)c.bemf_filter_tau_s/((double)c.bemf_filter_tau_s+(double)dts[d]);
  const double ref=atan2(a*sin((double)omega_sample),1-a*cos((double)omega_sample))+atan2(q*sin((double)omega_sample),1-q*cos((double)omega_sample));
  const double observed=wrap_pi_test(theta-(float)atan2((double)o.bemf_beta_v,(double)o.bemf_alpha_v));
  const double err=fabs(observed-ref);if(err>maxerr)maxerr=err;assert(err<0.00001);++cases;
 }
 printf("A99_PHASE_RESPONSE_PASS cases=%u max_abs_phase_error_rad=%.12g\n",cases,maxerr);return 0;
}
