#define CAMERA_MODEL_XIAO_ESP32S3 // Has PSRAM
#include <Arduino.h>
#include <Wire.h>
#include <FS.h>
#include <SD.h>
#include <SPI.h>
#include <SensorQMI8658.hpp>
#include <ESP_I2S.h>
#include <WiFi.h>
#include <BLEDevice.h>
#include "driver/gpio.h"
#include "esp_camera.h"
#include "camera_pins.h"
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// -------------------------- Hardware pin definitions --------------------------
// LEDs       
#define SD_CS_PIN 21 // SD card CS shares USER_LED
#define RGB_DATA_PIN 48        // RGB LED (WS2812), shared with camera Y9

// IMU (QMI8658C)
#define IMU_SDA 5
#define IMU_SCL 4
#define IMU_IRQ -1

// OLED Display (使用不同的 I2C 引脚，避免与 IMU 冲突)
#define OLED_SDA 7   // I2C 数据线（SDA）- 使用 GPIO7
#define OLED_SCL 8   // I2C 时钟线（SCL）- 使用 GPIO8
#define OLED_RST -1   // 复位引脚（低电平复位）- 可以更改为其他未使用的 GPIO（如 GPIO6, GPIO9, GPIO10 等）
#define OLED_WIDTH 128
#define OLED_HEIGHT 64
#define OLED_ADDR 0x3C  // I2C 地址
#define OLED_CMD  0x00  // 命令模式
#define OLED_DATA 0x40  // 数据模式

// Digital microphone (I2S PDM)
#define MIC_CLK 42
#define MIC_DATA 41

// ADC
#define ADC_PIN 15
#define ADC_SAMPLE_COUNT 10
#define ADC_REF_VOLTAGE 3.3f
#define ADC_MAX_VALUE 4095.0f
#define ADC_VOLTAGE_DIVIDER 2.0f // Set to 1.0f if no divider is used

// Data output interval
#define DATA_OUTPUT_INTERVAL_MS 1000
// IMU data ready timeout
#define IMU_DATA_TIMEOUT_MS 200

// MIC validation thresholds (tune if needed for environment)
#define MIC_SAMPLE_COUNT 256
#define MIC_MIN_PEAK 80
#define MIC_MIN_MEAN 10
#define MIC_MIN_RANGE 80
#define MIC_MIN_NONZERO 16

// ADC validation thresholds
#define ADC_MIN_RAW 50
#define ADC_MAX_RAW 4045

// -------------------------- WiFi test config --------------------------
const char *ap_ssid = "ESP32-CAM-SETUP";
const char *ap_password = "12345678";

// -------------------------- Globals --------------------------
SensorQMI8658 imu;
I2SClass I2S;
bool all_func_ok = true;
IMUdata acc;
IMUdata gyr;
bool data_output_mode = false;
String func_names[7] = {
  "QMI8658C_IMU", "Digital_Mic", "SD_Card",
  "OV5640_Camera", "WiFi", "BLE", "ADC"
};
bool func_status[7] = {false};

// OLED Display 对象（使用软件 I2C 或不同的硬件 I2C 总线）
// 注意：由于 IMU 使用 GPIO4/5，OLED 使用 GPIO6/7，需要创建第二个 Wire 对象
TwoWire WireOLED = TwoWire(1);  // 使用 I2C 总线 1
Adafruit_SSD1306 display(OLED_WIDTH, OLED_HEIGHT, &WireOLED, OLED_RST);
bool oled_initialized = false;

// SD 卡缓存全局变量（在 setup() 中初始化，避免在 loop() 中阻塞）
uint64_t g_sd_card_mb = 0;
uint64_t g_sd_total_mb = 0;
uint64_t g_sd_used_mb = 0;
bool g_sd_cache_valid = false;

// -------------------------- Utility functions --------------------------
// Release GPIO48 for camera use (disable RGB LED output)
void rgb_led_release() {
#if defined(ARDUINO_ARCH_ESP32)
  pinMode(RGB_DATA_PIN, INPUT);
  gpio_set_pull_mode((gpio_num_t)RGB_DATA_PIN, GPIO_FLOATING);
#else
  pinMode(RGB_DATA_PIN, INPUT);
  digitalWrite(RGB_DATA_PIN, LOW);
#endif
  delay(10);
}

