// NOTE: This file was repaired to fix a SIM-FAST IRQ handling regression that caused n=1 / fs=0 (no samples).
// Edits applied: AVG=1 (fastest), PI_I_LPF_HZ=300 Hz, iFmin/iFmax in PI_TRACE, slew 0.20.
// We now read exactly 1 CURRENT register at every ALERT FALLING edge (SIM-FAST),
// which restores ~7.1 kHz sampling during RL_CAL/DC/PI.
// ===== ESP32 + TB6612FNG + INA260 + Encodeur ===== (clean Rewrite) — ACTIVE CANVAS
// *** Boucle de courant sinusoïdal — version stable (SIM-FAST + LPF courant) ***
//
// Pipeline: RL_CAL (step + fit) -> DC_TEST (20%, 0.5 s) -> PI sinus courant
// Mesure INA260: SIM-FAST (1 lecture I2C par IRQ FALLING CNVR) ou STRICT réel
// TB6612 en locked‑antiphase: PWMA HIGH, AIN1/2 PWM complémentaires (50% = 0 A)

#include <Arduino.h>
#include <Wire.h>
#include <math.h>
#include "driver/pcnt.h"
#include "driver/ledc.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// ===== Options =====
#ifndef INA_SIM_FAST
#define INA_SIM_FAST 1     // 1: Wokwi SIM-FAST, 0: réel STRICT
#endif
#ifndef INA_USE_SHUNT_ONLY
#define INA_USE_SHUNT_ONLY 1 // 1: Shunt-only cont. (140 µs), 0: Shunt+Bus cont.
#endif
#ifndef VM_SUPPLY_V
#define VM_SUPPLY_V 12.0f
#endif
#ifndef PI_USE_FF_RL
#define PI_USE_FF_RL 1
#endif
#ifndef UFF_CLAMP
#define UFF_CLAMP 0.50f   // feedforward clamp (tighter to avoid dominance) [stable]
#endif // UFF_CLAMP // UFF_CLAMP
#ifndef UFF_SCALE
#define UFF_SCALE 1.00f   // unity FF (with clamp) improves in-band amplitude
#endif
 // UFF_SCALE // UFF_SCALE
#ifndef PI_TRACE_MS
#define PI_TRACE_MS 250u    // cadence logs temps réel PI (ms)
#endif
#ifndef PI_I_LPF_HZ
#define PI_I_LPF_HZ 250.0f  // slightly tighter LPF on current for PI & metrics (reduces HF excursions)
#endif // PI_I_LPF_HZ  // coupe du LPF de mesure pour PI & métriques
#ifndef U_RAMP_LIM
#define U_RAMP_LIM 0.15f    // gentler during amplitude ramp [tuned]
#endif // U_RAMP_LIMen
#ifndef U_BIAS
#define U_BIAS 0.00f        // disable bias in locked-antiphase
#endif
#ifndef ZERO_WIN_SIN
#define ZERO_WIN_SIN 0.60f  // fenêtre zéro |sin(phase)| < ZERO_WIN_SIN
#endif
#ifndef UMIN_WIN
#define UMIN_WIN 0.00f      // disable min-duty window in locked-antiphase
#endif
#ifndef REF_RAMP_MS
#define REF_RAMP_MS 1000u   // durée de rampe de l'amplitude de référence (ms)
#endif
#ifndef LOG_WIN_MS
#define LOG_WIN_MS 1000u    // pas de fenêtre logs (compat)
#endif
#ifndef PI_KI_SCALE
#define PI_KI_SCALE 0.50f  // slightly reduce integrator aggressiveness
#endif   // integrator strength (reduced) [tuned]
#ifndef NRMSE_TARGET
#define NRMSE_TARGET 0.50f  // target relative error for a 100/100 score
#endif
#ifndef KP_BOOST
#define KP_BOOST 1.10f  // slightly reduce proportional boost
#endif      // proportional boost (reduced) [tuned]
#ifndef KP_MIN_D
#define KP_MIN_D 0.12f      // floor on Kp (lower) [tuned]
#endif // KP_MIN_D

// ===== Brochage =====
#define PIN_TB_PWMA    19
#define PIN_TB_AIN1     5
#define PIN_TB_AIN2    23
#define PIN_TB_nSTBY    4

#define PIN_ENC_A      34
#define PIN_ENC_B      35

#define I2C_SDA        21
#define I2C_SCL        22
#define INA_ADDR       0x40  // 7-bit
#define PIN_INA_ALERT  27    // ALERT active LOW (CNVR)

// ===== LEDC (PWM pont H) [locked-antiphase] =====
static const ledc_mode_t       LEDC_MODE   = LEDC_LOW_SPEED_MODE;   // GPIOs larges
static const ledc_timer_t      LEDC_TIMER  = LEDC_TIMER_0;
static const ledc_timer_bit_t  LEDC_BITS   = LEDC_TIMER_10_BIT;     // 10 bits
static const uint32_t          LEDC_FREQ   = 20000;                 // 20 kHz
// Deux canaux pour LOCKED-ANTIPHASE (IN1/IN2 complémentaires)
static const ledc_channel_t    LEDC_CH1    = LEDC_CHANNEL_0;        // AIN1
static const ledc_channel_t    LEDC_CH2    = LEDC_CHANNEL_1;        // AIN2

// ===== PCNT (encodeur) =====
#define ENC_COUNTS_PER_MOTOR_REV 13
#define GEAR_RATIO 30.0f
static const pcnt_unit_t    PCNT_UNITX = PCNT_UNIT_0;
static const pcnt_channel_t PCNT_CH    = PCNT_CHANNEL_0;

