#include <Arduino.h>
#include <WiFi.h>
#include <WiFiMulti.h>
#include <WiFiClientSecure.h>
#include <WebSocketsClient.h>
#include <ESP32Servo.h>
#include <WebServer.h>
#include <HTTPClient.h>
#include <FS.h>
#include "SPIFFS.h"
#include "ESPmDNS.h"
#include <Ticker.h>

// 全局变量定义
WiFiMulti WiFiMulti;
WebSocketsClient webSocket;
Servo myServoLeft;   // 左车轮舵机
Servo myServoRight;  // 右车轮舵机
Servo myBig;         // 大臂舵机（180度）
Servo mySmall;       // 小臂舵机（180度）
Servo myWadb;        // 挖斗舵机（180度）
Servo servoJiuf;     // 机身舵机（改为180度）

// 舵机参数（统一180度舵机配置）
const int minPulse = 500;   // 最小脉宽(微秒)
const int maxPulse = 2500;  // 最大脉宽(微秒)
int servoPinLeft = 5;       // 左车轮引脚
int servoPinRight = 6;      // 右车轮引脚
const int stopPos = 90;     // 所有180度舵机的停止位置（中间点）
int baudRate = 115200;

// 引脚定义
int bigUpper = 18;    // 大臂上限限位开关（180度）
int bigLower = 8;     // 大臂下限限位开关（0度）
int smallUpper = 39;  // 小臂上限限位开关（180度）
int smallLower = 38;  // 小臂下限限位开关（0度）
int wadbUpper = 41;   // 挖斗上限限位开关（180度）
int wadbLower = 40;   // 挖斗下限限位开关（0度）
int bigExc = 7;       // 大臂舵机引脚
int smallExc = 17;    // 小臂舵机引脚
int wadb = 16;        // 挖斗舵机引脚
int fuselage = 4;     // 机身舵机引脚（180度）

// 状态变量
int bigstate = 2;                 // 0:下限  1:上限  2:可活动（大臂）
int smallstate = 2;               // 小臂状态（同上）
int wadbstate = 2;                // 挖斗状态（同上）

int bigAngle = 90;                // 大臂当前角度（初始停止位置）
int smallAngle = 90;              // 小臂当前角度（初始停止位置）
int wadbAngle = 90;               // 挖斗当前角度（初始停止位置）
int fuselageAngle = 90;           // 机身当前角度（初始停止位置，180度范围）
unsigned long extiTimeBig = 0;    // 大臂中断防抖时间
unsigned long extiTimeSmall = 0;  // 小臂中断防抖时间
unsigned long extiTimeWadb = 0;   // 挖斗中断防抖时间
unsigned long extiTimeFuselage = 0; // 机身边界防抖时间（若有机身限位）
int commandBig = -1;              // 大臂命令（0:下降 1:上升 -1:无）
int commandSmall = -1;            // 小臂命令（0:收缩 1:伸展 -1:无）
int commandWadb = -1;             // 挖斗命令（0:收 1:开 -1:无）
int commandFuselage = -1;         // 机身命令（0:左转 1:右转 -1:无）
bool extiBigOpen = false;         // 大臂上限 触发标志
bool extiBigClose = false;        // 大臂下限 触发标志
bool extiSmallOpen = false;       // 小臂上限 触发标志
bool extiSmallClose = false;      // 小臂下限 触发标志
bool extiWadbOpen = false;        // 挖斗上限 触发标志
bool extiWadbClose = false;       // 挖斗下限 触发标志
bool extiFuselageLeft = false;    // 机身左边界触发（若有机身限位）
bool extiFuselageRight = false;   // 机身后边界触发（若有机身限位）

// WiFi与设备信息
Ticker myTicker;
String flashWifi = "/wifi.txt";     // WiFi配置文件
WebServer webServer(80);            // WebServer端口
String ssid = "webserver";          // WiFi名称
String passwd = "www.nobiggie.cn";  // WiFi密码
String html;
WiFiClient wificlient;
String deviceName = "DYZKTEST0016";