// Serial separator
void print_separator() {
  Serial.println("--------------------------------------------------");
}

// -------------------------- OLED Display functions --------------------------
// OLED 硬件复位
void oled_hardware_reset() {
  if (OLED_RST != -1) {
    pinMode(OLED_RST, OUTPUT);
    digitalWrite(OLED_RST, LOW);
    delay(200);
    digitalWrite(OLED_RST, HIGH);
    delay(200);
  }
}

// 向 OLED 发送命令
bool oled_send_command(uint8_t cmd) {
  WireOLED.beginTransmission(OLED_ADDR);
  WireOLED.write(OLED_CMD);
  WireOLED.write(cmd);
  uint8_t error = WireOLED.endTransmission();
  if (error == 0) {
    delayMicroseconds(100);
    return true;
  }
  return false;
}

// 初始化 OLED 显示屏
bool init_oled() {
  // 初始化第二个 I2C 总线用于 OLED（避免与 IMU 冲突）
  WireOLED.begin(OLED_SDA, OLED_SCL);
  WireOLED.setClock(100000);
  delay(200);
  
  // 硬件复位
  oled_hardware_reset();
  
  // 尝试使用 Adafruit 库初始化
  bool init_success = false;
  if (display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    init_success = true;
  } else {
    // 手动初始化序列
    oled_send_command(0xAE);  // Display OFF
    delay(10);
    oled_send_command(0x00);
    oled_send_command(0x10);
    oled_send_command(0x40);
    oled_send_command(0x81);
    oled_send_command(0xFF);
    oled_send_command(0xA1);
    oled_send_command(0xC8);
    oled_send_command(0xA6);
    oled_send_command(0xA8);
    oled_send_command(0x3F);
    oled_send_command(0xD3);
    oled_send_command(0x00);
    oled_send_command(0xD5);
    oled_send_command(0x80);
    oled_send_command(0xD9);
    oled_send_command(0xF1);
    oled_send_command(0xDA);
    oled_send_command(0x12);
    oled_send_command(0xDB);
    oled_send_command(0x40);
    oled_send_command(0x20);
    oled_send_command(0x02);
    oled_send_command(0x8D);
    oled_send_command(0x14);
    oled_send_command(0xA4);
    delay(100);
    oled_send_command(0xAF);  // Display ON
    delay(200);
    
    // 再次尝试 Adafruit 库初始化
    if (display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR, false)) {
      init_success = true;
    } else {
      init_success = true;  // 假设手动初始化成功
    }
  }
  
  if (init_success) {
    // 清屏并显示启动信息
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println("Peripherals Test");
    display.setCursor(0, 16);
    display.println("Testing...");
    display.display();
    delay(200);
    
    oled_initialized = true;
    return true;
  } else {
    oled_initialized = false;
    return false;
  }
}

// 在 OLED 上显示测试结果
void oled_display_test_result() {
  if (!oled_initialized) {
    return;
  }
  
  // 重新确保 I2C 总线状态正确
  WireOLED.end();
  delay(50);
  WireOLED.begin(OLED_SDA, OLED_SCL);
  WireOLED.setClock(100000);
  delay(50);
  
  // 清屏
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  delay(50);
  
  int y = 0;
  
  // 显示测试结果状态
  if (all_func_ok) {
    // 全部通过，显示大字体
    display.setCursor(0, y);
    display.setTextSize(2);
    display.print("ALL PASS");
    display.setTextSize(1);
  } else {
    // 有失败，每个失败的外设单独显示一行
    display.setTextSize(1);
    const char* func_names_short[] = {"IMU", "MIC", "SD", "CAM", "WiFi", "BLE", "ADC"};
    
    for (int i = 0; i < 7; i++) {
      if (!func_status[i]) {
        display.setCursor(0, y);
        display.print(func_names_short[i]);
        display.print(" FAIL");
        y += 8;  // 每行 8 像素高度
        
        // 如果屏幕空间不够（最多显示 8 行，每行 8 像素 = 64 像素），停止显示
        if (y >= OLED_HEIGHT) {
          break;
        }
      }
    }
  }
  
  // 发送到显示屏
  display.display();
  delay(50);
  display.display();
  delay(200);
}