// ===== INA260 regs =====
#define INA260_REG_CONFIG        0x00
#define INA260_REG_CURRENT       0x01
#define INA260_REG_BUS           0x02
#define INA260_REG_MASK_ENABLE   0x06
#define  INA260_MASK_CNVR        0x0008
#define INA260_REG_MFR_ID        0xFE
#define INA260_REG_DIE_ID        0xFF

static const uint16_t CT_US_LUT[8] =  {140, 204, 332, 588, 1100, 2116, 4156, 8244};
static const uint16_t AVG_LUT[8] =   {  1,   4,  16,   64,   128,   256,   512, 1024};

// ===== État global =====
// Phases
enum SysPhase { PH_CAL=0, PH_DC_INIT, PH_DC_RUN, PH_PI_INIT, PH_PI_RUN, PH_DONE };
static SysPhase g_phase = PH_CAL;

// INA & acquisition
static float    g_ina_pred_hz = 0.0f;
static uint16_t g_ina_cfg_raw = 0;
volatile bool   g_ina_flag    = false;   // posé par IRQ FALLING
volatile uint32_t g_ina_irqc  = 0;
#if INA_SIM_FAST
volatile bool   g_alert_low_consumed = false; // utilisé dans STRICT only si besoin
#endif

// Timestamps/échantillons
volatile uint32_t last_i_us = 0;  // dernière date échantillon (us)
volatile uint32_t dt_i_us   = 0;  // dernier dt us
volatile uint32_t i_count   = 0;  // total samples
float             last_i_mA = 0.0f;

// Estimation Ts globale (moyenne des dt)
static uint64_t g_dt_acc_us = 0;
static uint32_t g_dt_cnt    = 0;

// Encodeur
volatile int16_t pcnt_count16 = 0;
long enc_total = 0;  // cumul

// ===== Helpers =====
static inline float clampf(float x, float lo, float hi){ return x<lo?lo:(x>hi?hi:x);} 
static inline uint32_t duty_to_counts(float d){ d=fabsf(d); if(d>1) d=1; return (uint32_t)lroundf(d*((1u<<LEDC_BITS)-1u)); }

// PWM / TB6612
static void tb6612_init(){
  pinMode(PIN_TB_AIN1,OUTPUT);
  pinMode(PIN_TB_AIN2,OUTPUT);
  pinMode(PIN_TB_PWMA,OUTPUT);
  pinMode(PIN_TB_nSTBY,OUTPUT);
  digitalWrite(PIN_TB_nSTBY,HIGH); // Sortie de veille
  digitalWrite(PIN_TB_PWMA, HIGH); // PWMA HIGH pour locked‑antiphase
  digitalWrite(PIN_TB_AIN1,LOW);
  digitalWrite(PIN_TB_AIN2,LOW);
}

static void pwm_hw_init(){
  ledc_timer_config_t t={};
  t.speed_mode=LEDC_MODE;
  t.duty_resolution=LEDC_BITS;
  t.timer_num=LEDC_TIMER;
  t.freq_hz=LEDC_FREQ;
  t.clk_cfg=LEDC_AUTO_CLK;
  ledc_timer_config(&t);
  // Canal AIN1
  ledc_channel_config_t c1={};
  c1.gpio_num=PIN_TB_AIN1; c1.speed_mode=LEDC_MODE; c1.channel=LEDC_CH1; c1.timer_sel=LEDC_TIMER; c1.duty=0; c1.hpoint=0; c1.intr_type=LEDC_INTR_DISABLE;
  ledc_channel_config(&c1);
  // Canal AIN2
  ledc_channel_config_t c2={};
  c2.gpio_num=PIN_TB_AIN2; c2.speed_mode=LEDC_MODE; c2.channel=LEDC_CH2; c2.timer_sel=LEDC_TIMER; c2.duty=0; c2.hpoint=0; c2.intr_type=LEDC_INTR_DISABLE;
  ledc_channel_config(&c2);
  // Neutre 50% / 50%
  uint32_t half = duty_to_counts(0.5f);
  ledc_set_duty(LEDC_MODE, LEDC_CH1, half); ledc_update_duty(LEDC_MODE, LEDC_CH1);
  ledc_set_duty(LEDC_MODE, LEDC_CH2, half); ledc_update_duty(LEDC_MODE, LEDC_CH2);
}

static inline void pwm_set(float u){
  if (!isfinite(u)) u=0.0f;
  u = clampf(u, -1.0f, +1.0f);
  float Df = 0.5f*(u + 1.0f);   // duty sur AIN1
  float Dg = 1.0f - Df;         // complémentaire sur AIN2
  uint32_t d1 = duty_to_counts(Df);
  uint32_t d2 = duty_to_counts(Dg);
  ledc_set_duty(LEDC_MODE, LEDC_CH1, d1); ledc_update_duty(LEDC_MODE, LEDC_CH1);
  ledc_set_duty(LEDC_MODE, LEDC_CH2, d2); ledc_update_duty(LEDC_MODE, LEDC_CH2);
}

