#include "v4l2worker.h"

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

#include <QBuffer>
#include <QImageReader>
#include <QThread>

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

V4l2Worker::V4l2Worker(QObject *parent)
    : QObject(parent)
{}

V4l2Worker::~V4l2Worker()
{
    stop();
}

// ── public slots ──────────────────────────────────────────────────────────────

void V4l2Worker::startCapture(const QString &device, int width, int height)
{
    if (m_running.load())
        stop();

    if (!openDevice(device, width, height))
        return;

    m_running.store(true);

    // Run the blocking capture loop in a plain std::thread so the Qt event
    // loop of this worker object stays responsive to stopCapture() signals.
    m_captureThread = std::thread([this] { captureLoop(); });
}

void V4l2Worker::stopCapture()
{
    stop();
}

// ── internal stop (safe to call from any thread) ──────────────────────────────

void V4l2Worker::stop()
{
    m_running.store(false);
    if (m_captureThread.joinable())
        m_captureThread.join();   // wait for captureLoop() to exit
    closeDevice();
}

// ── capture loop (runs in m_captureThread) ────────────────────────────────────

void V4l2Worker::captureLoop()
{
    v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (xioctl(m_fd, VIDIOC_STREAMON, &type) == -1) {
        emit errorOccurred(tr("VIDIOC_STREAMON failed: %1").arg(strerror(errno)));
        m_running.store(false);
        return;
    }

    while (m_running.load()) {
        fd_set fds;
        FD_ZERO(&fds);
        FD_SET(m_fd, &fds);
        timeval tv{0, 100000};   // 100 ms timeout → checks m_running 10x/sec
        const int r = ::select(m_fd + 1, &fds, nullptr, nullptr, &tv);
        if (r == 0) continue;
        if (r == -1) {
            if (errno == EINTR) continue;
            emit errorOccurred(tr("select() failed: %1").arg(strerror(errno)));
            break;
        }

        v4l2_buffer buf{};
        buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        if (xioctl(m_fd, VIDIOC_DQBUF, &buf) == -1) {
            if (errno == EAGAIN) continue;
            emit errorOccurred(tr("VIDIOC_DQBUF failed: %1").arg(strerror(errno)));
            break;
        }

        const QImage img = convertToImage(
            m_buffers[buf.index].start, buf.bytesused, m_width, m_height);
        if (!img.isNull())
            emit newFrame(img);

        if (xioctl(m_fd, VIDIOC_QBUF, &buf) == -1) {
            emit errorOccurred(tr("VIDIOC_QBUF failed: %1").arg(strerror(errno)));
            break;
        }
    }

    xioctl(m_fd, VIDIOC_STREAMOFF, &type);
    emit captureStopped();
}

// ── device open / close ───────────────────────────────────────────────────────

bool V4l2Worker::openDevice(const QString &device, int width, int height)
{
    m_width  = width;
    m_height = height;

    m_fd = ::open(device.toLocal8Bit().constData(), O_RDWR | O_NONBLOCK);
    if (m_fd == -1) {
        emit errorOccurred(tr("Cannot open %1: %2").arg(device, strerror(errno)));
        return false;
    }

    v4l2_capability cap{};
    if (xioctl(m_fd, VIDIOC_QUERYCAP, &cap) == -1) {
        emit errorOccurred(tr("VIDIOC_QUERYCAP failed: %1").arg(strerror(errno)));
        closeDevice();
        return false;
    }
    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        emit errorOccurred(tr("%1 is not a video capture device").arg(device));
        closeDevice();
        return false;
    }

    const uint32_t preferred[] = {
        V4L2_PIX_FMT_MJPEG,
        V4L2_PIX_FMT_YUYV,
        V4L2_PIX_FMT_NV12,
        0
    };

    m_pixFmt = 0;
    for (int i = 0; preferred[i]; ++i) {
        v4l2_format fmt{};
        fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        fmt.fmt.pix.width       = static_cast<uint32_t>(width);
        fmt.fmt.pix.height      = static_cast<uint32_t>(height);
        fmt.fmt.pix.pixelformat = preferred[i];
        fmt.fmt.pix.field       = V4L2_FIELD_ANY;
        if (xioctl(m_fd, VIDIOC_S_FMT, &fmt) == 0) {
            m_pixFmt = fmt.fmt.pix.pixelformat;
            m_width  = static_cast<int>(fmt.fmt.pix.width);
            m_height = static_cast<int>(fmt.fmt.pix.height);
            break;
        }
    }
    if (!m_pixFmt) {
        emit errorOccurred(tr("No supported pixel format found on %1").arg(device));
        closeDevice();
        return false;
    }

    QString fmtName;
    switch (m_pixFmt) {
        case V4L2_PIX_FMT_MJPEG: fmtName = "MJPEG"; break;
        case V4L2_PIX_FMT_YUYV:  fmtName = "YUYV";  break;
        case V4L2_PIX_FMT_NV12:  fmtName = "NV12";  break;
        default:                  fmtName = "?";     break;
    }

    if (!initMmap()) {
        closeDevice();
        return false;
    }

    emit captureStarted(device, m_width, m_height, fmtName);
    return true;
}