// 在 OLED 上显示 IMU、SD、ADC 数据
void oled_display_data() {
  if (!oled_initialized) {
    return;
  }
  
  // 清屏
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  
  int y = 0;
  char buffer[64];
  
  // 第1行：IMU 加速度数据（显示 X、Y、Z）
  display.setCursor(0, y);
  if (func_status[0]) {
    bool data_ready = false;
    unsigned long start_check = millis();
    while (millis() - start_check < 10) {
      if (imu.getDataReady()) {
        data_ready = true;
        break;
      }
      delayMicroseconds(100);
    }
    
    if (data_ready) {
      bool acc_ok = imu.getAccelerometer(acc.x, acc.y, acc.z);
      if (acc_ok) {
        snprintf(buffer, sizeof(buffer), "A:X%.1f Y%.1f Z%.1f", acc.x, acc.y, acc.z);
        display.print(buffer);
      } else {
        display.print("IMU Acc: n/a");
      }
    } else {
      display.print("IMU: wait");
    }
  } else {
    display.print("IMU: n/a");
  }
  y += 8;
  
  // 第2行：IMU 陀螺仪数据（显示 X、Y、Z）
  display.setCursor(0, y);
  if (func_status[0]) {
    if (imu.getDataReady()) {
      bool gyr_ok = imu.getGyroscope(gyr.x, gyr.y, gyr.z);
      if (gyr_ok) {
        snprintf(buffer, sizeof(buffer), "G:X%.1f Y%.1f Z%.1f", gyr.x, gyr.y, gyr.z);
        display.print(buffer);
      } else {
        display.print("IMU Gyr: n/a");
      }
    } else {
      display.print("Gyr: wait");
    }
  } else {
    display.print("Gyr: n/a");
  }
  y += 8;
  
  // 第3行：IMU 温度
  display.setCursor(0, y);
  if (func_status[0]) {
    float temp = imu.getTemperature_C();
    snprintf(buffer, sizeof(buffer), "T:%.2fC", temp);
    display.print(buffer);
  } else {
    display.print("T: n/a");
  }
  y += 8;
  
  // 第4行：SD 卡数据（只显示总容量，使用全局缓存）
  display.setCursor(0, y);
  if (func_status[2]) {
    if (g_sd_cache_valid) {
      snprintf(buffer, sizeof(buffer), "SD Card Size: %lluMB", g_sd_card_mb);
      display.print(buffer);
    } else {
      display.print("SD: reading...");
    }
  } else {
    display.print("SD: FAIL");
  }
  y += 8;
  
  // 第5行：ADC 数据
  display.setCursor(0, y);
  if (func_status[6]) {
    uint32_t sum = 0;
    for (int i = 0; i < 5; i++) {
      sum += analogRead(ADC_PIN);
      delayMicroseconds(100);
    }
    uint16_t raw = sum / 5;
    float adc_voltage = (raw * ADC_REF_VOLTAGE) / ADC_MAX_VALUE;
    float actual_voltage = adc_voltage * ADC_VOLTAGE_DIVIDER;
    snprintf(buffer, sizeof(buffer), "ADC:%.3fV", actual_voltage);
    display.print(buffer);
  } else {
    display.print("ADC: n/a");
  }
  
  // 发送到显示屏
  display.display();
}

