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

GVSPParser::GVSPParser(QObject *parent)
    : QObject(parent)
    , m_lastBlockId(0)
    , m_imageSequence(0)
{
    m_imageProcessingCount.storeRelaxed(0);
}

GVSPParser::~GVSPParser()
{
}

void GVSPParser::reset()
{
    m_leftIRState = StreamState();
    m_rightIRState = StreamState();
    m_rgbState = StreamState();
    m_depthState = StreamState();
    m_pointCloudState = StreamState();
    m_imageSequence = 0;
}

void GVSPParser::parsePacket(const QByteArray &packet)
{
    static int debugCount = 0;

    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);

    // GigE Vision 2.1 标准 GVSP 包头解析
    uint16_t blockId = ntohs(header->block_id);
    uint8_t packetType = header->packet_format;
    uint16_t packetId = ntohs(header->packet_id);

    // 打印前5个包的详细信息
    if (debugCount < 5) {
    // qDebug() << "[GVSPParser] Packet" << debugCount
    // << "Type:" << packetType
    // << "BlockID:" << blockId
    // << "PacketID:" << packetId
    // << "Size:" << packet.size();
        debugCount++;
    }

    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)
{
    // 最小大小检查：至少要有包头 + 2字节reserved + 2字节payload_type
    if (size < sizeof(GVSPPacketHeader) + 4) {
        qDebug() << "[GVSPParser] Leader packet too small:" << size << "bytes";
        return;
    }

    const GVSPPacketHeader *header = reinterpret_cast<const GVSPPacketHeader*>(data);
    uint32_t blockId = 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
        if (size < sizeof(GVSPPacketHeader) + sizeof(GVSPImageDataLeader)) {
            qDebug() << "[GVSPParser] Image leader too small";
            return;
        }
        const GVSPImageDataLeader *leader = reinterpret_cast<const GVSPImageDataLeader*>(data + sizeof(GVSPPacketHeader));

        uint32_t imageWidth = ntohl(leader->size_x);
        uint32_t imageHeight = ntohl(leader->size_y);
        uint32_t pixelFormat = ntohl(leader->pixel_format);

        // 根据像素格式选择对应的状态
        StreamState *state = nullptr;
        if (pixelFormat == PIXEL_FORMAT_MONO16_LEFT || pixelFormat == PIXEL_FORMAT_MONO8_LEFT) {
            state = &m_leftIRState;
        } else if (pixelFormat == PIXEL_FORMAT_MONO16_RIGHT || pixelFormat == PIXEL_FORMAT_MONO8_RIGHT) {
            state = &m_rightIRState;
        } else if (pixelFormat == PIXEL_FORMAT_MJPEG) {
            state = &m_rgbState;
        } else if (pixelFormat == PIXEL_FORMAT_MONO16 || pixelFormat == PIXEL_FORMAT_12BIT_GRAY) {
            // Legacy格式：根据序号分配
            int imageType = m_imageSequence % 2;
            state = (imageType == 0) ? &m_leftIRState : &m_rightIRState;
        }

        if (state) {
            state->blockId = blockId;
            state->imageWidth = imageWidth;
            state->imageHeight = imageHeight;
            state->pixelFormat = pixelFormat;

            // 根据pixel format设置期望大小
            if (pixelFormat == PIXEL_FORMAT_MJPEG) {
                // MJPEG是压缩格式，实际大小未知，设置为0表示动态接收
                state->expectedSize = 0;
            } else if (pixelFormat == PIXEL_FORMAT_MONO8_LEFT || pixelFormat == PIXEL_FORMAT_MONO8_RIGHT) {
                // 8-bit灰度格式（下采样）
                state->expectedSize = imageWidth * imageHeight;
            } else {
                // 16-bit或12-bit灰度等固定格式
                state->expectedSize = imageWidth * imageHeight * 2;
            }

            state->dataBuffer.clear();
            if (state->expectedSize > 0) {
                state->dataBuffer.reserve(state->expectedSize);
            }
            state->receivedSize = 0;
            state->isReceiving = true;
            state->packetCount = 0;
        }
    }
    else if (payload_type == PAYLOAD_TYPE_BINARY) {
        // Depth data leader
        const GVSPBinaryDataLeader *leader = reinterpret_cast<const GVSPBinaryDataLeader*>(data + sizeof(GVSPPacketHeader));

        m_depthState.blockId = blockId;
        m_depthState.expectedSize = ntohl(leader->file_size);
        m_depthState.dataBuffer.clear();
        m_depthState.dataBuffer.reserve(m_depthState.expectedSize);
        m_depthState.receivedSize = 0;
        m_depthState.isReceiving = true;
        m_depthState.packetCount = 0;
    }
    else if (payload_type == PAYLOAD_TYPE_POINTCLOUD) {
        // Point cloud data leader (vendor-specific 0x8000)
        const GVSPPointCloudDataLeader *leader = reinterpret_cast<const GVSPPointCloudDataLeader*>(data + sizeof(GVSPPacketHeader));

        m_pointCloudState.blockId = blockId;
        m_pointCloudState.expectedSize = ntohl(leader->data_size);
        m_pointCloudState.dataBuffer.clear();
        m_pointCloudState.dataBuffer.reserve(m_pointCloudState.expectedSize);
        m_pointCloudState.receivedSize = 0;
        m_pointCloudState.isReceiving = true;
        m_pointCloudState.packetCount = 0;
    }
}

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

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

    // 查找匹配的状态
    StreamState *state = nullptr;
    if (m_leftIRState.isReceiving && m_leftIRState.blockId == blockId) {
        state = &m_leftIRState;
    } else if (m_rightIRState.isReceiving && m_rightIRState.blockId == blockId) {
        state = &m_rightIRState;
    } else if (m_rgbState.isReceiving && m_rgbState.blockId == blockId) {
        state = &m_rgbState;
    } else if (m_depthState.isReceiving && m_depthState.blockId == blockId) {
        state = &m_depthState;
    } else if (m_pointCloudState.isReceiving && m_pointCloudState.blockId == blockId) {
        state = &m_pointCloudState;
    }

    if (!state) {
        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
    state->dataBuffer.append(reinterpret_cast<const char*>(payload), payload_size);
    state->receivedSize += payload_size;
    state->packetCount++;
}

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

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

    // 查找匹配的状态并处理
    if (m_leftIRState.isReceiving && m_leftIRState.blockId == blockId) {
        processImageData(&m_leftIRState);
        m_leftIRState.isReceiving = false;
        m_lastBlockId = blockId;
    } else if (m_rightIRState.isReceiving && m_rightIRState.blockId == blockId) {
        processImageData(&m_rightIRState);
        m_rightIRState.isReceiving = false;
        m_lastBlockId = blockId;
    } else if (m_rgbState.isReceiving && m_rgbState.blockId == blockId) {
        processImageData(&m_rgbState);
        m_rgbState.isReceiving = false;
        m_lastBlockId = blockId;
    } else if (m_depthState.isReceiving && m_depthState.blockId == blockId) {
        processDepthData(&m_depthState);
        m_depthState.isReceiving = false;
        m_lastBlockId = blockId;
    } else if (m_pointCloudState.isReceiving && m_pointCloudState.blockId == blockId) {
        processPointCloudData(&m_pointCloudState);
        m_pointCloudState.isReceiving = false;
        m_lastBlockId = blockId;
    }
}

