// camera_pipeline_ifdef_fixed.c
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/time.h>
#include <linux/videodev2.h>
#include <time.h>

// FFmpeg头文件 - 使用纯C包含
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libavutil/avutil.h>
#include <libavutil/imgutils.h>
#include <libavutil/opt.h>
#include <libswscale/swscale.h>

#include "peer_kcp_client.h"

// ==========================================
// 1. 配置区域：在这里开启或关闭时间打印
// ==========================================
#define DEBUG_TIMING // 注释掉这一行，所有时间打印和计算都会消失

// 定义打印宏
#ifdef DEBUG_TIMING
#define PRINT_TIME(fmt, ...) printf(fmt, ##__VA_ARGS__)
#else
#define PRINT_TIME(fmt, ...) // 什么都不做，编译器会优化掉
#endif

#define WIDTH 1280
#define HEIGHT 720
#define OUTPUT_WIDTH 640
#define OUTPUT_HEIGHT 360
#define NUM_BUFFERS 4
#define CLEAR(x) memset(&(x), 0, sizeof(x))

#define VIDEO_SERVER_ADDR_ENV "OMNI_VIDEO_SERVER_ADDR"
#define VIDEO_RELAY_ADDR_ENV "OMNI_VIDEO_RELAY_VIA"
#define VIDEO_BIND_IP_ENV "OMNI_VIDEO_BIND_IP"
#define VIDEO_BIND_DEVICE_ENV "OMNI_VIDEO_BIND_DEVICE"
#define VIDEO_PEER_ID_ENV "OMNI_VIDEO_PEER_ID"
#define VIDEO_TARGET_PEER_ENV "OMNI_VIDEO_TARGET_PEER"
#define VIDEO_DEFAULT_PEER_ID "peer-b-video"
#define VIDEO_DEFAULT_TARGET_PEER "peer-a-video"

typedef struct
{
    kcp_client_t *client;
    char target_peer[OMNI_MAX_PEER_ID];
} VideoSender;

static int video_sender_init(VideoSender *sender);
static int video_sender_send_packet(VideoSender *sender, const AVPacket *encoded_pkt, uint64_t timestamp);
static void video_sender_close(VideoSender *sender);

typedef struct
{
    void *start;
    size_t length;
} Buffer;

double get_time_ms()
{
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return ts.tv_sec * 1000.0 + ts.tv_nsec / 1000000.0;
}

int64_t get_realtime_ms()
{
    struct timespec ts;
    clock_gettime(CLOCK_REALTIME, &ts);
    return (int64_t)ts.tv_sec * 1000 + ts.tv_nsec / 1000000;
}

static const char *env_or_default(const char *name, const char *fallback)
{
    const char *value = getenv(name);

    if (value != NULL && value[0] != '\0')
        return value;
    return fallback;
}

static int video_sender_init(VideoSender *sender)
{
    const char *server_addr = getenv(VIDEO_SERVER_ADDR_ENV);
    const char *relay_addr = env_or_default(VIDEO_RELAY_ADDR_ENV, "");
    const char *bind_ip = env_or_default(VIDEO_BIND_IP_ENV, "");
    const char *bind_device = env_or_default(VIDEO_BIND_DEVICE_ENV, "");
    const char *peer_id = env_or_default(VIDEO_PEER_ID_ENV, VIDEO_DEFAULT_PEER_ID);
    const char *target_peer = env_or_default(VIDEO_TARGET_PEER_ENV, VIDEO_DEFAULT_TARGET_PEER);
    kcp_conn_options_t options;

    if (sender == NULL)
    {
        errno = EINVAL;
        return -1;
    }
    if (server_addr == NULL || server_addr[0] == '\0')
    {
        errno = EINVAL;
        fprintf(stderr, "%s is required\n", VIDEO_SERVER_ADDR_ENV);
        return -1;
    }

    memset(sender, 0, sizeof(*sender));
    snprintf(sender->target_peer, sizeof(sender->target_peer), "%s", target_peer);

    kcp_conn_options_set_video_defaults(&options);
    sender->client = kcp_client_dial_with_options(
        server_addr,
        relay_addr,
        peer_id,
        bind_ip,
        bind_device,
        &options,
        NULL,
        NULL,
        NULL,
        KCP_DEFAULT_STATS_INTERVAL_MS);
    if (sender->client == NULL)
        return -1;

    fprintf(stderr, "Video sender connected as %s -> %s\n",
            kcp_client_id(sender->client), sender->target_peer);

    return 0;
}

