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

using namespace tinyxml2;

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

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

ConfigResult CVrConfig::LoadConfig(const std::string& filePath)
{
    ConfigResult result;
    
    // 使用tinyxml2库加载XML文件
    XMLDocument doc;
    XMLError err = doc.LoadFile(filePath.c_str());
    if (err != XML_SUCCESS)
    {
        std::cerr << "open config file failed: " << filePath << std::endl;
        return result;
    }

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

    // 解析摄像头列表
    XMLElement* camerasElement = root->FirstChildElement("Cameras");
    if (camerasElement)
    {
        XMLElement* cameraElement = camerasElement->FirstChildElement("Camera");
        while (cameraElement)
        {
            DeviceInfo camera;
            if (cameraElement->Attribute("name"))
                camera.name = cameraElement->Attribute("name");
            
            if (cameraElement->Attribute("ip"))
                camera.ip = cameraElement->Attribute("ip");
            
            result.cameraList.push_back(camera);
            cameraElement = cameraElement->NextSiblingElement("Camera");
        }
    }

    // 解析设备列表
    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");
            
            result.deviceList.push_back(device);
            deviceElement = deviceElement->NextSiblingElement("Device");
        }
    }

    // 解析算法参数
    XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
    if (algoParamsElement)
    {
        // 解析编织袋参数
        XMLElement* bagParamElement = algoParamsElement->FirstChildElement("BagParam");
        if (bagParamElement)
        {
            if (bagParamElement->Attribute("bagL"))
                result.algorithmParams.bagParam.bagL = bagParamElement->DoubleAttribute("bagL");
            if (bagParamElement->Attribute("bagW"))
                result.algorithmParams.bagParam.bagW = bagParamElement->DoubleAttribute("bagW");
            if (bagParamElement->Attribute("bagH"))
                result.algorithmParams.bagParam.bagH = bagParamElement->DoubleAttribute("bagH");
        }
        
        // 解析垛参数
        XMLElement* pileParamElement = algoParamsElement->FirstChildElement("PileParam");
        if (pileParamElement)
        {
            if (pileParamElement->Attribute("pileL"))
                result.algorithmParams.pileParam.pileL = pileParamElement->DoubleAttribute("pileL");
            if (pileParamElement->Attribute("pileW"))
                result.algorithmParams.pileParam.pileW = pileParamElement->DoubleAttribute("pileW");
            if (pileParamElement->Attribute("pileH"))
                result.algorithmParams.pileParam.pileH = pileParamElement->DoubleAttribute("pileH");
        }
        
        // 解析滤波参数
        XMLElement* filterParamElement = algoParamsElement->FirstChildElement("FilterParam");
        if (filterParamElement)
        {
            if (filterParamElement->Attribute("continuityTh"))
                result.algorithmParams.filterParam.continuityTh = filterParamElement->DoubleAttribute("continuityTh");
            if (filterParamElement->Attribute("outlierTh"))
                result.algorithmParams.filterParam.outlierTh = filterParamElement->IntAttribute("outlierTh");
        }
        
        // 解析角点特征参数
        XMLElement* cornerParamElement = algoParamsElement->FirstChildElement("CornerParam");
        if (cornerParamElement)
        {
            if (cornerParamElement->Attribute("minEndingGap"))
                result.algorithmParams.cornerParam.minEndingGap = cornerParamElement->DoubleAttribute("minEndingGap");
            if (cornerParamElement->Attribute("scale"))
                result.algorithmParams.cornerParam.scale = cornerParamElement->DoubleAttribute("scale");
            if (cornerParamElement->Attribute("cornerTh"))
                result.algorithmParams.cornerParam.cornerTh = cornerParamElement->DoubleAttribute("cornerTh");
            if (cornerParamElement->Attribute("jumpCornerTh_1"))
                result.algorithmParams.cornerParam.jumpCornerTh_1 = cornerParamElement->DoubleAttribute("jumpCornerTh_1");
            if (cornerParamElement->Attribute("jumpCornerTh_2"))
                result.algorithmParams.cornerParam.jumpCornerTh_2 = cornerParamElement->DoubleAttribute("jumpCornerTh_2");
        }
        
        // 解析斜率参数
        XMLElement* slopeParamElement = algoParamsElement->FirstChildElement("SlopeParam");
        if (slopeParamElement)
        {
            if (slopeParamElement->Attribute("LSlopeZWin"))
                result.algorithmParams.slopeParam.LSlopeZWin = slopeParamElement->DoubleAttribute("LSlopeZWin");
            if (slopeParamElement->Attribute("validSlopeH"))
                result.algorithmParams.slopeParam.validSlopeH = slopeParamElement->DoubleAttribute("validSlopeH");
            if (slopeParamElement->Attribute("minLJumpH"))
                result.algorithmParams.slopeParam.minLJumpH = slopeParamElement->DoubleAttribute("minLJumpH");
            if (slopeParamElement->Attribute("minEndingGap"))
                result.algorithmParams.slopeParam.minEndingGap = slopeParamElement->DoubleAttribute("minEndingGap");
        }
        
        // 解析山谷参数
        XMLElement* valleyParamElement = algoParamsElement->FirstChildElement("ValleyParam");
        if (valleyParamElement)
        {
            if (valleyParamElement->Attribute("valleyMinH"))
                result.algorithmParams.valleyParam.valleyMinH = valleyParamElement->DoubleAttribute("valleyMinH");
            if (valleyParamElement->Attribute("valleyMaxW"))
                result.algorithmParams.valleyParam.valleyMaxW = valleyParamElement->DoubleAttribute("valleyMaxW");
        }
        
        // 解析增长参数
        XMLElement* growParamElement = algoParamsElement->FirstChildElement("GrowParam");
        if (growParamElement)
        {
            if (growParamElement->Attribute("maxLineSkipNum"))
                result.algorithmParams.growParam.maxLineSkipNum = growParamElement->IntAttribute("maxLineSkipNum");
            if (growParamElement->Attribute("yDeviation_max"))
                result.algorithmParams.growParam.yDeviation_max = growParamElement->DoubleAttribute("yDeviation_max");
            if (growParamElement->Attribute("maxSkipDistance"))
                result.algorithmParams.growParam.maxSkipDistance = growParamElement->DoubleAttribute("maxSkipDistance");
            if (growParamElement->Attribute("zDeviation_max"))
                result.algorithmParams.growParam.zDeviation_max = growParamElement->DoubleAttribute("zDeviation_max");
            if (growParamElement->Attribute("minLTypeTreeLen"))
                result.algorithmParams.growParam.minLTypeTreeLen = growParamElement->DoubleAttribute("minLTypeTreeLen");
            if (growParamElement->Attribute("minVTypeTreeLen"))
                result.algorithmParams.growParam.minVTypeTreeLen = growParamElement->DoubleAttribute("minVTypeTreeLen");
        }
        
        // 解析平面校准参数
        XMLElement* planeCalibParamElement = algoParamsElement->FirstChildElement("PlaneCalibParam");
        if (planeCalibParamElement)
        {
            if (planeCalibParamElement->Attribute("planeHeight"))
                result.algorithmParams.planeCalibParam.planeHeight = planeCalibParamElement->DoubleAttribute("planeHeight");
            
            // 读取旋转矩阵planeCalib[9] (3x3矩阵)
            if (planeCalibParamElement->Attribute("planeCalib_00"))
                result.algorithmParams.planeCalibParam.planeCalib[0] = planeCalibParamElement->DoubleAttribute("planeCalib_00");
            if (planeCalibParamElement->Attribute("planeCalib_01"))
                result.algorithmParams.planeCalibParam.planeCalib[1] = planeCalibParamElement->DoubleAttribute("planeCalib_01");
            if (planeCalibParamElement->Attribute("planeCalib_02"))
                result.algorithmParams.planeCalibParam.planeCalib[2] = planeCalibParamElement->DoubleAttribute("planeCalib_02");
            if (planeCalibParamElement->Attribute("planeCalib_10"))
                result.algorithmParams.planeCalibParam.planeCalib[3] = planeCalibParamElement->DoubleAttribute("planeCalib_10");
            if (planeCalibParamElement->Attribute("planeCalib_11"))
                result.algorithmParams.planeCalibParam.planeCalib[4] = planeCalibParamElement->DoubleAttribute("planeCalib_11");
            if (planeCalibParamElement->Attribute("planeCalib_12"))
                result.algorithmParams.planeCalibParam.planeCalib[5] = planeCalibParamElement->DoubleAttribute("planeCalib_12");
            if (planeCalibParamElement->Attribute("planeCalib_20"))
                result.algorithmParams.planeCalibParam.planeCalib[6] = planeCalibParamElement->DoubleAttribute("planeCalib_20");
            if (planeCalibParamElement->Attribute("planeCalib_21"))
                result.algorithmParams.planeCalibParam.planeCalib[7] = planeCalibParamElement->DoubleAttribute("planeCalib_21");
            if (planeCalibParamElement->Attribute("planeCalib_22"))
                result.algorithmParams.planeCalibParam.planeCalib[8] = planeCalibParamElement->DoubleAttribute("planeCalib_22");
            
            // 读取逆旋转矩阵invRMatrix[9] (3x3矩阵)
            if (planeCalibParamElement->Attribute("invRMatrix_00"))
                result.algorithmParams.planeCalibParam.invRMatrix[0] = planeCalibParamElement->DoubleAttribute("invRMatrix_00");
            if (planeCalibParamElement->Attribute("invRMatrix_01"))
                result.algorithmParams.planeCalibParam.invRMatrix[1] = planeCalibParamElement->DoubleAttribute("invRMatrix_01");
            if (planeCalibParamElement->Attribute("invRMatrix_02"))
                result.algorithmParams.planeCalibParam.invRMatrix[2] = planeCalibParamElement->DoubleAttribute("invRMatrix_02");
            if (planeCalibParamElement->Attribute("invRMatrix_10"))
                result.algorithmParams.planeCalibParam.invRMatrix[3] = planeCalibParamElement->DoubleAttribute("invRMatrix_10");
            if (planeCalibParamElement->Attribute("invRMatrix_11"))
                result.algorithmParams.planeCalibParam.invRMatrix[4] = planeCalibParamElement->DoubleAttribute("invRMatrix_11");
            if (planeCalibParamElement->Attribute("invRMatrix_12"))
                result.algorithmParams.planeCalibParam.invRMatrix[5] = planeCalibParamElement->DoubleAttribute("invRMatrix_12");
            if (planeCalibParamElement->Attribute("invRMatrix_20"))
                result.algorithmParams.planeCalibParam.invRMatrix[6] = planeCalibParamElement->DoubleAttribute("invRMatrix_20");
            if (planeCalibParamElement->Attribute("invRMatrix_21"))
                result.algorithmParams.planeCalibParam.invRMatrix[7] = planeCalibParamElement->DoubleAttribute("invRMatrix_21");
            if (planeCalibParamElement->Attribute("invRMatrix_22"))
                result.algorithmParams.planeCalibParam.invRMatrix[8] = planeCalibParamElement->DoubleAttribute("invRMatrix_22");
        }
        
    }

    // 解析调试参数（在AlgorithmParams外面）
    XMLElement* debugParamElement = root->FirstChildElement("DebugParam");
    if (debugParamElement)
    {
        if (debugParamElement->Attribute("enableDebug"))
            result.debugParam.enableDebug = debugParamElement->BoolAttribute("enableDebug");
        if (debugParamElement->Attribute("savePointCloud"))
            result.debugParam.savePointCloud = debugParamElement->BoolAttribute("savePointCloud");
        if (debugParamElement->Attribute("saveDebugImage"))
            result.debugParam.saveDebugImage = debugParamElement->BoolAttribute("saveDebugImage");
        if (debugParamElement->Attribute("printDetailLog"))
            result.debugParam.printDetailLog = debugParamElement->BoolAttribute("printDetailLog");
        if (debugParamElement->Attribute("debugOutputPath"))
            result.debugParam.debugOutputPath = debugParamElement->Attribute("debugOutputPath");
    }

    return result;
}

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("VrConfig");
    doc.InsertEndChild(root);

    // 添加摄像头列表
    XMLElement* camerasElement = doc.NewElement("Cameras");
    root->InsertEndChild(camerasElement);
    
    for (const auto& camera : configResult.cameraList)
    {
        XMLElement* cameraElement = doc.NewElement("Camera");
        cameraElement->SetAttribute("name", camera.name.c_str());
        cameraElement->SetAttribute("ip", camera.ip.c_str());
        camerasElement->InsertEndChild(cameraElement);
    }

    // 添加设备列表
    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* bagParamElement = doc.NewElement("BagParam");
    bagParamElement->SetAttribute("bagL", configResult.algorithmParams.bagParam.bagL);
    bagParamElement->SetAttribute("bagW", configResult.algorithmParams.bagParam.bagW);
    bagParamElement->SetAttribute("bagH", configResult.algorithmParams.bagParam.bagH);
    algoParamsElement->InsertEndChild(bagParamElement);
    
    // 添加垛参数
    XMLElement* pileParamElement = doc.NewElement("PileParam");
    pileParamElement->SetAttribute("pileL", configResult.algorithmParams.pileParam.pileL);
    pileParamElement->SetAttribute("pileW", configResult.algorithmParams.pileParam.pileW);
    pileParamElement->SetAttribute("pileH", configResult.algorithmParams.pileParam.pileH);
    algoParamsElement->InsertEndChild(pileParamElement);
    
    // 添加滤波参数
    XMLElement* filterParamElement = doc.NewElement("FilterParam");
    filterParamElement->SetAttribute("continuityTh", configResult.algorithmParams.filterParam.continuityTh);
    filterParamElement->SetAttribute("outlierTh", configResult.algorithmParams.filterParam.outlierTh);
    algoParamsElement->InsertEndChild(filterParamElement);
    
    // 添加角点特征参数
    XMLElement* cornerParamElement = doc.NewElement("CornerParam");
    cornerParamElement->SetAttribute("minEndingGap", configResult.algorithmParams.cornerParam.minEndingGap);
    cornerParamElement->SetAttribute("scale", configResult.algorithmParams.cornerParam.scale);
    cornerParamElement->SetAttribute("cornerTh", configResult.algorithmParams.cornerParam.cornerTh);
    cornerParamElement->SetAttribute("jumpCornerTh_1", configResult.algorithmParams.cornerParam.jumpCornerTh_1);
    cornerParamElement->SetAttribute("jumpCornerTh_2", configResult.algorithmParams.cornerParam.jumpCornerTh_2);
    algoParamsElement->InsertEndChild(cornerParamElement);
    
    // 添加斜率参数
    XMLElement* slopeParamElement = doc.NewElement("SlopeParam");
    slopeParamElement->SetAttribute("LSlopeZWin", configResult.algorithmParams.slopeParam.LSlopeZWin);
    slopeParamElement->SetAttribute("validSlopeH", configResult.algorithmParams.slopeParam.validSlopeH);
    slopeParamElement->SetAttribute("minLJumpH", configResult.algorithmParams.slopeParam.minLJumpH);
    slopeParamElement->SetAttribute("minEndingGap", configResult.algorithmParams.slopeParam.minEndingGap);
    algoParamsElement->InsertEndChild(slopeParamElement);
    
    // 添加山谷参数
    XMLElement* valleyParamElement = doc.NewElement("ValleyParam");
    valleyParamElement->SetAttribute("valleyMinH", configResult.algorithmParams.valleyParam.valleyMinH);
    valleyParamElement->SetAttribute("valleyMaxW", configResult.algorithmParams.valleyParam.valleyMaxW);
    algoParamsElement->InsertEndChild(valleyParamElement);
    
    // 添加增长参数
    XMLElement* growParamElement = doc.NewElement("GrowParam");
    growParamElement->SetAttribute("maxLineSkipNum", configResult.algorithmParams.growParam.maxLineSkipNum);
    growParamElement->SetAttribute("yDeviation_max", configResult.algorithmParams.growParam.yDeviation_max);
    growParamElement->SetAttribute("maxSkipDistance", configResult.algorithmParams.growParam.maxSkipDistance);
    growParamElement->SetAttribute("zDeviation_max", configResult.algorithmParams.growParam.zDeviation_max);
    growParamElement->SetAttribute("minLTypeTreeLen", configResult.algorithmParams.growParam.minLTypeTreeLen);
    growParamElement->SetAttribute("minVTypeTreeLen", configResult.algorithmParams.growParam.minVTypeTreeLen);
    algoParamsElement->InsertEndChild(growParamElement);
    
    // 添加平面校准参数
    XMLElement* planeCalibParamElement = doc.NewElement("PlaneCalibParam");
    planeCalibParamElement->SetAttribute("planeHeight", configResult.algorithmParams.planeCalibParam.planeHeight);
    
    // 保存旋转矩阵planeCalib[9] (3x3矩阵)
    planeCalibParamElement->SetAttribute("planeCalib_00", configResult.algorithmParams.planeCalibParam.planeCalib[0]);
    planeCalibParamElement->SetAttribute("planeCalib_01", configResult.algorithmParams.planeCalibParam.planeCalib[1]);
    planeCalibParamElement->SetAttribute("planeCalib_02", configResult.algorithmParams.planeCalibParam.planeCalib[2]);
    planeCalibParamElement->SetAttribute("planeCalib_10", configResult.algorithmParams.planeCalibParam.planeCalib[3]);
    planeCalibParamElement->SetAttribute("planeCalib_11", configResult.algorithmParams.planeCalibParam.planeCalib[4]);
    planeCalibParamElement->SetAttribute("planeCalib_12", configResult.algorithmParams.planeCalibParam.planeCalib[5]);
    planeCalibParamElement->SetAttribute("planeCalib_20", configResult.algorithmParams.planeCalibParam.planeCalib[6]);
    planeCalibParamElement->SetAttribute("planeCalib_21", configResult.algorithmParams.planeCalibParam.planeCalib[7]);
    planeCalibParamElement->SetAttribute("planeCalib_22", configResult.algorithmParams.planeCalibParam.planeCalib[8]);
    
    // 保存逆旋转矩阵invRMatrix[9] (3x3矩阵)
    planeCalibParamElement->SetAttribute("invRMatrix_00", configResult.algorithmParams.planeCalibParam.invRMatrix[0]);
    planeCalibParamElement->SetAttribute("invRMatrix_01", configResult.algorithmParams.planeCalibParam.invRMatrix[1]);
    planeCalibParamElement->SetAttribute("invRMatrix_02", configResult.algorithmParams.planeCalibParam.invRMatrix[2]);
    planeCalibParamElement->SetAttribute("invRMatrix_10", configResult.algorithmParams.planeCalibParam.invRMatrix[3]);
    planeCalibParamElement->SetAttribute("invRMatrix_11", configResult.algorithmParams.planeCalibParam.invRMatrix[4]);
    planeCalibParamElement->SetAttribute("invRMatrix_12", configResult.algorithmParams.planeCalibParam.invRMatrix[5]);
    planeCalibParamElement->SetAttribute("invRMatrix_20", configResult.algorithmParams.planeCalibParam.invRMatrix[6]);
    planeCalibParamElement->SetAttribute("invRMatrix_21", configResult.algorithmParams.planeCalibParam.invRMatrix[7]);
    planeCalibParamElement->SetAttribute("invRMatrix_22", configResult.algorithmParams.planeCalibParam.invRMatrix[8]);
    
    algoParamsElement->InsertEndChild(planeCalibParamElement);

    // 添加调试参数（在AlgorithmParams外面）
    XMLElement* debugParamElement = doc.NewElement("DebugParam");
    debugParamElement->SetAttribute("enableDebug", configResult.debugParam.enableDebug);
    debugParamElement->SetAttribute("savePointCloud", configResult.debugParam.savePointCloud);
    debugParamElement->SetAttribute("saveDebugImage", configResult.debugParam.saveDebugImage);
    debugParamElement->SetAttribute("printDetailLog", configResult.debugParam.printDetailLog);
    debugParamElement->SetAttribute("debugOutputPath", configResult.debugParam.debugOutputPath.c_str());
    root->InsertEndChild(debugParamElement);

    // 保存到文件
    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;
}