void GVSPParser::processImageData(GVSPParser::StreamState *state)
{
    if (!state) return;

    // 处理MJPEG格式（RGB相机）
    if (state->pixelFormat == PIXEL_FORMAT_MJPEG) {
        // RGB MJPEG
        emit rgbImageReceived(state->dataBuffer, state->blockId);
        m_imageSequence++;
        return;
    }

    // 处理Mono16格式（左右红外相机原始16位数据）
    // 使用像素格式直接区分左右，不依赖接收顺序
    if (state->pixelFormat == PIXEL_FORMAT_MONO16_LEFT) {
        // 检查数据大小
        if (state->dataBuffer.size() < state->expectedSize) {
            return;
        }
        // 左红外原始数据
        emit leftImageReceived(state->dataBuffer, state->blockId);
        m_imageSequence++;
        return;
    }

    if (state->pixelFormat == PIXEL_FORMAT_MONO16_RIGHT) {
        // 检查数据大小
        if (state->dataBuffer.size() < state->expectedSize) {
            return;
        }
        // 右红外原始数据
        emit rightImageReceived(state->dataBuffer, state->blockId);
        m_imageSequence++;
        return;
    }

    // 处理Mono8格式（左右红外相机下采样8位数据）
    if (state->pixelFormat == PIXEL_FORMAT_MONO8_LEFT) {
        // 检查数据大小
        if (state->dataBuffer.size() < state->expectedSize) {
            return;
        }
        // 左红外8位数据
        emit leftImageReceived(state->dataBuffer, state->blockId);
        m_imageSequence++;
        return;
    }

    if (state->pixelFormat == PIXEL_FORMAT_MONO8_RIGHT) {
        // 检查数据大小
        if (state->dataBuffer.size() < state->expectedSize) {
            return;
        }
        // 右红外8位数据
        emit rightImageReceived(state->dataBuffer, state->blockId);
        m_imageSequence++;
        return;
    }

    // 兼容旧版本：使用序号区分（legacy）
    if (state->pixelFormat == PIXEL_FORMAT_MONO16) {
        // 检查数据大小
        if (state->dataBuffer.size() < state->expectedSize) {
            return;
        }

        // 根据图像序号区分：偶数=左红外，奇数=右红外
        int imageType = m_imageSequence % 2;

        if (imageType == 0) {
            // 左红外原始数据
            emit leftImageReceived(state->dataBuffer, state->blockId);
        } else {
            // 右红外原始数据
            emit rightImageReceived(state->dataBuffer, state->blockId);
        }

        m_imageSequence++;
        return;
    }

    // 处理12-bit灰度格式 - 固定大小，需要检查
    if (state->dataBuffer.size() < state->expectedSize) {
        return;
    }

    // 处理12-bit灰度图像（左右红外相机）
    if (state->pixelFormat != PIXEL_FORMAT_12BIT_GRAY) {
        return;
    }

    // 节流机制：如果已有3个或更多图像在处理中，跳过当前帧
    if (m_imageProcessingCount.loadAcquire() >= 3) {
        m_imageSequence++;
        return;
    }

    // 增加处理计数
    m_imageProcessingCount.ref();

    // 复制数据到局部变量
    QByteArray dataCopy = state->dataBuffer;
    uint32_t blockId = state->blockId;
    size_t width = state->imageWidth;
    size_t height = state->imageHeight;
    int imageSeq = m_imageSequence;

    // 使用QtConcurrent在后台线程处理图像数据
    QtConcurrent::run([this, dataCopy, blockId, width, height, imageSeq]() {
        // Convert 16-bit depth data to 8-bit grayscale for display
        const uint16_t *src = reinterpret_cast<const uint16_t*>(dataCopy.constData());
        QImage image(width, height, QImage::Format_Grayscale8);

        // 优化：使用采样方式快速估算min/max（每隔16个像素采样一次）
        uint16_t minVal = 65535, maxVal = 0;
        size_t totalPixels = width * height;
        const size_t sampleStep = 16;  // 采样步长

        for (size_t i = 0; i < totalPixels; i += sampleStep) {
            uint16_t val = src[i];
            if (val > 0) {
                if (val < minVal) minVal = val;
                if (val > maxVal) maxVal = val;
            }
        }

        // 归一化（不翻转，保持原始方向）
        uint8_t *dst = image.bits();
        float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;

        for (size_t row = 0; row < height; row++) {
            for (size_t col = 0; col < width; col++) {
                size_t idx = row * width + col;
                uint16_t val = src[idx];
                dst[idx] = (val == 0) ? 0 : static_cast<uint8_t>((val - minVal) * scale);
            }
        }

        // 注意：此代码路径已废弃，下位机现在发送JPEG格式
        // 仅保留兼容旧代码的信号
        emit imageReceived(image, blockId);

        // 减少处理计数
        m_imageProcessingCount.deref();
    });

    // 增加图像序号
    m_imageSequence++;
}

void GVSPParser::processDepthData(GVSPParser::StreamState *state)
{
    if (!state) return;

    if (state->dataBuffer.size() < state->expectedSize) {
        return;
    }

    emit depthDataReceived(state->dataBuffer, state->blockId);
}

void GVSPParser::processPointCloudData(GVSPParser::StreamState *state)
{
    if (!state) return;

    if (state->dataBuffer.size() < state->expectedSize) {
        return;
    }

    // 点云数据直接发送，格式为 short 数组 (x, y, z, x, y, z, ...)
    emit pointCloudDataReceived(state->dataBuffer, state->blockId);
}