
#include "HdLib.h"

#include <string>

#include "CoordSys.h"
#include "FileSys.h"
// #include "Log.h"
#include "Profiler.h"
#include "Timer.h"
#include "Utils.h"

using namespace jf;

ShPtr_HdLib HdLib::m_spInstance = {};
ShPtr_HdLib HdLib::GetInstance() {
    if (nullptr == m_spInstance.get()) {
        ShPtr_HdLib temp(new HdLib());
        m_spInstance.swap(temp);
    }
    return m_spInstance;
}
void HdLib::Init(const std::string &strProjectPath, const std::string &strMapConfigPath) {
    jf::MapConfig::Init(strProjectPath, strMapConfigPath);
    jf::HdWorld::GetSingleton()->LoadAllTilesOnce();
}

/////////////////////////////////////////////////////////////////////
/***********第一部分：Link、Lane几何关系计算****************************/
////////////////////////////////////////////////////////////////////

/**
 * 查询一个点 在哪一个（或者几个）Link、Lane上(准确值）
 * @NOTE 输出的Link和Lane一一对应 没有去重
 * @return
 */
bool HdLib::GetLocateLanes(const Point &ptPoint, std::vector<HdLinkId> &vcthlOutLinkId, std::vector<HdLaneId> &vcthlOutLaneId) {
    //先求这个点大约位置所处于的位置，查找半径：20m。
    std::vector<const HdLink *> vcthlLink = {};
    if (false == GetDataNearby(ptPoint, 20, 20, vcthlLink)) {
        return false;
    }
    for (const HdLink *hdLink : vcthlLink) {
        for (HdLane hdLane : hdLink->vcthlLane) {
            if (IsPointInLane(ptPoint, hdLink, hdLane)) {
                vcthlOutLaneId.push_back(hdLane.liId);
                vcthlOutLinkId.push_back(hdLink->liId);
            }
        }
    }
    if (!vcthlOutLaneId.empty()) {
        return true;
    } else {
        return false;
    }
}

/**
 *  查询一个点 一定矩形范围内 的包含的所有link
 *  @NOTE 矩形范围大小参数： fLonIntervalHalfMetre
 *                       flatintervalhalfmetre
 * @return
 */
bool HdLib::GetDataNearby(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<const HdLink *> &vcthlOutLink, GetDataNearby_param p /*  = GetDataNearby_param::none */, bool bManageTile /* = true */) {
    std::set<const HdLink *> lks;
    Rect2D r = P2R(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre);

    lks = m_hwHdWorld->GetNodesInRect(r);

    if (lks.size() == 0) {
        return false;
    }
    // vector元素去重
    vcthlOutLink = jf::Utils::Set2Vector(lks);
    sort(vcthlOutLink.begin(), vcthlOutLink.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId < hlLink2->liId; });
    vcthlOutLink.erase(unique(vcthlOutLink.begin(), vcthlOutLink.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId == hlLink2->liId; }), vcthlOutLink.end());

    return true;
}

/**
 *  求某个点在一系列轨迹点上的Index位置
 */
void HdLib::GetPosInGeoLink(const Point &ptPos, const std::vector<std::vector<double>> &vctvdGeoLink, double &dOutToHeadDis, double &dOutToTailDis) {
    double dGeoDistance = -1;
    Point ptFootPos = {};
    int nNxtIndex = 0;
    HdLibMath::GetPerpendicularFoot(ptPos, vctvdGeoLink, ptFootPos, dGeoDistance, nNxtIndex);
    // 距离开头的距离
    double dToHeadDis = 0.0;
    // 开始点到垂足前一点的距离（nNxtIndex至少为1）
    for (int i = 0; i < nNxtIndex - 1; ++i) {
        dToHeadDis += PointsDis(vctvdGeoLink.at(i), vctvdGeoLink.at(i + 1));
    }
    // 垂足前一点到垂足点的距离
    dToHeadDis += PointsDis(vctvdGeoLink.at(nNxtIndex - 1), ptFootPos);
    dOutToHeadDis = dToHeadDis;

    // 距离结束点的距离
    double dToTailDis = 0.0;
    dToTailDis += PointsDis(ptFootPos, vctvdGeoLink.at(nNxtIndex));
    for (int i = nNxtIndex; i < vctvdGeoLink.size() - 1; ++i) {
        dToTailDis += PointsDis(vctvdGeoLink.at(i), vctvdGeoLink.at(i + 1));
    }
    dOutToTailDis = dToTailDis;

    return;
}

/**
 * 求某个点 距离哪个车道中心线最近 并求出中心线上最近的那个点
 */
bool HdLib::GetNearestPointfromLaneGeo(const Point &ptPoint, Point &ptOutPoint, double &dOutDistance) {
    std::vector<const HdLink *> vcthlLink = {};
    if (false == GetDataNearby(ptPoint, 20, 20, vcthlLink)) {
        return false;
    }
    double dMinDistance = std::numeric_limits<double>::max();
    HdLane hdLaneResult;
    for (const HdLink *hdLink : vcthlLink) {
        for (HdLane hdLane : hdLink->vcthlLane) {
            Point pointTmp = {};
            double distanceTmp;
            int IndexTmp;
            if ((true == HdLibMath::GetNearestPointFromArray(ptPoint, hdLane.vctvdGeoLink, pointTmp, distanceTmp, IndexTmp)) && (distanceTmp < dMinDistance)) {
                dMinDistance = distanceTmp;
                hdLaneResult = hdLane;
            }
        }
    }
    if (!hdLaneResult.vctvdGeoLink.empty()) {
        return HdLibMath::GetPerpendicularFoot(ptPoint, hdLaneResult.vctvdGeoLink, ptOutPoint, dOutDistance);
    } else {
        return false;
    }
}
/**
 * 判断一个点 是否在某一个lane内
 * @return
 */
bool HdLib::IsPointInLane(const Point &point, const HdLink *cphlLink, const HdLane &hdLane) {
    const std::vector<std::vector<double>> &cvctvdLeftEdgeGeo = cphlLink->vctheEdge[GetEdgeIndexById(cphlLink, hdLane.lLeftEdgeId)].vctvdGeoLink;
    const std::vector<std::vector<double>> &cvctvdRightEdgeGeo = cphlLink->vctheEdge[GetEdgeIndexById(cphlLink, hdLane.lRightEdgeId)].vctvdGeoLink;
    /***左右边界点的存放顺序是否相反***/
    bool isReverseFlag = false;
    if (hdLane.nWidth == 0) {
        if (PointsDis(cvctvdLeftEdgeGeo[0][0], cvctvdLeftEdgeGeo[0][1], cvctvdRightEdgeGeo[0][0], cvctvdRightEdgeGeo[0][1]) * 100 * 10 > 5 * 100 * 10)  //没有车道宽度的默认5m
            isReverseFlag = true;
    } else {
        if (PointsDis(cvctvdLeftEdgeGeo[0][0], cvctvdLeftEdgeGeo[0][1], cvctvdRightEdgeGeo[0][0], cvctvdRightEdgeGeo[0][1]) * 100 * 10 > hdLane.nWidth + 2 * 100 * 10) isReverseFlag = true;  //左右初始点的距离大于车道宽度+2m，则表示左右边界点存放顺序你相反
    }
    return HdLibMath::IsInRecArea(point, cvctvdLeftEdgeGeo, cvctvdRightEdgeGeo, isReverseFlag);
}

/////////////////////////////////////////////////////////////////////
/***********第二部分：路径规划模块相关 **********************************/
////////////////////////////////////////////////////////////////////

/**
 * 根据当前的 LinkEndpoint获取有联通关系的Link，并形成LinkEndpoint输出
 * @return
 */
bool HdLib::GetNextLinkEndpoints(const LinkEndpoint &leCurEndpoint, std::vector<LinkEndpoint> &vctleOutNextEndpoints) {
    // 加载
    // LoadTile(leCurEndpoint.ptEpNode);
    const HdLink *hlCurLink = GetLinkData(leCurEndpoint.lLinkId);
    if (!hlCurLink) {
        return false;
    }

    std::vector<const HdLink *> vcthlNextLinks = {};
    for (const HdLinkId &lLinkId : hlCurLink->vctliNxtLinkId) {
        const HdLink *hlNextLink = GetLinkData(lLinkId);
        if (hlNextLink) {
            vcthlNextLinks.push_back(hlNextLink);
        }
    }

    if (true == vcthlNextLinks.empty()) {
        return false;
    } else {
        for (const HdLink *cphlLink : vcthlNextLinks) {
            LinkEndpoint leEndpoint = {};
            leEndpoint.lLinkId = cphlLink->liId;
            if (false == cphlLink->vctvdGeoLink.empty()) {
                leEndpoint.ptEpNode = Point(cphlLink->vctvdGeoLink[cphlLink->vctvdGeoLink.size() - 1][0], cphlLink->vctvdGeoLink[cphlLink->vctvdGeoLink.size() - 1][1], 0);
            } else {
                leEndpoint.ptEpNode = Point(cphlLink->vcthlLane[0].vctvdGeoLink[cphlLink->vcthlLane[0].vctvdGeoLink.size() - 1][0], cphlLink->vcthlLane[0].vctvdGeoLink[cphlLink->vcthlLane[0].vctvdGeoLink.size() - 1][1], 0);
            }

            leEndpoint.dLength = cphlLink->dLength;

            vctleOutNextEndpoints.push_back(leEndpoint);
        }
        return true;
    }
}

