// MUST BE FIRST:
#define CAMERA_MODEL_XIAO_ESP32S3
#include "camera_pins.h"

// ===== OLED =====
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "esp_camera.h"

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

static float gray_buffer[SCREEN_WIDTH * SCREEN_HEIGHT];

// ===== Dither + Resize =====
void showDitheredPreview(const camera_fb_t* fb) {

  int sw = fb->width;
  int sh = fb->height;
  const uint8_t* src = fb->buf;

  float sx = (float)sw / SCREEN_WIDTH;
  float sy = (float)sh / SCREEN_HEIGHT;

  // Downsample to OLED resolution
  for (int y = 0; y < SCREEN_HEIGHT; y++) {
    int y0 = (int)(y * sy);
    int y1 = max(y0 + 1, (int)((y + 1) * sy));

    for (int x = 0; x < SCREEN_WIDTH; x++) {
      int x0 = (int)(x * sx);
      int x1 = max(x0 + 1, (int)((x + 1) * sx));

      uint32_t sum = 0;
      int count = 0;

      for (int yy = y0; yy < y1; yy++) {
        int row = yy * sw;
        for (int xx = x0; xx < x1; xx++) {
          sum += src[row + xx];
          count++;
        }
      }
      gray_buffer[y * SCREEN_WIDTH + x] = sum / (float)count;
    }
  }

  // Normalize
  float minv = 255, maxv = 0;
  for (int i = 0; i < SCREEN_WIDTH * SCREEN_HEIGHT; i++) {
    float v = gray_buffer[i];
    minv = min(minv, v);
    maxv = max(maxv, v);
  }

  float range = max(1.0f, maxv - minv);

  for (int i = 0; i < SCREEN_WIDTH * SCREEN_HEIGHT; i++)
    gray_buffer[i] = (gray_buffer[i] - minv) * (255.0f / range);

  // Floyd–Steinberg dithering
  for (int y = 0; y < SCREEN_HEIGHT; y++) {
    for (int x = 0; x < SCREEN_WIDTH; x++) {

      int i = y * SCREEN_WIDTH + x;
      float oldp = gray_buffer[i];
      float newp = oldp > 128 ? 255 : 0;
      float err = oldp - newp;

      gray_buffer[i] = newp;

      if (x + 1 < SCREEN_WIDTH) gray_buffer[i + 1] += err * 7 / 16.f;

      if (y + 1 < SCREEN_HEIGHT) {
        if (x > 0) gray_buffer[i + SCREEN_WIDTH - 1] += err * 3 / 16.f;
        gray_buffer[i + SCREEN_WIDTH] += err * 5 / 16.f;
        if (x + 1 < SCREEN_WIDTH)
          gray_buffer[i + SCREEN_WIDTH + 1] += err * 1 / 16.f;
      }
    }
  }

  // Draw result
  display.clearDisplay();
  for (int y = 0; y < SCREEN_HEIGHT; y++)
    for (int x = 0; x < SCREEN_WIDTH; x++)
      if (gray_buffer[y * SCREEN_WIDTH + x] > 128)
        display.drawPixel(x, y, SSD1306_WHITE);

  display.display();
}


// ===== Camera Init =====
bool initCamera() {

  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;

  config.pin_pwdn  = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;

  config.xclk_freq_hz = 20000000;
  config.frame_size = FRAMESIZE_QQVGA; // 160x120
  config.pixel_format = PIXFORMAT_GRAYSCALE;

  config.fb_location = CAMERA_FB_IN_PSRAM;
  config.fb_count = 1;
  config.grab_mode = CAMERA_GRAB_LATEST;

  esp_err_t err = esp_camera_init(&config);

  if (err != ESP_OK) {
    Serial.printf("Camera init failed: 0x%x\n", err);
    return false;
  }

  return true;
}


// ===== Setup =====
void setup() {
  Serial.begin(115200);

  Wire.begin();
  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("Init camera...");
  display.display();

  if (!initCamera()) {
    display.clearDisplay();
    display.setCursor(0, 0);
    display.println("CAMERA INIT FAILED");
    display.display();
    while (1);
  }

  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("Camera OK!");
  display.display();
  delay(500);
}


// ===== Loop: continuous preview =====
void loop() {
  camera_fb_t* fb = esp_camera_fb_get();
  if (!fb) return;

  showDitheredPreview(fb);
  esp_camera_fb_return(fb);
}