#include <Arduino.h>
#include <WiFi.h>
#include <WebServer.h>
#include "esp_camera.h"
#include "esp_heap_caps.h"
#include <freertos/queue.h>
#include <Preferences.h>
#include <WiFiUdp.h>
#include <stdarg.h>
#include <esp_now.h>  // ✅【改动 1】添加 ESP-NOW 头文件

#if 1
// ===== UDP通信类（最小改动）=====
class UDPPeer {
private:
  WiFiUDP udp;
  bool ready = false;
  uint16_t local_port = 8888;
  
  // 双缓存
  String receivedRaw;
  String parsedValue;
  
  // 连接参数
  const char* ssid = nullptr;
  const char* password = nullptr;
  IPAddress local_ip;
  IPAddress gateway;
  IPAddress subnet;
  
  // ✅ 监视器IP最后一位
  uint8_t monitor_ip_last = 101;
  
  // ✅ 改回普通成员变量（非静态）
  bool is_initialized = false;
  
  uint32_t last_maintenance = 0;
  const uint32_t MAINTENANCE_INTERVAL = 1000;
  
  // ✅【改动 2】新增：ESP-NOW消息标志
  bool espnowready=false;
  bool espnowHasMsg=false;

  void autoMaintain() {
    if (!is_initialized) return;
    
    if (millis() - last_maintenance < MAINTENANCE_INTERVAL) return;
       last_maintenance = millis();
       
    if (WiFi.status() != WL_CONNECTED) {      
      WiFi.config(local_ip, gateway, subnet);
      if (!ready)
      {
        //WiFi.disconnect(true);
        //delay(500);
        Serial.printf("正在启动网络连接！！！\n"); 
        WiFi.begin(ssid, password);
        delay(500);
        WiFi.begin(ssid, password);
        delay(500);
        WiFi.begin(ssid, password);
        delay(500);
        WiFi.begin(ssid, password);
        delay(500);
      }
      else
      {
       Serial.printf("网络中断，恢复中！！！\n"); 
       WiFi.reconnect();
       //esp_now_deinit();
      }
      return;
    }

    if (!ready) {
      ready = (udp.begin(local_port) == 1);
      if (ready) {
        //initEspNowRecv();
        Serial.printf("✅ 网络就绪: IP=%s 端口=%d\n", 
                      WiFi.localIP().toString().c_str(), local_port);
      } else {
        return;
      }
    }
  }

public:
  // ✅ 新增：访问 is_initialized 的成员方法
  bool isInitialized() {
    return is_initialized;
  }
  
  bool wifiisready() {
    return ready;
  }

  bool espnowisready(){
    return espnowready;
  }

  void setespnowreadyflg()
  {
     espnowready=true;
  } 

  void resetespnowreadyflg()
  {
     espnowready=false;
  } 

  // ✅ 设置监视器IP
  void setMonitorIPLast(uint8_t last_octet) {
    monitor_ip_last = last_octet;
  }
  
  // ✅ 获取完整监视器IP
  String getMonitorIP() {
    return String(gateway[0]) + "." + String(gateway[1]) + "." + 
           String(gateway[2]) + "." + String(monitor_ip_last);
  }

  bool connect(const char* _ssid, const char* _pwd,
               uint8_t gw1, uint8_t gw2, uint8_t gw3, uint8_t gw4,
               uint8_t local_ip_last, uint16_t port = 8888) {
    WiFi.disconnect(true);
    last_maintenance = 0;
    ready = false;
    espnowHasMsg = false;  // ✅ 重置ESP-NOW标志

    ssid = _ssid;
    password = _pwd;
    local_ip = IPAddress(gw1, gw2, gw3, local_ip_last);
    gateway = IPAddress(gw1, gw2, gw3, gw4);
    subnet = IPAddress(255, 255, 255, 0);
    local_port = port;
    is_initialized = true;
    
    Serial.printf("📶 wifi：%s， pwd：%s，配置网络: IP=%d.%d.%d.%d 网关=%d.%d.%d.%d 端口=%d\n", 
                  ssid, password, gw1, gw2, gw3, local_ip_last, gw1, gw2, gw3, gw4, port);
    Serial.println("💡 网络将在后台自动连接...");
    
    autoMaintain();
    return true;
  }

