/* V4L2 摄像头采集 -> NV12 (支持 multiplanar 与 single planar 节点)
 *
 * 兼容:
 *   - RV1106 rkisp_mainpath 节点 (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 输出 NV12 双平面)
 *   - USB 摄像头 (V4L2_BUF_TYPE_VIDEO_CAPTURE, 输出 YUYV 或 NV12 单平面)
 *
 * 输出: 统一为 hor_stride*ver_stride*3/2 的连续 NV12 缓冲 (Y 区 + UV 区)
 *
 * 注意: RV1106 上若需要 ISP 调校应走 rkaiq; 此模块只负责从 ISP 输出节点取 NV12.
 */
#include "camera_v4l2.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <poll.h>
#include <linux/videodev2.h>

#include "rk_mpi_mb.h"
#include "rk_mpi_sys.h"
#include "rk_mpi_vi.h"

#define ALIGN16(x) (((x)+15)&~15)
#define MAX_PLANES 2

struct camera_v4l2 {
    int fd;
    int width, height, fps;
    int hor_stride;          /* 节点输出 Y 平面 bytesperline */
    int src_stride;          /* V4L2 buffer 的实际 bytesperline */
    int ver_stride;          /* 节点输出有效行数 (一般 = height) */
    int is_yuyv;
    int is_mplane;
    int num_planes;           /* MPLANE 节点驱动实际协商的 plane 数 (1=整帧NV12在plane0) */
    struct { void *start[MAX_PLANES]; size_t len[MAX_PLANES]; } buf[4];
    int nbuf;
    int use_rkmpi;
    int vi_dev, vi_pipe, vi_chn;
    int vi_dev_owned;
    int vi_chn_enabled;
};

static int xioctl(int fd, unsigned long req, void *arg){
    int r;
    do { r = ioctl(fd, req, arg); } while(r < 0 && errno == EINTR);
    return r;
}

/* 探测节点是否为 multiplanar: 1=是, 0=否, -1=QUERYCAP 失败 */
static int probe_mplane(int fd){
    struct v4l2_capability cap = {0};
    if(xioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) return -1;
    __u32 caps = (cap.capabilities & V4L2_CAP_DEVICE_CAPS)
               ? cap.device_caps : cap.capabilities;
    if(caps & V4L2_CAP_VIDEO_CAPTURE_MPLANE) return 1;
    if(caps & V4L2_CAP_VIDEO_CAPTURE)      return 0;
    /* 驱动报 capabilities_ext 也可能 */
    return (caps & V4L2_CAP_VIDEO_CAPTURE_MPLANE) ? 1 : 0;
}