static int video_sender_send_packet(VideoSender *sender, const AVPacket *encoded_pkt, uint64_t timestamp)
{
    uint8_t *senddata;
    size_t payload_len;
    int rc;

    if (sender == NULL || sender->client == NULL || encoded_pkt == NULL)
    {
        errno = EINVAL;
        return -1;
    }
    if (encoded_pkt->size < 0)
    {
        errno = EINVAL;
        return -1;
    }

    payload_len = (size_t)encoded_pkt->size + sizeof(timestamp);
    senddata = (uint8_t *)malloc(payload_len);
    if (senddata == NULL)
    {
        errno = ENOMEM;
        return -1;
    }

    memcpy(senddata, encoded_pkt->data, (size_t)encoded_pkt->size);
    memcpy(senddata + encoded_pkt->size, &timestamp, sizeof(timestamp));

    rc = kcp_client_send_binary(
        sender->client,
        sender->target_peer,
        senddata,
        payload_len);
    free(senddata);
    return rc;
}

static void video_sender_close(VideoSender *sender)
{
    if (sender == NULL || sender->client == NULL)
        return;

    kcp_client_close(sender->client);
    kcp_client_free(sender->client);
    sender->client = NULL;
}

int open_v4l2_device(const char *device)
{
    int fd = open(device, O_RDWR | O_NONBLOCK);
    if (fd < 0)
    {
        perror("open device");
        return -1;
    }
    return fd;
}

int init_v4l2_device(int fd)
{
    struct v4l2_format fmt = {0};
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = WIDTH;
    fmt.fmt.pix.height = HEIGHT;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
    fmt.fmt.pix.field = V4L2_FIELD_NONE;

    if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0)
    {
        perror("VIDIOC_S_FMT");
        return -1;
    }

    PRINT_TIME("Set format: %dx%d MJPEG\n", WIDTH, HEIGHT);
    return 0;
}

int init_mmap(int fd, Buffer **buffers, int *num_buffers)
{
    struct v4l2_requestbuffers req = {0};
    req.count = NUM_BUFFERS;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0)
    {
        perror("VIDIOC_REQBUFS");
        return -1;
    }

    *num_buffers = req.count;
    *buffers = (Buffer *)calloc(req.count, sizeof(Buffer));

    for (int i = 0; i < req.count; i++)
    {
        struct v4l2_buffer buf = {0};
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;

        if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0)
        {
            perror("VIDIOC_QUERYBUF");
            return -1;
        }

        (*buffers)[i].length = buf.length;
        (*buffers)[i].start = mmap(NULL, buf.length,
                                   PROT_READ | PROT_WRITE,
                                   MAP_SHARED, fd, buf.m.offset);

        if ((*buffers)[i].start == MAP_FAILED)
        {
            perror("mmap");
            return -1;
        }
    }

    return 0;
}

AVCodecContext *create_mjpeg_decoder()
{
    const AVCodec *decoder = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
    if (!decoder)
    {
        fprintf(stderr, "MJPEG decoder not found\n");
        return NULL;
    }

    AVCodecContext *ctx = avcodec_alloc_context3(decoder);
    ctx->width = WIDTH;
    ctx->height = HEIGHT;
    ctx->pix_fmt = AV_PIX_FMT_YUVJ420P;  // 使用YUVJ420P
    ctx->color_range = AVCOL_RANGE_JPEG; // JPEG范围
    ctx->thread_count = 1;

    AVDictionary *opts = NULL;
    av_dict_set(&opts, "flags2", "+fast", 0);

    if (avcodec_open2(ctx, decoder, &opts) < 0)
    {
        avcodec_free_context(&ctx);
        av_dict_free(&opts);
        return NULL;
    }

    av_dict_free(&opts);
    printf("Decoder created with format: YUVJ420P\n");
    return ctx;
}

