#include <stdio.h>

#define TAG "CAM"
#include "esp_camera.h"
#define PWDN_GPIO_NUM -1
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 15
#define SIOD_GPIO_NUM 4
#define SIOC_GPIO_NUM 5

#define Y2_GPIO_NUM 11
#define Y3_GPIO_NUM 9
#define Y4_GPIO_NUM 8
#define Y5_GPIO_NUM 10
#define Y6_GPIO_NUM 12
#define Y7_GPIO_NUM 18
#define Y8_GPIO_NUM 17
#define Y9_GPIO_NUM 16

#define VSYNC_GPIO_NUM 6
#define HREF_GPIO_NUM 7
#define PCLK_GPIO_NUM 13

#include "esp_event.h"
#include "esp_system.h"
#include "esp_event.h"
#include "esp_wifi.h"
#include "esp_log.h"

#include "nvs_flash.h"
#include "string.h"

static camera_config_t camera_config = {
    .pin_pwdn = PWDN_GPIO_NUM,
    .pin_reset = RESET_GPIO_NUM,
    .pin_xclk = XCLK_GPIO_NUM,
    .pin_sccb_sda = SIOD_GPIO_NUM,
    .pin_sccb_scl = SIOC_GPIO_NUM,

    .pin_d7 = Y9_GPIO_NUM,
    .pin_d6 = Y8_GPIO_NUM,
    .pin_d5 = Y7_GPIO_NUM,
    .pin_d4 = Y6_GPIO_NUM,
    .pin_d3 = Y5_GPIO_NUM,
    .pin_d2 = Y4_GPIO_NUM,
    .pin_d1 = Y3_GPIO_NUM,
    .pin_d0 = Y2_GPIO_NUM,
    .pin_vsync = VSYNC_GPIO_NUM,
    .pin_href = HREF_GPIO_NUM,
    .pin_pclk = PCLK_GPIO_NUM,

    .xclk_freq_hz = 20000000, // EXPERIMENTAL: Set to 16MHz on ESP32-S2 or ESP32-S3 to enable EDMA mode
    .ledc_timer = LEDC_TIMER_0,
    .ledc_channel = LEDC_CHANNEL_0,

    .pixel_format = PIXFORMAT_JPEG, // YUV422,GRAYSCALE,RGB565,JPEG
    //.frame_size = FRAMESIZE_UXGA,   // QQVGA-UXGA, For ESP32, do not use sizes above QVGA when not JPEG. The performance of the ESP32-S series has improved a lot, but JPEG mode always gives better frame rates.+
    .frame_size = FRAMESIZE_128X128,   // QQVGA-UXGA, For ESP32, do not use sizes above QVGA when not JPEG. The performance of the ESP32-S series has improved a lot, but JPEG mode always gives better frame rates.

    .jpeg_quality = 12, // 0-63, for OV series camera sensors, lower number means higher quality
    .fb_count = 1,      // When jpeg mode is used, if fb_count more than one, the driver will work in continuous mode.
    //.fb_location = CAMERA_FB_IN_PSRAM,
    .fb_location = CAMERA_FB_IN_PSRAM,
    .grab_mode = CAMERA_GRAB_LATEST // CAMERA_GRAB_LATEST. Sets when buffers should be filled
};

esp_err_t esp_wifi_80211_tx(wifi_interface_t ifx, const void *buffer, int len, bool en_sys_seq);

esp_err_t camera_init()
{
    // initialize the camera
    esp_err_t err = esp_camera_init(&camera_config);
    if (err != ESP_OK)
    {
        ESP_LOGE(TAG, "Camera Init Failed");
        return err;
    }

    return ESP_OK;
}

typedef struct
{
    bool isVisible;
    size_t len;
    size_t width;
    size_t height;
    pixformat_t format;
    struct timeval timestamp;
} PreData;

esp_err_t camera_capture()
{
    ESP_LOGI(TAG, "Pregrab");
    camera_fb_t *fb = esp_camera_fb_get();
    ESP_LOGI(TAG, "Postgrab");
    if (!fb)
    {
        ESP_LOGE(TAG, "Camera Capture Failed");
        return ESP_FAIL;
    }

    if (fb->len < 2 || fb->buf[0] != 0xFF || fb->buf[1] != 0xD8) {
        ESP_LOGE(TAG, "Invalid JPEG data (no SOI marker)");
        esp_camera_fb_return(fb);
        return ESP_FAIL;
    }
    

    const size_t headerSize = sizeof(PreData);
    const size_t totalSize = fb->len + headerSize;
    uint8_t *dataOut = malloc(totalSize);
    if (!dataOut)
    {
        ESP_LOGE(TAG, "Failed to allocate memory");
        esp_camera_fb_return(fb);
        return ESP_ERR_NO_MEM;
    }

    // Fill PreData at the start
    PreData *pre = (PreData *)dataOut;
    pre->isVisible = true;
    pre->len = fb->len;
    pre->width = fb->width;
    pre->height = fb->height;
    pre->format = fb->format;
    pre->timestamp = fb->timestamp;

    // Copy image data after PreData
    memcpy(dataOut + headerSize, fb->buf, fb->len);

    // Now chunk and send
    const size_t chunkSize = 1400;
    size_t bytesSent = 0;
    while (bytesSent < totalSize)
    {
        size_t bytesToSend = (totalSize - bytesSent) > chunkSize ? chunkSize : (totalSize - bytesSent);

        int retries = 3;
        esp_err_t err;
        do
        {
            err = esp_wifi_80211_tx(WIFI_IF_AP, dataOut + bytesSent, bytesToSend, true);
            if (err != ESP_OK)
                ESP_LOGW(TAG, "Retrying chunk send...");
        } while (err != ESP_OK && --retries > 0);

        if (err != ESP_OK)
        {
            ESP_LOGE(TAG, "Failed to send chunk after retries");
            free(dataOut);
            esp_camera_fb_return(fb);
            return ESP_FAIL;
        }

        bytesSent += bytesToSend;
    }

    free(dataOut);
    esp_camera_fb_return(fb);
    return ESP_OK;
}

void app_main(void)
{

    camera_init();

    vTaskDelay(pdMS_TO_TICKS(300)); // Let the camera wake up fully

    while (1)
    {
        camera_capture();
        vTaskDelay(pdMS_TO_TICKS(50)); // 20 FPS = 1000/50ms
    }
}