// camera_pipeline_ifdef_fixed.c
#include <stdio.h>
#include <stdlib.h>
#include <string.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>

// ==========================================
// 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))

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;
}

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;  // 量化参数控制，8大小为10-15kb,2 大小为20-35KB
    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) {
    AVPacket* pkt = av_packet_alloc();
    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();
    ret = avcodec_receive_frame(decoder, *frame);
    
    av_packet_free(&pkt);
    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) {
    *pkt = av_packet_alloc();
    int ret = avcodec_send_frame(encoder, frame);
    if (ret < 0) return -1;
    
    ret = avcodec_receive_packet(encoder, *pkt);
    return ret;
}

int main() {
    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;
    }
    
    // 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 < 100; 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
        
        // 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);
    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)