#include <Arduino.h>
#include <WiFi.h>
#include <esp_task_wdt.h>
#include <ArduinoJson.h>
#include <Preferences.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <HTTPClient.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>

#define BUFFER_SIZE 2000  // 固定存储2000个数据点

// 环形缓冲区结构
struct CircularBuffer {
    float data[BUFFER_SIZE];
    unsigned long timestamps[BUFFER_SIZE]; // 存储时间戳
    int head;           // 最新数据位置
    int tail;           // 最旧数据位置
    int count;          // 当前有效数据数量
    bool isFull;        // 缓冲区是否已满
    float sum;          // 当前数据总和
};

CircularBuffer heartRateBuffer = {{0}, {0}, 0, 0, 0, false, 0};

// 滤波器定义
#define FILTER_SIZE 5  // 滤波器窗口大小

Preferences preferences;

// 设备ID变量
uint16_t currentDeviceId = 1001;  // 默认设备ID为1001
const uint16_t MIN_DEVICE_ID = 1000;  // 设备ID最小值
const uint16_t MAX_DEVICE_ID = 1999;  // 设备ID最大值

const uint32_t PHASE_SEND_INTERVAL = 1;
const uint32_t VITAL_SEND_INTERVAL = 10;

#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"

BLEServer* pServer = NULL;
BLECharacteristic* pCharacteristic = NULL;
bool deviceConnected = false;
bool oldDeviceConnected = false;
String receivedData = "";
String completeData = "";
unsigned long lastReceiveTime = 0;

WiFiClient client;
char ssid[32] = "13-205";
char password[64] = "12345678";
bool wifiConfigured = false;

const char* influxDBHost = "8.134.11.76";
const int influxDBPort = 8086;
const char* influxDBToken = "KuTa5ZsqoHIhi2IglOO06zExUYw1_mJ6K0mcA9X1y6O6CJDog3_Cgr8mUw1SwpuCCKRElqxa6wAhrrhsYPytkg==";
const char* influxDBOrg = "gzlg";
const char* influxDBBucket = "gzlg";

const unsigned long SENSOR_TIMEOUT = 40000;
static uint32_t packetCounter = 0;
static bool shouldSendOtherData = false;

unsigned long lastSensorUpdate = 0;

typedef struct {
    float breath_rate;
    float heart_rate;
    int rssi;
    uint8_t breath_valid;
    uint8_t heart_valid;
    uint8_t presence;
    uint8_t motion;
    int heartbeat_waveform;
    int breathing_waveform;
    int raw_signal;
} SensorData;

SensorData sensorData = {0};

// 呼吸率滤波相关变量定义
// 添加滤波器数组和索引（仅用于呼吸率）
float breathRateFilter[FILTER_SIZE] = {0};
int filterIndex = 0;
bool filterInitialized = false;


HardwareSerial mySerial2(2);
const int BAUD_RATE = 576000;
const int UART2_RX = 16;
const int UART2_TX = 17;
String rxBuffer = "";

// FreeRTOS任务和队列定义
QueueHandle_t phaseDataQueue;
QueueHandle_t vitalDataQueue;
TaskHandle_t bleSendTaskHandle = NULL;
TaskHandle_t vitalSendTaskHandle = NULL;
// 注意：bleSendTaskHandle现在用于蓝牙发送任务

#define QUEUE_SIZE 50
#define TASK_STACK_SIZE 8192
#define TASK_PRIORITY 1

typedef struct {
    int heartbeat_waveform;
    int breathing_waveform;
} PhaseData;

typedef struct {
    float heart_rate;
    float breath_rate;
    uint8_t presence;
    uint8_t motion;
    int rssi;
} VitalData;

// 添加流量控制类
class BLEFlowController {
private:
    size_t maxBytesPerSecond;
    size_t bytesSent;
    unsigned long lastResetTime;
    unsigned long lastSendTime;
    
public:
    BLEFlowController(size_t maxBps) : maxBytesPerSecond(maxBps), bytesSent(0) {
        lastResetTime = millis();
        lastSendTime = 0;
    }
    
    bool canSend(size_t dataSize) {
        unsigned long currentTime = millis();
        
        // 每秒重置计数器
        if(currentTime - lastResetTime >= 1000) {
            bytesSent = 0;
            lastResetTime = currentTime;
        }
        
        // 检查速率限制（放宽限制）
        if((bytesSent + dataSize) > maxBytesPerSecond) {
            return false;
        }
        
        // 最小发送间隔控制（重要！）
        if(currentTime - lastSendTime < 5) { // 进一步减小最小间隔到5ms
            return false;
        }
        
        return true;
    }
    
    bool check() {
        // 这是一个简化版本的检查方法，只检查最小发送间隔
        unsigned long currentTime = millis();
        if(currentTime - lastSendTime < 5) { // 最小间隔5ms
            return false;
        }
        return true;
    }
    
    void recordSend(size_t dataSize) {
        bytesSent += dataSize;
        lastSendTime = millis();
    }
    
    void reset() {
        bytesSent = 0;
        lastResetTime = millis();
        lastSendTime = 0;
    }
};

// 全局变量用于控制持续发送
bool continuousSendEnabled = false;
unsigned long continuousSendInterval = 1000; // 默认1秒发送一次
BLEFlowController bleFlow(500); // 提高到500 B/s限制，进一步提高数据传输速率

