/* Implementation of the cam_wrapper shim around esp32-camera. See cam_wrapper.h. */

#include "cam_wrapper.h"

#include <string.h>

#include "esp_log.h"
#include "esp_timer.h"

static void cam_wrapper_measure_fps(cam_wrapper_sensor_t sensor);

static const char *TAG = "cam_wrapper";

static sensor_t *s_sensor;
static char s_sensor_name[16] = "unknown";

static void fill_config(const cam_wrapper_config_t *in, camera_config_t *out)
{
    memset(out, 0, sizeof(*out));

    out->pin_pwdn = in->pin_pwdn;
    out->pin_reset = in->pin_reset;
    out->pin_xclk = in->pin_xclk;
    out->pin_sccb_sda = in->pin_sccb_sda;
    out->pin_sccb_scl = in->pin_sccb_scl;

    out->pin_d0 = in->pin_d[0];
    out->pin_d1 = in->pin_d[1];
    out->pin_d2 = in->pin_d[2];
    out->pin_d3 = in->pin_d[3];
    out->pin_d4 = in->pin_d[4];
    out->pin_d5 = in->pin_d[5];
    out->pin_d6 = in->pin_d[6];
    out->pin_d7 = in->pin_d[7];

    out->pin_vsync = in->pin_vsync;
    out->pin_href = in->pin_href;
    out->pin_pclk = in->pin_pclk;

    out->xclk_freq_hz = in->xclk_freq_hz;
    out->ledc_timer = (ledc_timer_t)in->ledc_timer;
    out->ledc_channel = (ledc_channel_t)in->ledc_channel;

    out->pixel_format = (pixformat_t)in->pixel_format;
    out->frame_size = (framesize_t)in->frame_size;
    out->jpeg_quality = in->jpeg_quality;
    out->fb_count = in->fb_count;
    out->fb_location = (camera_fb_location_t)in->fb_location;
    out->grab_mode = (camera_grab_mode_t)in->grab_mode;
    out->sccb_i2c_port = in->sccb_i2c_port;
}

esp_err_t cam_wrapper_init(const cam_wrapper_config_t *config, cam_wrapper_sensor_t sensor)
{
    camera_config_t cfg;

    (void)sensor;

    if (config == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    fill_config(config, &cfg);

    ESP_LOGI(TAG, "config: pwdn=%d reset=%d xclk=%d sda=%d scl=%d "
                  "d=[%d %d %d %d %d %d %d %d] vsync=%d href=%d pclk=%d "
                  "fmt=%d size=%d q=%d fb=%u loc=%d grab=%d i2c=%d vsync_inv=%d",
             cfg.pin_pwdn, cfg.pin_reset, cfg.pin_xclk,
             cfg.pin_sccb_sda, cfg.pin_sccb_scl,
             cfg.pin_d0, cfg.pin_d1, cfg.pin_d2, cfg.pin_d3,
             cfg.pin_d4, cfg.pin_d5, cfg.pin_d6, cfg.pin_d7,
             cfg.pin_vsync, cfg.pin_href, cfg.pin_pclk,
             (int)cfg.pixel_format, (int)cfg.frame_size, cfg.jpeg_quality,
             (unsigned)cfg.fb_count, (int)cfg.fb_location, (int)cfg.grab_mode,
             cfg.sccb_i2c_port, config->vsync_invert);

    /* Must be set before init: cam_init copies it into the driver object, and
     * ll_cam_set_pin applies it to the GPIO matrix for the VSYNC input. */
    cam_set_vsync_invert(config->vsync_invert != 0);

    esp_err_t err = esp_camera_init(&cfg);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "esp_camera_init failed: 0x%x", err);
        return err;
    }

    /* The driver probes the SCCB bus and binds a driver by the sensor's
     * PID/ID, so name the sensor from the matched entry of its table rather
     * than reporting something the driver does not actually know. */
    s_sensor = esp_camera_sensor_get();

    if (s_sensor != NULL) {
        sensor_id_t id = s_sensor->id;
        const camera_sensor_info_t *info = esp_camera_sensor_get_info(&id);

        if (info != NULL && info->name != NULL) {
            strncpy(s_sensor_name, info->name, sizeof(s_sensor_name) - 1);
            s_sensor_name[sizeof(s_sensor_name) - 1] = '\0';
        }

        ESP_LOGI(TAG, "sensor detected: %s (pid=0x%04x, ver=0x%02x)",
                 s_sensor_name, id.PID, id.VER);
    } else {
        ESP_LOGW(TAG, "camera initialized but no sensor handle is available");
    }

    /* Measure what the sensor actually produces, here, with no WiFi and no HTTP
     * in the picture. Everything downstream (socket writes, TCP, the browser) is
     * downstream of this number, so when the stream is slow this is what says
     * whether the problem is capture or transport. */
    cam_wrapper_fb_t probe;
    memset(&probe, 0, sizeof(probe));

    if (cam_wrapper_fb_get(&probe, sensor) == ESP_OK) {
        ESP_LOGI(TAG, "first frame: %ux%u, %u bytes, format %d",
                 (unsigned)probe.width, (unsigned)probe.height,
                 (unsigned)probe.len, probe.format);
        cam_wrapper_fb_release(&probe);

        cam_wrapper_measure_fps(sensor);
    } else {
        ESP_LOGW(TAG, "camera initialized but returned no frame");
    }

    return ESP_OK;
}

