#include "HoleDetectionPresenter.h"
#include "VrLog.h"
#include "VrError.h"

// 初始化TCP服务器
int HoleDetectionPresenter::InitTCPServer()
{
    LOG_DEBUG("Start initializing TCP server\n");

    if (m_pTCPServer) {
        LOG_WARNING("TCP server already initialized\n");
        return 0;
    }

    m_pTCPServer = new TCPServerProtocol();

    // 从配置获取TCP协议端口
    ConfigResult configResult = m_pConfigManager->GetConfigResult();
    int port = configResult.plcRobotServerConfig.tcpServerPort;

    int nRet = m_pTCPServer->Initialize(port);
    if (nRet != 0) {
        LOG_ERROR("Failed to initialize TCP server, return code: %d\n", nRet);
        delete m_pTCPServer;
        m_pTCPServer = nullptr;
        return nRet;
    }

    // 设置连接状态回调
    m_pTCPServer->SetConnectionCallback([this](bool connected) {
        this->OnTCPConnectionChanged(connected);
    });

    // 设置检测触发回调（文本协议：cameraIndex + robotPose）
    m_pTCPServer->SetDetectionTriggerCallback([this](int cameraIndex, const RobotFlangePose& robotPose) {
        return this->OnTCPDetectionTrigger(cameraIndex, robotPose);
    });

    LOG_INFO("TCP server protocol initialized successfully on port %d\n", port);
    return 0;
}

// TCP连接状态改变回调
void HoleDetectionPresenter::OnTCPConnectionChanged(bool connected)
{
    LOG_DEBUG("TCP connection status changed: %s\n", connected ? "connected" : "disconnected");

    m_bTCPConnected = connected;

    if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
        if (connected) {
            pStatus->OnStatusUpdate("TCP客户端已连接");
        } else {
            pStatus->OnStatusUpdate("TCP客户端已断开");
        }
        pStatus->OnRobotConnectionChanged(connected);
    }

    CheckAndUpdateWorkStatus();
}

// TCP检测触发回调
bool HoleDetectionPresenter::OnTCPDetectionTrigger(int cameraIndex, const RobotFlangePose& robotPose)
{
    if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
        pStatus->OnWorkStatusChanged(WorkStatus::Working);
    }

    LOG_DEBUG("TCP detection trigger: cameraIndex=%d, robotPose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n",
              cameraIndex, robotPose.x, robotPose.y, robotPose.z,
              robotPose.roll, robotPose.pitch, robotPose.yaw);

    if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
        pStatus->OnStatusUpdate("【TCP协议】接收到检测触发信号");
    }

    // 保存当前机器人法兰位姿
    m_currentRobotPose = robotPose;

    // 检查相机连接状态
    if (!m_bCameraConnected) {
        LOG_ERROR("Camera not connected, cannot start detection\n");
        if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
            pStatus->OnStatusUpdate("【TCP协议】相机未连接，无法启动检测");
        }
        return false;
    }

    // 启动检测
    int result = StartDetection(cameraIndex, false);
    bool success = (result == 0);

    if (success) {
        LOG_DEBUG("Detection started successfully via TCP trigger\n");
        if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
            pStatus->OnStatusUpdate("开始执行扫描检测...");
        }
    } else {
        LOG_ERROR("Failed to start detection via TCP trigger, error code: %d\n", result);
        if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
            pStatus->OnStatusUpdate("检测启动失败");
        }
    }

    return success;
}

// 发送检测结果给TCP客户端（文本格式）
void HoleDetectionPresenter::_SendDetectionResultToTCP(const HoleDetectionResult& detectionResult, int cameraIndex)
{
    if (!m_pTCPServer || !m_bTCPConnected) {
        LOG_DEBUG("TCP server not available, skipping result transmission\n");
        return;
    }

    LOG_DEBUG("Sending detection result to TCP clients, camera index: %d\n", cameraIndex);

    // 构建文本格式：PointNum_X1_Y1_Z1_Roll1_Pitch1_Yaw1/X2_Y2_Z2_Roll2_Pitch2_Yaw2/
    int holeCount = static_cast<int>(detectionResult.positions.size());
    std::string resultText = std::to_string(holeCount);

    for (size_t i = 0; i < detectionResult.positions.size(); i++) {
        const auto& pos = detectionResult.positions[i];
        char buf[256];
        snprintf(buf, sizeof(buf), "_%.2f_%.2f_%.2f_%.2f_%.2f_%.2f/", pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
        resultText += buf;
    }

    LOG_INFO("TCP result text: %s\n", resultText.c_str());

    int sendResult = m_pTCPServer->SendTextResult(resultText);
    if (sendResult == 0) {
        LOG_INFO("Detection result sent to TCP clients successfully\n");
        if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
            pStatus->OnStatusUpdate("【TCP协议】已发送检测结果");
        }
    } else {
        LOG_ERROR("Failed to send detection result to TCP clients, error code: %d\n", sendResult);
    }
}