  // ✅【改动 3】修改hasMessage()：优先检查ESP-NOW
  bool hasMessage() {
    autoMaintain();
    if (!is_initialized || !ready) return false;
    
    // 优先检查ESP-NOW消息
    if (espnowHasMsg) {
      return true;  // 数据已在receivedRaw中
    }
    
    // 再检查UDP消息（原有逻辑）
    receivedRaw = "";
    parsedValue = "";
    
    int len = udp.parsePacket();
    if (len <= 0 || len > 512) return false;
    
    char buf[513];
    int read = udp.read((uint8_t*)buf, min(len, 512));
    if (read > 0) {
      buf[read] = '\0';
      receivedRaw = String(buf);
      return true;
    }
    return false;
  }

  bool getMessageValue(const String& key) {
    parsedValue = "";
    if (receivedRaw.length() == 0) return false;
    
    String keyPattern = "\"" + key + "\"";
    int startIndex = receivedRaw.indexOf(keyPattern);
    if (startIndex == -1) return false;
    
    int colonIndex = receivedRaw.indexOf(":", startIndex);
    if (colonIndex == -1) return false;
    
    int valueStart = colonIndex + 1;
    while (valueStart < receivedRaw.length() && 
           receivedRaw.charAt(valueStart) == ' ') valueStart++;
    
    if (valueStart < receivedRaw.length() && receivedRaw.charAt(valueStart) == '"') {
      int valueEnd = receivedRaw.indexOf("\"", valueStart + 1);
      if (valueEnd != -1) {
        parsedValue = receivedRaw.substring(valueStart + 1, valueEnd);
        return true;
      }
    } else {
      int valueEnd = valueStart;
      while (valueEnd < receivedRaw.length() &&
             receivedRaw.charAt(valueEnd) != ',' &&
             receivedRaw.charAt(valueEnd) != '}' &&
             receivedRaw.charAt(valueEnd) != ' ') valueEnd++;
      while (valueEnd > valueStart && receivedRaw.charAt(valueEnd - 1) == ' ') valueEnd--;
      if (valueEnd > valueStart) {
        parsedValue = receivedRaw.substring(valueStart, valueEnd);
        return true;
      }
    }
    return false;
  }

  bool sendTo(const String& target_ip_str, const String& key, const char* format, ...) {
    autoMaintain();
    if (!is_initialized || !ready) 
    {
      Serial.println("连接还未完成！！！");
      return false;
    }
    IPAddress target_ip;
    if (!target_ip.fromString(target_ip_str)) return false;
    
    char content[256];
    va_list args;
    va_start(args, format);
    vsnprintf(content, sizeof(content) - 1, format, args);
    va_end(args);
    
    char json[300];
    snprintf(json, sizeof(json), "{\"%s\":\"%s\"}\r\n", key.c_str(), content);
    
    return udp.beginPacket(target_ip, local_port) && 
           udp.write((const uint8_t*)json, strlen(json)) && 
           udp.endPacket();
  }

  bool sendTo(const String& target_ip_str, const String& key, const String& value) {
    return sendTo(target_ip_str, key, "%s", value.c_str());
  }

  String lastValue() { return parsedValue; }
  String lastMessage() { return receivedRaw; }
  String getLocalIP() { 
    autoMaintain();
    return is_initialized ? WiFi.localIP().toString() : "未配置"; 
  }
  bool isConnected() { 
    autoMaintain();
    return is_initialized && ready; 
  }
  
  // ✅【改动 4】新增：获取本机MAC地址
  String getMacAddress() {
    return WiFi.macAddress();
  }
  
  // ✅【改动 5】新增：设置ESP-NOW消息（回调调用）
  void setEspNowMessage(const char* data, int len) {
    if (len <= 0 || len > 250) return;  // ESP-NOW最大250字节
    char buf[251];
    memcpy(buf, data, len);
    buf[len] = '\0';
    receivedRaw = String(buf);
    espnowHasMsg = true;
  }
  
  // ✅【改动 6】新增：清除ESP-NOW标志（解析后调用）
  void clearEspNowFlag() {
    espnowHasMsg = false;
  }
};

extern UDPPeer peer;
UDPPeer peer;

// ✅【改动 7】ESP-NOW接收回调（直接调用peer方法）
static void OnEspNowRecv(const uint8_t *mac, const uint8_t *data, int len) {
  peer.setEspNowMessage((const char*)data, len);
}