static camera_v4l2_t *camera_rkmpi_open(const camera_cfg_t *cfg){
    struct camera_v4l2 *c = calloc(1, sizeof(*c));
    if(!c) return NULL;
    c->fd = -1;
    c->width = cfg->width;
    c->height = cfg->height;
    c->fps = cfg->fps > 0 ? cfg->fps : 25;
    c->hor_stride = c->width;
    c->src_stride = c->width;
    c->ver_stride = c->height;
    c->use_rkmpi = 1;
    c->vi_dev = 0;
    c->vi_pipe = 0;
    c->vi_chn = 0;

    VI_DEV_ATTR_S dev_attr;
    VI_DEV_BIND_PIPE_S bind_pipe;
    VI_CHN_ATTR_S chn_attr;
    memset(&dev_attr, 0, sizeof(dev_attr));
    memset(&bind_pipe, 0, sizeof(bind_pipe));
    memset(&chn_attr, 0, sizeof(chn_attr));

    RK_S32 ret = RK_MPI_VI_GetDevAttr(c->vi_dev, &dev_attr);
    if(ret == RK_ERR_VI_NOT_CONFIG){
        ret = RK_MPI_VI_SetDevAttr(c->vi_dev, &dev_attr);
        if(ret != RK_SUCCESS){
            fprintf(stderr, "[cam] RK_MPI_VI_SetDevAttr fail %#x\n", ret);
            goto fail;
        }
    }else if(ret != RK_SUCCESS){
        fprintf(stderr, "[cam] RK_MPI_VI_GetDevAttr fail %#x\n", ret);
        goto fail;
    }

    ret = RK_MPI_VI_GetDevIsEnable(c->vi_dev);
    if(ret != RK_SUCCESS){
        ret = RK_MPI_VI_EnableDev(c->vi_dev);
        if(ret != RK_SUCCESS){
            fprintf(stderr, "[cam] RK_MPI_VI_EnableDev fail %#x\n", ret);
            goto fail;
        }
        c->vi_dev_owned = 1;

        bind_pipe.u32Num = 1;
        bind_pipe.PipeId[0] = c->vi_pipe;
        ret = RK_MPI_VI_SetDevBindPipe(c->vi_dev, &bind_pipe);
        if(ret != RK_SUCCESS){
            fprintf(stderr, "[cam] RK_MPI_VI_SetDevBindPipe fail %#x\n", ret);
            goto fail;
        }
    }

    chn_attr.stIspOpt.u32BufCount = 3;
    chn_attr.stIspOpt.enMemoryType = VI_V4L2_MEMORY_TYPE_DMABUF;
    chn_attr.stSize.u32Width = c->width;
    chn_attr.stSize.u32Height = c->height;
    chn_attr.enPixelFormat = RK_FMT_YUV420SP;
    chn_attr.enCompressMode = COMPRESS_MODE_NONE;
    chn_attr.u32Depth = 2;
    ret = RK_MPI_VI_SetChnAttr(c->vi_pipe, c->vi_chn, &chn_attr);
    if(ret != RK_SUCCESS){
        fprintf(stderr, "[cam] RK_MPI_VI_SetChnAttr fail %#x\n", ret);
        goto fail;
    }
    ret = RK_MPI_VI_EnableChn(c->vi_pipe, c->vi_chn);
    if(ret != RK_SUCCESS){
        fprintf(stderr, "[cam] RK_MPI_VI_EnableChn fail %#x\n", ret);
        goto fail;
    }
    c->vi_chn_enabled = 1;

    fprintf(stderr,
            "[cam] %s %dx%d@%d NV12 (RKMPI VI dev=%d pipe=%d chn=%d)\n",
            cfg->dev, c->width, c->height, c->fps,
            c->vi_dev, c->vi_pipe, c->vi_chn);
    return c;

fail:
    if(c->vi_chn_enabled)
        RK_MPI_VI_DisableChn(c->vi_pipe, c->vi_chn);
    if(c->vi_dev_owned)
        RK_MPI_VI_DisableDev(c->vi_dev);
    free(c);
    return NULL;
}