// PCNT
static void pcnt_init(){
  pcnt_config_t cfg={};
  cfg.pulse_gpio_num=PIN_ENC_A; 
  cfg.ctrl_gpio_num=PIN_ENC_B;
  cfg.lctrl_mode=PCNT_MODE_REVERSE; 
  cfg.hctrl_mode=PCNT_MODE_KEEP;
  cfg.pos_mode=PCNT_COUNT_INC; 
  cfg.neg_mode=PCNT_COUNT_DIS; // un seul front (A)
  cfg.counter_h_lim=32767; 
  cfg.counter_l_lim=-32768; 
  cfg.unit=PCNT_UNITX; 
  cfg.channel=PCNT_CH;
  pcnt_unit_config(&cfg);
  pcnt_set_filter_value(PCNT_UNITX, 1000); 
  pcnt_filter_enable(PCNT_UNITX);
  pcnt_counter_clear(PCNT_UNITX); 
  pcnt_counter_resume(PCNT_UNITX);
}
static inline void enc_tick(){ 
  int16_t v=0; 
  pcnt_get_counter_value(PCNT_UNITX,&v); 
  enc_total+=v; 
  pcnt_counter_clear(PCNT_UNITX);
}

// I2C utils INA
static bool i2c_write16(uint8_t reg, uint16_t val){ Wire.beginTransmission(INA_ADDR); Wire.write(reg); Wire.write((val>>8)&0xFF); Wire.write(val&0xFF); return Wire.endTransmission()==0; }
static bool i2c_read16(uint8_t reg, uint16_t &val){ Wire.beginTransmission(INA_ADDR); Wire.write(reg); if(Wire.endTransmission(false)!=0) return false; if(Wire.requestFrom(INA_ADDR,(uint8_t)2)!=2) return false; uint8_t msb=Wire.read(); uint8_t lsb=Wire.read(); val=((uint16_t)msb<<8)|lsb; return true; }

static inline uint16_t ina_cfg_make_fast(){
#if INA_USE_SHUNT_ONLY
  const uint16_t MODE=(5u<<0);  // Shunt cont.
#else
  const uint16_t MODE=(7u<<0);  // Shunt+Bus cont.
#endif
  const uint16_t AVG=(0u<<9); // AVG=1 (fastest). SIM-FAST: one CURRENT read per CNVR falling edge
  const uint16_t VBUS=(0u<<6);
  const uint16_t ISH=(0u<<3);
  return (AVG|VBUS|ISH|MODE);
}

static void ina_print_cfg(uint16_t cfg){ uint8_t mode=cfg&0x07, ish=(cfg>>3)&0x07, vbs=(cfg>>6)&0x07, avg=(cfg>>9)&0x07; uint32_t base_us= (mode==0x05? CT_US_LUT[ish] : mode==0x06? CT_US_LUT[vbs] : CT_US_LUT[ish]+CT_US_LUT[vbs]); uint32_t per_us= base_us*AVG_LUT[avg]; g_ina_pred_hz= per_us? (1e6f/(float)per_us):0.0f; Serial.printf("INA CFG=0x%04X (mode=0x%X, AVG=%u, VBUSCT=%u, ISHCT=%u) -> predicted ~ %.1f Hz\n", cfg, mode, AVG_LUT[avg], vbs, ish, g_ina_pred_hz);} 

void IRAM_ATTR ina_isr(){ g_ina_flag=true; g_ina_irqc++; }

static bool ina_init(){ Wire.begin(I2C_SDA, I2C_SCL, 1000000); uint16_t id=0; if(!i2c_read16(INA260_REG_MFR_ID,id) || id!=0x5449){ Serial.println("INA260 not found"); return false; } uint16_t cfg=ina_cfg_make_fast(); if(!i2c_write16(INA260_REG_CONFIG,cfg)){ Serial.println("CFG write fail"); return false;} delay(1); if(i2c_read16(INA260_REG_CONFIG,g_ina_cfg_raw)) ina_print_cfg(g_ina_cfg_raw); else Serial.println("CFG readback fail"); if(!i2c_write16(INA260_REG_MASK_ENABLE, INA260_MASK_CNVR)) Serial.println("MASK/ENABLE write fail"); last_i_us=0; dt_i_us=0; i_count=0; g_dt_acc_us=0; g_dt_cnt=0; return true; }

// ===== RL calibration =====
enum CalState { CAL_INIT, CAL_REST, CAL_STEP, CAL_DONE };
static CalState cal_state = CAL_INIT;
static const int      CAL_CYCLES  = 5;
static const float    CAL_DUTY    = 0.20f;
static const uint32_t CAL_STEP_MS = 200;
static const uint32_t CAL_REST_MS = 300;
static const int      CAL_MAX     = 2400;
static int      cal_idx   = 0;
static uint32_t cal_t0_ms = 0;
static int      cal_n     = 0;
static uint32_t cal_t_us[CAL_MAX];
static float    cal_i_mA[CAL_MAX];
static float    g_R= NAN, g_L= NAN, g_tau= NAN;
static float    cal_tau[CAL_CYCLES], cal_R[CAL_CYCLES], cal_L[CAL_CYCLES], cal_rmse[CAL_CYCLES];
static long     cal_enc0 = 0;

static void cal_reset(){ cal_state=CAL_INIT; cal_idx=0; }
static void cal_start_rest(uint32_t now){ cal_state=CAL_REST; cal_t0_ms=now; pwm_set(0); }
static void cal_start_step(uint32_t now){ cal_state=CAL_STEP; cal_t0_ms=now; cal_n=0; pwm_set(+CAL_DUTY);
#if INA_SIM_FAST
  { uint16_t raw=0; if (i2c_read16(INA260_REG_CURRENT, raw)) last_i_mA = ((int16_t)raw)*1.25f; }
#endif
  cal_enc0 = enc_total;
}

static void cal_on_sample(uint32_t t_us, float i_mA){ if(cal_state!=CAL_STEP) return; if(cal_n<CAL_MAX){ cal_t_us[cal_n]=t_us; cal_i_mA[cal_n]=i_mA; cal_n++; } }

