#include "BinocularMarkReceiver.h"
#include "json/json.h"
#include "VrLog.h"
#include <algorithm>
#include <chrono>
#include <cstring>


using namespace  VA;

// 静态工厂方法实现
int IBinocularMarkReceiver::CreateInstance(IBinocularMarkReceiver** ppReceiver)
{
    if (!ppReceiver) {
        return -1;
    }

    *ppReceiver = new BinocularMarkReceiver();
    return 0;
}

BinocularMarkReceiver::BinocularMarkReceiver()
    : m_pTcpClient(nullptr)
    , m_serverPort(0)
    , m_bConnected(false)
    , m_bSingleDetectionReady(false)
    , m_bImageDataReady(false)
    , m_bHeartbeatRunning(false)
    , m_nHeartbeatInterval(30)
{
    m_pTcpClient = IVrTCPClient::CreateInstance();
}

BinocularMarkReceiver::~BinocularMarkReceiver()
{
    Disconnect();

    if (m_pTcpClient) {
        IVrTCPClient::DestroyInstance(m_pTcpClient);
        m_pTcpClient = nullptr;
    }
}

int BinocularMarkReceiver::Connect(const std::string& serverIp, uint16_t serverPort)
{
    if (m_bConnected) {
        LOG_DEBUG("Already connected\n");
        return 0;
    }

    m_serverIp = serverIp;
    m_serverPort = serverPort;

    LOG_DEBUG("Connecting to %s:%d\n", serverIp.c_str(), serverPort);

    // 连接到服务器
    int ret = m_pTcpClient->LinkDevice(serverIp, serverPort, false, linkEventCallback, this);
    if (ret != 0) {
        LOG_DEBUG("LinkDevice failed: %d\n", ret);
        return -1;
    }

    // 启动工作线程
    ret = m_pTcpClient->StartWork(tcpRecvCallback, this);
    if (ret != 0) {
        LOG_DEBUG("StartWork failed: %d\n", ret);
        m_pTcpClient->CloseDevice();
        return -1;
    }

    LOG_DEBUG("Connect initiated\n");
    return 0;
}

int BinocularMarkReceiver::Disconnect()
{
    if (!m_bConnected) {
        return 0;
    }

    // 停止心跳线程
    m_bHeartbeatRunning = false;
    if (m_heartbeatThread.joinable()) {
        m_heartbeatThread.join();
    }

    // 断开连接
    if (m_pTcpClient) {
        m_pTcpClient->CloseDevice();
    }

    m_bConnected = false;
    m_dataBuffer.clear();

    return 0;
}

bool BinocularMarkReceiver::IsConnected() const
{
    return m_bConnected;
}

int BinocularMarkReceiver::TriggerDetection()
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());

    Json::FastWriter writer;
    std::string jsonData = writer.write(root);

    return sendJsonMessage("cmd_trigger", jsonData);
}

void BinocularMarkReceiver::SetMarkResultCallback(MarkResultCallback callback)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    m_markResultCallback = callback;
}

void BinocularMarkReceiver::SetEventCallback(EventCallback callback)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    m_eventCallback = callback;
}

// TCP连接状态回调
void BinocularMarkReceiver::linkEventCallback(IVrTCPClient* pClient, bool connected, void* pParam)
{
    BinocularMarkReceiver* pThis = static_cast<BinocularMarkReceiver*>(pParam);
    if (!pThis) return;

    pThis->m_bConnected = connected;

    if (connected) {
        LOG_DEBUG("Connected to server\n");
        // 触发连接事件
        if (pThis->m_eventCallback) {
            pThis->m_eventCallback(ReceiverEventType::CONNECTED, "");
        }

        // 启动心跳线程
        pThis->m_bHeartbeatRunning = true;
        pThis->m_heartbeatThread = std::thread(&BinocularMarkReceiver::heartbeatThreadFunc, pThis);
    } else {
        LOG_DEBUG("Disconnected from server\n");
        // 触发断开连接事件
        if (pThis->m_eventCallback) {
            pThis->m_eventCallback(ReceiverEventType::DISCONNECTED, "");
        }
    }
}