/**
 * 根据当前的 LaneEndpoint获取有联通关系的Lane，并形成LaneEndpoint输出
 * @author muyusheng
 */
bool HdLib::GetNextLaneEndpoints(const LaneEndpoint &leCurEndpoint, std::vector<LaneEndpoint> &vctleOutNextEndpoints) {
    const HdLink *hlCurLink = GetLinkData(leCurEndpoint.lLinkId);
    if (!hlCurLink) {
        return false;
    }
    std::map<HdLinkId, std::vector<HdLane>> NextRoadAndLaneMap;  //与当前laneEndpoint具有联通关系的lane以及lane所属的link
    for (const HdLinkId &lLinkId : hlCurLink->vctliNxtLinkId) {
        const HdLink *hlNextLink = GetLinkData(lLinkId);
        if (hlNextLink) {
            for (HdLane hlNextLane : hlNextLink->vcthlLane) {
                for (HdLaneId lNextLanePLaneId : hlNextLane.vctliPreLaneId) {
                    if (lNextLanePLaneId == leCurEndpoint.lLaneId) {
                        NextRoadAndLaneMap[lLinkId].push_back(hlNextLane);
                    }
                }
            }
        }
    }
    if (true == NextRoadAndLaneMap.empty()) {
        return false;
    }
    for (std::map<HdLinkId, std::vector<HdLane>>::iterator it = NextRoadAndLaneMap.begin(); it != NextRoadAndLaneMap.end(); ++it) {
        //排序
        sort(it->second.begin(), it->second.end(), [](HdLane hdLane1, HdLane hdLane2) {
            HdLaneId hdLaneId1 = hdLane1.liId;
            HdLaneId hdLaneId2 = hdLane2.liId;
            return hdLaneId1.lValue > hdLaneId2.lValue;
        });
        //去重
        it->second.erase(unique(it->second.begin(), it->second.end(),
                                [](HdLane hdLane1, HdLane hdLane2) {
                                    HdLaneId hdLaneId1 = hdLane1.liId;
                                    HdLaneId hdLaneId2 = hdLane2.liId;
                                    return hdLaneId1.lValue == hdLaneId2.lValue;
                                }),
                         it->second.end());
        const HdLink *RoadLinkTmp = GetLinkData(it->first);
        //遍历当前Endpoint联通的下一组Lane
        for (HdLane hdLane : it->second) {
            for (HdLane allHdLane : RoadLinkTmp->vcthlLane) {
                if (IsReachable(RoadLinkTmp, hdLane.liId, allHdLane.liId)) {
                    LaneEndpoint leEp = {};
                    // note：HdLane、HdLink的startPoint、EndPoint、GeoLink属性完全来自原来地图的属性（矢量方向）。HdLane、HdLink的联通关系属性则经过车道方向条件后加工形成。
                    if (allHdLane.nDirection == 2) {  //和矢量方向相同
                        leEp.ptEpNode = Point(allHdLane.vctvdGeoLink[allHdLane.vctvdGeoLink.size() - 1][0], allHdLane.vctvdGeoLink[allHdLane.vctvdGeoLink.size() - 1][1], allHdLane.vctvdGeoLink[allHdLane.vctvdGeoLink.size() - 1][2]);
                    } else if (allHdLane.nDirection == 3) {  //和矢量方向相反
                        leEp.ptEpNode = Point(allHdLane.vctvdGeoLink[0][0], allHdLane.vctvdGeoLink[0][1], allHdLane.vctvdGeoLink[0][2]);
                    } else {
                        std::cout << "=====ERROR=====lane" << allHdLane.liId << "方向需要特殊处理" << std::endl;
                    }
                    leEp.lLinkId = it->first;
                    leEp.lLaneId = allHdLane.liId;  // 取建立全联通后终点所在的车道线的ID
                    leEp.nStartLaneNum = hdLane.nSeqNum;
                    leEp.nEndLaneNum = allHdLane.nSeqNum;
                    leEp.dLength = RoadLinkTmp->dLength;
                    if (leEp.dLength >= abs(leEp.nStartLaneNum - leEp.nEndLaneNum) * 10) {  //如果link长度太短，那么不会变道
                        vctleOutNextEndpoints.push_back(leEp);
                    }
                }
            }
        }
    }
    return true;
}

bool HdLib::GetPreEndpoints(const LaneEndpoint &leCurEndpoint, std::vector<LaneEndpoint> &vctleOutPreEndpoint) {
    const HdLink *hlCurLink = GetLinkData(leCurEndpoint.lLinkId);
    if (!hlCurLink) {
        return false;
    }
    std::vector<const HdLink *> vctstPreLinks = {};
    std::vector<HdLaneId> vctlCurLinkLanes = {};
    for (const HdLinkId &lLinkId : hlCurLink->vctliPreLinkId) {
        const HdLink *cphlPreLink = GetLinkData(lLinkId);
        if (cphlPreLink) {
            for (HdLane hlPreLane : cphlPreLink->vcthlLane) {
                for (HdLaneId lPreLaneNLaneId : hlPreLane.vctliNxtLaneId) {
                    if ((lPreLaneNLaneId == leCurEndpoint.lLaneId) || (true == IsReachable(hlCurLink, lPreLaneNLaneId, leCurEndpoint.lLaneId))) {
                        vctlCurLinkLanes.push_back(lPreLaneNLaneId);
                        vctstPreLinks.push_back(cphlPreLink);
                    }
                }
            }
        }
    }

    // 去重
    sort(vctstPreLinks.begin(), vctstPreLinks.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId < hlLink2->liId; });
    vctstPreLinks.erase(unique(vctstPreLinks.begin(), vctstPreLinks.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId == hlLink2->liId; }), vctstPreLinks.end());

    if ((true == vctstPreLinks.empty()) || true == (*vctstPreLinks.begin())->vcthlLane.empty()) {
        return false;
    } else {
#if 0
        // 返回link内所有的全联通lane
            for (HdLaneId lCurLaneId : vctlCurLinkLanes)
            {
                GenPreEndpoints(lCurLaneId, vctstNextLinks, vctleOutNextEndpoints);
            }
#else
        // 返回由指定lane联通的全联通lane
        GenPreEndpoints(leCurEndpoint.lLaneId, vctstPreLinks, vctleOutPreEndpoint);
#endif
        return true;
    }
}

/**
 * 判断从Lane1是否可以变道到Lane2
 * @param hlLink Lane1、Lane2所属RoadLink
 * @param lLaneId1
 * @param lLaneId2
 * @return
 */
bool HdLib::IsReachable(const HdLink *hlLink, HdLaneId lLaneId1, HdLaneId lLaneId2) {
    int LaneSeqNum1 = -1;  // lane1在vcthlLane数组中的Index
    int LaneSeqNum2 = -1;  // lane2在vcthlLane数组中的Index
    for (int i = 0; i < hlLink->vcthlLane.size(); i++) {
        if (hlLink->vcthlLane[i].liId.lValue == lLaneId1.lValue) {
            LaneSeqNum1 = i;
        }
        if (hlLink->vcthlLane[i].liId.lValue == lLaneId2.lValue) {
            LaneSeqNum2 = i;
        }
    }
    if (LaneSeqNum1 == -1 || LaneSeqNum2 == -1) {
        return false;
    } else if (LaneSeqNum1 == LaneSeqNum2) {
        return true;
    }
    int nSeqNum1 = hlLink->vcthlLane[LaneSeqNum1].nSeqNum;  // lane1的lane_no
    int nSeqNum2 = hlLink->vcthlLane[LaneSeqNum2].nSeqNum;  // lane2的lane_no
    if (nSeqNum1 / nSeqNum2 < 0) {
        return false;
    } else if (nSeqNum1 > 0) {
        if (LaneSeqNum1 > LaneSeqNum2) {  // lane1在lane2的右面
            for (int j = LaneSeqNum1; j > LaneSeqNum2; j--) {
                int CrossType = GetEdgeCross(hlLink, hlLink->vcthlLane[j].lLeftEdgeId);
                if (!(CrossType == sc_nCrossDirec_BidirecAble || CrossType == sc_nCrossDirec_LeftAble)) {
                    return false;
                }
            }
            return true;
        }
        if (LaneSeqNum1 < LaneSeqNum2) {  // lane1在lane2的左面
            for (int j = LaneSeqNum1; j < LaneSeqNum2; j++) {
                int CrossType = GetEdgeCross(hlLink, hlLink->vcthlLane[j].lRightEdgeId);
                if (!(CrossType == sc_nCrossDirec_BidirecAble || CrossType == sc_nCrossDirec_RightAble)) {
                    return false;
                }
            }
            return true;
        }
    } else if (nSeqNum1 < 0) {
        if (LaneSeqNum1 > LaneSeqNum2) {  // lane1在lane2的左面
            for (int j = LaneSeqNum1; j > LaneSeqNum2; j--) {
                int CrossType = GetEdgeCross(hlLink, hlLink->vcthlLane[j].lRightEdgeId);
                if (!(CrossType == sc_nCrossDirec_BidirecAble || CrossType == sc_nCrossDirec_RightAble)) {
                    return false;
                }
            }
            return true;
        }
        if (LaneSeqNum1 < LaneSeqNum2) {  // lane1在lane2的右面
            for (int j = LaneSeqNum1; j < LaneSeqNum2; j++) {
                int CrossType = GetEdgeCross(hlLink, hlLink->vcthlLane[j].lLeftEdgeId);
                if (!(CrossType == sc_nCrossDirec_BidirecAble || CrossType == sc_nCrossDirec_LeftAble)) {
                    return false;
                }
            }
            return true;
        }
    }
}

