// =======================================
//   XIAO ESP32S3 CAMERA + GAME OVERLAY
//   • Receives Game NetPackets
//   • Captures Camera Feed
//   • AR Overlay: Draws square/circle on top
//   • Floyd–Steinberg → 64×32 → OLED
//   • No sending, no ESPNOW errors
// =======================================

#define CAMERA_MODEL_XIAO_ESP32S3
#include "camera_pins.h"

#include <esp_now.h>
#include <WiFi.h>
#include <esp_wifi.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "esp_camera.h"

#define OLED_W 128
#define OLED_H 64
#define OLED_ADDR 0x3C
Adafruit_SSD1306 display(OLED_W, OLED_H, &Wire, -1);

// --------------------------
// GAME PACKET (from game boards)
// --------------------------
struct PlayerState {
  int x;
  int y;
  int vx;
  int vy;
  bool onGround;
};

struct NetPacket {
  PlayerState st;
  bool ready;
};

// Store last received states
PlayerState p1, p2;
bool gotP1 = false, gotP2 = false;

// --------------------------
// Identify camera board
// --------------------------
uint8_t camA_MAC[6] = {0xB8,0xF8,0x62,0xF9,0xE2,0xC0};
uint8_t camB_MAC[6] = {0xB8,0xF8,0x62,0xF9,0xD6,0x38};
uint8_t camID = 0;

// --------------------------
// Camera downsample buffers
// --------------------------
float fb_gray[64*32];
uint8_t outFrame[64*32];

// =======================================
// 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;
  config.pixel_format = PIXFORMAT_GRAYSCALE;

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

  return (esp_camera_init(&config) == ESP_OK);
}

// =======================================
// AUTO CHOOSE camID from MAC
// =======================================
void determineCamID() {
  uint8_t mac[6];
  esp_wifi_get_mac(WIFI_IF_STA, mac);

  if (memcmp(mac, camA_MAC, 6)==0) camID = 1;
  else if (memcmp(mac, camB_MAC, 6)==0) camID = 2;
  else camID = 99;

  display.clearDisplay();
  display.setCursor(0,0);
  display.println("Camera Ready");
  display.print("ID: ");
  display.println(camID);
  display.display();
  delay(600);
}

// =======================================
// ESPNOW RECEIVE GAME PACKETS
// =======================================
void onDataRecv(const esp_now_recv_info_t *info,
                const uint8_t *data, int len)
{
  if (len != sizeof(NetPacket)) return;

  NetPacket pkt;
  memcpy(&pkt, data, sizeof(pkt));

  // Identify P1 or P2 based on MAC
  if (memcmp(info->src_addr, camA_MAC, 6)==0 ||
      memcmp(info->src_addr, camB_MAC, 6)==0) {
    return; // ignore camera boards
  }

  // Use MAC ordering to distinguish players
  if (info->src_addr[5] == 0xAC) {  // P1 MAC LSB
    p1 = pkt.st;
    gotP1 = true;
  } else if (info->src_addr[5] == 0x9C) { // P2 MAC LSB
    p2 = pkt.st;
    gotP2 = true;
  }
}

// =======================================
// DITHER + ROTATE + DOWN
// =======================================
void makeFrame(const camera_fb_t *fb) {
  int sw = fb->width;
  int sh = fb->height;
  const uint8_t* src = fb->buf;

  float sx = (float)sw / 64.0f;
  float sy = (float)sh / 32.0f;

  for(int y=0;y<32;y++){
    int syy = y*sy;
    for(int x=0;x<64;x++){
      int sxx = x*sx;
      fb_gray[y*64+x] = src[syy*sw + sxx];
    }
  }

  // Normalize
  float mn=255,mx=0;
  for(int i=0;i<2048;i++){
    float v=fb_gray[i];
    if(v<mn) mn=v;
    if(v>mx) mx=v;
  }
  float r = max(1.0f,mx-mn);
  for(int i=0;i<2048;i++)
    fb_gray[i]=(fb_gray[i]-mn)*(255.0f/r);

  // Floyd-Steinberg
  for(int y=0;y<32;y++){
    for(int x=0;x<64;x++){
      int idx=y*64+x;
      float oldp=fb_gray[idx];
      float newp=oldp>128?255:0;
      float err=oldp-newp;
      fb_gray[idx]=newp;

      if(x+1<64) fb_gray[idx+1]+=err*(7/16.f);
      if(y+1<32){
        if(x>0) fb_gray[idx+64-1]+=err*(3/16.f);
        fb_gray[idx+64]+=err*(5/16.f);
        if(x+1<64) fb_gray[idx+64+1]+=err*(1/16.f);
      }
    }
  }

  // Rotate 180°
  for (int y=0;y<32;y++)
    for(int x=0;x<64;x++){
      int src_i=y*64+x;
      int dst_i=(31-y)*64+(63-x);
      outFrame[dst_i]=(fb_gray[src_i]>128)?255:0;
    }
}

// =======================================
// ESPNOW INIT (RECEIVE ONLY)
// =======================================
void initESPNOW() {
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESPNOW FAIL");
    while(1);
  }
  esp_now_register_recv_cb(onDataRecv);
}

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

  Wire.begin();
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
  display.clearDisplay();
  display.setCursor(0,0);
  display.println("Booting...");
  display.display();

  initESPNOW();

  if (!initCamera()) {
    display.clearDisplay();
    display.println("CAM FAIL");
    display.display();
    while(1);
  }

  determineCamID();
}

// =======================================
// LOOP
// =======================================
void loop() {
  camera_fb_t* fb = esp_camera_fb_get();
  if (!fb) return;

  makeFrame(fb);
  esp_camera_fb_return(fb);

  // Draw camera
  display.clearDisplay();
  for(int y=0;y<32;y++)
    for(int x=0;x<64;x++)
      if(outFrame[y*64+x]>128)
        display.fillRect(x*2, y*2, 2,2, SSD1306_WHITE);

  // ----------------------------
  // OVERLAY GAME CHARACTERS
  // ----------------------------
  if (gotP1) {
    // square
    display.fillRect(p1.x, p1.y, 8, 8, SSD1306_WHITE);
  }
  if (gotP2) {
    // circle
    display.fillCircle(p2.x+4, p2.y+4, 4, SSD1306_WHITE);
  }

  display.display();
}