void processBLEConfig();
void saveWiFiConfig();
void loadWiFiConfig();
bool connectWiFi();
void processRadarData();
bool parseSensorLine(String line);
void sendRadarCommand(String cmd);
void updateStatusFlags();
void loadDeviceId(); // 加载设备ID
void saveDeviceId(); // 保存设备ID
bool processSetDeviceId(JsonDocument& doc); // 处理设置设备ID命令
void sendStatusToBLE(); // 发送状态信息到BLE
bool processQueryStatus(JsonDocument& doc); // 处理查询状态命令
bool processQueryRadarData(JsonDocument& doc); // 处理查询雷达数据命令
bool processStartContinuousSend(JsonDocument& doc); // 处理开始持续发送命令
bool processStopContinuousSend(JsonDocument& doc); // 处理停止持续发送命令
void sendRadarDataToBLE(); // 发送雷达数据到BLE
void sendDataInChunks(const String& data); // 分包发送函数
float addDataAndCalculateAverage(float newData); // 计算平均值并添加新数据

// FreeRTOS任务函数声明
void bleSendTask(void *parameter);
void vitalSendTask(void *parameter);

class MyServerCallbacks: public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      deviceConnected = true;
      // 注释掉这行以减少串口输出
      // Serial.println("BLE客户端已连接");
      
      // 发送连接状态信息
      sendStatusToBLE();
    };

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
      // 注释掉这行以减少串口输出
      // Serial.println("BLE客户端已断开");
      
      // 重置持续发送状态
      continuousSendEnabled = false;
      // 注释掉这行以减少串口输出
      // Serial.println("重置持续发送状态");
    }
};

class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string value = pCharacteristic->getValue();
      // 注释掉这行以减少串口输出
      // Serial.printf("🔵 收到BLE写入数据，长度: %d 字节\n", value.length());
      
      if (value.length() > 0) {
        String fragment = "";
        for (int i = 0; i < value.length(); i++)
          fragment += value[i];
        
        // 注释掉这行以减少串口输出
        // Serial.printf("📄 接收数据片段: %s\n", fragment.c_str());
        
        completeData += fragment;
        lastReceiveTime = millis();
        
        // 检查是否收到完整的JSON数据
        int openBrace = completeData.indexOf('{');
        int closeBrace = completeData.lastIndexOf('}');
        
        if (openBrace >= 0 && closeBrace > openBrace) {
          String jsonData = completeData.substring(openBrace, closeBrace + 1);
          completeData = completeData.substring(closeBrace + 1);
          
          // 注释掉这行以减少串口输出
          // Serial.printf("📥 完整JSON数据: %s\n", jsonData.c_str());
          
          // 将JSON数据存储到receivedData变量中供后续处理
          receivedData = jsonData;
        }
      }
    }
};

void setup() {
  // 启用串口通信以便观察滤波后的数据
  Serial.begin(115200);
  
  sensorData.breath_rate = 0;
  sensorData.heart_rate = 0;
  sensorData.rssi = 0;
  sensorData.breath_valid = 0;
  sensorData.heart_valid = 0;
  
  esp_task_wdt_init(30, true);
  esp_task_wdt_add(NULL);
  
  // 增加串口缓冲区大小到4096字节以处理大量雷达数据
  mySerial2.setRxBufferSize(4096);
  mySerial2.begin(BAUD_RATE, SERIAL_8N1, UART2_RX, UART2_TX);
  // 注释掉这行以减少串口输出
  // Serial.println("UART2配置完成，缓冲区大小: 4096字节");
  
  delay(1000);
  
  String startCmd = "{\"cmd\":\"setMonitor\",\"para\":1}\n";
  mySerial2.print(startCmd);
  delay(1000);
  // 注释掉这行以减少串口输出
  // Serial.println("启动雷达数据传输");
  
  preferences.begin("radar_data", false);
  
  // 加载设备ID和WiFi配置
  loadDeviceId();
  loadWiFiConfig();
  
  // 创建FreeRTOS队列
  phaseDataQueue = xQueueCreate(QUEUE_SIZE, sizeof(PhaseData));
  vitalDataQueue = xQueueCreate(QUEUE_SIZE, sizeof(VitalData));
  // bleDataQueue = xQueueCreate(QUEUE_SIZE, sizeof(BLEData));  // 移除：不再需要创建蓝牙数据队列
  
  // 注释掉这行以减少串口输出
  /*
  if (phaseDataQueue == NULL || vitalDataQueue == NULL) { // 移除：不再检查蓝牙数据队列
    Serial.println("❌ 队列创建失败");
  } else {
    Serial.println("✅ FreeRTOS队列创建成功");
  }
  */
  
  // 创建FreeRTOS任务
  xTaskCreatePinnedToCore(
    bleSendTask,
    "BleSendTask",
    TASK_STACK_SIZE,
    NULL,
    TASK_PRIORITY,
    &bleSendTaskHandle,
    1
  );
  
  xTaskCreatePinnedToCore(
    vitalSendTask,
    "VitalSendTask",
    TASK_STACK_SIZE,
    NULL,
    TASK_PRIORITY,
    &vitalSendTaskHandle,
    1
  );

  
  // 注释掉这行以减少串口输出
  // Serial.println("✅ FreeRTOS任务创建成功");
  
  BLEDevice::init("ESP32-Radar");
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());
  
  BLEService *pService = pServer->createService(SERVICE_UUID);
  pCharacteristic = pService->createCharacteristic(
                      CHARACTERISTIC_UUID,
                      BLECharacteristic::PROPERTY_READ |
                      BLECharacteristic::PROPERTY_WRITE |
                      BLECharacteristic::PROPERTY_NOTIFY
                    );
  pCharacteristic->setCallbacks(new MyCallbacks());
  pCharacteristic->addDescriptor(new BLE2902());
  
  pService->start();
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x06);
  pAdvertising->setMinPreferred(0x12);
  BLEDevice::startAdvertising();
  
  // 注释掉这行以减少串口输出
  // Serial.println("BLE已启动，设备名称: ESP32-Radar");
  
  if (wifiConfigured) {
    // 注释掉这行以减少串口输出
    // Serial.println("检测到已保存的WiFi配置，尝试连接...");
    if (connectWiFi()) {
      // 注释掉这行以减少串口输出
      // Serial.println("WiFi连接成功！");
    } else {
      // 注释掉这行以减少串口输出
      // Serial.println("WiFi连接失败，请通过BLE重新配置");
      wifiConfigured = false;
    }
  }
}

