#include "VrConfig.h"

#include <string>

#include "VrLog.h"

using namespace tinyxml2;

namespace {

const char* RootName()
{
    return "ParkingSpaceGuideConfig";
}

void LoadStringAttribute(XMLElement* element, const char* name, std::string& value)
{
    if (element && element->Attribute(name)) {
        value = element->Attribute(name);
    }
}

void LoadMvsCamera(XMLElement* root, MvsCameraConfig& config)
{
    XMLElement* element = root->FirstChildElement("MvsCamera");
    if (!element) {
        XMLElement* oldElement = root->FirstChildElement("HikCamera");
        if (oldElement) {
            config.enabled = oldElement->BoolAttribute("enabled", config.enabled);
        }
        return;
    }

    config.enabled = element->BoolAttribute("enabled", config.enabled);
    LoadStringAttribute(element, "serialNumber", config.serialNumber);
    config.deviceIndex = element->IntAttribute("deviceIndex", config.deviceIndex);
}

void SaveMvsCamera(XMLDocument& doc, XMLElement* root, const MvsCameraConfig& config)
{
    XMLElement* element = doc.NewElement("MvsCamera");
    element->SetAttribute("enabled", config.enabled);
    element->SetAttribute("serialNumber", config.serialNumber.c_str());
    element->SetAttribute("deviceIndex", config.deviceIndex);
    root->InsertEndChild(element);
}

void LoadRsLidar(XMLElement* root, RsLidarConfig& config)
{
    XMLElement* element = root->FirstChildElement("RsLidar");
    if (!element) {
        return;
    }

    LoadStringAttribute(element, "lidarType", config.lidarType);
    LoadStringAttribute(element, "hostAddress", config.hostAddress);
    LoadStringAttribute(element, "frameId", config.frameId);
    config.msopPort = static_cast<unsigned short>(element->UnsignedAttribute("msopPort", config.msopPort));
    config.difopPort = static_cast<unsigned short>(element->UnsignedAttribute("difopPort", config.difopPort));
    config.imuPort = static_cast<unsigned short>(element->UnsignedAttribute("imuPort", config.imuPort));
    config.minDistance = element->FloatAttribute("minDistance", config.minDistance);
    config.maxDistance = element->FloatAttribute("maxDistance", config.maxDistance);
    config.densePoints = element->BoolAttribute("densePoints", config.densePoints);
    config.useLidarClock = element->BoolAttribute("useLidarClock", config.useLidarClock);
    config.tsFirstPoint = element->BoolAttribute("tsFirstPoint", config.tsFirstPoint);
    config.waitForDifop = element->BoolAttribute("waitForDifop", config.waitForDifop);
    config.startAngle = element->FloatAttribute("startAngle", config.startAngle);
    config.endAngle = element->FloatAttribute("endAngle", config.endAngle);
    config.splitAngle = element->FloatAttribute("splitAngle", config.splitAngle);
    config.socketRecvBufBytes = element->UnsignedAttribute("socketRecvBufBytes", config.socketRecvBufBytes);
}

void SaveRsLidar(XMLDocument& doc, XMLElement* root, const RsLidarConfig& config)
{
    XMLElement* element = doc.NewElement("RsLidar");
    element->SetAttribute("lidarType", config.lidarType.c_str());
    element->SetAttribute("hostAddress", config.hostAddress.c_str());
    element->SetAttribute("msopPort", config.msopPort);
    element->SetAttribute("difopPort", config.difopPort);
    element->SetAttribute("imuPort", config.imuPort);
    element->SetAttribute("minDistance", config.minDistance);
    element->SetAttribute("maxDistance", config.maxDistance);
    element->SetAttribute("densePoints", config.densePoints);
    element->SetAttribute("useLidarClock", config.useLidarClock);
    element->SetAttribute("tsFirstPoint", config.tsFirstPoint);
    element->SetAttribute("waitForDifop", config.waitForDifop);
    element->SetAttribute("startAngle", config.startAngle);
    element->SetAttribute("endAngle", config.endAngle);
    element->SetAttribute("splitAngle", config.splitAngle);
    element->SetAttribute("frameId", config.frameId.c_str());
    element->SetAttribute("socketRecvBufBytes", config.socketRecvBufBytes);
    root->InsertEndChild(element);
}

void LoadLedDisplay(XMLElement* root, LedDisplayConfig& config)
{
    XMLElement* element = root->FirstChildElement("LedDisplay");
    if (!element) {
        return;
    }

    LoadStringAttribute(element, "ip", config.ip);
    config.port = element->IntAttribute("port", config.port);
    config.slaveId = element->IntAttribute("slaveId", config.slaveId);
    config.startAddress = element->IntAttribute("startAddress", config.startAddress);
    config.timeoutMs = element->IntAttribute("timeoutMs", config.timeoutMs);
    config.enabled = element->BoolAttribute("enabled", config.enabled);
    config.controllerType = element->IntAttribute("controllerType", config.controllerType);
    config.areaX = element->IntAttribute("areaX", config.areaX);
    config.areaY = element->IntAttribute("areaY", config.areaY);
    config.areaWidth = element->IntAttribute("areaWidth", config.areaWidth);
    config.areaHeight = element->IntAttribute("areaHeight", config.areaHeight);
    config.dynamicTimeoutSec = element->IntAttribute("dynamicTimeoutSec", config.dynamicTimeoutSec);
    config.displayMode = element->IntAttribute("displayMode", config.displayMode);
    config.speed = element->IntAttribute("speed", config.speed);
    config.stayTime = element->IntAttribute("stayTime", config.stayTime);
}

void SaveLedDisplay(XMLDocument& doc, XMLElement* root, const LedDisplayConfig& config)
{
    XMLElement* element = doc.NewElement("LedDisplay");
    element->SetAttribute("ip", config.ip.c_str());
    element->SetAttribute("port", config.port);
    element->SetAttribute("slaveId", config.slaveId);
    element->SetAttribute("startAddress", config.startAddress);
    element->SetAttribute("timeoutMs", config.timeoutMs);
    element->SetAttribute("enabled", config.enabled);
    element->SetAttribute("controllerType", config.controllerType);
    element->SetAttribute("areaX", config.areaX);
    element->SetAttribute("areaY", config.areaY);
    element->SetAttribute("areaWidth", config.areaWidth);
    element->SetAttribute("areaHeight", config.areaHeight);
    element->SetAttribute("dynamicTimeoutSec", config.dynamicTimeoutSec);
    element->SetAttribute("displayMode", config.displayMode);
    element->SetAttribute("speed", config.speed);
    element->SetAttribute("stayTime", config.stayTime);
    root->InsertEndChild(element);
}

void LoadUdpBroadcast(XMLElement* root, UdpBroadcastConfig& config)
{
    XMLElement* element = root->FirstChildElement("UdpBroadcast");
    if (!element) {
        return;
    }

    config.enabled = element->BoolAttribute("enabled", config.enabled);
    LoadStringAttribute(element, "address", config.address);
    config.port = element->IntAttribute("port", config.port);
    LoadStringAttribute(element, "targetId", config.targetId);
    LoadStringAttribute(element, "parkId", config.parkId);
}

void SaveUdpBroadcast(XMLDocument& doc, XMLElement* root, const UdpBroadcastConfig& config)
{
    XMLElement* element = doc.NewElement("UdpBroadcast");
    element->SetAttribute("enabled", config.enabled);
    element->SetAttribute("address", config.address.c_str());
    element->SetAttribute("port", config.port);
    element->SetAttribute("targetId", config.targetId.c_str());
    element->SetAttribute("parkId", config.parkId.c_str());
    root->InsertEndChild(element);
}

void LoadHttpServer(XMLElement* root, HttpServerConfig& config)
{
    XMLElement* element = root->FirstChildElement("HttpServer");
    if (!element) {
        return;
    }

    config.enabled = element->BoolAttribute("enabled", config.enabled);
    LoadStringAttribute(element, "address", config.address);
    config.port = element->IntAttribute("port", config.port);
    config.maxQueued = element->IntAttribute("maxQueued", config.maxQueued);
    config.maxThreads = element->IntAttribute("maxThreads", config.maxThreads);
}

void SaveHttpServer(XMLDocument& doc, XMLElement* root, const HttpServerConfig& config)
{
    XMLElement* element = doc.NewElement("HttpServer");
    element->SetAttribute("enabled", config.enabled);
    element->SetAttribute("address", config.address.c_str());
    element->SetAttribute("port", config.port);
    element->SetAttribute("maxQueued", config.maxQueued);
    element->SetAttribute("maxThreads", config.maxThreads);
    root->InsertEndChild(element);
}

const char* const kMatrixAttributeNames[9] = {
    "m00", "m01", "m02",
    "m10", "m11", "m12",
    "m20", "m21", "m22"
};

void LoadMatrix(XMLElement* root, const char* elementName, double values[9])
{
    XMLElement* element = root ? root->FirstChildElement(elementName) : nullptr;
    if (!element) {
        return;
    }
    for (int i = 0; i < 9; ++i) {
        values[i] = element->DoubleAttribute(kMatrixAttributeNames[i], values[i]);
    }
}

void SaveMatrix(XMLDocument& doc,
                XMLElement* root,
                const char* elementName,
                const double values[9])
{
    XMLElement* element = doc.NewElement(elementName);
    for (int i = 0; i < 9; ++i) {
        element->SetAttribute(kMatrixAttributeNames[i], values[i]);
    }
    root->InsertEndChild(element);
}

void LoadAlgorithmParams(XMLElement* root, VrAlgorithmParams& params)
{
    XMLElement* algo = root->FirstChildElement("AlgorithmParams");
    if (!algo) {
        return;
    }

    XMLElement* planeParking = algo->FirstChildElement("PlaneParkingParam");
    if (planeParking) {
        VrPlaneParkingParam& p = params.planeParkingParam;
        p.parkingPointX = planeParking->DoubleAttribute("parkingPointX", p.parkingPointX);
        p.parkingPointY = planeParking->DoubleAttribute("parkingPointY", p.parkingPointY);
        p.parkingPointZ = planeParking->DoubleAttribute("parkingPointZ", p.parkingPointZ);
        p.guideLinePointX = planeParking->DoubleAttribute("guideLinePointX", p.guideLinePointX);
        p.guideLinePointY = planeParking->DoubleAttribute("guideLinePointY", p.guideLinePointY);
        p.guideLinePointZ = planeParking->DoubleAttribute("guideLinePointZ", p.guideLinePointZ);
        p.guidingRange = planeParking->DoubleAttribute("guidingRange", p.guidingRange);
        p.parkingRange = planeParking->DoubleAttribute("parkingRange", p.parkingRange);
        p.distFromNoseToWheel = planeParking->DoubleAttribute("distFromNoseToWheel", p.distFromNoseToWheel);
    }

    XMLElement* groundCalibration = algo->FirstChildElement("PlaneGroundCalibrationParam");
    if (groundCalibration) {
        VrPlaneGroundCalibrationParam& p = params.groundCalibrationParam;
        p.planeHeight = groundCalibration->DoubleAttribute("planeHeight", p.planeHeight);
        LoadMatrix(groundCalibration, "PlaneCalib", p.planeCalib);
        LoadMatrix(groundCalibration, "InvRMatrix", p.invRMatrix);
    }

    XMLElement* treeGrow = algo->FirstChildElement("PlaneTreeGrowParam");
    if (treeGrow) {
        VrPlaneTreeGrowParam& p = params.treeGrowParam;
        p.yDeviationMax = treeGrow->DoubleAttribute("yDeviationMax", p.yDeviationMax);
        p.zDeviationMax = treeGrow->DoubleAttribute("zDeviationMax", p.zDeviationMax);
        p.maxLineSkipNum = treeGrow->IntAttribute("maxLineSkipNum", p.maxLineSkipNum);
        p.maxSkipDistance = treeGrow->DoubleAttribute("maxSkipDistance", p.maxSkipDistance);
        p.minLTypeTreeLen = treeGrow->DoubleAttribute("minLTypeTreeLen", p.minLTypeTreeLen);
        p.minVTypeTreeLen = treeGrow->DoubleAttribute("minVTypeTreeLen", p.minVTypeTreeLen);
    }

    XMLElement* guideDecision = algo->FirstChildElement("GuideDecisionParam");
    if (guideDecision) {
        VrGuideDecisionParam& p = params.guideDecisionParam;
        p.lateralTolerance = guideDecision->DoubleAttribute("lateralTolerance", p.lateralTolerance);
        p.angleTolerance = guideDecision->DoubleAttribute("angleTolerance", p.angleTolerance);
    }

    XMLElement* process = algo->FirstChildElement("ParkingProcessParam");
    if (process) {
        VrParkingProcessParam& p = params.processParam;
        p.approachStartDistance = process->DoubleAttribute("approachStartDistance", p.approachStartDistance);
        p.slowDistance = process->DoubleAttribute("slowDistance", p.slowDistance);
        p.stopDistanceTolerance = process->DoubleAttribute("stopDistanceTolerance", p.stopDistanceTolerance);
        p.stoppedShortMinDistance = process->DoubleAttribute("stoppedShortMinDistance", p.stoppedShortMinDistance);
        p.maxApproachSpeed = process->DoubleAttribute("maxApproachSpeed", p.maxApproachSpeed);
        p.stoppedSpeedThreshold = process->DoubleAttribute("stoppedSpeedThreshold", p.stoppedSpeedThreshold);
        p.speedFilterAlpha = process->DoubleAttribute("speedFilterAlpha", p.speedFilterAlpha);
        p.stopStableFrames = process->IntAttribute("stopStableFrames", p.stopStableFrames);
        p.stoppedShortStableFrames = process->IntAttribute("stoppedShortStableFrames", p.stoppedShortStableFrames);
        p.lostFrameThreshold = process->IntAttribute("lostFrameThreshold", p.lostFrameThreshold);
        p.completedHoldFrames = process->IntAttribute("completedHoldFrames", p.completedHoldFrames);
    }

    XMLElement* modelRecognition = algo->FirstChildElement("ModelRecognitionParam");
    if (modelRecognition) {
        VrModelRecognitionParam& p = params.modelRecognitionParam;
        p.modelVerifyDistance = modelRecognition->DoubleAttribute("modelVerifyDistance", p.modelVerifyDistance);
        p.confidenceThreshold = modelRecognition->DoubleAttribute("confidenceThreshold", p.confidenceThreshold);
        p.maxRecognitionAttempts = modelRecognition->IntAttribute("maxRecognitionAttempts", p.maxRecognitionAttempts);
    }
}

void SaveAlgorithmParams(XMLDocument& doc, XMLElement* root, const VrAlgorithmParams& params)
{
    XMLElement* algo = doc.NewElement("AlgorithmParams");
    root->InsertEndChild(algo);

    const VrPlaneParkingParam& planeParkingParam = params.planeParkingParam;
    XMLElement* planeParking = doc.NewElement("PlaneParkingParam");
    planeParking->SetAttribute("parkingPointX", planeParkingParam.parkingPointX);
    planeParking->SetAttribute("parkingPointY", planeParkingParam.parkingPointY);
    planeParking->SetAttribute("parkingPointZ", planeParkingParam.parkingPointZ);
    planeParking->SetAttribute("guideLinePointX", planeParkingParam.guideLinePointX);
    planeParking->SetAttribute("guideLinePointY", planeParkingParam.guideLinePointY);
    planeParking->SetAttribute("guideLinePointZ", planeParkingParam.guideLinePointZ);
    planeParking->SetAttribute("guidingRange", planeParkingParam.guidingRange);
    planeParking->SetAttribute("parkingRange", planeParkingParam.parkingRange);
    planeParking->SetAttribute("distFromNoseToWheel", planeParkingParam.distFromNoseToWheel);
    algo->InsertEndChild(planeParking);

    const VrPlaneGroundCalibrationParam& groundParam = params.groundCalibrationParam;
    XMLElement* groundCalibration = doc.NewElement("PlaneGroundCalibrationParam");
    groundCalibration->SetAttribute("planeHeight", groundParam.planeHeight);
    SaveMatrix(doc, groundCalibration, "PlaneCalib", groundParam.planeCalib);
    SaveMatrix(doc, groundCalibration, "InvRMatrix", groundParam.invRMatrix);
    algo->InsertEndChild(groundCalibration);

    const VrPlaneTreeGrowParam& treeParam = params.treeGrowParam;
    XMLElement* treeGrow = doc.NewElement("PlaneTreeGrowParam");
    treeGrow->SetAttribute("yDeviationMax", treeParam.yDeviationMax);
    treeGrow->SetAttribute("zDeviationMax", treeParam.zDeviationMax);
    treeGrow->SetAttribute("maxLineSkipNum", treeParam.maxLineSkipNum);
    treeGrow->SetAttribute("maxSkipDistance", treeParam.maxSkipDistance);
    treeGrow->SetAttribute("minLTypeTreeLen", treeParam.minLTypeTreeLen);
    treeGrow->SetAttribute("minVTypeTreeLen", treeParam.minVTypeTreeLen);
    algo->InsertEndChild(treeGrow);

    const VrGuideDecisionParam& guideDecisionParam = params.guideDecisionParam;
    XMLElement* guideDecision = doc.NewElement("GuideDecisionParam");
    guideDecision->SetAttribute("lateralTolerance", guideDecisionParam.lateralTolerance);
    guideDecision->SetAttribute("angleTolerance", guideDecisionParam.angleTolerance);
    algo->InsertEndChild(guideDecision);

    const VrParkingProcessParam& processParam = params.processParam;
    XMLElement* process = doc.NewElement("ParkingProcessParam");
    process->SetAttribute("approachStartDistance", processParam.approachStartDistance);
    process->SetAttribute("slowDistance", processParam.slowDistance);
    process->SetAttribute("stopDistanceTolerance", processParam.stopDistanceTolerance);
    process->SetAttribute("stoppedShortMinDistance", processParam.stoppedShortMinDistance);
    process->SetAttribute("maxApproachSpeed", processParam.maxApproachSpeed);
    process->SetAttribute("stoppedSpeedThreshold", processParam.stoppedSpeedThreshold);
    process->SetAttribute("speedFilterAlpha", processParam.speedFilterAlpha);
    process->SetAttribute("stopStableFrames", processParam.stopStableFrames);
    process->SetAttribute("stoppedShortStableFrames", processParam.stoppedShortStableFrames);
    process->SetAttribute("lostFrameThreshold", processParam.lostFrameThreshold);
    process->SetAttribute("completedHoldFrames", processParam.completedHoldFrames);
    algo->InsertEndChild(process);

    const VrModelRecognitionParam& modelParam = params.modelRecognitionParam;
    XMLElement* modelRecognition = doc.NewElement("ModelRecognitionParam");
    modelRecognition->SetAttribute("modelVerifyDistance", modelParam.modelVerifyDistance);
    modelRecognition->SetAttribute("confidenceThreshold", modelParam.confidenceThreshold);
    modelRecognition->SetAttribute("maxRecognitionAttempts", modelParam.maxRecognitionAttempts);
    algo->InsertEndChild(modelRecognition);
}

void LoadDebugParam(XMLElement* root, VrDebugParam& param)
{
    XMLElement* element = root->FirstChildElement("DebugParam");
    if (!element) {
        return;
    }

    param.enableDebug = element->BoolAttribute("enableDebug", param.enableDebug);
    param.savePointCloud = element->BoolAttribute("savePointCloud", param.savePointCloud);
    param.saveDebugImage = element->BoolAttribute("saveDebugImage", param.saveDebugImage);
    param.printDetailLog = element->BoolAttribute("printDetailLog", param.printDetailLog);
    LoadStringAttribute(element, "debugOutputPath", param.debugOutputPath);
}

void SaveDebugParam(XMLDocument& doc, XMLElement* root, const VrDebugParam& param)
{
    XMLElement* element = doc.NewElement("DebugParam");
    element->SetAttribute("enableDebug", param.enableDebug);
    element->SetAttribute("savePointCloud", param.savePointCloud);
    element->SetAttribute("saveDebugImage", param.saveDebugImage);
    element->SetAttribute("printDetailLog", param.printDetailLog);
    element->SetAttribute("debugOutputPath", param.debugOutputPath.c_str());
    root->InsertEndChild(element);
}

} // namespace