// TCP数据接收回调
void BinocularMarkReceiver::tcpRecvCallback(IVrTCPClient* pClient, const char* pData, const int nLen, void* pParam)
{
    BinocularMarkReceiver* pThis = static_cast<BinocularMarkReceiver*>(pParam);
    if (!pThis || !pData || nLen <= 0) return;

    // 追加到缓冲区
    {
        std::lock_guard<std::mutex> lock(pThis->m_mutex);
        pThis->m_dataBuffer.insert(pThis->m_dataBuffer.end(), pData, pData + nLen);
    }

    // 解析数据帧
    pThis->parseFrames();
}

// 心跳线程函数
void BinocularMarkReceiver::heartbeatThreadFunc()
{
    while (m_bHeartbeatRunning) {
        std::this_thread::sleep_for(std::chrono::seconds(m_nHeartbeatInterval));

        if (!m_bHeartbeatRunning) break;

        // 发送心跳消息
        Json::Value root;
        root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
            std::chrono::system_clock::now().time_since_epoch()).count());

        Json::FastWriter writer;
        std::string jsonData = writer.write(root);

        sendJsonMessage("heartbeat", jsonData);
    }
}

void BinocularMarkReceiver::parseFrames()
{
    std::lock_guard<std::mutex> lock(m_mutex);

    while (true) {
        // 查找帧头
        auto it = std::search(m_dataBuffer.begin(), m_dataBuffer.end(), FRAME_HEADER, FRAME_HEADER + FRAME_HEADER_SIZE);
        if (it == m_dataBuffer.end()) {
            m_dataBuffer.clear();
            break;
        }

        // 丢弃帧头之前的数据
        if (it != m_dataBuffer.begin()) {
            m_dataBuffer.erase(m_dataBuffer.begin(), it);
        }

        // 检查是否有足够的数据来读取长度字段
        if (m_dataBuffer.size() < FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE) {
            break;
        }

        // 读取数据长度（8字节ASCII字符串格式）
        std::string lengthStr(m_dataBuffer.begin() + FRAME_HEADER_SIZE, m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE);
        int64_t dataLength = std::stoll(lengthStr);

        // 检查数据长度是否合理（最大10MB）
        if (dataLength < 0 || dataLength > 10 * 1024 * 1024) {
            m_dataBuffer.erase(m_dataBuffer.begin(), m_dataBuffer.begin() + FRAME_HEADER_SIZE);
            continue;
        }

        // 计算完整帧的总长度
        size_t totalFrameLength = FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength + FRAME_TAIL_SIZE;

        // 检查是否有完整的帧
        if (m_dataBuffer.size() < totalFrameLength) {
            break;
        }

        // 提取JSON数据
        std::string jsonData(m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE,
                            m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength);

        // 验证帧尾
        std::string tail(m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength,
                        m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength + FRAME_TAIL_SIZE);
        if (tail != FRAME_TAIL) {
            m_dataBuffer.erase(m_dataBuffer.begin(), m_dataBuffer.begin() + FRAME_HEADER_SIZE);
            continue;
        }

        // 处理JSON消息
        handleJsonMessage(jsonData);

        // 移除已处理的帧
        m_dataBuffer.erase(m_dataBuffer.begin(), m_dataBuffer.begin() + totalFrameLength);
    }
}

void BinocularMarkReceiver::handleJsonMessage(const std::string& jsonData)
{
    Json::Reader reader;
    Json::Value root;

    if (!reader.parse(jsonData, root)) {
        LOG_DEBUG("JSON parse failed\n");
        return;
    }

    if (!root.isObject()) {
        LOG_DEBUG("JSON is not an object\n");
        return;
    }

    // 获取消息类型
    std::string messageType = root.get("msg_type", "").asString();
    LOG_DEBUG("Received message type: %s\n", messageType.c_str());

    if (messageType == "mark_result") {
        handleMarkResult(jsonData);
    } else if (messageType == "single_detection_result") {
        handleSingleDetectionResult(jsonData);
    } else if (messageType == "image_data") {
        handleImageData(jsonData);
    } else if (messageType == "heartbeat_ack") {
        handleHeartbeatAck(jsonData);
    } else if (messageType == "cmd_response") {
        handleCommandResponse(jsonData);
    } else {
        LOG_DEBUG("Unknown message type: %s\n", messageType.c_str());
    }
}

