#include <Arduino.h>
#include <Wire.h>
#include <TFT_eSPI.h>
#include <U8g2_for_TFT_eSPI.h>
#include <SparkFun_BNO08x_Arduino_Library.h>
#include <esp_system.h>
#include <cstdarg>
#include "test_config.h"
#include "evidence_rules.h"

TFT_eSPI tft;
U8g2_for_TFT_eSPI chinese;
BNO08x imu;
enum class Stage { Idle, Initializing, Warmup, Still, Ready, Motion, Finished };
struct Report {
  uint32_t good=0, bad=0, gaps=0, missing=0, duplicate=0, maxGap=0, timestampBack=0;
  uint32_t lastMs=0, rateBase=0;
  uint64_t sensorUs=0;
  uint8_t seq=0, accuracy=0;
  bool seen=false, timedOut=false;
  float hz=0;
};
struct Transport {
  uint32_t reads=0, readErrors=0, writes=0, writeErrors=0;
  uint32_t intTimeouts=0, hardwareResets=0, decodeErrors=0, oversized=0;
};
Report report[3];
Transport transport, initTransport;
Stage stage=Stage::Idle;
bool active=false, initialized=false, productResponded=false, armed=false, fault=false;
bool automaticSession=false;
bool inSensorCallback=false;
bool stillDone=false, stillOK=false, motionDone[3]={}, motionOK[3]={};
uint32_t bootId=0, sessionNo=0, started=0, streamStart=0, stageStart=0;
uint32_t ended=0, unexpectedResets=0, enableErrors=0, lastTick=0, lastDraw=0;
char module[25]="UNASSIGNED", runId[32]="", lineBuffer[96];
size_t lineLength=0;
bool lineOverflow=false;
int motionAxis=-1;
float accel[3]={}, gyro[3]={}, quat[4]={}, rpy[3]={};
float stillSum=0, stillSumSq=0, stillGyroPeak=0, stillMean=0, stillStd=0;
uint32_t stillAccelN=0, stillGyroN=0;
float motionPeak=0, motionAngle=0, motionBaseQuat[4]={};
uint32_t motionGyroN=0, motionQuatN=0;
bool motionHasBase=false;
void drawScreen();

