#include "esp_camera.h"
#include <WiFi.h>
#include <WebServer.h>
#include "driver/gpio.h"
#include <Wire.h>

#define CAMERA_MODEL_XIAO_ESP32S3 // Has PSRAM

#include "camera_pins.h"

// GPIO48 is shared with RGB LED on W11, need to release it before camera init
#define RGB_DATA_PIN 48

// If your camera module has RESET and PWDN pins connected to GPIOs, 
// uncomment and set these values (check your expansion board schematic)
// Common GPIO pins used for camera control on ESP32-S3:
// #define CUSTOM_PWDN_GPIO_NUM  46  // Example: if PWDN is connected to GPIO46
// #define CUSTOM_RESET_GPIO_NUM 45  // Example: if RESET is connected to GPIO45

// Try to auto-detect RESET and PWDN pins (experimental)
// This will try common GPIO pins if RESET/PWDN are not defined
#define TRY_AUTO_DETECT_PINS 1

// ===========================
// WiFi configuration
// ===========================
// Target router for camera streaming (set via web page, not hard-coded here)
String sta_ssid = "";
String sta_password = "";

// AP information used at boot time for WiFi configuration
const char *ap_ssid = "ESP32-CAM-SETUP";
const char *ap_password = "12345678";  // at least 8 characters

// Run config page on port 82 to avoid conflict with camera UI (80) and stream (81)
WebServer configServer(82);
bool wifiConfigured = false;   // whether WiFi configuration is completed and STA is connected
bool cameraStarted = false;    // whether camera HTTP server has been started

void startCameraServer();
void setupLedFlash(int pin);

// Release GPIO48 for camera use (disable RGB LED output)
void release_gpio48() {
  Serial.println("Releasing GPIO48 (RGB LED pin) for camera use...");
  
  // First, try to set it low
  pinMode(RGB_DATA_PIN, OUTPUT);
  digitalWrite(RGB_DATA_PIN, LOW);
  delay(10);
  
  // Then set to input floating
#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);
#endif
  
  delay(100);  // Wait for GPIO to stabilize
  Serial.println("GPIO48 released");
}

// Enable software pull-up resistors on I2C pins
void enable_i2c_pullups() {
  Serial.println("Enabling I2C pull-up resistors...");
  
#if defined(ARDUINO_ARCH_ESP32)
  // Enable internal pull-up resistors on I2C pins
  pinMode(SIOD_GPIO_NUM, INPUT_PULLUP);
  pinMode(SIOC_GPIO_NUM, INPUT_PULLUP);
  
  // Also set via GPIO driver for more control
  gpio_set_pull_mode((gpio_num_t)SIOD_GPIO_NUM, GPIO_PULLUP_ONLY);
  gpio_set_pull_mode((gpio_num_t)SIOC_GPIO_NUM, GPIO_PULLUP_ONLY);
  
  delay(50);  // Wait for pull-ups to stabilize
  
  // Check if pins are now HIGH
  int sda_state = digitalRead(SIOD_GPIO_NUM);
  int scl_state = digitalRead(SIOC_GPIO_NUM);
  
  Serial.printf("  SDA (GPIO%d): %s\n", SIOD_GPIO_NUM, sda_state == HIGH ? "HIGH" : "LOW");
  Serial.printf("  SCL (GPIO%d): %s\n", SIOC_GPIO_NUM, scl_state == HIGH ? "HIGH" : "LOW");
  
  if (sda_state == HIGH && scl_state == HIGH) {
    Serial.println("  ✓ I2C pull-ups enabled successfully");
  } else {
    Serial.println("  ⚠️  Warning: I2C pins still LOW after enabling pull-ups");
    Serial.println("     This may indicate hardware issue (short circuit or no power)");
  }
#else
  pinMode(SIOD_GPIO_NUM, INPUT_PULLUP);
  pinMode(SIOC_GPIO_NUM, INPUT_PULLUP);
  delay(50);
#endif
}