void BinocularMarkReceiver::handleMarkResult(const std::string& jsonStr)
{
    Json::Reader reader;
    Json::Value root;
    if (!reader.parse(jsonStr, root)) return;

    int64_t timestamp = root.get("timestamp", 0).asInt64();
    int errorCode = root.get("error_code", -1).asInt();

    std::vector<VrMark3D> marks;

    if (errorCode == 0 && root.isMember("marks")) {
        const Json::Value& marksArray = root["marks"];
        for (unsigned int i = 0; i < marksArray.size(); i++) {
            VrMark3D mark;
            mark.markID = marksArray[i].get("mark_id", -1).asInt();
            mark.x = marksArray[i].get("x", 0.0).asDouble();
            mark.y = marksArray[i].get("y", 0.0).asDouble();
            mark.z = marksArray[i].get("z", 0.0).asDouble();
            marks.push_back(mark);
        }
    }

    if (m_markResultCallback) {
        m_markResultCallback(marks, timestamp, errorCode);
    }
}

void BinocularMarkReceiver::handleHeartbeatAck(const std::string& jsonStr)
{
    // 心跳应答，暂时不处理
}

void BinocularMarkReceiver::handleCommandResponse(const std::string& jsonStr)
{
    // 命令应答，暂时不处理
}

std::string BinocularMarkReceiver::buildFrame(const std::string& jsonData)
{
    std::string frame;

    // 添加帧头
    frame.append(FRAME_HEADER, FRAME_HEADER_SIZE);

    // 添加数据长度（8字节ASCII字符串格式）
    char lengthStr[9];
    snprintf(lengthStr, sizeof(lengthStr), "%08zu", jsonData.size());
    frame.append(lengthStr, FRAME_LENGTH_SIZE);

    // 添加JSON数据
    frame.append(jsonData);

    // 添加帧尾
    frame.append(FRAME_TAIL, FRAME_TAIL_SIZE);

    return frame;
}

int BinocularMarkReceiver::sendJsonMessage(const std::string& messageType, const std::string& jsonData)
{
    if (!m_bConnected) {
        return -1;
    }

    // 解析JSON并添加msg_type字段
    Json::Reader reader;
    Json::Value root;
    if (!reader.parse(jsonData, root)) {
        root = Json::Value(Json::objectValue);
    }
    root["msg_type"] = messageType;

    // 转换为JSON字符串
    Json::FastWriter writer;
    std::string finalJsonData = writer.write(root);

    // 构造数据帧
    std::string frame = buildFrame(finalJsonData);

    // 发送数据
    if (!m_pTcpClient->SendData(frame.c_str(), frame.size())) {
        return -1;
    }

    return 0;
}

// ========== 新增接口实现 ==========

IBinocularMarkReceiver::SingleDetectionResult BinocularMarkReceiver::RequestSingleDetection(int timeoutMs)
{
    SingleDetectionResult result;
    result.errorCode = -1;

    if (!m_bConnected) {
        return result;
    }

    // 发送请求
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);

    m_bSingleDetectionReady = false;
    if (sendJsonMessage("cmd_single_detection", jsonData) != 0) {
        return result;
    }

    // 使用condition_variable同步等待结果
    std::unique_lock<std::mutex> lock(m_mutex);
    if (m_cvSingleDetection.wait_for(lock, std::chrono::milliseconds(timeoutMs),
                                     [this] { return m_bSingleDetectionReady; })) {
        return m_pendingSingleDetectionResult;
    }

    result.errorCode = -2; // 超时
    return result;
}

IBinocularMarkReceiver::ImageData BinocularMarkReceiver::RequestSingleImage(int timeoutMs)
{
    ImageData result;
    result.timestamp = 0;

    if (!m_bConnected) {
        return result;
    }

    // 发送请求
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);

    m_bImageDataReady = false;
    if (sendJsonMessage("cmd_single_image", jsonData) != 0) {
        return result;
    }

    // 使用condition_variable同步等待结果
    std::unique_lock<std::mutex> lock(m_mutex);
    if (m_cvImageData.wait_for(lock, std::chrono::milliseconds(timeoutMs), [this] { return m_bImageDataReady; })) {
        return m_pendingImageData;
    }

    return result;
}

int BinocularMarkReceiver::StartWork()
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_start_work", jsonData);
}

int BinocularMarkReceiver::StopWork()
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_stop_work", jsonData);
}

int BinocularMarkReceiver::SetCalibrationMatrix(const std::string& calibrationXml)
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    root["calibration_xml"] = calibrationXml;
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_set_calibration", jsonData);
}

