#include "v4l2_capture.h"
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <linux/videodev2.h>
#include <cstring>
#include <cstdio>
#include <algorithm>

// 四字符码构造宏 (使用 v4l2_fourcc 避免重复定义)
// 注意: 这里使用videodev2.h已有的v4l2_fourcc宏

namespace {

// 将 v4l2_capability 的 driver[16] / card[32] 转成 C 字符串 (v4l2_capability 使用 __u8)
inline std::string to_cstr(const __u8 *arr, size_t len) {
    std::string s;
    s.reserve(len);
    for (size_t i = 0; i < len && arr[i] != '\0'; ++i) {
        char c = static_cast<char>(arr[i]);
        if (c >= 32 && c < 127) s += c;  // 只保留可打印 ASCII
    }
    // 去除末尾空格
    while (!s.empty() && s.back() == ' ') s.pop_back();
    return s;
}

// 打印 v4l2 格式调试信息
void dumpFormat(struct v4l2_format &fmt, const char *msg) {
    printf("%s: width=%u height=%u pixfmt=0x%08x '%c%c%c%c' bpl=%u size=%u\n",
           msg,
           fmt.fmt.pix.width,
           fmt.fmt.pix.height,
           fmt.fmt.pix.pixelformat,
           (fmt.fmt.pix.pixelformat) & 0xFF,
           (fmt.fmt.pix.pixelformat >> 8) & 0xFF,
           (fmt.fmt.pix.pixelformat >> 16) & 0xFF,
           (fmt.fmt.pix.pixelformat >> 24) & 0xFF,
           fmt.fmt.pix.bytesperline,
           fmt.fmt.pix.sizeimage);
}

}  // namespace

V4l2Capture::~V4l2Capture() {
    close();
}

bool V4l2Capture::open(const std::string &device) {
    if (fd_ >= 0) {
        return false;
    }
    fd_ = ::open(device.c_str(), O_RDWR | O_NONBLOCK, 0);
    if (fd_ < 0) {
        printf("V4l2Capture: open %s failed: %s\n", device.c_str(), strerror(errno));
        return false;
    }

    // 检查设备能力
    struct v4l2_capability cap;
    if (ioctl(fd_, VIDIOC_QUERYCAP, &cap) < 0) {
        printf("V4l2Capture: VIDIOC_QUERYCAP failed: %s\n", strerror(errno));
        ::close(fd_);
        fd_ = -1;
        return false;
    }
    printf("V4l2Capture: opened %s, driver='%s' card='%s', caps=0x%x\n",
           device.c_str(),
           to_cstr(cap.driver, sizeof(cap.driver)).c_str(),
           to_cstr(cap.card, sizeof(cap.card)).c_str(),
           cap.capabilities);

    // 必须支持 V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        printf("V4l2Capture: not a video capture device\n");
        ::close(fd_);
        fd_ = -1;
        return false;
    }
    if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
        printf("V4l2Capture: does not support streaming I/O\n");
        ::close(fd_);
        fd_ = -1;
        return false;
    }

    return true;
}

void V4l2Capture::close() {
    if (streaming_) {
        stopStreaming();
    }
    buffers_.clear();
    if (fd_ >= 0) {
        ::close(fd_);
        fd_ = -1;
    }
}

bool V4l2Capture::setFormat(uint32_t width, uint32_t height, uint32_t pixelformat) {
    if (fd_ < 0) return false;

    struct v4l2_format fmt;
    std::memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = width;
    fmt.fmt.pix.height = height;
    fmt.fmt.pix.pixelformat = pixelformat;
    fmt.fmt.pix.field = V4L2_FIELD_NONE;  // 逐行扫描

    if (ioctl(fd_, VIDIOC_S_FMT, &fmt) < 0) {
        printf("V4l2Capture: VIDIOC_S_FMT failed: %s\n", strerror(errno));
        return false;
    }

    dumpFormat(fmt, "V4l2Capture: setFormat");

    width_ = fmt.fmt.pix.width;
    height_ = fmt.fmt.pix.height;
    pixelformat_ = fmt.fmt.pix.pixelformat;
    bytes_per_line_ = fmt.fmt.pix.bytesperline;

    // 如果驱动不按请求设置, 给出警告
    if (width_ != width || height_ != height) {
        printf("V4l2Capture: warning: requested %ux%u but got %ux%u\n",
               width, height, width_, height_);
    }
    if (pixelformat_ != pixelformat) {
        char req[5] = {0}, got[5] = {0};
        req[0] = pixelformat & 0xFF; req[1] = (pixelformat>>8)&0xFF;
        req[2] = (pixelformat>>16)&0xFF; req[3] = (pixelformat>>24)&0xFF;
        got[0] = pixelformat_ & 0xFF; got[1] = (pixelformat_>>8)&0xFF;
        got[2] = (pixelformat_>>16)&0xFF; got[3] = (pixelformat_>>24)&0xFF;
        printf("V4l2Capture: warning: requested '%s' but got '%s'\n", req, got);
    }

    return true;
}

