/*
 * JeLamp_S3_Master.ino
 * Board 1: XIAO ESP32-S3 Sense — SenseCraft AI inference + I2C master
 *
 * Wiring (I2C to ESP32-C3 slave):
 *   S3 SDA(D4) GPIO5  ->  C3 SDA(D4) GPIO6
 *   S3 SCL(D5) GPIO6  ->  C3 SCL(D5) GPIO7
 *   GND               ->  GND
 *
 * Requires: SSCMA_Micro_Core library (SenseCraft AI)
 * Companion: JeLamp_C3_Slave.ino or JeLamp_C3_Slave_Web.ino on ESP32-C3
 */

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

SET_LOOP_TASK_STACK_SIZE(40 * 1024);

SSCMAMicroCore instance;
SSCMAMicroCore::VideoCapture capture;

#define C3_SLAVE_ADDR 0x08
#define I2C_SDA       5    // ESP32-S3 D4
#define I2C_SCL       6    // ESP32-S3 D5

// Packet format: [type 1 byte][data...]
// type: 0x01=Box, 0x02=Class, 0x03=Point, 0xFF=Perf

void sendBox(const SSCMAMicroCore::Box& box) {
    uint8_t buf[12];
    buf[0] = 0x01;

    int16_t x = (int16_t)(box.x * 100);
    int16_t y = (int16_t)(box.y * 100);
    int16_t w = (int16_t)(box.w * 100);
    int16_t h = (int16_t)(box.h * 100);
    int16_t score = (int16_t)(box.score * 100);
    int8_t  target = (int8_t)box.target;

    buf[1] = (x >> 8) & 0xFF;
    buf[2] = x & 0xFF;
    buf[3] = (y >> 8) & 0xFF;
    buf[4] = y & 0xFF;
    buf[5] = (w >> 8) & 0xFF;
    buf[6] = w & 0xFF;
    buf[7] = (h >> 8) & 0xFF;
    buf[8] = h & 0xFF;
    buf[9] = (score >> 8) & 0xFF;
    buf[10] = score & 0xFF;
    buf[11] = target;

    Wire.beginTransmission(C3_SLAVE_ADDR);
    Wire.write(buf, 12);
    Wire.endTransmission();
}

void sendClass(const SSCMAMicroCore::Class& cls) {
    uint8_t buf[4];
    buf[0] = 0x02;

    int16_t score = (int16_t)(cls.score * 100);
    int8_t  target = (int8_t)cls.target;

    buf[1] = (score >> 8) & 0xFF;
    buf[2] = score & 0xFF;
    buf[3] = target;

    Wire.beginTransmission(C3_SLAVE_ADDR);
    Wire.write(buf, 4);
    Wire.endTransmission();
}

void sendPoint(const SSCMAMicroCore::Point& point) {
    uint8_t buf[10];
    buf[0] = 0x03;

    int16_t x = (int16_t)(point.x * 100);
    int16_t y = (int16_t)(point.y * 100);
    int16_t z = (int16_t)(point.z * 100);
    int16_t score = (int16_t)(point.score * 100);
    int8_t  target = (int8_t)point.target;

    buf[1] = (x >> 8) & 0xFF;
    buf[2] = x & 0xFF;
    buf[3] = (y >> 8) & 0xFF;
    buf[4] = y & 0xFF;
    buf[5] = (z >> 8) & 0xFF;
    buf[6] = z & 0xFF;
    buf[7] = (score >> 8) & 0xFF;
    buf[8] = score & 0xFF;
    buf[9] = target;

    Wire.beginTransmission(C3_SLAVE_ADDR);
    Wire.write(buf, 10);
    Wire.endTransmission();
}

void sendPerf(int32_t preprocess, int32_t inference, int32_t postprocess) {
    uint8_t buf[7];
    buf[0] = 0xFF;

    buf[1] = (preprocess >> 8) & 0xFF;
    buf[2] = preprocess & 0xFF;
    buf[3] = (inference >> 8) & 0xFF;
    buf[4] = inference & 0xFF;
    buf[5] = (postprocess >> 8) & 0xFF;
    buf[6] = postprocess & 0xFF;

    Wire.beginTransmission(C3_SLAVE_ADDR);
    Wire.write(buf, 7);
    Wire.endTransmission();
}

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

    Wire.begin(I2C_SDA, I2C_SCL);
    Wire.setClock(100000);

    MA_RETURN_IF_UNEXPECTED(capture.begin(SSCMAMicroCore::VideoCapture::DefaultCameraConfigXIAOS3));
    MA_RETURN_IF_UNEXPECTED(instance.begin(SSCMAMicroCore::Config::DefaultConfig));

    Serial.println("S3 Master Init done");
}

void loop() {
    auto frame = capture.getManagedFrame();
    MA_RETURN_IF_UNEXPECTED(instance.invoke(frame));

    for (const auto& box : instance.getBoxes()) {
        Serial.printf("Box: x=%f, y=%f, w=%f, h=%f, score=%f, target=%d\n",
                      box.x, box.y, box.w, box.h, box.score, box.target);
        sendBox(box);
        delay(5);
    }

    for (const auto& cls : instance.getClasses()) {
        Serial.printf("Class: target=%d, score=%f\n", cls.target, cls.score);
        sendClass(cls);
        delay(5);
    }

    for (const auto& point : instance.getPoints()) {
        Serial.printf("Point: x=%f, y=%f, z=%f, score=%f, target=%d\n",
                      point.x, point.y, point.z, point.score, point.target);
        sendPoint(point);
        delay(5);
    }

    for (const auto& kp : instance.getKeypoints()) {
        Serial.printf("Keypoints box: x=%f, y=%f, w=%f, h=%f, score=%f, target=%d\n",
                      kp.box.x, kp.box.y, kp.box.w, kp.box.h, kp.box.score, kp.box.target);
        sendBox(kp.box);
        delay(5);
        for (const auto& point : kp.points) {
            sendPoint(point);
            delay(5);
        }
    }

    auto perf = instance.getPerf();
    Serial.printf("Perf: preprocess=%dms, inference=%dms, postprocess=%dms\n",
                  perf.preprocess, perf.inference, perf.postprocess);
    sendPerf(perf.preprocess, perf.inference, perf.postprocess);

    delay(100);
}