#pragma once
#include <Arduino.h>
#include <U8g2lib.h>
#include <Wire.h>
#include <math.h>
U8G2_SSD1306_128X64_NONAME_F_HW_I2C screen(U8G2_R0,U8X8_PIN_NONE,22,21);
bool oledReady=false,environmentLocked=false;
double lockedSpeed=343;float lockedTemp=NAN,lockedRh=NAN;
float temperatureNow=NAN,humidityNow=NAN,candidateTemp=NAN,candidateRh=NAN;
uint32_t environmentTime=0;
double soundSpeed(double t,double rh) {
 const double T=t+273.15,p=101325,xc=.0004;
 const double psv=exp(1.2811805e-5*T*T-.019509874*T+34.04926034-6353.6311/T);
 const double f=1.00062+3.14e-8*p+5.6e-7*t*t,xw=rh/100*f*psv/p;
 return 331.5024+.603055*t-.000528*t*t+(51.471935+.1495874*t-.000782*t*t)*xw
 +(-1.82e-7+3.73e-8*t-2.93e-10*t*t)*p+(-85.20931-.228525*t+5.91e-5*t*t)*xc
 -2.835149*xw*xw-2.15e-13*p*p+29.179762*xc*xc+.000486*xw*p*xc;
}

// 一帧确认孤立跳点；快运动用最近3点，连续稳定才使用长窗口。
struct MotionChannel {
 struct Point { double t,d; } points[20], raw[3];
 int count=0,rawCount=0;double lastTrigger=0;
 double distance=NAN,velocity=NAN;
 const char* status="WAIT";
 void reset(){count=rawCount=0;lastTrigger=0;distance=velocity=NAN;status="NO ECHO";}
 void update(int64_t trigger,int64_t width){
  if(!environmentLocked){reset();status="WAIT T";return;}
  updateMeasurement(double(trigger)+width/2.,width*lockedSpeed/2000000.,true);
 }
 void updateMeasurement(double time,double measured,bool rejectSpikes){
  if(lastTrigger&&(time<=lastTrigger||time-lastTrigger>300000))reset();
  lastTrigger=time;distance=velocity=NAN;status="WARM";
  if(measured<.02||measured>(rejectSpikes?4.:2.)){reset();status="RANGE";return;}
  Point p={double(time),measured};
  if(rejectSpikes){
   if(rawCount==3){raw[0]=raw[1];raw[1]=raw[2];rawCount=2;}
   raw[rawCount++]=p;if(rawCount<3)return;
   p=raw[1];
   const double predicted=raw[0].d+(raw[2].d-raw[0].d)*(p.t-raw[0].t)/(raw[2].t-raw[0].t);
   if(fabs(raw[2].d-raw[0].d)<.02 && fabs(p.d-predicted)>.04){
    status="JUMP";Serial.printf("# outlier t_us=%.0f raw_m=%.6f\n",p.t,p.d);return;
   }
  }
  distance=p.d;
  while(count && p.t-points[0].t>1250000.){for(int i=1;i<count;i++)points[i-1]=points[i];count--;}
  if(count==20){for(int i=1;i<count;i++)points[i-1]=points[i];count--;}
  points[count++]=p;if(count<3)return;
  double low=points[0].d,high=low;
  for(int i=1;i<count;i++){low=min(low,points[i].d);high=max(high,points[i].d);}
  const bool stable=count>=5&&p.t-points[0].t>=1100000.&&high-low<.015;
  const int first=stable?0:count-3,n=count-first;
  if(p.t-points[first].t>450000.&&!stable)return;
  double ts[20],tm=0,ym=0;
  for(int i=0;i<n;i++){ts[i]=(points[first+i].t-points[first].t)/1e6;tm+=ts[i]/n;ym+=points[first+i].d/n;}
  double num=0,denom=0;
  for(int i=0;i<n;i++){num+=(ts[i]-tm)*(points[first+i].d-ym);denom+=(ts[i]-tm)*(ts[i]-tm);}
  if(denom<=0){reset();status="TIME";return;}
  const double v=num/denom;double sumSq=0;
  for(int i=0;i<n;i++){double e=points[first+i].d-ym-v*(ts[i]-tm);sumSq+=e*e;}
  const double threshold=max(.003,min(.01,2*sqrt(sumSq/(n-2)/denom)));
  velocity=fabs(v)<=threshold?0:v;
  // 曲率/加速度也会产生残差：提示不稳定，仍显示短窗口平均速度。
  status=sqrt(sumSq/n)>.01?"JUMP":velocity==0?"STILL":v>0?"AWAY":"TOWARD";
 }
} channels[2];
void updateEnvironment(float t,float rh){
 temperatureNow=t;humidityNow=rh;environmentTime=millis();
 if(!environmentLocked&&t>=0&&t<=30&&rh>=0&&rh<=100){
  if(isfinite(candidateTemp)&&fabs(t-candidateTemp)<=2&&fabs(rh-candidateRh)<=10){
   lockedSpeed=soundSpeed(t,rh);lockedTemp=t;lockedRh=rh;environmentLocked=true;
   channels[0].reset();
   Serial.printf("# environment_locked temp=%.1f rh=%.1f c=%.6f\n",t,rh,lockedSpeed);
  }
  candidateTemp=t;candidateRh=rh;
 }else if(!environmentLocked){candidateTemp=candidateRh=NAN;}
}
void setupDisplay(){
 Wire.begin(21,22);Wire.setTimeOut(20);Wire.beginTransmission(0x3C);oledReady=Wire.endTransmission()==0;
 Serial.printf("# oled detected=%d addr=0x3C\n",oledReady);
 if(oledReady){screen.setI2CAddress(0x3C*2);screen.setBusClock(400000);screen.begin();}
}
const char* chineseStatus(const char* status) {
 if(!strcmp(status,"AWAY"))return "远离";
 if(!strcmp(status,"TOWARD"))return "靠近";
 if(!strcmp(status,"STILL"))return "近静止";
 if(!strcmp(status,"JUMP"))return "不稳定";
 if(!strcmp(status,"RANGE"))return "超范围";
 if(!strcmp(status,"NO ECHO"))return "无回波";
 if(!strcmp(status,"WAIT T"))return "等温度";
 return "采集中";
}
void testChineseFont(){
 screen.setFont(u8g2_font_wqy12_t_gb2312);
 const uint16_t chars[]={0x58f0,0x6fc0,0x5149,0x5f85,0x6e29,0x5ea6,0x8865,0x507f,0x8fdc,0x79bb,0x9760,0x8fd1,0x9759,0x6b62,0x4e0d,0x7a33,0x5b9a,0x8d85,0x8303,0x56f4,0x65e0,0x56de,0x6ce2,0x7b49,0x91c7,0x96c6,0x4e2d};
 int missing=0;for(auto ch:chars)if(!u8g2_IsGlyph(screen.getU8g2(),ch))missing++;
 Serial.printf("# chinese_font missing=%d label_width=%u available_width=62\n",missing,screen.getUTF8Width("温度补偿"));
}
void drawDisplay() {
 if(!oledReady)return;
 screen.clearBuffer();char d[20],v[20],text[44];
 for(int i=0;i<2;i++){
  auto &c=channels[i];const int top=i*33;
  const bool fresh=c.lastTrigger && esp_timer_get_time()-c.lastTrigger<500000;
  screen.setFont(u8g2_font_wqy12_t_gb2312);
  screen.drawUTF8(0,top+11,i==0?"超声波":"激光");
  screen.drawUTF8(80,top+11,fresh?chineseStatus(c.status):"等待");
  if(fresh&&isfinite(c.distance))snprintf(d,sizeof(d),"%.1fcm",c.distance*100);else snprintf(d,sizeof(d),"--cm");
  if(fresh&&isfinite(c.velocity))snprintf(v,sizeof(v),fabs(c.velocity)<.0005?"%.3fm/s":"%+.3fm/s",fabs(c.velocity)<.0005?0.:c.velocity);else snprintf(v,sizeof(v),"--m/s");
  snprintf(text,sizeof(text),"%s %s",d,v);screen.setFont(u8g2_font_6x12_tf);screen.drawStr(0,top+27,text);
 }
 screen.drawHLine(0,31,128);screen.sendBuffer();
 Serial.printf("# oled A_d=%.6f A_v=%.6f A_status=%s B_d=%.6f B_v=%.6f B_status=%s\n",channels[0].distance,channels[0].velocity,channels[0].status,channels[1].distance,channels[1].velocity,channels[1].status);
}