// 工具函数：十六进制打印
void hexdump(const void* mem, uint32_t len, uint8_t cols = 16) {
  const uint8_t* src = (const uint8_t*)mem;
  Serial.printf("\n[HEXDUMP] Address: 0x%08X len: 0x%X (%d)", (ptrdiff_t)src, len, len);
  for (uint32_t i = 0; i < len; i++) {
    if (i % cols == 0) {
      Serial.printf("\n[0x%08X] 0x%08X: ", (ptrdiff_t)src, i);
    }
    Serial.printf("%02X ", *src);
    src++;
  }
  Serial.printf("\n");
}

// WebSocket事件处理
void webSocketEvent(WStype_t type, uint8_t* payload, size_t length) {
  String msg;
  switch (type) {
    case WStype_DISCONNECTED:
      Serial.printf("[WSc] Disconnected!\n");
      // 断开连接时复位所有舵机到停止位置
      myServoLeft.write(stopPos);
      myServoRight.write(stopPos);
      myBig.write(stopPos);
      mySmall.write(stopPos);
      myWadb.write(stopPos);
      servoJiuf.write(stopPos);  // 机身回到中间停止位置
      commandBig = -1;
      commandSmall = -1;
      commandWadb = -1;
      commandFuselage = -1;
      break;

    case WStype_CONNECTED:
      Serial.printf("[WSc] Connected to url: %s\n", payload);
      webSocket.sendTXT("+NET_" + deviceName + "-");  // 发送设备标识
      break;

    case WStype_TEXT:
      for (int i = 0; i < length; i++) {
        msg += (char)payload[i];
      }
      if (msg.indexOf("heart#") != -1) break;  // 忽略心跳包
      Serial.println("收到指令: " + msg);

      // 车轮控制（保持不变）
      if (msg.indexOf("ltw#") != -1) {  // 左前进
        myServoLeft.write(130);
      }
      if (msg.indexOf("rtw#") != -1) {  // 右前进
        myServoRight.write(130);
      }
      if (msg.indexOf("lts#") != -1) {  // 左后退
        myServoLeft.write(50);
        break;
      }
      if (msg.indexOf("rts#") != -1) {  // 右后退
        myServoRight.write(50);
        break;
      }
      if (msg.indexOf("mix#") != -1) {  // 停止移动
        myServoLeft.write(stopPos);
        myServoRight.write(stopPos);
        break;
      }

      // 机身旋转控制（改为180度角度控制）
      if (msg.indexOf("cztz#") != -1) {  // 机身左转（未到左边界0度）
        fuselageAngle = constrain(fuselageAngle - 1, 0, 360);  // 角度减小（左转）
        servoJiuf.write(fuselageAngle);
        commandFuselage = 0;
        break;
      }
      if (msg.indexOf("czty#") != -1) {  // 机身右转（未到右边界180度）
        fuselageAngle = constrain(fuselageAngle + 1, 0, 360);  // 角度增大（右转）
        servoJiuf.write(fuselageAngle);
        commandFuselage = 1;
        break;
      }
      if (msg.indexOf("js#0") != -1) {  // 机身停止（回到中间90度）
        servoJiuf.write(stopPos);
        fuselageAngle = stopPos;
        commandFuselage = -1;
        break;
      }

      // 大臂控制（180度范围：0-180，90度停止）
      if (msg.indexOf("dbs#") != -1 && bigstate != 1) {  // 大臂升（未到180度上限）
        bigAngle = constrain(bigAngle + 1, 0, 180);       // 限制角度在0-180
        myBig.write(bigAngle);
        commandBig = 1;
        break;
      }
      if (msg.indexOf("dbx#") != -1 && bigstate != 0) {  // 大臂降（未到0度下限）
        bigAngle = constrain(bigAngle - 1, 0, 180);
        myBig.write(bigAngle);
        commandBig = 0;
        break;
      }
      if (msg.indexOf("db#0") != -1) {  // 大臂停（回到90度）
        myBig.write(stopPos);
        bigAngle = stopPos;
        commandBig = -1;
        break;
      }

      // 小臂控制（180度范围：0-180，90度停止）
      if (msg.indexOf("xbs#") != -1 && smallstate != 1) {  // 小臂伸（未到180度上限）
        smallAngle = constrain(smallAngle + 1, 0, 180);
        mySmall.write(smallAngle);
        commandSmall = 1;
        break;
      }
      if (msg.indexOf("xbx#") != -1 && smallstate != 0) {  // 小臂缩（未到0度下限）
        smallAngle = constrain(smallAngle - 1, 0, 180);
        mySmall.write(smallAngle);
        commandSmall = 0;
        break;
      }
      if (msg.indexOf("xb#0") != -1) {  // 小臂停（回到90度）
        mySmall.write(stopPos);
        smallAngle = stopPos;
        commandSmall = -1;
        break;
      }

      // 挖斗控制（180度范围：0-180，90度停止）
      if (msg.indexOf("wds#") != -1 && wadbstate != 1) {  // 挖斗开（未到180度上限）
        wadbAngle = constrain(wadbAngle + 1, 0, 180);
        myWadb.write(wadbAngle);
        commandWadb = 1;
        break;
      }
      if (msg.indexOf("wdx#") != -1 && wadbstate != 0) {  // 挖斗收（未到0度下限）
        wadbAngle = constrain(wadbAngle - 1, 0, 180);
        myWadb.write(wadbAngle);
        commandWadb = 0;
        break;
      }
      if (msg.indexOf("wd#0") != -1) {  // 挖斗停（回到90度）
        myWadb.write(stopPos);
        wadbAngle = stopPos;
        commandWadb = -1;
        break;
      }
      break;

    // 其他事件类型处理（保持不变）
    case WStype_BIN:
      Serial.printf("[WSc] 收到二进制数据，长度: %u\n", length);
      hexdump(payload, length);
      break;
    case WStype_ERROR:
    case WStype_FRAGMENT_TEXT_START:
    case WStype_FRAGMENT_BIN_START:
    case WStype_FRAGMENT:
    case WStype_FRAGMENT_FIN:
      break;
    case WStype_PING:
      Serial.printf("[WSc] 收到PING\n");
      break;
    case WStype_PONG:
      Serial.printf("[WSc] 收到PONG\n");
      break;
  }
}