AVCodecContext *create_mjpeg_encoder()
{
    const AVCodec *encoder = avcodec_find_encoder(AV_CODEC_ID_MJPEG);
    if (!encoder)
    {
        fprintf(stderr, "MJPEG encoder not found\n");
        return NULL;
    }

    AVCodecContext *ctx = avcodec_alloc_context3(encoder);
    ctx->width = OUTPUT_WIDTH;
    ctx->height = OUTPUT_HEIGHT;
    ctx->pix_fmt = AV_PIX_FMT_YUVJ420P; // 使用YUVJ420P
    ctx->time_base = (AVRational){1, 30};
    ctx->qmin = 8;
    ctx->qmax = 31;
    ctx->flags |= AV_CODEC_FLAG_QSCALE;
    ctx->global_quality = FF_QP2LAMBDA * 5;

    AVDictionary *opts = NULL;
    av_dict_set(&opts, "huffman", "default", 0);

    if (avcodec_open2(ctx, encoder, &opts) < 0)
    {
        avcodec_free_context(&ctx);
        av_dict_free(&opts);
        return NULL;
    }

    av_dict_free(&opts);
    printf("Encoder created with format: YUVJ420P\n");
    return ctx;
}

int decode_mjpeg_frame(AVCodecContext *decoder, const uint8_t *data, int size, AVFrame **frame)
{
    if (frame == NULL)
        return -1;

    *frame = NULL;
    AVPacket *pkt = av_packet_alloc();
    if (!pkt)
        return -1;

    pkt->data = (uint8_t *)data;
    pkt->size = size;

    int ret = avcodec_send_packet(decoder, pkt);
    if (ret < 0)
    {
        av_packet_free(&pkt);
        return -1;
    }

    *frame = av_frame_alloc();
    if (!*frame)
    {
        av_packet_free(&pkt);
        return -1;
    }
    ret = avcodec_receive_frame(decoder, *frame);

    av_packet_free(&pkt);
    if (ret < 0)
        av_frame_free(frame);
    return ret;
}

int scale_frame(AVFrame *src, AVFrame **dst)
{
    // 简单缩放，不设置复杂的色彩空间参数
    struct SwsContext *sws_ctx = sws_getContext(
        src->width, src->height, src->format,
        OUTPUT_WIDTH, OUTPUT_HEIGHT, AV_PIX_FMT_YUVJ420P,
        SWS_BILINEAR, NULL, NULL, NULL);

    if (!sws_ctx)
    {
        fprintf(stderr, "Failed to create sws context\n");
        return -1;
    }

    *dst = av_frame_alloc();
    if (!*dst)
    {
        sws_freeContext(sws_ctx);
        return -1;
    }

    (*dst)->width = OUTPUT_WIDTH;
    (*dst)->height = OUTPUT_HEIGHT;
    (*dst)->format = AV_PIX_FMT_YUVJ420P;

    if (av_frame_get_buffer(*dst, 0) < 0)
    {
        fprintf(stderr, "Failed to allocate frame buffer\n");
        av_frame_free(dst);
        sws_freeContext(sws_ctx);
        return -1;
    }

    int ret = sws_scale(sws_ctx, (const uint8_t *const *)src->data, src->linesize,
                        0, src->height, (*dst)->data, (*dst)->linesize);

    sws_freeContext(sws_ctx);

    if (ret < 0)
    {
        fprintf(stderr, "sws_scale failed\n");
        av_frame_free(dst);
        return -1;
    }

    return 0;
}