void loop() {
    esp_task_wdt_reset();
    
    if (!deviceConnected && oldDeviceConnected) {
        delay(500);
        pServer->startAdvertising();
        // 注释掉这行以减少串口输出
        // Serial.println("开始BLE广播");
        oldDeviceConnected = deviceConnected;
    }
    if (deviceConnected && !oldDeviceConnected) {
        oldDeviceConnected = deviceConnected;
    }
    
    processBLEConfig();

    // 移除WiFi连接检查，使雷达数据处理不依赖WiFi连接
    esp_task_wdt_reset();
    processRadarData();
    esp_task_wdt_reset();
    updateStatusFlags();
    
    // 注意：持续发送雷达数据的逻辑已移至FreeRTOS任务处理
    // 主循环不再直接处理蓝牙数据发送，避免影响雷达数据接收和解析
    
    delay(1);
}

// 处理查询状态命令
bool processQueryStatus(JsonDocument& doc) {
  const char* command = doc["command"];
  if (command != nullptr && strcmp(command, "queryStatus") == 0) {
    // 发送设备状态信息
    if (deviceConnected) {
      String statusMsg = String("{\"type\":\"deviceStatus\",\"success\":true,\"deviceId\":") + 
                        String(currentDeviceId) + 
                        String(",\"wifiConfigured\":") + 
                        String(wifiConfigured ? "true" : "false") +
                        String(",\"wifiConnected\":") + 
                        String(WiFi.status() == WL_CONNECTED ? "true" : "false") +
                        String(",\"ipAddress\":\"") + 
                        (WiFi.status() == WL_CONNECTED ? WiFi.localIP().toString() : "") + 
                        String("\"}");
      
      // 直接发送状态查询响应数据（不使用队列，因为这是即时响应）
      pCharacteristic->setValue(statusMsg.c_str());
      pCharacteristic->notify();
      Serial.println("已发送设备状态信息");
    }
    return true;
  }
  return false;
}

// 处理查询雷达数据命令
bool processQueryRadarData(JsonDocument& doc) {
  const char* command = doc["command"];
  if (command != nullptr && strcmp(command, "queryRadarData") == 0) {
    Serial.println("收到查询雷达数据命令");
    
    // 发送最新的雷达数据
    if (deviceConnected) {
      String radarDataMsg = String("{\"type\":\"radarData\",\"success\":true") +
                           String(",\"deviceId\":") + String(currentDeviceId) +
                           String(",\"timestamp\":") + String(millis()) +
                           String(",\"presence\":") + String(sensorData.presence) +
                           String(",\"heartRate\":") + String(sensorData.heart_rate, 1) +
                           String(",\"breathRate\":") + String(sensorData.breath_rate, 1) +
                           String(",\"motion\":") + String(sensorData.motion) +
                           String(",\"rssi\":") + String(sensorData.rssi) +
                           String(",\"heartbeatWaveform\":") + String(sensorData.heartbeat_waveform) +
                           String(",\"breathingWaveform\":") + String(sensorData.breathing_waveform) +
                           String(",\"rawSignal\":") + String(sensorData.raw_signal) +
                           String("}");
      
      // 直接发送查询响应数据（不使用队列，因为这是即时响应）
      pCharacteristic->setValue(radarDataMsg.c_str());
      pCharacteristic->notify();
      Serial.println("已发送雷达数据");
      Serial.printf("发送的数据: %s\n", radarDataMsg.c_str());
    } else {
      Serial.println("BLE未连接，无法发送雷达数据");
    }
    return true;
  }
  return false;
}