CVrConfig::CVrConfig() = default;

CVrConfig::~CVrConfig() = default;

int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResult)
{
    XMLDocument doc;
    const 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()) != RootName()) {
        LOG_ERR("Config file format error: root element is not %s\n", RootName());
        return LOAD_CONFIG_INVALID_FORMAT;
    }

    configResult = ConfigResult();
    LoadMvsCamera(root, configResult.mvsCamera);
    LoadRsLidar(root, configResult.lidarConfig);
    LoadLedDisplay(root, configResult.ledDisplayConfig);
    LoadUdpBroadcast(root, configResult.udpBroadcastConfig);
    LoadHttpServer(root, configResult.httpServerConfig);
    LoadAlgorithmParams(root, configResult.algorithmParams);
    LoadDebugParam(root, configResult.debugParam);
    configResult.Normalize();

    return LOAD_CONFIG_SUCCESS;
}

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

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

    XMLElement* root = doc.NewElement(RootName());
    doc.InsertEndChild(root);

    SaveMvsCamera(doc, root, configResult.mvsCamera);
    SaveRsLidar(doc, root, configResult.lidarConfig);
    SaveLedDisplay(doc, root, configResult.ledDisplayConfig);
    SaveUdpBroadcast(doc, root, configResult.udpBroadcastConfig);
    SaveHttpServer(doc, root, configResult.httpServerConfig);
    SaveAlgorithmParams(doc, root, configResult.algorithmParams);
    SaveDebugParam(doc, root, configResult.debugParam);

    const XMLError err = doc.SaveFile(filePath.c_str());
    if (err != XML_SUCCESS) {
        LOG_ERR("save config file failed: %s\n", filePath.c_str());
        return false;
    }

    if (m_pNotify) {
        m_pNotify->OnConfigChanged(configResult);
    }

    return true;
}

void CVrConfig::SetConfigChangeNotify(IVrConfigChangeNotify* notify)
{
    m_pNotify = notify;
}

bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
{
    if (!ppVrConfig) {
        return false;
    }

    *ppVrConfig = new CVrConfig();
    return *ppVrConfig != nullptr;
}