// Try to initialize RESET and PWDN on common GPIO pins
void try_init_camera_pins(int pwdn_pin, int reset_pin) {
  if (pwdn_pin >= 0) {
    Serial.printf("  Trying PWDN on GPIO%d\n", pwdn_pin);
    pinMode(pwdn_pin, OUTPUT);
    digitalWrite(pwdn_pin, LOW);  // Power on camera (PWDN is active LOW)
    delay(50);
  }
  
  if (reset_pin >= 0) {
    Serial.printf("  Trying RESET on GPIO%d\n", reset_pin);
    pinMode(reset_pin, OUTPUT);
    digitalWrite(reset_pin, HIGH);  // Release reset (RESET is active LOW)
    delay(50);
    digitalWrite(reset_pin, LOW);   // Reset pulse (active LOW)
    delay(100);  // Hold reset for 100ms
    digitalWrite(reset_pin, HIGH); // Release reset
    delay(200);  // Wait for camera to stabilize after reset
  }
}

// Initialize camera GPIO pins (PWDN, RESET if available)
void init_camera_gpio() {
  Serial.println("Initializing camera GPIO pins...");
  
  // Use custom pins if defined, otherwise use pins from camera_pins.h
  int pwdn_pin = PWDN_GPIO_NUM;
  int reset_pin = RESET_GPIO_NUM;
  
#ifdef CUSTOM_PWDN_GPIO_NUM
  pwdn_pin = CUSTOM_PWDN_GPIO_NUM;
  Serial.println("  Using custom PWDN pin");
#endif

#ifdef CUSTOM_RESET_GPIO_NUM
  reset_pin = CUSTOM_RESET_GPIO_NUM;
  Serial.println("  Using custom RESET pin");
#endif
  
  // Initialize PWDN pin (Power Down, active LOW)
  if (pwdn_pin >= 0) {
    Serial.printf("  Setting PWDN pin GPIO%d to LOW (power on)\n", pwdn_pin);
    pinMode(pwdn_pin, OUTPUT);
    digitalWrite(pwdn_pin, LOW);  // Power on camera (PWDN is active LOW)
    delay(50);
  } else {
    Serial.println("  PWDN pin not defined (using -1)");
    Serial.println("  Note: According to expansion board schematic:");
    Serial.println("        PWDN (Pin 8) is hard-wired to VCC_3V3 via 10K pull-up");
    Serial.println("        This means PWDN is always HIGH (power down state if active LOW)");
    Serial.println("        Some camera modules may not work if they require PWDN=LOW to power on");
    Serial.println("        If camera doesn't power on, it may be incompatible with this expansion board");
  }
  
  // Initialize RESET pin (active LOW, pulse to reset)
  if (reset_pin >= 0) {
    Serial.printf("  Setting RESET pin GPIO%d\n", reset_pin);
    pinMode(reset_pin, OUTPUT);
    digitalWrite(reset_pin, HIGH);  // Release reset (RESET is active LOW)
    delay(50);
    digitalWrite(reset_pin, LOW);   // Reset pulse (active LOW)
    delay(100);  // Hold reset for 100ms
    digitalWrite(reset_pin, HIGH); // Release reset
    delay(200);  // Wait for camera to stabilize after reset
  } else {
    Serial.println("  RESET pin not defined (using -1)");
    Serial.println("  Note: According to expansion board schematic:");
    Serial.println("        RESET (Pin 6) is hard-wired to VCC_3V3 via 10K pull-up");
    Serial.println("        This means RESET is always HIGH (released state)");
    Serial.println("        Camera cannot be hardware reset via GPIO");
    // Even without RESET pin, wait for camera to stabilize
    delay(300);
  }
  
  Serial.println("Camera GPIO pins initialized");
}

// Check camera power status by checking data pins
bool check_camera_power() {
  Serial.println("Checking camera power status...");
  
  // Check Y4 pin state (known to be HIGH when camera is powered)
  pinMode(Y4_GPIO_NUM, INPUT);
  delay(10);
  int y4_state = digitalRead(Y4_GPIO_NUM);
  
  Serial.printf("  Y4 (GPIO%d): %s\n", Y4_GPIO_NUM, y4_state == HIGH ? "HIGH" : "LOW");
  
  if (y4_state == HIGH) {
    Serial.println("  ✓ Camera appears to be powered (Y4 is HIGH)");
    return true;
  } else {
    Serial.println("  ⚠️  Camera may not be powered (Y4 is LOW)");
    Serial.println("     This is normal for cameras without power or before initialization");
    return false;
  }
}

