#define CAMERA_MODEL_XIAO_ESP32S3 // Has PSRAM
#include <Arduino.h>
#include <Wire.h>
#include <FS.h>
#include <SD.h>
#include <SPI.h>
#include <FastLED.h>
#include <U8g2lib.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"

// OLED 0.96" 128x64 I2C display pins (SDA=GPIO1, SCL=GPIO2)
#define OLED_SDA 1
#define OLED_SCL 2
#define OLED_I2C_ADDR 0x3C  // Common I2C address for SSD1306 (try 0x3D if 0x3C doesn't work)

// RGB LED pin
#define RGB_DATA_PIN 48
#define NUM_LEDS 1
#define LED_TYPE WS2812
#define COLOR_ORDER GRB

// Hardware pin definitions
#define USER_LED_PIN 21        // USER_LED + SD card CS
#define SD_CS_PIN USER_LED_PIN // SD card CS shares USER_LED

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

// 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

// Test configuration
#define IMU_DATA_TIMEOUT_MS 200
#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
#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";

// U8g2 OLED display object (SSD1306 128x64 I2C)
// U8G2_SSD1306_128X64_NONAME_F_HW_I2C: Full buffer, Hardware I2C
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

// Sensor objects
SensorQMI8658 imu;
I2SClass I2S;
IMUdata acc;
IMUdata gyr;

// Test status
bool all_func_ok = true;
String func_names[7] = {
  "IMU", "Mic", "SD",
  "Camera", "WiFi", "BLE", "ADC"
};
bool func_status[7] = {false};

// RGB LED
CRGB leds[NUM_LEDS];

// 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);
}

// Scan I2C bus for OLED
bool scan_i2c_for_oled() {
  Serial.println("Scanning I2C bus for OLED...");
  byte error, address;
  int nDevices = 0;
  
  for (address = 0x3C; address <= 0x3D; address++) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    
    if (error == 0) {
      Serial.printf("I2C device found at address 0x%02X\n", address);
      nDevices++;
      return true;
    }
  }
  
  if (nDevices == 0) {
    Serial.println("No I2C devices found (OLED not detected)");
  }
    return false;
  }

// Update status display using U8g2
void update_status_display() {
  Serial.println("Updating OLED display...");
  
  u8g2.clearBuffer();
  
  // Set font (small font for 128x64 OLED)
  u8g2.setFont(u8g2_font_6x10_tf);  // Small font for compact display
  
  // Title
  u8g2.setCursor(0, 8);
  u8g2.print("Factory Test");
  
  // Draw separator line
  u8g2.drawHLine(0, 10, 128);
  
  // Display test results (compact format)
  int y_pos = 20;
  int line_height = 8;
  
  for (int i = 0; i < 7; i++) {
    char status_text[32];
    sprintf(status_text, "%d:%s %s", i+1, func_names[i].c_str(), func_status[i] ? "OK" : "FAIL");
    
    u8g2.setCursor(0, y_pos + i * line_height);
    u8g2.print(status_text);
  }
  
  // Send buffer to display
  u8g2.sendBuffer();
  Serial.println("OLED display updated");
}

// -------------------------- Test 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)");
  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];
    I2S.readBytes((char*)buf, sizeof(buf));
    size_t bytes_read = I2S.readBytes((char*)buf, sizeof(buf));
    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.println("OK");
    } else {
      Serial.println("FAIL");
    }
  } 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]) {
      Serial.println("OK");
    } else {
      Serial.println("FAIL (no card)");
    }
  } else {
    Serial.println("FAIL (mount failed)");
    func_status[2] = false;
  }
  all_func_ok &= func_status[2];
}

// Release camera pins (OLED uses I2C GPIO1,2 - no conflict with camera)
void release_camera_pins() {
  // OLED uses I2C (GPIO1, 2) - no pin conflict with camera
  // This function is kept for compatibility but not needed for OLED
  Serial.println("OLED uses I2C (GPIO1, 2) - no pin conflict with camera");
}