/**
 * 查询一个点 在哪一个（或者几个）Link上 并形成LinkEndpoint输出（准确值）
 * @return
 */
bool HdLib::GetLocateEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<LinkEndpoint> &vctleOutLinkEp) {
    auto func_IsInLinkArea = [this](const Point &ptPoint, const HdLink *cphlLink) {
        const HdLane &chlFirstLane = cphlLink->vcthlLane[0];
        const HdLane &chlLastLane = cphlLink->vcthlLane[cphlLink->vcthlLane.size() - 1];
        const std::vector<std::vector<double>> &cvctvdLeftEdgeGeo = cphlLink->vctheEdge[GetEdgeIndexById(cphlLink, chlFirstLane.lLeftEdgeId)].vctvdGeoLink;
        const std::vector<std::vector<double>> &cvctvdRightEdgeGeo = cphlLink->vctheEdge[GetEdgeIndexById(cphlLink, chlLastLane.lRightEdgeId)].vctvdGeoLink;

        /***左右边界点的存放顺序是否相反***/
        bool isReverseFlag = false;
        if (chlFirstLane.nWidth == 0) {
            if (PointsDis(cvctvdLeftEdgeGeo[0][0], cvctvdLeftEdgeGeo[0][1], cvctvdRightEdgeGeo[0][0], cvctvdRightEdgeGeo[0][1]) * 100 * 10 > cphlLink->vcthlLane.size() * 4 * 100 * 10)  //没有车道宽度的默认4m
                isReverseFlag = true;
        } else {
            if (PointsDis(cvctvdLeftEdgeGeo[0][0], cvctvdLeftEdgeGeo[0][1], cvctvdRightEdgeGeo[0][0], cvctvdRightEdgeGeo[0][1]) * 100 * 10 > cphlLink->vcthlLane.size() * chlFirstLane.nWidth + 2 * 100 * 10)
                isReverseFlag = true;  //左右初始点的距离大于车道宽度+2m，则表示左右边界点存放顺序你相反
        }
        return HdLibMath::IsInRecArea(ptPoint, cvctvdLeftEdgeGeo, cvctvdRightEdgeGeo, isReverseFlag);
    };

    std::vector<const HdLink *> vcthlLink = {};
    if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) {
        return false;
    }

    int nVctLinkCnt = vcthlLink.size();
    for (size_t i = 0; i < nVctLinkCnt; i++) {
        if (true == func_IsInLinkArea(ptPoint, vcthlLink[i])) {
            LinkEndpoint leEndpoint = {};
            const HdLink *cphlLink = vcthlLink[i];
            leEndpoint.ptEpNode = Point(cphlLink->vctvdGeoLink[cphlLink->vctvdGeoLink.size() - 1][0], cphlLink->vctvdGeoLink[cphlLink->vctvdGeoLink.size() - 1][1], cphlLink->vctvdGeoLink[cphlLink->vctvdGeoLink.size() - 1][2]);
            leEndpoint.lLinkId = cphlLink->liId;
            leEndpoint.dLength = cphlLink->dLength;
            vctleOutLinkEp.push_back(leEndpoint);
        }
    }

    if (false == vctleOutLinkEp.empty()) {
        return true;
    } else {
        return false;
    }
}

/**
 *  查询一个点 在哪一个（或者几个）lane上 并形成laneEndpoint输出（准确值）
 * @return
 */
bool HdLib::GetLocateEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<LaneEndpoint> &vctleOutLaneEp) {
    //先求这个点大致位置所处于的link
    std::vector<const HdLink *> vcthlLink = {};
    if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) {
        return false;
    }
    for (const HdLink *hdLink : vcthlLink) {
        for (HdLane hdLane : hdLink->vcthlLane) {
            if (IsPointInLane(ptPoint, hdLink, hdLane)) {
                LaneEndpoint leEndpoint = {};
                HdLane hlLane = hdLane;
                leEndpoint.lLinkId = hdLink->liId;
                leEndpoint.lLaneId = hlLane.liId;
                leEndpoint.nStartLaneNum = hlLane.nSeqNum;
                leEndpoint.nEndLaneNum = hlLane.nSeqNum;
                vctleOutLaneEp.push_back(leEndpoint);
            }
        }
    }
    if (false == vctleOutLaneEp.empty()) {
        return true;
    } else {
        return false;
    }
}

/**
 * 生成某一个Link中可穿越变道的车道lane_no队列
 * @param hlLink
 * @param mapOutIdxLane
 * @param vctvnOutRightCrossable
 * @param vctvnOutLeftCrossable
 */
void HdLib::GenCrossable(const HdLink *hlLink, std::map<int, const HdLane *> &mapOutIdxLane, std::vector<std::vector<int>> &vctvnOutRightCrossable, std::vector<std::vector<int>> &vctvnOutLeftCrossable) {
    for (size_t i = 0; i < hlLink->vcthlLane.size(); i++) {
        const HdLane *hlLane = &hlLink->vcthlLane[i];
        mapOutIdxLane[hlLane->nSeqNum] = hlLane;
    }
    // 找出可以向右变道的集合：按照从左到右的顺序存储
    std::vector<int> vctnRightCrossable1 = {};
    std::vector<int> vctnRightCrossable2 = {};
    for (auto iterRight = mapOutIdxLane.begin(); iterRight != mapOutIdxLane.end(); ++iterRight) {
        int nRightCross = GetEdgeCross(hlLink, iterRight->second->lRightEdgeId);
        if ((nRightCross == sc_nCrossDirec_BidirecAble || nRightCross == sc_nCrossDirec_RightAble)) {
            if (iterRight->second->nSeqNum < 0) {  //对于双向通行的link中的lane分类讨论
                vctnRightCrossable1.push_back(iterRight->first);
            } else {
                vctnRightCrossable2.push_back(iterRight->first);
            }
        }
    }
    if (!vctnRightCrossable1.empty()) {
        vctvnOutRightCrossable.push_back(vctnRightCrossable1);
    }
    if (!vctnRightCrossable2.empty()) {
        vctvnOutRightCrossable.push_back(vctnRightCrossable2);
    }

    // 找出可以向左变道的集合：按照从右到左的顺序存储
    std::vector<int> vctnLeftCrossable1 = {};
    std::vector<int> vctnLeftCrossable2 = {};
    for (auto iterLeft = mapOutIdxLane.rbegin(); iterLeft != mapOutIdxLane.rend(); ++iterLeft) {
        int nLeftCross = GetEdgeCross(hlLink, iterLeft->second->lLeftEdgeId);
        if ((nLeftCross == sc_nCrossDirec_BidirecAble || nLeftCross == sc_nCrossDirec_LeftAble)) {
            if (iterLeft->second->nSeqNum < 0) {  //对于双向通行的link中的lane分类讨论
                vctnLeftCrossable1.push_back(iterLeft->first);
            } else {
                vctnLeftCrossable2.push_back(iterLeft->first);
            }
        }
    }
    if (!vctnLeftCrossable1.empty()) {
        vctvnOutLeftCrossable.push_back(vctnLeftCrossable1);
    }
    if (!vctnLeftCrossable2.empty()) {
        vctvnOutLeftCrossable.push_back(vctnLeftCrossable2);
    }
}

bool HdLib::GetLinkPreNxt(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctliOutPreLinkIds, std::vector<HdLinkId> &vctliOutNxtLinkdIds) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (nullptr == cphlLink) {
        return false;
    }

    vctliOutPreLinkIds = cphlLink->vctliPreLinkId;
    vctliOutNxtLinkdIds = cphlLink->vctliNxtLinkId;

    return true;
}

bool HdLib::GetNearEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<Point> &vctptOutPoint, std::vector<LinkEndpoint> &vctleOutLinkEp, std::vector<double> &vctdOutDistance) {
    std::vector<const HdLink *> vcthlLink = {};
    if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) {
        return false;
    }

    struct MidResult {
        int nIndexI = -1;
        double dDistance = -1;
        Point ptPoint = {};
    };

    std::vector<MidResult> vctmrResult = {};
    int nVctLinkCnt = vcthlLink.size();
    for (size_t i = 0; i < nVctLinkCnt; i++) {
        MidResult mrCurResult = {};

        // 获取车道参考线上的所有点
        int n = 0;
        const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[i]->vctvdGeoLink;
        if (true == HdLibMath::GetNearestPointFromArray(ptPoint, vctvdGeoLink, mrCurResult.ptPoint, mrCurResult.dDistance, n)) {
            mrCurResult.nIndexI = i;
            vctmrResult.push_back(mrCurResult);
        }
    }

    auto func_SortByDis = [](MidResult &r1, MidResult &r2) { return r1.dDistance < r2.dDistance; };

    sort(vctmrResult.begin(), vctmrResult.end(), func_SortByDis);
    for (auto mrResult : vctmrResult) {
        vctptOutPoint.push_back(mrResult.ptPoint);
        LinkEndpoint leEndpoint = {};
        const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[mrResult.nIndexI]->vctvdGeoLink;
        leEndpoint.ptEpNode = Point(vctvdGeoLink[vctvdGeoLink.size() - 1][0], vctvdGeoLink[vctvdGeoLink.size() - 1][1], vctvdGeoLink[vctvdGeoLink.size() - 1][2]);
        leEndpoint.lLinkId = vcthlLink[mrResult.nIndexI]->liId;
        leEndpoint.dLength = vcthlLink[mrResult.nIndexI]->dLength;
        vctleOutLinkEp.push_back(leEndpoint);
        vctdOutDistance.push_back(mrResult.dDistance);
    }

    if (false == vctmrResult.empty()) {
        return true;
    } else {
        return false;
    }
}