// 大臂上限 中断处理
void IRAM_ATTR handleInterruptBigUpper() {
  unsigned long currentTime = millis();
  if (currentTime - extiTimeBig > 50) {  // 50ms防抖
    extiTimeBig = currentTime;
    if (digitalRead(bigUpper) == LOW) {  // 限位触发（低电平有效）
      bigstate = 1;                      // 标记为上限 
      extiBigOpen = true;
    }
  }
}

// 大臂下限 中断处理
void IRAM_ATTR handleInterruptBigLower() {
  unsigned long currentTime = millis();
  if (currentTime - extiTimeBig > 50) {  // 50ms防抖
    extiTimeBig = currentTime;
    if (digitalRead(bigLower) == LOW) {  // 限位触发
      bigstate = 0;                      // 标记为下限 
      extiBigClose = true;
    }
  }
}

// 小臂上限 中断处理
void IRAM_ATTR handleInterruptSmallUpper() {
  unsigned long currentTime = millis();
  if (currentTime - extiTimeSmall > 50) {  // 50ms防抖
    extiTimeSmall = currentTime;
    if (digitalRead(smallUpper) == LOW) {  // 限位触发
      smallstate = 1;                      // 标记为上限 
      extiSmallOpen = true;
    }
  }
}

// 小臂下限 中断处理
void IRAM_ATTR handleInterruptSmallLower() {
  unsigned long currentTime = millis();
  if (currentTime - extiTimeSmall > 50) {  // 50ms防抖
    extiTimeSmall = currentTime;
    if (digitalRead(smallLower) == LOW) {  // 限位触发
      smallstate = 0;                      // 标记为下限 
      extiSmallClose = true;
    }
  }
}

