// cam_enc_bench.c : V4L2 캡처 + TurboJPEG 인코딩 벤치마크 (WS/추론 없음)

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <signal.h>
#include <errno.h>
#include <pthread.h>
#include <linux/videodev2.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <time.h>
#include <unistd.h>
#include <stdint.h>
#include <turbojpeg.h>

#include "v4l2_interface.h"
#include "nc_utils.h"

// -----------------------------
// 설정
// -----------------------------
#define VIS0_MAX_CH         (0)
#define VIS1_MAX_CH         (1)
#define VIDEO_MAX_CH        (VIS0_MAX_CH + VIS1_MAX_CH)

#define VIDEO_BUFFER_NUM    (3)
#define VIDEO_WIDTH         (1920)
#define VIDEO_HEIGHT        (1080)

// 인코딩 워커 설정
#define ENC_WORKER_NUM      (4)
#define ENC_QUEUE_SIZE      (16)

// -----------------------------
// 전역
// -----------------------------
static volatile int g_running = 1;
st_nc_v4l2_config v4l2_config[VIDEO_MAX_CH];


// -----------------------------
// 인코딩 큐 구조체
// -----------------------------
typedef struct {
    int         valid;
    int         ch;
    int         w, h;
    int         stride;
    size_t      buf_size;
    uint8_t    *data;      // RGB24 복사 버퍼
} enc_job_t;

static enc_job_t       g_enc_queue[ENC_QUEUE_SIZE];
static int             g_enc_head = 0;
static int             g_enc_tail = 0;
static pthread_mutex_t g_enc_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_cond_t  g_enc_cond  = PTHREAD_COND_INITIALIZER;

// 간단 통계용
static unsigned long   g_enc_frame_count = 0;
static pthread_mutex_t g_stat_mutex = PTHREAD_MUTEX_INITIALIZER;
static struct timespec g_fps_last_ts = {0, 0};
static unsigned long   g_fps_last_cnt = 0;

// -----------------------------
// Ctrl+C 핸들러
// -----------------------------
static void on_sigint(int sig) {
    (void)sig;
    g_running = 0;

    // 워커들이 cond wait에서 깨어나도록 브로드캐스트
    pthread_mutex_lock(&g_enc_mutex);
    pthread_cond_broadcast(&g_enc_cond);
    pthread_mutex_unlock(&g_enc_mutex);
}

// -----------------------------
// 채널 설정
// -----------------------------
static void set_v4l2_config(void)
{
    // VIS0 영역 (현재 VIS0_MAX_CH = 0 이라서 실제론 없지만 구조 유지)
    for (int i = 0; i < VIS0_MAX_CH; i++) {
        v4l2_config[i].video_buf.video_device_num = CNN_DEVICE_NUM(VISION0) + i;
        v4l2_config[i].video_buf.video_fd         = -1;
        v4l2_config[i].dma_mode                   = INTERLEAVE;
        v4l2_config[i].img_process                = MODE_DS;
        v4l2_config[i].pixformat                  = V4L2_PIX_FMT_RGB24;
        v4l2_config[i].crop_x_start               = 0;
        v4l2_config[i].crop_y_start               = 0;
        v4l2_config[i].crop_width                 = 0;
        v4l2_config[i].crop_height                = 0;
        v4l2_config[i].ds_width                   = VIDEO_WIDTH;
        v4l2_config[i].ds_height                  = VIDEO_HEIGHT;
    }

    // VIS1 영역 (실제 1ch)
    for (int j = VIS0_MAX_CH; j < VIDEO_MAX_CH; j++) {
        v4l2_config[j].video_buf.video_device_num = CNN_DEVICE_NUM(VISION1) + j;
        v4l2_config[j].video_buf.video_fd         = -1;
        v4l2_config[j].dma_mode                   = INTERLEAVE;
        v4l2_config[j].img_process                = MODE_DS;
        v4l2_config[j].pixformat                  = V4L2_PIX_FMT_RGB24;
        v4l2_config[j].crop_x_start               = 0;
        v4l2_config[j].crop_y_start               = 0;
        v4l2_config[j].crop_width                 = 0;
        v4l2_config[j].crop_height                = 0;
        v4l2_config[j].ds_width                   = VIDEO_WIDTH;
        v4l2_config[j].ds_height                  = VIDEO_HEIGHT;
    }
}