// Check I2C pin states
void check_i2c_pins() {
  Serial.println("Checking I2C pin states...");
  
  // Set pins to input and check their state
  pinMode(SIOD_GPIO_NUM, INPUT);
  pinMode(SIOC_GPIO_NUM, INPUT);
  delay(10);
  
  int sda_state = digitalRead(SIOD_GPIO_NUM);
  int scl_state = digitalRead(SIOC_GPIO_NUM);
  
  Serial.printf("  SDA (GPIO%d): %s\n", SIOD_GPIO_NUM, sda_state == HIGH ? "HIGH" : "LOW");
  Serial.printf("  SCL (GPIO%d): %s\n", SIOC_GPIO_NUM, scl_state == HIGH ? "HIGH" : "LOW");
  
  if (sda_state == LOW || scl_state == LOW) {
    Serial.println("  Warning: I2C pins are LOW - may indicate:");
    Serial.println("    - Missing pull-up resistors");
    Serial.println("    - Short circuit");
    Serial.println("    - Camera not powered");
  } else if (sda_state == HIGH && scl_state == HIGH) {
    Serial.println("  I2C pins are HIGH - pull-up resistors may be present");
  }
}

// Test I2C communication with camera
bool test_camera_i2c() {
  Serial.println("Testing I2C communication with camera...");
  
  // Check pin states first
  check_i2c_pins();
  
  // Try different I2C speeds
  for (uint32_t speed = 100000; speed <= 400000; speed *= 2) {
    Wire.begin(SIOD_GPIO_NUM, SIOC_GPIO_NUM);
    Wire.setClock(speed);
    delay(200);  // Wait for I2C to stabilize
    
    byte error, address;
    int nDevices = 0;
    
    Serial.printf("Scanning I2C bus at %d Hz...\n", speed);
    for (address = 1; address < 127; address++) {
      Wire.beginTransmission(address);
      error = Wire.endTransmission();
      if (error == 0) {
        Serial.printf("  I2C device found at address 0x%02X", address);
        if (address == 0x3C || address == 0x78) {
          Serial.println(" (likely OV5640/OV2640/OV3660)");
        } else {
          Serial.println();
        }
        nDevices++;
      } else if (error == 4) {
        Serial.printf("  Unknown error at address 0x%02X\n", address);
      }
    }
    
    if (nDevices > 0) {
      Serial.printf("  I2C scan complete. Found %d device(s).\n", nDevices);
      return true;
    }
    
    delay(100);
  }
  
  Serial.println("  No I2C devices found at any speed!");
  Serial.println("  Possible issues:");
  Serial.println("    - Camera not connected");
  Serial.println("    - Wrong I2C pins");
  Serial.println("    - Camera power issue");
  Serial.println("    - I2C pull-up resistors missing");
  Serial.println("    - Camera needs RESET/PWDN pins to be initialized");
  Serial.println("  Note: Some cameras may not respond to I2C scan but still work.");
  return false;
}

// Print camera pin configuration for debugging
void print_camera_pins() {
  Serial.println("\n=== Camera Pin Configuration ===");
  Serial.printf("I2C: SDA=GPIO%d, SCL=GPIO%d\n", SIOD_GPIO_NUM, SIOC_GPIO_NUM);
  Serial.printf("Data: Y2=GPIO%d, Y3=GPIO%d, Y4=GPIO%d, Y5=GPIO%d\n", 
                 Y2_GPIO_NUM, Y3_GPIO_NUM, Y4_GPIO_NUM, Y5_GPIO_NUM);
  Serial.printf("Data: Y6=GPIO%d, Y7=GPIO%d, Y8=GPIO%d, Y9=GPIO%d\n", 
                 Y6_GPIO_NUM, Y7_GPIO_NUM, Y8_GPIO_NUM, Y9_GPIO_NUM);
  Serial.printf("Control: XCLK=GPIO%d, PCLK=GPIO%d\n", XCLK_GPIO_NUM, PCLK_GPIO_NUM);
  Serial.printf("Control: VSYNC=GPIO%d, HREF=GPIO%d\n", VSYNC_GPIO_NUM, HREF_GPIO_NUM);
  Serial.printf("Control: PWDN=GPIO%d, RESET=GPIO%d\n", PWDN_GPIO_NUM, RESET_GPIO_NUM);
  Serial.println("================================\n");
}

