#include "VrConfig.h"
#include <iostream>
#include <vector>
#include <string>

#include "VrLog.h"
#include "ConfigXmlUtils.h"

using namespace tinyxml2;

namespace {

void LoadWorkpieceProfileParams(
    XMLElement* parentElement,
    VrWorkpieceAlgorithmProfile& profile)
{
    if (!parentElement) {
        return;
    }

    XMLElement* workpieceHoleParamElement = parentElement->FirstChildElement("WorkpieceHoleParam");
    if (workpieceHoleParamElement)
    {
        if (workpieceHoleParamElement->Attribute("workpieceType"))
            profile.workpieceHoleParam.workpieceType = workpieceHoleParamElement->IntAttribute("workpieceType");
        if (workpieceHoleParamElement->Attribute("holeDiameter"))
            profile.workpieceHoleParam.holeDiameter = workpieceHoleParamElement->DoubleAttribute("holeDiameter");
        if (workpieceHoleParamElement->Attribute("bigHoleDiameter"))
            profile.workpieceHoleParam.bigHoleDiameter = workpieceHoleParamElement->DoubleAttribute("bigHoleDiameter");
        if (workpieceHoleParamElement->Attribute("holeDist_L"))
            profile.workpieceHoleParam.holeDist_L = workpieceHoleParamElement->DoubleAttribute("holeDist_L");
        if (workpieceHoleParamElement->Attribute("holeDist_W"))
            profile.workpieceHoleParam.holeDist_W = workpieceHoleParamElement->DoubleAttribute("holeDist_W");
        if (workpieceHoleParamElement->Attribute("xLen"))
            profile.workpieceHoleParam.xLen = workpieceHoleParamElement->DoubleAttribute("xLen");
        if (workpieceHoleParamElement->Attribute("yLen"))
            profile.workpieceHoleParam.yLen = workpieceHoleParamElement->DoubleAttribute("yLen");
        if (workpieceHoleParamElement->Attribute("H"))
            profile.workpieceHoleParam.H = workpieceHoleParamElement->DoubleAttribute("H");
    }

    XMLElement* filterParamElement = parentElement->FirstChildElement("FilterParam");
    if (filterParamElement)
    {
        if (filterParamElement->Attribute("continuityTh"))
            profile.filterParam.continuityTh = filterParamElement->DoubleAttribute("continuityTh");
        if (filterParamElement->Attribute("outlierTh"))
            profile.filterParam.outlierTh = filterParamElement->DoubleAttribute("outlierTh");
    }

    XMLElement* lineSegParamElement = parentElement->FirstChildElement("LineSegParam");
    if (lineSegParamElement)
    {
        if (lineSegParamElement->Attribute("distScale"))
            profile.lineSegParam.distScale = lineSegParamElement->DoubleAttribute("distScale");
        if (lineSegParamElement->Attribute("segGapTh_y"))
            profile.lineSegParam.segGapTh_y = lineSegParamElement->DoubleAttribute("segGapTh_y");
        if (lineSegParamElement->Attribute("segGapTh_z"))
            profile.lineSegParam.segGapTh_z = lineSegParamElement->DoubleAttribute("segGapTh_z");
    }

    XMLElement* growParamElement = parentElement->FirstChildElement("GrowParam");
    if (growParamElement)
    {
        if (growParamElement->Attribute("maxLineSkipNum"))
            profile.growParam.maxLineSkipNum = growParamElement->IntAttribute("maxLineSkipNum");
        if (growParamElement->Attribute("yDeviation_max"))
            profile.growParam.yDeviation_max = growParamElement->DoubleAttribute("yDeviation_max");
        if (growParamElement->Attribute("maxSkipDistance"))
            profile.growParam.maxSkipDistance = growParamElement->DoubleAttribute("maxSkipDistance");
        if (growParamElement->Attribute("zDeviation_max"))
            profile.growParam.zDeviation_max = growParamElement->DoubleAttribute("zDeviation_max");
        if (growParamElement->Attribute("minLTypeTreeLen"))
            profile.growParam.minLTypeTreeLen = growParamElement->DoubleAttribute("minLTypeTreeLen");
        if (growParamElement->Attribute("minVTypeTreeLen"))
            profile.growParam.minVTypeTreeLen = growParamElement->DoubleAttribute("minVTypeTreeLen");
    }
}

void SaveWorkpieceProfileParams(
    XMLDocument& doc,
    XMLElement* algoParamsElement,
    const VrWorkpieceAlgorithmProfile& profile)
{
    XMLElement* profileElement = doc.NewElement("WorkpieceParamGroup");
    profileElement->SetAttribute("workpieceNumber", profile.workpieceNumber);
    algoParamsElement->InsertEndChild(profileElement);

    XMLElement* workpieceHoleParamElement = doc.NewElement("WorkpieceHoleParam");
    workpieceHoleParamElement->SetAttribute("workpieceType", profile.workpieceHoleParam.workpieceType);
    workpieceHoleParamElement->SetAttribute("holeDiameter", profile.workpieceHoleParam.holeDiameter);
    workpieceHoleParamElement->SetAttribute("bigHoleDiameter", profile.workpieceHoleParam.bigHoleDiameter);
    workpieceHoleParamElement->SetAttribute("holeDist_L", profile.workpieceHoleParam.holeDist_L);
    workpieceHoleParamElement->SetAttribute("holeDist_W", profile.workpieceHoleParam.holeDist_W);
    workpieceHoleParamElement->SetAttribute("xLen", profile.workpieceHoleParam.xLen);
    workpieceHoleParamElement->SetAttribute("yLen", profile.workpieceHoleParam.yLen);
    workpieceHoleParamElement->SetAttribute("H", profile.workpieceHoleParam.H);
    profileElement->InsertEndChild(workpieceHoleParamElement);

    XMLElement* filterParamElement = doc.NewElement("FilterParam");
    filterParamElement->SetAttribute("continuityTh", profile.filterParam.continuityTh);
    filterParamElement->SetAttribute("outlierTh", profile.filterParam.outlierTh);
    profileElement->InsertEndChild(filterParamElement);

    XMLElement* lineSegParamElement = doc.NewElement("LineSegParam");
    lineSegParamElement->SetAttribute("distScale", profile.lineSegParam.distScale);
    lineSegParamElement->SetAttribute("segGapTh_y", profile.lineSegParam.segGapTh_y);
    lineSegParamElement->SetAttribute("segGapTh_z", profile.lineSegParam.segGapTh_z);
    profileElement->InsertEndChild(lineSegParamElement);

    XMLElement* growParamElement = doc.NewElement("GrowParam");
    growParamElement->SetAttribute("maxLineSkipNum", profile.growParam.maxLineSkipNum);
    growParamElement->SetAttribute("yDeviation_max", profile.growParam.yDeviation_max);
    growParamElement->SetAttribute("maxSkipDistance", profile.growParam.maxSkipDistance);
    growParamElement->SetAttribute("zDeviation_max", profile.growParam.zDeviation_max);
    growParamElement->SetAttribute("minLTypeTreeLen", profile.growParam.minLTypeTreeLen);
    growParamElement->SetAttribute("minVTypeTreeLen", profile.growParam.minVTypeTreeLen);
    profileElement->InsertEndChild(growParamElement);
}

} // namespace