// 处理开始持续发送命令
bool processStartContinuousSend(JsonDocument& doc) {
  const char* command = doc["command"];
  if (command != nullptr && strcmp(command, "startContinuousSend") == 0) {
    // 获取发送间隔参数（可选，默认1000ms）
    if (doc.containsKey("interval")) {
      continuousSendInterval = doc["interval"];
      // 限制最小间隔为100ms，最大间隔为10000ms
      if (continuousSendInterval < 100) continuousSendInterval = 100;
      if (continuousSendInterval > 10000) continuousSendInterval = 10000;
    }
    
    continuousSendEnabled = true;
    bleFlow.reset(); // 重置流量控制器
    
    Serial.printf("⚙️ 启动持续发送模式，间隔: %lu ms\n", continuousSendInterval);
    
    // 发送清晰格式的确认消息
    if (deviceConnected) {
      String confirmMsg = String("[START]Interval:") + String(continuousSendInterval);
      
      // 直接发送确认消息
      pCharacteristic->setValue(confirmMsg.c_str());
      pCharacteristic->notify();
      Serial.println("✅ 启动确认消息发送成功");
      
      delay(5); // 减少延迟以提高实时性
      Serial.println("🚀 已启动持续发送模式");
    } else {
      Serial.println("❌ BLE未连接，无法发送确认消息");
    }
    return true;
  }
  return false;
}

// 处理停止持续发送命令
bool processStopContinuousSend(JsonDocument& doc) {
  const char* command = doc["command"];
  if (command != nullptr && strcmp(command, "stopContinuousSend") == 0) {
    continuousSendEnabled = false;
    
    Serial.println("🛑 停止持续发送模式");
    
    // 发送清晰格式的确认消息
    if (deviceConnected) {
      String confirmMsg = String("[STOP]ContinuousSend");
      
      // 直接发送确认消息
      pCharacteristic->setValue(confirmMsg.c_str());
      pCharacteristic->notify();
      Serial.println("✅ 停止确认消息发送成功");
      
      delay(5); // 减少延迟以提高实时性
      Serial.println("⏹️ 已停止持续发送模式");
    } else {
      Serial.println("❌ BLE未连接，无法发送确认消息");
    }
    return true;
  }
  return false;
}

// 分包发送函数 - 优化版本，解决分包混乱和数据截断问题
void sendDataInChunks(const String& data) {
    const int MAX_PACKET_SIZE = 20; // BLE最大包大小
    const int HEADER_SIZE = 6; // 包头大小 "[N/M]"
    const int CHUNK_SIZE = MAX_PACKET_SIZE - HEADER_SIZE; // 实际数据大小
    
    int totalLength = data.length();
    int numChunks = (totalLength + CHUNK_SIZE - 1) / CHUNK_SIZE;
    
    // 只有在数据较长需要分包时才显示分包信息
    // 注释掉这行以减少串口输出
    // Serial.printf("📦 开始分包发送，总长度: %d, 分包数: %d\n", totalLength, numChunks);
    
    for(int i = 0; i < numChunks; i++) {
        int start = i * CHUNK_SIZE;
        int chunkLength = min(CHUNK_SIZE, totalLength - start);
        String chunk = data.substring(start, start + chunkLength);
        
        // 构造简单包头: "[序号/总数]"，确保总长度不超过6字符
        // 例如: "[1/5]" = 4字符
        String packetHeader = String("[") + String(i+1) + String("/") + String(numChunks) + String("]");
        
        // 确保总包大小不超过20字节
        int maxDataLength = MAX_PACKET_SIZE - packetHeader.length();
        if (chunk.length() > maxDataLength) {
            chunk = chunk.substring(0, maxDataLength);
        }
        
        String packet = packetHeader + chunk;
        
        // 注释掉这行以减少串口输出
        // Serial.printf("📤 发送分包 %s: %s\n", packetHeader.c_str(), chunk.c_str());
        
        // 检查BLE连接状态
        if (!deviceConnected) {
            // 注释掉这行以减少串口输出
            // Serial.println("❌ BLE未连接，无法发送数据");
            return;
        }
        
        // 发送数据包
        pCharacteristic->setValue(packet.c_str());
        pCharacteristic->notify();
        // 注释掉这行以减少串口输出
        // Serial.println("✅ 分包发送成功");
        
        // 等待一段时间确保接收端处理完成，避免数据丢失
        if (i < numChunks - 1) {
            // 注释掉这行以减少串口输出
            // Serial.printf("⏳ 等待接收端处理第%d个包...\n", i+1);
            delay(20); // 进一步减少等待时间以提高实时性
        }
    }
    
    // 注释掉这行以减少串口输出
    // Serial.println("📦 分包发送完成");
}

// 发送雷达数据到BLE（已移至FreeRTOS任务处理）
void sendRadarDataToBLE() {
    // 此函数已废弃，雷达数据发送现在由FreeRTOS任务处理
    // 保留此函数以防其他代码引用
    Serial.println("ℹ️ 雷达数据发送已移至FreeRTOS任务处理");
}

// 处理BLE配置和命令
void processBLEConfig() {
  // 增强超时处理机制
  if (completeData.length() > 0 && (millis() - lastReceiveTime > 3000)) {
    completeData = "";
  }
  
  if (receivedData.length() > 0) {
    String bleData = receivedData;
    receivedData = "";
    bleData.trim();
    
    if (bleData.startsWith("{") && bleData.endsWith("}")) {
      JsonDocument doc;
      DeserializationError error = deserializeJson(doc, bleData);
    
      if (error) {
        if (deviceConnected) {
          String responseMsg = String("{\"type\":\"error\",\"message\":\"配置格式错误，请使用JSON格式\"}");
          
          // 使用分包发送函数发送错误消息
          sendDataInChunks(responseMsg);
        }
      } else {
        // 处理各种命令
        bool processed = false;
        
        // 处理设置设备ID命令
        if (!processed) processed = processSetDeviceId(doc);
        
        // 处理查询状态命令
        if (!processed) processed = processQueryStatus(doc);
        
        // 处理开始持续发送命令
        if (!processed) processed = processStartContinuousSend(doc);
        
        // 处理停止持续发送命令
        if (!processed) processed = processStopContinuousSend(doc);
        
        // 如果没有处理任何命令，发送错误响应
        if (!processed) {
          if (deviceConnected) {
            String responseMsg = String("{\"type\":\"error\",\"message\":\"未知命令\"}");
            
            // 使用分包发送函数发送错误消息
            sendDataInChunks(responseMsg);
          }
        }
      }
    } else {
    }
  }
}