bool V4l2Capture::setFps(uint32_t fps) {
    if (fd_ < 0) return false;

    struct v4l2_streamparm parm;
    std::memset(&parm, 0, sizeof(parm));
    parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (ioctl(fd_, VIDIOC_G_PARM, &parm) < 0) {
        printf("V4l2Capture: VIDIOC_G_PARM failed: %s\n", strerror(errno));
        return false;
    }

    // 尝试设置 FPS. 需要 V4L2_CAP_TIMEPERFRAME 支持
    if (parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) {
        parm.parm.capture.timeperframe.numerator = 1;
        parm.parm.capture.timeperframe.denominator = fps;
        if (ioctl(fd_, VIDIOC_S_PARM, &parm) < 0) {
            printf("V4l2Capture: VIDIOC_S_PARM fps=%u failed: %s\n", fps, strerror(errno));
            return false;
        }
        printf("V4l2Capture: set fps=%u (actual %u/%u)\n",
               fps,
               parm.parm.capture.timeperframe.denominator,
               parm.parm.capture.timeperframe.numerator);
    } else {
        printf("V4l2Capture: device does not support V4L2_CAP_TIMEPERFRAME\n");
    }
    return true;
}

bool V4l2Capture::setControl(uint32_t id, int32_t value) {
    if (fd_ < 0) return false;

    struct v4l2_control ctrl;
    std::memset(&ctrl, 0, sizeof(ctrl));
    ctrl.id = id;
    ctrl.value = value;

    if (ioctl(fd_, VIDIOC_S_CTRL, &ctrl) < 0) {
        // 有些控制可能是只读的或不存在, 打印警告但不失败
        // 继续尝试 VIDIOC_TRY_CTRL 看是否可用
        struct v4l2_ext_controls ext_ctrls = {};
        struct v4l2_ext_control ext_ctrl = {};
        ext_ctrl.id = id;
        ext_ctrl.value = value;
        ext_ctrls.count = 1;
        ext_ctrls.controls = &ext_ctrl;

        if (ioctl(fd_, VIDIOC_TRY_EXT_CTRLS, &ext_ctrls) < 0) {
            printf("V4l2Capture: VIDIOC_S_CTRL id=0x%08x value=%d failed: %s (ignored)\n",
                   id, value, strerror(errno));
            return false;
        } else {
            printf("V4l2Capture: VIDIOC_S_CTRL id=0x%08x value=%d not supported (ignored)\n", id, value);
            return false;
        }
    }
    printf("V4l2Capture: set control id=0x%08x value=%d\n", id, value);
    return true;
}

bool V4l2Capture::initMmap(uint32_t buffer_count) {
    if (fd_ < 0) return false;

    // 请求 MMAP 缓冲区
    struct v4l2_requestbuffers req;
    std::memset(&req, 0, sizeof(req));
    req.count = buffer_count;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    if (ioctl(fd_, VIDIOC_REQBUFS, &req) < 0) {
        printf("V4l2Capture: VIDIOC_REQBUFS failed: %s\n", strerror(errno));
        return false;
    }
    if (req.count < buffer_count) {
        printf("V4l2Capture: only %u buffers available (requested %u)\n", req.count, buffer_count);
        buffer_count = req.count;
    }
    printf("V4l2Capture: allocated %u MMAP buffers\n", (unsigned)req.count);

    buffers_.resize(buffer_count);
    for (uint32_t i = 0; i < buffer_count; ++i) {
        struct v4l2_buffer buf;
        std::memset(&buf, 0, sizeof(buf));
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;

        if (ioctl(fd_, VIDIOC_QUERYBUF, &buf) < 0) {
            printf("V4l2Capture: VIDIOC_QUERYBUF %u failed: %s\n", i, strerror(errno));
            return false;
        }

        buffers_[i].length = buf.length;
        buffers_[i].start = mmap(nullptr, buf.length,
                                 PROT_READ | PROT_WRITE, MAP_SHARED,
                                 fd_, buf.m.offset);
        if (buffers_[i].start == MAP_FAILED) {
            printf("V4l2Capture: mmap buffer %u failed: %s\n", i, strerror(errno));
            return false;
        }
        buffers_[i].queued = false;
        printf("V4l2Capture: buffer %u: addr=%p len=%zu\n",
               i, buffers_[i].start, buffers_[i].length);
    }
    return true;
}