// 4. OV5640 camera test
void test_camera() {
  Serial.print("[4/7] Testing OV5640 camera...");
  rgb_led_release();
  delay(100);
  
  bool has_psram = psramFound();
  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);
    esp_camera_deinit();
    delay(300);
    Serial.println("OK");
  } else {
    Serial.printf("FAIL (0x%x)\n", err);
  }
  
  // Immediately release camera pins that conflict with display
  // This must be done right after camera deinit
  release_camera_pins();
  delay(100);
  
  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.println("OK");
  } else {
    Serial.println("FAIL");
  }
  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" : "FAIL");
  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.println("OK");
  } else {
    Serial.println("FAIL");
  }
  all_func_ok &= func_status[6];
}


void setup() {
  Serial.begin(115200);
  while (!Serial && millis() < 3000) {}
  Serial.println("==========================================================");
  Serial.println("W11 Factory Test (Display Mode)");
  Serial.println("==========================================================");

  // Skip OLED initialization at start to avoid hanging
  // Will initialize after all tests complete
  Serial.println("OLED initialization will be done after tests complete");

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

  // Run all tests
  Serial.println("Running all tests...");
  test_imu();
  delay(100);
  test_microphone();
  delay(100);
  test_sd_card();
  delay(100);
  test_camera();
  delay(500);
  test_wifi();
  delay(100);
  test_ble();
  delay(100);
  test_adc();

  // Print summary to serial
  Serial.println("--------------------------------------------------");
  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");
  }

  // Print final status
  if (all_func_ok) {
    Serial.println("--------------------------------------------------");
    Serial.println("All tests passed!");
    digitalWrite(USER_LED_PIN, HIGH); // Ensure SD CS is idle high
  } else {
    Serial.println("\nTest failed! Some devices are not working.");
  }

  // Initialize OLED display (after all tests to avoid hanging)
  Serial.println("--------------------------------------------------");
  Serial.println("Initializing OLED display...");
  
  // Reinitialize I2C for OLED (IMU test may have changed Wire config)
  Serial.println("Setting up I2C for OLED (SDA=GPIO1, SCL=GPIO2)...");
  Wire.end();  // End current Wire instance
  delay(10);
  Wire.begin(OLED_SDA, OLED_SCL);  // Reinitialize with OLED pins
  Wire.setClock(100000);  // Use slower speed (100kHz) for reliability
  delay(50);
  
  // Scan I2C to verify OLED connection BEFORE trying to initialize
  Serial.println("Scanning I2C bus for OLED...");
  bool oled_detected = scan_i2c_for_oled();
  
  if (!oled_detected) {
    Serial.println("WARNING: OLED not detected on I2C bus!");
    Serial.println("Skipping OLED initialization to avoid hanging.");
    Serial.println("Check connections: SDA=GPIO1, SCL=GPIO2, VCC=3.3V, GND=GND");
  } else {
    // OLED detected, try to initialize
    Serial.println("OLED detected on I2C bus, initializing U8g2...");
    
    // Try to initialize with detected address
    u8g2.setI2CAddress(OLED_I2C_ADDR);
    Serial.println("Calling u8g2.begin()...");
    Serial.println("(If program hangs here, OLED may not be responding)");
    
    u8g2.begin();
    Serial.println("u8g2.begin() completed successfully!");
    
    delay(50);
    
    // Clear display
    Serial.println("Clearing display...");
    u8g2.clearBuffer();
    u8g2.sendBuffer();
    delay(100);
    
    // Display test results on OLED
    Serial.println("Displaying test results on OLED...");
    update_status_display();
    delay(200);
    
    // Force refresh multiple times to ensure display updates
    Serial.println("Refreshing display...");
    for (int i = 0; i < 3; i++) {
      u8g2.sendBuffer();
    delay(50);
    }
    Serial.println("OLED display update completed!");
  }

  Serial.println("Setup complete");
  Serial.println("==========================================================");
  Serial.println("OLED Display: I2C (SDA=GPIO1, SCL=GPIO2)");
  Serial.println("No pin conflict with camera - both can work simultaneously");
  Serial.println("==========================================================");
  }

void loop() {
  // U8g2 doesn't need periodic updates - display is updated when needed
  delay(100);
}