bool HdLib::GetNearEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<Point> &vctptOutPoint, std::vector<LaneEndpoint> &vctleOutLinkEp, std::vector<double> &vctdOutDistance) {
    std::vector<const HdLink *> vcthlLink = {};
    if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) {
        return false;
    }

    struct MidResult {
        int nIndexI = -1;
        int nIndexJ = -1;
        double dDistance = -1;
        Point ptPoint = {};
    };

    std::vector<MidResult> vctmrResult = {};
    int nVctLinkCnt = vcthlLink.size();
    for (size_t i = 0; i < nVctLinkCnt; i++) {
        int nVctLaneCnt = vcthlLink[i]->vcthlLane.size();
        for (size_t j = 0; j < nVctLaneCnt; j++) {
            MidResult mrCurResult = {};

            // 获取指定车道中心线上的所有点
            int n = 0;
            const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[i]->vcthlLane[j].vctvdGeoLink;
            if (true == HdLibMath::GetNearestPointFromArray(ptPoint, vctvdGeoLink, mrCurResult.ptPoint, mrCurResult.dDistance, n)) {
                mrCurResult.nIndexI = i;
                mrCurResult.nIndexJ = j;
                vctmrResult.push_back(mrCurResult);
            }
        }
    }

    auto func_SortByDis = [](MidResult &r1, MidResult &r2) { return r1.dDistance < r2.dDistance; };

    sort(vctmrResult.begin(), vctmrResult.end(), func_SortByDis);
    for (auto mrResult : vctmrResult) {
        vctptOutPoint.push_back(mrResult.ptPoint);
        LaneEndpoint leEndpoint = {};
        HdLane hlLane = vcthlLink[mrResult.nIndexI]->vcthlLane[mrResult.nIndexJ];
        leEndpoint.ptEpNode = Point(hlLane.vctvdGeoLink[hlLane.vctvdGeoLink.size() - 1][0], hlLane.vctvdGeoLink[hlLane.vctvdGeoLink.size() - 1][1], hlLane.vctvdGeoLink[hlLane.vctvdGeoLink.size() - 1][2]);
        leEndpoint.lLinkId = vcthlLink[mrResult.nIndexI]->liId;
        leEndpoint.lLaneId = hlLane.liId;
        leEndpoint.nStartLaneNum = hlLane.nSeqNum;
        leEndpoint.nEndLaneNum = hlLane.nSeqNum;
        leEndpoint.dLength = vcthlLink[mrResult.nIndexI]->dLength;
        vctleOutLinkEp.push_back(leEndpoint);
        vctdOutDistance.push_back(mrResult.dDistance);
    }

    if (false == vctmrResult.empty()) {
        return true;
    } else {
        return false;
    }
}

bool HdLib::GetNearestEndpoint(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, Point &ptOutPoint, LinkEndpoint &leOutEndpoint, double &dOutDistance) {
    std::vector<const HdLink *> vcthlLink = {};
    if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) {
        return false;
    }

    int nVctLinkCnt = vcthlLink.size();
    int nBestIndexI = -1;
    Point ptBestPoint = {};
    double dMinDistance = std::numeric_limits<double>::max();
    for (size_t i = 0; i < nVctLinkCnt; i++) {
        double d = 0;
        Point p = {};

        // 获取车道参考线上的所有点
        int n = 0;
        const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[i]->vctvdGeoLink;
        if ((true == HdLibMath::GetNearestPointFromArray(ptPoint, vctvdGeoLink, p, d, n)) && (d < dMinDistance)) {
            nBestIndexI = i;
            ptBestPoint = p;
            dMinDistance = d;
        }
    }

    if (nBestIndexI != -1) {
        ptOutPoint = ptBestPoint;
        const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[nBestIndexI]->vctvdGeoLink;
        leOutEndpoint.ptEpNode = Point(vctvdGeoLink[vctvdGeoLink.size() - 1][0], vctvdGeoLink[vctvdGeoLink.size() - 1][1], vctvdGeoLink[vctvdGeoLink.size() - 1][2]);
        leOutEndpoint.lLinkId = vcthlLink[nBestIndexI]->liId;
        leOutEndpoint.dLength = vcthlLink[nBestIndexI]->dLength;
        dOutDistance = dMinDistance;
        return true;
    } else {
        return false;
    }
}

bool HdLib::GetNearestEndpoint(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, Point &ptOutPoint, LaneEndpoint &leOutEndpoint, double &dOutDistance) {
    std::vector<const HdLink *> vcthlLink = {};
    if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) {
        return false;
    }

    int nVctLinkCnt = vcthlLink.size();
    int nBestIndexI = -1;
    int nBestIndexJ = -1;
    Point ptBestPoint = {};
    double dMinDistance = std::numeric_limits<double>::max();
    for (size_t i = 0; i < nVctLinkCnt; i++) {
        double d = 0;
        Point p = {};
        int nVctLaneCnt = vcthlLink[i]->vcthlLane.size();
        for (size_t j = 0; j < nVctLaneCnt; j++) {
            // 获取指定车道中心线上的所有点
            int n = 0;
            const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[i]->vcthlLane[j].vctvdGeoLink;
            if ((true == HdLibMath::GetNearestPointFromArray(ptPoint, vctvdGeoLink, p, d, n)) && (d < dMinDistance)) {
                nBestIndexI = i;
                nBestIndexJ = j;
                ptBestPoint = p;
                dMinDistance = d;
            }
        }
    }

    if ((nBestIndexI != -1) && (nBestIndexJ != -1)) {
        ptOutPoint = ptBestPoint;
        HdLane lane = vcthlLink[nBestIndexI]->vcthlLane[nBestIndexJ];
        leOutEndpoint.ptEpNode = Point(lane.vctvdGeoLink[lane.vctvdGeoLink.size() - 1][0], lane.vctvdGeoLink[lane.vctvdGeoLink.size() - 1][1], lane.vctvdGeoLink[lane.vctvdGeoLink.size() - 1][2]);
        leOutEndpoint.lLinkId = vcthlLink[nBestIndexI]->liId;
        leOutEndpoint.lLaneId = lane.liId;
        leOutEndpoint.nStartLaneNum = lane.nSeqNum;
        leOutEndpoint.nEndLaneNum = lane.nSeqNum;
        leOutEndpoint.dLength = vcthlLink[nBestIndexI]->dLength;
        dOutDistance = dMinDistance;
        return true;
    } else {
        return false;
    }
}

std::vector<Point> HdLib::LoadGpses() { return m_hwHdWorld->GetSave()->LoadGpses(); }

void HdLib::GenPreEndpoints(HdLaneId lCurLaneId, const std::vector<const HdLink *> &vcthlPreLink, std::vector<LaneEndpoint> &vctleOutPreEps) {
    for (const HdLink *cphlPreLink : vcthlPreLink) {
        std::map<int, LaneEndpointExt> mapLaneNumEpExt = {};
        for (const HdLane &hlLane : cphlPreLink->vcthlLane) {
            LaneEndpointExt leEndpointExt = {};
            if (hlLane.vctvdGeoLink.empty()) {
                continue;
            }
            leEndpointExt.ptEpNode = Point(hlLane.vctvdGeoLink[hlLane.vctvdGeoLink.size() - 1][0], hlLane.vctvdGeoLink[hlLane.vctvdGeoLink.size() - 1][1], hlLane.vctvdGeoLink[0][2]);
            leEndpointExt.lLinkId = cphlPreLink->liId;
            leEndpointExt.lLaneId = hlLane.liId;
            leEndpointExt.nStartLaneNum = hlLane.nSeqNum;
            leEndpointExt.nEndLaneNum = hlLane.nSeqNum;
            leEndpointExt.dLength = cphlPreLink->dLength;

            leEndpointExt.ptSpNode = Point(hlLane.vctvdGeoLink[0][0], hlLane.vctvdGeoLink[0][1], hlLane.vctvdGeoLink[0][2]);
            leEndpointExt.nLaneNum = hlLane.nSeqNum;
            int nLeftCross = GetEdgeCross(cphlPreLink, hlLane.lLeftEdgeId);
            leEndpointExt.bLChangable = (nLeftCross == sc_nCrossDirec_BidirecAble || nLeftCross == sc_nCrossDirec_LeftAble) ? true : false;
            int nRightCross = GetEdgeCross(cphlPreLink, hlLane.lRightEdgeId);
            leEndpointExt.bRChangable = (nRightCross == sc_nCrossDirec_BidirecAble || nRightCross == sc_nCrossDirec_RightAble) ? true : false;
            leEndpointExt.vctNxtLanes = hlLane.vctliNxtLaneId;

            mapLaneNumEpExt[leEndpointExt.nLaneNum] = leEndpointExt;
        }
        GenPreRLanes(lCurLaneId, cphlPreLink, mapLaneNumEpExt, vctleOutPreEps);
    }
}