static bool cal_fit_step(int n, const uint32_t* t, const float* i, float duty, float* iinf, float* tau, float* R, float* L, float* rn){
  if(n<16) return false; int tail=max(10,n/5); double s=0; for(int k=n-tail;k<n;k++) s+=i[k]; double i_end=s/tail; if(i_end<=1.0) return false; double tgt=0.6321205588*i_end; int k63=-1; for(int k=0;k<n;k++){ if(i[k]>=tgt){k63=k;break;} } if(k63<0) return false; double tau_s=((double)t[k63]-(double)t[0])/1e6; if(tau_s<=0) return false; double R_= (VM_SUPPLY_V*duty)/(i_end/1000.0); double L_= R_*tau_s; double se=0; for(int k=0;k<n;k++){ double tt=((double)t[k]-(double)t[0])/1e6; double ih=i_end*(1.0-exp(-tt/tau_s)); double e=(i[k]-ih); se+=e*e;} double rmse=sqrt(se/n); *iinf=i_end; *tau=tau_s; *R=R_; *L=L_; *rn=(i_end>1e-9? rmse/i_end:1.0); return true; }

static void cal_finish_cycle(){
  uint32_t dt_us = (cal_n>1)? (cal_t_us[cal_n-1] - cal_t_us[0]) : 0;
  double fs_cyc = (dt_us>0 && cal_n>1) ? (1000000.0 * (double)(cal_n-1)) / (double)dt_us : 0.0;
  long dcounts = enc_total - cal_enc0;
  double rpmM = (dt_us>0) ? (60.0 * (double)dcounts / ((double)ENC_COUNTS_PER_MOTOR_REV) / ((double)dt_us*1e-6)) : NAN;
  double rpmOut = rpmM / GEAR_RATIO;
  float iinf,tau,R,L,rn; bool ok=cal_fit_step(cal_n,cal_t_us,cal_i_mA,CAL_DUTY,&iinf,&tau,&R,&L,&rn);
  if(!ok){ Serial.printf("RL_CAL cycle %d: FIT_FAIL (n=%d) fs=%.1f Hz rpmM=%.1f rpmOut=%.1f\n", cal_idx+1, cal_n, fs_cyc, rpmM, rpmOut); cal_tau[cal_idx]=NAN; cal_R[cal_idx]=NAN; cal_L[cal_idx]=NAN; cal_rmse[cal_idx]=1.0f; }
  else { cal_tau[cal_idx]=tau; cal_R[cal_idx]=R; cal_L[cal_idx]=L; cal_rmse[cal_idx]=rn; Serial.printf("RL_CAL cycle %d: n=%d fs=%.1f Hz rpmM=%.1f rpmOut=%.1f i_inf=%.1f mA tau=%.3f s R=%.2f ohm L=%.6f H rmseN=%.3f\n", cal_idx+1, cal_n, fs_cyc, rpmM, rpmOut, iinf, tau, R, L, rn);} }

static void cal_summary(){ int m=0; double sR=0,sL=0,sT=0,sE=0,sR2=0,sL2=0,sT2=0,sE2=0; for(int k=0;k<CAL_CYCLES;k++){ if(!isnan(cal_R[k])&&!isnan(cal_L[k])&&!isnan(cal_tau[k])){ m++; double R=cal_R[k],L=cal_L[k],T=cal_tau[k],E=cal_rmse[k]; sR+=R; sL+=L; sT+=T; sE+=E; sR2+=R*R; sL2+=L*L; sT2+=T*T; sE2+=E*E; }} if(m==0){ Serial.println("RL_CAL SUMMARY: no valid cycles"); Serial.println("END RL_CAL"); g_phase=PH_DC_INIT; return;} double mR=sR/m,mL=sL/m,mT=sT/m,mE=sE/m; double sdR=sqrt(fmax(0.0,sR2/m-mR*mR)); double sdL=sqrt(fmax(0.0,sL2/m-mL*mL)); double sdT=sqrt(fmax(0.0,sT2/m-mT*mT)); int score=(int)lround(fmax(0.0,100.0*(1.0-mE/0.15))); Serial.printf("RL_CAL SUMMARY: cycles=%d R=%.3f+/-%.3f ohm  L=%.6f+/-%.6f H  tau=%.4f+/-%.4f s  errN=%.3f  SCORE=%d/100\n", m,mR,sdR,mL,sdL,mT,sdT,mE,score); g_R=(float)mR; g_L=(float)mL; g_tau=(float)mT; Serial.printf("#PARAM R=%.4f ohm L=%.6f H tau=%.6f s SCORE=%d\n", g_R,g_L,g_tau,score); Serial.println("END RL_CAL"); g_phase=PH_DC_INIT; }

static void cal_begin(){ Serial.printf("#RL_CAL  D=%.2f  Tstep=%ums  Trest=%ums  cycles=%d  V=%.1fV  INA~%.1f Hz\n", CAL_DUTY,(unsigned)CAL_STEP_MS,(unsigned)CAL_REST_MS,CAL_CYCLES,VM_SUPPLY_V,g_ina_pred_hz); cal_reset(); }

static void cal_tick(uint32_t now){ switch(cal_state){ case CAL_INIT: cal_start_rest(now); break; case CAL_REST: if(now-cal_t0_ms>=CAL_REST_MS) cal_start_step(now); break; case CAL_STEP: if(now-cal_t0_ms>=CAL_STEP_MS){ pwm_set(0); cal_finish_cycle(); cal_idx++; if(cal_idx>=CAL_CYCLES){ cal_state=CAL_DONE; cal_summary(); } else cal_start_rest(now);} break; case CAL_DONE: default: break; }}