// Configuration home page: scan nearby WiFi, allow user to select SSID and enter password
void handleRoot() {
  int n = WiFi.scanNetworks();

  String html = "<!DOCTYPE html><html><head><meta charset='utf-8'>"
                "<title>WiFi Setup - ESP32 Camera</title>"
                "<meta name='viewport' content='width=device-width,initial-scale=1'>"
                "<style>body{font-family:-apple-system,BlinkMacSystemFont,Segoe UI,Roboto,Helvetica,Arial,sans-serif;"
                "margin:0;padding:16px;background:#f5f5f5;color:#333;}"
                ".card{max-width:480px;margin:0 auto;background:#fff;border-radius:12px;padding:20px;"
                "box-shadow:0 4px 12px rgba(0,0,0,0.08);}"
                "h1{font-size:20px;margin:0 0 12px;}"
                "p{font-size:14px;line-height:1.6;}"
                "label{display:block;margin:12px 0 6px;font-weight:600;font-size:14px;}"
                "select,input{width:100%;padding:8px 10px;font-size:14px;border-radius:6px;border:1px solid #ccc;"
                "box-sizing:border-box;}"
                "button{margin-top:16px;width:100%;padding:10px 0;border:none;border-radius:20px;background:#1976d2;"
                "color:#fff;font-size:15px;font-weight:600;cursor:pointer;}"
                "button:active{background:#125aa0;}"
                ".tip{background:#e3f2fd;border-radius:8px;padding:10px 12px;font-size:13px;margin-bottom:12px;}"
                ".tip strong{display:block;margin-bottom:4px;}"
                ".small{font-size:12px;color:#777;margin-top:8px;}"
                "</style></head><body><div class='card'>"
                "<h1>WiFi Configuration</h1>"
                "<div class='tip'><strong>How to use</strong>"
          "<p>1. On your phone or PC, connect to this device AP: SSID <b>";
  html += ap_ssid;
  html += "</b>, password <b>";
  html += ap_password;
  html += "</b>.</p>"
          "<p>2. After connected, open your browser and visit the IP shown on this page with port 82 (usually <b>192.168.4.1:82</b>).</p>"
          "<p>3. Select your home Wi‑Fi below, enter its password and click \"Save & Connect\".</p>"
          "</div>";

  html += "<form action='/save' method='GET'>";
  html += "<label>Nearby Wi‑Fi networks</label>";
  html += "<select name='ssid'>";
  if (n <= 0) {
    html += "<option value=''>No Wi‑Fi found, refresh this page to rescan</option>";
  } else {
    for (int i = 0; i < n; i++) {
      String s = WiFi.SSID(i);
      if (s.length() == 0) continue;
      html += "<option value='";
      html += s;
      html += "'>";
      html += s;
      html += " (";
      html += WiFi.RSSI(i);
      html += "dBm)";
      html += (WiFi.encryptionType(i) == WIFI_AUTH_OPEN) ? " open" : "";
      html += "</option>";
    }
  }
  html += "</select>";

  html += "<label>Wi‑Fi password</label>";
  html += "<input type='password' name='password' placeholder='Enter the password of selected Wi‑Fi'>";

  html += "<button type='submit'>Save & Connect</button>";
  html += "<div class='small'>Note: After configuration is successful, connect your phone/PC back to your home Wi‑Fi, then visit the IP address shown in the serial monitor to view the camera stream.</div>";
  html += "</form></div></body></html>";

  configServer.send(200, "text/html", html);
}