int encode_frame(AVCodecContext *encoder, AVFrame *frame, AVPacket **pkt)
{
    if (pkt == NULL)
        return -1;

    *pkt = NULL;
    *pkt = av_packet_alloc();
    if (!*pkt)
        return -1;

    int ret = avcodec_send_frame(encoder, frame);
    if (ret < 0)
    {
        av_packet_free(pkt);
        return -1;
    }

    ret = avcodec_receive_packet(encoder, *pkt);
    if (ret < 0)
        av_packet_free(pkt);
    return ret;
}

int main()
{
    VideoSender sender;

    memset(&sender, 0, sizeof(sender));

    PRINT_TIME("=== V4L2 Direct Capture + FFmpeg Processing ===\n");

    // 1. Open V4L2 device
    int fd = open_v4l2_device("/dev/video0");
    if (fd < 0)
    {
        fprintf(stderr, "Failed to open camera device\n");
        return 1;
    }

    // 2. Initialize V4L2
    if (init_v4l2_device(fd) < 0)
    {
        close(fd);
        return 1;
    }

    // 3. Setup MMAP buffers
    Buffer *buffers = NULL;
    int num_buffers = 0;
    if (init_mmap(fd, &buffers, &num_buffers) < 0)
    {
        close(fd);
        return 1;
    }

    // 4. Create FFmpeg codecs
    AVCodecContext *decoder = create_mjpeg_decoder();
    AVCodecContext *encoder = create_mjpeg_encoder();
    if (!decoder || !encoder)
    {
        fprintf(stderr, "Failed to create codecs\n");
        close(fd);
        return 1;
    }

    if (video_sender_init(&sender) < 0)
    {
        perror("video_sender_init");
        video_sender_close(&sender);
        avcodec_free_context(&encoder);
        avcodec_free_context(&decoder);
        close(fd);
        return 1;
    }

    // 5. Queue buffers
    for (int i = 0; i < num_buffers; i++)
    {
        struct v4l2_buffer buf = {0};
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;

        if (ioctl(fd, VIDIOC_QBUF, &buf) < 0)
        {
            perror("VIDIOC_QBUF");
            close(fd);
            return 1;
        }
    }

    // 6. Start streaming
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(fd, VIDIOC_STREAMON, &type) < 0)
    {
        perror("VIDIOC_STREAMON");
        close(fd);
        return 1;
    }

    // 7. Benchmark
    // 使用宏控制打印表头
    PRINT_TIME("\nRunning benchmark (100 frames)...\n");
    PRINT_TIME("Frame | Capture | Decode | Scale | Encode | Total | Size | Marker\n");
    PRINT_TIME("------|---------|--------|-------|--------|-------|------|--------\n");

    for (int i = 0; i < 10000; i++)
    {
// 只有在开启 DEBUG_TIMING 时才声明这些时间变量
#ifdef DEBUG_TIMING
        double total_start = get_time_ms();
        double capture_start, capture_end;
        double decode_start, decode_end;
        double scale_start, scale_end;
        double encode_start, encode_end;
#endif

        // Wait for frame
        fd_set fds;
        FD_ZERO(&fds);
        FD_SET(fd, &fds);

        struct timeval tv = {2, 0};
        int r = select(fd + 1, &fds, NULL, NULL, &tv);
        if (r <= 0)
        {
            PRINT_TIME("Timeout waiting for frame\n");
            break;
        }

#ifdef DEBUG_TIMING
        capture_start = get_time_ms();
#endif

        // Dequeue buffer
        struct v4l2_buffer buf = {0};
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;

        if (ioctl(fd, VIDIOC_DQBUF, &buf) < 0)
        {
            perror("VIDIOC_DQBUF");
            break;
        }

#ifdef DEBUG_TIMING
        capture_end = get_time_ms();
#endif

// Decode
#ifdef DEBUG_TIMING
        decode_start = get_time_ms();
#endif

        AVFrame *decoded_frame = NULL;
        int ret = decode_mjpeg_frame(decoder,
                                     (uint8_t *)buffers[buf.index].start, buf.bytesused, &decoded_frame);

#ifdef DEBUG_TIMING
        decode_end = get_time_ms();
#endif

        if (ret < 0 || !decoded_frame)
        {
            PRINT_TIME("Frame %d: Decode failed\n", i + 1);
            ioctl(fd, VIDIOC_QBUF, &buf);
            continue;
        }

// Scale
#ifdef DEBUG_TIMING
        scale_start = get_time_ms();
#endif

        AVFrame *scaled_frame = NULL;
        if (scale_frame(decoded_frame, &scaled_frame) < 0)
        {
            PRINT_TIME("Frame %d: Scale failed\n", i + 1);
            av_frame_free(&decoded_frame);
            ioctl(fd, VIDIOC_QBUF, &buf);
            continue;
        }

#ifdef DEBUG_TIMING
        scale_end = get_time_ms();
#endif

// Encode
#ifdef DEBUG_TIMING
        encode_start = get_time_ms();
#endif

        AVPacket *encoded_pkt = NULL;
        if (encode_frame(encoder, scaled_frame, &encoded_pkt) < 0)
        {
            PRINT_TIME("Frame %d: Encode failed\n", i + 1);
        }
#ifdef DEBUG_TIMING
        if (encoded_pkt && i % 50 == 0)
        {
            char filename[100];
            sprintf(filename, "frame_%04d.jpg", i + 1);

            FILE *f = fopen(filename, "wb");
            if (f)
            {
                fwrite(encoded_pkt->data, 1, encoded_pkt->size, f);
                fclose(f);
                PRINT_TIME("Saved as %s\n", filename);
            }
        }
#endif

#ifdef DEBUG_TIMING
        encode_end = get_time_ms();
        double total_end = get_time_ms();
#endif

// 打印结果
#ifdef DEBUG_TIMING
        PRINT_TIME("%5d | %7.1f | %6.1f | %5.1f | %6.1f | %5.1f | %4d KB  | 0x%02x\n",
                   i + 1,
                   capture_end - capture_start,
                   decode_end - decode_start,
                   scale_end - scale_start,
                   encode_end - encode_start,
                   total_end - total_start,
                   encoded_pkt ? encoded_pkt->size / 1024 : 0,
                   encoded_pkt && encoded_pkt->size > 1 ? encoded_pkt->data[1] : 0);
#else
        // 如果不开启宏，也打印一些基本信息
        printf("Frame %d processed\n", i + 1);
#endif
        int64_t unixtime_ms;
        if (encoded_pkt)
        {
            unixtime_ms = get_realtime_ms();
        }
        if (encoded_pkt && video_sender_send_packet(&sender, encoded_pkt, unixtime_ms) != 0)
        {
            perror("video_sender_send_packet");
            av_frame_free(&decoded_frame);
            av_frame_free(&scaled_frame);
            av_packet_free(&encoded_pkt);
            ioctl(fd, VIDIOC_QBUF, &buf);
            break;
        }

        // Cleanup
        av_frame_free(&decoded_frame);
        av_frame_free(&scaled_frame);
        if (encoded_pkt)
            av_packet_free(&encoded_pkt);

        // Requeue buffer
        if (ioctl(fd, VIDIOC_QBUF, &buf) < 0)
        {
            perror("VIDIOC_QBUF");
            break;
        }
    }

    // 8. Stop streaming
    ioctl(fd, VIDIOC_STREAMOFF, &type);

    // 9. Cleanup
    for (int i = 0; i < num_buffers; i++)
    {
        if (buffers[i].start != MAP_FAILED)
        {
            munmap(buffers[i].start, buffers[i].length);
        }
    }
    free(buffers);
    video_sender_close(&sender);
    avcodec_free_context(&encoder);
    avcodec_free_context(&decoder);
    close(fd);

    return 0;
}
// gcc -o v1_camera_pipeline_ifdef v1_camera_pipeline_ifdef.c $(pkg-config --cflags --libs libavformat libavcodec libavutil libswscale)