int HdLib::GetEdgeCross(const HdLink *hlLink, const HdEdgeId &lId) {
    for (HdEdge heEdge : hlLink->vctheEdge) {
        if (heEdge.lId == lId) {
            return heEdge.nCross;
        }
    }
    return -1;
}

void HdLib::GenPreRLanes(HdLaneId lCurLaneId, const HdLink *cphlPreLink, const std::map<int, LaneEndpointExt> &mapLaneNumEpExt, std::vector<LaneEndpoint> &vctleOutPreEps) {
    // 记录每个车道的终点上绑定的上一组hdEndpoint的集合
    std::map<HdLaneId, std::vector<LaneEndpoint>> mapEndIdEps = {};

    // 先将现有的车道加入map
    for (auto it = mapLaneNumEpExt.begin(); it != mapLaneNumEpExt.end(); ++it) {
        for (HdLaneId lLaneId : it->second.vctNxtLanes) {
            mapEndIdEps[lLaneId].push_back(it->second);
        }
    }

    // 检查向左向右的联通关系
    std::map<int, const HdLane *> mapIdxLane = {};
    std::vector<std::vector<int>> vctvnRightCrossable = {};
    std::vector<std::vector<int>> vctvnLeftCrossable = {};
    GenCrossable(cphlPreLink, mapIdxLane, vctvnRightCrossable, vctvnLeftCrossable);

    // 针对可以向右变道的车道，向右创建全联通关系
    for (std::vector<int> vctnRightCrossable : vctvnRightCrossable) {
        if (false == vctnRightCrossable.empty()) {
            // 先排序
            sort(vctnRightCrossable.begin(), vctnRightCrossable.end());

            int nVctSize = vctnRightCrossable.size();
            //            for (size_t i = vctnRightCrossable[0]; (i <= vctnRightCrossable[nVctSize - 1]) && (mapLaneNumEpExt.find(i) != mapLaneNumEpExt.end()); ++i) {
            for (size_t i = vctnRightCrossable[0]; (i <= vctnRightCrossable[nVctSize - 1]) && (mapLaneNumEpExt.find(i) != mapLaneNumEpExt.end()); ++i) {
                for (size_t j = i + 1; (j <= vctnRightCrossable[nVctSize - 1]) && (mapLaneNumEpExt.find(j) != mapLaneNumEpExt.end()); ++j) {
                    // 非直行车道不允许变道
                    // if ((mapIdxLane.find(j) != mapIdxLane.end()) && (true == mapIdxLane[j].arrows.empty() || std::string::npos == mapIdxLane[j].arrows.find(sc_cArrow_Straight))) {
                    //     continue;
                    // }
                    LaneEndpoint leEp = {};
                    leEp.ptEpNode = mapLaneNumEpExt.at(j).ptEpNode;
                    leEp.lLinkId = mapLaneNumEpExt.at(j).lLinkId;
                    leEp.lLaneId = mapLaneNumEpExt.at(j).lLaneId;  // 取建立全联通后终点所在的车道线的ID
                    leEp.nStartLaneNum = mapLaneNumEpExt.at(i).nStartLaneNum;
                    leEp.nEndLaneNum = mapLaneNumEpExt.at(j).nEndLaneNum;
                    // leEp.dLength = D2XYZ(P2XYZ(mapLaneNumEpExt.at(i).ptSpNode), P2XYZ(mapLaneNumEpExt.at(j).ptEpNode));
                    leEp.dLength = mapLaneNumEpExt.at(i).dLength;

                    for (HdLaneId lLaneId : mapLaneNumEpExt.at(j).vctNxtLanes) {
                        mapEndIdEps[lLaneId].push_back(leEp);
                    }
                }
            }
        }
    }

    // 针对可以向左变道的车道，向左创建全联通关系
    for (std::vector<int> vctnLeftCrossable : vctvnLeftCrossable) {
        if (false == vctnLeftCrossable.empty()) {
            // 先排序
            sort(vctnLeftCrossable.begin(), vctnLeftCrossable.end());

            int nVctSize = vctnLeftCrossable.size();
            for (size_t i = vctnLeftCrossable[nVctSize - 1]; (i >= vctnLeftCrossable[0]) && (mapLaneNumEpExt.find(i) != mapLaneNumEpExt.end()); --i) {
                for (size_t j = i - 1; (j >= vctnLeftCrossable[0]) && (mapLaneNumEpExt.find(j) != mapLaneNumEpExt.end()); --j) {
                    // 非直行车道不允许变道
                    // if ((mapIdxLane.find(j) != mapIdxLane.end()) && (true == mapIdxLane[j].arrows.empty() || std::string::npos == mapIdxLane[j].arrows.find(sc_cArrow_Straight))) {
                    //     continue;
                    // }
                    LaneEndpoint leEp = {};
                    leEp.ptEpNode = mapLaneNumEpExt.at(j).ptEpNode;
                    leEp.lLinkId = mapLaneNumEpExt.at(j).lLinkId;
                    leEp.lLaneId = mapLaneNumEpExt.at(j).lLaneId;  // 取建立全联通后终点所在的车道线的ID
                    leEp.nStartLaneNum = mapLaneNumEpExt.at(i).nStartLaneNum;
                    leEp.nEndLaneNum = mapLaneNumEpExt.at(j).nEndLaneNum;
                    // leEp.dLength = D2XYZ(P2XYZ(mapLaneNumEpExt.at(i).ptSpNode), P2XYZ(mapLaneNumEpExt.at(j).ptEpNode));
                    leEp.dLength = mapLaneNumEpExt.at(i).dLength;

                    for (HdLaneId lLaneId : mapLaneNumEpExt.at(j).vctNxtLanes) {
                        mapEndIdEps[lLaneId].push_back(leEp);
                    }
                }
            }
        }
    }

    // 导出结果
    for (LaneEndpoint leEp : mapEndIdEps[lCurLaneId]) {
        if (leEp.dLength >= abs(leEp.nStartLaneNum - leEp.nEndLaneNum) * 10) {
            vctleOutPreEps.push_back(leEp);
        }
    }
}

bool HdLib::LoadTile(const Point &ptPoint, bool bManageTile /* = true */) {
    Rect2D r = P2R(ptPoint, 1000, 1000);
    if (bManageTile) {
        m_hwHdWorld->LoadTileInRect(r);
        // m_hwHdWorld->RemoveUnsedTiles(ptPoint);
    }

    return true;
}

/////////////////////////////////////////////////////////////////////
/***********第三部分：获取Link、Lane原始属性的所有api********************/
////////////////////////////////////////////////////////////////////

//********************Link相关api********************//
const HdLink *HdLib::GetLinkData(const HdLinkId &lLinkId) {
    const HdLink *ret = m_hwHdWorld->GetNodeData(lLinkId);
    if (!ret) {
        return nullptr;
    }
    return ret;
}

bool HdLib::GetLinkLength(const HdLinkId &lLinkId, double &dOutLinkLength) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        dOutLinkLength = cphlLink->dLength;
        return true;
    }

    return false;
}

bool HdLib::GetLinkType(const HdLinkId &lLinkId, int &nOutLinkType) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        nOutLinkType = cphlLink->nType;
        return true;
    }

    return false;
}

bool HdLib::GetLinkStartAngle(const HdLinkId &lLinkId, double &dOutStartAngle) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        dOutStartAngle = cphlLink->dStartAngle;
        return true;
    }

    return false;
}

bool HdLib::GetLinkSpeedLimit(const HdLinkId &lLinkId, int &nOutSpeedLimit) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        nOutSpeedLimit = cphlLink->nSpeedLimit;
        return true;
    }

    return false;
}

bool HdLib::GetLinkLimitValue(const HdLinkId &lLinkId, int nLimitType, int &nOutLimitValue) {
    // TODO
    // static const int sc_nIndFaciType1_Ban = 3;
    // const HdLink *cphlLink = GetLinkData(lLinkId);
    // if (cphlLink != NULL) {
    //     for (hdindfacility hfFaci : cphlLink->indfacilities) {
    //         if (hfFaci.type1 == sc_nIndFaciType1_Ban) {
    //             if (hfFaci.type2 == nLimitType) {
    //                 nOutLimitValue = hfFaci.value;
    //             }
    //         }
    //     }
    //     return true;
    // }

    return false;
}

bool HdLib::GetLinkRank(const HdLinkId &lLinkId, int &nOutRank) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        nOutRank = cphlLink->nRank;
        return true;
    }

    return false;
}

bool HdLib::GetLinkPre(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctlOutPLinkId) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        if (false == cphlLink->vctliPreLinkId.empty()) {
            vctlOutPLinkId = cphlLink->vctliPreLinkId;
            return true;
        }
    }

    return false;
}

bool HdLib::GetLinkNext(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctlOutNLinkId) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        if (false == cphlLink->vctliNxtLinkId.empty()) {
            vctlOutNLinkId = cphlLink->vctliNxtLinkId;
            return true;
        }
    }

    return false;
}