// -------------------------- Pre-check functions --------------------------
// 1. QMI8658C IMU test
void test_imu() {
  Serial.print("[1/7] Testing QMI8658C IMU...");
  Wire.begin(IMU_SDA, IMU_SCL);
  if (!imu.begin(Wire, QMI8658_L_SLAVE_ADDRESS, IMU_SDA, IMU_SCL)) {
    Serial.println("FAIL (not detected)");
    func_status[0] = false;
    all_func_ok &= func_status[0];
    return;
  }

  bool self_ok = imu.selfTestAccel() && imu.selfTestGyro();
  if (!self_ok) {
    Serial.println("FAIL (self-test fail)");
    func_status[0] = false;
    all_func_ok &= func_status[0];
    return;
  }

  imu.configAccelerometer(
    SensorQMI8658::ACC_RANGE_4G,
    SensorQMI8658::ACC_ODR_1000Hz,
    SensorQMI8658::LPF_MODE_0);
  imu.configGyroscope(
    SensorQMI8658::GYR_RANGE_64DPS,
    SensorQMI8658::GYR_ODR_896_8Hz,
    SensorQMI8658::LPF_MODE_3);
  imu.enableGyroscope();
  imu.enableAccelerometer();

  bool data_ok = false;
  bool acc_ok = false;
  bool gyr_ok = false;
  uint32_t start = millis();
  while (millis() - start < IMU_DATA_TIMEOUT_MS) {
    if (imu.getDataReady()) {
      acc_ok = imu.getAccelerometer(acc.x, acc.y, acc.z);
      gyr_ok = imu.getGyroscope(gyr.x, gyr.y, gyr.z);
      data_ok = acc_ok || gyr_ok;
      break;
    }
    delay(5);
  }

  if (!data_ok) {
    Serial.println("FAIL (no data)");
    func_status[0] = false;
    all_func_ok &= func_status[0];
    return;
  }

  Serial.println("OK (self-test pass)");
  if (acc_ok) {
    Serial.print("ACCEL: x->");
    Serial.print(acc.x);
    Serial.print(", y->");
    Serial.print(acc.y);
    Serial.print(", z->");
    Serial.print(acc.z);
    Serial.print("  ");
  } else {
    Serial.print("ACCEL: n/a  ");
  }
  if (gyr_ok) {
    Serial.print("GYRO: x->");
    Serial.print(gyr.x);
    Serial.print(", y->");
    Serial.print(gyr.y);
    Serial.print(", z->");
    Serial.print(gyr.z);
    Serial.print("  ");
  } else {
    Serial.print("GYRO: n/a  ");
  }
  Serial.print("Temperature:");
  Serial.print(imu.getTemperature_C());
  Serial.println(" C");

  func_status[0] = true;
  all_func_ok &= func_status[0];
}

// 2. Digital microphone test
void test_microphone() {
  Serial.print("[2/7] Testing digital mic...");
  I2S.setPinsPdmRx(MIC_CLK, MIC_DATA);
  if (I2S.begin(I2S_MODE_PDM_RX, 16000, I2S_DATA_BIT_WIDTH_16BIT, I2S_SLOT_MODE_MONO)) {
    int16_t buf[MIC_SAMPLE_COUNT];
    // Warm-up read to flush initial samples
    I2S.readBytes((char*)buf, sizeof(buf));
    size_t bytes_read = I2S.readBytes((char*)buf, sizeof(buf)); // ESP_I2S expects char*
    I2S.end();

    if (bytes_read != sizeof(buf)) {
      Serial.println("FAIL (read timeout)");
      func_status[1] = false;
      all_func_ok &= func_status[1];
      return;
    }

    int16_t min_v = 32767;
    int16_t max_v = -32768;
    uint32_t sum_abs = 0;
    uint16_t non_zero = 0;
    for (int i = 0; i < MIC_SAMPLE_COUNT; i++) {
      int16_t v = buf[i];
      if (v < min_v) min_v = v;
      if (v > max_v) max_v = v;
      int abs_v = v >= 0 ? v : -v;
      sum_abs += (uint32_t)abs_v;
      if (abs_v > 1) non_zero++;
    }

    int peak = max_v;
    int neg_peak = -((int)min_v);
    if (neg_peak > peak) peak = neg_peak;
    int range = (int)max_v - (int)min_v;
    uint32_t mean_abs = sum_abs / MIC_SAMPLE_COUNT;

    func_status[1] = (peak >= MIC_MIN_PEAK) &&
                     (range >= MIC_MIN_RANGE) &&
                     (mean_abs >= MIC_MIN_MEAN) &&
                     (non_zero >= MIC_MIN_NONZERO);

    if (func_status[1]) {
      Serial.print("OK (peak=");
      Serial.print(peak);
      Serial.print(", mean=");
      Serial.print(mean_abs);
      Serial.print(", range=");
      Serial.print(range);
      Serial.print(", nonzero=");
      Serial.print(non_zero);
      Serial.println(")");
    } else {
      Serial.print("FAIL (peak=");
      Serial.print(peak);
      Serial.print(", mean=");
      Serial.print(mean_abs);
      Serial.print(", range=");
      Serial.print(range);
      Serial.print(", nonzero=");
      Serial.print(non_zero);
      Serial.println(")");
    }
  } else {
    Serial.println("FAIL (I2S init failed)");
    func_status[1] = false;
  }
  all_func_ok &= func_status[1];
}

