#include "esp32cam.h"
#include "appconfig.h"
#include "logging.h"
#include <esp_timer.h>

namespace
{
constexpr uint32_t FLASH_SETTLE_TIME_MS = 300;
constexpr uint32_t CAPTURE_TIMEOUT_MS = 2500;
constexpr uint64_t FLASH_SAFETY_TIMEOUT_US = 3000000;
constexpr uint8_t MAX_FRAME_ATTEMPTS = 4;

#if PHOTO_QUALITY == PHOTO_QUALITY_LOW
constexpr framesize_t PHOTO_FRAME_SIZE = FRAMESIZE_VGA;
constexpr int PHOTO_JPEG_QUALITY = 12;
constexpr char PHOTO_PROFILE_DESCRIPTION[] = "LOW: VGA 640x480, JPEG quality 12";
#elif PHOTO_QUALITY == PHOTO_QUALITY_MEDIUM
constexpr framesize_t PHOTO_FRAME_SIZE = FRAMESIZE_SVGA;
constexpr int PHOTO_JPEG_QUALITY = 8;
constexpr char PHOTO_PROFILE_DESCRIPTION[] =
    "MEDIUM: SVGA 800x600, JPEG quality 8";
#else
constexpr framesize_t PHOTO_FRAME_SIZE = FRAMESIZE_UXGA;
constexpr int PHOTO_JPEG_QUALITY = 4;
constexpr char PHOTO_PROFILE_DESCRIPTION[] =
    "HIGH: UXGA 1600x1200, JPEG quality 4";
#endif

int64_t frameTimestampUs(const camera_fb_t *frame)
{
    return static_cast<int64_t>(frame->timestamp.tv_sec) * 1000000LL +
           static_cast<int64_t>(frame->timestamp.tv_usec);
}
}

bool CameraApp::begin()
{
    const bool hasPsram = psramFound();

    config.ledc_channel = LEDC_CHANNEL_0;
    config.ledc_timer = LEDC_TIMER_0;

    config.pin_d0 = CAM_PIN_D0;
    config.pin_d1 = CAM_PIN_D1;
    config.pin_d2 = CAM_PIN_D2;
    config.pin_d3 = CAM_PIN_D3;
    config.pin_d4 = CAM_PIN_D4;
    config.pin_d5 = CAM_PIN_D5;
    config.pin_d6 = CAM_PIN_D6;
    config.pin_d7 = CAM_PIN_D7;

    config.pin_xclk = CAM_PIN_XCLK;
    config.pin_pclk = CAM_PIN_PCLK;
    config.pin_vsync = CAM_PIN_VSYNC;
    config.pin_href = CAM_PIN_HREF;

    config.pin_sccb_sda = CAM_PIN_SIOD;
    config.pin_sccb_scl = CAM_PIN_SIOC;

    config.pin_pwdn = CAM_PIN_PWDN;
    config.pin_reset = CAM_PIN_RESET;

    config.xclk_freq_hz = 20000000;
    config.pixel_format = PIXFORMAT_JPEG;

    if (hasPsram)
    {
        config.frame_size = PHOTO_FRAME_SIZE;
        config.jpeg_quality = PHOTO_JPEG_QUALITY;
        config.fb_count = 2;
        config.fb_location = CAMERA_FB_IN_PSRAM;
        config.grab_mode = CAMERA_GRAB_LATEST;
        LOG_PRINTF("Camera profile: %s, 2 buffers in PSRAM\n",
                   PHOTO_PROFILE_DESCRIPTION);
    }
    else
    {
        // Pełna rozdzielczość nie jest niezawodna bez PSRAM.
        config.frame_size = FRAMESIZE_VGA;
        config.jpeg_quality = 8;
        config.fb_count = 1;
        config.fb_location = CAMERA_FB_IN_DRAM;
        config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
        LOG_PRINTLN(
            "PSRAM unavailable; using VGA 640x480 and one buffer in DRAM");
    }

    esp_err_t err = esp_camera_init(&config);

    if (err != ESP_OK)
    {
        LOG_PRINTF("Camera init failed: 0x%x\n", err);
        esp_camera_deinit();
        return false;
    }
    sensor_t *s = esp_camera_sensor_get();

    if (s == nullptr)
    {
        LOG_PRINTLN("Camera sensor is unavailable after initialization");
        esp_camera_deinit();
        return false;
    }

    s->set_vflip(s, 1);   // obrót w pionie
    s->set_hmirror(s, 1); // odbicie w poziomie
    s->set_gain_ctrl(s, 1);
    s->set_exposure_ctrl(s, 1);
    s->set_whitebal(s, 1);
    s->set_awb_gain(s, 1);

    // Neutralna obróbka zachowuje szczegóły w światłach i cieniach.
    s->set_brightness(s, 0);
    s->set_contrast(s, 0);
    s->set_saturation(s, 0);
    s->set_sharpness(s, 1);

    if (!startCaptureWorker())
    {
        LOG_PRINTLN("Failed to start camera capture worker");
        esp_camera_deinit();
        return false;
    }

    LOG_PRINTLN("Camera initialized");
    return true;
}

