#include "GrabBagPresenter.h"
#include "VrError.h"
#include "VrSimpleLog.h"
#include <QtCore/QCoreApplication>
#include <QtCore/QFileInfo>
#include <QtCore/QDir>

GrabBagPresenter::GrabBagPresenter()
    : m_vrConfig(nullptr)
    , m_vrEyeDevice(nullptr)
    , m_pStatus(nullptr)
    , m_pRobotProtocol(nullptr)
{

}

GrabBagPresenter::~GrabBagPresenter()
{
    // 释放机械臂协议
    if (m_pRobotProtocol) {
        m_pRobotProtocol->Deinitialize();
        delete m_pRobotProtocol;
        m_pRobotProtocol = nullptr;
    }
    
    // 释放其他资源
    if (m_vrEyeDevice) {
        // 关闭设备
        m_vrEyeDevice->CloseDevice();
    }
}

int GrabBagPresenter::Init()
{
    // 初始化VrConfig模块
    IVrConfig::CreateInstance(&m_vrConfig);

    // 获取可执行文件路径
    QString exePath = QCoreApplication::applicationFilePath();

    // 获取可执行文件所在目录
    QString configPath = QFileInfo(exePath).absoluteDir().path() + "/config.xml";
    
    // 读取IP列表
    ConfigResult configResult = m_vrConfig->LoadConfig(configPath.toStdString());
    if (configResult.cameraList.size() == 0) {
        m_pStatus->OnStatusUpdate("读取配置信息列表失败");
        m_pStatus->OnCamera1StatusChanged(false);
        m_pStatus->OnCamera2StatusChanged(false);
        m_pStatus->OnCameraCountChanged(0);
        return ERR_CODE(DEV_CONFIG_ERR);
    }

    // 初始化机械臂协议
    int nRet = InitRobotProtocol();
    if (nRet != 0) {
        m_pStatus->OnStatusUpdate("机械臂服务初始化失败");
        m_pStatus->OnWorkStatusChanged(WorkStatus::Error);
    } else {
        m_pStatus->OnStatusUpdate("机械臂服务初始化成功");
    }
    
    // 通知UI相机个数
    int cameraCount = configResult.cameraList.size();
    m_pStatus->OnCameraCountChanged(cameraCount);
    
    // 初始化VrEyeDevice模块
    IVrEyeDevice::CreateObject(&m_vrEyeDevice);
    
    nRet = m_vrEyeDevice->InitDevice();
    ERR_CODE_RETURN(nRet);


    if (configResult.cameraList.size() >= 1) {
        // 尝试打开相机1
        nRet = m_vrEyeDevice->OpenDevice(configResult.cameraList[0].ip.c_str(), &GrabBagPresenter::_StaticCameraNotify, this);
        // 通过回调更新相机1状态
        m_pStatus->OnCamera1StatusChanged(SUCCESS == nRet);
        m_pStatus->OnStatusUpdate(SUCCESS == nRet ? "相机1连接成功" : "相机1连接失败");
    }
    else {
        m_pStatus->OnCamera1StatusChanged(false);
    }
    m_pStatus->OnWorkStatusChanged(SUCCESS == nRet ? WorkStatus::Ready : WorkStatus::Error);
    ERR_CODE_RETURN(nRet);
    
    if (configResult.cameraList.size() >= 2) {
        // 尝试打开相机2
        nRet = m_vrEyeDevice->OpenDevice(configResult.cameraList[1].ip.c_str(), &GrabBagPresenter::_StaticCameraNotify, this);
        // 通过回调更新相机2状态
        m_pStatus->OnCamera2StatusChanged(SUCCESS == nRet);
        m_pStatus->OnStatusUpdate(SUCCESS == nRet ? "相机2连接成功" : "相机2连接失败");
    }
    else {
        // 如果只有一个相机，则将相机2状态设为未连接
        m_pStatus->OnCamera2StatusChanged(false);
    }
    ERR_CODE_RETURN(nRet);

    m_pStatus->OnStatusUpdate("设备初始化完成");
    m_pStatus->OnWorkStatusChanged(SUCCESS == nRet ? WorkStatus::Ready : WorkStatus::Error);
    return SUCCESS;
}

// 初始化机械臂协议
int GrabBagPresenter::InitRobotProtocol()
{
    LOG_DEBUG("GrabBagPresenter::InitRobotProtocol - 开始初始化机械臂协议\n");
    
    // 创建RobotProtocol实例
    if(nullptr == m_pRobotProtocol){
        m_pRobotProtocol = new RobotProtocol();
    }
    
    // 设置连接状态回调
    m_pRobotProtocol->SetConnectionCallback([this](bool connected) {
        this->OnRobotConnectionChanged(connected);
    });

    // 设置工作信号回调
    m_pRobotProtocol->SetWorkSignalCallback([this](bool startWork) {
        this->OnRobotWorkSignal(startWork);
    });
    
    // 初始化协议服务（使用端口503，避免与原有ModbusTCP冲突）
    int nRet = m_pRobotProtocol->Initialize();
    ERR_CODE_RETURN(nRet);

    LOG_INFO("GrabBagPresenter::InitRobotProtocol - 机械臂协议初始化成功\n");
    return SUCCESS;
    
}