// 3. SD card test
void test_sd_card() {
  Serial.print("[3/7] Testing SD card...");
  pinMode(SD_CS_PIN, OUTPUT);
  digitalWrite(SD_CS_PIN, HIGH); delay(100);
  if (SD.begin(SD_CS_PIN)) {
    func_status[2] = SD.cardType() != CARD_NONE;
    if (func_status[2]) {
      uint64_t size_mb = SD.cardSize() / (1024ULL * 1024ULL);
      // 保存到全局变量，后续直接使用，不再读取
      g_sd_card_mb = size_mb;
      g_sd_total_mb = SD.totalBytes() / (1024ULL * 1024ULL);
      g_sd_used_mb = SD.usedBytes() / (1024ULL * 1024ULL);
      g_sd_cache_valid = true;
      Serial.print("OK (mounted), size=");
      Serial.print(size_mb);
      Serial.println(" MB");
    } else {
      Serial.println("FAIL (no card)");
      g_sd_cache_valid = false;
    }
  } else {
    Serial.println("FAIL (mount failed)");
    func_status[2] = false;
    g_sd_cache_valid = false;
  }
  all_func_ok &= func_status[2];
}

// 4. OV5640 camera test (pre-init)
void test_camera() {
  Serial.print("[4/7] Testing OV5640 camera...");
  rgb_led_release();
  delay(100); // Small delay before camera operations
  
  // Check PSRAM availability using more reliable method
  bool has_psram = psramFound();
  if (!has_psram) {
    Serial.println("WARNING: PSRAM not found, camera may have limited performance");
  }
  
  camera_config_t temp_config = {};
  temp_config.pin_pwdn = PWDN_GPIO_NUM;
  temp_config.pin_reset = RESET_GPIO_NUM;
  temp_config.pin_xclk = XCLK_GPIO_NUM;
  temp_config.pin_sscb_sda = SIOD_GPIO_NUM;
  temp_config.pin_sscb_scl = SIOC_GPIO_NUM;
  temp_config.pin_d7 = Y9_GPIO_NUM;
  temp_config.pin_d6 = Y8_GPIO_NUM;
  temp_config.pin_d5 = Y7_GPIO_NUM;
  temp_config.pin_d4 = Y6_GPIO_NUM;
  temp_config.pin_d3 = Y5_GPIO_NUM;
  temp_config.pin_d2 = Y4_GPIO_NUM;
  temp_config.pin_d1 = Y3_GPIO_NUM;
  temp_config.pin_d0 = Y2_GPIO_NUM;
  temp_config.pin_vsync = VSYNC_GPIO_NUM;
  temp_config.pin_href = HREF_GPIO_NUM;
  temp_config.pin_pclk = PCLK_GPIO_NUM;
  temp_config.xclk_freq_hz = 20000000;
  temp_config.ledc_timer = LEDC_TIMER_0;
  temp_config.ledc_channel = LEDC_CHANNEL_0;
  temp_config.pixel_format = PIXFORMAT_JPEG;
  temp_config.frame_size = FRAMESIZE_QVGA;
  temp_config.jpeg_quality = 12;
  temp_config.fb_count = 1;
  temp_config.fb_location = has_psram ? CAMERA_FB_IN_PSRAM : CAMERA_FB_IN_DRAM;
  
  esp_err_t err = esp_camera_init(&temp_config);
  func_status[3] = (err == ESP_OK);
  
  if (func_status[3]) {
    delay(200); // Give camera time to stabilize
    esp_camera_deinit(); // Deinit after test; stream stage will re-init
    delay(300); // Ensure resources are fully released
    Serial.println("OK (init success)");
  } else {
    Serial.printf("FAIL (init failed: 0x%x)\n", err);
  }
  
  all_func_ok &= func_status[3];
}

// 5. WiFi module test
void test_wifi() {
  Serial.print("[5/7] Testing WiFi...");
  WiFi.mode(WIFI_AP);
  func_status[4] = WiFi.softAP(ap_ssid, ap_password);
  IPAddress ap_ip = WiFi.softAPIP();
  WiFi.softAPdisconnect(true);
  if (func_status[4]) {
    Serial.print("OK (AP mode), IP=");
    Serial.println(ap_ip.toString());
  } else {
    Serial.println("FAIL (AP mode failed)");
  }
  all_func_ok &= func_status[4];
}