// ===== DC test =====
static const uint32_t DC_MS = 500;
static float duty_cmd = 0.0f;
static uint32_t dc_t0_ms=0, dc_us0=0; static long dc_enc0=0; static double dc_is=0; static uint32_t dc_n=0;
static void dc_begin(uint32_t now){ dc_t0_ms=now; dc_us0=micros(); dc_enc0=enc_total; dc_is=0; dc_n=0; pwm_set(+0.20f); Serial.printf("#DC_TEST duty=%.2f dur=%ums\n", 0.20f, (unsigned)DC_MS);} 
static void dc_tick(uint32_t now){ if(now-dc_t0_ms>=DC_MS){ pwm_set(0); uint32_t us1=micros(); double dt=(double)(us1-dc_us0)*1e-6; long d=enc_total-dc_enc0; double rpmM=(dt>0)?(60.0*((double)d)/((double)ENC_COUNTS_PER_MOTOR_REV)/dt):NAN; double rpmOut=rpmM/GEAR_RATIO; double iDC=(dc_n>0)?(dc_is/dc_n):NAN; double fs_dc = (us1>dc_us0 && dc_n>1) ? (1000000.0 * (double)(dc_n-1)) / (double)(us1 - dc_us0) : 0.0; Serial.printf("DC_RES iDC=%.1f mA rpmM=%.1f rpmOut=%.1f dt=%.3fs fs=%.1f Hz\n", iDC, rpmM, rpmOut, dt, fs_dc); g_phase=PH_PI_INIT; }}

// ===== PI =====
static float g_Ts_s = 0.0005f; // période d'échantillonnage effective (s)
static float g_fc_hz = 150.0f;  // bande passante cible (Hz)
static float g_Kp_d = 0.0f;    // duty per Amp
static float g_Ki_d = 0.0f;    // duty per (A*s)
static float g_Iint = 0.0f;    // intégrateur (duty)
static const float g_Ulim = 1.0f;
// Rampe d'amplitude
static float g_amp_cur_mA = 0.0f;
static float g_amp_step_mA = 0.0f;
static uint32_t g_pi_win_t0 = 0;
static uint32_t g_pi_t0_ms  = 0;  // début test
static const uint32_t g_pi_test_ms = 8000; // longer test for better steady-state stats (auto-stop enabled)
static uint32_t g_pi_win_ms = LOG_WIN_MS;

// Réf sinus et logs
static float g_ref_amp_mA = 300.0f; // reduced for tighter tracking (was 400)
static float g_ref_bias_mA= 0.0f;
static float g_ref_f_hz   = 5.0f;
static float g_dph = 0.0f;   // rad par échantillon
static float g_ph  = 0.0f;   // phase courante
static float g_wref= 0.0f;   // rad/s
// Filtrage mesure courant (utilisé par PI et métriques)
static float g_i_filt_mA = 0.0f;
static float g_i_alpha   = 0.0f;
// Trace cadence & vitesses
static uint32_t g_trace_t0 = 0; // pour PI_TRACE cadence
static uint32_t g_trace_cnt = 0;
static float    g_fs_ema   = 0.0f;  // EMA de la cadence d'acquisition (Hz)
static long     g_enc_trace0 = 0;    // compteur encodeur à la dernière trace
static uint32_t g_enc_us0    = 0;    // timestamp us à la dernière trace
static uint32_t slew_hits=0, minDuty_hits=0;

// Accum fenêtre
static uint32_t pi_n=0; static double pi_e2=0, pi_u2=0, pi_u1=0, pi_uff2=0, pi_upi2=0; static uint32_t pi_sat=0;
static float pi_iFmin = +INFINITY, pi_iFmax = -INFINITY; // window min/max of filtered currentt
static double pi_is=0, pi_irs=0, pi_Sis=0, pi_Sic=0, pi_S2is=0, pi_S2ic=0, pi_Aref_sum=0;
// Accum totaux
static uint32_t pi_N=0; static double pi_E2=0, pi_U2=0, pi_U1=0, pi_UFF2T=0, pi_UPI2T=0, pi_It=0, pi_IREF2T=0; static uint32_t pi_SAT=0;
static double pi_SisT=0, pi_SicT=0, pi_S2isT=0, pi_S2icT=0, pi_ArefT=0;