// -----------------------------
// V4L2 초기화
// -----------------------------
static int v4l2_initialize(void)
{
    for (int i = 0; i < VIDEO_MAX_CH; i++) {
        v4l2_config[i].video_buf.video_fd =
            nc_v4l2_open(v4l2_config[i].video_buf.video_device_num, true);

        if (v4l2_config[i].video_buf.video_fd == errno) {
            fprintf(stderr, "[error] nc_v4l2_open() failure (ch=%d)\n", i);
            return -1;
        }
        if (nc_v4l2_init_device_and_stream_on(&v4l2_config[i], VIDEO_BUFFER_NUM) < 0) {
            fprintf(stderr, "[error] nc_v4l2_init_device_and_stream_on() failure (ch=%d)\n", i);
            return -1;
        }
    }

    nc_v4l2_show_user_config(&v4l2_config[0], VIDEO_MAX_CH);
    return 0;
}

// -----------------------------
// 인코딩 큐 enqueue
// -----------------------------
static void enc_enqueue(int ch, int w, int h, int stride, const void *src)
{
    size_t bytes = (size_t)stride * (size_t)h;

    pthread_mutex_lock(&g_enc_mutex);

    int next_tail = (g_enc_tail + 1) % ENC_QUEUE_SIZE;
    if (next_tail == g_enc_head) {
        // 큐가 가득 찼으면 가장 오래된 것 하나 drop
        g_enc_head = (g_enc_head + 1) % ENC_QUEUE_SIZE;
    }

    enc_job_t *job = &g_enc_queue[g_enc_tail];
    if (!job->data || job->buf_size < bytes) {
        free(job->data);
        job->data = (uint8_t*)malloc(bytes);
        job->buf_size = bytes;
    }

    memcpy(job->data, src, bytes);
    job->ch     = ch;
    job->w      = w;
    job->h      = h;
    job->stride = stride;
    job->valid  = 1;

    g_enc_tail = next_tail;

    pthread_cond_signal(&g_enc_cond);
    pthread_mutex_unlock(&g_enc_mutex);
}

// -----------------------------
// 인코딩 워커 스레드
// -----------------------------
static void* enc_worker_thread(void *arg)
{
    intptr_t wid = (intptr_t)arg;
    tjhandle tj = tjInitCompress();
    if (!tj) {
        fprintf(stderr, "[ENC%ld] tjInitCompress failed: %s\n",
                (long)wid, tjGetErrorStr());
        return NULL;
    }

    while (g_running) {
        enc_job_t job;
        memset(&job, 0, sizeof(job));

        // 큐에서 job 하나 가져오기
        pthread_mutex_lock(&g_enc_mutex);
        while (g_enc_head == g_enc_tail && g_running) {
            pthread_cond_wait(&g_enc_cond, &g_enc_mutex);
        }
        if (!g_running) {
            pthread_mutex_unlock(&g_enc_mutex);
            break;
        }

        job = g_enc_queue[g_enc_head];  // 구조체 복사 (data 포인터 공유)
        g_enc_head = (g_enc_head + 1) % ENC_QUEUE_SIZE;
        pthread_mutex_unlock(&g_enc_mutex);

        if (!job.valid || !job.data) continue;

        unsigned char *jpegBuf = NULL;
        unsigned long  jpegSize = 0;

        struct timespec t0, t1;
        clock_gettime(CLOCK_MONOTONIC, &t0);

        int rc = tjCompress2(
            tj,
            job.data,
            job.w,
            job.stride,
            job.h,
            TJPF_RGB,          // V4L2_PIX_FMT_RGB24 기준
            &jpegBuf,
            &jpegSize,
            TJSAMP_420,
            45,                // quality
            TJFLAG_FASTDCT
        );

        clock_gettime(CLOCK_MONOTONIC, &t1);

        if (rc != 0) {
            fprintf(stderr, "[ENC%ld] tjCompress2 error: %s\n",
                    (long)wid, tjGetErrorStr());
            if (jpegBuf) tjFree(jpegBuf);
            continue;
        }

        // double sec_diff  = (double)(t1.tv_sec  - t0.tv_sec);
        // double nsec_diff = (double)(t1.tv_nsec - t0.tv_nsec);

        // double ms = sec_diff * 1000.0 +
        //             nsec_diff / 1e6;

        pthread_mutex_lock(&g_stat_mutex);
        g_enc_frame_count++;
        unsigned long n = g_enc_frame_count;

        // ---- FPS 계산 ----
        struct timespec now;
        clock_gettime(CLOCK_MONOTONIC, &now);

        if (g_fps_last_ts.tv_sec == 0 && g_fps_last_ts.tv_nsec == 0) {
            // 첫 호출 초기화
            g_fps_last_ts   = now;
            g_fps_last_cnt  = n;
        } else {
            double sec = (double)(now.tv_sec  - g_fps_last_ts.tv_sec) +
                         (double)(now.tv_nsec - g_fps_last_ts.tv_nsec) / 1e9;

            if (sec >= 1.0) {  // 1초 이상 지났을 때만 출력
                unsigned long diff = n - g_fps_last_cnt;
                double fps = (double)diff / sec;
                printf("[ENC_STAT] total=%lu fps=%.2f\n", n, fps);

                g_fps_last_ts  = now;
                g_fps_last_cnt = n;
            }
        }
        pthread_mutex_unlock(&g_stat_mutex);

        // // 간단 로그
        // printf("[ENC%ld] frame=%lu ch=%d w=%d h=%d size=%lu bytes time=%.3f ms\n",
        //        (long)wid, n, job.ch, job.w, job.h,
        //        (unsigned long)jpegSize, ms);

        tjFree(jpegBuf);
    }

    tjDestroy(tj);
    return NULL;
}