// 挖斗上限中断处理
void IRAM_ATTR handleInterruptWadbUpper() {
  unsigned long currentTime = millis();
  if (currentTime - extiTimeWadb > 50) {  // 50ms防抖
    extiTimeWadb = currentTime;
    if (digitalRead(wadbUpper) == LOW) {  // 限位触发
      wadbstate = 1;                      // 标记为上限 
      extiWadbOpen = true;
    }
  }
}

// 挖斗下限 中断处理
void IRAM_ATTR handleInterruptWadbLower() {
  unsigned long currentTime = millis();
  if (currentTime - extiTimeWadb > 50) {  // 50ms防抖
    extiTimeWadb = currentTime;
    if (digitalRead(wadbLower) == LOW) {  // 限位触发
      wadbstate = 0;                      // 标记为下限 
      extiWadbClose = true;
    }
  }
}



void setup() {
  Serial.begin(baudRate);
  for (uint8_t t = 4; t > 0; t--) {
    Serial.printf("[SETUP] 启动等待 %d...\n", t);
    delay(1000);
  }

  // 初始化SPIFFS（如需读取WiFi配置）
  if (!SPIFFS.begin(true)) {
    Serial.println("SPIFFS初始化失败");
  }

  // 连接WiFi（保持不变）
  WiFiMulti.addAP(ssid.c_str(), passwd.c_str());
  Serial.print("连接WiFi...");
  while (WiFiMulti.run() != WL_CONNECTED) {
    delay(100);
    Serial.print(".");
  }
  Serial.println("\nWiFi已连接，IP: " + WiFi.localIP().toString());

  // 舵机初始化（分配PWM定时器）
  ESP32PWM::allocateTimer(0);
  myServoLeft.setPeriodHertz(50);
  myServoLeft.attach(servoPinLeft, minPulse, maxPulse);
  myServoLeft.write(stopPos);

  ESP32PWM::allocateTimer(1);
  myServoRight.setPeriodHertz(50);
  myServoRight.attach(servoPinRight, minPulse, maxPulse);
  myServoRight.write(stopPos);

  ESP32PWM::allocateTimer(2);  // 大臂（180度）
  myBig.setPeriodHertz(50);
  myBig.attach(bigExc, minPulse, maxPulse);
  myBig.write(stopPos);  // 初始停止在90度

  ESP32PWM::allocateTimer(3);  // 小臂（180度）
  mySmall.setPeriodHertz(50);
  mySmall.attach(smallExc, minPulse, maxPulse);
  mySmall.write(stopPos);  // 初始停止在90度

  ESP32PWM::allocateTimer(4);  // 挖斗（180度）
  myWadb.setPeriodHertz(50);
  myWadb.attach(wadb, minPulse, maxPulse);
  myWadb.write(stopPos);  // 初始停止在90度

  // 机身舵机初始化（改为180度控制，与其他舵机统一）
  ESP32PWM::allocateTimer(5);
  servoJiuf.setPeriodHertz(50);  // 50Hz舵机标准频率
  servoJiuf.attach(fuselage, minPulse, maxPulse);  // 共用脉宽参数
  servoJiuf.write(stopPos);  // 初始位置：中间90度（停止状态）
  Serial.println("所有舵机初始化完成");

  // 限位开关初始化（上拉输入，低电平触发）
  pinMode(bigUpper, INPUT_PULLUP);    // 对应180度上限
  pinMode(bigLower, INPUT_PULLUP);    // 对应0度下限
  pinMode(smallUpper, INPUT_PULLUP);  // 对应180度上限
  pinMode(smallLower, INPUT_PULLUP);  // 对应0度下限
  pinMode(wadbUpper, INPUT_PULLUP);   // 对应180度上限
  pinMode(wadbLower, INPUT_PULLUP);   // 对应0度下限
  // 若有机身限位开关，添加引脚初始化（示例）：
  // pinMode(fuselageLeftPin, INPUT_PULLUP);   // 机身左边界（0度）
  // pinMode(fuselageRightPin, INPUT_PULLUP);  // 机身后边界（180度）

  // 绑定限位中断（机身限位若有则添加）
  attachInterrupt(digitalPinToInterrupt(bigUpper), handleInterruptBigUpper, CHANGE);
  attachInterrupt(digitalPinToInterrupt(bigLower), handleInterruptBigLower, CHANGE);
  attachInterrupt(digitalPinToInterrupt(smallUpper), handleInterruptSmallUpper, CHANGE);
  attachInterrupt(digitalPinToInterrupt(smallLower), handleInterruptSmallLower, CHANGE);
  attachInterrupt(digitalPinToInterrupt(wadbUpper), handleInterruptWadbUpper, CHANGE);
  attachInterrupt(digitalPinToInterrupt(wadbLower), handleInterruptWadbLower, CHANGE);
  // 若有机身限位，添加中断绑定（示例）：
  // attachInterrupt(digitalPinToInterrupt(fuselageLeftPin), handleInterruptFuselageLeft, CHANGE);
  // attachInterrupt(digitalPinToInterrupt(fuselageRightPin), handleInterruptFuselageRight, CHANGE);
  Serial.println("限位中断初始化完成");

  // WebSocket初始化（保持不变）
  webSocket.beginSSL("www.dyzk520.com", 443, "/ws");
  webSocket.onEvent(webSocketEvent);
  webSocket.setReconnectInterval(5000);  // 重连间隔5秒
  Serial.println("WebSocket初始化完成");
}

