#include "VrCameraSimulator.h"
#include "LaserDataLoader.h"

#include <QDebug>
#include <QDir>
#include <QFileInfo>
#include <QDirIterator>

#include <cstring>
#include <random>
#include <chrono>
#include <algorithm>

namespace {

/// Enumerate local adapters and return the first non-loopback IPv4 address.
/// Used so the discovery response advertises an IP the SDK can actually
/// connect to on TCP 6679 (getsockname on an INADDR_ANY socket gives 0.0.0.0).
bool GetFirstLocalIPv4(unsigned char out[4])
{
#ifdef _WIN32
    char hostname[256] = {};
    if (gethostname(hostname, sizeof(hostname)) != 0)
        return false;

    addrinfo hints = {};
    hints.ai_family = AF_INET;
    hints.ai_socktype = SOCK_DGRAM;
    hints.ai_flags = AI_PASSIVE;

    addrinfo* pResult = nullptr;
    if (getaddrinfo(hostname, nullptr, &hints, &pResult) != 0)
        return false;

    bool found = false;
    for (addrinfo* p = pResult; p != nullptr; p = p->ai_next) {
        if (p->ai_family == AF_INET) {
            const sockaddr_in* sa = reinterpret_cast<const sockaddr_in*>(p->ai_addr);
            const unsigned long ip = ntohl(sa->sin_addr.s_addr);
            // Skip loopback (127.x.x.x) and link-local (169.254.x.x).
            if (ip != 0x7f000001 && (ip & 0xffff0000) != 0xa9fe0000) {
                out[0] = static_cast<unsigned char>((ip >> 24) & 0xFF);
                out[1] = static_cast<unsigned char>((ip >> 16) & 0xFF);
                out[2] = static_cast<unsigned char>((ip >> 8) & 0xFF);
                out[3] = static_cast<unsigned char>(ip & 0xFF);
                found = true;
                break;
            }
        }
    }
    freeaddrinfo(pResult);
    return found;
#else
    (void)out;
    return false;
#endif
}

} // namespace

// ============================================================================
//  Constructor / Destructor
// ============================================================================

VrCameraSimulator::VrCameraSimulator(QObject* parent)
    : QObject(parent)
{
    InitWinsock();
    InitRegisterMap();

    // Generate a deterministic-but-unique serial number and MAC
    m_deviceIP = QString("%1.%2.%3.%4")
        .arg(m_deviceIPBytes[0]).arg(m_deviceIPBytes[1])
        .arg(m_deviceIPBytes[2]).arg(m_deviceIPBytes[3]);

    m_deviceMAC = QString("%1:%2:%3:%4:%5:%6")
        .arg(m_deviceMACBytes[0], 2, 16, QChar('0'))
        .arg(m_deviceMACBytes[1], 2, 16, QChar('0'))
        .arg(m_deviceMACBytes[2], 2, 16, QChar('0'))
        .arg(m_deviceMACBytes[3], 2, 16, QChar('0'))
        .arg(m_deviceMACBytes[4], 2, 16, QChar('0'))
        .arg(m_deviceMACBytes[5], 2, 16, QChar('0'));

    m_serialNumber = QString::fromLatin1(
        reinterpret_cast<const char*>(m_deviceSNBytes), 8);

    // Default calibration matrix: identity 4x4 (16 doubles)
    m_calibMatrix.resize(16 * sizeof(double));
    double* mat = reinterpret_cast<double*>(m_calibMatrix.data());
    for (int i = 0; i < 16; i++) mat[i] = (i % 5 == 0) ? 1.0 : 0.0;
}

VrCameraSimulator::~VrCameraSimulator()
{
    Stop();
}

// ============================================================================
//  Configuration
// ============================================================================

void VrCameraSimulator::SetImageWidth(int w)  { if (w > 0 && w <= 4096) m_imageWidth = w; }
void VrCameraSimulator::SetImageHeight(int h) { if (h > 0 && h <= 4096) m_imageHeight = h; }
void VrCameraSimulator::SetFrameRate(int fps) { if (fps > 0 && fps <= 120) m_frameRate = fps; }

// ============================================================================
//  Lifecycle: Start / Stop
// ============================================================================

bool VrCameraSimulator::Start()
{
    if (m_running) return true;

    // Create UDP socket for discovery responder
    if (!CreateUdpSocket()) {
        emit LogMessage("Failed to bind UDP port 6789", true);
        return false;
    }

    // Create TCP listening socket for command channel
    if (!CreateTcpListenSocket()) {
        emit LogMessage("Failed to bind TCP port 6679", true);
        closesocket(m_udpSocket);
        m_udpSocket = INVALID_SOCKET;
        return false;
    }

    m_running = true;

    // Launch threads
    m_udpThread   = std::thread(&VrCameraSimulator::UdpListenerLoop, this);
    m_tcpThread   = std::thread(&VrCameraSimulator::TcpServerLoop, this);

    emit StatusChanged("Running");
    emit LogMessage(QString("Virtual camera started on %1 (UDP:%2, TCP:%3)")
                    .arg(m_deviceIP).arg(m_udpPort).arg(m_tcpPort));
    return true;
}

void VrCameraSimulator::Stop()
{
    if (!m_running) return;

    m_running  = false;
    m_streaming = false;

    // Close all sockets to unblock the server/client threads.
    {
        std::lock_guard<std::mutex> lock(m_clientMutex);
        for (SOCKET s : m_clientSockets) {
            closesocket(s);
        }
        m_clientSockets.clear();
        if (m_tcpClientSocket != INVALID_SOCKET) {
            closesocket(m_tcpClientSocket);
            m_tcpClientSocket = INVALID_SOCKET;
        }
    }
    if (m_tcpListenSocket != INVALID_SOCKET) {
        closesocket(m_tcpListenSocket);
        m_tcpListenSocket = INVALID_SOCKET;
    }
    if (m_udpSocket != INVALID_SOCKET) {
        closesocket(m_udpSocket);
        m_udpSocket = INVALID_SOCKET;
    }

    // Join threads
    if (m_udpThread.joinable())    m_udpThread.join();
    if (m_tcpThread.joinable())    m_tcpThread.join();
    if (m_clientThread.joinable()) m_clientThread.join();
    if (m_streamThread.joinable()) m_streamThread.join();

    m_connectedClients = 0;
    emit StatusChanged("Stopped");
    emit LogMessage("Virtual camera stopped");
}

// ============================================================================
//  Winsock Initialization
// ============================================================================

bool VrCameraSimulator::InitWinsock()
{
#ifdef _WIN32
    WSADATA wsaData;
    return WSAStartup(MAKEWORD(2, 2), &wsaData) == 0;
#else
    return true;
#endif
}

void VrCameraSimulator::SetSocketReuseAddr(SOCKET s)
{
    int opt = 1;
    setsockopt(s, SOL_SOCKET, SO_REUSEADDR,
#ifdef _WIN32
               reinterpret_cast<const char*>(&opt), sizeof(opt));
#else
               &opt, sizeof(opt));
#endif
}

bool VrCameraSimulator::CreateUdpSocket()
{
    m_udpSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
    if (m_udpSocket == INVALID_SOCKET) return false;

    SetSocketReuseAddr(m_udpSocket);

    // Enable broadcast reception
    int broadcast = 1;
    setsockopt(m_udpSocket, SOL_SOCKET, SO_BROADCAST,
#ifdef _WIN32
               reinterpret_cast<const char*>(&broadcast), sizeof(broadcast));
#else
               &broadcast, sizeof(broadcast));
#endif

    sockaddr_in addr = {};
    addr.sin_family      = AF_INET;
    addr.sin_port        = htons(static_cast<uint16_t>(m_udpPort));
    addr.sin_addr.s_addr = INADDR_ANY;

    if (bind(m_udpSocket, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) == SOCKET_ERROR) {
        closesocket(m_udpSocket);
        m_udpSocket = INVALID_SOCKET;
        return false;
    }

    return true;
}

bool VrCameraSimulator::CreateTcpListenSocket()
{
    m_tcpListenSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
    if (m_tcpListenSocket == INVALID_SOCKET) return false;

    SetSocketReuseAddr(m_tcpListenSocket);

    sockaddr_in addr = {};
    addr.sin_family      = AF_INET;
    addr.sin_port        = htons(static_cast<uint16_t>(m_tcpPort));
    addr.sin_addr.s_addr = INADDR_ANY;

    if (bind(m_tcpListenSocket, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) == SOCKET_ERROR) {
        closesocket(m_tcpListenSocket);
        m_tcpListenSocket = INVALID_SOCKET;
        return false;
    }

    if (listen(m_tcpListenSocket, 10) == SOCKET_ERROR) {
        closesocket(m_tcpListenSocket);
        m_tcpListenSocket = INVALID_SOCKET;
        return false;
    }

    return true;
}