// -----------------------------
// 메인
// -----------------------------
int main(int argc, char** argv)
{
    (void)argc; (void)argv;

    // SIGINT 핸들링
    struct sigaction sa;
    sa.sa_handler = on_sigint;
    sigemptyset(&sa.sa_mask);
    sa.sa_flags = SA_RESETHAND;
    sigaction(SIGINT, &sa, NULL);

    // 채널/디바이스 설정
    memset(v4l2_config, 0, sizeof(v4l2_config));
    set_v4l2_config();

    // V4L2 시작
    if (v4l2_initialize() < 0) {
        fprintf(stderr, "v4l2_initialize failed\n");
        return -1;
    }

    // 인코더 워커 스레드 시작
    pthread_t enc_threads[ENC_WORKER_NUM];
    for (int i = 0; i < ENC_WORKER_NUM; i++) {
        pthread_create(&enc_threads[i], NULL,
                       enc_worker_thread, (void*)(intptr_t)i);
    }

    printf("[APP] capture + TurboJPEG encode only (no WS, no CNN)\n");
    printf("[APP] press Ctrl+C to stop\n");

    // 메인 루프: 캡처 → 인코딩 큐 enqueue → QBUF
    while (g_running) {
        for (int ch = 0; ch < VIDEO_MAX_CH; ch++) {
            if (v4l2_config[ch].video_buf.video_fd < 0) continue;

            struct v4l2_buffer video_buf;
            CLEAR(video_buf);

            // DQBUF (프레임 수신)
            if (nc_v4l2_dequeue_buffer(v4l2_config[ch].video_buf.video_fd, &video_buf) == -1) {
                // non-fatal
                continue;
            }

            // 프레임 포인터
            void* frame_ptr = v4l2_config[ch].video_buf.buffers[video_buf.index].start;

            // RGB24 기준으로 인코딩 큐에 넣기
            int w      = VIDEO_WIDTH;
            int h      = VIDEO_HEIGHT;
            int stride = w * 3;

            enc_enqueue(ch, w, h, stride, frame_ptr);
            
            static struct timespec last_ts = {0, 0};
            struct timespec now;
            clock_gettime(CLOCK_MONOTONIC, &now);

            static unsigned long last_cnt = 0;

            if (last_ts.tv_sec != 0) {
                double sec = (double)(now.tv_sec - last_ts.tv_sec) +
                            (double)(now.tv_nsec - last_ts.tv_nsec) / 1e9;

                if (sec >= 1.0) {
                    unsigned long cur_cnt = g_enc_frame_count;
                    double fps = (double)(cur_cnt - last_cnt) / sec;
                    printf("[ENC_STAT] fps=%.2f\n", fps);
                    last_cnt = cur_cnt;
                    last_ts = now;
                }
            } else {
                last_ts = now;
            }

            // QBUF (재사용)
            if (nc_v4l2_queue_buffer(v4l2_config[ch].video_buf.video_fd, video_buf.index) == -1) {
                fprintf(stderr, "Error VIDIOC_QBUF buffer %d (ch=%d)\n",
                        video_buf.index, ch);
            }
        }

        // 너무 바쁘면 살짝 양보 (원하면 줄이거나 제거 가능)
        // usleep(1000);
    }

    // 종료 처리: 스트림 오프 & 닫기
    for (int i = 0; i < VIDEO_MAX_CH; i++) {
        if (v4l2_config[i].video_buf.video_fd >= 0) {
            close(v4l2_config[i].video_buf.video_fd);
            v4l2_config[i].video_buf.video_fd = -1;
        }
    }

    // 워커 종료 대기 (굳이 안 해도 프로세스 종료되지만 깔끔하게)
    for (int i = 0; i < ENC_WORKER_NUM; i++) {
        pthread_join(enc_threads[i], NULL);
    }

    // 큐 버퍼 free
    for (int i = 0; i < ENC_QUEUE_SIZE; i++) {
        free(g_enc_queue[i].data);
        g_enc_queue[i].data = NULL;
        g_enc_queue[i].buf_size = 0;
    }

    return 0;
}