void loop() {
  webSocket.loop();  // 处理WebSocket通信

  // 发送心跳包（每800ms）
  static unsigned long lastPingTime = 0;
  if (millis() - lastPingTime >= 800) {
    webSocket.sendTXT("+NET_" + deviceName + "&111111-");
    lastPingTime = millis();
  }

  // 处理大臂限位触发
  if (extiBigOpen) {
    Serial.println("大臂到达180度上限");
    bigAngle = 180;  // 强制设为最大角度
    myBig.write(bigAngle);
    commandBig = -1;  // 停止动作
    extiBigOpen = false;
  }
  if (extiBigClose) {
    Serial.println("大臂到达0度下限");
    bigAngle = 0;  // 强制设为最小角度
    myBig.write(bigAngle);
    commandBig = -1;  // 停止动作
    extiBigClose = false;
  }

  // 处理小臂限位触发
  if (extiSmallOpen) {
    Serial.println("小臂到达180度上限");
    smallAngle = 180;  // 强制设为最大角度
    mySmall.write(smallAngle);
    commandSmall = -1;  // 停止动作
    extiSmallOpen = false;
  }
  if (extiSmallClose) {
    Serial.println("小臂到达0度下限");
    smallAngle = 0;  // 强制设为最小角度
    mySmall.write(smallAngle);
    commandSmall = -1;  // 停止动作
    extiSmallClose = false;
  }

  // 处理挖斗限位触发
  if (extiWadbOpen) {
    Serial.println("挖斗到达180度上限");
    wadbAngle = 180;  // 强制设为最大角度
    myWadb.write(wadbAngle);
    commandWadb = -1;  // 停止动作
    extiWadbOpen = false;
  }
  if (extiWadbClose) {
    Serial.println("挖斗到达0度下限");
    wadbAngle = 0;  // 强制设为最小角度
    myWadb.write(wadbAngle);
    commandWadb = -1;  // 停止动作
    extiWadbClose = false;
  }

  // 处理机身限位触发（若有机身限位可启用）
  if (extiFuselageLeft) {
    Serial.println("机身到达0度左边界");
    fuselageAngle = 0;  // 强制设为最小角度
    servoJiuf.write(fuselageAngle);
    commandFuselage = -1;  // 停止动作
    extiFuselageLeft = false;
  }
  if (extiFuselageRight) {
    Serial.println("机身到达180度右边界");
    fuselageAngle = 180;  // 强制设为最大角度
    servoJiuf.write(fuselageAngle);
    commandFuselage = -1;  // 停止动作
    extiFuselageRight = false;
  }
}