// 6. BLE module test
void test_ble() {
  Serial.print("[6/7] Testing BLE...");
  BLEDevice::init("W11_Test");
  func_status[5] = true;
  BLEDevice::deinit();
  Serial.println(func_status[5] ? "OK (init ok)" : "FAIL (init failed)");
  all_func_ok &= func_status[5];
}

// 7. ADC sampling test
void test_adc() {
  Serial.print("[7/7] Testing ADC...");
  pinMode(ADC_PIN, INPUT);
  uint32_t sum = 0;
  uint16_t min_v = 4095;
  uint16_t max_v = 0;
  for (int i = 0; i < ADC_SAMPLE_COUNT; i++) {
    uint16_t v = analogRead(ADC_PIN);
    sum += v;
    if (v < min_v) min_v = v;
    if (v > max_v) max_v = v;
    delay(10);
  }
  uint16_t avg = sum / ADC_SAMPLE_COUNT;
  bool in_range = (avg >= ADC_MIN_RAW) && (avg <= ADC_MAX_RAW);
  func_status[6] = in_range;
  if (func_status[6]) {
    Serial.print("OK (avg=");
    Serial.print(avg);
    Serial.print(", min=");
    Serial.print(min_v);
    Serial.print(", max=");
    Serial.print(max_v);
    Serial.println(")");
  } else {
    Serial.print("FAIL (avg=");
    Serial.print(avg);
    Serial.print(", min=");
    Serial.print(min_v);
    Serial.print(", max=");
    Serial.print(max_v);
    Serial.println(")");
  }
  all_func_ok &= func_status[6];
}

// -------------------------- Data output functions --------------------------
void print_imu_data() {
  if (!imu.getDataReady()) {
    Serial.println("IMU: data not ready");
    return;
  }

  bool acc_ok = imu.getAccelerometer(acc.x, acc.y, acc.z);
  bool gyr_ok = imu.getGyroscope(gyr.x, gyr.y, gyr.z);

  if (!acc_ok && !gyr_ok) {
    Serial.println("IMU: read failed");
    return;
  }

  Serial.print("IMU ");
  if (acc_ok) {
    Serial.print("ACCEL: x->");
    Serial.print(acc.x);
    Serial.print(", y->");
    Serial.print(acc.y);
    Serial.print(", z->");
    Serial.print(acc.z);
    Serial.print("  ");
  } else {
    Serial.print("ACCEL: n/a  ");
  }

  if (gyr_ok) {
    Serial.print("GYRO: x->");
    Serial.print(gyr.x);
    Serial.print(", y->");
    Serial.print(gyr.y);
    Serial.print(", z->");
    Serial.print(gyr.z);
    Serial.print("  ");
  } else {
    Serial.print("GYRO: n/a  ");
  }

  Serial.print("Temperature:");
  Serial.print(imu.getTemperature_C());
  Serial.println("°C");
}

void print_adc_data() {
  uint32_t sum = 0;
  for (int i = 0; i < ADC_SAMPLE_COUNT; i++) {
    sum += analogRead(ADC_PIN);
    delay(2);
  }
  uint16_t raw = sum / ADC_SAMPLE_COUNT;
  float adc_voltage = (raw * ADC_REF_VOLTAGE) / ADC_MAX_VALUE;
  float actual_voltage = adc_voltage * ADC_VOLTAGE_DIVIDER;
  Serial.printf("ADC: raw=%u, voltage=%.3fV\n", raw, actual_voltage);
}

void print_sd_data() {
  // 如果缓存有效，直接使用缓存值，不进行任何 SD 卡操作（避免卡住）
  if (g_sd_cache_valid) {
    Serial.printf("SD: card=%llu MB, total=%llu MB, used=%llu MB\n",
                  g_sd_card_mb, g_sd_total_mb, g_sd_used_mb);
    return;
  }
  
  // 如果缓存无效，尝试读取一次（但只读取一次，避免卡住）
  if (func_status[2] && SD.cardType() != CARD_NONE) {
    g_sd_card_mb = SD.cardSize() / (1024ULL * 1024ULL);
    g_sd_total_mb = SD.totalBytes() / (1024ULL * 1024ULL);
    g_sd_used_mb = SD.usedBytes() / (1024ULL * 1024ULL);
    g_sd_cache_valid = true;
    Serial.printf("SD: card=%llu MB, total=%llu MB, used=%llu MB\n",
                  g_sd_card_mb, g_sd_total_mb, g_sd_used_mb);
  } else {
    Serial.println("SD: no card");
  }
}