// ✅【改动 8】初始化ESP-NOW接收
bool initEspNowRecv() {
  if (esp_now_init() != ESP_OK) {
    Serial.println("❌ ESP-NOW初始化失败");
    return 0;
  }  

  esp_now_register_recv_cb(OnEspNowRecv);
  Serial.println("✅ ESP-NOW接收已启用");
  return 1;
}

// ===== 默认配置 =====
#define DEFAULT_SSID "ESP32-Camera"
#define DEFAULT_PWD "12345678"
#define DEFAULT_GW1 192
#define DEFAULT_GW2 168
#define DEFAULT_GW3 4
#define DEFAULT_GW4 1
#define DEFAULT_LOCAL_LAST 100
#define DEFAULT_MONITOR_LAST 101

// ===== Flash配置管理 =====
void loadConfigFromFlash() {
  Preferences prefs;
  if (!prefs.begin("cam_cfg", true)) {
    Serial.println("⚠️  Flash配置不存在，使用默认值");
    peer.connect(DEFAULT_SSID, DEFAULT_PWD, 
                 DEFAULT_GW1, DEFAULT_GW2, DEFAULT_GW3, DEFAULT_GW4, 
                 DEFAULT_LOCAL_LAST);
    peer.setMonitorIPLast(DEFAULT_MONITOR_LAST);
    prefs.end();
    return;
  }
  
  String ssid = prefs.getString("ssid", DEFAULT_SSID);
  String pwd = prefs.getString("pwd", DEFAULT_PWD);
  uint8_t gw1 = prefs.getUChar("gw1", DEFAULT_GW1);
  uint8_t gw2 = prefs.getUChar("gw2", DEFAULT_GW2);
  uint8_t gw3 = prefs.getUChar("gw3", DEFAULT_GW3);
  uint8_t gw4 = prefs.getUChar("gw4", DEFAULT_GW4);
  uint8_t local_last = prefs.getUChar("local", DEFAULT_LOCAL_LAST);
  uint8_t monitor_last = prefs.getUChar("monitor", DEFAULT_MONITOR_LAST);
  
  prefs.end();
  
  peer.connect(ssid.c_str(), pwd.c_str(), gw1, gw2, gw3, gw4, local_last);
  peer.setMonitorIPLast(monitor_last);
  
  Serial.println("✅ 从Flash加载配置成功");
}

void saveConfigToFlash(const char* ssid, const char* pwd,
                      uint8_t gw1, uint8_t gw2, uint8_t gw3, uint8_t gw4,
                      uint8_t local_last, uint8_t monitor_last) {
  Preferences prefs;
  if (!prefs.begin("cam_cfg", false)) {
    Serial.println("❌ Flash配置保存失败");
    return;
  }
  
  prefs.putString("ssid", ssid);
  prefs.putString("pwd", pwd);
  prefs.putUChar("gw1", gw1);
  prefs.putUChar("gw2", gw2);
  prefs.putUChar("gw3", gw3);
  prefs.putUChar("gw4", gw4);
  prefs.putUChar("local", local_last);
  prefs.putUChar("monitor", monitor_last);
  
  prefs.end();
  Serial.println("✅ 配置已保存到Flash");
}