void processRadarData() {
    static unsigned long lastDataTime = 0;
    static unsigned long noDataWarningTime = 0;
    static uint32_t totalLinesReceived = 0;
    static uint32_t validLinesReceived = 0;
    static uint32_t phasePacketCounter = 0;
    static uint32_t vitalPacketCounter = 0;
    static bool processingData = false;
    
    if (processingData) {
        return;
    }
    
    int available = mySerial2.available();
    if (available > 0) {
        processingData = true;
        lastDataTime = millis();
        
        bool lineProcessed = false;
        while (mySerial2.available() > 0 && !lineProcessed) {
            char c = mySerial2.read();
            rxBuffer += c;
            
            // 增加缓冲区大小检查，防止溢出
            if (rxBuffer.length() > 1000) {
                rxBuffer = "";
            }
            
            if (c == '\n' || c == '\r') {
                if (rxBuffer.length() > 0) {
                    totalLinesReceived++;
                    bool parseResult = parseSensorLine(rxBuffer);
                    if (parseResult) {
                        validLinesReceived++;
                        
                        // 更新全局传感器数据
                        lastSensorUpdate = millis();
                        
                        // 使用FreeRTOS队列发送数据到任务
                        phasePacketCounter++;
                        if (phasePacketCounter >= PHASE_SEND_INTERVAL) {
                            PhaseData phaseData;
                            phaseData.heartbeat_waveform = sensorData.heartbeat_waveform;
                            phaseData.breathing_waveform = sensorData.breathing_waveform;
                            
                            if (xQueueSend(phaseDataQueue, &phaseData, 0) == pdTRUE) {
                            } else {
                            }
                            phasePacketCounter = 0;
                        }
                        
                        vitalPacketCounter++;
                        if (vitalPacketCounter >= VITAL_SEND_INTERVAL) {
                            VitalData vitalData;
                            vitalData.heart_rate = sensorData.heart_rate;
                            vitalData.breath_rate = sensorData.breath_rate;
                            vitalData.presence = sensorData.presence;
                            vitalData.motion = sensorData.motion;
                            vitalData.rssi = sensorData.rssi;
                            
                            if (xQueueSend(vitalDataQueue, &vitalData, 0) == pdTRUE) {
                            } else {
                            }
                            vitalPacketCounter = 0;
                        }
                    } else {
                    }
                    
                    rxBuffer = "";
                    lineProcessed = true;
                }
            }
            
            esp_task_wdt_reset();
        }
        
        if (!lineProcessed && available > 0) {
        }
        
        processingData = false;
    } else {
        if (millis() - lastDataTime > 5000 && millis() - noDataWarningTime > 5000) {
            noDataWarningTime = millis();
        }
    }
}


// FreeRTOS任务：蓝牙数据发送（精简格式以节省空间）
void bleSendTask(void *parameter) {
    while (1) {
        PhaseData phaseData;
        
        // 从队列接收相位数据用于蓝牙发送
        if (xQueueReceive(phaseDataQueue, &phaseData, portMAX_DELAY) == pdTRUE) {
            esp_task_wdt_reset();
            
            // 蓝牙传输 - 构造并发送完整的雷达数据（包含所有7个字段，无标识符以节省空间）
            if (deviceConnected && continuousSendEnabled) {
                // 构造完整雷达数据格式（去掉"radar"标识以节省空间）
                // 格式: "heartRate|breathRate|heartbeatWaveform|breathingWaveform|presence|motion|rssi|checksum"
                String radarDataCore = String(sensorData.heart_rate, 1) + String("|") +
                                     String(sensorData.breath_rate, 1) + String("|") +
                                     String(phaseData.heartbeat_waveform) + String("|") +
                                     String(phaseData.breathing_waveform) + String("|") +
                                     String(sensorData.presence) + String("|") +
                                     String(sensorData.motion) + String("|") +
                                     String(sensorData.rssi);
                
                // 计算CRC校验和
                unsigned int crc = 0xFFFF;
                for (int i = 0; i < radarDataCore.length(); i++) {
                    crc ^= (unsigned int)radarDataCore.charAt(i);
                    for (int j = 0; j < 8; j++) {
                        if (crc & 0x0001) {
                            crc >>= 1;
                            crc ^= 0xA001;
                        } else {
                            crc >>= 1;
                        }
                    }
                }
                
                // 添加校验和到数据末尾
                String radarDataMsg = radarDataCore + String("|") + String(crc, HEX);
                
                // 注释掉这行以减少串口输出
                // Serial.printf("📤 通过蓝牙发送完整雷达数据: %s\n", radarDataMsg.c_str());
                
                // 检查数据长度决定发送方式
                const int MAX_BLE_PACKET_SIZE = 20; // BLE最大包大小
                if (radarDataMsg.length() <= MAX_BLE_PACKET_SIZE) {
                    // 数据较短，直接发送
                    pCharacteristic->setValue(radarDataMsg.c_str());
                    pCharacteristic->notify();
                    // 注释掉这行以减少串口输出
                    // Serial.println("✅ 完整雷达数据蓝牙发送成功");
                } else {
                    // 数据较长，使用分包发送
                    // 注释掉这行以减少串口输出
                    // Serial.println("🔄 雷达数据较长，使用分包发送");
                    sendDataInChunks(radarDataMsg);
                }
            }
            
            esp_task_wdt_reset();
        }
        
        vTaskDelay(1 / portTICK_PERIOD_MS); // 减少延迟以提高实时性
    }
}