/* Grabs PROBE_FRAMES frames back to back and logs the achieved rate.
 *
 * This runs on the capture path with nothing else contending for the sensor, so
 * it is the ceiling the stream can ever reach. Comparing it against the rate the
 * stream actually achieves localises the bottleneck: equal means the transport is
 * keeping up and the sensor is the limit, much lower means something between the
 * two is dropping frames. */
#define PROBE_FRAMES 20

static void cam_wrapper_measure_fps(cam_wrapper_sensor_t sensor)
{
    uint32_t total = 0;
    int64_t start = esp_timer_get_time();
    int taken = 0;

    for (int i = 0; i < PROBE_FRAMES; i++) {
        cam_wrapper_fb_t fb;
        memset(&fb, 0, sizeof(fb));

        if (cam_wrapper_fb_get(&fb, sensor) != ESP_OK) {
            ESP_LOGW(TAG, "fps probe: no frame at index %d", i);
            continue;
        }

        total += (uint32_t)fb.len;
        taken++;
        cam_wrapper_fb_release(&fb);
    }

    int64_t elapsed_us = esp_timer_get_time() - start;

    if (taken == 0 || elapsed_us <= 0) {
        ESP_LOGW(TAG, "fps probe: got no frames");
        return;
    }

    double fps = (double)taken * 1000000.0 / (double)elapsed_us;

    ESP_LOGI(TAG, "fps probe: %d frames in %lld ms -> %.1f fps, avg %u bytes "
                  "(sensor ceiling, no network involved)",
             taken, (long long)(elapsed_us / 1000), fps,
             (unsigned)(total / (uint32_t)taken));
}

esp_err_t cam_wrapper_fb_get(cam_wrapper_fb_t *out, cam_wrapper_sensor_t sensor)
{
    (void)sensor;

    if (out == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    memset(out, 0, sizeof(*out));

    camera_fb_t *fb = esp_camera_fb_get();
    if (fb == NULL) {
        return ESP_FAIL;
    }

    out->data = fb->buf;
    out->len = fb->len;
    out->width = fb->width;
    out->height = fb->height;
    out->format = (int)fb->format;
    out->handle = fb;

    return ESP_OK;
}

void cam_wrapper_fb_release(cam_wrapper_fb_t *fb)
{
    if (fb == NULL || fb->handle == NULL) {
        return;
    }

    esp_camera_fb_return((camera_fb_t *)fb->handle);

    fb->data = NULL;
    fb->len = 0;
    fb->handle = NULL;
}

esp_err_t cam_wrapper_reconfigure(const cam_wrapper_config_t *config)
{
    camera_config_t cfg;

    if (config == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    fill_config(config, &cfg);

    return esp_camera_reconfigure(&cfg);
}

esp_err_t cam_wrapper_deinit(void)
{
    esp_err_t err = esp_camera_deinit();

    s_sensor = NULL;
    strcpy(s_sensor_name, "unknown");

    return err;
}

const char *cam_wrapper_sensor_name(void)
{
    return s_sensor_name;
}