// ===== 串口指令解析 =====
void handleSerialCommand() {
  if (!Serial.available()) return;
  
  String cmd = Serial.readStringUntil('\n');
  cmd.trim();
  
  if (!cmd.startsWith("wificar:")) 
  {
    if(cmd.endsWith("LZU")||cmd.endsWith("LZU\r"))
    {
       peer.sendTo(peer.getMonitorIP(), "f", cmd);
       Serial.printf("📤 已发送到 %s: {\"f\":%s}\n", peer.getMonitorIP().c_str(), cmd);
    }
    // ✅【改动 9】ESPNOW分支：回复MAC+IP给主机
    else if(cmd.endsWith("ESPNOW")||cmd.endsWith("ESPNOW\r"))
    {
      String mac = peer.getMacAddress();
      String ip = peer.getLocalIP();
      char json[150];
      snprintf(json, sizeof(json), "{\"mac\":\"%s\",\"ip\":\"%s\"}\r\n", 
               mac.c_str(), ip.c_str());
      bool ok = peer.sendTo(peer.getMonitorIP(), "ESPNOW_REG", json);
      if(ok) {
        Serial.printf("✅ ESPNOW注册: MAC=%s IP=%s\n", mac.c_str(), ip.c_str());
      } else {
        Serial.printf("❌ 发送ESPNOW注册信息失败\n");
      }
    }
    return;
  }
  
  int pos[7];
  int count = 0;
  for (int i = 8; i < cmd.length() && count < 7; i++) {
    if (cmd.charAt(i) == ':') {
      pos[count++] = i;
    }
  }
  
  if (count < 7) {
    Serial.println("❌ 指令格式错误");
    return;
  }
  
  String ssid = cmd.substring(8, pos[0]);
  String pwd = cmd.substring(pos[0] + 1, pos[1]);
  uint8_t gw1 = cmd.substring(pos[1] + 1, pos[2]).toInt();
  uint8_t gw2 = cmd.substring(pos[2] + 1, pos[3]).toInt();
  uint8_t gw3 = cmd.substring(pos[3] + 1, pos[4]).toInt();
  uint8_t gw4 = cmd.substring(pos[4] + 1, pos[5]).toInt();
  uint8_t local_last = cmd.substring(pos[5] + 1, pos[6]).toInt();
  uint8_t monitor_last = cmd.substring(pos[6] + 1).toInt();
  
  peer.connect(ssid.c_str(), pwd.c_str(), gw1, gw2, gw3, gw4, local_last);
  peer.setMonitorIPLast(monitor_last);
  saveConfigToFlash(ssid.c_str(), pwd.c_str(), gw1, gw2, gw3, gw4, local_last, monitor_last);
  
  Serial.println("✅ WiFi配置已更新并保存");
}

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  Serial.println("\n╔════════════════════════════════════════════════════════════════╗");
  Serial.println("║  ESP32-S3 摄像头 UDP+ESP-NOW 通信系统 v1.3                 ║");
  Serial.println("║  支持Flash配置 | 串口配置 | UDP+ESP-NOW双通道统一解析    ║");
  Serial.println("╚════════════════════════════════════════════════════════════════╝");
  
  loadConfigFromFlash();
  //esp_now_deinit();
  //initEspNowRecv();  // ✅【改动 10】初始化ESP-NOW接收
}

void loop() {
  handleSerialCommand();
  
  
  if (peer.isInitialized()) {
    // if (peer.wifiisready())   
    // {
    //   if(!peer.espnowisready())
    //   {
    //    if(initEspNowRecv())
    //    {
    //     peer.setespnowreadyflg();
    //     String mac = peer.getMacAddress();
    //   String ip = peer.getLocalIP();
    //   char json[150];
    //   snprintf(json, sizeof(json), "mac:%s;ip:%s", 
    //            mac.c_str(), ip.c_str());
    //   bool ok = peer.sendTo(peer.getMonitorIP(), "r", json);//ESPNOW_REG
    //   if(ok) {
    //     Serial.printf("✅ ESPNOW注册: MAC=%s IP=%s\n", mac.c_str(), ip.c_str());
    //   } else {
    //     Serial.printf("❌ 发送ESPNOW注册信息失败\n");
    //   }
    //    }
    //   }
    // }
    // else if(peer.espnowisready())
    // {       
    //     peer.resetespnowreadyflg();
    //     esp_now_deinit();
    // } //k10的总线扩展机制导espnow优势不能有效发挥，每次查阅按钮时，会直接打断espnow通讯，再启动会延迟将近200ms（感觉是硬件通道重启）；
          //若是常规udp通讯，不一定被按钮查阅打断，但是会被此间隔时间冲断，再连接延迟好使120ms左右（感觉是软件握手重启）
          //也就是采用k10板子，espnow效果反而更差于udp通讯
   

    if (peer.hasMessage()) {
      if (peer.getMessageValue("c")) {
        Serial.printf("%sZHY\n", peer.lastValue().c_str());
    //      Serial.printf("📶 Wi-Fi 模式：%s\n", 
    //               WiFi.getMode() == WIFI_AP ? "AP" : 
    //               WiFi.getMode() == WIFI_STA ? "STA" : "AP+STA");
    // Serial.printf("📶 信道：%d\n", WiFi.channel());
    // Serial.printf("📶 ESP-NOW：%s\n", peer.espnowisready()? "已启用" : "未启用");
    // Serial.printf("📶 状态：%d\n", WiFi.status());
      }
      // ✅【改动 11】解析后清除ESP-NOW标志（避免重复处理）
      peer.clearEspNowFlag();
    }
  }  
  delay(1);
}