static void pi_init(){ // Ts depuis moyenne des dt
  float Ts = (g_dt_cnt>10)? (float)((double)g_dt_acc_us/(double)g_dt_cnt)*1e-6f : 0.0005f;
  Ts = clampf(Ts, 50e-6f, 5e-3f);
  g_Ts_s = Ts;
  float fs = 1.0f/Ts;
  float tCT = 140e-6f; (void)tCT; // info latence
  float fc = 180.0f;  // fixed safe bandwidth for 5 Hz ref (works with AVG=4 + LPF)
  g_fc_hz = fc;
  float wc = 2.0f*(float)M_PI*fc; // <- keep at 180 Hz as requested
  float Kp = fmaxf(0.0f, 2.0f*g_L*wc - g_R);
  float Ki = g_L*wc*wc;
  g_Kp_d = fmaxf(KP_MIN_D, KP_BOOST * (Kp/VM_SUPPLY_V));
  g_Ki_d = (Ki/VM_SUPPLY_V) * PI_KI_SCALE;
  // Réf sinus et intégrateur
  g_ph = 0.0f;
  g_dph = 2.0f*(float)M_PI * g_ref_f_hz * Ts;
  g_wref = g_dph / Ts;
  g_Iint = 0.0f;
  // LPF mesure courant
  {
    float wc_lpf = 2.0f*(float)M_PI*PI_I_LPF_HZ;
    g_i_alpha = expf(-g_Ts_s * wc_lpf);
    g_i_alpha = clampf(g_i_alpha, 0.0f, 0.9999f);
    g_i_filt_mA = last_i_mA;
  }
  // Rampe amplitude
  g_amp_cur_mA = 0.0f;
  g_amp_step_mA = (REF_RAMP_MS>0) ? (g_ref_amp_mA * (Ts*1000.0f) / (float)REF_RAMP_MS) : g_ref_amp_mA;
  // Fenêtres / totaux
  g_pi_t0_ms = millis();
  g_pi_win_t0 = g_pi_t0_ms;
  g_trace_t0 = g_pi_t0_ms;
  g_trace_cnt = 0;
  pi_n=0; pi_e2=pi_u2=pi_u1=0; pi_sat=0;
  pi_is=pi_irs=pi_Sis=pi_Sic=pi_S2is=pi_S2ic=0;
  pi_N=0; pi_E2=pi_U2=pi_U1=pi_It=0; pi_SAT=0;
  pi_SisT=pi_SicT=pi_S2isT=pi_S2icT=0;
  minDuty_hits=0; slew_hits=0;
  Serial.printf("#PI_INIT Ts=%.6f s fs=%.1f Hz fc=%.1f Hz Kp_d=%.4f Ki_d=%.1f REF=sin(%.1fHz, %.0f mA, ramp %ums)\n",
                g_Ts_s, 1.0f/g_Ts_s, g_fc_hz, g_Kp_d, g_Ki_d, g_ref_f_hz, g_ref_amp_mA, (unsigned)REF_RAMP_MS);
  Serial.println("PI_KICK");
}

static inline void pi_on_sample(float i_mA){
  // Avancement de phase
  g_ph += g_dph;
  if (g_ph > 2.0f*(float)M_PI) g_ph -= 2.0f*(float)M_PI;

  // Référence et erreur (A)
  const float s = sinf(g_ph);
  const float c = cosf(g_ph);
  if (g_amp_cur_mA < g_ref_amp_mA) { g_amp_cur_mA += g_amp_step_mA; if (g_amp_cur_mA > g_ref_amp_mA) g_amp_cur_mA = g_ref_amp_mA; }
  const float iref_mA = g_ref_bias_mA + g_amp_cur_mA * s;
  // Filtrage mesure
  g_i_filt_mA = g_i_alpha * g_i_filt_mA + (1.0f - g_i_alpha) * i_mA;
  const float i_used_mA = g_i_filt_mA;
  if (g_i_filt_mA < pi_iFmin) pi_iFmin = g_i_filt_mA;
  if (g_i_filt_mA > pi_iFmax) pi_iFmax = g_i_filt_mA;
  const float e_A = (iref_mA - i_used_mA) * 1e-3f;
  const float di_ref_A_dt = (g_amp_cur_mA * 1e-3f) * g_wref * c;

  // Feedforward RL
  float u_ff = 0.0f;
#if PI_USE_FF_RL
  u_ff = UFF_SCALE * ((g_R * (iref_mA * 1e-3f) + g_L * di_ref_A_dt) / VM_SUPPLY_V);
  if (u_ff >  UFF_CLAMP) u_ff =  UFF_CLAMP;
  if (u_ff < -UFF_CLAMP) u_ff = -UFF_CLAMP;
#endif

  const float u_pi = g_Kp_d * e_A + g_Iint;
  float u_unsat = u_ff + u_pi; if (!isfinite(u_unsat)) u_unsat = 0.0f;

  float u_lim = (g_amp_cur_mA < (g_ref_amp_mA - 1e-3f)) ? U_RAMP_LIM : g_Ulim;
  float u_sat = clampf(u_unsat, -u_lim, u_lim);

  float u = u_sat;
  bool minActive = false; bool biasActive = false;
  const float i_abs = fabsf(i_used_mA);
  bool in_zero = (fabsf(s) < ZERO_WIN_SIN);
  if (in_zero && i_abs < 100.0f) {
    const float usgn = (iref_mA >= 0.0f) ? +1.0f : -1.0f;
    float u_biased = u_unsat + usgn * U_BIAS; biasActive = true;
    u = clampf(u_biased, -u_lim, u_lim);
    if (fabsf(u) < UMIN_WIN) { u = usgn * UMIN_WIN; minDuty_hits++; minActive = true; }
  }

  static float u_prev = 0.0f;
  const float du_max = 0.20f; // per-sample slew limit (slightly relaxed to reduce slew_hits)
  float du = u - u_prev; bool slewActive=false;
  if (du > du_max)  { u = u_prev + du_max; slew_hits++; slewActive=true; }
  else if (du < -du_max) { u = u_prev - du_max; slew_hits++; slewActive=true; }
  u_prev = u;

  // Anti-windup & intégration conditionnelle
  const bool small_ref = fabsf(iref_mA) < 20.0f; // mA
  bool allow_backcalc = !(minActive || slewActive || biasActive);
  float back_term = allow_backcalc ? (u - u_unsat) : 0.0f; if (!isfinite(back_term)) back_term = 0.0f; back_term = clampf(back_term, -0.2f, +0.2f);
  bool satActive = (u != u_unsat) && ((u == u_lim) || (u == -u_lim) || (fabsf(u)-fabsf(u_unsat) < -1e-6f));
  bool allow_integrate = !small_ref;
  if (satActive) { if ((u >= u_lim && e_A > 0) || (u <= -u_lim && e_A < 0)) allow_integrate = false; }
  if (slewActive) allow_integrate = false;
  float leak_alpha = clampf(g_Ts_s / 2.0f, 0.0f, 0.01f);
  float Inew = g_Iint * (1.0f - leak_alpha);
  if (allow_integrate) Inew += g_Ki_d * g_Ts_s * e_A;
  float Kb = allow_backcalc ? (0.2f * g_Ki_d) : 0.0f;
  Inew += Kb * back_term;
  if (!isfinite(Inew)) Inew = 0.0f; g_Iint = Inew;

  if (!isfinite(u)) u = 0.0f; pwm_set(u);

  // Trace temps réel compact
  {
    uint32_t nowms = millis();
    if (nowms - g_trace_t0 >= PI_TRACE_MS) {
      float fs_tr = g_fs_ema; // EMA cadence mesure
      uint32_t nowus = micros();
      uint32_t dtus = (g_enc_us0==0) ? 0 : (nowus - g_enc_us0);
      long dcounts = enc_total - g_enc_trace0;
      double rpmM = (dtus>0) ? (60.0 * (double)dcounts / ((double)ENC_COUNTS_PER_MOTOR_REV) / ((double)dtus*1e-6)) : NAN;
      double rpmOut = rpmM / GEAR_RATIO;
      Serial.printf("PI_TRACE t=%lums A_ramp=%.1f mA iF=%.1f mA iraw=%.1f mA iref=%.1f mA e=%.1f mA u=%.3f uff=%.3f upi=%.3f fs=%.1f Hz rpmM=%.1f rpmOut=%.1f sat=%d\n",
                    (unsigned long)nowms, g_amp_cur_mA, g_i_filt_mA, i_mA, iref_mA, (iref_mA - g_i_filt_mA),
                    u, u_ff, u_pi, fs_tr, rpmM, rpmOut, (fabs(u) > 0.98f));
      g_trace_t0 = nowms; g_trace_cnt++;
      g_enc_trace0 = enc_total; g_enc_us0 = nowus;
    }
  }

  // Accumulation métriques par fenêtre
  {
    pi_n++;
    pi_e2  += (double)(e_A*e_A);
    pi_u2  += (double)(u*u);
    pi_u1  += (double)u;
    pi_uff2+= (double)(u_ff*u_ff);
    pi_upi2+= (double)(u_pi*u_pi);
    if (fabs(u) > 0.98f) pi_sat++;
    pi_is      += (double)g_i_filt_mA;
    pi_irs     += (double)iref_mA;
    pi_Sis     += (double)g_i_filt_mA * (double)s;
    pi_Sic     += (double)g_i_filt_mA * (double)c;
    float s2 = sinf(2.0f*g_ph), c2 = cosf(2.0f*g_ph);
    pi_S2is    += (double)g_i_filt_mA * (double)s2;
    pi_S2ic    += (double)g_i_filt_mA * (double)c2;
    pi_Aref_sum+= (double)fabsf(iref_mA);
  }

  // Accumulation totaux
  {
    pi_N++;
    pi_E2   += (double)(e_A*e_A);
    pi_U2   += (double)(u*u);
    pi_U1   += (double)u;
    pi_UFF2T+= (double)(u_ff*u_ff);
    pi_UPI2T+= (double)(u_pi*u_pi);
    pi_It   += (double)g_i_filt_mA;
    pi_IREF2T += (double)(iref_mA*iref_mA);
    pi_SAT  += (fabs(u) > 0.98f) ? 1 : 0;
    pi_SisT += (double)g_i_filt_mA * (double)s;
    pi_SicT += (double)g_i_filt_mA * (double)c;
    float s2 = sinf(2.0f*g_ph), c2 = cosf(2.0f*g_ph);
    pi_S2isT+= (double)g_i_filt_mA * (double)s2;
    pi_S2icT+= (double)g_i_filt_mA * (double)c2;
    pi_ArefT+= (double)fabsf(iref_mA);
  }
}