// ============================================================================
//  UDP Discovery Listener Thread
// ============================================================================

void VrCameraSimulator::UdpListenerLoop()
{
    emit LogMessage("UDP discovery listener started on port " + QString::number(m_udpPort));

    // Set receive timeout so we can check m_running periodically
#ifdef _WIN32
    DWORD timeout = 500;
    setsockopt(m_udpSocket, SOL_SOCKET, SO_RCVTIMEO,
               reinterpret_cast<const char*>(&timeout), sizeof(timeout));
#else
    struct timeval tv = {0, 500000};
    setsockopt(m_udpSocket, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
#endif

    while (m_running) {
        sockaddr_in senderAddr = {};
        socklen_t addrLen = sizeof(senderAddr);

        int n = recvfrom(m_udpSocket,
#ifdef _WIN32
                         reinterpret_cast<char*>(m_udpRecvBuf),
#else
                         m_udpRecvBuf,
#endif
                         UDP_BUF_SIZE, 0,
                         reinterpret_cast<sockaddr*>(&senderAddr), &addrLen);

        if (n <= 0) {
            // Timeout or error — loop back to check m_running
            continue;
        }

        // Check for discovery request: preamble 0xe1 0xe2 0xe3 0xe4
        if (n >= 34 && m_udpRecvBuf[0] == 0xe1 && m_udpRecvBuf[1] == 0xe2 &&
            m_udpRecvBuf[2] == 0xe3 && m_udpRecvBuf[3] == 0xe4) {

            // Check for "SB" packet start marker at offset 12
            if (m_udpRecvBuf[12] == 0x53 && m_udpRecvBuf[13] == 0x42) {
                // cmdType at offset 14-15 (big-endian)
                uint16_t cmdType = (static_cast<uint16_t>(m_udpRecvBuf[14]) << 8)
                                 |  static_cast<uint16_t>(m_udpRecvBuf[15]);

                if (cmdType == 0x0002) {  // SearchDev
                    char senderIP[INET_ADDRSTRLEN];
                    inet_ntop(AF_INET, &senderAddr.sin_addr, senderIP, sizeof(senderIP));

                    emit LogMessage(QString("Discovery request from %1").arg(senderIP));

                    QByteArray response = BuildDiscoveryResponse(
                        QByteArray(reinterpret_cast<const char*>(m_udpRecvBuf), n),
                        senderAddr);

                    if (!response.isEmpty()) {
                        sendto(m_udpSocket, response.constData(), response.size(), 0,
                               reinterpret_cast<const sockaddr*>(&senderAddr), sizeof(senderAddr));
                        emit LogMessage("Sent discovery response to " + QString(senderIP));
                    }
                }
            }
        }
    }

    emit LogMessage("UDP listener stopped");
}

// ============================================================================
//  TCP Server Thread (accept loop)
// ============================================================================

void VrCameraSimulator::TcpServerLoop()
{
    emit LogMessage("TCP server listening on port " + QString::number(m_tcpPort));

    // Set accept timeout so we can poll m_running.
#ifdef _WIN32
    DWORD timeout = 500;
    setsockopt(m_tcpListenSocket, SOL_SOCKET, SO_RCVTIMEO,
               reinterpret_cast<const char*>(&timeout), sizeof(timeout));
#else
    struct timeval tv = {0, 500000};
    setsockopt(m_tcpListenSocket, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
#endif

    while (m_running) {
        sockaddr_in clientAddr = {};
        socklen_t addrLen = sizeof(clientAddr);

        SOCKET clientSock = accept(m_tcpListenSocket,
                                   reinterpret_cast<sockaddr*>(&clientAddr),
                                   &addrLen);

        if (clientSock == INVALID_SOCKET) {
            // Timeout or error — loop back to check m_running
            continue;
        }

        char clientIP[INET_ADDRSTRLEN];
        inet_ntop(AF_INET, &clientAddr.sin_addr, clientIP, sizeof(clientIP));

        emit LogMessage(QString("TCP client connected from %1").arg(clientIP));

        {
            std::lock_guard<std::mutex> lock(m_clientMutex);
            // Replace the current push target but keep old client sockets alive
            // so they can still receive their own heartbeat responses.
            m_tcpClientSocket = clientSock;
            m_clientSockets.push_back(clientSock);
            m_connectedClients = static_cast<int>(m_clientSockets.size());
        }

        m_frameSeq = 0;
        emit ClientConnected(QString(clientIP));
        emit StatusChanged("Client connected");

        // Handle this client on its own thread so a slow/stuck client never
        // blocks accept() for new connections.
        std::thread(&VrCameraSimulator::TcpClientLoop, this, clientSock).detach();
    }
}

void VrCameraSimulator::RemoveClientSocket(SOCKET s)
{
    std::lock_guard<std::mutex> lock(m_clientMutex);
    auto it = std::find(m_clientSockets.begin(), m_clientSockets.end(), s);
    if (it != m_clientSockets.end()) {
        m_clientSockets.erase(it);
    }
    if (m_tcpClientSocket == s) {
        m_tcpClientSocket = INVALID_SOCKET;
    }
}

// ============================================================================
//  TCP Client Handler
// ============================================================================

void VrCameraSimulator::TcpClientLoop(SOCKET clientSock)
{
    uint8_t buf[TCP_BUF_SIZE];

    // On Windows an accepted socket INHERITS the listen socket's SO_RCVTIMEO
    // (500 ms here). SDK heartbeats arrive every few seconds, so a short recv
    // timeout makes us drop a healthy client. Clear the timeout so recv blocks
    // indefinitely; Stop() closes this socket to wake recv.
#ifdef _WIN32
    DWORD clientTimeout = 0;
    setsockopt(clientSock, SOL_SOCKET, SO_RCVTIMEO,
               reinterpret_cast<const char*>(&clientTimeout), sizeof(clientTimeout));
#else
    struct timeval clientTimeout = {0, 0};
    setsockopt(clientSock, SOL_SOCKET, SO_RCVTIMEO, &clientTimeout, sizeof(clientTimeout));
#endif

    // Each client thread keeps its own frame buffer.
    std::vector<uint8_t> recvBuf(TCP_BUF_SIZE);
    int recvPos = 0;

    while (m_running) {
        int n = recv(clientSock,
#ifdef _WIN32
                     reinterpret_cast<char*>(buf),
#else
                     buf,
#endif
                     TCP_BUF_SIZE, 0);

        if (n <= 0) {
            emit LogMessage("TCP client disconnected (recv returned " + QString::number(n) + ")");
            break;  // Connection closed or error
        }

        // Append new data to this client's receive buffer
        if (recvPos + n > TCP_BUF_SIZE) {
            recvPos = 0;  // safety: drop stale data
        }
        memcpy(recvBuf.data() + recvPos, buf, n);
        recvPos += n;

        // Try to parse complete frames from the buffer
        while (m_running) {
            TcpOperaType op;
            uint32_t cmd, seq;
            QByteArray payload;
            int consumed = 0;

            bool parsed = ParseVzebFrame(recvBuf.data(), recvPos, consumed,
                                         op, cmd, seq, payload);
            if (parsed && consumed > 0) {
                // Shift remaining data
                int remaining = recvPos - consumed;
                if (remaining > 0) {
                    memmove(recvBuf.data(), recvBuf.data() + consumed, remaining);
                }
                recvPos = remaining;
            }

            if (!parsed) break;

            if (op == TcpOperaType::Request) {
                emit LogMessage(QString("TCP Cmd %1 seq=%2 payload=%3 bytes")
                                .arg(cmd).arg(seq).arg(payload.size()));

                QByteArray response = HandleCommand(cmd, seq, payload);

                if (!response.isEmpty()) {
                    std::lock_guard<std::mutex> lock(m_sendMutex);
                    int sent = send(clientSock, response.constData(), response.size(), 0);
                    if (sent <= 0) {
                        emit LogMessage("TCP send failed", true);
                    }
                }
            }
            // Post (push) messages from camera are handled in StreamLoop
        }
    }

    // Clean up this client's socket.
    RemoveClientSocket(clientSock);
    closesocket(clientSock);
}

// ============================================================================
//  Stream Thread
// ============================================================================

void VrCameraSimulator::StreamLoop()
{
    emit LogMessage("Stream engine started");

    int frameIdx = 0;
    bool lastLine = false;
    using Clock = std::chrono::steady_clock;
    auto nextFrameTime = Clock::now();

    while (m_running && m_streaming) {
        // Grab the next laser line (cycles through loaded files; fall back to
        // an empty line if no laser data was loaded). GetNextLaserLine locks
        // the laser-data mutex itself.
        CachedLaserLine line;
        if (!m_laserFileLines.empty()) {
            line = GetNextLaserLine();
        }

        // Grab the current stereo image pair (loaded or checkerboard fallback).
        QImage leftImg, rightImg;
        {
            std::lock_guard<std::mutex> lock(m_imageMutex);
            if (!m_leftImages.empty()) {
                int idx = frameIdx % static_cast<int>(m_leftImages.size());
                leftImg  = m_leftImages[idx];
                rightImg = (idx < static_cast<int>(m_rightImages.size()))
                           ? m_rightImages[idx] : m_rightImages[0];
            }
        }
        if (leftImg.isNull()) {
            leftImg  = GenerateCheckerboardQImage(frameIdx, 0, 0);
            rightImg = GenerateCheckerboardQImage(frameIdx, 32, 32);
        }

        emit ImageGenerated(leftImg, rightImg);

        // Mark the last line of a scan cycle so the app sees bEndOnceScan.
        if (m_scanLineIdx > 0 && m_scanLineIdx % m_scanLinesPerCycle == 0) {
            lastLine = true;
        } else {
            lastLine = false;
        }

        // Build and push the SDK-compatible 3D laser frame.
        QByteArray frame = BuildLaserFrame(line, frameIdx, leftImg, rightImg, lastLine);
        if (!frame.isEmpty()) {
            SendPostCommand(Cmd_PushLaserResult, frame);
        }

        m_frameSeq = frameIdx;
        m_scanLineIdx++;
        frameIdx++;

        // Frame rate pacing
        nextFrameTime += std::chrono::milliseconds(1000 / (std::max)(1, m_frameRate));
        auto now = Clock::now();
        if (nextFrameTime > now) {
            std::this_thread::sleep_until(nextFrameTime);
        } else {
            nextFrameTime = now + std::chrono::milliseconds(1000 / (std::max)(1, m_frameRate));
        }
    }

    emit LogMessage("Stream engine stopped");
}

// ============================================================================
//  3D Laser Frame Builder (SDK-compatible PushLaserResult payload)
// ============================================================================
// Layout (laser product, n3DTotleHeadLength=256, n3DFrameHeadLength=104):
//   [0..139]  index head (140 = 256 - 20 - 96). pOffsetVal[0]=left image
//             offset, [1]=right image offset, [2]=3D offset (all relative to
//             pFrameInfoData = payload + 140).
//   [140..]   pFrameInfoData:
//     [+0]     frame idx  (uint; low 16 bits since LaserDataVersion>=1)
//     [+4]     label = 4  (uint)
//     [+8]     encode info (uint): low 16 = encodeNo, bit0x00010000 = last line
//     [+12]    timestamp  (uint)
//     [+16]    RX (ushort)
//     [+18]    LX (ushort)
//     [+20]    height (ushort)
//     [+22]    width  (ushort)
//     [+24]    4 unread bytes
//     [+28]    nPointCnt (ushort)
//     [+30]    Y (ushort)
//     [+32..63]  left image info (label=1)
//     [+64..95]  right image info (label=2)
//     [+96..127] center image info (zeros)
//     [+128..]   data area:
//       3D points (nPointCnt * 16, each x/y/z float + nUV uint)
//       left image pixels (h*w, 8-bit gray)
//       right image pixels (h*w, 8-bit gray)
QByteArray VrCameraSimulator::BuildLaserFrame(const CachedLaserLine& line, int frameIdx,
                                              const QImage& leftImg, const QImage& rightImg,
                                              bool lastLine)
{
    const int nIndexHeadSize = 140;  // 256 - 20 - 96
    const int nInfoHeadSize  = 128;  // 3D info(32) + left(32) + right(32) + center(32)
    const int n3DOffset      = nInfoHeadSize;  // 128, relative to pFrameInfoData

    // Clamp point count to SDK's VZ_MAX_POINT_COUNT (3072).
    int nPointCnt = line.pointCount;
    if (nPointCnt > 3072) nPointCnt = 3072;
    if (nPointCnt < 0)    nPointCnt = 0;

    // Image pixel size (clamp to something small so frames stay manageable).
    int imgW = leftImg.width();
    int imgH = leftImg.height();
    if (imgW <= 0 || imgH <= 0) { imgW = 320; imgH = 240; }
    if (imgW > 960) imgW = 960;
    if (imgH > 960) imgH = 960;
    const int imgSize = imgW * imgH;

    const int nLeftImageOffset  = n3DOffset + nPointCnt * 16;
    const int nRightImageOffset = nLeftImageOffset + imgSize;

    const int totalFrameSize = nIndexHeadSize + nInfoHeadSize + nPointCnt * 16 + 2 * imgSize;

    QByteArray frame(totalFrameSize, 0);
    uint8_t* d = reinterpret_cast<uint8_t*>(frame.data());

    // ---- Index head (offsets relative to pFrameInfoData) ----
    uint32_t* pOffsetVal = reinterpret_cast<uint32_t*>(d);
    pOffsetVal[0] = static_cast<uint32_t>(nLeftImageOffset);
    pOffsetVal[1] = static_cast<uint32_t>(nRightImageOffset);
    pOffsetVal[2] = static_cast<uint32_t>(n3DOffset);

    uint8_t* pInfo = d + nIndexHeadSize;   // pFrameInfoData

    // ---- 3D info header ----
    uint32_t ts = static_cast<uint32_t>(m_frameSeq * 1000 + frameIdx);
    uint32_t encodeInfo = (static_cast<uint32_t>(frameIdx) & 0xffff) |
                          (lastLine ? 0x00010000u : 0u);
    uint32_t fIdx16 = static_cast<uint32_t>(frameIdx) & 0xffff;

    uint32_t* pI = reinterpret_cast<uint32_t*>(pInfo);
    pI[0] = fIdx16;                 // frame idx
    pI[1] = 4;                      // label = 4 (3D)
    pI[2] = encodeInfo;             // encode info
    pI[3] = ts;                     // time stamp
    uint16_t* pS = reinterpret_cast<uint16_t*>(pInfo + 16);
    pS[0] = 0;                      // RX
    pS[1] = 0;                      // LX
    pS[2] = static_cast<uint16_t>(imgH);  // height
    pS[3] = static_cast<uint16_t>(imgW);  // width
    pI[6] = 0;                      // 4 unread bytes
    uint16_t* pCnt = reinterpret_cast<uint16_t*>(pInfo + 28);
    pCnt[0] = static_cast<uint16_t>(nPointCnt);
    pCnt[1] = 0;                    // Y

    // ---- Left / right image info ----
    auto fillImageInfo = [&](uint8_t* pImgInfo, uint32_t label) {
        uint32_t* pi = reinterpret_cast<uint32_t*>(pImgInfo);
        pi[0] = fIdx16;
        pi[1] = label;
        uint16_t* ps = reinterpret_cast<uint16_t*>(pImgInfo + 8);
        ps[0] = static_cast<uint16_t>(imgH);
        ps[1] = static_cast<uint16_t>(imgW);
        pi[3] = ts;
        uint16_t* po = reinterpret_cast<uint16_t*>(pImgInfo + 20);
        po[0] = 0;  // oriY
        po[1] = 0;  // oriX
    };
    fillImageInfo(pInfo + 32, 1);   // left, label=1
    fillImageInfo(pInfo + 64, 2);   // right, label=2

    // ---- 3D points (16 bytes each: x,y,z float + nUV uint) ----
    uint8_t* p3D = pInfo + n3DOffset;
    for (int i = 0; i < nPointCnt; i++) {
        float x = (i < static_cast<int>(line.x.size())) ? line.x[i] : 0.0f;
        float y = (i < static_cast<int>(line.y.size())) ? line.y[i] : 0.0f;
        float z = (i < static_cast<int>(line.z.size())) ? line.z[i] : 0.0f;
        memcpy(p3D + i * 16 + 0,  &x, 4);
        memcpy(p3D + i * 16 + 4,  &y, 4);
        memcpy(p3D + i * 16 + 8,  &z, 4);
        // nUV = 0
    }

    // ---- Image pixels (8-bit gray) ----
    auto copyGray = [](const QImage& img, uint8_t* dst, int w, int h) {
        QImage gray = img.convertToFormat(QImage::Format_Grayscale8);
        QImage scaled = gray.scaled(w, h, Qt::IgnoreAspectRatio, Qt::SmoothTransformation);
        for (int y = 0; y < h; y++) {
            const uint8_t* src = scaled.constScanLine(y);
            memcpy(dst + y * w, src, static_cast<size_t>(w));
        }
    };
    copyGray(leftImg,  pInfo + nLeftImageOffset,  imgW, imgH);
    copyGray(rightImg, pInfo + nRightImageOffset, imgW, imgH);

    return frame;
}

void VrCameraSimulator::SendPostCommand(uint32_t cmd, const QByteArray& payload)
{
    std::lock_guard<std::mutex> lock(m_sendMutex);
    SOCKET client = m_tcpClientSocket;
    if (client == INVALID_SOCKET) return;

    QByteArray frame = PackTcpFrame(TcpOperaType::Post, cmd, m_frameSeq, payload);
    int sent = send(client, frame.constData(), frame.size(), 0);
    if (sent <= 0) {
        emit LogMessage("TCP push send failed", true);
    }
}

// ============================================================================
//  Command Handlers
// ============================================================================

QByteArray VrCameraSimulator::HandleCommand(uint32_t cmd, uint32_t seq,
                                             const QByteArray& payload)
{
    switch (cmd) {
    case Cmd_OpenDevice:    return HandleOpenDevice(seq, payload);
    case Cmd_ReadRegister:  return HandleReadRegister(seq, payload);
    case Cmd_WriteRegister: return HandleWriteRegister(seq, payload);
    case Cmd_DeviceOption:  return HandleDeviceOption(seq, payload);
    case Cmd_ReadData:      return HandleReadData(seq, payload);
    case Cmd_WriteData:     return HandleWriteData(seq, payload);
    case Cmd_ExtDevice:     return HandleExtDevice(seq, payload);

    case Cmd_StartStream:
        emit LogMessage("StartStream command received");
        // Stop any previous stream thread before starting a new one.
        m_streaming = false;
        if (m_streamThread.joinable()) m_streamThread.join();
        m_streaming = true;
        m_scanLineIdx = 0;
        m_streamThread = std::thread(&VrCameraSimulator::StreamLoop, this);
        return BuildTcpResponse(seq, cmd, QByteArray(), true);

    case Cmd_StopStream:
        emit LogMessage("StopStream command received");
        m_streaming = false;
        if (m_streamThread.joinable()) m_streamThread.join();
        return BuildTcpResponse(seq, cmd, QByteArray(), true);

    case Cmd_GetImage: {
        // Synchronous single-frame grab: return a full 3D frame as the
        // response data (SDK parses it identically to a pushed frame).
        emit LogMessage("GetImage command received");
        CachedLaserLine line;
        if (!m_laserFileLines.empty()) line = GetNextLaserLine();
        QImage leftImg, rightImg;
        {
            std::lock_guard<std::mutex> lock(m_imageMutex);
            if (!m_leftImages.empty()) {
                int idx = m_frameSeq % static_cast<int>(m_leftImages.size());
                leftImg  = m_leftImages[idx];
                rightImg = (idx < static_cast<int>(m_rightImages.size()))
                           ? m_rightImages[idx] : m_rightImages[0];
            }
        }
        if (leftImg.isNull()) {
            leftImg  = GenerateCheckerboardQImage(m_frameSeq, 0, 0);
            rightImg = GenerateCheckerboardQImage(m_frameSeq, 32, 32);
        }
        QByteArray frame = BuildLaserFrame(line, m_frameSeq, leftImg, rightImg, false);
        m_frameSeq++;
        return BuildTcpResponse(seq, cmd, frame, true);
    }

    case Cmd_Trigger:
    case Cmd_TriggerN:
    case Cmd_PushLaserResult:
    case Cmd_PushDataEx:
        // These are usually sent from camera to host.
        // When received as a request, acknowledge.
        return BuildTcpResponse(seq, cmd, QByteArray(), true);

    default:
        emit LogMessage(QString("Unknown command %1, ack'ing anyway").arg(cmd), true);
        return BuildTcpResponse(seq, cmd, QByteArray(), true);
    }
}

QByteArray VrCameraSimulator::HandleOpenDevice(uint32_t seq, const QByteArray& /*payload*/)
{
    // Build a device capability response.
    // The real device returns a capability struct.
    // We return: resolution (w,h), frame rate range, capability flags.
    QByteArray data;
    data.resize(64);  // padded

    // Resolution
    uint32_t* p = reinterpret_cast<uint32_t*>(data.data());
    p[0] = static_cast<uint32_t>(m_imageWidth);
    p[1] = static_cast<uint32_t>(m_imageHeight);

    // Capability flags (uint32 bitfield matching SVzXilCapability)
    uint32_t cap = 0;
    cap |= (1 << 1);   // bIsSupportRGBSensor
    cap |= (1 << 9);   // bSupportSwingMotor
    cap |= (1 << 10);  // bSupportDynamicRGBD
    p[2] = cap;

    // Device type: LaserRobotEye = 3
    p[3] = 3;

    // Version
    p[4] = 0x01000000;  // 1.0.0.0

    // Product type: LaserEye = 1
    p[5] = 1;

    emit LogMessage("OpenDevice: returned device capabilities");
    return BuildTcpResponse(seq, Cmd_OpenDevice, data, true);
}

QByteArray VrCameraSimulator::HandleReadRegister(uint32_t seq, const QByteArray& payload)
{
    // Request payload: 4-byte register address only (see _GetCustomAddr in the SDK).
    if (payload.size() < 4) {
        return BuildTcpResponse(seq, Cmd_ReadRegister, QByteArray(), false);
    }

    uint32_t addr;
    memcpy(&addr, payload.constData(), 4);

    // Return the exact-length value stored for this register. The SDK checks
    // that the returned block size matches the caller's expected size.
    QByteArray value = ReadRegister(addr);

    emit LogMessage(QString("ReadRegister addr=0x%1 len=%2")
                    .arg(addr, 8, 16, QChar('0')).arg(value.size()));
    return BuildTcpResponse(seq, Cmd_ReadRegister, value, true);
}

QByteArray VrCameraSimulator::HandleWriteRegister(uint32_t seq, const QByteArray& payload)
{
    // Request payload: 4-byte register address + register data (see _SetCustomAddr).
    if (payload.size() < 4) {
        return BuildTcpResponse(seq, Cmd_WriteRegister, QByteArray(), false);
    }

    uint32_t addr;
    memcpy(&addr, payload.constData(), 4);

    QByteArray value = payload.mid(4);
    WriteRegister(addr, value);

    // Apply config side-effects
    if (addr == 0x00400014 && value.size() >= 4) {  // FPS
        int fps;
        memcpy(&fps, value.constData(), 4);
        if (fps > 0 && fps <= 120) m_frameRate = fps;
    } else if (addr == 0x00400018 && value.size() >= 4) {  // EXP
        uint32_t expo;
        memcpy(&expo, value.constData(), 4);
        (void)expo;
    }

    emit LogMessage(QString("WriteRegister addr=0x%1 len=%2")
                    .arg(addr, 8, 16, QChar('0')).arg(value.size()));
    return BuildTcpResponse(seq, Cmd_WriteRegister, QByteArray(), true);
}

QByteArray VrCameraSimulator::HandleDeviceOption(uint32_t seq, const QByteArray& payload)
{
    // TCP DeviceOption request payload starts with a 2-byte option id
    // (EVzXilinxDeviceOption), optionally followed by config data.
    if (payload.size() < 2) {
        return BuildTcpResponse(seq, Cmd_DeviceOption, QByteArray(), false);
    }

    uint16_t option;
    memcpy(&option, payload.constData(), 2);

    QByteArray response;

    switch (option) {
    case 1:  // Query IP
        response.resize(4);
        memcpy(response.data(), m_deviceIPBytes, 4);
        break;
    case 5:  // Query IP type (1=Static)
        response.resize(4);
        *reinterpret_cast<uint32_t*>(response.data()) = 1;  // Static IP
        break;
    case 6:  // Query limit info (frame rate, exposure, gain ranges)
        response.resize(24);
        {
            uint32_t* r = reinterpret_cast<uint32_t*>(response.data());
            r[0] = 1; r[1] = 120;    // frame rate min/max
            r[2] = 1; r[3] = 100000; // exposure min/max (us)
            r[4] = 0; r[5] = 255;    // gain min/max
        }
        break;
    case 7:  // Config network info - acknowledge
        response.resize(4);
        response.fill(0);
        break;
    case 8:  // QueryConfigNetWorkInfo: [IPType(4)][IP(4)][mask(4)][gateway(4)]
        response.resize(16);
        {
            uint32_t* r = reinterpret_cast<uint32_t*>(response.data());
            r[0] = 1;  // IPType = StaticIP
            memcpy(r + 1, m_deviceIPBytes, 4);
            // mask 255.255.255.0
            response[8] = static_cast<char>(255);
            response[9] = static_cast<char>(255);
            response[10] = static_cast<char>(255);
            response[11] = 0;
            // gateway: same subnet, .1
            response[12] = m_deviceIPBytes[0];
            response[13] = m_deviceIPBytes[1];
            response[14] = m_deviceIPBytes[2];
            response[15] = 1;
        }
        break;
    default:
        response.resize(4);
        response.fill(0);
        break;
    }

    emit LogMessage(QString("DeviceOption option=%1").arg(option));
    return BuildTcpResponse(seq, Cmd_DeviceOption, response, true);
}

QByteArray VrCameraSimulator::HandleReadData(uint32_t seq, const QByteArray& payload)
{
    if (payload.size() < 8) {
        return BuildTcpResponse(seq, Cmd_ReadData, QByteArray(), false);
    }

    const uint32_t* p = reinterpret_cast<const uint32_t*>(payload.constData());
    uint32_t addr    = p[0];  // start address
    uint32_t dataLen = p[1];  // bytes to read

    QByteArray response;
    response.resize(static_cast<int>(dataLen));
    response.fill(0);

    // Return calibration matrix for known ranges
    if (addr >= 0x00300000 && addr < 0x00400000) {
        // Calibration data region
        int offset = static_cast<int>(addr - 0x00300000);
        if (offset < m_calibMatrix.size()) {
            int copyLen = (std::min)(static_cast<int>(dataLen),
                                   m_calibMatrix.size() - offset);
            memcpy(response.data(), m_calibMatrix.constData() + offset, copyLen);
        }
    } else if (addr >= 0x40000000 && addr < 0x48000000) {
        // User data region
        int offset = static_cast<int>(addr - 0x40000000);
        if (offset < m_userData.size()) {
            int copyLen = (std::min)(static_cast<int>(dataLen),
                                   m_userData.size() - offset);
            memcpy(response.data(), m_userData.constData() + offset, copyLen);
        }
    }

    emit LogMessage(QString("ReadData addr=0x%1 len=%2").arg(addr, 8, 16, QChar('0')).arg(dataLen));
    return BuildTcpResponse(seq, Cmd_ReadData, response, true);
}

QByteArray VrCameraSimulator::HandleWriteData(uint32_t seq, const QByteArray& payload)
{
    if (payload.size() < 8) {
        return BuildTcpResponse(seq, Cmd_WriteData, QByteArray(), false);
    }

    const uint32_t* p = reinterpret_cast<const uint32_t*>(payload.constData());
    uint32_t addr = p[0];

    int dataLen = payload.size() - 8;
    QByteArray data = payload.mid(8, dataLen);

    // Store in user data region
    if (addr >= 0x40000000 && addr < 0x48000000) {
        int offset = static_cast<int>(addr - 0x40000000);
        if (m_userData.size() < offset + dataLen) {
            m_userData.resize(offset + dataLen);
        }
        memcpy(m_userData.data() + offset, data.constData(), dataLen);
    }

    emit LogMessage(QString("WriteData addr=0x%1 len=%2").arg(addr, 8, 16, QChar('0')).arg(dataLen));
    return BuildTcpResponse(seq, Cmd_WriteData, QByteArray(), true);
}

// ============================================================================
//  External Device handler (swing motor over ExtDevice command).
//  Request payload: [extType uint][5 x ushort swing command] (14 bytes).
//  The swing command bytes are: [0]=read/write, [1]=command code (LE ushort
//  at payload[6..7]), [2]=register addr, [3..4]=value. See VzSwingMotorAPI.h
//  c_sSwingMotorCommand[]. Response data = 4-byte result value.
// ============================================================================
QByteArray VrCameraSimulator::HandleExtDevice(uint32_t seq, const QByteArray& payload)
{
    if (payload.size() < 14) {
        // Could be a config write without an expected data block; still ack.
        return BuildTcpResponse(seq, Cmd_ExtDevice, QByteArray(), true);
    }

    // extType at [0..3] (0 = swing motor)
    uint32_t extType = 0;
    memcpy(&extType, payload.constData(), 4);

    // Swing sub-command code: ushort[1] at payload[6..7] (little-endian).
    uint16_t swingCmd = 0;
    memcpy(&swingCmd, payload.constData() + 6, 2);
    // Read/write flag: ushort[0] at payload[4..5].
    uint16_t rwFlag = 0;
    memcpy(&rwFlag, payload.constData() + 4, 2);

    uint32_t result = 0;

    // Command codes from VzSwingMotorAPI.h c_sSwingMotorCommand[].
    switch (swingCmd) {
    case 0x0024:  // GetVersion
        result = 8;               // version >= 8 enables GetMotorMaxAngle path
        break;
    case 0x0025:  // GetMotorMaxAngle (hardware max angle, degrees*10)
        result = 750;
        break;
    case 0x001B:  // GetMaxAngle (degrees*10)
        result = 750;
        break;
    case 0x0020:  // GetReductionRatio
        // Must succeed (non-failing); affects speed-range calc: ratio=reduction/36.
        // 360 -> ratio 10 -> speed range ~16.7..133.3 deg/s.
        result = 360;
        break;
    case 0x000F:  // QueryStatus
        // bit15 = 1 marks a valid status word; bit14 = 1 means Busy, 0 = Idle.
        // Without bit15 the SDK's _QueryValidStatus returns SwingMotor_Err.
        result = 0x8000;          // valid + idle
        break;
    case 0x001F:  // GetMotorSpeed
        result = 36;              // current speed (deg/s)
        break;
    case 0x001C:  // GetMotorStartPos
        result = 0;
        break;
    case 0x001D:  // GetMotorEndPos
        result = 0;
        break;
    case 0x0012:  // GetCurPos
        result = 0;
        break;
    case 0x001E:  // GetLaserDeviceLight
        result = 0;
        break;
    default:
        // Write commands and unknown reads simply return 0 (success).
        result = 0;
        break;
    }

    QByteArray respData(reinterpret_cast<const char*>(&result), sizeof(result));
    emit LogMessage(QString("ExtDevice swingCmd=0x%1 rw=%2 -> %3")
                    .arg(swingCmd, 4, 16, QChar('0')).arg(rwFlag).arg(result));
    return BuildTcpResponse(seq, Cmd_ExtDevice, respData, true);
}

// ============================================================================
//  TCP Protocol Framing (VZEB / VZEE)
// ============================================================================

QByteArray VrCameraSimulator::PackTcpFrame(TcpOperaType op, uint32_t cmd,
                                             uint32_t seq, const QByteArray& payload)
{
    uint32_t totalLen = TCP_HEADER_SIZE + static_cast<uint32_t>(payload.size()) + VZEE_TAIL_LEN;

    QByteArray frame;
    frame.resize(static_cast<int>(totalLen));
    uint8_t* d = reinterpret_cast<uint8_t*>(frame.data());

    // "VZEB" head
    d[0] = 'V'; d[1] = 'Z'; d[2] = 'E'; d[3] = 'B';

    // Total length (little-endian)
    memcpy(d + 4, &totalLen, 4);

    // Operation type
    uint32_t opVal = static_cast<uint32_t>(op);
    memcpy(d + 8, &opVal, 4);

    // Command
    memcpy(d + 12, &cmd, 4);

    // Sequence number
    memcpy(d + 16, &seq, 4);

    // Payload
    if (!payload.isEmpty()) {
        memcpy(d + TCP_HEADER_SIZE, payload.constData(), payload.size());
    }

    // "VZEE" tail
    int tailOff = TCP_HEADER_SIZE + payload.size();
    d[tailOff]     = 'V';
    d[tailOff + 1] = 'Z';
    d[tailOff + 2] = 'E';
    d[tailOff + 3] = 'E';

    return frame;
}

bool VrCameraSimulator::UnpackTcpFrame(const QByteArray& frame, TcpOperaType& op,
                                         uint32_t& cmd, uint32_t& seq,
                                         QByteArray& payload)
{
    if (frame.size() < TCP_HEADER_SIZE + VZEE_TAIL_LEN) return false;

    const uint8_t* d = reinterpret_cast<const uint8_t*>(frame.constData());

    // Check head
    if (d[0] != 'V' || d[1] != 'Z' || d[2] != 'E' || d[3] != 'B')
        return false;

    uint32_t totalLen;
    memcpy(&totalLen, d + 4, 4);
    if (totalLen != static_cast<uint32_t>(frame.size())) return false;

    memcpy(reinterpret_cast<uint32_t*>(&op), d + 8, 4);
    memcpy(&cmd, d + 12, 4);
    memcpy(&seq, d + 16, 4);

    int payloadLen = frame.size() - TCP_HEADER_SIZE - VZEE_TAIL_LEN;
    if (payloadLen > 0) {
        payload = frame.mid(TCP_HEADER_SIZE, payloadLen);
    } else {
        payload.clear();
    }

    // Check tail
    int tailOff = TCP_HEADER_SIZE + payloadLen;
    if (d[tailOff] != 'V' || d[tailOff+1] != 'Z' ||
        d[tailOff+2] != 'E' || d[tailOff+3] != 'E')
        return false;

    return true;
}

bool VrCameraSimulator::ParseVzebFrame(const uint8_t* data, int len, int& consumed,
                                         TcpOperaType& op, uint32_t& cmd,
                                         uint32_t& seq, QByteArray& payload)
{
    consumed = 0;
    if (len < TCP_HEADER_SIZE + VZEE_TAIL_LEN) return false;

    // Find "VZEB" head
    int headPos = -1;
    for (int i = 0; i <= len - 4; i++) {
        if (data[i] == 'V' && data[i+1] == 'Z' &&
            data[i+2] == 'E' && data[i+3] == 'B') {
            headPos = i;
            break;
        }
    }

    if (headPos < 0) return false;

    // Read total length from header
    if (headPos + 8 > len) return false;
    uint32_t totalLen;
    memcpy(&totalLen, data + headPos + 4, 4);

    // Sanity check
    if (totalLen < TCP_HEADER_SIZE + VZEE_TAIL_LEN || totalLen > 1024 * 1024)
        return false;

    if (headPos + static_cast<int>(totalLen) > len) return false;

    // Build a QByteArray from this segment and unpack
    QByteArray frame(reinterpret_cast<const char*>(data + headPos),
                     static_cast<int>(totalLen));

    if (!UnpackTcpFrame(frame, op, cmd, seq, payload)) return false;

    consumed = headPos + static_cast<int>(totalLen);
    return true;
}

// ============================================================================
//  Build response helpers
// ============================================================================

QByteArray VrCameraSimulator::BuildTcpResponse(uint32_t seqNum, uint32_t command,
                                                 const QByteArray& payload, bool success)
{
    // The SDK expects the response payload to start with a 4-byte error/return
    // code, followed by the returned data block (_RecviceDataWithCheck strips
    // the first 4 bytes as errCode and exposes the rest as the data block).
    QByteArray body;
    uint32_t errCode = success ? 0 : 1;
    body.append(reinterpret_cast<const char*>(&errCode), sizeof(errCode));
    body.append(payload);
    return PackTcpFrame(TcpOperaType::Respond, command, seqNum, body);
}

// ============================================================================
//  UDP Discovery Response Builder
// ============================================================================

QByteArray VrCameraSimulator::BuildDiscoveryResponse(const QByteArray& request,
                                                       const sockaddr_in& /*senderAddr*/)
{
    // Advertise our real local IP so the SDK can reach back on TCP 6679.
    // The hard-coded 192.168.1.200 default only works if this machine owns
    // that address; otherwise the SDK's Open() TCP connect fails with
    // keErrorCode_NetworkConnectFailed (-69999). getsockname() on a socket
    // bound to INADDR_ANY returns 0.0.0.0, so enumerate adapters instead.
    {
        uint8_t localIP[4] = {0, 0, 0, 0};
        if (GetFirstLocalIPv4(localIP)) {
            m_deviceIPBytes[0] = localIP[0];
            m_deviceIPBytes[1] = localIP[1];
            m_deviceIPBytes[2] = localIP[2];
            m_deviceIPBytes[3] = localIP[3];
            m_deviceIP = QString("%1.%2.%3.%4")
                             .arg(localIP[0]).arg(localIP[1])
                             .arg(localIP[2]).arg(localIP[3]);
        }
    }

    // Parse the request to extract the serial number field (bytes 4-11)
    QByteArray sn(8, 0);
    if (request.size() >= 12) {
        sn = request.mid(4, 8);
    }

    // Build the response matching SVzXilinxDeviceInfo layout (68 bytes, MSVC
    // default packing with 3 bytes padding after the 1-byte product type):
    //   [0]    eProductType (1)
    //   [1..3] padding
    //   [4..7] nVersionCode (LE)
    //   [8..39] szDeviceVersion (32)
    //   [40..41] nResolutionWidth (LE)
    //   [42..43] nResolutionHeight (LE)
    //   [44]   eIPType
    //   [45..48] byDeviceIP
    //   [49..54] byDeviceMAC
    //   [55..58] byDeviceGW
    //   [59..66] szSN
    //   total 68 bytes

    QByteArray devInfo;
    devInfo.resize(68);
    devInfo.fill(0);
    uint8_t* d = reinterpret_cast<uint8_t*>(devInfo.data());

    // eProductType = 1 (LaserEye). The SDK ignores this and hard-codes
    // eDeviceType = keDeviceType_LaserRobotEye.
    d[0] = 1;

    // nVersionCode (offset 4, little-endian)
    uint32_t verCode = 0x01000000;
    memcpy(d + 4, &verCode, 4);

    // szDeviceVersion (offset 8, 32 bytes) — null-terminated
    const char* verStr = "VrVirtualCam v1.0";
    strncpy(reinterpret_cast<char*>(d + 8), verStr, 31);

    // nResolutionWidth / nResolutionHeight (offset 40/42, little-endian)
    uint16_t w = static_cast<uint16_t>(m_imageWidth);
    uint16_t h = static_cast<uint16_t>(m_imageHeight);
    memcpy(d + 40, &w, 2);
    memcpy(d + 42, &h, 2);

    // eIPType (offset 44) = 1 (Static IP)
    d[44] = 1;

    // byDeviceIP (offset 45)
    memcpy(d + 45, m_deviceIPBytes, 4);

    // byDeviceMAC (offset 49)
    memcpy(d + 49, m_deviceMACBytes, 6);

    // byDeviceGW (offset 55) = gateway (same subnet, .1)
    d[55] = m_deviceIPBytes[0];
    d[56] = m_deviceIPBytes[1];
    d[57] = m_deviceIPBytes[2];
    d[58] = 1;

    // szSN (offset 59)
    memcpy(d + 59, m_deviceSNBytes, 8);

    // Now build the full UDP packet
    QByteArray packet;

    // Preamble (little-endian: bytes are e1,e2,e3,e4)
    packet.append('\xe1');
    packet.append('\xe2');
    packet.append('\xe3');
    packet.append('\xe4');

    // SN (8 bytes)
    packet.append(sn);

    // RecvUDPDataType = 0x0040 (Command) — big-endian: 0x00 0x40
    packet.append(static_cast<char>(0x00));
    packet.append(static_cast<char>(0x40));

    // shCommand + shCommandLength (2+2 bytes, zero) — SDK reads these but does
    // not validate them.
    packet.append(4, '\0');

    // shCommandSequen (2 bytes, not validated) — "SB"
    packet.append(static_cast<char>(0x42));
    packet.append(static_cast<char>(0x53));

    // nReserveData (4 bytes)
    packet.append(4, '\0');

    // shPackageHead "SB" (2 bytes)
    packet.append(static_cast<char>(0x42));
    packet.append(static_cast<char>(0x53));

    // AckType = 0x0004 (CommandData) — big-endian: 0x00 0x04
    packet.append(static_cast<char>(0x00));
    packet.append(static_cast<char>(0x04));

    // Command Length (big-endian).
    // SDK: shRemainLen = shCommandLen - 14; requires >= sizeof(SVzXilinxDeviceInfo)=68
    // to parse the device info. So shCommandLen must be >= 82.
    uint16_t cmdLen = static_cast<uint16_t>(14 + devInfo.size());  // = 82
    packet.append(static_cast<char>((cmdLen >> 8) & 0xFF));
    packet.append(static_cast<char>(cmdLen & 0xFF));

    // Checksum (4 bytes, placeholder = 0)
    packet.append(4, '\0');

    // Command Sequence (2 bytes, match request seq if available)
    uint16_t cmdSeq = 1;
    if (request.size() >= 36) {
        cmdSeq = (static_cast<uint16_t>(static_cast<uint8_t>(request[34])) << 8)
               |  static_cast<uint16_t>(static_cast<uint8_t>(request[35]));
    }
    packet.append(static_cast<char>((cmdSeq >> 8) & 0xFF));
    packet.append(static_cast<char>(cmdSeq & 0xFF));

    // Device Info payload
    packet.append(devInfo);

    // Package end "EB" = 0x4245. After the device info exactly 2 bytes must
    // remain so the SDK takes the "shRemainLen == 2" branch (reads this tail).
    // The SDK reads one more package end unconditionally afterwards; that read
    // runs past the buffer end and fails harmlessly (device already parsed).
    packet.append(static_cast<char>(0x42));
    packet.append(static_cast<char>(0x45));

    return packet;
}

// ============================================================================
//  Register Map
// ============================================================================

void VrCameraSimulator::InitRegisterMap()
{
    std::lock_guard<std::mutex> lock(m_regMutex);
    m_registers.clear();

    // Helper: store a uint32 value as a 4-byte little-endian blob.
    auto putU32 = [this](uint32_t addr, uint32_t value) {
        QByteArray data(reinterpret_cast<const char*>(&value), sizeof(value));
        m_registers[addr] = data;
    };

    // ---- Camera registers ----
    putU32(0x00100000, 0x00000003);  // DevType: 3 = LaserRobotEye
    putU32(0x0010000C, 0x00000008);  // HardwareVersion: 8
    putU32(0x00100010, 0x00000001);  // AlgoIPVersion
    putU32(0x00100014, 0x01000000);  // PSVersion
    putU32(0x0010002C, 0x00000000);  // DataMode: 0 = Data (not Data+Image)
    putU32(0x00100030, 0x00000001);  // DeviceID
    // Capability: bit1 = bIsSupportRGBSensor, bit9 = bSupportSwingMotor.
    // (RGB bit lets SetRGBDExposeThres succeed; RGB camera init is best-effort.)
    putU32(0x00100034, 0x00000202);
    putU32(0x00100094, 0x01000000);  // PLVersion
    putU32(0x00100090, 0x00000000);  // EnableCalibROIFlag = 0 (no calib ROI)
    putU32(0x00100078, 0x00000001);  // LaserDataVersion = 1

    // Serial number (8 bytes)
    m_registers[0x00100004] = QByteArray(reinterpret_cast<const char*>(m_deviceSNBytes), 8);

    // ---- Status registers ----
    // Phase status: 16 bytes each, all-zero (means valid, != 0xff)
    m_registers[0x00210000] = QByteArray(16, 0);  // LeftPhase
    m_registers[0x00210010] = QByteArray(16, 0);  // RightPhase
    m_registers[0x00210020] = QByteArray(16, 0);  // CenterPhase
    putU32(0x00200018, 0x00000000);  // Status_Stream = 0

    // ---- Sensor registers ----
    // Sensor ROI (5x uint32): [Width, Height, Y, LeftX, RightX]
    QByteArray roi(20, 0);
    uint32_t* roiArr = reinterpret_cast<uint32_t*>(roi.data());
    roiArr[0] = static_cast<uint32_t>(m_imageWidth);
    roiArr[1] = static_cast<uint32_t>(m_imageHeight);
    roiArr[2] = 0;  // Y
    roiArr[3] = 0;  // LeftX
    roiArr[4] = 0;  // RightX
    m_registers[0x00400000] = roi;

    putU32(0x00400014, static_cast<uint32_t>(m_frameRate));  // FPS
    putU32(0x00400018, 5000);        // EXP
    putU32(0x0040001C, 100);         // GAIN Left
    putU32(0x00400020, 100);         // GAIN Right

    // RGB sensor registers (read by _UpdateColorPixelFormat when RGB is enabled)
    putU32(0x00400138, 0);           // Center_Type = Color (0)
    putU32(0x0040013C, 0);           // Center_Pixel_Format = RGGB (0)
    putU32(0x004000CC, 0);           // CenterAutoExposeThres (float)

    // FullDetectROI: 4 x SVzNLROIRect (left/right/calib-left/calib-right).
    // SVzNLROIRect = 4 ints (left,right,top,bottom) = 16 bytes each, 64 total.
    {
        QByteArray fullROI(4 * 16, 0);
        int* r = reinterpret_cast<int*>(fullROI.data());
        // Left / right full-frame ROI.
        r[0] = 0;  r[1] = m_imageWidth;  r[2] = 0;  r[3] = m_imageHeight;   // left
        r[4] = 0;  r[5] = m_imageWidth;  r[6] = 0;  r[7] = m_imageHeight;   // right
        // Calib left/right left as 0.
        m_registers[0x0040006C] = fullROI;
    }

    // ---- Calibration ----
    // QMatrix: 16 doubles (128 bytes), identity matrix
    QByteArray qmat(16 * sizeof(double), 0);
    double* qm = reinterpret_cast<double*>(qmat.data());
    for (int i = 0; i < 16; i++) qm[i] = (i % 5 == 0) ? 1.0 : 0.0;
    m_registers[0x00300000] = qmat;
    // ConvertMatrixData (GetCalibMatrix): 16 doubles, identity
    m_registers[0x00600088] = qmat;
    putU32(0x00300080, 0x00000000);  // Parallax_Offset

    // ---- Product registers ----
    putU32(0x00700000, 0x00000000);  // ProductType: keProjectType_None
    putU32(0x00700004, 256);         // 3DDataHeadLength
    putU32(0x00700008, 104);         // 3DDataFrameLength
    // SupportImageWidth must be >= the full-frame ROI width (1280), otherwise
    // BeginGetAutoDetect aborts Data-mode laser detect with
    // keErrorCode_Device_NoSupport_Current_ROI (-79923).
    putU32(0x00710004, 2048);        // SupportMaxROIWidth

    // ---- External device: swing motor ----
    putU32(0x00500000, 0x00000001);  // IsSupportSwing = 1
    putU32(0x00500004, 0x00000001);  // EnableSwing = 1
    putU32(0x00500008, 0x00000000);  // SwingScanMode: Once
    // SwingWorkRange: 2 floats (near/far distance, mm)
    {
        float fRange[2] = {500.0f, 1500.0f};
        m_registers[0x0050000C] = QByteArray(reinterpret_cast<const char*>(fRange),
                                             sizeof(fRange));
    }
}

QByteArray VrCameraSimulator::ReadRegister(uint32_t addr) const
{
    std::lock_guard<std::mutex> lock(m_regMutex);
    auto it = m_registers.find(addr);
    if (it != m_registers.end()) {
        return it->second;
    }
    // Unknown register: return 4 zero bytes (matches common register width).
    return QByteArray(4, 0);
}

void VrCameraSimulator::WriteRegister(uint32_t addr, const QByteArray& data)
{
    std::lock_guard<std::mutex> lock(m_regMutex);
    m_registers[addr] = data;
}

bool VrCameraSimulator::HasRegister(uint32_t addr) const
{
    std::lock_guard<std::mutex> lock(m_regMutex);
    return m_registers.find(addr) != m_registers.end();
}

// ============================================================================
//  Image & Laser Data Loading
// ============================================================================

void VrCameraSimulator::SetImageDirectory(const QString& dir)
{
    m_imageDir = dir;
    LoadImagesFromDirectory();
}

void VrCameraSimulator::LoadImagesFromDirectory()
{
    std::lock_guard<std::mutex> lock(m_imageMutex);
    m_leftImages.clear();
    m_rightImages.clear();
    m_imageFiles.clear();
    m_imageReplayIdx = 0;

    if (m_imageDir.isEmpty()) {
        emit ImageDirectoryChanged(0);
        emit LogMessage("Image directory cleared");
        return;
    }

    QDir dir(m_imageDir);
    if (!dir.exists()) {
        emit LogMessage("Image directory not found: " + m_imageDir, true);
        emit ImageDirectoryChanged(0);
        return;
    }

    // Find all *-L.* files and match with *-R.* counterparts
    QStringList filters = {"*.png", "*.bmp", "*.jpg", "*.jpeg", "*.PNG", "*.BMP", "*.JPG", "*.JPEG"};
    QStringList allFiles;

    QStringList nameFilters;
    for (const auto& f : filters) {
        nameFilters << f;
    }

    QDirIterator it(m_imageDir, nameFilters, QDir::Files);
    while (it.hasNext()) {
        allFiles << it.next();
    }

    // Find left images and match right pairs
    QSet<QString> matchedBases;
    for (const QString& filePath : allFiles) {
        QFileInfo fi(filePath);
        QString name = fi.completeBaseName();  // e.g. "image-L" or "image-R"

        if (name.endsWith("-L") || name.endsWith("-l")) {
            QString base = name.left(name.size() - 2);  // remove "-L"
            if (matchedBases.contains(base)) continue;

            // Find corresponding right image
            QString rightPath;
            for (const QString& rp : allFiles) {
                QFileInfo rfi(rp);
                QString rname = rfi.completeBaseName();
                if ((rname == base + "-R" || rname == base + "-r") && rp != filePath) {
                    rightPath = rp;
                    break;
                }
            }

            if (!rightPath.isEmpty()) {
                QImage leftImg(filePath);
                QImage rightImg(rightPath);
                if (!leftImg.isNull() && !rightImg.isNull()) {
                    m_imageFiles << base;
                    m_leftImages.push_back(leftImg);
                    m_rightImages.push_back(rightImg);
                    matchedBases.insert(base);
                }
            }
        }
    }

    emit LogMessage(QString("Loaded %1 stereo image pairs from %2")
                    .arg(m_leftImages.size()).arg(m_imageDir));
    emit ImageDirectoryChanged(static_cast<int>(m_leftImages.size()));
}

void VrCameraSimulator::SetLaserDataDirectory(const QString& dir)
{
    m_laserDataDir = dir;
    LoadLaserDataFromDirectory();
}

int VrCameraSimulator::LaserDataTotalLines() const
{
    int total = 0;
    for (const auto& fileLines : m_laserFileLines) {
        total += static_cast<int>(fileLines.size());
    }
    return total;
}

void VrCameraSimulator::LoadLaserDataFromDirectory()
{
    std::lock_guard<std::recursive_mutex> lock(m_laserDataMutex);
    m_laserDataFiles.clear();
    m_laserFileLines.clear();
    m_laserFileReplayIdx = 0;
    m_laserLineReplayIdx = 0;

    if (m_laserDataDir.isEmpty()) {
        emit LaserDataDirectoryChanged(0, 0);
        emit LogMessage("Laser data directory cleared");
        return;
    }

    QDir dir(m_laserDataDir);
    if (!dir.exists()) {
        emit LogMessage("Laser data directory not found: " + m_laserDataDir, true);
        emit LaserDataDirectoryChanged(0, 0);
        return;
    }

    // Find all .txt and .dat files
    QStringList filters = {"*.txt", "*.dat", "*.TXT", "*.DAT"};
    QStringList nameFilters;
    for (const auto& f : filters) nameFilters << f;

    QDirIterator it(m_laserDataDir, nameFilters, QDir::Files);
    QStringList filePaths;
    while (it.hasNext()) filePaths << it.next();
    filePaths.sort();

    int totalLines = 0;
    LaserDataLoader loader;

    for (const QString& filePath : filePaths) {
        std::string path = filePath.toStdString();
        std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> laserLines;
        int lineNum = 0;
        float scanSpeed = 0;
        int maxTimeStamp = 0;
        int clockPerSecond = 0;

        int result = loader.LoadLaserScanData(path, laserLines, lineNum,
                                               scanSpeed, maxTimeStamp, clockPerSecond);
        if (result != 0 || laserLines.empty()) {
            emit LogMessage(QString("Skipping %1 (no valid laser data)").arg(filePath), true);
            continue;
        }

        m_laserDataFiles << filePath;

        std::vector<CachedLaserLine> cachedLines;
        for (const auto& linePair : laserLines) {
            const SVzLaserLineData& lineData = linePair.second;
            CachedLaserLine cached;
            cached.pointCount = lineData.nPointCount;
            cached.timestamp  = lineData.llTimeStamp;
            cached.frameIdx   = lineData.llFrameIdx;

            if (lineData.nPointCount > 0 && lineData.p3DPoint) {
                cached.x.resize(lineData.nPointCount);
                cached.y.resize(lineData.nPointCount);
                cached.z.resize(lineData.nPointCount);

                if (linePair.first == keResultDataType_Position ||
                    linePair.first == keResultDataType_PositionF) {
                    const SVzNL3DPosition* pts = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
                    for (int i = 0; i < lineData.nPointCount; i++) {
                        cached.x[i] = static_cast<float>(pts[i].pt3D.x);
                        cached.y[i] = static_cast<float>(pts[i].pt3D.y);
                        cached.z[i] = static_cast<float>(pts[i].pt3D.z);
                    }
                } else if (linePair.first == keResultDataType_PointXYZ) {
                    const SVzNLPointXYZ* pts = static_cast<const SVzNLPointXYZ*>(lineData.p3DPoint);
                    for (int i = 0; i < lineData.nPointCount; i++) {
                        cached.x[i] = pts[i].x;
                        cached.y[i] = pts[i].y;
                        cached.z[i] = pts[i].z;
                    }
                } else if (linePair.first == keResultDataType_PointXYZRGBA) {
                    const SVzNLPointXYZRGBA* pts = static_cast<const SVzNLPointXYZRGBA*>(lineData.p3DPoint);
                    for (int i = 0; i < lineData.nPointCount; i++) {
                        cached.x[i] = pts[i].x;
                        cached.y[i] = pts[i].y;
                        cached.z[i] = pts[i].z;
                    }
                }
            }

            cachedLines.push_back(cached);
        }

        m_laserFileLines.push_back(cachedLines);
        totalLines += static_cast<int>(cachedLines.size());

        // Free VZNLSDK-managed memory
        loader.FreeLaserScanData(laserLines);
    }

    emit LogMessage(QString("Loaded %1 files / %2 total laser lines from %3")
                    .arg(m_laserDataFiles.size()).arg(totalLines).arg(m_laserDataDir));
    emit LaserDataDirectoryChanged(m_laserDataFiles.size(), totalLines);
}

VrCameraSimulator::CachedLaserLine VrCameraSimulator::GetNextLaserLine()
{
    std::lock_guard<std::recursive_mutex> lock(m_laserDataMutex);
    CachedLaserLine empty;

    if (m_laserFileLines.empty()) return empty;

    // Cycle through files and lines
    if (m_laserFileReplayIdx >= static_cast<int>(m_laserFileLines.size())) {
        m_laserFileReplayIdx = 0;
        m_laserLineReplayIdx = 0;
    }

    const auto& currentFile = m_laserFileLines[m_laserFileReplayIdx];
    if (currentFile.empty()) {
        m_laserFileReplayIdx++;
        m_laserLineReplayIdx = 0;
        if (m_laserFileReplayIdx >= static_cast<int>(m_laserFileLines.size())) {
            m_laserFileReplayIdx = 0;
        }
        return GetNextLaserLine();  // try next file
    }

    if (m_laserLineReplayIdx >= static_cast<int>(currentFile.size())) {
        m_laserLineReplayIdx = 0;
        m_laserFileReplayIdx++;
        if (m_laserFileReplayIdx >= static_cast<int>(m_laserFileLines.size())) {
            m_laserFileReplayIdx = 0;
        }
        return GetNextLaserLine();  // try next file
    }

    CachedLaserLine line = currentFile[m_laserLineReplayIdx];
    m_laserLineReplayIdx++;
    return line;
}

void VrCameraSimulator::ResetLaserReplay()
{
    std::lock_guard<std::recursive_mutex> lock(m_laserDataMutex);
    m_laserFileReplayIdx = 0;
    m_laserLineReplayIdx = 0;
}

// ============================================================================
//  Image Generator: Checkerboard Pattern (fallback when no images loaded)
// ============================================================================

QByteArray VrCameraSimulator::GenerateCheckerboardGray(int frameIdx, int offsetX, int offsetY)
{
    const int sqSize = 64;
    const int size   = m_imageWidth * m_imageHeight;
    QByteArray buf(size, '\0');
    uint8_t* pixels = reinterpret_cast<uint8_t*>(buf.data());

    int shift = frameIdx % sqSize;

    for (int y = 0; y < m_imageHeight; y++) {
        for (int x = 0; x < m_imageWidth; x++) {
            int cx = (x + offsetX + shift) / sqSize;
            int cy = (y + offsetY + shift) / sqSize;
            pixels[y * m_imageWidth + x] = ((cx + cy) % 2 == 0) ? 220 : 30;
        }
    }

    return buf;
}

QImage VrCameraSimulator::GenerateCheckerboardQImage(int frameIdx, int offsetX, int offsetY)
{
    QByteArray raw = GenerateCheckerboardGray(frameIdx, offsetX, offsetY);
    QImage img(reinterpret_cast<const uint8_t*>(raw.constData()),
               m_imageWidth, m_imageHeight, QImage::Format_Grayscale8);
    return img.copy();  // deep copy since raw is temporary
}