主机端（espnow版本）
#include <WiFi.h>
#include <WiFiUdp.h>
#include <stdarg.h>
#include <esp_now.h>  // ✅ 新增：ESP-NOW 头文件

// ✅ 新增：ESP-NOW 对等节点结构（最多 10 个从机）
#define MAX_SLAVES 10
typedef struct {
  uint8_t mac[6];
  IPAddress ip;
  bool active;
} SlaveNode;

class UDPPeer {
private:
  WiFiUDP udp;
  bool ready = false;
  uint16_t local_port = 8888;
  
  // 双缓存
  String receivedRaw;
  String parsedValue;
  
  // 连接参数
  const char* ssid = nullptr;
  const char* password = nullptr;
  IPAddress local_ip;
  IPAddress gateway;
  IPAddress subnet;
  
  // ✅ 新增：AP模式标志
  bool is_ap_mode = false;
  
  // ✅ 新增：网络配置缓存
  String current_ssid = "";
  String current_password = "";
  uint8_t cached_gw1 = 0, cached_gw2 = 0, cached_gw3 = 0, cached_gw4 = 0;
  
  // 状态标志
  bool is_initialized = false;
  uint32_t last_maintenance = 0;
  const uint32_t MAINTENANCE_INTERVAL = 1000;
  
  // ✅ 新增：ESP-NOW 相关
  bool espnowInited = false;
  SlaveNode slaves[MAX_SLAVES];  // 从机注册表
  uint8_t slaveCount = 0;

  // ✅ 新增：解析 MAC 字符串 "AA:BB:CC:DD:EE:FF" → 字节数组
  bool parseMacString(const String& macStr, uint8_t* macBuf) {
    int vals[6];
    if (sscanf(macStr.c_str(), "%x:%x:%x:%x:%x:%x", 
               &vals[0], &vals[1], &vals[2], &vals[3], &vals[4], &vals[5]) != 6) {
      return false;
    }
    for (int i = 0; i < 6; i++) macBuf[i] = (uint8_t)vals[i];
    return true;
  }
  
  // ✅ 新增：初始化 ESP-NOW（网络就绪后调用）
  void initEspNow() {
    if (espnowInited) return;
    if (esp_now_init() != ESP_OK) {
      Serial.println("❌ ESP-NOW 初始化失败");
      return;
    }
    // 注册发送回调（可选，用于确认发送状态）
    esp_now_register_send_cb([](const uint8_t* mac, esp_now_send_status_t status) {
      // Serial.printf("ESP-NOW Send to %02x... %s\n", mac[5], 
      //              status == ESP_NOW_SEND_SUCCESS ? "OK" : "FAIL");
    });
    espnowInited = true;
    Serial.println("✅ ESP-NOW 已启用");
  }
  
  // ✅ 新增：注册/更新从机（MAC+IP 映射）
  bool registerSlave(const String& macStr, const String& ipStr) {
    if (!espnowInited) initEspNow();
    
    uint8_t mac[6];
    if (!parseMacString(macStr, mac)) return false;
    
    IPAddress ip;
    if (!ip.fromString(ipStr)) return false;
    
    // 检查 MAC 是否已存在 → 更新 IP
    for (int i = 0; i < slaveCount; i++) {
      if (slaves[i].active && memcmp(slaves[i].mac, mac, 6) == 0) {
        slaves[i].ip = ip;
        Serial.printf("🔄 更新从机: MAC=%s IP=%s\n", macStr.c_str(), ipStr.c_str());
        return true;
      }
    }
    
    // 新从机 → 添加到列表
    if (slaveCount >= MAX_SLAVES) {
      Serial.println("⚠️ 从机列表已满，无法添加");
      return false;
    }
    
    SlaveNode* node = &slaves[slaveCount++];
    memcpy(node->mac, mac, 6);
    node->ip = ip;
    node->active = true;
    
    // 注册到 ESP-NOW 对等列表
    esp_now_peer_info_t peerInfo = {};
    memcpy(peerInfo.peer_addr, mac, 6);
    peerInfo.channel = 0;   // 自动使用 Wi-Fi 当前信道
    peerInfo.encrypt = false;
    if (esp_now_add_peer(&peerInfo) == ESP_OK) {
      Serial.printf("✅ 注册从机: MAC=%s IP=%s ( #%d )\n", 
                    macStr.c_str(), ipStr.c_str(), slaveCount);
      return true;
    } else {
      Serial.println("❌ ESP-NOW 添加对等节点失败");
      slaveCount--;  // 回滚
      return false;
    }
  }
  