camera_v4l2_t *camera_v4l2_open(const camera_cfg_t *cfg){
    if(!cfg || !cfg->dev) return NULL;
    if(strcmp(cfg->dev, "/dev/video11") == 0)
        return camera_rkmpi_open(cfg);
    struct camera_v4l2 *c = calloc(1, sizeof(*c));
    if(!c) return NULL;
    c->fd = -1;
    c->fd = open(cfg->dev, O_RDWR | O_NONBLOCK);
    if(c->fd < 0){ fprintf(stderr,"[cam] open %s: %s\n", cfg->dev, strerror(errno)); free(c); return NULL; }
    c->width = cfg->width; c->height = cfg->height; c->fps = cfg->fps>0?cfg->fps:25;
    c->is_yuyv = (cfg->format && strcmp(cfg->format,"YUYV")==0);

    int mp = probe_mplane(c->fd);
    if(mp < 0){ fprintf(stderr,"[cam] QUERYCAP fail\n"); goto fail; }
    c->is_mplane = mp;
    /* MPLANE 节点不支持 YUYV, 自动改 NV12 */
    if(c->is_mplane && c->is_yuyv){
        fprintf(stderr,"[cam] MPLANE 节点不支持 YUYV, 改用 NV12\n");
        c->is_yuyv = 0;
    }

    enum v4l2_buf_type btype = c->is_mplane ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE
                                             : V4L2_BUF_TYPE_VIDEO_CAPTURE;

    /* 设置格式 */
    if(c->is_mplane){
        struct v4l2_format fmt = {0};
        fmt.type = btype;
        fmt.fmt.pix_mp.width  = c->width;
        fmt.fmt.pix_mp.height = c->height;
        fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12;
        fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
        fmt.fmt.pix_mp.num_planes = MAX_PLANES;
        if(xioctl(c->fd, VIDIOC_S_FMT, &fmt) < 0){
            fprintf(stderr,"[cam] S_FMT(MPLANE) fail: %s\n", strerror(errno));
            goto fail;
        }
        c->width  = fmt.fmt.pix_mp.width;
        c->height = fmt.fmt.pix_mp.height;
        /* 驱动可能只给 1 个 plane (整帧 NV12 在 plane[0]), 不能假设是 2 */
        c->num_planes = fmt.fmt.pix_mp.num_planes;
        if(c->num_planes < 1) c->num_planes = 1;
        if(c->num_planes > MAX_PLANES) c->num_planes = MAX_PLANES;
        c->hor_stride = fmt.fmt.pix_mp.plane_fmt[0].bytesperline;
        if(c->hor_stride == 0) c->hor_stride = ALIGN16(c->width);
        c->src_stride = c->hor_stride;
        int ys = fmt.fmt.pix_mp.plane_fmt[0].sizeimage;
        if(c->num_planes == 1){
            /* 单 plane: sizeimage = hor_stride * ver_stride * 3/2 (Y 区 + UV 区) */
            c->ver_stride = (c->hor_stride > 0) ? (int)((size_t)ys * 2 / ((size_t)3 * (size_t)c->hor_stride)) : c->height;
        } else {
            /* 双 plane: plane[0] 仅 Y, sizeimage = hor_stride * ver_stride */
            c->ver_stride = (c->hor_stride > 0) ? (ys / c->hor_stride) : c->height;
        }
        if(c->ver_stride < c->height) c->ver_stride = c->height;
    } else {
        struct v4l2_format fmt = {0};
        fmt.type = btype;
        fmt.fmt.pix.width  = c->width; fmt.fmt.pix.height = c->height;
        fmt.fmt.pix.pixelformat = c->is_yuyv ? V4L2_PIX_FMT_YUYV : V4L2_PIX_FMT_NV12;
        fmt.fmt.pix.field = V4L2_FIELD_NONE;
        if(xioctl(c->fd, VIDIOC_S_FMT, &fmt) < 0){ fprintf(stderr,"[cam] S_FMT fail: %s\n", strerror(errno)); goto fail; }
        c->width  = fmt.fmt.pix.width;
        c->height = fmt.fmt.pix.height;
        c->src_stride = fmt.fmt.pix.bytesperline;
        if(c->src_stride == 0)
            c->src_stride = c->is_yuyv ? c->width * 2 : c->width;
        c->hor_stride = c->is_yuyv ? ALIGN16(c->width) : c->src_stride;
        if(c->is_yuyv){
            c->ver_stride = ALIGN16(c->height);
        }else{
            int sizeimage = fmt.fmt.pix.sizeimage;
            c->ver_stride = (c->src_stride > 0 && sizeimage > 0)
                          ? (int)((size_t)sizeimage * 2 /
                                  ((size_t)3 * (size_t)c->src_stride))
                          : ALIGN16(c->height);
            if(c->ver_stride < c->height) c->ver_stride = c->height;
        }
    }

    /* fps */
    struct v4l2_streamparm sp = {0};
    sp.type = btype;
    sp.parm.capture.timeperframe.numerator = 1;
    sp.parm.capture.timeperframe.denominator = c->fps;
    xioctl(c->fd, VIDIOC_S_PARM, &sp);

    /* 申请 buffer */
    struct v4l2_requestbuffers rb = {0};
    rb.count = 4; rb.type = btype; rb.memory = V4L2_MEMORY_MMAP;
    if(xioctl(c->fd, VIDIOC_REQBUFS, &rb) < 0){ fprintf(stderr,"[cam] REQBUFS fail: %s\n", strerror(errno)); goto fail; }
    c->nbuf = rb.count > 4 ? 4 : (int)rb.count;
    for(int i=0;i<c->nbuf;i++){
        struct v4l2_buffer b = {0};
        struct v4l2_plane planes[MAX_PLANES] = {0};
        b.type = btype; b.memory = V4L2_MEMORY_MMAP; b.index = i;
        if(c->is_mplane){ b.m.planes = planes; b.length = MAX_PLANES; }
        if(xioctl(c->fd, VIDIOC_QUERYBUF, &b) < 0){ fprintf(stderr,"[cam] QUERYBUF fail: %s\n", strerror(errno)); goto fail; }
        if(c->is_mplane){
            for(int p=0;p<(int)b.length;p++){
                if(planes[p].length == 0) break;
                c->buf[i].len[p]   = planes[p].length;
                c->buf[i].start[p] = mmap(NULL, planes[p].length, PROT_READ|PROT_WRITE,
                                          MAP_SHARED, c->fd, planes[p].m.mem_offset);
                if(c->buf[i].start[p] == MAP_FAILED){ fprintf(stderr,"[cam] mmap(plane%d) fail: %s\n", p, strerror(errno)); goto fail; }
            }
        } else {
            c->buf[i].len[0] = b.length;
            c->buf[i].start[0] = mmap(NULL, b.length, PROT_READ|PROT_WRITE,
                                      MAP_SHARED, c->fd, b.m.offset);
            if(c->buf[i].start[0] == MAP_FAILED){ fprintf(stderr,"[cam] mmap fail: %s\n", strerror(errno)); goto fail; }
        }
        if(xioctl(c->fd, VIDIOC_QBUF, &b) < 0){ fprintf(stderr,"[cam] QBUF fail: %s\n", strerror(errno)); goto fail; }
    }
    if(xioctl(c->fd, VIDIOC_STREAMON, &btype) < 0){ fprintf(stderr,"[cam] STREAMON fail: %s\n", strerror(errno)); goto fail; }

    fprintf(stderr,"[cam] %s %dx%d@%d %s%s stride=%dx%d planes=%d\n", cfg->dev,
            c->width,c->height,c->fps,
            c->is_mplane?"MPLANE ":"", c->is_yuyv?"YUYV":"NV12",
            c->hor_stride,c->ver_stride, c->is_mplane?c->num_planes:1);
    return c;
fail:
    if(c->fd>=0){
        for(int i=0;i<c->nbuf;i++)
            for(int p=0;p<MAX_PLANES;p++)
                if(c->buf[i].start[p] && c->buf[i].start[p]!=MAP_FAILED)
                    munmap(c->buf[i].start[p], c->buf[i].len[p]);
        close(c->fd);
    }
    free(c);
    return NULL;
}