// Handle saving WiFi credentials and connect to router
void handleSave() {
  if (!configServer.hasArg("ssid") || !configServer.hasArg("password")) {
    configServer.send(400, "text/plain", "Missing parameter ssid or password");
    return;
  }

  sta_ssid = configServer.arg("ssid");
  sta_password = configServer.arg("password");

  String resp = "<!DOCTYPE html><html><head><meta charset='utf-8'>"
                "<title>Connecting to router</title>"
                "<meta name='viewport' content='width=device-width,initial-scale=1'>"
                "<style>body{font-family:-apple-system,BlinkMacSystemFont,Segoe UI,Roboto,Helvetica,Arial,sans-serif;"
                "margin:0;padding:16px;background:#f5f5f5;color:#333;}"
                ".card{max-width:480px;margin:0 auto;background:#fff;border-radius:12px;padding:20px;"
                "box-shadow:0 4px 12px rgba(0,0,0,0.08);}"
                "h1{font-size:20px;margin:0 0 12px;}"
                "p{font-size:14px;line-height:1.6;}"
                "code{background:#eee;padding:2px 4px;border-radius:4px;font-size:13px;}"
                "</style></head><body><div class='card'>"
                "<h1>Connecting to router...</h1>"
                "<p>Selected Wi‑Fi: <b>";
  resp += sta_ssid;
  resp += "</b></p>"
          "<p>The device is trying to connect, please wait about 10 seconds, then:</p>"
          "<p>1. Switch your phone/PC Wi‑Fi to the router you just selected;</p>"
          "<p>2. In your browser, visit the IP address printed in the serial monitor to see the camera stream.</p>";

  configServer.send(200, "text/html", resp + "</div></body></html>");

  // Start connecting to target router
  WiFi.softAPdisconnect(true);
  WiFi.mode(WIFI_STA);
  WiFi.begin(sta_ssid.c_str(), sta_password.c_str());
  WiFi.setSleep(false);

  Serial.println("Connecting to selected WiFi...");
  int retry = 0;
  while (WiFi.status() != WL_CONNECTED && retry < 40) { // about 20 seconds
    delay(500);
    Serial.print(".");
    retry++;
  }
  Serial.println();

  if (WiFi.status() == WL_CONNECTED) {
    wifiConfigured = true;
    Serial.println("WiFi connected via web config");
    Serial.print("IP address: ");
    Serial.println(WiFi.localIP());

    // Start camera server
    if (!cameraStarted) {
      startCameraServer();
      cameraStarted = true;
      Serial.print("Camera Ready! Use 'http://");
      Serial.print(WiFi.localIP());
      Serial.println("' to connect");
    }

    // Configuration server is no longer needed
    configServer.stop();
  } else {
    Serial.println("Failed to connect to selected WiFi");
  }
}