camera_fb_t *CameraApp::captureWithFlash()
{
    discardPendingCaptureResults();

    if (captureWorkerAvailable == nullptr ||
        xSemaphoreTake(captureWorkerAvailable, 0) != pdTRUE)
    {
        LOG_PRINTLN("Camera capture worker is still busy after a timeout");
        return nullptr;
    }

    const uint32_t captureStartedAt = millis();
    const int64_t captureDeadlineTimestampUs =
        esp_timer_get_time() + CAPTURE_TIMEOUT_MS * 1000LL;
    if (!flashOn())
    {
        LOG_PRINTLN("Flash safety timer could not be armed");
        xSemaphoreGive(captureWorkerAvailable);
        return nullptr;
    }

    const int64_t earliestAcceptedTimestamp = esp_timer_get_time() +
                                              FLASH_SETTLE_TIME_MS * 1000LL;
    delay(FLASH_SETTLE_TIME_MS);

    CaptureRequest request{
        ++nextCaptureRequestId,
        earliestAcceptedTimestamp,
        captureDeadlineTimestampUs};

    if (captureRequestQueue == nullptr ||
        xQueueSend(captureRequestQueue, &request, 0) != pdTRUE)
    {
        LOG_PRINTLN("Failed to queue camera capture request");
        xSemaphoreGive(captureWorkerAvailable);
        flashOff();
        return nullptr;
    }

    while (millis() - captureStartedAt < CAPTURE_TIMEOUT_MS)
    {
        const uint32_t elapsed = millis() - captureStartedAt;
        const TickType_t remainingTicks =
            pdMS_TO_TICKS(CAPTURE_TIMEOUT_MS - elapsed);
        CaptureResult result{};

        if (xQueueReceive(captureResultQueue, &result, remainingTicks) != pdTRUE)
        {
            break;
        }

        if (result.id != request.id)
        {
            LOG_PRINTF("Discarding late camera result for request %lu\n",
                       static_cast<unsigned long>(result.id));
            release(result.frame);
            continue;
        }

        flashOff();
        if (result.frame == nullptr)
        {
            LOG_PRINTLN("Camera capture failed");
        }
        return result.frame;
    }

    flashOff();
    LOG_PRINTF("Camera capture timed out after %lu ms\n",
               static_cast<unsigned long>(millis() - captureStartedAt));
    return nullptr;
}

int CameraApp::configButtonAndFlash()
{
    pinMode(BUTTON_PIN, INPUT_PULLUP); // Internal pull-up
    pinMode(FLASH_PIN, OUTPUT);
    digitalWrite(FLASH_PIN, LOW);

    if (flashSafetyTimer == nullptr)
    {
        esp_timer_create_args_t timerArguments{};
        timerArguments.callback = flashSafetyTimerCallback;
        timerArguments.arg = this;
        timerArguments.dispatch_method = ESP_TIMER_TASK;
        timerArguments.name = "flash-safety";

        const esp_err_t timerResult =
            esp_timer_create(&timerArguments, &flashSafetyTimer);
        if (timerResult != ESP_OK)
        {
            return static_cast<int>(timerResult);
        }
    }

    return static_cast<int>(ESP_OK);
}
void CameraApp::release(camera_fb_t *frame)
{
    if (frame != nullptr)
    {
        esp_camera_fb_return(frame);
    }
}

void CameraApp::blinkFlash(uint8_t count, uint16_t onTimeMs,
                           uint16_t offTimeMs)
{
    for (uint8_t blink = 0; blink < count; ++blink)
    {
        if (!flashOn())
        {
            return;
        }
        delay(onTimeMs);
        flashOff();

        if (blink + 1U < count)
        {
            delay(offTimeMs);
        }
    }
}

