#include "FrameObserver.h"
#include <string.h>
#include <stdio.h>
#include <sys/ioctl.h>
#include <unistd.h>

namespace AVT {
namespace VmbAPI {
namespace Examples {

FrameObserver::FrameObserver(const CameraPtr& pCamera) 
    : IFrameObserver(pCamera), m_videoFd(-1)
{
    if (!SetupVideoDevice()) {
        fprintf(stderr, "Failed to setup video device\n");
    }
}

FrameObserver::~FrameObserver()
{
    CloseVideoDevice();
}

bool FrameObserver::SetupVideoDevice()
{
    m_videoFd = open("/dev/video61", O_RDWR);
    if (m_videoFd >= 0) {
        // 初始化视频格式
        memset(&m_vfmt, 0, sizeof(m_vfmt));
        m_vfmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
        
        // 设置默认格式（实际分辨率将在收到第一帧时更新）
        m_vfmt.fmt.pix.width = 640;
        m_vfmt.fmt.pix.height = 480;
        m_vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
        m_vfmt.fmt.pix.field = V4L2_FIELD_NONE;
        
        if (ioctl(m_videoFd, VIDIOC_S_FMT, &m_vfmt) < 0) {
            perror("Set video format");
            close(m_videoFd);
            m_videoFd = -1;
        }

        return true;
    }
    return false;
}

void FrameObserver::CloseVideoDevice()
{
    if (m_videoFd >= 0) {
        close(m_videoFd);
        m_videoFd = -1;
    }
}

void FrameObserver::FrameReceived(const FramePtr pFrame) 
{
    std::lock_guard<std::mutex> lock(m_deviceMutex);
    if (m_videoFd < 0) return;

    // 获取帧信息和数据
    VmbUchar_t* pBuffer = nullptr;
    VmbUint32_t nSize = 0;
    VmbUint32_t width = 0, height = 0;
    VmbPixelFormatType pixelFormat;
    
    if (pFrame->GetImage(pBuffer) != VmbErrorSuccess ||
        pFrame->GetImageSize(nSize) != VmbErrorSuccess ||
        pFrame->GetWidth(width) != VmbErrorSuccess ||
        pFrame->GetHeight(height) != VmbErrorSuccess ||
        pFrame->GetPixelFormat(pixelFormat) != VmbErrorSuccess) {
        return;
    }

    // 根据相机实际格式设置V4L2格式
    m_vfmt.fmt.pix.width = width;
    m_vfmt.fmt.pix.height = height;
    
    switch (pixelFormat) {
        case VmbPixelFormatMono8:
            m_vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;  // 单色8位
            m_vfmt.fmt.pix.sizeimage = width * height;
            break;
        case VmbPixelFormatBayerRG8:
            m_vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SRGGB8;  // Bayer格式
            m_vfmt.fmt.pix.sizeimage = width * height;
            break;
        default:  // 默认按YUYV处理
            m_vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
            m_vfmt.fmt.pix.sizeimage = width * height * 2;
    }

    // 更新V4L2设备格式
    if (ioctl(m_videoFd, VIDIOC_S_FMT, &m_vfmt) < 0) {
        perror("Update video format");
        return;
    }

    // 写入数据
    ssize_t written = write(m_videoFd, pBuffer, nSize);
    if (written != (ssize_t)nSize) {
        perror("Write to video device");
    }

    m_pCamera->QueueFrame(pFrame);
}
}}} // namespace AVT::VmbAPI::Examples