  // ✅ 新增：根据 IP 查找从机 MAC
  uint8_t* findSlaveByIP(const String& ipStr) {
    IPAddress target;
    if (!target.fromString(ipStr)) return nullptr;
    
    for (int i = 0; i < slaveCount; i++) {
      if (slaves[i].active && slaves[i].ip == target) {
        return slaves[i].mac;
      }
    }
    return nullptr;
  }
  
  // ✅ 新增：ESP-NOW 发送（内部使用）
  bool sendEspNow(const uint8_t* mac, const char* data, int len) {
    if (!espnowInited) initEspNow();
    if (!mac || !data || len <= 0 || len > 250) return false;  // ESP-NOW 最大 250 字节
    Serial.printf("📶 信道：%d\n", WiFi.channel());
  /* int sendreslut=esp_now_send(mac, (uint8_t*)data, len);
 Serial.printf("📶 Wi-Fi 模式：%s\n", 
                  WiFi.getMode() == WIFI_AP ? "AP" : 
                  WiFi.getMode() == WIFI_STA ? "STA" : "AP+STA");
    Serial.printf("📶 信道：%d\n", WiFi.channel());
    Serial.printf("📶 状态：%d\n", WiFi.status());
    Serial.printf("📶 ESP-NOW：%s\n", espnowInited ? "已启用" : "未启用");
    

    Serial.printf("ESP-NOW 发送, 信道：%d, mac:%s:%s, 状态:0x%X\n",WiFi.channel(), mac, data, sendreslut); */
    return  esp_now_send(mac, (uint8_t*)data, len)== ESP_OK;
  }

  // ✅ 修正后的轻量维护
  void autoMaintain() {
    if (!is_initialized) return;
    if (millis() - last_maintenance < MAINTENANCE_INTERVAL) return;
    last_maintenance = millis();

    if (is_ap_mode) {
      if (!ready) {
        ready = (udp.begin(local_port) == 1);
        if (ready) Serial.printf("✅ AP模式UDP就绪: 端口=%d\n", local_port);
      }
      return;
    }
    
    if (WiFi.status() != WL_CONNECTED) {    
      WiFi.config(local_ip, gateway, subnet);
      if (!ready) {
        Serial.printf("start local net 正在启动网络连接！！！\n"); 
        WiFi.begin(ssid, password);
        delay(1000);
      } else {
        Serial.printf("reconnect 网络中断，恢复中！！！\n"); 
        WiFi.reconnect();
      }
      return;
    }
    
    if (!ready) {
      ready = (udp.begin(local_port) == 1);
      if (ready) {
        Serial.printf("✅ 网络就绪: IP=%s 端口=%d\n", 
                      WiFi.localIP().toString().c_str(), local_port);
        // ✅ 网络就绪后初始化 ESP-NOW
        //initEspNow();
      } else {
        return;
      }
    }
  }

public:
  // ✅ STA模式连接
  bool connect(const char* _ssid, const char* _pwd,
               uint8_t gw1, uint8_t gw2, uint8_t gw3, uint8_t gw4,
               uint8_t local_ip_last, uint16_t port = 8888) {
    WiFi.mode(WIFI_STA);
    WiFi.disconnect(true);
    last_maintenance = 0;
    ready = false;
    ssid = _ssid; password = _pwd;
    local_ip = IPAddress(gw1, gw2, gw3, local_ip_last);
    gateway = IPAddress(gw1, gw2, gw3, gw4);
    subnet = IPAddress(255, 255, 255, 0);
    local_port = port;
    is_initialized = true;
    is_ap_mode = false;
    
    current_ssid = String(_ssid);
    current_password = String(_pwd);
    cached_gw1 = gw1; cached_gw2 = gw2; cached_gw3 = gw3; cached_gw4 = gw4;
    
    Serial.printf("📶 配置STA网络: IP=%d.%d.%d.%d 网关=%d.%d.%d.%d 端口=%d\n", 
                  gw1, gw2, gw3, local_ip_last, gw1, gw2, gw3, gw4, port);
    Serial.println("💡 网络将在后台自动连接...");
    
    autoMaintain();
    return true;
  }