bool CameraApp::startCaptureWorker()
{
    captureRequestQueue = xQueueCreate(1, sizeof(CaptureRequest));
    // Dwa miejsca pozwalają odebrać spóźniony wynik i bieżący wynik bez wycieku.
    captureResultQueue = xQueueCreate(2, sizeof(CaptureResult));
    captureWorkerAvailable = xSemaphoreCreateBinary();

    if (captureRequestQueue == nullptr || captureResultQueue == nullptr ||
        captureWorkerAvailable == nullptr)
    {
        if (captureRequestQueue != nullptr)
        {
            vQueueDelete(captureRequestQueue);
            captureRequestQueue = nullptr;
        }
        if (captureResultQueue != nullptr)
        {
            vQueueDelete(captureResultQueue);
            captureResultQueue = nullptr;
        }
        if (captureWorkerAvailable != nullptr)
        {
            vSemaphoreDelete(captureWorkerAvailable);
            captureWorkerAvailable = nullptr;
        }
        return false;
    }

    xSemaphoreGive(captureWorkerAvailable);

    const BaseType_t taskResult = xTaskCreatePinnedToCore(
        captureWorkerEntry,
        "camera-capture",
        AppConfig::CAMERA_TASK_STACK_BYTES,
        this,
        AppConfig::CAMERA_TASK_PRIORITY,
        &captureWorkerTask,
        AppConfig::CAMERA_TASK_CORE);

    if (taskResult != pdPASS)
    {
        vQueueDelete(captureRequestQueue);
        vQueueDelete(captureResultQueue);
        vSemaphoreDelete(captureWorkerAvailable);
        captureRequestQueue = nullptr;
        captureResultQueue = nullptr;
        captureWorkerAvailable = nullptr;
        captureWorkerTask = nullptr;
        return false;
    }

    return true;
}

void CameraApp::discardPendingCaptureResults()
{
    if (captureResultQueue == nullptr)
    {
        return;
    }

    CaptureResult result{};
    while (xQueueReceive(captureResultQueue, &result, 0) == pdTRUE)
    {
        LOG_PRINTF("Releasing late camera frame for request %lu\n",
                   static_cast<unsigned long>(result.id));
        release(result.frame);
    }
}

void CameraApp::captureWorkerEntry(void *parameter)
{
    static_cast<CameraApp *>(parameter)->captureWorkerLoop();
}

void CameraApp::captureWorkerLoop()
{
    while (true)
    {
        CaptureRequest request{};
        if (xQueueReceive(
                captureRequestQueue, &request, portMAX_DELAY) != pdTRUE)
        {
            continue;
        }

        camera_fb_t *acceptedFrame = nullptr;
        for (uint8_t attempt = 0; attempt < MAX_FRAME_ATTEMPTS; ++attempt)
        {
            camera_fb_t *frame = esp_camera_fb_get();
            if (frame == nullptr)
            {
                break;
            }

            const int64_t timestamp = frameTimestampUs(frame);
            if (timestamp >= request.earliestAcceptedTimestampUs)
            {
                acceptedFrame = frame;
                break;
            }
            esp_camera_fb_return(frame);
        }

        const bool requestExpired =
            esp_timer_get_time() >= request.deadlineTimestampUs;
        if (requestExpired)
        {
            if (acceptedFrame != nullptr)
            {
                esp_camera_fb_return(acceptedFrame);
            }
            LOG_PRINTF("Camera result expired for request %lu\n",
                       static_cast<unsigned long>(request.id));
        }
        else
        {
            CaptureResult result{request.id, acceptedFrame};
            if (xQueueSend(captureResultQueue, &result, 0) != pdTRUE &&
                acceptedFrame != nullptr)
            {
                esp_camera_fb_return(acceptedFrame);
            }
        }

        xSemaphoreGive(captureWorkerAvailable);

        LOG_PRINTF(
            "Camera task minimum free stack: %u bytes\n",
            static_cast<unsigned>(uxTaskGetStackHighWaterMark(nullptr)));
    }
}

void CameraApp::flashSafetyTimerCallback(void *parameter)
{
    static_cast<CameraApp *>(parameter)->forceFlashOff();
}

bool CameraApp::flashOn()
{
    digitalWrite(FLASH_PIN, HIGH);

    if (flashSafetyTimer == nullptr)
    {
        // Pozwala zasygnalizować błąd utworzenia timera podczas setup().
        // Zdjęcia nie są wtedy wykonywane, ponieważ setup zostaje zatrzymany.
        return true;
    }

    esp_timer_stop(flashSafetyTimer);
    if (esp_timer_start_once(
            flashSafetyTimer, FLASH_SAFETY_TIMEOUT_US) != ESP_OK)
    {
        forceFlashOff();
        LOG_PRINTLN("Failed to arm flash safety timeout");
        return false;
    }

    return true;
}
void CameraApp::flashOff()
{
    forceFlashOff();
    if (flashSafetyTimer != nullptr)
    {
        esp_timer_stop(flashSafetyTimer);
    }
}

void CameraApp::forceFlashOff()
{
    digitalWrite(FLASH_PIN, LOW);
}
