#include "camera.hpp"
#include "uyvy_converter.hpp"
#include <linux/videodev2.h>
#include <sys/eventfd.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <poll.h>
#include <fcntl.h>
#include <unistd.h>
#include <time.h>
#include <cerrno>
#include <cstring>
#include <limits>
#include <stdexcept>

namespace cms {
std::uint64_t monotonicNs() {
    timespec time{};
    if(clock_gettime(CLOCK_MONOTONIC,&time)!=0)throw std::runtime_error("clock_gettime failed");
    return std::uint64_t(time.tv_sec)*1000000000ULL+time.tv_nsec;
}
namespace {
void checkedIoctl(int fd,unsigned long request,void* argument,char const* operation) {
    int status;
    do {status=ioctl(fd,request,argument);} while(status<0&&errno==EINTR);
    if(status<0)throw std::runtime_error(std::string(operation)+": "+std::strerror(errno));
}
}
struct Camera::Impl {
    struct Buffer {void* data{};std::size_t size{};};
    CameraConfig config;
    int fd{-1},stopFd{-1};bool streaming{};
    std::uint32_t stride{},lastSequence{};bool haveSequence{};
    std::uint64_t index{};
    std::uint32_t consecutiveErrors{};
    std::atomic<bool> stopped{false};
    std::vector<Buffer> buffers;
    std::unique_ptr<UyvyConverter> converter;
    explicit Impl(CameraConfig c):config(std::move(c)) {
        if(config.width<=0||config.width>8192||config.height<=0||config.height>8192||config.width%2||config.fps<=0||config.fps>240||config.buffers<2||config.buffers>32||config.timeout_ms<1)
            throw std::invalid_argument("Invalid camera configuration");
        try {
            stopFd=eventfd(0,EFD_CLOEXEC|EFD_NONBLOCK);
            if(stopFd<0)throw std::runtime_error("Cannot create camera stop event");
            fd=open(config.device.c_str(),O_RDWR|O_NONBLOCK|O_CLOEXEC);
            if(fd<0)throw std::runtime_error("Cannot open "+config.device+": "+std::strerror(errno));
            v4l2_capability capability{};checkedIoctl(fd,VIDIOC_QUERYCAP,&capability,"VIDIOC_QUERYCAP");
            auto caps=(capability.capabilities&V4L2_CAP_DEVICE_CAPS)?capability.device_caps:capability.capabilities;
            if(!(caps&V4L2_CAP_VIDEO_CAPTURE)||!(caps&V4L2_CAP_STREAMING))throw std::runtime_error("Camera does not support single-plane streaming capture");
            v4l2_format format{};format.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
            format.fmt.pix.width=config.width;format.fmt.pix.height=config.height;
            format.fmt.pix.pixelformat=V4L2_PIX_FMT_UYVY;format.fmt.pix.field=V4L2_FIELD_NONE;
            checkedIoctl(fd,VIDIOC_S_FMT,&format,"VIDIOC_S_FMT");
            if(format.fmt.pix.pixelformat!=V4L2_PIX_FMT_UYVY||format.fmt.pix.width!=static_cast<unsigned>(config.width)||format.fmt.pix.height!=static_cast<unsigned>(config.height))
                throw std::runtime_error("Camera rejected the configured UYVY resolution; refusing mismatched mask coordinates");
            stride=format.fmt.pix.bytesperline;
            if(stride<static_cast<unsigned>(config.width*2))throw std::runtime_error("Invalid camera row stride");
            v4l2_streamparm params{};params.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
            if(ioctl(fd,VIDIOC_G_PARM,&params)==0&&(params.parm.capture.capability&V4L2_CAP_TIMEPERFRAME)) {
                params.parm.capture.timeperframe.numerator=1;params.parm.capture.timeperframe.denominator=config.fps;
                checkedIoctl(fd,VIDIOC_S_PARM,&params,"VIDIOC_S_PARM");
                auto const& interval=params.parm.capture.timeperframe;
                if(!interval.numerator||interval.denominator!=static_cast<unsigned>(config.fps)*interval.numerator)
                    throw std::runtime_error("Camera rejected requested frame rate");
            }
            v4l2_requestbuffers request{};request.count=config.buffers;
            request.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;request.memory=V4L2_MEMORY_MMAP;
            checkedIoctl(fd,VIDIOC_REQBUFS,&request,"VIDIOC_REQBUFS");
            if(request.count<2)throw std::runtime_error("Insufficient V4L2 buffers");
            buffers.resize(request.count);
            for(unsigned i=0;i<request.count;++i) {
                v4l2_buffer buffer{};buffer.type=request.type;buffer.memory=request.memory;buffer.index=i;
                checkedIoctl(fd,VIDIOC_QUERYBUF,&buffer,"VIDIOC_QUERYBUF");
                if(buffer.length<std::size_t(stride)*config.height)throw std::runtime_error("Camera buffer smaller than negotiated frame");
                buffers[i].size=buffer.length;
                auto mapping=mmap(nullptr,buffer.length,PROT_READ|PROT_WRITE,MAP_SHARED,fd,buffer.m.offset);
                if(mapping==MAP_FAILED)throw std::runtime_error("Camera mmap failed");
                buffers[i].data=mapping;checkedIoctl(fd,VIDIOC_QBUF,&buffer,"VIDIOC_QBUF");
            }
            converter=std::make_unique<UyvyConverter>(config.width,config.height);
            auto type=V4L2_BUF_TYPE_VIDEO_CAPTURE;checkedIoctl(fd,VIDIOC_STREAMON,&type,"VIDIOC_STREAMON");streaming=true;
        } catch(...) {release();throw;}
    }
    ~Impl(){release();}
    void release() noexcept {
        if(streaming){auto type=V4L2_BUF_TYPE_VIDEO_CAPTURE;ioctl(fd,VIDIOC_STREAMOFF,&type);streaming=false;}
        for(auto const& b:buffers)if(b.data)munmap(b.data,b.size);
        buffers.clear();
        if(fd>=0){close(fd);fd=-1;}
        if(stopFd>=0){close(stopFd);stopFd=-1;}
    }
    void stop() noexcept {
        stopped=true;std::uint64_t one=1;
        if(stopFd>=0){auto ignored=write(stopFd,&one,sizeof(one));(void)ignored;}
    }
    std::optional<CapturedFrame> readFrame() {
        while(!stopped.load()) {
            pollfd descriptors[2]={{fd,POLLIN,0},{stopFd,POLLIN,0}};
            int ready=poll(descriptors,2,config.timeout_ms);
            if(ready<0&&errno==EINTR)continue;
            if(stopped.load()||(descriptors[1].revents&POLLIN))return {};
            if(ready==0)throw std::runtime_error("Camera capture timeout: "+config.device);
            if(ready<0||(descriptors[0].revents&(POLLERR|POLLHUP|POLLNVAL)))throw std::runtime_error("Camera disconnected or poll failed");
            v4l2_buffer buffer{};buffer.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;buffer.memory=V4L2_MEMORY_MMAP;
            int status=ioctl(fd,VIDIOC_DQBUF,&buffer);
            if(status<0&&(errno==EAGAIN||errno==EINTR))continue;
            if(status<0)throw std::runtime_error("VIDIOC_DQBUF: "+std::string(std::strerror(errno)));
            if(buffer.index>=buffers.size())throw std::runtime_error("Invalid V4L2 buffer index");
            // The GMSL driver can mark its startup buffer as corrupt. Requeue
            // it without conversion or inference; persistent corruption is fatal.
            if(buffer.flags&V4L2_BUF_FLAG_ERROR) {
                checkedIoctl(fd,VIDIOC_QBUF,&buffer,"VIDIOC_QBUF(error frame)");
                if(++consecutiveErrors>=static_cast<unsigned>(config.fps))
                    throw std::runtime_error("Camera produced one second of consecutive error frames");
                continue;
            }
            CapturedFrame frame;
            try {
                if(buffer.bytesused<std::size_t(stride)*config.height||buffer.bytesused>buffers[buffer.index].size)
                    throw std::runtime_error("Invalid camera frame: flags="+std::to_string(buffer.flags)+", used="+std::to_string(buffer.bytesused)+", required="+std::to_string(std::size_t(stride)*config.height)+", allocated="+std::to_string(buffers[buffer.index].size));
                auto& m=frame.metadata;
                m.discarded_error_frames=consecutiveErrors;consecutiveErrors=0;
                m.driver_sequence=buffer.sequence;m.received_monotonic_ns=monotonicNs();
                m.driver_timestamp_ns=std::uint64_t(buffer.timestamp.tv_sec)*1000000000ULL+std::uint64_t(buffer.timestamp.tv_usec)*1000;
                m.driver_timestamp_monotonic=(buffer.flags&V4L2_BUF_FLAG_TIMESTAMP_MASK)==V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
                m.capture_monotonic_ns=m.received_monotonic_ns; // Host dequeue clock; retain raw driver timestamp separately.
                if(haveSequence) {
                    auto delta=static_cast<std::uint32_t>(buffer.sequence-lastSequence);
                    if(delta==0||delta>0x7fffffffU)throw std::runtime_error("Camera sequence reset or repeated frame");
                    m.skipped_driver_frames=delta-1;index+=delta;
                }
                m.frame_index=index;
                lastSequence=buffer.sequence;haveSequence=true;
                // Convert before requeuing. No published result references MMAP memory.
                frame.image=converter->convert(buffers[buffer.index].data,stride);
            } catch(...) {ioctl(fd,VIDIOC_QBUF,&buffer);throw;}
            checkedIoctl(fd,VIDIOC_QBUF,&buffer,"VIDIOC_QBUF");return frame;
        }
        return {};
    }
};
Camera::Camera(CameraConfig config):impl_(std::make_unique<Impl>(std::move(config))){}
Camera::~Camera()=default;
std::optional<CapturedFrame> Camera::read(){return impl_->readFrame();}
void Camera::requestStop() noexcept {impl_->stop();}
}