#include "core/GVSPParser.h"
#include <QDebug>
#include <cstring>
#include <winsock2.h>

GVSPParser::GVSPParser(QObject *parent)
    : QObject(parent)
    , m_isReceiving(false)
    , m_dataType(0)
    , m_currentBlockId(0)
    , m_expectedSize(0)
    , m_receivedSize(0)
    , m_imageWidth(0)
    , m_imageHeight(0)
    , m_pixelFormat(0)
    , m_lastBlockId(0)
    , m_packetCount(0)
{
}

GVSPParser::~GVSPParser()
{
}

void GVSPParser::reset()
{
    m_isReceiving = false;
    m_dataType = 0;
    m_currentBlockId = 0;
    m_dataBuffer.clear();
    m_expectedSize = 0;
    m_receivedSize = 0;
    m_packetCount = 0;
}

void GVSPParser::parsePacket(const QByteArray &packet)
{
    if (packet.size() < sizeof(GVSPPacketHeader)) {
        return;
    }

    const uint8_t *data = reinterpret_cast<const uint8_t*>(packet.constData());
    const GVSPPacketHeader *header = reinterpret_cast<const GVSPPacketHeader*>(data);

    // 注释掉调试日志以提高性能
    // static int debugCount = 0;
    // if (debugCount < 3) {
    //     qDebug() << "Packet" << debugCount << "first 16 bytes (hex):";
    //     QString hexStr;
    //     for (int i = 0; i < qMin(16, packet.size()); i++) {
    //         hexStr += QString("%1 ").arg(data[i], 2, 16, QChar('0'));
    //     }
    //     qDebug() << hexStr;
    //     debugCount++;
    // }

    // GVSP包头格式：status(2) + block_id(2) + packet_format(4)
    // 包类型在packet_format的高字节
    uint32_t packet_fmt = ntohl(header->packet_fmt_id);
    uint8_t packetType = (packet_fmt >> 24) & 0xFF;
    uint16_t blockId = ntohs(header->block_id);

    // 注释掉频繁的日志输出以提高性能
    // static int leaderCount = 0;
    // static int trailerCount = 0;

    switch (packetType) {
        case GVSP_LEADER_PACKET:
            handleLeaderPacket(data, packet.size());
            break;
        case GVSP_PAYLOAD_PACKET:
            handlePayloadPacket(data, packet.size());
            break;
        case GVSP_TRAILER_PACKET:
            handleTrailerPacket(data, packet.size());
            break;
        default:
            break;
    }
}

void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
{
    if (size < sizeof(GVSPPacketHeader) + sizeof(GVSPImageDataLeader)) {
        return;
    }

    const GVSPPacketHeader *header = reinterpret_cast<const GVSPPacketHeader*>(data);
    m_currentBlockId = ntohs(header->block_id);

    // Check payload type
    const uint16_t *payload_type_ptr = reinterpret_cast<const uint16_t*>(data + sizeof(GVSPPacketHeader) + 2);
    uint16_t payload_type = ntohs(*payload_type_ptr);

    if (payload_type == PAYLOAD_TYPE_IMAGE) {
        // Image data leader
        const GVSPImageDataLeader *leader = reinterpret_cast<const GVSPImageDataLeader*>(data + sizeof(GVSPPacketHeader));
        
        m_dataType = 1;
        m_imageWidth = ntohl(leader->size_x);
        m_imageHeight = ntohl(leader->size_y);
        m_pixelFormat = ntohl(leader->pixel_format);
        m_expectedSize = m_imageWidth * m_imageHeight * 2; // 12-bit packed in 16-bit
        
        m_dataBuffer.clear();
        m_dataBuffer.reserve(m_expectedSize);
        m_receivedSize = 0;
        m_isReceiving = true;
        m_packetCount = 0;

        // 注释掉频繁的日志输出
        // qDebug() << "Image Leader: Block" << m_currentBlockId
        //          << "Size:" << m_imageWidth << "x" << m_imageHeight;
    }
    else if (payload_type == PAYLOAD_TYPE_BINARY) {
        // Depth data leader
        const GVSPBinaryDataLeader *leader = reinterpret_cast<const GVSPBinaryDataLeader*>(data + sizeof(GVSPPacketHeader));
        
        m_dataType = 3;
        m_expectedSize = ntohl(leader->file_size);
        
        m_dataBuffer.clear();
        m_dataBuffer.reserve(m_expectedSize);
        m_receivedSize = 0;
        m_isReceiving = true;
        m_packetCount = 0;

        // 注释掉频繁的日志输出
        // qDebug() << "Depth Leader: Block" << m_currentBlockId
        //          << "Size:" << m_expectedSize << "bytes";
    }
}

void GVSPParser::handlePayloadPacket(const uint8_t *data, size_t size)
{
    if (!m_isReceiving) {
        return;
    }

    if (size <= sizeof(GVSPPacketHeader)) {
        return;
    }

    // Extract payload data (skip header)
    const uint8_t *payload = data + sizeof(GVSPPacketHeader);
    size_t payload_size = size - sizeof(GVSPPacketHeader);

    // Append to buffer
    m_dataBuffer.append(reinterpret_cast<const char*>(payload), payload_size);
    m_receivedSize += payload_size;
    m_packetCount++;
}

void GVSPParser::handleTrailerPacket(const uint8_t *data, size_t size)
{
    if (!m_isReceiving) {
        return;
    }

    // 注释掉频繁的日志输出
    // qDebug() << "Trailer received: Block" << m_currentBlockId
    //          << "Received" << m_receivedSize << "/" << m_expectedSize << "bytes"
    //          << "Packets:" << m_packetCount;

    // Process complete data
    if (m_dataType == 1) {
        processImageData();
    } else if (m_dataType == 3) {
        processDepthData();
    }

    // Reset state
    m_isReceiving = false;
    m_lastBlockId = m_currentBlockId;
}

void GVSPParser::processImageData()
{
    if (m_dataBuffer.size() < m_expectedSize) {
        // 注释掉频繁的警告日志
        // qDebug() << "Warning: Incomplete image data" << m_dataBuffer.size() << "/" << m_expectedSize;
        return;
    }

    // Convert 16-bit depth data to 8-bit grayscale for display
    const uint16_t *src = reinterpret_cast<const uint16_t*>(m_dataBuffer.constData());
    QImage image(m_imageWidth, m_imageHeight, QImage::Format_Grayscale8);

    // Find min/max for normalization
    uint16_t minVal = 65535, maxVal = 0;
    for (size_t i = 0; i < m_imageWidth * m_imageHeight; i++) {
        uint16_t val = src[i];
        if (val > 0) {
            if (val < minVal) minVal = val;
            if (val > maxVal) maxVal = val;
        }
    }

    // Normalize to 0-255
    uint8_t *dst = image.bits();
    float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;

    for (size_t y = 0; y < m_imageHeight; y++) {
        for (size_t x = 0; x < m_imageWidth; x++) {
            size_t idx = y * m_imageWidth + x;

            uint16_t val = src[idx];
            if (val == 0) {
                dst[idx] = 0;
            } else {
                dst[idx] = static_cast<uint8_t>((val - minVal) * scale);
            }
        }
    }

    emit imageReceived(image, m_currentBlockId);
}

void GVSPParser::processDepthData()
{
    if (m_dataBuffer.size() < m_expectedSize) {
        // 注释掉频繁的警告日志
        // qDebug() << "Warning: Incomplete depth data" << m_dataBuffer.size() << "/" << m_expectedSize;
        return;
    }

    emit depthDataReceived(m_dataBuffer, m_currentBlockId);
}