#include <Arduino.h>
#include <esp_camera.h>
#include <SSCMA_Micro_Core.h>

SET_LOOP_TASK_STACK_SIZE(40 * 1024);

SSCMAMicroCore model;
SSCMAMicroCore::VideoCapture camera;

uint32_t frameCount = 0;
bool systemReady = false;

static const float kMinScore = 0.50f;

const char* gestureName(int target) {
  switch (target) {
    case 0: return "paper";
    case 1: return "rock";
    case 2: return "scissors";
    default: return "unknown";
  }
}

bool probeCameraFrame() {
  camera_fb_t* fb = esp_camera_fb_get();
  if (fb == nullptr || fb->buf == nullptr) {
    Serial.println("CAMERA_PROBE_FAIL: esp_camera_fb_get returned null");
    return false;
  }
  Serial.printf(
    "CAMERA_PROBE_OK: %ux%u, format=%d, bytes=%u\n",
    fb->width,
    fb->height,
    fb->format,
    fb->len
  );
  esp_camera_fb_return(fb);
  return true;
}

void setup() {
  Serial.begin(115200);
  delay(2000);

  Serial.println();
  Serial.println("WILDCARD_WEEK_GESTURE_AI_START");
  Serial.println("Board: XIAO ESP32S3 Sense");
  Serial.println("Labels: 0=paper, 1=rock, 2=scissors");
  Serial.println("Serial: 115200 baud");
  Serial.printf("PSRAM found: %s\n", psramFound() ? "yes" : "NO - enable OPI PSRAM in Tools menu");

  camera_config_t camConfig = SSCMAMicroCore::VideoCapture::DefaultCameraConfigXIAOS3;
  camConfig.fb_count = 2;
  camConfig.grab_mode = CAMERA_GRAB_LATEST;
  if (!psramFound()) {
    camConfig.fb_location = CAMERA_FB_IN_DRAM;
    camConfig.frame_size = FRAMESIZE_QQVGA;
    Serial.println("WARN: using DRAM fallback (240x240 needs PSRAM)");
  }

  Serial.println("Initializing camera...");
  auto camRet = camera.begin(camConfig);
  if (!camRet.success) {
    Serial.print("CAMERA_INIT_FAIL: ");
    Serial.println(camRet.message.c_str());
    Serial.println("Check: Sense camera board attached, ribbon cable seated, PSRAM=OPI PSRAM");
    return;
  }
  delay(500);

  if (!probeCameraFrame()) {
    Serial.println("Camera init OK but capture failed - reseat ribbon cable and reset");
    return;
  }

  Serial.println("Initializing SSCMA Micro Core...");
  auto modelRet = model.begin(SSCMAMicroCore::Config::DefaultConfig);
  if (!modelRet.success) {
    Serial.print("MODEL_INIT_FAIL: ");
    Serial.println(modelRet.message.c_str());
    Serial.println("Deploy gesture model in SenseCraft first, do not erase flash on upload");
    return;
  }

  systemReady = true;
  Serial.println("INIT_DONE");
}

void loop() {
  if (!systemReady) {
    delay(2000);
    return;
  }

  auto frame = camera.getManagedFrame();
  if (frame == nullptr) {
    Serial.println("CAMERA_FRAME_NULL");
    delay(500);
    return;
  }

  auto invokeRet = model.invoke(frame);
  if (!invokeRet.success) {
    Serial.print("INVOKE_FAIL: ");
    Serial.println(invokeRet.message.c_str());
    delay(500);
    return;
  }
  frameCount++;

  int bestTarget = -1;
  float bestScore = 0.0f;

  for (const auto& box : model.getBoxes()) {
    if (box.score > bestScore) {
      bestScore = box.score;
      bestTarget = box.target;
    }
  }

  for (const auto& cls : model.getClasses()) {
    if (cls.score > bestScore) {
      bestScore = cls.score;
      bestTarget = cls.target;
    }
  }

  if (bestTarget >= 0 && bestScore >= kMinScore) {
    Serial.printf(
      "GESTURE=%s,target=%d,score=%.3f,frame=%lu\n",
      gestureName(bestTarget),
      bestTarget,
      bestScore,
      frameCount
    );
  } else {
    Serial.printf("GESTURE=none,frame=%lu\n", frameCount);
  }

  auto perf = model.getPerf();
  Serial.printf(
    "PERF,preprocess=%dms,inference=%dms,postprocess=%dms\n",
    perf.preprocess,
    perf.inference,
    perf.postprocess
  );

  delay(300);
}