CVrConfig::CVrConfig() : m_pNotify(nullptr)
{
    // 构造函数
}

CVrConfig::~CVrConfig()
{
    // 析构函数
}

int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResult)
{
    // 使用tinyxml2库加载XML文件
    XMLDocument doc;
    XMLError err = doc.LoadFile(filePath.c_str());
    if (err != XML_SUCCESS)
    {
        LOG_ERR("open config file failed: %s\n", filePath.c_str());
        return LOAD_CONFIG_FILE_NOT_FOUND;
    }

    // 获取根元素
    XMLElement* root = doc.RootElement();
    if (!root || std::string(root->Name()) != "WorkpieceHoleConfig")
    {
        std::cerr << "config file format error: root element is not WorkpieceHoleConfig" << std::endl;
        return LOAD_CONFIG_INVALID_FORMAT;
    }

    // 解析摄像头列表
    ConfigXmlUtils::LoadCameraList(root, configResult.cameraList);

    // 解析设备列表
    XMLElement* devicesElement = root->FirstChildElement("Devices");
    if (devicesElement)
    {
        XMLElement* deviceElement = devicesElement->FirstChildElement("Device");
        while (deviceElement)
        {
            DeviceInfo device;
            if (deviceElement->Attribute("name"))
                device.name = deviceElement->Attribute("name");

            if (deviceElement->Attribute("ip"))
                device.ip = deviceElement->Attribute("ip");

            configResult.deviceList.push_back(device);
            deviceElement = deviceElement->NextSiblingElement("Device");
        }
    }

    // 每次加载都从完整的工件默认参数组开始，避免复用对象时残留旧数据。
    configResult.algorithmParams = VrAlgorithmParams();

    // 解析算法参数
    XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
    if (algoParamsElement)
    {
        XMLElement* profileElement = algoParamsElement->FirstChildElement("WorkpieceParamGroup");
        if (profileElement) {
            while (profileElement) {
                const int workpieceNumber = profileElement->IntAttribute("workpieceNumber", 0);
                VrWorkpieceAlgorithmProfile* workpieceProfile =
                    configResult.algorithmParams.FindWorkpieceProfile(workpieceNumber);
                if (workpieceProfile) {
                    LoadWorkpieceProfileParams(profileElement, *workpieceProfile);
                }
                profileElement = profileElement->NextSiblingElement("WorkpieceParamGroup");
            }
        } else {
            // 兼容旧版单套参数：旧参数归入工件1，其余工件保留 test 默认值。
            VrWorkpieceAlgorithmProfile* workpieceProfile =
                configResult.algorithmParams.FindWorkpieceProfile(1);
            if (workpieceProfile) {
                LoadWorkpieceProfileParams(algoParamsElement, *workpieceProfile);
            }
        }

        // 解析多相机平面校准参数
        ConfigXmlUtils::LoadPlaneCalibParams(algoParamsElement, configResult.algorithmParams.planeCalibParam);
    }

    // 解析调试参数
    ConfigXmlUtils::LoadDebugParam(root, configResult.debugParam);

    // 解析串口配置
    ConfigXmlUtils::LoadSerialConfig(root, configResult.serialConfig);

    // 解析手眼标定矩阵列表（支持多相机）
    ConfigXmlUtils::LoadHandEyeCalibMatrixs(root, configResult.handEyeCalibMatrixList);

    // 解析TCP服务端配置（PLC和机械臂）
    XMLElement* tcpServerConfigElement = root->FirstChildElement("TcpServerConfig");
    if (tcpServerConfigElement)
    {
        if (tcpServerConfigElement->Attribute("plcServerIp"))
            configResult.plcRobotServerConfig.plcServerIp = tcpServerConfigElement->Attribute("plcServerIp");
        if (tcpServerConfigElement->Attribute("plcServerPort"))
            configResult.plcRobotServerConfig.plcServerPort = tcpServerConfigElement->IntAttribute("plcServerPort");
        if (tcpServerConfigElement->Attribute("robotServerIp"))
            configResult.plcRobotServerConfig.robotServerIp = tcpServerConfigElement->Attribute("robotServerIp");
        if (tcpServerConfigElement->Attribute("robotServerPort"))
            configResult.plcRobotServerConfig.robotServerPort = tcpServerConfigElement->IntAttribute("robotServerPort");

        // 解析PLC寄存器地址配置
        if (tcpServerConfigElement->Attribute("addrPhotoRequest"))
            configResult.plcRobotServerConfig.registerConfig.addrPhotoRequest = tcpServerConfigElement->IntAttribute("addrPhotoRequest");
        if (tcpServerConfigElement->Attribute("addrDataComplete"))
            configResult.plcRobotServerConfig.registerConfig.addrDataComplete = tcpServerConfigElement->IntAttribute("addrDataComplete");
        if (tcpServerConfigElement->Attribute("addrCoordDataStart"))
            configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart = tcpServerConfigElement->IntAttribute("addrCoordDataStart");

        // 解析姿态输出顺序配置
        if (tcpServerConfigElement->Attribute("poseOutputOrder"))
            configResult.plcRobotServerConfig.poseOutputOrder = tcpServerConfigElement->IntAttribute("poseOutputOrder");
        else
            configResult.plcRobotServerConfig.poseOutputOrder = POSE_ORDER_RPY;  // 默认RPY

        // 解析方向向量反向配置
        if (tcpServerConfigElement->Attribute("dirVectorInvert"))
            configResult.plcRobotServerConfig.dirVectorInvert = tcpServerConfigElement->IntAttribute("dirVectorInvert");
        else
            configResult.plcRobotServerConfig.dirVectorInvert = DIR_INVERT_YZ;  // 默认YZ反向（兼容原有行为）

        // 解析数据字节序配置
        if (tcpServerConfigElement->Attribute("byteOrder"))
            configResult.plcRobotServerConfig.byteOrder = tcpServerConfigElement->IntAttribute("byteOrder");
        else
            configResult.plcRobotServerConfig.byteOrder = BYTE_ORDER_BIG_ENDIAN;  // 默认大端序
    }

    return LOAD_CONFIG_SUCCESS;
}

bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResult)
{
    // 创建XML文档
    XMLDocument doc;

    // 添加声明
    XMLDeclaration* declaration = doc.NewDeclaration("xml version=\"1.0\" encoding=\"UTF-8\"");
    doc.InsertFirstChild(declaration);

    // 创建根元素
    XMLElement* root = doc.NewElement("WorkpieceHoleConfig");
    doc.InsertEndChild(root);

    // 添加摄像头列表
    ConfigXmlUtils::SaveCameraList(doc, root, configResult.cameraList);

    // 添加设备列表
    XMLElement* devicesElement = doc.NewElement("Devices");
    root->InsertEndChild(devicesElement);

    for (const auto& device : configResult.deviceList)
    {
        XMLElement* deviceElement = doc.NewElement("Device");
        deviceElement->SetAttribute("name", device.name.c_str());
        deviceElement->SetAttribute("ip", device.ip.c_str());
        devicesElement->InsertEndChild(deviceElement);
    }

    // 添加算法参数
    XMLElement* algoParamsElement = doc.NewElement("AlgorithmParams");
    root->InsertEndChild(algoParamsElement);

    // 每个工件保存一套独立算法参数。
    for (int workpieceNumber = 1; workpieceNumber <= 5; ++workpieceNumber) {
        const VrWorkpieceAlgorithmProfile* workpieceProfile =
            configResult.algorithmParams.FindWorkpieceProfile(workpieceNumber);
        if (workpieceProfile) {
            SaveWorkpieceProfileParams(doc, algoParamsElement, *workpieceProfile);
        }
    }

    // 添加多相机平面校准参数
    ConfigXmlUtils::SavePlaneCalibParams(doc, algoParamsElement, configResult.algorithmParams.planeCalibParam);

    // 添加调试参数
    ConfigXmlUtils::SaveDebugParam(doc, root, configResult.debugParam);

    // 添加串口配置
    ConfigXmlUtils::SaveSerialConfig(doc, root, configResult.serialConfig);

    // 添加手眼标定矩阵列表（支持多相机）
    ConfigXmlUtils::SaveHandEyeCalibMatrixs(doc, root, configResult.handEyeCalibMatrixList);

    // 添加TCP服务端配置（PLC和机械臂）
    XMLElement* tcpServerConfigElement = doc.NewElement("TcpServerConfig");
    tcpServerConfigElement->SetAttribute("plcServerIp", configResult.plcRobotServerConfig.plcServerIp.c_str());
    tcpServerConfigElement->SetAttribute("plcServerPort", configResult.plcRobotServerConfig.plcServerPort);
    tcpServerConfigElement->SetAttribute("robotServerIp", configResult.plcRobotServerConfig.robotServerIp.c_str());
    tcpServerConfigElement->SetAttribute("robotServerPort", configResult.plcRobotServerConfig.robotServerPort);
    // 保存PLC寄存器地址配置
    tcpServerConfigElement->SetAttribute("addrPhotoRequest", configResult.plcRobotServerConfig.registerConfig.addrPhotoRequest);
    tcpServerConfigElement->SetAttribute("addrDataComplete", configResult.plcRobotServerConfig.registerConfig.addrDataComplete);
    tcpServerConfigElement->SetAttribute("addrCoordDataStart", configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart);
    // 保存姿态输出顺序配置
    tcpServerConfigElement->SetAttribute("poseOutputOrder", configResult.plcRobotServerConfig.poseOutputOrder);
    // 保存方向向量反向配置
    tcpServerConfigElement->SetAttribute("dirVectorInvert", configResult.plcRobotServerConfig.dirVectorInvert);
    // 保存数据字节序配置
    tcpServerConfigElement->SetAttribute("byteOrder", configResult.plcRobotServerConfig.byteOrder);
    root->InsertEndChild(tcpServerConfigElement);

    // 保存到文件
    XMLError err = doc.SaveFile(filePath.c_str());
    if (err != XML_SUCCESS)
    {
        std::cerr << "无法保存配置文件: " << filePath << std::endl;
        return false;
    }

    // 触发配置改变通知
    if (m_pNotify)
    {
        m_pNotify->OnConfigChanged(configResult);
    }

    return true;
}

// 设置配置改变通知回调
void CVrConfig::SetConfigChangeNotify(IVrConfigChangeNotify* notify)
{
    m_pNotify = notify;
}


/**
 * @brief 创建实例
 * @return 实例
 */
bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
{
    *ppVrConfig = new CVrConfig();
    return *ppVrConfig != nullptr;
}