void camera_v4l2_close(camera_v4l2_t *c){
    if(!c) return;
    if(c->use_rkmpi){
        if(c->vi_chn_enabled)
            RK_MPI_VI_DisableChn(c->vi_pipe, c->vi_chn);
        if(c->vi_dev_owned)
            RK_MPI_VI_DisableDev(c->vi_dev);
        free(c);
        return;
    }
    enum v4l2_buf_type t = c->is_mplane ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE : V4L2_BUF_TYPE_VIDEO_CAPTURE;
    xioctl(c->fd, VIDIOC_STREAMOFF, &t);
    for(int i=0;i<c->nbuf;i++)
        for(int p=0;p<MAX_PLANES;p++)
            if(c->buf[i].start[p] && c->buf[i].start[p]!=MAP_FAILED)
                munmap(c->buf[i].start[p], c->buf[i].len[p]);
    close(c->fd);
    free(c);
}

static void yuyv_to_nv12(const uint8_t *in, int src_stride, int w, int h,
                         uint8_t *out, int hs, int vs){
    for(int y=0;y<h;y++){
        const uint8_t *iy = in + (size_t)y*src_stride;
        uint8_t *oy = out + (size_t)y*hs;
        for(int x=0;x<w;x++) oy[x] = iy[x*2];
    }
    int ch = h/2;
    uint8_t *uv = out + (size_t)hs*vs;
    for(int y=0;y<ch;y++){
        const uint8_t *iy = in + (size_t)(y*2)*src_stride;
        uint8_t *ou = uv + (size_t)y*hs;
        for(int x=0;x<w/2;x++){
            ou[x*2]   = iy[x*4+1];
            ou[x*2+1] = iy[x*4+3];
        }
    }
}