void V4l2Worker::closeDevice()
{
    freeMmap();
    if (m_fd != -1) {
        ::close(m_fd);
        m_fd = -1;
    }
}

bool V4l2Worker::initMmap()
{
    v4l2_requestbuffers req{};
    req.count  = MMAP_BUFFERS;
    req.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;
    if (xioctl(m_fd, VIDIOC_REQBUFS, &req) == -1) {
        emit errorOccurred(tr("VIDIOC_REQBUFS failed: %1").arg(strerror(errno)));
        return false;
    }
    m_nBuffers = static_cast<int>(req.count);

    for (int i = 0; i < m_nBuffers; ++i) {
        v4l2_buffer buf{};
        buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index  = static_cast<uint32_t>(i);
        if (xioctl(m_fd, VIDIOC_QUERYBUF, &buf) == -1) {
            emit errorOccurred(tr("VIDIOC_QUERYBUF failed: %1").arg(strerror(errno)));
            return false;
        }
        m_buffers[i].length = buf.length;
        m_buffers[i].start  = ::mmap(nullptr, buf.length,
                                      PROT_READ | PROT_WRITE, MAP_SHARED,
                                      m_fd, buf.m.offset);
        if (m_buffers[i].start == MAP_FAILED) {
            emit errorOccurred(tr("mmap failed: %1").arg(strerror(errno)));
            return false;
        }
        if (xioctl(m_fd, VIDIOC_QBUF, &buf) == -1) {
            emit errorOccurred(tr("VIDIOC_QBUF failed: %1").arg(strerror(errno)));
            return false;
        }
    }
    return true;
}

void V4l2Worker::freeMmap()
{
    for (int i = 0; i < m_nBuffers; ++i) {
        if (m_buffers[i].start && m_buffers[i].start != MAP_FAILED) {
            ::munmap(m_buffers[i].start, m_buffers[i].length);
            m_buffers[i].start = nullptr;
        }
    }
    m_nBuffers = 0;
}

QImage V4l2Worker::convertToImage(const void *data, size_t size, int width, int height)
{
    switch (m_pixFmt) {
    case V4L2_PIX_FMT_MJPEG: {
        QByteArray ba(static_cast<const char *>(data), static_cast<int>(size));
        QBuffer buf(&ba);
        QImageReader reader(&buf, "JPEG");
        return reader.read();
    }
    case V4L2_PIX_FMT_YUYV:
        return yuyv2Rgb(static_cast<const uchar *>(data), width, height);
    case V4L2_PIX_FMT_NV12:
        return nv122Rgb(static_cast<const uchar *>(data), width, height);
    default:
        return {};
    }
}

QImage V4l2Worker::yuyv2Rgb(const uchar *src, int width, int height)
{
    QImage img(width, height, QImage::Format_RGB32);
    for (int y = 0; y < height; ++y) {
        const uchar *row = src + y * width * 2;
        QRgb *dst = reinterpret_cast<QRgb *>(img.scanLine(y));
        for (int x = 0; x < width; x += 2) {
            const int Y0 = row[0], U = row[1]-128, Y1 = row[2], V = row[3]-128;
            row += 4;
            auto clamp = [](int v) { return static_cast<uchar>(v<0?0:v>255?255:v); };
            dst[x]   = qRgb(clamp(Y0+1.402f*V), clamp(Y0-0.344f*U-0.714f*V), clamp(Y0+1.772f*U));
            dst[x+1] = qRgb(clamp(Y1+1.402f*V), clamp(Y1-0.344f*U-0.714f*V), clamp(Y1+1.772f*U));
        }
    }
    return img;
}

QImage V4l2Worker::nv122Rgb(const uchar *src, int width, int height)
{
    QImage img(width, height, QImage::Format_RGB32);
    const uchar *uvPlane = src + width * height;
    auto clamp = [](int v) { return static_cast<uchar>(v<0?0:v>255?255:v); };
    for (int y = 0; y < height; ++y) {
        const uchar *yRow  = src + y * width;
        const uchar *uvRow = uvPlane + (y/2) * width;
        QRgb *dst = reinterpret_cast<QRgb *>(img.scanLine(y));
        for (int x = 0; x < width; ++x) {
            const int Y = yRow[x], U = uvRow[x&~1]-128, V = uvRow[(x&~1)+1]-128;
            dst[x] = qRgb(clamp(Y+1.402f*V), clamp(Y-0.344f*U-0.714f*V), clamp(Y+1.772f*U));
        }
    }
    return img;
}