bool HdLib::GetLinkNext(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<HdLinkId> &vctlOutNLinkId) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        if (false == cphlLink->vctliNxtLinkId.empty()) {
            std::vector<const HdLink *> vctstNextLinks = {};
            for (const HdLinkId &lNLinkId : cphlLink->vctliNxtLinkId) {
                const HdLink *hlNextLink = GetLinkData(lNLinkId);
                if (hlNextLink) {
                    for (HdLane hlNextLane : hlNextLink->vcthlLane) {
                        for (HdLaneId lNextLanePLaneId : hlNextLane.vctliPreLaneId) {
                            if (lNextLanePLaneId == cphlLink->vcthlLane[nLaneSeqNum - 1].liId) {
                                vctstNextLinks.push_back(hlNextLink);
                            }
                        }
                    }
                }
            }

            // 去重
            sort(vctstNextLinks.begin(), vctstNextLinks.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId < hlLink2->liId; });
            vctstNextLinks.erase(unique(vctstNextLinks.begin(), vctstNextLinks.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId == hlLink2->liId; }), vctstNextLinks.end());
            for (const HdLink *cphlNLink : vctstNextLinks) {
                vctlOutNLinkId.push_back(cphlNLink->liId);
            }
            return true;
        }
    }

    return false;
}

//********************Lane相关api********************//
/**
 * 根据linkId和lane_no获取lane对象
 * @param lLinkId
 * @param lane_no  对应地图中的lane图层中的lane_no
 * @param hdLaneOut
 * @return
 */
bool HdLib::GetLaneData(const HdLinkId &lLinkId, int lane_no, HdLane &hdLaneOut) {
    const HdLink *ret = m_hwHdWorld->GetNodeData(lLinkId);
    for (const HdLane hdLane : ret->vcthlLane) {
        if (hdLane.nSeqNum == lane_no) {
            hdLaneOut = hdLane;
            return true;
        }
    }
    return false;
}

/**
 * 根据linkID和nLaneSeqNum 获取车道lane的ID
 * @param lLinkId
 * @param nLaneSeqNum  对应地图中的lane图层中的lane_no
 * @param lOutLaneId
 * @return
 */
bool HdLib::GetLaneId(const HdLinkId &lLinkId, int nLaneSeqNum, HdLaneId &lOutLaneId) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if ((cphlLink != NULL)) {
        for (HdLane hdLane : cphlLink->vcthlLane) {
            if (hdLane.nSeqNum == nLaneSeqNum) {
                lOutLaneId = hdLane.liId;
                return true;
            }
        }
    }
    return false;
}

bool HdLib::GetLaneGeoLink(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<std::vector<double>> &vctvdOutGeoLink) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (const HdLane &hlLane : cphlLink->vcthlLane) {
            if (hlLane.nSeqNum == nLaneSeqNum) {
                vctvdOutGeoLink = hlLane.vctvdGeoLink;
                return true;
            }
        }
    }
    return false;
}

bool HdLib::GetLaneNum(const HdLinkId &lLinkId, const HdLaneId &lLaneId, int &nOutLaneSeqNum) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (size_t i = 0; i < cphlLink->vcthlLane.size(); i++) {
            const HdLane &hlLane = cphlLink->vcthlLane.at(i);
            if (hlLane.liId.lValue == lLaneId.lValue) {
                nOutLaneSeqNum = hlLane.nSeqNum;
                return true;
            }
        }
    }

    return false;
}

bool HdLib::GetLaneType(const HdLinkId &lLinkId, int nLaneSeqNum, int &nOutLaneType) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) {
        nOutLaneType = cphlLink->vcthlLane[nLaneSeqNum - 1].nType;
        return true;
    }

    return false;
}

bool HdLib::GetLaneWidth(const HdLinkId &lLinkId, int nLaneSeqNum, int &nOutLaneWidth) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) {
        nOutLaneWidth = cphlLink->vcthlLane[nLaneSeqNum - 1].nWidth;
        return true;
    }

    return false;
}

bool HdLib::GetLaneEdgeId(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, HdEdgeId &lOutEdgeId) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) {
        lOutEdgeId = bIsLeft == true ? cphlLink->vcthlLane[nLaneSeqNum - 1].lLeftEdgeId : cphlLink->vcthlLane[nLaneSeqNum - 1].lRightEdgeId;
        return true;
    }

    return false;
}

bool HdLib::GetLaneNext(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<HdLaneId> &vctlOutNLaneId) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) {
        vctlOutNLaneId = cphlLink->vcthlLane[nLaneSeqNum - 1].vctliNxtLaneId;
        return true;
    }

    return false;
}

bool HdLib::GetLaneNext(const HdLinkId &lLinkId, const HdLaneId &lLaneId, std::vector<HdLaneId> &vctlOutNLaneId) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (const HdLane &hlLane : cphlLink->vcthlLane) {
            if (hlLane.liId == lLaneId) {
                vctlOutNLaneId = hlLane.vctliNxtLaneId;
                return true;
            }
        }
    }

    return false;
}

bool HdLib::GetLaneGeoLink(const HdLinkId &lLinkId, const HdLaneId &lLaneId, std::vector<std::vector<double>> &vctvdOutGeoLink) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (const HdLane &chlLane : cphlLink->vcthlLane) {
            if (chlLane.liId == lLaneId) {
                vctvdOutGeoLink = chlLane.vctvdGeoLink;
                return true;
            }
        }
    }

    return false;
}

bool HdLib::GetLaneCurvature(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutCurvature) {
    auto func_GetNearestLaneAdas = [this](const std::vector<HdLaneAdas> vctlaLaneADASes, const Point &ptInPoint, int &nOutIndex) {
        double dMinDistance = std::numeric_limits<double>::max();
        int nTmpIndex = 0;
        for (size_t j = 0; j < vctlaLaneADASes.size(); ++j) {
            Point ptGeo = Point(vctlaLaneADASes[j].dLon, vctlaLaneADASes[j].dLat, 0);
            double d = PointsDis(ptGeo, ptInPoint);
            if (d < dMinDistance) {
                dMinDistance = d;
                nTmpIndex = j;
            }
        }

        nOutIndex = nTmpIndex;

        return true;
    };

    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (size_t i = 0; i < cphlLink->vcthlLane.size(); i++) {
            if (cphlLink->vcthlLane[i].liId == lLaneId) {
                int nLaneAdasIndex = 0;
                if (false == cphlLink->vcthlLane[i].vctlaLaneAdas.empty()) {
                    func_GetNearestLaneAdas(cphlLink->vcthlLane[i].vctlaLaneAdas, ptInPoint, nLaneAdasIndex);
                    dOutCurvature = cphlLink->vcthlLane[i].vctlaLaneAdas[nLaneAdasIndex].dOutCurvature;
                    return true;
                }
            }
        }
    }

    return false;
}

bool HdLib::GetLaneRoll(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutRoll) {
    auto func_GetNearestLaneAdas = [this](const std::vector<HdLaneAdas> vctlaLaneADASes, const Point &ptInPoint, int &nOutIndex) {
        double dMinDistance = std::numeric_limits<double>::max();
        int nTmpIndex = 0;
        for (size_t j = 0; j < vctlaLaneADASes.size(); ++j) {
            Point ptGeo = Point(vctlaLaneADASes[j].dLon, vctlaLaneADASes[j].dLat, 0);
            double d = PointsDis(ptGeo, ptInPoint);
            if (d < dMinDistance) {
                dMinDistance = d;
                nTmpIndex = j;
            }
        }

        nOutIndex = nTmpIndex;

        return true;
    };

    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (size_t i = 0; i < cphlLink->vcthlLane.size(); i++) {
            if (cphlLink->vcthlLane[i].liId == lLaneId) {
                int nLaneAdasIndex = 0;
                if (false == cphlLink->vcthlLane[i].vctlaLaneAdas.empty()) {
                    func_GetNearestLaneAdas(cphlLink->vcthlLane[i].vctlaLaneAdas, ptInPoint, nLaneAdasIndex);
                    dOutRoll = cphlLink->vcthlLane[i].vctlaLaneAdas[nLaneAdasIndex].dOutRoll;
                    return true;
                }
            }
        }
    }

    return false;
}

bool HdLib::GetLanePitch(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutPitch) {
    auto func_GetNearestLaneAdas = [this](const std::vector<HdLaneAdas> vctlaLaneADASes, const Point &ptInPoint, int &nOutIndex) {
        double dMinDistance = std::numeric_limits<double>::max();
        int nTmpIndex = 0;
        for (size_t j = 0; j < vctlaLaneADASes.size(); ++j) {
            Point ptGeo = Point(vctlaLaneADASes[j].dLon, vctlaLaneADASes[j].dLat, 0);
            double d = PointsDis(ptGeo, ptInPoint);
            if (d < dMinDistance) {
                dMinDistance = d;
                nTmpIndex = j;
            }
        }

        nOutIndex = nTmpIndex;

        return true;
    };

    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (size_t i = 0; i < cphlLink->vcthlLane.size(); i++) {
            if (cphlLink->vcthlLane[i].liId == lLaneId) {
                int nLaneAdasIndex = 0;
                if (false == cphlLink->vcthlLane[i].vctlaLaneAdas.empty()) {
                    func_GetNearestLaneAdas(cphlLink->vcthlLane[i].vctlaLaneAdas, ptInPoint, nLaneAdasIndex);
                    dOutPitch = cphlLink->vcthlLane[i].vctlaLaneAdas[nLaneAdasIndex].dOutPitch;
                    return true;
                }
            }
        }
    }

    return false;
}