// 机械臂协议回调函数实现
void GrabBagPresenter::OnRobotConnectionChanged(bool connected)
{
    LOG_INFO("GrabBagPresenter::OnRobotConnectionChanged - 机械臂连接状态变化:%s\n", (connected ? "已连接" : "已断开"));
    
    if (m_pStatus) {
        m_pStatus->OnRobotConnectionChanged(connected);
        m_pStatus->OnStatusUpdate(connected ? "机械臂已连接" : "机械臂连接断开");
    }
}

void GrabBagPresenter::OnRobotWorkSignal(bool startWork)
{
    LOG_INFO("GrabBagPresenter::OnRobotWorkSignal - 收到机械臂工作信号:%s\n", (startWork ? "开始工作" : "停止工作"));
    
    if (nullptr == m_pStatus) {
        return;
    }
    if (startWork) {
        m_pStatus->OnStatusUpdate("收到开始工作信号，启动检测");
        StartDetection();
    } else {
        m_pStatus->OnStatusUpdate("收到停止工作信号，停止检测");
        StopDetection();
    }
}

void GrabBagPresenter::SetStatusCallback(IYGrabBagStatus* status)
{
    m_pStatus = status;
}

// 模拟检测函数，用于演示
int GrabBagPresenter::StartDetection()
{    
    LOG_INFO("GrabBagPresenter::StartDetection - 开始检测\n");
    
    // 通知UI工作状态变更为"正在工作"
    if (m_pStatus) {
        m_pStatus->OnWorkStatusChanged(WorkStatus::Working);
    }

    // 创建测试图像
    QImage image(":/resource/result_image.jpg");
    
    // 创建测试数据
    DetectionResult testResult;
    testResult.image = image;
    
    // 创建第一层数据
    GrabBagPositionLayout layer1;
    layer1.layerIndex = 1;
    
    GrabBagPosition pos1;
    pos1.x = 100.5;
    pos1.y = 200.3;
    pos1.z = 50.22;
    pos1.roll = 45.0;
    pos1.pitch = 10.0;
    pos1.yaw = 20.0;
    
    GrabBagPosition pos2;
    pos2.x = 110.8;
    pos2.y = 220.5;
    pos2.z = 52.1;
    pos2.roll = 48.5;
    pos2.pitch = 12.3;
    pos2.yaw = 22.7;
    
    layer1.position.push_back(pos1);
    layer1.position.push_back(pos2);
    
    // 创建第二层数据
    GrabBagPositionLayout layer2;
    layer2.layerIndex = 2;
    
    GrabBagPosition pos3;
    pos3.x = 150.2;
    pos3.y = 180.7;
    pos3.z = 55.0;
    pos3.roll = 30.0;
    pos3.pitch = 15.0;
    pos3.yaw = 25.0;
    
    layer2.position.push_back(pos3);
    
    // 添加层数据到结果
    testResult.positionLayout.push_back(layer1);
    testResult.positionLayout.push_back(layer2);
    
    // 调用检测结果回调函数
    m_pStatus->OnDetectionResult(testResult);
    
    // 更新状态
    m_pStatus->OnStatusUpdate("检测完成，发现3个目标");
    
    // 检测完成后，将工作状态更新为"完成"
    if (m_pStatus) {
        m_pStatus->OnWorkStatusChanged(WorkStatus::Completed);
    }
    
    // 更新机械臂协议状态
    if (m_pRobotProtocol) {
        
        // 更新检测结果坐标（以第一个检测到的目标为例）
        if (!testResult.positionLayout.empty() && !testResult.positionLayout[0].position.empty()) {
            const auto& firstPos = testResult.positionLayout[0].position[0];
            RobotProtocol::RobotCoordinate detectedCoord;
            detectedCoord.x = firstPos.x;
            detectedCoord.y = firstPos.y;
            detectedCoord.z = firstPos.z;
            detectedCoord.rx = firstPos.roll;
            detectedCoord.ry = firstPos.pitch;
            detectedCoord.rz = firstPos.yaw;
            m_pRobotProtocol->SetRobotCoordinate(detectedCoord);
            
            LOG_INFO("GrabBagPresenter::StartDetection - 更新检测结果坐标: X=%.2f, Y=%.2f, Z=%.2f\n", 
                     detectedCoord.x, detectedCoord.y, detectedCoord.z);
                 }
    }

    return 0;
}

int GrabBagPresenter::StopDetection()
{
    LOG_INFO("GrabBagPresenter::StopDetection - 停止检测\n");
    
    // 通知UI工作状态变更为"就绪"
    if (m_pStatus) {
        m_pStatus->OnWorkStatusChanged(WorkStatus::Ready);
        m_pStatus->OnStatusUpdate("检测已停止");
    }

    
    return 0;
}

// 静态回调函数实现
void GrabBagPresenter::_StaticCameraNotify(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
{
    // 从pInfoParam获取this指针，转换回GrabBagPresenter*类型
    GrabBagPresenter* pThis = reinterpret_cast<GrabBagPresenter*>(pInfoParam);
    if (pThis)
    {
        // 调用实例的非静态成员函数
        pThis->_CameraNotify(eStatus, pExtData, nDataLength, pInfoParam);
    }
}

void GrabBagPresenter::_CameraNotify(EVzDeviceWorkStatus eStatus, void *pExtData, unsigned int nDataLength, void *pInfoParam)
{

}