// ===== Acquisition =====
static inline void on_current_sample_us(uint16_t raw, uint32_t t_us){
  last_i_mA = ((int16_t)raw) * 1.25f; // mA/LSB
  if(last_i_us!=0){ dt_i_us = t_us - last_i_us; g_dt_acc_us += dt_i_us; g_dt_cnt++; float fs = (dt_i_us>0)? (1000000.0f/(float)dt_i_us) : 0.0f; if(g_fs_ema<=0) g_fs_ema=fs; else g_fs_ema = 0.90f*g_fs_ema + 0.10f*fs; }
  last_i_us = t_us; i_count++;
  if(g_phase==PH_CAL){ cal_on_sample(t_us, last_i_mA); }
  else if(g_phase==PH_DC_RUN){ dc_is += last_i_mA; dc_n++; }
  else if(g_phase==PH_PI_RUN){ pi_on_sample(last_i_mA); }
}

static void ina_arm_cnvr_irq(){
#if INA_SIM_FAST
  // rien de spécial: IRQ CNVR déjà routée
#else
  // Strict TI: re-armer CNVR via MASK/ENABLE si nécessaire
  uint16_t m=0; i2c_read16(INA260_REG_MASK_ENABLE,m); m |= INA260_MASK_CNVR; i2c_write16(INA260_REG_MASK_ENABLE,m);
#endif
}

static void ina_irq_tick(){
  if(!g_ina_flag) return; // rien à faire
  g_ina_flag=false;
#if INA_SIM_FAST
  // SIM-FAST : une lecture par front descendant d'ALERT
  uint16_t raw=0; if(i2c_read16(INA260_REG_CURRENT, raw)) on_current_sample_us(raw, micros());
#else
  // STRICT TI : vérifier le flag CNVR
  uint16_t flags=0; if(i2c_read16(INA260_REG_MASK_ENABLE, flags) && (flags & INA260_MASK_CNVR)){ uint16_t raw=0; if(i2c_read16(INA260_REG_CURRENT, raw)) on_current_sample_us(raw, micros()); }
#endif
}