// -------------------------- Main program --------------------------
void setup() {
  // Init serial
  Serial.begin(115200);
  while (!Serial && millis() < 5000);
  Serial.println("==========================================================");
  Serial.println("W11 Factory Test + Data Output");
  Serial.println("==========================================================");

  // 初始化 OLED 显示屏（在测试之前）
  // init_oled() 函数内部已经显示了启动信息，这里不需要重复显示
  init_oled();
  if (!oled_initialized) {
    Serial.println("警告：OLED 初始化失败！");
  } else {
    Serial.println("OLED 初始化成功，等待测试开始...");
    delay(500);  // 短暂延迟，让用户看到启动信息
  }

  // Keep RGB LED pin released during tests
  rgb_led_release();

  // Run all tests
  test_imu();
  test_microphone();
  test_sd_card();
  test_camera();
  delay(500); // Extra delay after camera test to ensure resources are released
  test_wifi();
  test_ble();
  test_adc();

  // Print summary
  print_separator();
  Serial.println("==================== Test Summary ====================");
  for (int i = 0; i < 7; i++) {
    Serial.printf("[%d/7] %s: %s\n", i+1, func_names[i].c_str(), func_status[i] ? "PASS" : "FAIL");
  }

  // 在 OLED 上显示测试结果
  if (oled_initialized) {
    oled_display_test_result();
    delay(2000);  // 显示测试结果 2 秒
  }

  // 无论测试是否全部通过，都进入数据输出模式（显示 IMU、SD、ADC 数据）
  print_separator();
  Serial.println("==================== Data Output Mode ====================");
  data_output_mode = true;
  
  // 如果 OLED 已初始化，立即显示一次数据
  if (oled_initialized) {
    delay(500);  // 等待 IMU 和 SD 初始化完成
    oled_display_data();
    delay(200);
  }
  
  // 初始化 IMU（如果测试通过）
  if (func_status[0]) {
    Wire.begin(IMU_SDA, IMU_SCL);
    imu.begin(Wire, QMI8658_L_SLAVE_ADDRESS, IMU_SDA, IMU_SCL);
    imu.configAccelerometer(
      SensorQMI8658::ACC_RANGE_4G,
      SensorQMI8658::ACC_ODR_1000Hz,
      SensorQMI8658::LPF_MODE_0);
    imu.configGyroscope(
      SensorQMI8658::GYR_RANGE_64DPS,
      SensorQMI8658::GYR_ODR_896_8Hz,
      SensorQMI8658::LPF_MODE_3);
    imu.enableGyroscope();
    imu.enableAccelerometer();
    delay(200);  // 等待 IMU 数据就绪
  }
  
  // 初始化 SD 卡（如果测试通过）
  // 注意：SD 卡大小信息已在测试阶段读取并保存到全局变量，这里不再重复读取
  if (func_status[2]) {
    pinMode(SD_CS_PIN, OUTPUT);
    digitalWrite(SD_CS_PIN, HIGH);
    SD.begin(SD_CS_PIN);
  }
  
  // 如果 OLED 已初始化，立即显示一次数据（确保从测试结果切换到数据显示）
  if (oled_initialized) {
    delay(500);  // 等待 IMU 和 SD 初始化完成
    oled_display_data();
    delay(200);
  }
}

void loop() {
  if (!data_output_mode) {
    delay(1000);
    return;
  }

  static uint32_t last_output = 0;
  static uint32_t last_oled_update = 0;
  uint32_t now = millis();
  
  // 串口输出（每1秒）
  if (now - last_output >= DATA_OUTPUT_INTERVAL_MS) {
    last_output = now;
    print_imu_data();
    print_adc_data();
    
    // SD 卡数据读取可能很慢，放在最后，即使卡住也不影响其他数据输出
    print_sd_data();
    Serial.println();
  }
  
  // OLED 显示更新（每500ms，持续更新数据）
  if (now - last_oled_update >= 500) {
    last_oled_update = now;
    if (oled_initialized) {
      oled_display_data();
    }
  }
  
  delay(10);  // 减少 CPU 占用
}