void setup() {
  Serial.begin(115200);
  Serial.setDebugOutput(true);
  delay(500);
  Serial.println();
  Serial.println("OV5640 Camera Stream (Wi-Fi config on port 82)");
  
  // Print camera pin configuration
  print_camera_pins();

  // Release GPIO48 (shared with RGB LED) before camera initialization
  release_gpio48();
  delay(500);  // Longer delay for GPIO stabilization

  // Check camera power status before initialization
  bool camera_powered_before = check_camera_power();
  Serial.println();

  // Enable I2C pull-up resistors (for cameras without hardware pull-ups)
  enable_i2c_pullups();
  delay(200);

  // Initialize camera GPIO pins (PWDN, RESET if available)
  init_camera_gpio();
  delay(500);  // Wait for camera to power up
  
  // Check camera power status after initialization
  bool camera_powered_after = check_camera_power();
  Serial.println();
  
  if (!camera_powered_before && !camera_powered_after) {
    Serial.println("⚠️  WARNING: Camera does not appear to be powered!");
    Serial.println("   Possible causes:");
    Serial.println("   1. Camera module not receiving power from expansion board");
    Serial.println("   2. Camera module needs PWDN pin to be set LOW (power on)");
    Serial.println("      - Expansion board has PWDN hard-wired to HIGH (VCC_3V3 via 10K)");
    Serial.println("      - If camera requires PWDN=LOW, it cannot work with this expansion board");
    Serial.println("   3. Camera module needs RESET pin initialization");
    Serial.println("      - Expansion board has RESET hard-wired to HIGH (VCC_3V3 via 10K)");
    Serial.println("      - Hardware reset is not possible via GPIO");
    Serial.println("   4. Camera module hardware failure");
    Serial.println("   5. Expansion board power supply issue");
    Serial.println();
    Serial.println("   Recommendations:");
    Serial.println("   - Check camera module datasheet: Does it require PWDN=LOW to power on?");
    Serial.println("   - If yes, camera module may be incompatible with this expansion board");
    Serial.println("   - Try a camera module that works with PWDN=HIGH (always powered)");
    Serial.println("   - Check expansion board power LED (if present)");
    Serial.println("   - Verify camera module is firmly seated");
    Serial.println("   - Compare working vs non-working camera module specifications");
    Serial.println();
  }

  // Test I2C communication (optional, some cameras may not respond to scan)
  bool i2c_ok = test_camera_i2c();
  if (!i2c_ok) {
    Serial.println("Camera I2C test failed. Please check:");
    Serial.println("  1. Camera module is properly connected");
    Serial.println("  2. I2C pins (SDA/SCL) are correct");
    Serial.println("  3. Camera module has power");
    Serial.println("  4. I2C pull-up resistors (usually on camera module)");
    Serial.println("Note: Some cameras may not respond to I2C scan but still work.");
    Serial.println("Continuing with camera initialization...");
    delay(500);
  } else {
    delay(300);
  }
  
  // Re-initialize I2C for camera (let camera driver handle it)
  Wire.end();
  delay(100);

  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;
  
  // Use custom pins if defined, otherwise use pins from camera_pins.h
#ifdef CUSTOM_PWDN_GPIO_NUM
  config.pin_pwdn = CUSTOM_PWDN_GPIO_NUM;
#else
  config.pin_pwdn = PWDN_GPIO_NUM;
#endif

#ifdef CUSTOM_RESET_GPIO_NUM
  config.pin_reset = CUSTOM_RESET_GPIO_NUM;
#else
  config.pin_reset = RESET_GPIO_NUM;
#endif
  config.xclk_freq_hz = 20000000;
  config.frame_size = FRAMESIZE_UXGA;
  config.pixel_format = PIXFORMAT_JPEG; // for streaming
  //config.pixel_format = PIXFORMAT_RGB565; // for face detection/recognition
  config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
  config.fb_location = CAMERA_FB_IN_PSRAM;
  config.jpeg_quality = 12;
  config.fb_count = 1;

  // if PSRAM IC present, init with UXGA resolution and higher JPEG quality
  //                      for larger pre-allocated frame buffer.
  if(config.pixel_format == PIXFORMAT_JPEG){
    if(psramFound()){
      config.jpeg_quality = 10;
      config.fb_count = 2;
      config.grab_mode = CAMERA_GRAB_LATEST;
    } else {
      // Limit the frame size when PSRAM is not available
      config.frame_size = FRAMESIZE_SVGA;
      config.fb_location = CAMERA_FB_IN_DRAM;
    }
  } else {
    // Best option for face detection/recognition
    config.frame_size = FRAMESIZE_240X240;
#if CONFIG_IDF_TARGET_ESP32S3
    config.fb_count = 2;
#endif
  }

  // Camera init with retry
  Serial.println("Initializing camera...");
  Serial.printf("  XCLK: %d MHz\n", config.xclk_freq_hz / 1000000);
  Serial.printf("  Frame size: %d\n", config.frame_size);
  Serial.printf("  FB count: %d\n", config.fb_count);
  delay(200);
  
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed with error 0x%x\n", err);
    Serial.println("Trying again with lower resolution and slower clock...");
    delay(1000);
    
    // Retry with lower settings
    config.frame_size = FRAMESIZE_VGA;
    config.xclk_freq_hz = 16000000;  // Lower frequency for OV5640
    config.fb_count = 1;  // Single buffer
    if (!psramFound()) {
      config.fb_location = CAMERA_FB_IN_DRAM;
  }
    config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
    
    Serial.printf("  Retry - XCLK: %d MHz, Frame size: VGA\n", config.xclk_freq_hz / 1000000);
    delay(500);
    err = esp_camera_init(&config);
    
    if (err != ESP_OK) {
      Serial.printf("Camera init failed again with error 0x%x\n", err);
      Serial.println("\n=== Troubleshooting Guide ===");
      Serial.println("Camera initialization failed. Please check:");
      Serial.println();
      Serial.println("1. HARDWARE CONNECTION:");
      Serial.println("   - Ensure OV5640 camera module is properly connected to the expansion board");
      Serial.println("   - Check all pins are making good contact (no loose connections)");
      Serial.println("   - Verify camera module is compatible with W11");
      Serial.println();
      Serial.println("2. POWER SUPPLY:");
      Serial.println("   - Camera module needs stable 3.3V power");
      Serial.println("   - Check if expansion board is providing power to camera");
      Serial.println("   - Try disconnecting and reconnecting the camera module");
      Serial.println();
      Serial.println("3. CAMERA MODEL:");
      Serial.println("   - This program is designed for OV5640 camera");
      Serial.println("   - If you have OV2640, use the OV2640 program instead");
      Serial.println("   - Check camera module label to confirm model");
      Serial.println();
      Serial.println("4. I2C COMMUNICATION:");
      Serial.printf("   - I2C pins: SDA=GPIO%d, SCL=GPIO%d\n", SIOD_GPIO_NUM, SIOC_GPIO_NUM);
      if (!i2c_ok) {
        Serial.println("   - I2C scan found NO devices (camera not responding)");
      }
      Serial.println("   - Check I2C pull-up resistors (usually on camera module)");
      Serial.println();
      Serial.println("5. GPIO CONFLICTS:");
      Serial.println("   - Ensure no other peripherals are using camera pins");
      Serial.println("   - GPIO48 (Y9) should be released (RGB LED disabled)");
      Serial.println();
      Serial.println("6. TESTING:");
      Serial.println("   - Try running the reference program: 03_DVP_Camare_SaveSD");
      Serial.println("   - If that works, compare the configurations");
      Serial.println("   - Check serial monitor for any other error messages");
      Serial.println();
      Serial.println("Program will continue but camera features will be disabled.");
      Serial.println("WiFi configuration page is still available.");
      Serial.println("================================\n");
      return;  // Exit setup if camera init fails
    }
  }
  
  Serial.println("Camera initialized successfully!");

  sensor_t * s = esp_camera_sensor_get();
  
  if (s != NULL) {
  // Print sensor information for debugging
  Serial.printf("Camera sensor PID: 0x%02X\n", s->id.PID);
  
  // Configure according to different sensors
  if (s->id.PID == OV3660_PID) {
      Serial.println("OV3660 sensor detected");
    s->set_vflip(s, 1); // flip it back
    s->set_brightness(s, 1); // up the brightness just a bit
    s->set_saturation(s, -2); // lower the saturation
  } else if (s->id.PID == OV5640_PID) {
      Serial.println("OV5640 sensor detected and configured");
    // Special configuration for OV5640 (if needed)
    // These parameters can be adjusted according to actual effects
    // s->set_vflip(s, 1); // Vertical flip if needed
    // s->set_brightness(s, 0); // Brightness adjustment
    // s->set_saturation(s, 0); // Saturation adjustment
    } else if (s->id.PID == OV2640_PID) {
      Serial.println("Warning: OV2640 detected. This program is optimized for OV5640.");
      Serial.println("Please use the OV2640 program for best results.");
    } else {
      Serial.printf("Warning: Unknown sensor PID 0x%02X\n", s->id.PID);
      Serial.println("Expected OV5640 (0x5640) or OV3660 (0x3660)");
    }
  } else {
    Serial.println("Warning: Could not get camera sensor");
  }
  
  // drop down frame size for higher initial frame rate
  if(config.pixel_format == PIXFORMAT_JPEG){
    s->set_framesize(s, FRAMESIZE_QVGA);
  }

// Setup LED FLash if LED pin is defined in camera_pins.h
#if defined(LED_GPIO_NUM)
  setupLedFlash(LED_GPIO_NUM);
#endif

  // Enter WiFi configuration mode: start AP and launch a simple config web page
  WiFi.mode(WIFI_AP_STA);
  bool ap_ok = WiFi.softAP(ap_ssid, ap_password);
  delay(100);  // wait a bit for AP to be ready

  IPAddress apIP = WiFi.softAPIP();
  Serial.println("===== WiFi CONFIG MODE =====");
  if (ap_ok) {
    Serial.print("Connect to AP SSID: ");
    Serial.print(ap_ssid);
    Serial.print("  password: ");
    Serial.println(ap_password);
    Serial.print("Open browser and visit: http://");
    Serial.print(apIP);
    Serial.println(":82  to configure WiFi");
  } else {
    Serial.println("Failed to start AP!");
  }

  configServer.on("/", handleRoot);
  configServer.on("/save", handleSave);
  configServer.begin();
}

void loop() {
  if (!wifiConfigured) {
    configServer.handleClient();
    delay(10);
  } else {
    delay(1000);
  }
}