#define main historical_tests_main
#include "test_drill_smo.c"
#undef main
#ifndef A98_PLL_KI
#define A98_PLL_KI 80000.0f
#endif
static unsigned noise_state=1;
static float noise(float amplitude){noise_state=noise_state*1664525u+1013904223u;return amplitude*((float)(noise_state>>8)/8388607.5f-1.0f);}
static void run(const char *name,int dir,float noise_i,float noise_v,float jitter,float r_scale,float l_scale){
 const float plant_r=0.5f,plant_l=0.0005f,ke=0.0003f,ip=0.05f;
 drill_smo_config c=config();c.max_dt_s=0.000060f;c.pll_ki_per_s2=A98_PLL_KI;c.phase_resistance_ohm*=r_scale;c.phase_inductance_h*=l_scale;
 drill_smo_state s;assert(drill_smo_init(&s,&c));drill_smo_output o={0};noise_state=1;
 float t=0,theta=0.7f,max_angle=0,max_rpm_error=0,final_error=0,final_rpm=0;unsigned monitored=0,unlocked=0,rejected=0,wrong_sign=0;bool last_lock=false;unsigned lock_losses=0,steady_n=0;double steady_square=0;float steady_peak=0;
 while(t<1.5f){
  const float dt=0.00005f+noise(jitter),rpm=t<0.20f?5000.0f:t<0.70f?5000.0f+90000.0f*(t-0.20f):50000.0f;
  const float accel=t>=0.20f&&t<0.70f?90000.0f:0;
  const float speed=(float)dir*rpm*TWO_PI/60.0f*4.0f;
  const float sn=sinf(theta),cs=cosf(theta),ia=ip*cs,ib=ip*sn;
  drill_smo_input i={.voltage_alpha_v=plant_r*ia-(plant_l*ip+ke)*speed*sn+noise(noise_v),.voltage_beta_v=plant_r*ib+(plant_l*ip+ke)*speed*cs+noise(noise_v),.current_alpha_a=ia+noise(noise_i),.current_beta_a=ib+noise(noise_i),.dt_s=dt,.commanded_direction=(int8_t)dir};
  if(!drill_smo_step(&s,&i,&o)){++rejected;break;}
  theta=wrap_pi_test(theta+speed*dt+0.5f*(float)dir*accel*TWO_PI/60.0f*4.0f*dt*dt);t+=dt;
  if(t>0.10f){
   float ae=fabsf(wrap_pi_test(o.electrical_angle_rad-theta)),re=fabsf(o.mechanical_speed_rpm-(float)dir*rpm);
   if(ae>max_angle)max_angle=ae;if(re>max_rpm_error)max_rpm_error=re;
   ++monitored;if(!o.observer_locked)++unlocked;
   if(o.observer_locked&&(float)dir*o.mechanical_speed_rpm<=0)++wrong_sign;
   if(last_lock&&!o.observer_locked)++lock_losses;last_lock=o.observer_locked;
   final_error=ae;final_rpm=o.mechanical_speed_rpm;
   if(t>1.20f){++steady_n;steady_square+=(double)re*(double)re;if(re>steady_peak)steady_peak=re;}
  }
 }
 printf("%s,%d,%.6f,%.6f,%.6f,%.3f,%.3f,%u,%u,%u,%u,%u,%.6f,%.3f,%.6f,%.3f,%d,%.6f,%.6f\n",name,dir,noise_i,noise_v,jitter*1e6f,r_scale,l_scale,monitored,unlocked,rejected,wrong_sign,lock_losses,max_angle,max_rpm_error,final_error,final_rpm,o.observer_locked,sqrt(steady_square/(double)steady_n),(double)steady_peak);
}
int main(void){
 puts("Scenario,Direction,CurrentNoisePeakA,VoltageNoisePeakV,SampleIntervalJitterPeakUs,RScale,LScale,MonitoredSamples,UnlockedSamples,Rejected,WrongDirectionWhileLocked,LockLosses,MaxAngleErrorRad,MaxRPMError,FinalAngleErrorRad,FinalRPM,FinalLocked,SteadyRPMErrorRMS,SteadyRPMErrorPeak");
 for(int d=-1;d<=1;d+=2){run("ramp",d,0,0,0,1,1);run("noise_jitter",d,0.005f,0.025f,0.000005f,1,1);run("parameters_low",d,0.005f,0.025f,0.000005f,0.8f,0.8f);run("parameters_high",d,0.005f,0.025f,0.000005f,1.2f,1.2f);}
 return 0;
}