bool V4l2Capture::startStreaming() {
    if (fd_ < 0 || streaming_) return false;

    // 把所有缓冲区入队
    for (uint32_t i = 0; i < buffers_.size(); ++i) {
        struct v4l2_buffer buf;
        std::memset(&buf, 0, sizeof(buf));
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;
        buf.bytesused = 0;  // 驱动会填充

        if (ioctl(fd_, VIDIOC_QBUF, &buf) < 0) {
            printf("V4l2Capture: VIDIOC_QBUF %u failed: %s\n", i, strerror(errno));
            return false;
        }
        buffers_[i].queued = true;
    }

    // 启动流
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(fd_, VIDIOC_STREAMON, &type) < 0) {
        printf("V4l2Capture: VIDIOC_STREAMON failed: %s\n", strerror(errno));
        return false;
    }
    streaming_ = true;
    printf("V4l2Capture: streaming started\n");
    return true;
}

bool V4l2Capture::stopStreaming() {
    if (fd_ < 0 || !streaming_) return false;

    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(fd_, VIDIOC_STREAMOFF, &type) < 0) {
        printf("V4l2Capture: VIDIOC_STREAMOFF failed: %s\n", strerror(errno));
        return false;
    }
    streaming_ = false;
    last_dequeued_ = -1;
    printf("V4l2Capture: streaming stopped\n");
    return true;
}

bool V4l2Capture::getFrame(Frame &frame, int timeout_ms) {
    if (fd_ < 0 || !streaming_) return false;

    fd_set fds;
    FD_ZERO(&fds);
    FD_SET(fd_, &fds);

    struct timeval tv;
    tv.tv_sec = timeout_ms / 1000;
    tv.tv_usec = (timeout_ms % 1000) * 1000;

    int r = select(fd_ + 1, &fds, nullptr, nullptr, &tv);
    if (r < 0) {
        if (errno == EINTR) return getFrame(frame, timeout_ms);  // 重试
        printf("V4l2Capture: select failed: %s\n", strerror(errno));
        return false;
    }
    if (r == 0) {
        // 超时
        return false;
    }

    struct v4l2_buffer buf;
    std::memset(&buf, 0, sizeof(buf));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;

    if (ioctl(fd_, VIDIOC_DQBUF, &buf) < 0) {
        if (errno == EAGAIN || errno == EIO) {
            return false;  // 再试
        }
        printf("V4l2Capture: VIDIOC_DQBUF failed: %s\n", strerror(errno));
        return false;
    }

    // 如果之前有未归还的缓冲区, 先归还
    if (last_dequeued_ >= 0) {
        releaseFrameByIndex(last_dequeued_);
    }

    uint32_t idx = buf.index;
    if (idx >= buffers_.size()) {
        printf("V4l2Capture: invalid buffer index %u\n", idx);
        return false;
    }

    last_dequeued_ = idx;
    if ((buf.flags & V4L2_BUF_FLAG_ERROR) || buf.bytesused > buffers_[idx].length) {
        releaseFrameByIndex(idx);
        last_dequeued_ = -1;
        return false;
    }

    frame.data = buffers_[idx].start;
    frame.size = buf.bytesused;
    frame.width = width_;
    frame.height = height_;
    frame.pixelformat = pixelformat_;
    frame.bytes_per_line = bytes_per_line_;
    frame.sequence = buf.sequence;
    frame.timestamp_us = (uint64_t)buf.timestamp.tv_sec * 1000000ULL + buf.timestamp.tv_usec;
    frame.index = idx;

    return true;
}

void V4l2Capture::releaseFrame(const Frame &frame) {
    if (frame.index >= 0) {
        releaseFrameByIndex(frame.index);
        last_dequeued_ = -1;
    }
}

void V4l2Capture::releaseFrameByIndex(int idx) {
    if (idx < 0 || (uint32_t)idx >= buffers_.size()) return;
    struct v4l2_buffer buf;
    std::memset(&buf, 0, sizeof(buf));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = idx;
    if (ioctl(fd_, VIDIOC_QBUF, &buf) < 0) {
        printf("V4l2Capture: VIDIOC_QBUF %d failed: %s\n", idx, strerror(errno));
    }
    buffers_[idx].queued = true;
}