// FreeRTOS任务：发送生命体征数据
void vitalSendTask(void *parameter) {
    while (1) {
        VitalData vitalData;
        
        // 从队列接收生命体征数据用于WiFi数据库发送
        if (xQueueReceive(vitalDataQueue, &vitalData, portMAX_DELAY) == pdTRUE) {
            esp_task_wdt_reset();
            
            // WiFi数据库传输 - 一直发送生命体征数据（不通过蓝牙）
            if (WiFi.status() == WL_CONNECTED) {
                HTTPClient http;
                http.setTimeout(2000);
                
                String url = String("http://") + String(influxDBHost) + ":" + String(influxDBPort) + "/api/v2/write?org=" + String(influxDBOrg) + "&bucket=" + String(influxDBBucket) + "&precision=ns";
                
                http.begin(url);
                http.addHeader("Authorization", String("Token ") + String(influxDBToken));
                http.addHeader("Content-Type", "text/plain");
                http.setReuse(true);
                
                // 使用可变的设备ID
                String deviceId = String(currentDeviceId);
                
                String vitalDataStr = "";
                bool firstField = true;
                
                // 心率数据 - 使用浮点数格式
                if (vitalData.heart_rate > 0) {
                    if (!firstField) vitalDataStr += ",";
                    vitalDataStr += "heartRate=";
                    vitalDataStr += String(vitalData.heart_rate / 1.0, 1);
                    firstField = false;
                }
                
                // 呼吸率数据 - 使用浮点数格式
                if (vitalData.breath_rate > 0) {
                    if (!firstField) vitalDataStr += ",";
                    vitalDataStr += "breathingRate=";
                    vitalDataStr += String(vitalData.breath_rate / 1.0, 1);
                    firstField = false;
                }
                
                // 人检数据 - 确保值为0或1，使用整数格式
                if (true) { // 总是发送人检数据
                    if (!firstField) vitalDataStr += ",";
                    vitalDataStr += "personDetected=";
                    // 确保值为0或1
                    int presenceValue = (vitalData.presence > 0) ? 1 : 0;
                    vitalDataStr += String(presenceValue) + "i"; // 整数格式
                    firstField = false;
                }
                
                // 人体活动数据 - 确保值为0或1，使用整数格式
                if (true) { // 总是发送人体活动数据
                    if (!firstField) vitalDataStr += ",";
                    vitalDataStr += "humanActivity=";
                    // 确保值为0或1
                    int motionValue = (vitalData.motion > 0) ? 1 : 0;
                    vitalDataStr += String(motionValue) + "i"; // 整数格式
                    firstField = false;
                }
                
                // RSSI数据 - 使用整数格式
                if (vitalData.rssi != 0) {
                    if (!firstField) vitalDataStr += ",";
                    vitalDataStr += "rssi=";
                    vitalDataStr += String(vitalData.rssi) + "i"; // 整数格式
                    firstField = false;
                }
                
                // 如果有数据要发送，则构造完整的行协议字符串
                if (vitalDataStr.length() > 0) {
                    vitalDataStr = "device_data,deviceId=" + deviceId + " " + vitalDataStr;
                    //Serial.println("📤 发送生命体征数据到数据库: " + vitalDataStr);
                    
                    int httpResponseCode = http.POST(vitalDataStr);
                    
                    if (httpResponseCode == 204) {
                        //Serial.println("✅ 生命体征数据发送到数据库成功");
                    } else {
                        String errorMsg = String("❌ 生命体征数据发送到数据库失败: ") + String(httpResponseCode);
                        Serial.println(errorMsg);
                    }
                }
                
                http.end();
            } else {
                Serial.println("❌ WiFi未连接，无法发送生命体征数据到数据库");
            }
            
            esp_task_wdt_reset();
        }
        
        vTaskDelay(5 / portTICK_PERIOD_MS);
    }
}

void updateStatusFlags() {
    static unsigned long lastStatusUpdate = 0;
}