int camera_v4l2_get_frame(camera_v4l2_t *c, uint8_t *nv12_out, size_t out_size,
                          int hor_stride, int ver_stride, int height){
    if(!c || !nv12_out || hor_stride < c->width || ver_stride < height ||
       height > c->height ||
       out_size < (size_t)hor_stride * (size_t)ver_stride * 3 / 2){
        fprintf(stderr, "[cam] invalid output buffer/stride\n");
        return -1;
    }

    /* DQBUF 带重试: rkisp 起流初期(AE 收敛/sensor 起流)常见 EIO 丢帧, 属正常现象 */
    if(c->use_rkmpi){
        VIDEO_FRAME_INFO_S frame;
        memset(&frame, 0, sizeof(frame));
        RK_S32 ret = RK_MPI_VI_GetChnFrame(c->vi_pipe, c->vi_chn, &frame, 2000);
        if(ret != RK_SUCCESS){
            fprintf(stderr, "[cam] RK_MPI_VI_GetChnFrame timeout/fail %#x\n", ret);
            return -2;
        }

        int result = 0;
        RK_MPI_SYS_MmzFlushCache(frame.stVFrame.pMbBlk, RK_TRUE);
        const uint8_t *src =
            (const uint8_t *)RK_MPI_MB_Handle2VirAddr(frame.stVFrame.pMbBlk);
        int src_hs = frame.stVFrame.u32VirWidth > 0
                   ? (int)frame.stVFrame.u32VirWidth : c->width;
        int src_vs = frame.stVFrame.u32VirHeight > 0
                   ? (int)frame.stVFrame.u32VirHeight : c->height;
        size_t src_size = (size_t)RK_MPI_MB_GetSize(frame.stVFrame.pMbBlk);
        size_t required = (size_t)src_hs * (size_t)src_vs * 3 / 2;
        if(!src || src_hs < c->width || src_vs < height || src_size < required){
            fprintf(stderr,
                    "[cam] invalid RKMPI VI frame addr=%p stride=%dx%d size=%zu need=%zu\n",
                    src, src_hs, src_vs, src_size, required);
            result = -1;
        }else{
            for(int y = 0; y < height; ++y)
                memcpy(nv12_out + (size_t)y * hor_stride,
                       src + (size_t)y * src_hs, c->width);
            const uint8_t *src_uv = src + (size_t)src_hs * src_vs;
            uint8_t *dst_uv = nv12_out + (size_t)hor_stride * ver_stride;
            for(int y = 0; y < height / 2; ++y)
                memcpy(dst_uv + (size_t)y * hor_stride,
                       src_uv + (size_t)y * src_hs, c->width);
        }

        RK_S32 release_ret =
            RK_MPI_VI_ReleaseChnFrame(c->vi_pipe, c->vi_chn, &frame);
        if(release_ret != RK_SUCCESS){
            fprintf(stderr, "[cam] RK_MPI_VI_ReleaseChnFrame fail %#x\n", release_ret);
            return -1;
        }
        return result;
    }

    struct v4l2_buffer b;
    struct v4l2_plane planes[MAX_PLANES];
    int drops = 0;
    for(;;){
        struct pollfd pfd = {0};
        pfd.fd = c->fd;
        pfd.events = POLLIN | POLLPRI | POLLERR;
        int poll_ret = poll(&pfd, 1, 2000);
        if(poll_ret == 0){
            fprintf(stderr, "[cam] frame timeout (sensor/ISP produced no frame)\n");
            return -2;
        }
        if(poll_ret < 0){
            if(errno == EINTR) return -2;
            fprintf(stderr, "[cam] poll fail: %s\n", strerror(errno));
            return -1;
        }

        memset(&b, 0, sizeof(b));
        memset(planes, 0, sizeof(planes));
        b.type = c->is_mplane ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE : V4L2_BUF_TYPE_VIDEO_CAPTURE;
        b.memory = V4L2_MEMORY_MMAP;
        if(c->is_mplane){ b.m.planes = planes; b.length = MAX_PLANES; }
        if(ioctl(c->fd, VIDIOC_DQBUF, &b) == 0){
            if(b.flags & V4L2_BUF_FLAG_ERROR){
                /* 坏帧: 归还后继续等下一帧 */
                xioctl(c->fd, VIDIOC_QBUF, &b);
                if(++drops > 240){ fprintf(stderr, "[cam] too many error frames\n"); return -1; }
                continue;
            }
            break; /* 拿到好帧 */
        }
        if(errno == EIO){
            if(++drops > 240){ fprintf(stderr, "[cam] DQBUF EIO x%d\n", drops); return -1; }
            continue;
        }
        if(errno == EAGAIN) continue;
        if(errno == EINTR) return -2;
        fprintf(stderr, "[cam] DQBUF fail: %s\n", strerror(errno));
        return -1;
    }

    int src_hs = c->src_stride;   /* 节点实际 bytesperline */
    int copy_w = c->width;         /* 有效像素宽度 */

    if(c->is_yuyv){
        yuyv_to_nv12((const uint8_t*)c->buf[b.index].start[0], src_hs,
                     c->width, c->height,
                     nv12_out, hor_stride, ver_stride);
    } else if(c->is_mplane){
        /* NV12 MPLANE: plane[0]=Y; plane[1]=UV (若驱动按单 plane 协商则 UV 紧跟 Y 区) */
        const uint8_t *py  = (const uint8_t*)c->buf[b.index].start[0];
        const uint8_t *puv = (const uint8_t*)c->buf[b.index].start[1];
        if(!py){ fprintf(stderr, "[cam] plane0 null\n"); xioctl(c->fd, VIDIOC_QBUF, &b); return -1; }
        if(!puv){
            /* 单 plane 布局: 整帧 NV12 连续存放, UV 区起始于 Y 区之后 */
            puv = py + (size_t)src_hs * (size_t)c->ver_stride;
        }
        /* 拷贝 Y: 行复制 (源 stride 可能 > width, 目标用 hor_stride) */
        for(int y=0;y<height;y++)
            memcpy(nv12_out + (size_t)y*hor_stride, py + (size_t)y*src_hs, copy_w);
        /* 拷贝 UV (NV12: UV 交错, 行数 = ver_stride/2) */
        uint8_t *uv_out = nv12_out + (size_t)hor_stride*ver_stride;
        for(int y=0;y<height/2;y++)
            memcpy(uv_out + (size_t)y*hor_stride, puv + (size_t)y*src_hs, copy_w);
    } else {
        /* NV12 single planar: Y 区 + UV 区 在同一 buffer */
        const uint8_t *s = c->buf[b.index].start[0];
        for(int y=0;y<height;y++)
            memcpy(nv12_out+(size_t)y*hor_stride, s+(size_t)y*src_hs, copy_w);
        for(int y=0;y<height/2;y++)
            memcpy(nv12_out+(size_t)hor_stride*ver_stride+(size_t)y*hor_stride,
                   s+(size_t)src_hs*c->ver_stride+(size_t)y*src_hs, copy_w);
    }
    xioctl(c->fd, VIDIOC_QBUF, &b);
    return 0;
}

void camera_v4l2_get_stride(camera_v4l2_t *c, int *hs, int *vs){
    if(c){ if(hs)*hs=c->hor_stride; if(vs)*vs=c->ver_stride; }
}