const char *stageName() {
  switch(stage) {
    case Stage::Idle:return "IDLE"; case Stage::Initializing:return "INIT";
    case Stage::Warmup:return "WARMUP"; case Stage::Still:return "STILL";
    case Stage::Ready:return "READY"; case Stage::Motion:return "MOTION";
    default:return "FINISHED";
  }
}
void logEvent(const char *event, const char *fmt="", ...) {
  char detail[640]; va_list args; va_start(args,fmt);
  vsnprintf(detail,sizeof(detail),fmt,args); va_end(args);
  const uint32_t now=millis();
  Serial.printf("EV,boot_ms=%lu,test_ms=%lu,module=%s,run=%s,stage=%s,event=%s,%s\n",
    (unsigned long)now,(unsigned long)(active?now-started:stage==Stage::Finished?ended-started:0),
    module,runId,stageName(),event,detail);
  // Serial summaries may span many lines; drain data after each line, not after
  // the entire summary. Never reenter SH2 from a sensor callback's diagnostic log.
  static bool draining=false;
  if(active&&initialized&&!draining&&!inSensorCallback) {
    draining=true;
    for(int n=0;n<8&&digitalRead(BNO_INT)==LOW;++n) {sh2_service();yield();}
    draining=false;
  }
}
// Local vendor hooks count every decoded event, including multiple events in one SHTP packet.
// They do no serial IO. This avoids silently retaining only the last event in a packet.
void bno08xDiagnosticTransport(uint8_t code) {
  Transport &t=armed?transport:initTransport;
  switch(code) {
    case 1:++t.reads;break; case 2:++t.readErrors;break;
    case 3:++t.writes;break;case 4:++t.writeErrors;break;
    case 5:++t.intTimeouts;break;case 6:++t.hardwareResets;break;
    case 7:++t.decodeErrors;break;case 8:++t.oversized;break;
  }
  if (armed && (code==2||code==4||code==5||code==6||code==7||code==8)) fault=true;
}
void bno08xDiagnosticEvent(const sh2_SensorValue_t *v) {
  if(!armed||!active) return;
  int k=-1; float values[4]; int count=3;
  if(v->sensorId==SH2_ACCELEROMETER) {
    k=0;values[0]=v->un.accelerometer.x;values[1]=v->un.accelerometer.y;values[2]=v->un.accelerometer.z;
  } else if(v->sensorId==SH2_GYROSCOPE_CALIBRATED) {
    k=1;values[0]=v->un.gyroscope.x;values[1]=v->un.gyroscope.y;values[2]=v->un.gyroscope.z;
  } else if(v->sensorId==SH2_ROTATION_VECTOR) {
    k=2;count=4;values[0]=v->un.rotationVector.real;values[1]=v->un.rotationVector.i;
    values[2]=v->un.rotationVector.j;values[3]=v->un.rotationVector.k;
  } else return;
  Report &r=report[k]; const uint32_t now=millis();
  bool valid=finiteVector(values,count,k==0?160.0f:k==1?36.0f:1.15f);
  float norm=0;
  if(k==2) {
    for(int i=0;i<4;++i) norm+=values[i]*values[i];
    norm=sqrtf(norm);valid=valid&&norm>=0.85f&&norm<=1.15f;
  }
  if(r.seen) {
    const uint8_t step=(uint8_t)(v->sequence-r.seq);
    if(step==0) {++r.duplicate;valid=false;}
    else if(step!=1) {r.missing+=(uint8_t)(step-1);fault=true;}
    // SH2 timestamps are estimates from host receive time and report delay, not a sensor clock.
    if(v->timestamp<=r.sensorUs)++r.timestampBack;
  }
  if(!valid) {++r.bad;fault=true;return;}
  const uint32_t gap=now-(r.seen?r.lastMs:streamStart);
  if(gap>r.maxGap)r.maxGap=gap;
  if(gap>GAP_MS&&!r.timedOut) {++r.gaps;fault=true;}
  r.timedOut=false;r.lastMs=now;r.sensorUs=v->timestamp;r.seq=v->sequence;
  r.accuracy=v->status&3;r.seen=true;++r.good;
  if(k==0) {
    memcpy(accel,values,sizeof(accel));
    if(stage==Stage::Still) {
      const float g=sqrtf(accel[0]*accel[0]+accel[1]*accel[1]+accel[2]*accel[2]);
      stillSum+=g;stillSumSq+=g*g;++stillAccelN;
    }
  } else if(k==1) {
    memcpy(gyro,values,sizeof(gyro));
    if(stage==Stage::Still) {
      const float w=sqrtf(gyro[0]*gyro[0]+gyro[1]*gyro[1]+gyro[2]*gyro[2]);
      stillGyroPeak=fmaxf(stillGyroPeak,w);++stillGyroN;
    }
    if(stage==Stage::Motion) {motionPeak=fmaxf(motionPeak,fabsf(gyro[motionAxis]));++motionGyroN;}
  } else {
    for(int i=0;i<4;++i)quat[i]=values[i]/norm;
    const float w=quat[0],x=quat[1],y=quat[2],z=quat[3];
    rpy[0]=atan2f(2*(w*x+y*z),1-2*(x*x+y*y))*RAD_TO_DEG;
    rpy[1]=asinf(fmaxf(-1,fminf(1,2*(w*y-z*x))))*RAD_TO_DEG;
    rpy[2]=atan2f(2*(w*z+x*y),1-2*(y*y+z*z))*RAD_TO_DEG;
    if(stage==Stage::Motion) {
      ++motionQuatN;
      if(!motionHasBase) {memcpy(motionBaseQuat,quat,sizeof(quat));motionHasBase=true;}
      float dot=0;for(int i=0;i<4;++i)dot+=quat[i]*motionBaseQuat[i];
      motionAngle=fmaxf(motionAngle,2*acosf(fminf(1,fabsf(dot)))*RAD_TO_DEG);
    }
  }
}
void onSensorReport(void *,sh2_SensorEvent_t *event) {
  inSensorCallback=true;
  sh2_SensorValue_t value{};
  if(sh2_decodeSensorEvent(&value,event)!=SH2_OK) {
    bno08xDiagnosticTransport(7);
    static uint8_t details=0;
    if(details<8) {++details;logEvent("DECODE_ERROR","report_id=%u,len=%u",event->reportId,event->len);}
    inSensorCallback=false;return;
  }
  bno08xDiagnosticEvent(&value);
  inSensorCallback=false;
}
uint32_t elapsedStream() {return initialized?(active?millis():ended)-streamStart:0;}
uint32_t anomalyCount() {
  uint32_t n=transport.readErrors+transport.writeErrors+transport.intTimeouts+
    transport.hardwareResets+transport.decodeErrors+transport.oversized+unexpectedResets+enableErrors;
  for(auto &r:report)n+=r.bad+r.gaps+r.missing;
  return n+(fault&&n==0?1:0); // Includes initialization anomalies latched before streaming.
}
Evidence streamResult() {
  uint32_t counts[3]={report[0].good,report[1].good,report[2].good};
  return streamEvidence(initialized,elapsedStream(),MIN_STREAM_MS,anomalyCount(),counts,
                        MIN_OBSERVED_HZ,MAX_OBSERVED_HZ);
}
Evidence result() {return overallEvidence(streamResult(),stillDone,stillOK,motionDone,motionOK);}
const char *evidenceName(Evidence e) {
  return e==Evidence::Observed?"OBSERVED":e==Evidence::Review?"REVIEW":"INCOMPLETE";
}
bool reportsOn() {
  bool a=imu.enableReport(SH2_ACCELEROMETER,REPORT_US);
  bool g=imu.enableReport(SH2_GYROSCOPE_CALIBRATED,REPORT_US);
  bool q=imu.enableReport(SH2_ROTATION_VECTOR,REPORT_US);
  if(!a||!g||!q){++enableErrors;fault=true;}
  logEvent("REPORT_ENABLE","acc=%d,gyro=%d,quat=%d,request_us=%lu",a,g,q,(unsigned long)REPORT_US);
  return a&&g&&q;
}
void checkTimeouts() {
  const uint32_t now=millis();
  for(int i=0;i<3;++i) {
    Report &r=report[i];
    const uint32_t gap=now-(r.seen?r.lastMs:streamStart);
    if(gap>r.maxGap)r.maxGap=gap;
    if(gap>GAP_MS&&!r.timedOut) {
      r.timedOut=true;++r.gaps;fault=true;
      logEvent("REPORT_TIMEOUT","type=%d,gap_ms=%lu",i,(unsigned long)gap);
    }
  }
}
void summary() {
  logEvent("STATE","active=%d,initialized=%d,reports_armed=%d,int_level=%d,session_mode=%s",
    active,initialized,armed,digitalRead(BNO_INT),!runId[0]?"none":automaticSession?"automatic_boot":"operator");
  logEvent("SUMMARY","overall=%s,communication=%s,stream=%s,stream_ms=%lu,anomalies=%lu,still=%s,X=%s,Y=%s,Z=%s",
    evidenceName(result()),productResponded?"PRODUCT_RESPONSE":"NOT_OBSERVED",evidenceName(streamResult()),
    (unsigned long)elapsedStream(),(unsigned long)anomalyCount(),
    stillDone?(stillOK?"COARSE_WINDOW":"REVIEW"):"NOT_RUN",
    motionDone[0]?(motionOK[0]?"OBSERVED":"REVIEW"):"NOT_RUN",
    motionDone[1]?(motionOK[1]?"OBSERVED":"REVIEW"):"NOT_RUN",
    motionDone[2]?(motionOK[2]?"OBSERVED":"REVIEW"):"NOT_RUN");
  for(int i=0;i<3;++i) {
    const Report &r=report[i];
    logEvent("REPORT_SUMMARY","type=%d,valid=%lu,invalid=%lu,timeout_episodes=%lu,sequence_missing=%lu,duplicates=%lu,sh2_time_nonmonotonic=%lu,max_gap_ms=%lu,mean_hz=%.2f,last_accuracy=%u",
      i,(unsigned long)r.good,(unsigned long)r.bad,(unsigned long)r.gaps,(unsigned long)r.missing,
      (unsigned long)r.duplicate,(unsigned long)r.timestampBack,(unsigned long)r.maxGap,elapsedStream()?r.good*1000.0f/elapsedStream():0,r.accuracy);
  }
  logEvent("TRANSPORT","read=%lu,read_error=%lu,write=%lu,write_error=%lu,int_wait_timeout=%lu,host_hw_reset=%lu,device_reset_event=%lu,decode_or_frame_error=%lu,oversize=%lu,report_enable_error=%lu",
    (unsigned long)transport.reads,(unsigned long)transport.readErrors,(unsigned long)transport.writes,
    (unsigned long)transport.writeErrors,(unsigned long)transport.intTimeouts,(unsigned long)transport.hardwareResets,
    (unsigned long)unexpectedResets,(unsigned long)transport.decodeErrors,(unsigned long)transport.oversized,(unsigned long)enableErrors);
  logEvent("INIT_TRANSPORT","read=%lu,read_error=%lu,write=%lu,write_error=%lu,int_wait_timeout=%lu,planned_host_hw_reset=%lu,decode_or_frame_error=%lu,oversize=%lu",
    (unsigned long)initTransport.reads,(unsigned long)initTransport.readErrors,(unsigned long)initTransport.writes,
    (unsigned long)initTransport.writeErrors,(unsigned long)initTransport.intTimeouts,(unsigned long)initTransport.hardwareResets,
    (unsigned long)initTransport.decodeErrors,(unsigned long)initTransport.oversized);
}
void finishSession(const char *reason) {
  if(!active)return;
  if(initialized)checkTimeouts();
  ended=millis();active=false;armed=false;stage=Stage::Finished;
  logEvent("FINISH","reason=%s,test_duration_ms=%lu",reason,(unsigned long)(ended-started));
  summary();
}
bool validId(const char *id) {
  const size_t n=strlen(id);if(!n||n>24)return false;
  for(size_t i=0;i<n;++i)if(!isalnum((unsigned char)id[i])&&id[i]!='_'&&id[i]!='-')return false;
  return true;
}
void startSession(const char *id,bool automatic=false) {
  if(!validId(id)){logEvent("COMMAND_REJECT","reason=id_1_to_24_ASCII_letters_digits_dash_underscore");return;}
  if(active) {
    if(automaticSession&&!automatic)finishSession("REPLACED_BY_OPERATOR_LABEL");
    else {logEvent("COMMAND_REJECT","reason=finish_current_session_first");return;}
  }
  sh2_close();
  for(auto &r:report)r=Report{};
  transport=Transport{};initTransport=Transport{};unexpectedResets=enableErrors=0;
  initialized=productResponded=armed=fault=stillDone=stillOK=false;motionAxis=-1;
  memset(motionDone,0,sizeof(motionDone));memset(motionOK,0,sizeof(motionOK));
  memset(accel,0,sizeof(accel));memset(gyro,0,sizeof(gyro));memset(quat,0,sizeof(quat));memset(rpy,0,sizeof(rpy));
  stillSum=stillSumSq=stillGyroPeak=stillMean=stillStd=0;stillAccelN=stillGyroN=0;
  strlcpy(module,id,sizeof(module));
  automaticSession=automatic;
  snprintf(runId,sizeof(runId),"%08lX-%04lu",(unsigned long)bootId,(unsigned long)++sessionNo);
  active=true;started=millis();stage=Stage::Initializing;
  drawScreen();
  logEvent("START","fw=%s,address=0x%02X,SDA=%d,SCL=%d,INT=%d,RST=%d,I2C_Hz=%lu,id_source=%s,unique_serial=not_read",
    FW_ID,BNO_ADDRESS,BNO_SDA,BNO_SCL,BNO_INT,BNO_RST,(unsigned long)I2C_HZ,
    automaticSession?"automatic_boot_label":"operator_label");
  // Dedicated bus: probe the selected address only; an ACK never constitutes a result.
  Wire.beginTransmission(BNO_ADDRESS);uint8_t ack=Wire.endTransmission();
  logEvent("I2C_PROBE","address=0x%02X,wire_result=%u",BNO_ADDRESS,ack);
  if(!imu.begin(BNO_ADDRESS,Wire,BNO_INT,BNO_RST)) {
    logEvent("INIT_FAILED","reason=no_valid_SH2_product_response");finishSession("INIT_FAILED");return;
  }
  if(!imu.prodIds.numEntries) {logEvent("INIT_FAILED","reason=empty_product_response");finishSession("INIT_FAILED");return;}
  productResponded=true;
  sh2_setSensorCallback(onSensorReport,nullptr);
  for(unsigned i=0;i<imu.prodIds.numEntries;++i) {
    const auto &p=imu.prodIds.entry[i];
    logEvent("PRODUCT","entry=%u,sw_part=%lu,version=%u.%u.%u,build=%lu,reset_cause=%u,unique_serial=not_available",
      i,(unsigned long)p.swPartNumber,p.swVersionMajor,p.swVersionMinor,p.swVersionPatch,(unsigned long)p.swBuildNumber,p.resetCause);
  }
  if(!reportsOn()){finishSession("REPORT_ENABLE_FAILED");return;}
  (void)imu.wasReset();
  logEvent("INIT_TRANSPORT","read_error=%lu,write_error=%lu,int_wait_timeout=%lu,host_hw_reset=%lu",
    (unsigned long)initTransport.readErrors,(unsigned long)initTransport.writeErrors,
    (unsigned long)initTransport.intTimeouts,(unsigned long)initTransport.hardwareResets);
  // Initialization contains planned resets; errors/timeouts still require review.
  if(initTransport.readErrors||initTransport.writeErrors||initTransport.intTimeouts||initTransport.decodeErrors||initTransport.oversized)fault=true;
  initialized=true;armed=true;streamStart=stageStart=millis();stage=Stage::Warmup;
  logEvent("STAGE_BEGIN","duration_ms=%lu,instruction=place_module_still",(unsigned long)WARMUP_MS);
}
void startAutomaticSession() {
  char id[25];
  snprintf(id,sizeof(id),"AUTO_%08lX",(unsigned long)bootId);
  startSession(id,true);
}
void startMotion(int axis) {
  if(!active||stage!=Stage::Ready){logEvent("COMMAND_REJECT","reason=wait_for_READY");return;}
  // Repeating a stage never overwrites its prior outcome; use a new session for retest.
  if(motionDone[axis]){logEvent("COMMAND_REJECT","reason=axis_already_recorded_start_new_session_for_retest");return;}
  motionAxis=axis;motionPeak=motionAngle=0;motionGyroN=motionQuatN=0;motionHasBase=false;
  stage=Stage::Motion;stageStart=millis();logEvent("STAGE_BEGIN","axis=%c,duration_ms=%lu,instruction=rotate_about_module_axis_slowly",'X'+axis,(unsigned long)MOVE_MS);
}
void updateStages() {
  const uint32_t dt=millis()-stageStart;
  if(stage==Stage::Warmup&&dt>=WARMUP_MS) {
    stage=Stage::Still;stageStart=millis();logEvent("STAGE_BEGIN","duration_ms=%lu,instruction=do_not_move",(unsigned long)STILL_MS);
  } else if(stage==Stage::Still&&dt>=STILL_MS) {
    stillDone=true;stillMean=stillAccelN?stillSum/stillAccelN:0;
    stillStd=stillAccelN?sqrtf(fmaxf(0,stillSumSq/stillAccelN-stillMean*stillMean)):0;
    stillOK=stillAccelN>=STILL_MS*MIN_OBSERVED_HZ/1000&&stillGyroN>=STILL_MS*MIN_OBSERVED_HZ/1000&&
      stillMean>=8.3f&&stillMean<=11.3f&&stillStd<=0.25f&&stillGyroPeak<=0.15f;
    logEvent("STILL_RESULT","result=%s,acc_n=%lu,gyro_n=%lu,g_mean_ms2=%.4f,g_std_ms2=%.4f,gyro_norm_peak_rads=%.4f",
      stillOK?"COARSE_WINDOW":"REVIEW",(unsigned long)stillAccelN,(unsigned long)stillGyroN,stillMean,stillStd,stillGyroPeak);
    stage=Stage::Ready;logEvent("READY","instruction=send_X_then_Y_then_Z_each_8_seconds");
  } else if(stage==Stage::Motion&&dt>=MOVE_MS) {
    motionDone[motionAxis]=true;
    motionOK[motionAxis]=motionGyroN>=MOVE_MS*MIN_OBSERVED_HZ/1000&&motionQuatN>=MOVE_MS*MIN_OBSERVED_HZ/1000&&motionPeak>=0.6f&&motionAngle>=20;
    logEvent("MOTION_RESULT","axis=%c,result=%s,gyro_n=%lu,quat_n=%lu,axis_gyro_peak_rads=%.4f,orientation_change_deg=%.2f",
      'X'+motionAxis,motionOK[motionAxis]?"OBSERVED":"REVIEW",(unsigned long)motionGyroN,(unsigned long)motionQuatN,motionPeak,motionAngle);
    stage=Stage::Ready;
  }
}
void info() {
  Serial.printf("FW,%s,title=BNO086 module test,board=ESP32-2432S028R,display=%s,address=0x%02X,boot=%08lX\n",FW_ID,
#ifdef ST7789_DRIVER
    "ST7789",
#else
    "ILI9341",
#endif
    BNO_ADDRESS,(unsigned long)bootId);
  Serial.println("HELP,START M001 | X | Y | Z | STATUS | FINISH | INFO; disconnect power before changing modules");
  logEvent("STATE","active=%d,initialized=%d,reports_armed=%d,int_level=%d,session_mode=%s,boot_action=new_automatic_session",
    active,initialized,armed,digitalRead(BNO_INT),!runId[0]?"none":automaticSession?"automatic_boot":"operator");
}
void command(const char *s) {
  if(!strncmp(s,"START ",6))startSession(s+6);
  else if(!strcmp(s,"X"))startMotion(0);else if(!strcmp(s,"Y"))startMotion(1);else if(!strcmp(s,"Z"))startMotion(2);
  else if(!strcmp(s,"FINISH"))finishSession("OPERATOR_FINISH");
  else if(!strcmp(s,"STATUS")){if(initialized&&active)checkTimeouts();summary();}
  else if(!strcmp(s,"INFO"))info();
  else logEvent("COMMAND_REJECT","reason=unknown_command");
}
void readCommands() {
  while(Serial.available()) {
    const char c=Serial.read();
    if(c=='\r')continue;
    if(c=='\n') {
      if(lineOverflow)logEvent("COMMAND_REJECT","reason=line_too_long");
      else if(lineLength){lineBuffer[lineLength]='\0';command(lineBuffer);}
      lineLength=0;lineOverflow=false;
    } else if(lineLength<sizeof(lineBuffer)-1)lineBuffer[lineLength++]=c;
    else lineOverflow=true;
  }
}
void telemetry() {
  if(stage==Stage::Finished){lastTick=millis();return;}
  const uint32_t now=millis();const uint32_t dt=now-lastTick;
  for(auto &r:report){r.hz=dt?(r.good-r.rateBase)*1000.0f/dt:0;r.rateBase=r.good;}
  lastTick=now;
  if(!active||!initialized)return;
  logEvent("SAMPLE","a_ms2=%.3f/%.3f/%.3f,g_dps=%.2f/%.2f/%.2f,rpy_deg=%.1f/%.1f/%.1f,hz=%.1f/%.1f/%.1f,valid=%lu/%lu/%lu,age_ms=%lu/%lu/%lu,sh2_estimated_us=%llu/%llu/%llu,accuracy=%u/%u/%u,anomalies=%lu",
    accel[0],accel[1],accel[2],gyro[0]*RAD_TO_DEG,gyro[1]*RAD_TO_DEG,gyro[2]*RAD_TO_DEG,rpy[0],rpy[1],rpy[2],report[0].hz,report[1].hz,report[2].hz,
    (unsigned long)report[0].good,(unsigned long)report[1].good,(unsigned long)report[2].good,
    (unsigned long)(now-(report[0].seen?report[0].lastMs:streamStart)),(unsigned long)(now-(report[1].seen?report[1].lastMs:streamStart)),(unsigned long)(now-(report[2].seen?report[2].lastMs:streamStart)),
    (unsigned long long)report[0].sensorUs,(unsigned long long)report[1].sensorUs,(unsigned long long)report[2].sensorUs,
    report[0].accuracy,report[1].accuracy,report[2].accuracy,(unsigned long)anomalyCount());
}
void text(int y,const char *s,int font=2,uint16_t color=TFT_WHITE) {
  // Opaque glyph backgrounds and padding remove the previous line's trailing text.
  tft.setTextColor(color,TFT_BLACK);tft.setTextPadding(304);tft.drawString(s,6,y,font);
  // Drain ready data between rows so display drawing cannot delay an entire frame.
  if(active&&initialized&&digitalRead(BNO_INT)==LOW) sh2_service();
}
void drawScreen() {
  static bool painted=false, previousLayoutInitialized=false;
  char s[112];
  // Clear only when the two layouts change; regular updates never blank the full screen.
  if(!painted||previousLayoutInitialized!=initialized) {
    tft.fillScreen(TFT_BLACK);
    chinese.setForegroundColor(TFT_WHITE);chinese.drawUTF8(6,17,"BNO086 模块测试");
    painted=true;previousLayoutInitialized=initialized;
  }
  snprintf(s,sizeof(s),"ID:%s  run:%04lu",module,(unsigned long)sessionNo);text(21,s);
  uint32_t remaining=0;uint32_t dt=millis()-stageStart;
  const uint32_t limit=stage==Stage::Warmup?WARMUP_MS:stage==Stage::Still?STILL_MS:stage==Stage::Motion?MOVE_MS:0;
  if(limit&&dt<limit)remaining=(limit-dt+999)/1000;
  snprintf(s,sizeof(s),"%s %c %lus   0x%02X %s",stageName(),stage==Stage::Motion?'X'+motionAxis:' ',(unsigned long)remaining,BNO_ADDRESS,
    initialized?"SH2 OK":stage==Stage::Idle?"WAIT START":stage==Stage::Initializing?"STARTING":"NO SH2");text(39,s,2);
  if(!initialized) {
    text(68,stage==Stage::Finished?"INIT INCOMPLETE":stage==Stage::Initializing?"Starting sensor...":"Waiting for START",4);
    text(108,"SDA27 SCL22 INT35 RST19");text(130,"GPIO21 = BACKLIGHT");
    text(158,"TF card OUT / power 3.3V");text(204,"INFO / STATUS / FINISH");return;
  }
  const uint32_t now=active?millis():ended;
  const bool freshA=report[0].seen&&now-report[0].lastMs<=GAP_MS;
  const bool freshG=report[1].seen&&now-report[1].lastMs<=GAP_MS;
  const bool freshQ=report[2].seen&&now-report[2].lastMs<=GAP_MS;
  snprintf(s,sizeof(s),"A m/s2: %+.2f %+.2f %+.2f %s",accel[0],accel[1],accel[2],freshA?"":"OLD");text(59,s);
  snprintf(s,sizeof(s),"G d/s: %+.1f %+.1f %+.1f %s",gyro[0]*RAD_TO_DEG,gyro[1]*RAD_TO_DEG,gyro[2]*RAD_TO_DEG,freshG?"":"OLD");text(77,s);
  snprintf(s,sizeof(s),"RPY: %+.0f %+.0f %+.0f %s",rpy[0],rpy[1],rpy[2],freshQ?"":"OLD");text(95,s);
  snprintf(s,sizeof(s),"Hz A/G/Q: %.1f / %.1f / %.1f",report[0].hz,report[1].hz,report[2].hz);text(113,s);
  snprintf(s,sizeof(s),"Valid: %lu / %lu / %lu",(unsigned long)report[0].good,(unsigned long)report[1].good,(unsigned long)report[2].good);text(131,s);
  snprintf(s,sizeof(s),"I2Cerr:%lu Bad:%lu Gap:%lu Rst:%lu",
    (unsigned long)(transport.readErrors+transport.writeErrors+initTransport.readErrors+initTransport.writeErrors),
    (unsigned long)(report[0].bad+report[1].bad+report[2].bad+transport.decodeErrors+initTransport.decodeErrors),
    (unsigned long)(report[0].gaps+report[1].gaps+report[2].gaps+transport.intTimeouts+initTransport.intTimeouts),
    (unsigned long)(unexpectedResets+transport.hardwareResets));text(149,s);
  snprintf(s,sizeof(s),"Still:%s XYZ:%c%c%c   %lus/60s",stillDone?(stillOK?"OK":"CHECK"):"WAIT",
    motionDone[0]?(motionOK[0]?'O':'!'):'-',motionDone[1]?(motionOK[1]?'O':'!'):'-',motionDone[2]?(motionOK[2]?'O':'!'):'-',(unsigned long)(elapsedStream()/1000));text(167,s);
  const char *status=(fault||anomalyCount())?"REVIEW":stage==Stage::Finished?evidenceName(result()):"TESTING";
  text(188,status,4,TFT_WHITE);
  text(219,automaticSession?"AUTO label; START M001 to assign":
    stage==Stage::Ready?"Serial X/Y/Z; FINISH after 60s":"Serial STATUS / FINISH");
}
void setup() {
  Serial.begin(115200);bootId=esp_random();
  // Keep unused SPI peripherals deselected. GPIO19 is NOT initialized as SD MISO.
  pinMode(5,OUTPUT);digitalWrite(5,HIGH);pinMode(33,OUTPUT);digitalWrite(33,HIGH);
  pinMode(BNO_INT,INPUT); // GPIO35 has no internal pull-up; external bias is an optional module-specific check.
  Wire.setBufferSize(128);Wire.begin(BNO_SDA,BNO_SCL,I2C_HZ);Wire.setTimeOut(50);
  tft.init();tft.setRotation(1);tft.invertDisplay(false);
  chinese.begin(tft);chinese.setFont(u8g2_font_wqy16_t_gb2312);chinese.setFontMode(1);
  info();lastTick=millis();drawScreen();
  startAutomaticSession();
}
void loop() {
  readCommands();
  if(active&&initialized) {
    // BNO086 does not support I2C polling without INT. Drain ready packets only.
    for(int n=0;n<24&&digitalRead(BNO_INT)==LOW;++n) {sh2_service();yield();}
    if(imu.wasReset()) {
      ++unexpectedResets;fault=true;logEvent("DEVICE_RESET","instruction=review_power_bus_and_module");
      for(auto &r:report){r.seen=false;r.timedOut=false;}
      if(!reportsOn())finishSession("REPORT_REENABLE_FAILED");
    }
    if(active){checkTimeouts();updateStages();}
  }
  if(millis()-lastTick>=1000)telemetry();
  if(millis()-lastDraw>=500){lastDraw=millis();drawScreen();}
  delay(1);
}