  // ✅ AP模式连接
  bool connectAP(const char* _ssid, const char* _pwd, uint16_t port = 8888) {
    WiFi.disconnect(true);
    WiFi.mode(WIFI_AP);
    last_maintenance = 0;
    ready = false;
    WiFi.softAPConfig(IPAddress(192, 168, 4, 1), 
                      IPAddress(192, 168, 4, 1), 
                      IPAddress(255, 255, 255, 0));
    
    if (!WiFi.softAP(_ssid, _pwd, 1, false, 10)) {
      Serial.println("❌ AP启动失败");
      return false;
    }

    local_port = port;
    ready = (udp.begin(local_port) == 1);
    is_initialized = true;
    is_ap_mode = true;
    
    current_ssid = String(_ssid);
    current_password = String(_pwd);
    cached_gw1 = 192; cached_gw2 = 168; cached_gw3 = 4; cached_gw4 = 1;
    
    Serial.printf("✅ AP模式启动: 热点=\"%s\" 密码=\"%s\"\n", _ssid, _pwd);
    Serial.printf("🌐 本机IP: %s | 端口: %d | 客户端: %d/%d\n", 
                  WiFi.softAPIP().toString().c_str(), port,
                  WiFi.softAPgetStationNum(), 10);
    
    // ✅ AP模式也初始化 ESP-NOW
    initEspNow();
    return true;
  }

  // ✅ 向小车发送配置
  void sendConfigToDevice(uint8_t car_last_octet, uint8_t monitor_last_octet) {
    if (!is_initialized) {
      Serial.println("❌ 手柄未配置网络，无法发送配置");
      return;
    }
    if (car_last_octet == cached_gw4 || car_last_octet == 0 || car_last_octet == 255) {
      Serial.println("❌ 无效的小车编号（需1~254，且不能与网关相同）");
      return;
    }
    
    uint8_t local_last = is_ap_mode ? 1 : local_ip[3];
    if(monitor_last_octet != 255) local_last = monitor_last_octet;  

    Serial.printf("wificar:%s:%s:%d:%d:%d:%d:%d:%d\nWZHY",
                  current_ssid.c_str(), current_password.c_str(),
                  cached_gw1, cached_gw2, cached_gw3, cached_gw4,
                  car_last_octet, local_last);
    Serial1.printf("wificar:%s:%s:%d:%d:%d:%d:%d:%d\nWZHY",
                   current_ssid.c_str(), current_password.c_str(),
                   cached_gw1, cached_gw2, cached_gw3, cached_gw4,
                   car_last_octet, local_last);
    
    Serial.printf("✅ 已配置小车 #%d → IP=%d.%d.%d.%d | 监视器=%d.%d.%d.%d\n",
                  car_last_octet,
                  cached_gw1, cached_gw2, cached_gw3, car_last_octet,
                  cached_gw1, cached_gw2, cached_gw3, local_last);
  }

  // ✅ 接收：解析 ESPNOW_REG 并自动注册
  bool hasMessage() {
    autoMaintain();
    if (!is_initialized || !ready) return false;
    
    receivedRaw = "";
    parsedValue = "";
    
    int len = udp.parsePacket();
    if (len <= 0 || len > 512) return false;
    
    char buf[513];
    int read = udp.read((uint8_t*)buf, min(len, 512));
    if (read > 0) {
      buf[read] = '\0';
      receivedRaw = String(buf);
      
      // ✅ 解析 ESPNOW_REG 格式: "mac:AA:BB:CC:DD:EE:FF;ip:192.168.4.6"
      if (getMessageValue("r")) {
        String raw = lastValue();  // "mac:AA:BB:CC:DD:EE:FF;ip:192.168.4.6"
        
        // 提取 MAC
        int macStart = raw.indexOf("mac:");
        int macEnd = raw.indexOf(";", macStart);
        // 提取 IP
        int ipStart = raw.indexOf("ip:", macEnd);
        
        if (macStart != -1 && macEnd != -1 && ipStart != -1) {
          String macStr = raw.substring(macStart + 4, macEnd);  // +4 跳过 "mac:"
          String ipStr = raw.substring(ipStart + 3);             // +3 跳过 "ip:"
          
          // ✅ 注册/更新从机
          registerSlave(macStr, ipStr);
        }
      }
      
      return true;
    }
    return false;
  }