bool parseSensorLine(String line) {
    if (!line.startsWith("HBR01")) {
        // 注释掉这行以减少串口输出
        // Serial.println("❌ 数据行不以HBR01开头");
        return false;
    }
    
    int colonIndex = line.indexOf(':');
    if (colonIndex == -1) {
        // 注释掉这行以减少串口输出
        // Serial.println("❌ 数据行缺少冒号分隔符");
        return false;
    }
    
    String dataPart = line.substring(colonIndex + 1);
    dataPart.trim();
    
    // 注释掉这行以减少串口输出
    // Serial.printf("🔍 解析数据部分: %s\n", dataPart.c_str());
    
    int values[8];
    int startIndex = 0;
    int commaIndex;
    int fieldCount = 0;
    
    for (int i = 0; i < 8; i++) {
        commaIndex = dataPart.indexOf(',', startIndex);
        
        if (commaIndex == -1 && i == 7) {
            String valueStr = dataPart.substring(startIndex);
            valueStr.trim();
            values[i] = valueStr.toInt();
            fieldCount++;
            // 注释掉这行以减少串口输出
            // Serial.printf("  字段%d: %s -> %d\n", i+1, valueStr.c_str(), values[i]);
        } else if (commaIndex != -1) {
            String valueStr = dataPart.substring(startIndex, commaIndex);
            valueStr.trim();
            values[i] = valueStr.toInt();
            startIndex = commaIndex + 1;
            fieldCount++;
            // 注释掉这行以减少串口输出
            // Serial.printf("  字段%d: %s -> %d\n", i+1, valueStr.c_str(), values[i]);
        } else {
            // 注释掉这行以减少串口输出
            // Serial.printf("❌ 解析错误: 字段%d缺失，总共找到%d个字段\n", i+1, fieldCount);
            return false;
        }
    }
    
    sensorData.presence = values[0];
    sensorData.heart_rate = (float)values[1];
    sensorData.breath_rate = (float)values[2];
    sensorData.motion = values[3];
    sensorData.rssi = values[4];
    sensorData.heartbeat_waveform = values[5];
    sensorData.breathing_waveform = values[6];
    sensorData.raw_signal = values[7];
    
    sensorData.heart_valid = (sensorData.heart_rate > 0 && sensorData.heart_rate < 200);
    sensorData.breath_valid = (sensorData.breath_rate >= 0.1f && sensorData.breath_rate <= 60.0f);
    
    // 只有在检测到人的情况下才进行心率滤波处理
    if (sensorData.presence > 0) {
        sensorData.heart_rate = addDataAndCalculateAverage(sensorData.heart_rate);
    }
    
    // 只有在检测到人的情况下才进行呼吸率滤波处理
    if (sensorData.presence > 0) {
        // 对呼吸率继续使用原有的移动平均滤波器
        breathRateFilter[filterIndex] = sensorData.breath_rate;
        
        // 计算滤波后的呼吸率值（移动平均）
        float filteredBreathRate = 0;
        int count = filterInitialized ? FILTER_SIZE : (filterIndex + 1);
        
        for (int i = 0; i < count; i++) {
            filteredBreathRate += breathRateFilter[i];
        }
        
        filteredBreathRate /= count;
        
        // 更新滤波器索引
        filterIndex = (filterIndex + 1) % FILTER_SIZE;
        if (filterIndex == 0) {
            filterInitialized = true;
        }
        
        // 使用滤波后的呼吸率值
        sensorData.breath_rate = filteredBreathRate;
    }
    
    lastSensorUpdate = millis();
    
    // 按照指定格式输出心率和呼吸率的原始数据
    Serial.printf("Heart_Rate_RAW:%.2f\n", sensorData.heart_rate);
    Serial.printf("Breath_Rate_RAW:%.2f\n", sensorData.breath_rate);
    
    /*
    // 注释掉原来的详细输出
    Serial.printf("✅ 解析成功: 有人=%d, 心率=%.1f, 呼吸=%.1f, 运动=%d, 波形=%d/%d\n", 
                 sensorData.presence, sensorData.heart_rate, 
                 sensorData.breath_rate, sensorData.motion,
                 sensorData.heartbeat_waveform, sensorData.breathing_waveform);
    */
    
    return true;
}

void saveWiFiConfig() {
  preferences.putString("ssid", ssid);
  preferences.putString("password", password);
  preferences.putBool("configured", true);
  Serial.println("WiFi配置已保存到Flash");
}

void loadWiFiConfig() {
  wifiConfigured = preferences.getBool("configured", false);
  if (wifiConfigured) {
    preferences.getString("ssid", ssid, sizeof(ssid));
    preferences.getString("password", password, sizeof(password));
    Serial.printf("从Flash加载WiFi配置 - SSID: %s\n", ssid);
  }
}

bool connectWiFi() {
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, password);

    Serial.println("[WiFi] 正在尝试连接...");
    int attempts = 0;
    while (WiFi.status() != WL_CONNECTED && attempts < 20) {
        delay(500);
        Serial.printf("[WiFi] 尝试连接中，当前状态: %d\n", WiFi.status());
        attempts++;
    }

    if (WiFi.status() == WL_CONNECTED) {
        Serial.println("[WiFi] 连接成功！");
        Serial.printf("[WiFi] 分配的IP地址: %s\n", WiFi.localIP().toString().c_str());
        return true;
    } else {
        Serial.println("[WiFi] 连接超时，未能成功连接到WiFi。");
        Serial.printf("[WiFi] 最终状态码: %d\n", WiFi.status());
        return false;
    }
}

void sendRadarCommand(String cmd) {
    if (!cmd.endsWith("\n")) {
        cmd += "\n";
    }
    mySerial2.print(cmd);
    Serial.println("📤 [雷达命令] " + cmd);
}