int BinocularMarkReceiver::SetExposureTime(double exposureTime)
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    root["exposure_time"] = exposureTime;
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_set_exposure_time", jsonData);
}

int BinocularMarkReceiver::SetGain(double gain)
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    root["gain"] = gain;
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_set_gain", jsonData);
}

void BinocularMarkReceiver::handleSingleDetectionResult(const std::string& jsonStr)
{
    LOG_DEBUG("handleSingleDetectionResult start, JSON size: %zu\n", jsonStr.size());

    Json::Reader reader;
    Json::Value root;
    if (!reader.parse(jsonStr, root)) {
        LOG_DEBUG("handleSingleDetectionResult JSON parse failed\n");
        return;
    }

    LOG_DEBUG("handleSingleDetectionResult JSON parsed\n");

    int64_t timestamp = root.get("timestamp", 0).asInt64();
    int errorCode = root.get("error_code", -1).asInt();
    std::string leftImageBase64 = root.get("left_image", "").asString();
    std::string rightImageBase64 = root.get("right_image", "").asString();

    LOG_DEBUG("handleSingleDetectionResult got data: timestamp=%lld, error=%d, left_size=%zu, right_size=%zu\n",
              timestamp, errorCode, leftImageBase64.size(), rightImageBase64.size());

    std::vector<VrMark3D> marks;
    if (errorCode == 0 && root.isMember("marks")) {
        const Json::Value& marksArray = root["marks"];
        for (unsigned int i = 0; i < marksArray.size(); i++) {
            VrMark3D mark;
            mark.markID = marksArray[i].get("mark_id", -1).asInt();
            mark.x = marksArray[i].get("x", 0.0).asDouble();
            mark.y = marksArray[i].get("y", 0.0).asDouble();
            mark.z = marksArray[i].get("z", 0.0).asDouble();
            marks.push_back(mark);
        }
    }

    LOG_DEBUG("handleSingleDetectionResult parsed %zu marks\n", marks.size());

    // 存储结果并通知等待的线程
    {
        // std::lock_guard<std::mutex> lock(m_mutex);
        LOG_DEBUG("handleSingleDetectionResult storing data\n");
        m_pendingSingleDetectionResult.marks = marks;
        m_pendingSingleDetectionResult.leftImageBase64 = leftImageBase64;
        m_pendingSingleDetectionResult.rightImageBase64 = rightImageBase64;
        m_pendingSingleDetectionResult.timestamp = timestamp;
        m_pendingSingleDetectionResult.errorCode = errorCode;
        m_bSingleDetectionReady = true;
        LOG_DEBUG("handleSingleDetectionResult data stored\n");
    }
    LOG_DEBUG("handleSingleDetectionResult notifying\n");
    m_cvSingleDetection.notify_one();
    LOG_DEBUG("handleSingleDetectionResult complete\n");
}

void BinocularMarkReceiver::handleImageData(const std::string& jsonStr)
{
    LOG_DEBUG("handleImageData start, JSON size: %zu\n", jsonStr.size());

    Json::Reader reader;
    Json::Value root;
    if (!reader.parse(jsonStr, root)) {
        LOG_DEBUG("handleImageData JSON parse failed\n");
        return;
    }

    LOG_DEBUG("handleImageData JSON parsed\n");

    int64_t timestamp = root.get("timestamp", 0).asInt64();
    std::string leftImageBase64 = root.get("left_image", "").asString();
    std::string rightImageBase64 = root.get("right_image", "").asString();

    LOG_DEBUG("handleImageData got data: timestamp=%lld, left_size=%zu, right_size=%zu\n",  timestamp, leftImageBase64.size(), rightImageBase64.size());

    // 存储结果并通知等待的线程
    {
        // std::lock_guard<std::mutex> lock(m_mutex);
        LOG_DEBUG("handleImageData storing data\n");
        m_pendingImageData.leftImageBase64 = leftImageBase64;
        m_pendingImageData.rightImageBase64 = rightImageBase64;
        m_pendingImageData.timestamp = timestamp;
        m_bImageDataReady = true;
        LOG_DEBUG("handleImageData data stored\n");
    }
    LOG_DEBUG("handleImageData notifying\n");
    m_cvImageData.notify_one();
    LOG_DEBUG("handleImageData complete\n");
}