  // ✅ 解析字段（不变）
  bool getMessageValue(const String& key) {
    parsedValue = "";
    if (receivedRaw.length() == 0) return false;
    
    String keyPattern = "\"" + key + "\"";
    int startIndex = receivedRaw.indexOf(keyPattern);
    if (startIndex == -1) return false;
    
    int colonIndex = receivedRaw.indexOf(":", startIndex);
    if (colonIndex == -1) return false;
    
    int valueStart = colonIndex + 1;
    while (valueStart < receivedRaw.length() && 
           receivedRaw.charAt(valueStart) == ' ') valueStart++;
    
    if (valueStart < receivedRaw.length() && receivedRaw.charAt(valueStart) == '"') {
      int valueEnd = receivedRaw.indexOf("\"", valueStart + 1);
      if (valueEnd != -1) {
        parsedValue = receivedRaw.substring(valueStart + 1, valueEnd);
        return true;
      }
    } else {
      int valueEnd = valueStart;
      while (valueEnd < receivedRaw.length() &&
             receivedRaw.charAt(valueEnd) != ',' &&
             receivedRaw.charAt(valueEnd) != '}' &&
             receivedRaw.charAt(valueEnd) != ' ') valueEnd++;
      while (valueEnd > valueStart && receivedRaw.charAt(valueEnd - 1) == ' ') valueEnd--;
      if (valueEnd > valueStart) {
        parsedValue = receivedRaw.substring(valueStart, valueEnd);
        return true;
      }
    }
    return false;
  }

  // ✅ 发送：智能路由（UDP/ESP-NOW 自动选择）
  bool sendTo(const String& target_ip_str, const String& key, const char* format, ...) {
    autoMaintain();
    if (!is_initialized || !ready) return false;
    
    IPAddress target_ip;
    if (!target_ip.fromString(target_ip_str)) return false;
    
    // ✅ 智能路由：如果目标 IP 是已注册从机，优先使用 ESP-NOW
    uint8_t* slaveMac = findSlaveByIP(target_ip_str);
    if (slaveMac && espnowInited) {
      // 构造 ESP-NOW 消息（保持 JSON 格式，与 UDP 一致）
      char content[256];
      va_list args;
      va_start(args, format);
      vsnprintf(content, sizeof(content) - 1, format, args);
      va_end(args);
      
      char json[300];
      snprintf(json, sizeof(json), "{\"%s\":\"%s\"}", key.c_str(), content);
      
      // ✅ 使用 ESP-NOW 发送（更快、更低延迟）
      if (sendEspNow(slaveMac, json, strlen(json))) {
        Serial.println("⚠️ ESP-NOW 发送");
        delay(10);
        return true;
      }
      // ESP-NOW 失败时，降级使用 UDP（可选）
      //Serial.println("⚠️ ESP-NOW 发送失败，降级使用 UDP");
    }
    
    // ✅ 默认：使用 UDP 发送
    char content[256];
    va_list args;
    va_start(args, format);
    vsnprintf(content, sizeof(content) - 1, format, args);
    va_end(args);
    
    char json[300];
    snprintf(json, sizeof(json), "{\"%s\":\"%s\"}\r\n", key.c_str(), content);
    
    return udp.beginPacket(target_ip, local_port) && 
           udp.write((const uint8_t*)json, strlen(json)) && 
           udp.endPacket();
  }

  // ✅ 积木友好重载
  bool sendTo(const String& target_ip_str, const String& key, const String& value) {
    return sendTo(target_ip_str, key, "%s", value.c_str());
  }

  // ✅ 辅助方法
  String lastValue() { return parsedValue; }
  String lastMessage() { return receivedRaw; }
  String getLocalIP() { 
    autoMaintain();
    return is_initialized ? 
           (is_ap_mode ? WiFi.softAPIP().toString() : WiFi.localIP().toString()) : 
           "未配置"; 
  }
  bool isConnected() { 
    autoMaintain();
    return is_initialized && ready; 
  }
  bool isAPMode() { return is_ap_mode; }
  
  // ✅ 新增：获取已注册从机数量（调试用）
  int getRegisteredSlaveCount() { return slaveCount; }
};

extern UDPPeer peer;