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

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

using namespace tinyxml2;

CVrConfig::CVrConfig() : m_pNotify(nullptr)
{
}

CVrConfig::~CVrConfig()
{
}

int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResult)
{
    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()) != "HolePitPositionConfig")
    {
        std::cerr << "config file format error: root element is not HolePitPositionConfig" << 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");
        }
    }

    // 解析算法参数
    XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
    if (algoParamsElement)
    {
        // 解析线段参数
        XMLElement* lineSegElement = algoParamsElement->FirstChildElement("LineSegParam");
        if (lineSegElement)
        {
            if (lineSegElement->Attribute("distScale"))
                configResult.algorithmParams.lineSegParam.distScale = lineSegElement->DoubleAttribute("distScale");
            if (lineSegElement->Attribute("segGapTh_y"))
                configResult.algorithmParams.lineSegParam.segGapTh_y = lineSegElement->DoubleAttribute("segGapTh_y");
            if (lineSegElement->Attribute("segGapTh_z"))
                configResult.algorithmParams.lineSegParam.segGapTh_z = lineSegElement->DoubleAttribute("segGapTh_z");
        }

        // 解析角点参数
        XMLElement* cornerElement = algoParamsElement->FirstChildElement("CornerParam");
        if (cornerElement)
        {
            if (cornerElement->Attribute("cornerTh"))
                configResult.algorithmParams.cornerParam.cornerTh = cornerElement->DoubleAttribute("cornerTh");
            if (cornerElement->Attribute("scale"))
                configResult.algorithmParams.cornerParam.scale = cornerElement->DoubleAttribute("scale");
            if (cornerElement->Attribute("minEndingGap"))
                configResult.algorithmParams.cornerParam.minEndingGap = cornerElement->DoubleAttribute("minEndingGap");
            if (cornerElement->Attribute("minEndingGap_z"))
                configResult.algorithmParams.cornerParam.minEndingGap_z = cornerElement->DoubleAttribute("minEndingGap_z");
            if (cornerElement->Attribute("jumpCornerTh_1"))
                configResult.algorithmParams.cornerParam.jumpCornerTh_1 = cornerElement->DoubleAttribute("jumpCornerTh_1");
            if (cornerElement->Attribute("jumpCornerTh_2"))
                configResult.algorithmParams.cornerParam.jumpCornerTh_2 = cornerElement->DoubleAttribute("jumpCornerTh_2");
        }

        // 解析噪声滤除参数
        XMLElement* outlierElement = algoParamsElement->FirstChildElement("OutlierFilterParam");
        if (outlierElement)
        {
            if (outlierElement->Attribute("continuityTh"))
                configResult.algorithmParams.outlierFilterParam.continuityTh = outlierElement->DoubleAttribute("continuityTh");
            if (outlierElement->Attribute("outlierTh"))
                configResult.algorithmParams.outlierFilterParam.outlierTh = outlierElement->DoubleAttribute("outlierTh");
        }

        // 解析树生长参数
        XMLElement* treeGrowElement = algoParamsElement->FirstChildElement("TreeGrowParam");
        if (treeGrowElement)
        {
            if (treeGrowElement->Attribute("maxLineSkipNum"))
                configResult.algorithmParams.treeGrowParam.maxLineSkipNum = treeGrowElement->IntAttribute("maxLineSkipNum");
            if (treeGrowElement->Attribute("yDeviation_max"))
                configResult.algorithmParams.treeGrowParam.yDeviation_max = treeGrowElement->DoubleAttribute("yDeviation_max");
            if (treeGrowElement->Attribute("maxSkipDistance"))
                configResult.algorithmParams.treeGrowParam.maxSkipDistance = treeGrowElement->DoubleAttribute("maxSkipDistance");
            if (treeGrowElement->Attribute("zDeviation_max"))
                configResult.algorithmParams.treeGrowParam.zDeviation_max = treeGrowElement->DoubleAttribute("zDeviation_max");
            if (treeGrowElement->Attribute("minLTypeTreeLen"))
                configResult.algorithmParams.treeGrowParam.minLTypeTreeLen = treeGrowElement->DoubleAttribute("minLTypeTreeLen");
            if (treeGrowElement->Attribute("minVTypeTreeLen"))
                configResult.algorithmParams.treeGrowParam.minVTypeTreeLen = treeGrowElement->DoubleAttribute("minVTypeTreeLen");
        }

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

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

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

    // 解析手眼标定矩阵列表
    ConfigXmlUtils::LoadHandEyeCalibMatrixs(root, configResult.handEyeCalibMatrixList);

    // 解析TCP服务端配置
    XMLElement* tcpServerConfigElement = root->FirstChildElement("TcpServerConfig");
    if (tcpServerConfigElement)
    {
        if (tcpServerConfigElement->Attribute("tcpServerPort"))
            configResult.plcRobotServerConfig.tcpServerPort = tcpServerConfigElement->IntAttribute("tcpServerPort");
        else
            configResult.plcRobotServerConfig.tcpServerPort = 7800;

        if (tcpServerConfigElement->Attribute("poseOutputOrder"))
            configResult.plcRobotServerConfig.poseOutputOrder = tcpServerConfigElement->IntAttribute("poseOutputOrder");
        else
            configResult.plcRobotServerConfig.poseOutputOrder = POSE_ORDER_RPY;

        if (tcpServerConfigElement->Attribute("dirVectorInvert"))
            configResult.plcRobotServerConfig.dirVectorInvert = tcpServerConfigElement->IntAttribute("dirVectorInvert");
        else
            configResult.plcRobotServerConfig.dirVectorInvert = DIR_INVERT_YZ;
    }

    return LOAD_CONFIG_SUCCESS;
}

bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResult)
{
    XMLDocument doc;

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

    XMLElement* root = doc.NewElement("HolePitPositionConfig");
    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);

    // 线段参数
    XMLElement* lineSegElement = doc.NewElement("LineSegParam");
    lineSegElement->SetAttribute("distScale", configResult.algorithmParams.lineSegParam.distScale);
    lineSegElement->SetAttribute("segGapTh_y", configResult.algorithmParams.lineSegParam.segGapTh_y);
    lineSegElement->SetAttribute("segGapTh_z", configResult.algorithmParams.lineSegParam.segGapTh_z);
    algoParamsElement->InsertEndChild(lineSegElement);

    // 角点参数
    XMLElement* cornerElement = doc.NewElement("CornerParam");
    cornerElement->SetAttribute("cornerTh", configResult.algorithmParams.cornerParam.cornerTh);
    cornerElement->SetAttribute("scale", configResult.algorithmParams.cornerParam.scale);
    cornerElement->SetAttribute("minEndingGap", configResult.algorithmParams.cornerParam.minEndingGap);
    cornerElement->SetAttribute("minEndingGap_z", configResult.algorithmParams.cornerParam.minEndingGap_z);
    cornerElement->SetAttribute("jumpCornerTh_1", configResult.algorithmParams.cornerParam.jumpCornerTh_1);
    cornerElement->SetAttribute("jumpCornerTh_2", configResult.algorithmParams.cornerParam.jumpCornerTh_2);
    algoParamsElement->InsertEndChild(cornerElement);

    // 噪声滤除参数
    XMLElement* outlierElement = doc.NewElement("OutlierFilterParam");
    outlierElement->SetAttribute("continuityTh", configResult.algorithmParams.outlierFilterParam.continuityTh);
    outlierElement->SetAttribute("outlierTh", configResult.algorithmParams.outlierFilterParam.outlierTh);
    algoParamsElement->InsertEndChild(outlierElement);

    // 树生长参数
    XMLElement* treeGrowElement = doc.NewElement("TreeGrowParam");
    treeGrowElement->SetAttribute("maxLineSkipNum", configResult.algorithmParams.treeGrowParam.maxLineSkipNum);
    treeGrowElement->SetAttribute("yDeviation_max", configResult.algorithmParams.treeGrowParam.yDeviation_max);
    treeGrowElement->SetAttribute("maxSkipDistance", configResult.algorithmParams.treeGrowParam.maxSkipDistance);
    treeGrowElement->SetAttribute("zDeviation_max", configResult.algorithmParams.treeGrowParam.zDeviation_max);
    treeGrowElement->SetAttribute("minLTypeTreeLen", configResult.algorithmParams.treeGrowParam.minLTypeTreeLen);
    treeGrowElement->SetAttribute("minVTypeTreeLen", configResult.algorithmParams.treeGrowParam.minVTypeTreeLen);
    algoParamsElement->InsertEndChild(treeGrowElement);

    // 平面校准参数
    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服务端配置
    XMLElement* tcpServerConfigElement = doc.NewElement("TcpServerConfig");
    tcpServerConfigElement->SetAttribute("tcpServerPort", configResult.plcRobotServerConfig.tcpServerPort);
    tcpServerConfigElement->SetAttribute("poseOutputOrder", configResult.plcRobotServerConfig.poseOutputOrder);
    tcpServerConfigElement->SetAttribute("dirVectorInvert", configResult.plcRobotServerConfig.dirVectorInvert);
    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;
}

bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
{
    *ppVrConfig = new CVrConfig();
    return *ppVrConfig != nullptr;
}