bool HdLib::GetLaneAzimuth(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutAzimuth) {
    auto func_GetNearestLaneAdas = [this](const std::vector<HdLaneAdas> vctlaLaneADASes, const Point &ptInPoint, int &nOutIndex) {
        double dMinDistance = std::numeric_limits<double>::max();
        int nTmpIndex = 0;
        for (size_t j = 0; j < vctlaLaneADASes.size(); ++j) {
            Point ptGeo = Point(vctlaLaneADASes[j].dLon, vctlaLaneADASes[j].dLat, 0);
            double d = PointsDis(ptGeo, ptInPoint);
            if (d < dMinDistance) {
                dMinDistance = d;
                nTmpIndex = j;
            }
        }

        nOutIndex = nTmpIndex;

        return true;
    };

    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (size_t i = 0; i < cphlLink->vcthlLane.size(); i++) {
            if (cphlLink->vcthlLane[i].liId == lLaneId) {
                int nLaneAdasIndex = 0;
                if (false == cphlLink->vcthlLane[i].vctlaLaneAdas.empty()) {
                    func_GetNearestLaneAdas(cphlLink->vcthlLane[i].vctlaLaneAdas, ptInPoint, nLaneAdasIndex);
                    dOutAzimuth = cphlLink->vcthlLane[i].vctlaLaneAdas[nLaneAdasIndex].dOutAzimuth;
                    return true;
                }
            }
        }
    }

    return false;
}

bool HdLib::GetEdgeCnt(const HdLinkId &lLinkId, int &nOutEdgesCnt) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        nOutEdgesCnt = cphlLink->vctheEdge.size();
        return true;
    }

    return false;
}

bool HdLib::GetEdgeId(const HdLinkId &lLinkId, int nEdgeIdx, HdEdgeId &lOutEdgeId) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        lOutEdgeId = cphlLink->vctheEdge[nEdgeIdx].lId;
        return true;
    }

    return false;
}

bool HdLib::GetEdgeCross(const HdLinkId &lLinkId, HdEdgeId lId, int &nOutEdgeCross) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (size_t i = 0; i < cphlLink->vctheEdge.size(); i++) {
            if (cphlLink->vctheEdge[i].lId == lId) {
                nOutEdgeCross = cphlLink->vctheEdge[i].nCross;
                return true;
            }
        }
    }

    return false;
}

bool HdLib::GetEdgeColor(const HdLinkId &lLinkId, HdEdgeId lId, int &nOutEdgeColor) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (size_t i = 0; i < cphlLink->vctheEdge.size(); i++) {
            if (cphlLink->vctheEdge[i].lId == lId) {
                if (true == cphlLink->vctheEdge[i].vcthmMark.empty()) {
                    nOutEdgeColor = 1;  // white for default
                } else {
                    nOutEdgeColor = cphlLink->vctheEdge[i].vcthmMark[0].nColor;
                }
                return true;
            }
        }
    }

    return false;
}

bool HdLib::GetLaneEdgeCrossType(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, int &nOutCrossType) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) {
        HdEdgeId lId = bIsLeft == true ? cphlLink->vcthlLane[nLaneSeqNum - 1].lLeftEdgeId : cphlLink->vcthlLane[nLaneSeqNum - 1].lRightEdgeId;
        int nEdgeIdx = 0;
        for (size_t i = 0; i < cphlLink->vctheEdge.size(); i++) {
            if (cphlLink->vctheEdge[i].lId == lId) {
                nOutCrossType = cphlLink->vctheEdge[i].nCross;
                return true;
            }
        }
    }

    return false;
}

bool HdLib::GetLaneEdgeGeoLink(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, std::vector<std::vector<double>> &vctvdOutGeoLink) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) {
        HdEdgeId lId = bIsLeft == true ? cphlLink->vcthlLane[nLaneSeqNum - 1].lLeftEdgeId : cphlLink->vcthlLane[nLaneSeqNum - 1].lRightEdgeId;
        int nEdgeIdx = 0;
        for (size_t i = 0; i < cphlLink->vctheEdge.size(); i++) {
            if (cphlLink->vctheEdge[i].lId == lId) {
                vctvdOutGeoLink = cphlLink->vctheEdge[i].vctvdGeoLink;
                return true;
            }
        }
    }

    return false;
}

bool HdLib::GetEdgeGeoLink(const HdLinkId &lLinkId, HdEdgeId lId, std::vector<std::vector<double>> &vctvdOutGeoLink) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (size_t i = 0; i < cphlLink->vctheEdge.size(); i++) {
            if (cphlLink->vctheEdge[i].lId == lId) {
                vctvdOutGeoLink = cphlLink->vctheEdge[i].vctvdGeoLink;
                return true;
            }
        }
    }

    return false;
}

bool HdLib::GetStopLineGeoLink(const HdLinkId &clLinkId, int nType, std::vector<std::vector<double>> &vctvdOutGeoLink) {
    const HdLink *cphlLink = GetLinkData(clLinkId);
    if ((cphlLink != NULL) && (false == cphlLink->vctslStopLine.empty())) {
        for (size_t i = 0; i < cphlLink->vctslStopLine.size(); i++) {
            if (cphlLink->vctslStopLine[i].nType == nType) {
                vctvdOutGeoLink = cphlLink->vctslStopLine[i].vctvdGeoLink;
                return true;
            }
        }
    }

    return false;
}

bool HdLib::GetTrafficLightGeoLink(const HdLinkId &clLinkId, int nLaneSeqNum, int nType, std::vector<std::vector<double>> &vctvdOutGeoLink) { return true; }

bool HdLib::GetOverHeadGeoLink(const HdLinkId &clLinkId, int nLaneSeqNum, int nType, std::vector<std::vector<double>> &vctvdOutGeoLink) {
    const HdLink *cphlLink = GetLinkData(clLinkId);
    if (cphlLink != NULL) {
        for (ObjOverHead objOverHead : cphlLink->vctOverHead) {
            for (HdLaneId hdLaneId : objOverHead.vctlLaneId) {
                if (hdLaneId == cphlLink->vcthlLane[nLaneSeqNum - 1].liId) {
                    vctvdOutGeoLink = objOverHead.vctvdGeoLink;
                    return true;
                }
            }
        }
    }
    return false;
}

bool HdLib::GetTrafficSign(const HdLinkId &clLinkId, std::vector<int> &vctnOutShape, std::vector<int> &vctnOutType1, std::vector<int> &vctnOutType2, std::vector<std::string> &vctstOutText, std::vector<std::vector<std::vector<double>>> &vctvvOutGeoArea) {
    // const HdLink *cphlLink = GetLinkData(clLinkId);
    // if ((cphlLink != NULL) && (false == cphlLink->vcttsTrafSign.empty())) {
    //     for (size_t i = 0; i < cphlLink->vcttsTrafSign.size(); i++) {
    //         vctnOutShape.push_back(cphlLink->vcttsTrafSign[i].nShape);
    //         vctnOutType1.push_back(cphlLink->vcttsTrafSign[i].nType1);
    //         vctnOutType2.push_back(cphlLink->vcttsTrafSign[i].nType2);
    //         vctstOutText.push_back(cphlLink->vcttsTrafSign[i].strText);
    //         vctvvOutGeoArea.push_back(cphlLink->vcttsTrafSign[i].vctvdGeoArea);
    //     }
    //     return true;
    // }

    return false;
}

bool HdLib::GetTrafficSign(const Point &cptPoint, std::vector<int> &vctnOutShape, std::vector<int> &vctnOutType1, std::vector<int> &vctnOutType2, std::vector<std::string> &vctstOutText, std::vector<std::vector<std::vector<double>>> &vctvvOutGeoArea) {
    std::vector<HdLinkId> vcthlLinkId;
    std::vector<const HdLink *> vcthlLink = {};
    if (true == GetDataNearby(cptPoint, MapConfig::tile_lon_interval, MapConfig::tile_lat_interval, vcthlLink)) {
        for (auto link : vcthlLink) {
            vcthlLinkId.push_back(link->liId);
        }
        for (const HdLinkId &lLinkId : vcthlLinkId) {
            GetTrafficSign(lLinkId, vctnOutShape, vctnOutType1, vctnOutType2, vctstOutText, vctvvOutGeoArea);
        }
        return true;
    }

    return false;
}

bool HdLib::GetTrafficSignGeoArea(const HdLinkId &clLinkId, int nShape, std::vector<std::vector<double>> &vctvdOutGeoArea) {
    // const HdLink *cphlLink = GetLinkData(clLinkId);
    // if ((cphlLink != NULL) && (false == cphlLink->vcttsTrafSign.empty())) {
    //     for (size_t i = 0; i < cphlLink->vcttsTrafSign.size(); i++) {
    //         if (cphlLink->vcttsTrafSign[i].nShape == nShape) {
    //             vctvdOutGeoArea = cphlLink->vcttsTrafSign[i].vctvdGeoArea;
    //             return true;
    //         }
    //     }
    // }

    return false;
}

bool HdLib::IsLaneContainArrow(const HdLinkId &lLinkId, int nLaneSeqNum, char cArrowType) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) {
        if (cphlLink->vcthlLane[nLaneSeqNum - 1].strArrows.find(sc_cArrow_LeftConf) != std::string::npos) {
            return true;
        }
    }

    return false;
}