// ===== Setup / Loop =====
void setup(){ Serial.begin(115200); delay(100);
  Serial.println(); Serial.println("BOOT");
  tb6612_init(); pwm_hw_init();
  pinMode(PIN_INA_ALERT, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(PIN_INA_ALERT), ina_isr, FALLING);
  if(!ina_init()){ Serial.println("INA INIT FAIL"); while(1) delay(1000);} ina_arm_cnvr_irq();
  pcnt_init();
  Serial.printf("#ACQ MODE=%s IRQ (1 read/CNVR)\n", INA_SIM_FAST?"SIM_FAST":"STRICT");
  cal_begin();
}

void loop(){ uint32_t now=millis(); ina_irq_tick(); enc_tick();
  switch(g_phase){
    case PH_CAL: cal_tick(now); break;
    case PH_DC_INIT: dc_begin(now); g_phase=PH_DC_RUN; break;
    case PH_DC_RUN: dc_tick(now); break;
    case PH_PI_INIT: pi_init(); Serial.println("PI_START"); g_phase=PH_PI_RUN; pwm_set(0.0f); break;
    case PH_PI_RUN:
      if ((now - g_pi_win_t0) >= g_pi_win_ms) {
        g_pi_win_t0=now;
        if(pi_n>0){
          double n=(double)pi_n;
          double B1 = 2.0*sqrt(pi_Sis*pi_Sis + pi_Sic*pi_Sic)/n;
          double ph  = atan2(pi_Sic,pi_Sis);
          double A1  = (double)(pi_Aref_sum/n);
          double errRMS = sqrt(pi_e2/n) * 1000.0; // mA
          double dutyRMS = sqrt(pi_u2/n);
          double dutyMean= pi_u1/n;
          double iDC     = pi_is/n;
          double irefDC  = pi_irs/n;
          double gain    = (A1>1e-6)?(B1/A1):NAN;
          double ph_deg  = ph*(180.0/M_PI);
          double B2      = 2.0*sqrt(pi_S2is*pi_S2is + pi_S2ic*pi_S2ic)/n;
          double THD2    = (B1>1e-6)?(B2/B1):NAN;
          double satPct  = (100.0*(double)pi_sat)/n;
          double nrmse   = (A1>1e-6)?(errRMS / A1):NAN; // both in mA
          Serial.printf("PI_TRACE t=%lums n=%u iDC=%.1f mA irefDC=%.1f mA amp=%.1f mA gain=%.3f phase=%.1f deg THD2=%.3f errRMS=%.1f mA NRMSE=%.3f dutyMean=%.3f dutyRMS=%.3f iFmin=%.1f mA iFmax=%.1f mA sat=%.1f%%\n",
                        (unsigned long)now, pi_n, iDC, irefDC, B1, gain, ph_deg, THD2, errRMS, nrmse, dutyMean, dutyRMS, pi_iFmin, pi_iFmax, satPct);
          // reset window accumulators
          pi_n=0; pi_e2=pi_u2=pi_u1=pi_uff2=pi_upi2=0; pi_sat=0; 
          pi_is=pi_irs=pi_Sis=pi_Sic=pi_S2is=pi_S2ic=0; pi_Aref_sum=0; 
          pi_iFmin=+INFINITY; pi_iFmax=-INFINITY;
        }
      }
      // auto-stop after test duration
      if ((now - g_pi_t0_ms) >= g_pi_test_ms) {
        pwm_set(0.0f);
        g_phase = PH_DONE;
        // compute and print final summary
        double nT = (double)pi_N;
        if (nT>0) {
          double errRMS_T = sqrt(pi_E2/nT) * 1000.0; // mA
          double dutyRMS_T= sqrt(pi_U2/nT);
          double dutyMean_T= pi_U1/nT;
          double B_T = 2.0*sqrt(pi_SisT*pi_SisT + pi_SicT*pi_SicT)/nT;
          double A_T = (double)g_ref_amp_mA;
          double gain_T = (A_T>1e-6)?(B_T/A_T):NAN;
          double ph_T_deg = atan2(pi_SicT,pi_SisT)*(180.0/M_PI);
          double B2_T = 2.0*sqrt(pi_S2isT*pi_S2isT + pi_S2icT*pi_S2icT)/nT;
          double THD2_T = (B_T>1e-6)?(B2_T/B_T):NAN;
          double nrmse_T = (A_T>1e-6)?(errRMS_T/A_T):NAN;
          int score = (int)lround(fmax(0.0,100.0*(1.0-(nrmse_T/NRMSE_TARGET))));
          Serial.printf("PI_SUMMARY n=%u iDC=%.1f mA amp=%.1f mA gain=%.3f phase=%.1f deg THD2=%.5f errRMS=%.1f mA NRMSE=%.3f dutyMean=%.3f dutyRMS=%.3f sat=%.1f%% minDuty_hits=%u slew_hits=%u SCORE=%d/100\n",
                        pi_N, pi_It/nT, B_T, gain_T, ph_T_deg, THD2_T, errRMS_T, nrmse_T, dutyMean_T, dutyRMS_T, (100.0*(double)pi_SAT)/nT, minDuty_hits, slew_hits, score);
        }
      }
      break;
    case PH_DONE: default: break;
  }
}


// ====== END OF FILE ======