// 处理设置设备ID命令
bool processSetDeviceId(JsonDocument& doc) {
  const char* command = doc["command"];
  if (command != nullptr && strcmp(command, "setDeviceId") == 0) {
    // 处理设置设备ID的命令
    uint16_t newDeviceId = doc["newDeviceId"];
    
    // 验证设备ID范围
    if (newDeviceId < MIN_DEVICE_ID || newDeviceId > MAX_DEVICE_ID) {
      Serial.printf("[错误] 设备ID超出范围，有效范围: %d-%d\n", MIN_DEVICE_ID, MAX_DEVICE_ID);
      if (deviceConnected) {
        String errorMsg = String("{\"type\":\"error\",\"message\":\"设备ID超出范围，有效范围: ") + 
                         String(MIN_DEVICE_ID) + "-" + String(MAX_DEVICE_ID) + "\"}";
        
        // 直接发送错误响应数据
        pCharacteristic->setValue(errorMsg.c_str());
        pCharacteristic->notify();
      }
      return true; // 已处理该命令
    }
    
    currentDeviceId = newDeviceId;
    
    Serial.printf("[设备ID] 已设置新的设备ID: %u\n", currentDeviceId);
    
    // 保存设备ID到Preferences
    saveDeviceId();
    
    // 发送确认消息
    if (deviceConnected) {
      String confirmMsg = String("{\"type\":\"setDeviceIdResult\",\"success\":true,\"message\":\"设备ID设置成功\",\"newDeviceId\":") + 
                         String(newDeviceId) + "}";
      
      // 直接发送设置设备ID响应数据
      pCharacteristic->setValue(confirmMsg.c_str());
      pCharacteristic->notify();
      
      // 发送更新后的状态信息
      sendStatusToBLE();
    }
    return true;
  }
  return false;
}

// 加载设备ID
void loadDeviceId() {
  currentDeviceId = preferences.getUShort("deviceId", 1001);
  Serial.printf("从Flash加载设备ID: %u\n", currentDeviceId);
}

// 保存设备ID
void saveDeviceId() {
  preferences.putUShort("deviceId", currentDeviceId);
  Serial.printf("设备ID已保存到Flash: %u\n", currentDeviceId);
}

// 发送状态信息到BLE
void sendStatusToBLE() {
  if (deviceConnected) {
    String statusMsg = String("{\"type\":\"status\",\"wifiConfigured\":") + 
                      String(wifiConfigured ? "true" : "false") +
                      String(",\"wifiConnected\":") + 
                      String(WiFi.status() == WL_CONNECTED ? "true" : "false") +
                      String(",\"ipAddress\":\"") + 
                      (WiFi.status() == WL_CONNECTED ? WiFi.localIP().toString() : "") + "\"" +
                      String(",\"deviceId\":") + 
                      String(currentDeviceId) + "}";
    
    // 直接发送状态信息数据
    pCharacteristic->setValue(statusMsg.c_str());
    pCharacteristic->notify();
    Serial.println("已发送连接状态信息");
  }
}


float addDataAndCalculateAverage(float newData) {
    // 只有在检测到人的情况下才进行滤波处理
    if (sensorData.presence == 0) {
        return newData; // 没有检测到人时，直接返回原始数据
    }
    
    unsigned long currentTime = millis();
    unsigned long oneMinuteAgo = currentTime - 60000; // 一分钟前的时间
    
    // 移除过期的旧数据（一分钟前的数据）
    while (heartRateBuffer.count > 0 && heartRateBuffer.timestamps[heartRateBuffer.tail] < oneMinuteAgo) {
        heartRateBuffer.sum -= heartRateBuffer.data[heartRateBuffer.tail];
        heartRateBuffer.tail = (heartRateBuffer.tail + 1) % BUFFER_SIZE;
        heartRateBuffer.count--;
        heartRateBuffer.isFull = (heartRateBuffer.count == BUFFER_SIZE);
    }
    
    // 添加新数据
    if (heartRateBuffer.count < BUFFER_SIZE) {
        // 缓冲区未满，直接添加
        heartRateBuffer.head = (heartRateBuffer.head + 1) % BUFFER_SIZE;
        if (heartRateBuffer.count == 0) heartRateBuffer.tail = heartRateBuffer.head;
        heartRateBuffer.data[heartRateBuffer.head] = newData;
        heartRateBuffer.timestamps[heartRateBuffer.head] = currentTime;
        heartRateBuffer.sum += newData;
        heartRateBuffer.count++;
        heartRateBuffer.isFull = (heartRateBuffer.count == BUFFER_SIZE);
    } else {
        // 缓冲区已满，替换最旧数据
        heartRateBuffer.sum -= heartRateBuffer.data[heartRateBuffer.tail]; // 移除最旧数据
        heartRateBuffer.tail = (heartRateBuffer.tail + 1) % BUFFER_SIZE; // 移动尾指针
        heartRateBuffer.head = (heartRateBuffer.head + 1) % BUFFER_SIZE; // 移动头指针
        heartRateBuffer.data[heartRateBuffer.head] = newData; // 存储新数据
        heartRateBuffer.timestamps[heartRateBuffer.head] = currentTime;
        heartRateBuffer.sum += newData; // 添加新数据到总和
    }
    
    // 计算并返回平均值
    if (heartRateBuffer.count > 0) {
        return heartRateBuffer.sum / heartRateBuffer.count;
    } else {
        return 0;
    }
}