bool HdLib::GetLaneArrow(const HdLinkId &lLinkId, const HdLaneId &lLaneId, std::string &strOutArrows) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if (cphlLink != NULL) {
        for (const HdLane &hlLane : cphlLink->vcthlLane) {
            if (hlLane.liId == lLaneId) {
                strOutArrows = hlLane.strArrows;
                for (const ObjArrow &arArrow : hlLane.vctarArrow) {
                    strOutArrows += arArrow.strType;
                }
                return true;
            }
        }
    }

    return false;
}

bool HdLib::ConvertGeoLink(const std::vector<std::vector<double>> &vctvdGeoLink, std::vector<Point> &vctptOutGeoLink) {
    for (std::vector<double> geo : vctvdGeoLink) {
        vctptOutGeoLink.push_back(Point(geo[0], geo[1], geo[2]));
    }

    return true;
}

bool HdLib::ConvertGeoLink(const std::vector<Point> &vctptGeoLink, std::vector<std::vector<double>> &vctvdOutGeoLink) {
    for (Point ptPoint : vctptGeoLink) {
        std::vector<double> vctdGeo = {ptPoint.dLon, ptPoint.dLat, ptPoint.dAlt};
        vctvdOutGeoLink.push_back(vctdGeo);
    }

    return true;
}

int HdLib::GetLinkSpeedLimit(const LaneEndpoint &leEndpoint) {
    // 加载
    // LoadTile(leEndpoint.ptEpNode);
    const HdLink *hlCurLink = GetLinkData(leEndpoint.lLinkId);
    if (!hlCurLink) {
        return -1;
    } else {
        return hlCurLink->nSpeedLimit;
    }
}

bool HdLib::HasStopLine(const LaneEndpoint &leEndpoint) {
    // 加载
    // LoadTile(leEndpoint.ptEpNode);
    const HdLink *hlCurLink = GetLinkData(leEndpoint.lLinkId);
    if (!hlCurLink) {
        return false;
    } else {
        for (const HdEdge &heEdge : hlCurLink->vctheEdge) {
            static const int sc_nEdgeType_StopLine = 7;
            if (sc_nEdgeType_StopLine == heEdge.nType) {
                return true;
            }
        }
        return false;
    }
}

bool HdLib::GetGeoLink(const LaneEndpoint &leEndpoint, std::vector<std::vector<double>> &vctvdOutGeoLink, bool bManageTile /* = false*/) {
    // 加载
    if (true == bManageTile) {
        Rect2D r = P2R(leEndpoint.ptEpNode, MapConfig::tile_lon_interval, MapConfig::tile_lon_interval);
        m_hwHdWorld->LoadTileInRect(r);
        // m_hwHdWorld->RemoveUnsedTiles(leEndpoint.ptEpNode);
    }

    const HdLink *hlLink = GetLinkData(leEndpoint.lLinkId);
    if (!hlLink) {
        return false;
    }

    if (leEndpoint.nStartLaneNum == leEndpoint.nEndLaneNum) {
        vctvdOutGeoLink = hlLink->vcthlLane[leEndpoint.nEndLaneNum - 1].vctvdGeoLink;
    } else {
        vctvdOutGeoLink.push_back(hlLink->vcthlLane[leEndpoint.nStartLaneNum - 1].vctvdGeoLink[0]);
        vctvdOutGeoLink.push_back(hlLink->vcthlLane[leEndpoint.nEndLaneNum - 1].vctvdGeoLink[hlLink->vcthlLane[leEndpoint.nEndLaneNum - 1].vctvdGeoLink.size() - 1]);
    }

    return true;
}

bool HdLib::GetGeoLink(const HdLinkId &lLinkId, std::vector<std::vector<double>> &vctvdOutGeoLink) {
    const HdLink *hlLink = GetLinkData(lLinkId);
    if (!hlLink) {
        return false;
    }

    vctvdOutGeoLink = hlLink->vctvdGeoLink;

    return true;
}

double HdLib::GetDistanceToLeftEdge(const LaneEndpoint &leEndpoint, const Point &ptPoint) {
    // 加载
    // LoadTile(leEndpoint.ptEpNode);
    const HdLink *hlLink = GetLinkData(leEndpoint.lLinkId);
    if (!hlLink) {
        return -1;
    } else {
        int nLeftestLaneIndex = 0;
        double dMinDistance = std::numeric_limits<double>::max();

        int nIndex = GetEdgeIndexById(hlLink, hlLink->vcthlLane[nLeftestLaneIndex].lLeftEdgeId);
        if (nIndex != -1) {
            Point ptOutPoint = {};
            double dOutDistance = 0;

            HdLibMath::GetPerpendicularFoot(ptPoint, hlLink->vctheEdge[nIndex].vctvdGeoLink, ptOutPoint, dOutDistance);
            dMinDistance = dOutDistance;
        }

        return dMinDistance;
    }
}

double HdLib::GetDistanceToRightEdge(const LaneEndpoint &leEndpoint, const Point &ptPoint) {
    // 加载
    // LoadTile(leEndpoint.ptEpNode);
    const HdLink *hlLink = GetLinkData(leEndpoint.lLinkId);
    if (!hlLink) {
        return -1;
    } else {
        int nRightestLaneIndex = hlLink->vcthlLane.size() - 1;
        double dMinDistance = std::numeric_limits<double>::max();

        int nIndex = GetEdgeIndexById(hlLink, hlLink->vcthlLane[nRightestLaneIndex].lRightEdgeId);
        if (nIndex != -1) {
            Point ptOutPoint = {};
            double dOutDistance = 0;

            HdLibMath::GetPerpendicularFoot(ptPoint, hlLink->vctheEdge[nIndex].vctvdGeoLink, ptOutPoint, dOutDistance);
            dMinDistance = dOutDistance;
        }

        return dMinDistance;
    }
}

int HdLib::GetEdgeIndexById(const HdLink *hlLink, HdEdgeId lId) {
    for (int i = 0; i < hlLink->vctheEdge.size(); ++i) {
        if (hlLink->vctheEdge[i].lId == lId) {
            return i;
        }
    }
    return -1;
}

bool HdLib::GetLinkIdsInRect(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<HdLinkId> &vctlOutLinkId, bool bManageTile /* = true */) {
    auto start = Timer::Now();

    // rect r = P2R<hdconfig>(ingps);
    Rect2D r = P2R(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre);
    if (bManageTile) {
        m_hwHdWorld->LoadTileInRect(r);
        // m_hwHdWorld->RemoveUnsedTiles(ingps);
    }

    std::set<const HdLink *> links = m_hwHdWorld->GetNodesInRect(r);
    std::vector<const HdLink *> ls = jf::Utils::Set2Vector(links);

    return true;
}

std::set<HdLinkId> HdLib::GetNodeIdsInRect(const Rect2D &rc2Rect) { return m_hwHdWorld->GetNodeIdsInRect(rc2Rect); }

bool HdLib::GetPosInHdLink(const Point &ptCurPos, const HdLinkId &lLinkId, double &dOutToHeadDis, double &dOutToTailDis) {
    const HdLink *stLink = GetLinkData(lLinkId);
    if (!stLink) {
        return false;
    }

    Point ptFootPos = {};
    double dNearDis = -1;
    int nNxtIndex = 0;
    if (false == HdLibMath::GetNearestPointFromArray(ptCurPos, stLink->vctvdGeoLink, ptFootPos, dNearDis, nNxtIndex)) {
        return false;
    }

    double dRealLength = 0;
    for (int i = 0; i < nNxtIndex - 1; i++) {
        dRealLength += PointsDis(Point(stLink->vctvdGeoLink[i]), Point(stLink->vctvdGeoLink[i + 1]));
    }
    dOutToHeadDis = dRealLength + PointsDis(Point(stLink->vctvdGeoLink[nNxtIndex - 1]), ptFootPos);

    dRealLength = PointsDis(ptFootPos, Point(stLink->vctvdGeoLink[nNxtIndex]));
    for (size_t i = nNxtIndex; i < stLink->vctvdGeoLink.size() - 1; ++i) {
        dRealLength += PointsDis(Point(stLink->vctvdGeoLink[i]), Point(stLink->vctvdGeoLink[i + 1]));
    }
    dOutToTailDis = dRealLength;

    return true;
}

bool HdLib::GetLanesInTypes(std::vector<int> &vctnLaneType, const HdLink *hlLink, std::vector<const HdLane *> &vcthlOutLanes) {
    for (auto &hlLane : hlLink->vcthlLane) {
        if (true == Utils::VectorContains(vctnLaneType, hlLane.nType)) {
            vcthlOutLanes.push_back(&hlLane);
        }
    }

    return true;
}

bool HdLib::GetLanesOutTypes(std::vector<int> &vctnLaneType, const HdLink *hlLink, std::vector<const HdLane *> &vcthlOutLanes) {
    for (auto &hlLane : hlLink->vcthlLane) {
        if (false == Utils::VectorContains(vctnLaneType, hlLane.nType)) {
            vcthlOutLanes.push_back(&hlLane);
        }
    }

    return true;
};

bool HdLib::GetLaneCnt(const HdLinkId &lLinkId, int &nOutLaneCnt) {
    const HdLink *cphlLink = GetLinkData(lLinkId);
    if ((cphlLink != NULL)) {
        nOutLaneCnt = cphlLink->vcthlLane.size();
        return true;
    }
    return false;
}
