#include "MapInterface.hpp"

#include <sys/time.h>

#include <cmath>
#include <iostream>
#include <limits>

namespace jf {
static const int sc_nMaxPoints = 50;
static const int sc_nPostSpeed = 10;
MapInterface::MapInterface(const std::string& strPrjPath, const std::string& strCfgPath, int nTgtLen) : m_spHdLib(HdLib::GetInstance()), m_nTgtLen(nTgtLen), m_dMinPointsDis(nTgtLen / sc_nMaxPoints) { m_spHdLib->Init(strPrjPath, strCfgPath); }

MapInterface::~MapInterface() {}

bool MapInterface::GetMapData(const Adma& adAdma, MapData& mdOutMapData) {
    static HdLinkId s_liLastLinkId = {};
    static HdLaneId s_liLastLaneId = {};
    static double s_dLastHeading = 0.0;

    static int s_nProcessCnt = 0;
    static Point s_ptLastValidLocate = {};
    s_nProcessCnt++;

    auto func_FinishInfo = [this](long lStartTime, bool bSuccess, const Adma& adAdma, MapData& mdOutMapData) {
        long lEndTime = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1);

        mdOutMapData.bOnMapFlag = bSuccess;

        MakeGnss(adAdma, lEndTime - lStartTime, mdOutMapData.gnssInfo);
        if (s_nProcessCnt % sc_nPostSpeed == 0) {
            return true;
        } else {
            return false;
        }
    };

    auto func_IsInCrossing = [this](const HdLinkId& liLinkId) {
        const HdLink* cphlLink = m_spHdLib->GetLinkData(liLinkId);
        if (nullptr == cphlLink) {
            return false;
        }

        for (int i = 0; i < cphlLink->vcthlLane.size(); ++i) {
            const HdLane& hlLane = cphlLink->vcthlLane.at(i);
            if (hlLane.nType == HdLane::JunctionLane) {
                return true;
            }
        }

        return false;
    };

    long lStartTime = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1);

    Point ptInPoint(adAdma.ins_long_abs, adAdma.ins_lat_abs, adAdma.ins_height);

    std::vector<HdLinkId> vctliLocateLinkIds = {};
    std::vector<HdLaneId> vctliLocateLaneIds = {};
    HdLinkId liLocateLinkId = {};
    HdLaneId liLocateLaneId = {};
    if (false == m_spHdLib->GetLocateLanes(ptInPoint, vctliLocateLinkIds, vctliLocateLaneIds)) {
        // 上次结果不可用
        if (s_liLastLaneId.lValue == 0) {
            s_liLastLinkId = {};
            s_liLastLaneId = {};
            return func_FinishInfo(lStartTime, false, adAdma, mdOutMapData);
        }
        // 上次结果可以用：分情况判断
        else {
            // 右转时临时脱离
            if (PointsDis(ptInPoint, s_ptLastValidLocate) <= 20 && true == func_IsInCrossing(s_liLastLinkId)) {
                vctliLocateLinkIds.push_back(s_liLastLinkId);
                vctliLocateLaneIds.push_back(s_liLastLaneId);
            }
            // 距离太远
            else {
                s_liLastLinkId = {};
                s_liLastLaneId = {};
                return func_FinishInfo(lStartTime, false, adAdma, mdOutMapData);
            }
        }
    }

    if (vctliLocateLaneIds.size() >= 2) {
        // 首先，选择航向角匹配的车道
        std::vector<int> vctnAngleMatchedLanes = {};
        for (int i = 0; i < vctliLocateLaneIds.size(); ++i) {
            double dAzimuth = 0.0;
            m_spHdLib->GetLaneAzimuth(vctliLocateLinkIds.at(i), vctliLocateLaneIds.at(i), ptInPoint, dAzimuth);
            double dAngleSub = 0.0;
            if (adAdma.ins_yaw != 0) {
                dAngleSub = std::min(std::abs(dAzimuth - adAdma.ins_yaw), std::abs(360 - (dAzimuth - adAdma.ins_yaw)));
            } else if (s_dLastHeading != 0) {
                dAngleSub = std::min(std::abs(dAzimuth - s_dLastHeading), std::abs(360 - (dAzimuth - s_dLastHeading)));
            } else {
                return func_FinishInfo(lStartTime, false, adAdma, mdOutMapData);
            }
            if (dAngleSub < 45) {
                vctnAngleMatchedLanes.push_back(i);
            }
        }

        // 多个车道符合条件或者没有符合条件的车道
        if (vctnAngleMatchedLanes.size() >= 2 || vctnAngleMatchedLanes.empty() == true) {
            // 应该尝试使用INS的高程作为判断，但INS的高程并不是很精确，所以不使用
            liLocateLinkId = s_liLastLinkId;
            liLocateLaneId = s_liLastLaneId;
        } else {
            liLocateLinkId = vctliLocateLinkIds.at(vctnAngleMatchedLanes.at(0));
            liLocateLaneId = vctliLocateLaneIds.at(vctnAngleMatchedLanes.at(0));
        }
    } else {
        // 即使只有一条车道符合条件，也可能存在坐标点与实际车道不符合的情况
        if (true == func_IsInCrossing(vctliLocateLinkIds.at(0))) {
            double dAzimuth = 0.0;
            m_spHdLib->GetLaneAzimuth(vctliLocateLinkIds.at(0), vctliLocateLaneIds.at(0), ptInPoint, dAzimuth);
            double dAngleSub = std::min(std::abs(dAzimuth - adAdma.ins_yaw), std::abs(360 - (dAzimuth - adAdma.ins_yaw)));
            if (dAngleSub < 45) {
                liLocateLinkId = vctliLocateLinkIds.at(0);
                liLocateLaneId = vctliLocateLaneIds.at(0);
            }
            // 角度不匹配，使用上次结果
            else {
                liLocateLinkId = s_liLastLinkId;
                liLocateLaneId = s_liLastLaneId;
            }
        } else {
            liLocateLinkId = vctliLocateLinkIds.at(0);
            liLocateLaneId = vctliLocateLaneIds.at(0);
        }
    }

    // 更新数据，以备下次使用
    if (0 != liLocateLinkId.lValue) {
        s_ptLastValidLocate = ptInPoint;
        s_liLastLinkId = liLocateLinkId;
        s_liLastLaneId = liLocateLaneId;
    } else {
        return func_FinishInfo(lStartTime, false, adAdma, mdOutMapData);
    }
    if (0.0 != adAdma.ins_yaw) {
        s_dLastHeading = adAdma.ins_yaw;
    }

    // 输出频率为Adma的十分之一
    if (s_nProcessCnt % sc_nPostSpeed == 0) {
        if (false == MakeRoad(adAdma, liLocateLinkId, liLocateLaneId, m_nTgtLen, mdOutMapData.road)) {
            return func_FinishInfo(lStartTime, false, adAdma, mdOutMapData);
        }
        return func_FinishInfo(lStartTime, true, adAdma, mdOutMapData);
    } else {
        return false;
    }
}

bool MapInterface::MakeRoad(const Adma& adAdma, const HdLinkId& liLinkId, const HdLaneId& liLaneId, double dBroadDis, Road& rdOutRoadData) {
    double dToHeadDis = 0;
    double dToTailDis = 0;
    std::vector<HdLinkId> vctlFutureLinkId = {};
    std::vector<HdLaneId> vctlFutureLaneId = {};
    std::vector<double> vctdFutureLength = {};
    Point ptCurLoc(adAdma.ins_long_abs, adAdma.ins_lat_abs, adAdma.ins_height);

    auto func_IsInCrossing = [this](const HdLinkId& liLinkId) {
        const HdLink* cphlLink = m_spHdLib->GetLinkData(liLinkId);
        if (nullptr == cphlLink) {
            return false;
        }

        for (int i = 0; i < cphlLink->vcthlLane.size(); ++i) {
            const HdLane& hlLane = cphlLink->vcthlLane.at(i);
            if (hlLane.nType == HdLane::JunctionLane) {
                return true;
            }
        }

        return false;
    };

    auto func_GetLanePriority = [this, &func_IsInCrossing](const Adma& adAdma, const HdLinkId& liLinkId, const HdLaneId& liLaneId) {
        const HdLink* cphlLink = m_spHdLib->GetLinkData(liLinkId);
        std::string strArrows = "";
        m_spHdLib->GetLaneArrow(liLinkId, liLaneId, strArrows);
        int nLaneSeqNum = 0;
        m_spHdLib->GetLaneNum(liLinkId, liLaneId, nLaneSeqNum);
        bool bIsInCrossing = func_IsInCrossing(liLinkId);
        // 先检查打灯：左
        if (adAdma.turn_lamp_state == 1 && nLaneSeqNum == 1 && false == bIsInCrossing) {
            return LanePriority::LeftFirst;
        }
        // 先检查打灯：右
        else if (adAdma.turn_lamp_state == 2 && nLaneSeqNum == cphlLink->vcthlLane.size() && false == bIsInCrossing) {
            return LanePriority::RightFirst;
        }
        // 再检查地面标识：直行
        else if ((true == strArrows.empty()) || (strArrows.npos != strArrows.find(sc_cArrow_Straight)) || adAdma.turn_lamp_state == 0) {
            return LanePriority::Straight;
        }
        // 再检查地面标识：左转
        else if (strArrows.npos != strArrows.find(sc_cArrow_LeftTurn) && strArrows.npos == strArrows.find(sc_cArrow_Straight)) {
            return LanePriority::LeftFirst;
        }
        // 再检查地面标识：右转
        else if (strArrows.npos != strArrows.find(sc_cArrow_RightTurn) && strArrows.npos == strArrows.find(sc_cArrow_Straight)) {
            return LanePriority::RightFirst;
        }
        // 默认
        else {
            return LanePriority::Straight;
        }
    };

    LanePriority nLanePrior = func_GetLanePriority(adAdma, liLinkId, liLaneId);

    if (true == GetFutureLinks(adAdma, liLinkId, liLaneId, dBroadDis, nLanePrior, dToHeadDis, dToTailDis, vctlFutureLinkId, vctlFutureLaneId, vctdFutureLength)) {
        for (size_t i = 0; i < vctlFutureLinkId.size(); i++) {
            HdLinkId liFutureLinkId = vctlFutureLinkId[i];
            HdLaneId liFutureLaneId = vctlFutureLaneId[i];
            double dFutureLength = vctdFutureLength[i];
            const HdLink* cphlFutureLink = m_spHdLib->GetLinkData(liFutureLinkId);
            if (NULL == cphlFutureLink) {
                return false;
            }

            Point ptStartGeoFoot = {};
            int nStartGeoIndex = 0;
            Point ptEndGeoFoot = {};
            int nEndGeoIndex = 0;
            if (0 == i) {
                std::vector<std::vector<double>> vctvdLaneGeoLink = {};
                m_spHdLib->GetLaneGeoLink(liFutureLinkId, liFutureLaneId, vctvdLaneGeoLink);
                GetPosInGeoLink(ptCurLoc, vctvdLaneGeoLink, true, ptStartGeoFoot, nStartGeoIndex);
            }
            if (vctlFutureLinkId.size() - 1 == i) {
                std::vector<std::vector<double>> vctvdLaneGeoLink = {};
                m_spHdLib->GetLaneGeoLink(liFutureLinkId, liFutureLaneId, vctvdLaneGeoLink);
                GetLenPointAndIndex(cphlFutureLink->dLength - (dFutureLength - dBroadDis), vctvdLaneGeoLink, true, ptEndGeoFoot, nEndGeoIndex);
            }

            MakeRoadLink(dFutureLength, cphlFutureLink, liFutureLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, nLanePrior, rdOutRoadData.roadLinks);

            if (false == rdOutRoadData.stopLine.bStopLineValidFlag) {
                MakeStopLine(dFutureLength, cphlFutureLink, liFutureLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, nLanePrior, rdOutRoadData.stopLine);
            }

            if (false == rdOutRoadData.pedestrianCrossing.bPedestrianCrossingValidFlag) {
                MakePedestrianCrossing(dFutureLength, cphlFutureLink, liFutureLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, nLanePrior, rdOutRoadData.pedestrianCrossing);
            }

            if (false == rdOutRoadData.crossing.bCrossingValidFlag) {
                MakeCrossing(dFutureLength, cphlFutureLink, liFutureLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, nLanePrior, rdOutRoadData.crossing);
            }

            if (false == rdOutRoadData.ramp.bRampValidFlag) {
                MakeRamp(dFutureLength, cphlFutureLink, liFutureLaneId, ptStartGeoFoot, 0, ptEndGeoFoot, nEndGeoIndex, dBroadDis, nLanePrior, rdOutRoadData.ramp);
            }

            if (false == rdOutRoadData.changePoint.bChangeValidFlag) {
                MakeChange(dFutureLength, cphlFutureLink, liFutureLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, nLanePrior, rdOutRoadData.changePoint);
            }

            if (false == rdOutRoadData.speed.bSpeedLimitValidFlag) {
                MakeSpeed(dFutureLength, cphlFutureLink, liFutureLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, nLanePrior, rdOutRoadData.speed);
            }

            if (false == rdOutRoadData.trafficLight.bTrafficLightValidFlag) {
                MakeTrafficLight(dFutureLength, cphlFutureLink, liFutureLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, nLanePrior, rdOutRoadData.trafficLight);
            }

            if (false == rdOutRoadData.rsu.bRsuValidFlag) {
                MakeRSU(dFutureLength, cphlFutureLink, liFutureLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, nLanePrior, rdOutRoadData.rsu);
            }

            if (false == rdOutRoadData.tunnel.bTunnelValidFlag) {
                MakeTunnel(dFutureLength, cphlFutureLink, liFutureLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, nLanePrior, rdOutRoadData.tunnel);
            }

            if (rdOutRoadData.u8_CurrentLane <= 0) {
                for (int i = 0; i < cphlFutureLink->vcthlLane.size(); i++) {
                    if (cphlFutureLink->vcthlLane.at(i).liId == liFutureLaneId) {
                        rdOutRoadData.u8_CurrentLane = cphlFutureLink->vcthlLane.at(i).nSeqNum;
                        break;
                    }
                }
            }

            if (-1 == rdOutRoadData.f_dashSolidChangeDis) {
                double dDashSolidChangeDis = 0;
                MakeDashSolidChangeDis(dFutureLength, cphlFutureLink, liFutureLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, nLanePrior, dDashSolidChangeDis);
                rdOutRoadData.f_dashSolidChangeDis = dDashSolidChangeDis;
            }
        }
    }

    return true;
}

bool MapInterface::MakeGnss(const Adma& adAdma, uint64_t lMapCostTime, Gnss& gnOutGnss) {
    long lNowTime = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1);  // use this
    gnOutGnss.u64_time_msec = adAdma.ins_time_msec;
    gnOutGnss.u64_time_week = adAdma.ins_time_week;
    gnOutGnss.u64_EpochTimestamp = adAdma.ins_gnss_ts_msec_epoch;
    gnOutGnss.locationRel;  // todo
    gnOutGnss.locationAbs.d_longitude = adAdma.ins_long_abs;
    gnOutGnss.locationAbs.d_latitude = adAdma.ins_lat_abs;
    gnOutGnss.locationAbs.f_altitude = adAdma.ins_height;
    gnOutGnss.f_pitch = adAdma.ins_pitch;
    gnOutGnss.f_roll = adAdma.ins_roll;
    gnOutGnss.f_yaw = adAdma.ins_yaw;  // 应客户要求修改 2021/11/02
    gnOutGnss.f_acceleX = adAdma.acc_hor_x;
    gnOutGnss.f_acceleY = adAdma.acc_hor_y;
    gnOutGnss.f_acceleZ = adAdma.acc_hor_z;
    gnOutGnss.u8_turnLampState = adAdma.turn_lamp_state;
    gnOutGnss.u64_AdmaMsgReceivedTime = adAdma.start_timestamp_ms_local;
    gnOutGnss.u64_SensorNodeProcessTime = lNowTime - adAdma.start_timestamp_ms_local;
    gnOutGnss.u64_MapProcessTime = lMapCostTime;

    return true;
}

bool MapInterface::GetFutureLinks(const Adma& adAdma, const HdLinkId& liLinkId, const HdLaneId& liLaneId, double dBroadDis, LanePriority& nInOutLanePrior, double& dOutToHeadDis, double& dOutToTailDis, std::vector<HdLinkId>& vctliOutFutureLinkId,
                                  std::vector<HdLaneId>& vctliOutFutureLaneId, std::vector<double>& vctdOutRealLength) {
    Point ptCurPos(adAdma.ins_long_abs, adAdma.ins_lat_abs, adAdma.ins_height);

    if (false == m_spHdLib->GetPosInHdLink(ptCurPos, liLinkId, dOutToHeadDis, dOutToTailDis)) {
        return false;
    }
    if (dOutToTailDis > dBroadDis) {
        vctliOutFutureLinkId.push_back(liLinkId);
        vctliOutFutureLaneId.push_back(liLaneId);
        vctdOutRealLength.push_back(dOutToTailDis);
        return true;
    }

    HdLinkId liCurLinkId = liLinkId;
    HdLaneId liCurLaneId = liLaneId;
    double dRealLength = dOutToTailDis;
    vctliOutFutureLinkId.push_back(liCurLinkId);
    vctliOutFutureLaneId.push_back(liCurLaneId);
    vctdOutRealLength.push_back(dRealLength);

    double dCurLaneEndAngle = GetLaneAngle(liLinkId, liLaneId, false);

    std::vector<HdLinkId> vctliNextLinkIds = {};
    while (true == m_spHdLib->GetLinkNext(liCurLinkId, vctliNextLinkIds)) {
        HdLinkId liNxtLinkId = {};
        HdLaneId liNxtLaneId = {};
        if (false == GetNextLinkIdLaneId(liCurLinkId, liCurLaneId, dCurLaneEndAngle, nInOutLanePrior, liNxtLinkId, liNxtLaneId)) {
            break;
        }
        if (liNxtLaneId == 0) {
            // 找到了车道终点
            break;
        }
        dCurLaneEndAngle = GetLaneAngle(liCurLinkId, liCurLaneId, false);
        liCurLinkId = liNxtLinkId;
        liCurLaneId = liNxtLaneId;

        double dLength = 0;
        m_spHdLib->GetLinkLength(liCurLinkId, dLength);
        dRealLength += dLength;
        vctliOutFutureLinkId.push_back(liCurLinkId);
        vctliOutFutureLaneId.push_back(liNxtLaneId);
        vctdOutRealLength.push_back(dRealLength);
        if (dRealLength > dBroadDis) {
            break;
        }
    };

    return true;
}

void MapInterface::MakeRoadLink(double dLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, LanePriority nLanePrior,
                                std::vector<RoadLink>& vctrlOutRoadLink) {
    RoadLink rlCurLink = {};

    auto func_ConvertLaneArrow = [](const std::string strArrow, uint8_t& nOutTurn) {
        if (strArrow.size() == 0) {
            nOutTurn = nOutTurn | 0x01;
        } else {
            if (strArrow.find(sc_cArrow_LeftUTurn) != strArrow.npos) {
                nOutTurn = nOutTurn | 0x10;
            }
            if (strArrow.find(sc_cArrow_LeftTurn) != strArrow.npos) {
                nOutTurn = nOutTurn | 0x40;
            }
            if (strArrow.find(sc_cArrow_Straight) != strArrow.npos) {
                nOutTurn = nOutTurn | 0x01;
            }
            if (strArrow.find(sc_cArrow_RightTurn) != strArrow.npos) {
                nOutTurn = nOutTurn | 0x04;
            }
            if (strArrow.find(sc_cArrow_RightUTurn) != strArrow.npos) {
                nOutTurn = nOutTurn | 0x10;
            }
            if (strArrow.find(sc_cArrow_LeftConf) != strArrow.npos) {
                nOutTurn = nOutTurn | 0x80;
            }
            if (strArrow.find(sc_cArrow_RightConf) != strArrow.npos) {
                nOutTurn = nOutTurn | 0x02;
            }
        }
    };

    auto func_MakeLaneEdge = [this](const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, int nEdgeIndex, int nMaxPoints, LaneEdge& leOutCurEdge) {
        if (nEdgeIndex < 0) {
            return;
        }
        const HdEdge& heEdge = cphlLink->vctheEdge.at(nEdgeIndex);
        Point ptEdgeStartGeo = {};
        int nEdgeStartIndex = 0;
        Point ptEdgeEndGeo = {};
        int nEdgeEndIndex = 0;

        // 第一个Link开始
        if (0 != ptStartGeoFoot.dLon) {
            GetPosInGeoLink(ptStartGeoFoot, heEdge.vctvdGeoLink, true, ptEdgeStartGeo, nEdgeStartIndex);
        }

        // 最后一个Link结束
        if (0 != ptEndGeoFoot.dLon) {
            GetPosInGeoLink(ptEndGeoFoot, heEdge.vctvdGeoLink, false, ptEdgeEndGeo, nEdgeEndIndex);
        }

        ConvertGeoLink(heEdge.vctvdGeoLink, ptEdgeStartGeo, nEdgeStartIndex, ptEdgeEndGeo, nEdgeEndIndex, leOutCurEdge.edgeGeometry);
        while (leOutCurEdge.edgeGeometry.size() > nMaxPoints) {
            SimplifyPoint(leOutCurEdge.edgeGeometry, nMaxPoints);
        }
        // 第一个Link开始
        if (0 != ptStartGeoFoot.dLon) {
            ConvertPoint3D(ptEdgeStartGeo, leOutCurEdge.laneLineChangeStart);
            ConvertPoint3D(heEdge.enEndNode.vctdGeoNode, leOutCurEdge.laneLineChangeEnd);
        }
        // 最后一个Link结束
        else if (0 != ptEndGeoFoot.dLon) {
            ConvertPoint3D(heEdge.enStartNode.vctdGeoNode, leOutCurEdge.laneLineChangeStart);
            ConvertPoint3D(ptEdgeEndGeo, leOutCurEdge.laneLineChangeEnd);
        }
        // 中间Link
        else {
            ConvertPoint3D(heEdge.enStartNode.vctdGeoNode, leOutCurEdge.laneLineChangeStart);
            ConvertPoint3D(heEdge.enEndNode.vctdGeoNode, leOutCurEdge.laneLineChangeEnd);
        }
        leOutCurEdge.f_LineLongth = cphlLink->dLength;
        ConvertEdgeCross(heEdge, leOutCurEdge.u8_laneEdgeType);
        if (false == heEdge.vcthmMark.empty()) {
            leOutCurEdge.u8_laneColor = heEdge.vcthmMark.at(0).nColor;
        }
        leOutCurEdge.u8_laneEdgeNum = cphlLink->vctheEdge.size();
        leOutCurEdge.u8_laneEdgeIndex = heEdge.nNum;
        leOutCurEdge.u8_laneLeftRoadEdge = nEdgeIndex == 0 ? 1 : 0;
        leOutCurEdge.u8_laneRightRoadEdge = nEdgeIndex == cphlLink->vctheEdge.size() - 1 ? 1 : 0;
        leOutCurEdge.EdgeId = heEdge.lId;
    };

    auto func_GetConnectedEdgeId = [](const HdLink* cphlLink, JfDataId lPreEdgeEndNodeId, int nLaneSeqNum, bool bIsLeftLeft, HdEdgeId& lOutEdgeId) {
        if (-1 == lPreEdgeEndNodeId) {
            return false;
        }
        std::vector<HdEdgeId> vctheConnectEdgeId = {};
        for (const HdEdge& heEdge : cphlLink->vctheEdge) {
            if (heEdge.enStartNode.lId == lPreEdgeEndNodeId) {
                vctheConnectEdgeId.push_back(heEdge.lId);
            }
        }
        if (vctheConnectEdgeId.size() == 0) {
            return false;
        }
        // 只有一个结果
        else if (vctheConnectEdgeId.size() == 1) {
            lOutEdgeId = vctheConnectEdgeId.at(0);
            return true;
        }
        // 有多个结果，选择一个最接近的
        else {
            if (true == bIsLeftLeft && nLaneSeqNum >= 2) {
                for (HdEdgeId lEdgeId : vctheConnectEdgeId) {
                    if (lEdgeId == cphlLink->vcthlLane.at(nLaneSeqNum - 1).lLeftEdgeId) {
                        lOutEdgeId = lEdgeId;
                        return true;
                    }
                }
            }
            if (false == bIsLeftLeft && nLaneSeqNum <= cphlLink->vcthlLane.size() - 1) {
                for (HdEdgeId lEdgeId : vctheConnectEdgeId) {
                    if (lEdgeId == cphlLink->vcthlLane.at(nLaneSeqNum - 1).lRightEdgeId) {
                        lOutEdgeId = lEdgeId;
                        return true;
                    }
                }
            }
        }
        return false;
    };

    auto func_GetEdgeEndNodeId = [](const HdLink* cphlLink, HdEdgeId lEdgeId) {
        for (const HdEdge& heEdge : cphlLink->vctheEdge) {
            if (heEdge.lId == lEdgeId) {
                return heEdge.enEndNode.lId;
            }
        }
    };

    // 先计算该Link的长度（实际使用的长度）
    double dLinkLength = 0.0;
    if (ptStartGeoFoot.dLon != 0) {
        dLinkLength = dLength;
    } else if (ptEndGeoFoot.dLon != 0) {
        dLinkLength = cphlLink->dLength - (dLength - dBroadDis);
    } else {
        dLinkLength = cphlLink->dLength;
    }
    // 根据长度计算分配到该Link上的点的个数
    int nMaxPoints = int(dLinkLength / dBroadDis * sc_nMaxPoints);
    if (nMaxPoints <= 1) {
        nMaxPoints = 2;
    }

    // 记录已经存储的Edge
    std::vector<HdEdgeId> vctlSavedEdgeId = {};

    static JfDataId s_lLeftLeftEdgeEndNodeId = -1;
    static JfDataId s_lRightRightEdgeEndNodeId = -1;
    int nLeftLeftEdgeIndex = -1;
    int nLeftEdgeIndex = -1;
    int nRightEdgeIndex = -1;
    int nRightRightEdgeIndex = -1;
    int nEmtpyEdgeCnt = 0;
    for (int i = 0; i < cphlLink->vcthlLane.size(); ++i) {
        if (cphlLink->vcthlLane.at(i).liId == liLaneId) {
            // 左
            HdEdgeId lLeftEdgeId = cphlLink->vcthlLane.at(i).lLeftEdgeId;
            nLeftEdgeIndex = m_spHdLib->GetEdgeIndexById(cphlLink, lLeftEdgeId);
            vctlSavedEdgeId.push_back(lLeftEdgeId);

            // 右
            HdEdgeId lRightEdgeId = cphlLink->vcthlLane.at(i).lRightEdgeId;
            nRightEdgeIndex = m_spHdLib->GetEdgeIndexById(cphlLink, lRightEdgeId);
            vctlSavedEdgeId.push_back(lRightEdgeId);

            // 左左
            // 第一条Link为准
            if (ptStartGeoFoot.dLon != 0) {
                if (i != 0) {
                    HdEdgeId lLeftLeftEdgeId = cphlLink->vcthlLane.at(i - 1).lLeftEdgeId;
                    nLeftLeftEdgeIndex = m_spHdLib->GetEdgeIndexById(cphlLink, lLeftLeftEdgeId);
                    s_lLeftLeftEdgeEndNodeId = func_GetEdgeEndNodeId(cphlLink, lLeftLeftEdgeId);
                    vctlSavedEdgeId.push_back(lLeftLeftEdgeId);
                }
            }
            // 第二条Link以后
            else {
                if (i != 0) {
                    if (-1 != s_lLeftLeftEdgeEndNodeId) {
                        HdEdgeId lLeftLeftEdgeId = 0;
                        if (true == func_GetConnectedEdgeId(cphlLink, s_lLeftLeftEdgeEndNodeId, i, true, lLeftLeftEdgeId) && lLeftLeftEdgeId != lLeftEdgeId) {
                            nLeftLeftEdgeIndex = m_spHdLib->GetEdgeIndexById(cphlLink, lLeftLeftEdgeId);
                            s_lLeftLeftEdgeEndNodeId = func_GetEdgeEndNodeId(cphlLink, lLeftLeftEdgeId);
                            vctlSavedEdgeId.push_back(lLeftLeftEdgeId);
                        } else {
                            s_lLeftLeftEdgeEndNodeId = -1;
                        }
                    }
                }
            }

            // 右右
            // 第一条Link为准
            if (ptStartGeoFoot.dLon != 0) {
                if (i != cphlLink->vcthlLane.size() - 1) {
                    HdEdgeId lRightRightEdgeId = cphlLink->vcthlLane.at(i + 1).lRightEdgeId;
                    nRightRightEdgeIndex = m_spHdLib->GetEdgeIndexById(cphlLink, lRightRightEdgeId);
                    s_lRightRightEdgeEndNodeId = func_GetEdgeEndNodeId(cphlLink, lRightRightEdgeId);
                    vctlSavedEdgeId.push_back(lRightRightEdgeId);
                }
            }
            // 第二条Link以后
            else {
                if (i != cphlLink->vcthlLane.size() - 1) {
                    if (-1 != s_lRightRightEdgeEndNodeId) {
                        HdEdgeId lRightRightEdgeId = 0;
                        if (true == func_GetConnectedEdgeId(cphlLink, s_lRightRightEdgeEndNodeId, i + 2, false, lRightRightEdgeId) && lRightRightEdgeId != lRightEdgeId) {
                            nRightRightEdgeIndex = m_spHdLib->GetEdgeIndexById(cphlLink, lRightRightEdgeId);
                            s_lRightRightEdgeEndNodeId = func_GetEdgeEndNodeId(cphlLink, lRightRightEdgeId);
                            vctlSavedEdgeId.push_back(lRightRightEdgeId);
                        } else {
                            s_lRightRightEdgeEndNodeId = -1;
                        }
                    }
                }
            }
            break;
        }
    }

    int nLaneSeqNum = 0;
    m_spHdLib->GetLaneNum(cphlLink->liId, liLaneId, nLaneSeqNum);

    if (nLeftLeftEdgeIndex == -1) {
        nEmtpyEdgeCnt++;  // 空边界线计数增1
    }
    if (nRightRightEdgeIndex == -1) {
        nEmtpyEdgeCnt++;  // 空边界线计数增1
    }
    // 设置数组大小
    rlCurLink.edges.resize(cphlLink->vctheEdge.size() + nEmtpyEdgeCnt);

    // 开始和结束索引
    int nStartEdgeIndex = 0;
    int nEndEdgeIndex = 0;
    if (nLeftEdgeIndex != -1) {
        func_MakeLaneEdge(cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, nLeftEdgeIndex, nMaxPoints, rlCurLink.edges[nLaneSeqNum - 1]);
        nStartEdgeIndex = nLaneSeqNum - 1;
    }
    if (nLeftLeftEdgeIndex != -1) {
        func_MakeLaneEdge(cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, nLeftLeftEdgeIndex, nMaxPoints, rlCurLink.edges[nLaneSeqNum - 2]);
        nStartEdgeIndex = nLaneSeqNum - 2;
    } else {
        // func_MakeLaneEdge(cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, -1, nMaxPoints, rlCurLink.edges[nLaneSeqNum - 2]);
        nStartEdgeIndex = nLaneSeqNum - 2;
    }
    if (nRightEdgeIndex != -1) {
        func_MakeLaneEdge(cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, nRightEdgeIndex, nMaxPoints, rlCurLink.edges[nLaneSeqNum]);
        nEndEdgeIndex = nLaneSeqNum;
    }
    if (nRightRightEdgeIndex != -1) {
        func_MakeLaneEdge(cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, nRightRightEdgeIndex, nMaxPoints, rlCurLink.edges[nLaneSeqNum + 1]);
        nEndEdgeIndex = nLaneSeqNum + 1;
    } else {
        // func_MakeLaneEdge(cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, -1, nMaxPoints, rlCurLink.edges[nLaneSeqNum + 1]);
        nEndEdgeIndex = nLaneSeqNum + 1;
    }

    // 保存索引与与原来的Edge索引不同
    int nCurSaveIndex = 0;
    for (int i = 0; i < cphlLink->vctheEdge.size(); ++i) {
        // 检查是否是已经保存的车道边缘线
        bool bIsEdgeSaved = false;
        for (int j = 0; j < vctlSavedEdgeId.size(); ++j) {
            if (vctlSavedEdgeId.at(j) == cphlLink->vctheEdge.at(i).lId) {
                bIsEdgeSaved = true;
                break;
            }
        }
        if (bIsEdgeSaved) {
            continue;
        }
        // 保存索引是范围检查
        if (nCurSaveIndex >= nStartEdgeIndex && nCurSaveIndex <= nEndEdgeIndex) {
            nCurSaveIndex = nEndEdgeIndex + 1;
        }
        if (nCurSaveIndex < cphlLink->vctheEdge.size() + nEmtpyEdgeCnt) {
            func_MakeLaneEdge(cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, i, nMaxPoints, rlCurLink.edges[nCurSaveIndex]);
        }
        nCurSaveIndex++;
    }

    for (int i = 0; i < cphlLink->vcthlLane.size(); ++i) {
        Lane lnLane = {};
        const HdLane& hlLane = cphlLink->vcthlLane.at(i);

        Point ptLaneStartGeo = {};
        int nLaneStartIndex = 0;
        Point ptLaneEndGeo = {};
        int nLaneEndIndex = 0;

        // 第一个Link开始
        if (0 != ptStartGeoFoot.dLon) {
            GetPosInGeoLink(ptStartGeoFoot, hlLane.vctvdGeoLink, true, ptLaneStartGeo, nLaneStartIndex);
        }

        // 最后一个Link结束
        if (0 != ptEndGeoFoot.dLon) {
            GetPosInGeoLink(ptEndGeoFoot, hlLane.vctvdGeoLink, false, ptLaneEndGeo, nLaneEndIndex);
        }

        ConvertGeoLink(hlLane.vctvdGeoLink, ptLaneStartGeo, nLaneStartIndex, ptLaneEndGeo, nLaneEndIndex, lnLane.geoLines);
        while (lnLane.geoLines.size() > nMaxPoints) {
            SimplifyPoint(lnLane.geoLines, nMaxPoints);
        }
        lnLane.f_width = float(hlLane.nWidth) / 1000;
        double dCurvature = 0.0;
        m_spHdLib->GetLaneCurvature(cphlLink->liId, hlLane.liId, hlLane.vctvdGeoLink.at(hlLane.vctvdGeoLink.size() / 2), dCurvature);
        lnLane.f_curvature = dCurvature;
        double dPitch = 0.0;
        m_spHdLib->GetLanePitch(cphlLink->liId, hlLane.liId, hlLane.vctvdGeoLink.at(hlLane.vctvdGeoLink.size() / 2), dPitch);
        lnLane.f_slope = dPitch;
        std::string strLaneArrow = "";
        m_spHdLib->GetLaneArrow(cphlLink->liId, hlLane.liId, strLaneArrow);
        func_ConvertLaneArrow(strLaneArrow, lnLane.u8_turnWaiting);
        lnLane.u8_laneIndex = hlLane.nSeqNum;
        lnLane.u8_laneType = hlLane.nType;
        rlCurLink.lanes.push_back(lnLane);
    }

    rlCurLink.u8_driveLaneNumber = (uint8_t)nLaneSeqNum;
    rlCurLink.u8_driveRoadProbability = 100;
    rlCurLink.u8_TotalLaneNum = cphlLink->vcthlLane.size();

    vctrlOutRoadLink.push_back(rlCurLink);

    return;
}

void MapInterface::MakeStopLine(double dPastLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, LanePriority nLanePrior,
                                StopLine& slOutStopLine) {
    for (int i = 0; i < cphlLink->vctslStopLine.size(); ++i) {
        const ObjStopLine& slStopLine = cphlLink->vctslStopLine.at(i);

        double dReportDis = 0;
        if (true == IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, slStopLine.vctvdGeoLink.at(0), dReportDis)) {
            slOutStopLine.bStopLineValidFlag = true;
            slOutStopLine.f_stopLineDis = dReportDis;
            ConvertGeoLink(slStopLine.vctvdGeoLink, Point(), 0, Point(), 0, slOutStopLine.stopLinePoints);
        }
    }

    return;
}

void MapInterface::MakePedestrianCrossing(double dPastLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, LanePriority nLanePrior,
                                          PedestrianCrossing& pcOutPedestrianCrossing) {
    for (int i = 0; i < cphlLink->vctcrCrossing.size(); ++i) {
        const ObjCrossing& crCrossing = cphlLink->vctcrCrossing.at(i);

        double dReportDis = 0;
        if (true == IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, crCrossing.vctvdGeoArea.at(0), dReportDis)) {
            pcOutPedestrianCrossing.bPedestrianCrossingValidFlag = true;
            pcOutPedestrianCrossing.f_pedestrianCrossingDis = dReportDis;
            ConvertGeoLink(crCrossing.vctvdGeoArea, Point(), 0, Point(), 0, pcOutPedestrianCrossing.f_pedestrianCrossingPoints);
        }
    }

    return;
}

void MapInterface::MakeCrossing(double dPastLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, LanePriority nLanePrior,
                                Crossing& crOutCrossing) {
    for (int i = 0; i < cphlLink->vctcaCrossArea.size(); ++i) {
        const RoadCrossArea& caCrossArea = cphlLink->vctcaCrossArea.at(i);

        double dReportDis = 0;
        if (true == IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, caCrossArea.vctvdGeoArea.at(0), dReportDis)) {
            crOutCrossing.bCrossingValidFlag = true;
            crOutCrossing.f_crossingDistance = dReportDis;
            ConvertGeoLink(caCrossArea.vctvdGeoArea, Point(), 0, Point(), 0, crOutCrossing.geoAreaPoints);
        }
    }

    return;
}

void MapInterface::MakeRamp(double dPastLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, LanePriority nLanePrior, Ramp& rmOutRamp) {
    const static int sc_nRamp_Exit = 0x00;   // 出口
    const static int sc_nRamp_Enter = 0x01;  // 入口
    // 当前地图里，所有的匝道都是和他并行的车道做的同一个link，后续如果地图有更新，需要检查是否是同样的情况，如果拆分了link，则需要分开处理。
    // 由于此处逻辑较为复杂，所以采取了逐步处理的情况，牺牲了效率

    auto func_CheckRamp = [this](const HdLink* cphlCurLink, int& nOutRampType, std::vector<double>& vctdOutRampStart, std::vector<double>& vctdOutRampEnd, std::vector<std::vector<double>>& vctvdOutLeftEdgeGeoLink,
                                 std::vector<std::vector<double>>& vctvdOutRightEdgeGeoLink) {
        // 先检查是否有出口入口车道
        bool bIsEnterExitLane = false;
        int nRampType = 0;
        for (int i = 0; i < cphlCurLink->vcthlLane.size(); i++) {
            const HdLane& hlCurLane = cphlCurLink->vcthlLane.at(i);
            if (HdLane::EnterLane == hlCurLane.nType || HdLane::ExitLane == hlCurLane.nType) {
                bIsEnterExitLane = true;
                nRampType = hlCurLane.nType;
                break;
            }
        }

        // 如果都是普通车道，直接返回
        if (false == bIsEnterExitLane) {
            return false;
        }

        // 向后方寻找出口车道和入口车道的起点link
        bool bIsStartLinkFound = false;
        const HdLink* cphlPreLink = cphlCurLink;
        const HdLink* cphlFirstLink = cphlCurLink;
        do {
            // 后方可能有多个Link相连结，只有包含出口入口车道的才使用
            bool bIsEnterExitLink = false;
            for (HdLinkId liPrePreLinkId : cphlPreLink->vctliPreLinkId) {
                const HdLink* cphlPrePreLink = m_spHdLib->GetLinkData(liPrePreLinkId);
                bool bIsEnterExitLane = false;
                for (int i = 0; i < cphlPrePreLink->vcthlLane.size(); ++i) {
                    const HdLane& hlPrePreLane = cphlPrePreLink->vcthlLane.at(i);
                    if (nRampType == hlPrePreLane.nType) {
                        bIsEnterExitLink = true;
                        break;
                    }
                }

                if (true == bIsEnterExitLink) {
                    // 找到了，更新遍历Link
                    cphlPreLink = cphlPrePreLink;
                    break;
                }
            }
            // 已经遍历完所有，但没有找到
            if (false == bIsEnterExitLink) {
                bIsStartLinkFound = true;
                cphlFirstLink = cphlPreLink;
            }
        } while (false == bIsStartLinkFound);

        // 当前Link不是匝道的起点
        if (cphlFirstLink->liId.lValue != cphlCurLink->liId.lValue) {
            return false;
        }

        // 向前方寻找出口车道和入口车道的终点link
        bool bIsEndLinkFound = false;
        const HdLink* cphlNxtLink = cphlCurLink;
        const HdLink* cphlLastLink = cphlCurLink;
        do {
            // 前方可能有多个Link相连结，只有包含出口入口车道的才使用
            bool bIsEnterExitLink = false;
            for (HdLinkId liNxtNxtLinkId : cphlNxtLink->vctliNxtLinkId) {
                const HdLink* cphlNxtNxtLink = m_spHdLib->GetLinkData(liNxtNxtLinkId);
                bool bIsEnterExitLane = false;
                for (int i = 0; i < cphlNxtNxtLink->vcthlLane.size(); ++i) {
                    const HdLane& hlNxtNxtLane = cphlNxtNxtLink->vcthlLane.at(i);
                    if (nRampType == hlNxtNxtLane.nType) {
                        bIsEnterExitLink = true;
                        break;
                    }
                }

                if (true == bIsEnterExitLink) {
                    // 找到了，更新遍历Link
                    cphlNxtLink = cphlNxtNxtLink;
                    break;
                }
            }
            // 已经遍历完所有，但没有找到
            if (false == bIsEnterExitLink) {
                bIsEndLinkFound = true;
                cphlLastLink = cphlNxtLink;
            }
        } while (false == bIsEndLinkFound);

        // 匝道类型
        for (int i = 0; i < cphlCurLink->vcthlLane.size(); i++) {
            const HdLane& hlCurLane = cphlCurLink->vcthlLane.at(i);
            // 入口
            if (HdLane::EnterLane == hlCurLane.nType) {
                nOutRampType = sc_nRamp_Enter;
                break;
            }
            // 出口
            else if (HdLane::ExitLane == hlCurLane.nType) {
                nOutRampType = sc_nRamp_Exit;
                break;
            }
        }

        // 如果是入口车道，则多输出后方一条Link的数据
        if (nOutRampType == sc_nRamp_Enter) {
            HdLaneId liLeftPreLaneId = {};
            HdLaneId liRightPreLaneId = {};
            for (int i = 0; i <= cphlFirstLink->vcthlLane.size() - 1; ++i) {
                const HdLane& hlLane = cphlFirstLink->vcthlLane.at(i);
                if (HdLane::EnterLane == hlLane.nType && false == hlLane.vctliPreLaneId.empty()) {
                    liLeftPreLaneId = hlLane.vctliPreLaneId.at(0);
                    break;
                }
            }
            for (int i = cphlFirstLink->vcthlLane.size() - 1; i >= 0; --i) {
                const HdLane& hlLane = cphlFirstLink->vcthlLane.at(i);
                if (HdLane::EnterLane == hlLane.nType && false == hlLane.vctliPreLaneId.empty()) {
                    liRightPreLaneId = hlLane.vctliPreLaneId.at(hlLane.vctliPreLaneId.size() - 1);
                    break;
                }
            }

            for (HdLinkId liFirstPreLink : cphlFirstLink->vctliPreLinkId) {
                const HdLink* cphlFirstPreLink = m_spHdLib->GetLinkData(liFirstPreLink);
                if (nullptr == cphlFirstPreLink) {
                    continue;
                }
                HdEdgeId lLeftEdgeId = 0;
                HdEdgeId lRightEdgeId = 0;
                for (const HdLane& hlPreLane : cphlFirstPreLink->vcthlLane) {
                    if (hlPreLane.liId == liLeftPreLaneId) {
                        lLeftEdgeId = hlPreLane.lLeftEdgeId;
                    }
                    if (hlPreLane.liId == liRightPreLaneId) {
                        lRightEdgeId = hlPreLane.lRightEdgeId;
                    }
                }
                // 根据ID找到对应的GeoLink
                for (int i = 0; i < cphlFirstPreLink->vctheEdge.size(); ++i) {
                    if (cphlFirstPreLink->vctheEdge[i].lId == lLeftEdgeId) {
                        vctvdOutLeftEdgeGeoLink.insert(vctvdOutLeftEdgeGeoLink.end(), cphlFirstPreLink->vctheEdge[i].vctvdGeoLink.begin(), cphlFirstPreLink->vctheEdge[i].vctvdGeoLink.end());
                    }
                    if (cphlFirstPreLink->vctheEdge[i].lId == lRightEdgeId) {
                        vctvdOutRightEdgeGeoLink.insert(vctvdOutRightEdgeGeoLink.end(), cphlFirstPreLink->vctheEdge[i].vctvdGeoLink.begin(), cphlFirstPreLink->vctheEdge[i].vctvdGeoLink.end());
                    }
                }
            }
        }

        bool bIsCheckFinished = false;
        const HdLink* cphlLink = cphlFirstLink;
        do {
            // 是否当前是最后一个Link
            if (cphlLink->liId == cphlLastLink->liId) {
                bIsCheckFinished = true;
            }

            // 遍历一边当前link，找到最左和最右的EdgeId
            HdEdgeId lLeftEdgeId = 0;
            HdEdgeId lRightEdgeId = 0;
            for (int i = 0; i < cphlLink->vcthlLane.size(); ++i) {
                const HdLane& hlLane = cphlLink->vcthlLane.at(i);
                if (nRampType == hlLane.nType) {
                    if (lLeftEdgeId == 0) {
                        lLeftEdgeId = hlLane.lLeftEdgeId;
                    }
                    lRightEdgeId = hlLane.lRightEdgeId;
                }
            }

            // 根据ID找到对应的GeoLink
            for (int i = 0; i < cphlLink->vctheEdge.size(); ++i) {
                if (cphlLink->vctheEdge[i].lId == lLeftEdgeId) {
                    if (cphlLink->liId == cphlFirstLink->liId) {
                        vctdOutRampStart = cphlLink->vctheEdge[i].vctvdGeoLink.at(0);
                    }
                    if (cphlLink->liId == cphlLastLink->liId) {
                        vctdOutRampEnd = cphlLink->vctheEdge[i].vctvdGeoLink.at(cphlLink->vctheEdge[i].vctvdGeoLink.size() - 1);
                    }
                    vctvdOutLeftEdgeGeoLink.insert(vctvdOutLeftEdgeGeoLink.end(), cphlLink->vctheEdge[i].vctvdGeoLink.begin(), cphlLink->vctheEdge[i].vctvdGeoLink.end());
                }
                if (cphlLink->vctheEdge[i].lId == lRightEdgeId) {
                    vctvdOutRightEdgeGeoLink.insert(vctvdOutRightEdgeGeoLink.end(), cphlLink->vctheEdge[i].vctvdGeoLink.begin(), cphlLink->vctheEdge[i].vctvdGeoLink.end());
                }
            }

            // 更新当前遍历Link
            for (HdLinkId liNxtLinkId : cphlLink->vctliNxtLinkId) {
                const HdLink* cphlNxtLink = m_spHdLib->GetLinkData(liNxtLinkId);
                for (int i = 0; i < cphlNxtLink->vcthlLane.size(); i++) {
                    const HdLane& hlNxtLane = cphlNxtLink->vcthlLane.at(i);
                    if (nRampType == hlNxtLane.nType) {
                        cphlLink = cphlNxtLink;
                        break;
                    }
                }
            }
        } while (false == bIsCheckFinished);

        // 如果是出口车道，则多输出前方一条Link的数据
        if (nOutRampType == sc_nRamp_Exit) {
            HdLaneId liLeftNxtLaneId = {};
            HdLaneId liRightNxtLaneId = {};
            for (int i = 0; i <= cphlLastLink->vcthlLane.size() - 1; ++i) {
                const HdLane& hlLane = cphlLastLink->vcthlLane.at(i);
                if (HdLane::ExitLane == hlLane.nType) {
                    liLeftNxtLaneId = hlLane.vctliNxtLaneId.at(0);
                    break;
                }
            }
            for (int i = cphlLastLink->vcthlLane.size() - 1; i >= 0; --i) {
                const HdLane& hlLane = cphlLastLink->vcthlLane.at(i);
                if (HdLane::ExitLane == hlLane.nType) {
                    liRightNxtLaneId = hlLane.vctliNxtLaneId.at(hlLane.vctliNxtLaneId.size() - 1);
                    break;
                }
            }

            for (HdLinkId liLastNxtLink : cphlLastLink->vctliNxtLinkId) {
                const HdLink* cphlLastNxtLink = m_spHdLib->GetLinkData(liLastNxtLink);
                if (nullptr == cphlLastNxtLink) {
                    continue;
                }
                HdEdgeId lLeftEdgeId = 0;
                HdEdgeId lRightEdgeId = 0;
                for (const HdLane& hlNxtLane : cphlLastNxtLink->vcthlLane) {
                    if (hlNxtLane.liId == liLeftNxtLaneId) {
                        lLeftEdgeId = hlNxtLane.lLeftEdgeId;
                    }
                    if (hlNxtLane.liId == liRightNxtLaneId) {
                        lRightEdgeId = hlNxtLane.lRightEdgeId;
                    }
                }
                // 根据ID找到对应的GeoLink
                for (int i = 0; i < cphlLastNxtLink->vctheEdge.size(); ++i) {
                    if (cphlLastNxtLink->vctheEdge[i].lId == lLeftEdgeId) {
                        vctvdOutLeftEdgeGeoLink.insert(vctvdOutLeftEdgeGeoLink.end(), cphlLastNxtLink->vctheEdge[i].vctvdGeoLink.begin(), cphlLastNxtLink->vctheEdge[i].vctvdGeoLink.end());
                    }
                    if (cphlLastNxtLink->vctheEdge[i].lId == lRightEdgeId) {
                        vctvdOutRightEdgeGeoLink.insert(vctvdOutRightEdgeGeoLink.end(), cphlLastNxtLink->vctheEdge[i].vctvdGeoLink.begin(), cphlLastNxtLink->vctheEdge[i].vctvdGeoLink.end());
                    }
                }
            }
        }

        return true;
    };

    auto func_MakeLaneEdge = [this](const HdLink* cphlLink, bool bIsLeft, const std::vector<std::vector<double>> vctvdEdgeGeoLink, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex,
                                    std::vector<LaneEdge>& vctleOutLaneEdge) {
        LaneEdge leCurEdge = {};

        Point ptEdgeStartGeo = {};
        int nEdgeStartIndex = 0;
        Point ptEdgeEndGeo = {};
        int nEdgeEndIndex = 0;

        // // 第一个Link开始
        // if (0 != ptStartGeoFoot.dLon) {
        //     GetPosInGeoLink(ptStartGeoFoot, vctvdEdgeGeoLink, true, ptEdgeStartGeo, nEdgeStartIndex);
        // }

        // // 最后一个Link结束
        // if (0 != ptEndGeoFoot.dLon) {
        //     GetPosInGeoLink(ptEndGeoFoot, vctvdEdgeGeoLink, false, ptEdgeEndGeo, nEdgeEndIndex);
        // }

        ConvertGeoLink(vctvdEdgeGeoLink, ptEdgeStartGeo, nEdgeStartIndex, ptEdgeEndGeo, nEdgeEndIndex, leCurEdge.edgeGeometry);
        ConvertPoint3D(vctvdEdgeGeoLink.at(0), leCurEdge.laneLineChangeStart);
        ConvertPoint3D(vctvdEdgeGeoLink.at(vctvdEdgeGeoLink.size() - 1), leCurEdge.laneLineChangeEnd);
        for (int i = 0; i < vctvdEdgeGeoLink.size() - 1; ++i) {
            leCurEdge.f_LineLongth += PointsDis(vctvdEdgeGeoLink.at(i), vctvdEdgeGeoLink.at(i + 1));
        };
        // 先找到最左和最右的车道
        HdEdgeId lLeftEdgeId = 0;
        HdEdgeId lRightEdgeId = 0;
        for (int i = 0; i < cphlLink->vcthlLane.size(); ++i) {
            const HdLane& hlLane = cphlLink->vcthlLane.at(i);
            if (HdLane::EnterLane == hlLane.nType || HdLane::ExitLane == hlLane.nType) {
                if (lLeftEdgeId == 0) {
                    lLeftEdgeId = hlLane.lLeftEdgeId;
                }
                lRightEdgeId = hlLane.lRightEdgeId;
            }
        }
        // 逐个遍历
        for (int i = 0; i < cphlLink->vctheEdge.size(); ++i) {
            const HdEdge& heEdge = cphlLink->vctheEdge.at(i);
            if ((bIsLeft == true && heEdge.lId == lLeftEdgeId) || (bIsLeft == false && heEdge.lId == lRightEdgeId)) {
                ConvertEdgeCross(heEdge, leCurEdge.u8_laneEdgeType);
                if (false == heEdge.vcthmMark.empty()) {
                    leCurEdge.u8_laneColor = heEdge.vcthmMark.at(0).nColor;
                }
                leCurEdge.u8_laneEdgeIndex = heEdge.nNum;
                leCurEdge.u8_laneLeftRoadEdge = i == 0 ? 1 : 0;
                leCurEdge.u8_laneRightRoadEdge = i == cphlLink->vctheEdge.size() - 1 ? 1 : 0;
                leCurEdge.EdgeId = heEdge.lId;
            }
        }
        leCurEdge.u8_laneEdgeNum = 2;
        vctleOutLaneEdge.push_back(leCurEdge);
    };

    Point ptRamp = {};
    double dStartReportDis = 0.0;
    double dEndReportDis = 0.0;
    int nRampType = 0;
    std::vector<double> vctdStartPoint = {};
    std::vector<double> vctdEndPoint = {};
    std::vector<std::vector<double>> vctvdLeftGeoLink = {};
    std::vector<std::vector<double>> vctvdRightGeoLink = {};
    if (true == func_CheckRamp(cphlLink, nRampType, vctdStartPoint, vctdEndPoint, vctvdLeftGeoLink, vctvdRightGeoLink)) {
        // 开始点满足播报要求时，都播报
        if (true == IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, vctdStartPoint, dStartReportDis)) {
            rmOutRamp.bRampValidFlag = true;
            rmOutRamp.u8_rampType = nRampType;
            rmOutRamp.f_rampStartPointDis = dStartReportDis;
            ConvertPoint3D(vctdStartPoint, rmOutRamp.rampStartPoint);
            IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, vctvdLeftGeoLink.at(vctvdLeftGeoLink.size() - 1), dEndReportDis);
            rmOutRamp.f_rampEndPointDis = dEndReportDis;
            ConvertPoint3D(vctvdLeftGeoLink.at(vctvdLeftGeoLink.size() - 1), rmOutRamp.rampEndPoint);
            func_MakeLaneEdge(cphlLink, true, vctvdLeftGeoLink, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, rmOutRamp.rampEdges);
            func_MakeLaneEdge(cphlLink, false, vctvdRightGeoLink, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, rmOutRamp.rampEdges);
            // while (rmOutRamp.rampEdges[0].edgeGeometry.size() > sc_nMaxPoints) {
            //     SimplifyPoint(rmOutRamp.rampEdges[0].edgeGeometry, sc_nMaxPoints);
            // }
            // while (rmOutRamp.rampEdges[1].edgeGeometry.size() > sc_nMaxPoints) {
            //     SimplifyPoint(rmOutRamp.rampEdges[1].edgeGeometry, sc_nMaxPoints);
            // }
            // int size1 = rmOutRamp.rampEdges[0].edgeGeometry.size();
            // int size2 = rmOutRamp.rampEdges[1].edgeGeometry.size();
            // if (size1 - size2 > 3) {
            //     while (size1 - size2 > 3) {
            //         SimplifyPoint(rmOutRamp.rampEdges[0].edgeGeometry, size2);
            //         size1 = rmOutRamp.rampEdges[0].edgeGeometry.size();
            //     }
            //     return;
            // }
            // while (size1 - size2 < -3) {
            //     SimplifyPoint(rmOutRamp.rampEdges[1].edgeGeometry, size1);
            //     size2 = rmOutRamp.rampEdges[1].edgeGeometry.size();
            // }
        }
    }

    return;
}

void MapInterface::MakeChange(double dPastLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, LanePriority nLanePrior,
                              Change& cgOutChange) {
    auto func_GetChange = [this](const HdLink* cphlCurLink, const HdLaneId& liCurLaneId, LanePriority nLanePrior, int& nOutChangeType, Point& ptOutChangePoint) {
        bool bLaneCntChange = false;
        // const HdLink* cphlNxtLink = cphlCurLink;
        int nCurLaneCnt = cphlCurLink->vcthlLane.size();

        double dCurLaneEndAngle = GetLaneAngle(cphlCurLink->liId, liCurLaneId, false);
        HdLinkId hlNxtLinkId = {};
        HdLaneId hlNxtLaneId = {};
        if (false == GetNextLinkIdLaneId(cphlCurLink->liId, liCurLaneId, dCurLaneEndAngle, nLanePrior, hlNxtLinkId, hlNxtLaneId)) {
            return false;
        }

        const HdLink* cphlNxtLink = m_spHdLib->GetLinkData(hlNxtLinkId);
        if (nullptr == cphlNxtLink) {
            return false;
        }
        if (nCurLaneCnt == cphlNxtLink->vcthlLane.size()) {
            nOutChangeType = 0x00;  // 不变
        } else {
            if (nCurLaneCnt > cphlNxtLink->vcthlLane.size()) {
                for (const HdLane& hlNxtNxtLane : cphlNxtLink->vcthlLane) {
                    if (hlNxtNxtLane.vctliPreLaneId.size() != 1) {
                        ptOutChangePoint = hlNxtNxtLane.vctvdGeoLink.at(0);
                        break;
                    }
                }
                nOutChangeType = 0x01;  // 减少
            } else {
                for (const HdLane& hlNxtLane : cphlNxtLink->vcthlLane) {
                    if (hlNxtLane.vctliNxtLaneId.size() != 1) {
                        // ptOutChangePoint = *hlNxtLane.vctvdGeoLink.end();
                        ptOutChangePoint = hlNxtLane.vctvdGeoLink.at(hlNxtLane.vctvdGeoLink.size() - 1);
                        break;
                    }
                }
                nOutChangeType = 0x02;  // 增加
            }
            bLaneCntChange = true;
        }

        return bLaneCntChange;
    };

    int nChangeType = 0;
    Point ptChangePoint = {};
    if (true == func_GetChange(cphlLink, liLaneId, nLanePrior, nChangeType, ptChangePoint)) {
        double dReportDis = 0.0;
        if (true == IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, ptChangePoint, dReportDis)) {
            cgOutChange.bChangeValidFlag = true;
            cgOutChange.i32_changeType = nChangeType;
            cgOutChange.f_changeDistance = dReportDis;
            ConvertPoint3D(std::vector<double>{ptChangePoint.dLon, ptChangePoint.dLat, ptChangePoint.dAlt}, cgOutChange.changePoint);
        }
    }

    return;
}

void MapInterface::MakeSpeed(double dPastLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, LanePriority nLanePrior, Speed& spOutSpeed) {
    auto func_GetSpeedChange = [this](const HdLink* cphlCurLink, const HdLaneId& liCurLaneId, LanePriority nLanePrior, int& nOutCurSpeedMax, int& nOutCurSpeedMin, int& nOutNewSpeedMax, int& nOutNewSpeedMin, Point& ptOutSpeedPoint) {
        double dSpeedKeepLength = 0;
        bool bSpeedChange = false;
        double dCurLaneEndAngle = GetLaneAngle(cphlCurLink->liId, liCurLaneId, false);
        for (const HdLane& hlCurLane : cphlCurLink->vcthlLane) {
            if (hlCurLane.liId == liCurLaneId) {
                nOutCurSpeedMin = hlCurLane.nMinSpeed;
                nOutCurSpeedMax = hlCurLane.nMaxSpeed;
                break;
            }
        }

        HdLinkId hlNxtLinkId = {};
        HdLaneId hlNxtLaneId = {};
        if (false == GetNextLinkIdLaneId(cphlCurLink->liId, liCurLaneId, dCurLaneEndAngle, nLanePrior, hlNxtLinkId, hlNxtLaneId)) {
            return false;
        }

        const HdLink* cphlNxtLink = m_spHdLib->GetLinkData(hlNxtLinkId);
        if (nullptr == cphlNxtLink) {
            return false;
        }
        for (const HdLane& hlNxtLane : cphlNxtLink->vcthlLane) {
            if (hlNxtLane.liId == hlNxtLaneId) {
                if (nOutCurSpeedMax != hlNxtLane.nMaxSpeed || nOutCurSpeedMin != hlNxtLane.nMinSpeed) {
                    nOutNewSpeedMax = hlNxtLane.nMaxSpeed;
                    nOutNewSpeedMin = hlNxtLane.nMinSpeed;
                    ptOutSpeedPoint = hlNxtLane.vctvdGeoLink.at(0);
                    bSpeedChange = true;
                }
                break;
            }
        }

        return bSpeedChange;
    };

    int nCurSpeedMin = 0;
    int nCurSpeedMax = 0;
    int nNewSpeedMin = 0;
    int nNewSpeedMax = 0;
    Point ptChangePoint = {};
    if (true == func_GetSpeedChange(cphlLink, liLaneId, nLanePrior, nCurSpeedMax, nCurSpeedMin, nNewSpeedMax, nNewSpeedMin, ptChangePoint)) {
        double dReportDis = 0.0;
        if (true == IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, ptChangePoint, dReportDis)) {
            spOutSpeed.bSpeedLimitValidFlag = true;
            spOutSpeed.u16_CurrentRoadSDLimit_Max = nCurSpeedMax;
            spOutSpeed.u16_CurrentRoadSDLimit_Min = nCurSpeedMin;
            spOutSpeed.u16_NextRoadSDLimit_Max = nNewSpeedMax;
            spOutSpeed.u6_NextRoadSDLimit_Min = nNewSpeedMin;
            spOutSpeed.f_distanceToNextSpeedLimit = dReportDis;
        }
    }
}

void MapInterface::MakeTrafficLight(double dPastLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, LanePriority nLanePrior,
                                    TrafficLight& tlOutTrafficLight) {
    // 检查作用在自己Link上的红绿灯
    for (int i = 0; i < cphlLink->vcthlLane.size(); ++i) {
        const HdLane& hlLane = cphlLink->vcthlLane.at(i);
        // 使用存储在作用车道上的红绿灯信息，所以距离需要加上车道的最末点到
        for (int j = 0; j < hlLane.vcttlTrafficLight.size(); ++j) {
            const ObjTrafficLight& tlTrafficLight = hlLane.vcttlTrafficLight.at(j);

            double dReportDis = 0;
            if (true == IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, tlTrafficLight.vctvdGeoArea.at(0), dReportDis)) {
                tlOutTrafficLight.bTrafficLightValidFlag = true;
                ConvertPoint3D(tlTrafficLight.vctvdGeoArea.at(0), tlOutTrafficLight.trafficLightLoc);
                tlOutTrafficLight.f_distanceToTrafficLight = dReportDis + PointsDis(hlLane.vctvdGeoLink.at(hlLane.vctvdGeoLink.size() - 1), tlTrafficLight.vctvdGeoArea.at(0));
                break;
            }
        }
        if (tlOutTrafficLight.bTrafficLightValidFlag == true) {
            break;
        }
    }

    // 检查自己的Link内有没有红绿灯
    if (tlOutTrafficLight.bTrafficLightValidFlag == false) {
        for (int i = 0; i < cphlLink->vcttlTrafficLight.size(); ++i) {
            const ObjTrafficLight& tlTrafficLight = cphlLink->vcttlTrafficLight.at(i);
            double dReportDis = 0;
            if (true == IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, tlTrafficLight.vctvdGeoArea.at(0), dReportDis)) {
                tlOutTrafficLight.bTrafficLightValidFlag = true;
                ConvertPoint3D(tlTrafficLight.vctvdGeoArea.at(0), tlOutTrafficLight.trafficLightLoc);
                tlOutTrafficLight.f_distanceToTrafficLight = dReportDis;
                break;
            }
        }
    }

    return;
}

void MapInterface::MakeRSU(double dPastLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, LanePriority nLanePrior, RSU& rsOutRSU) {
    for (int i = 0; i < cphlLink->vctRsuPoint.size(); ++i) {
        const ObjPoint& ptPoint = cphlLink->vctRsuPoint.at(i);

        double dReportDis = 0;
        if (true == IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, ptPoint.vctdGeoNode, dReportDis)) {
            rsOutRSU.bRsuValidFlag = true;
            ConvertPoint3D(ptPoint.vctdGeoNode, rsOutRSU.rsuLocation);
            rsOutRSU.f_distanceToRsu = dReportDis;
        }
    }

    return;
}

void MapInterface::MakeTunnel(double dPastLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, LanePriority nLanePrior,
                              Tunnel& tnOutTunnel) {
    const static int sc_nTunnel_None = 0;
    const static int sc_nTunnel_Start = 11;  // 隧道起点
    const static int sc_nTunnel_End = 12;    // 隧道终点
    auto func_CheckTunnel = [this](const HdLink* cphlCurLink, Point& ptOutTunnelStartPoint, Point& ptOutTunnelEndPoint, double& dOutTunnelLength) {
        if (HdLink::Tunnel == cphlCurLink->nType) {
            double dTunnelLength = 0.0;
            HdLinkId liTunnelStartLinkId = cphlCurLink->liId;
            const HdLink* cphlPreLink = cphlCurLink;
            bool bInTunnel = true;
            do {
                bInTunnel = false;
                // 向前检查
                for (const HdLinkId& liPrePreLinkId : cphlPreLink->vctliPreLinkId) {
                    const HdLink* cphlPrePreLink = m_spHdLib->GetLinkData(liPrePreLinkId);
                    if (nullptr == cphlPrePreLink) {
                        continue;
                    }
                    if (cphlPreLink->nType == HdLink::Tunnel) {
                        ptOutTunnelStartPoint = cphlPreLink->jcStartJunc.vctdGeoNode;
                        dTunnelLength += cphlPreLink->dLength;
                        cphlPreLink = cphlPrePreLink;
                        liTunnelStartLinkId = cphlPrePreLink->liId;
                        bInTunnel = true;
                    }
                }
            } while (bInTunnel == true);

            HdLinkId liTunnelEndLinkId = cphlCurLink->liId;
            const HdLink* cphlNxtLink = cphlCurLink;
            do {
                bInTunnel = false;
                // 向后检查
                for (const HdLinkId& liNxtNxtLinkId : cphlNxtLink->vctliNxtLinkId) {
                    const HdLink* cphlNxtNxtLink = m_spHdLib->GetLinkData(liNxtNxtLinkId);
                    if (nullptr == cphlNxtNxtLink) {
                        continue;
                    }
                    if (cphlNxtNxtLink->nType == HdLink::Tunnel) {
                        ptOutTunnelEndPoint = cphlNxtLink->jcStartJunc.vctdGeoNode;
                        cphlNxtLink = cphlNxtNxtLink;
                        dTunnelLength += cphlNxtLink->dLength;  // 注意长度计算的时机不同，当前link的长度在向前检查时被计入了，这里不能重复计算
                        liTunnelEndLinkId = cphlNxtNxtLink->liId;
                        bInTunnel = true;
                    }
                }
            } while (bInTunnel == true);

            dOutTunnelLength = dTunnelLength;

            return true;
        }

        return false;
    };

    Point ptTunnelStartPoint = {};
    Point ptTunnelEndPoint = {};
    double dTunnelLength = 0.0;
    double dReportDis = 0.0;
    if (true == func_CheckTunnel(cphlLink, ptTunnelStartPoint, ptTunnelEndPoint, dTunnelLength)) {
        if (true == IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, ptTunnelStartPoint, dReportDis)) {
            tnOutTunnel.bTunnelValidFlag = true;
            tnOutTunnel.u8_TunnelType = 0x00;
            tnOutTunnel.f_distanceToTunnel = dReportDis;
            tnOutTunnel.f_tunnel_length = dTunnelLength;
            ConvertPoint3D(std::vector<double>{ptTunnelStartPoint.dLon, ptTunnelStartPoint.dLat, ptTunnelStartPoint.dAlt}, tnOutTunnel.startPoint);
            ConvertPoint3D(std::vector<double>{ptTunnelEndPoint.dLon, ptTunnelEndPoint.dLat, ptTunnelEndPoint.dAlt}, tnOutTunnel.endPoint);
        }
        // 不再播报
        // // 已经在隧道中
        // else {
        //     tnOutTunnel.bTunnelValidFlag = true;
        //     tnOutTunnel.u8_TunnelType = 0x00;
        //     tnOutTunnel.f_distanceToTunnel = 0;
        //     tnOutTunnel.f_tunnel_length = dTunnelLength;
        //     ConvertPoint3D(std::vector<double>{ptTunnelEndPoint.dLon, ptTunnelEndPoint.dLat, ptTunnelEndPoint.dAlt}, tnOutTunnel.endPoint);
        // }
    }

    return;
}

void MapInterface::MakeDashSolidChangeDis(double dPastLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, LanePriority nLanePrior,
                                          double& dOutDashSolidChangeDis) {
    auto func_GetDashSolidChgPoint = [this](const HdLink* cphlCurLink, const HdLaneId& liCurLaneId, LanePriority nLanePrior, Point& ptOutChangePoint) {
        if (cphlCurLink->vctliNxtLinkId.empty() == true || cphlCurLink->vctliNxtLinkId.size() != 1) {
            return false;
        }
        const HdLink* cphlNxtLink = m_spHdLib->GetLinkData(cphlCurLink->vctliNxtLinkId.at(0));
        if (nullptr == cphlNxtLink) {
            return false;
        }
        // 遍历当前车道边缘线
        for (const HdEdge& heCurEdge : cphlCurLink->vctheEdge) {
            // 遍历下一link车道边缘线
            for (const HdEdge& heNxtEdge : cphlNxtLink->vctheEdge) {
                // 两组边缘线连接点相同
                if (heCurEdge.enEndNode.lId == heNxtEdge.enStartNode.lId) {
                    // 跨越属性有变化
                    if (heCurEdge.nCross != heNxtEdge.nCross) {
                        ptOutChangePoint = heCurEdge.vctvdGeoLink.at(heCurEdge.vctvdGeoLink.size() - 1);
                        return true;
                    }
                }
            }
        }

        return false;
    };

    Point ptChangePoint = {};
    if (true == func_GetDashSolidChgPoint(cphlLink, liLaneId, nLanePrior, ptChangePoint)) {
        double dReportDis = 0.0;
        if (true == IsBroad(dPastLength, cphlLink, liLaneId, ptStartGeoFoot, nStartGeoIndex, ptEndGeoFoot, nEndGeoIndex, dBroadDis, ptChangePoint, dReportDis)) {
            dOutDashSolidChangeDis = dReportDis;
            return;
        }
    }

    dOutDashSolidChangeDis = -1;
    return;
}

void MapInterface::ConvertPoint3D(const Point& ptFrom, Point3D& ptTo) {
    ptTo.d_longitude = ptFrom.dLon;
    ptTo.d_latitude = ptFrom.dLat;
    ptTo.f_altitude = ptFrom.dAlt;
    return;
}

void MapInterface::ConvertPoint3D(const std::vector<double>& vctdFrom, Point3D& ptTo) {
    if (vctdFrom.size() < 3) {
        return;
    }
    ptTo.d_longitude = vctdFrom[0];
    ptTo.d_latitude = vctdFrom[1];
    // 根据国家相关法律规定，高程不能返回
    ptTo.f_altitude = 0;
    return;
}

void MapInterface::ConvertGeoLink(const std::vector<std::vector<double>>& vctvdFrom, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, std::vector<Point3D>& vctptTo) {
    // 针对原来的点，做插值处理（间隔=0.5米）
    static const double sc_dPointsDis = 2;
    int nCurIdx = 0;
    std::vector<std::vector<double>> vctvdInsertGeo = {};
    for (int i = 0, j = i + 1; j <= vctvdFrom.size() - 1; ++i, ++j) {
        std::vector<Point> vctptInterPoints = jf::InterP(vctvdFrom[i], vctvdFrom[j], sc_dPointsDis);
        std::vector<std::vector<double>> vctvdInterPoints = {};
        m_spHdLib->ConvertGeoLink(vctptInterPoints, vctvdInterPoints);
        vctvdInsertGeo.insert(vctvdInsertGeo.begin() + nCurIdx, vctvdInterPoints.begin(), vctvdInterPoints.end());
        nCurIdx = nCurIdx + vctptInterPoints.size();
    }

    // 插值后，重新计算起始点和最终点
    if (ptStartGeoFoot.dLon != 0) {
        jf::Point ptPoint = {};
        double dDis = 0.0;
        HdLibMath::GetPerpendicularFoot(ptStartGeoFoot, vctvdInsertGeo, ptPoint, dDis, nStartGeoIndex);
    }
    if (ptEndGeoFoot.dLon != 0) {
        jf::Point ptPoint = {};
        double dDis = 0.0;
        HdLibMath::GetPerpendicularFoot(ptEndGeoFoot, vctvdInsertGeo, ptPoint, dDis, nEndGeoIndex);
        nEndGeoIndex = nEndGeoIndex - 1;  // 向前寻找，需要减去一个点，因为垂足的index给的是垂足后面的那个点
    }

    int nFromGeoIndex = ptStartGeoFoot.dLon == 0 ? 0 : nStartGeoIndex;
    int nToGeoIndex = ptEndGeoFoot.dLon == 0 ? vctvdInsertGeo.size() - 1 : nEndGeoIndex;
    if (0 != ptStartGeoFoot.dLon) {
        Point3D ptTo = {};
        ConvertPoint3D(std::vector<double>{ptStartGeoFoot.dLon, ptStartGeoFoot.dLat, ptStartGeoFoot.dAlt}, ptTo);
        vctptTo.push_back(ptTo);
    }
    for (int i = nFromGeoIndex; i <= nToGeoIndex; ++i) {
        Point3D ptTo = {};
        ConvertPoint3D(vctvdInsertGeo[i], ptTo);
        vctptTo.push_back(ptTo);
    }
    if (0 != ptEndGeoFoot.dLon) {
        Point3D ptTo = {};
        ConvertPoint3D(std::vector<double>{ptEndGeoFoot.dLon, ptEndGeoFoot.dLat, ptEndGeoFoot.dAlt}, ptTo);
        vctptTo.push_back(ptTo);
    }
}

void MapInterface::ConvertEdgeCross(const HdEdge& heEdge, uint8_t& nOutCross) {
    switch (heEdge.nType) {
        case HdEdge::CrossValue::Unable:
            if (heEdge.nMarkCount == 1) {
                nOutCross = 1;
            } else {
                nOutCross = 5;
            }
            break;
        case HdEdge::CrossValue::BidirecAble:
            if (heEdge.nMarkCount == 1) {
                nOutCross = 2;
            } else {
                nOutCross = 6;
            }
            break;
        case HdEdge::CrossValue::LeftAble:
            nOutCross = 4;
            break;
        case HdEdge::CrossValue::RightAble:
            nOutCross = 3;
            break;
        default:
            break;
    }
}

bool MapInterface::IsBroad(double dPastLength, const HdLink* cphlLink, const HdLaneId& liLaneId, const Point& ptStartGeoFoot, int nStartGeoIndex, const Point& ptEndGeoFoot, int nEndGeoIndex, double dBroadDis, const Point& ptTargetPoint,
                           double& dOutReportDis) {
    auto func_GetTgtDisInLane = [this](const Point& ptTargetPoint, const HdLink* cphlLink, const HdLaneId& liLaneId) {
        double dTgtToHeadDis = 0.0;
        double dTgtToTailDis = 0.0;
        for (const HdLane& hlLane : cphlLink->vcthlLane) {
            if (hlLane.liId == liLaneId) {
                // 计算播报对象到车道中心线起点的距离
                GetPosInGeoLink(ptTargetPoint, hlLane.vctvdGeoLink, dTgtToHeadDis, dTgtToTailDis);
                break;
            }
        }
        return dTgtToHeadDis;
    };

    // 第一个link
    if (ptStartGeoFoot.dLon != 0 && ptEndGeoFoot.dLon == 0) {
        // 目标点距离Lane开始的距离
        double dToTargetFootDis = func_GetTgtDisInLane(ptTargetPoint, cphlLink, liLaneId);
        // 当前点距离Lane开始的距离
        double dToStartGeoDis = func_GetTgtDisInLane(ptStartGeoFoot, cphlLink, liLaneId);

        // 播报物在开始点的后方
        if (dToStartGeoDis > dToTargetFootDis) {
            return false;
        }
        // 播报物在开始点的前方
        else {
            dOutReportDis = dToTargetFootDis - dToStartGeoDis;
        }
    }

    // 中间Link
    if (ptStartGeoFoot.dLon == 0 && ptEndGeoFoot.dLon == 0) {
        double dToTargetFootDis = func_GetTgtDisInLane(ptTargetPoint, cphlLink, liLaneId);
        // 由于Past Length中已经计入了当前Link的长度，所以需要减去
        dOutReportDis = dPastLength + dToTargetFootDis - cphlLink->dLength;
    }

    // 最末Link
    if (ptStartGeoFoot.dLon == 0 && ptEndGeoFoot.dLon != 0) {
        // 目标点距离Lane开始的距离
        double dToTargetFootDis = func_GetTgtDisInLane(ptTargetPoint, cphlLink, liLaneId);
        // 结束点距离Lane开始的距离
        double dToEndGeoDis = func_GetTgtDisInLane(ptEndGeoFoot, cphlLink, liLaneId);

        // 播报物在结束点的前方
        if (dToTargetFootDis > dToEndGeoDis) {
            return false;
        }
        // 播报物在结束点的后方
        else {
            // 由于Past Length中已经计入了当前Link的长度，所以需要减去
            dOutReportDis = dPastLength + dToTargetFootDis - cphlLink->dLength;
        }
    }

    // 只有一条Link
    if (ptStartGeoFoot.dLon != 0 && ptEndGeoFoot.dLon != 0) {
        // 目标点距离Lane开始的距离
        double dToTargetFootDis = func_GetTgtDisInLane(ptTargetPoint, cphlLink, liLaneId);
        // 当前点距离Lane开始的距离
        double dToStartGeoDis = func_GetTgtDisInLane(ptStartGeoFoot, cphlLink, liLaneId);
        // 结束点距离Lane开始的距离
        double dToEndGeoDis = func_GetTgtDisInLane(ptEndGeoFoot, cphlLink, liLaneId);

        // 播报物在开始点的后方,或者结束点的前方
        if (dToStartGeoDis > dToTargetFootDis || dToEndGeoDis < dToTargetFootDis) {
            return false;
        }
        // 播报物在开始点和结束点的中间
        else {
            dOutReportDis = dToTargetFootDis - dToStartGeoDis;
        }
    }

    if (dOutReportDis < dBroadDis) {
        return true;
    }

    return false;
}

void MapInterface::GetPosInGeoLink(const Point& ptPos, const std::vector<std::vector<double>>& vctvdGeoLink, bool bIsChkFuture, Point& ptOutFoot, int& nOutNearestIndex) {
    double dToHeadDis = 0.0;
    double dToTailDis = 0.0;
    return GetPosInGeoLink(ptPos, vctvdGeoLink, bIsChkFuture, ptOutFoot, nOutNearestIndex, dToHeadDis, dToTailDis);
}

void MapInterface::GetPosInGeoLink(const Point& ptPos, const std::vector<std::vector<double>>& vctvdGeoLink, double& dOutToHeadDis, double& dOutToTailDis) {
    Point ptFoot = {};
    int nNearestIndex = 0;
    return GetPosInGeoLink(ptPos, vctvdGeoLink, true, ptFoot, nNearestIndex, dOutToHeadDis, dOutToTailDis);
}

void MapInterface::GetPosInGeoLink(const Point& ptPos, const std::vector<std::vector<double>>& vctvdGeoLink, bool bIsChkFuture, Point& ptOutFoot, int& nOutNearestIndex, double& dOutToHeadDis, double& dOutToTailDis) {
    double dGeoDistance = -1;
    int nNxtIndex = 0;
    HdLibMath::GetPerpendicularFoot(ptPos, vctvdGeoLink, ptOutFoot, dGeoDistance, nNxtIndex);
    m_spHdLib->GetPosInGeoLink(ptPos, vctvdGeoLink, dOutToHeadDis, dOutToTailDis);
    if (true == bIsChkFuture) {
        // 向前方检查，所给点到车道的垂足点下一点
        nOutNearestIndex = nNxtIndex;
    } else {
        // 向后方检查，所给点到车道的垂足点上一点, 该值至少为1
        nOutNearestIndex = nNxtIndex - 1;
    }

    return;
}

void MapInterface::GetLenPointAndIndex(double dLength, const std::vector<std::vector<double>>& vctvdGeoLink, bool bIsChkFuture, Point& ptOutLenPoint, int& nOutNearestIndex) {
    double dGeoDistance = -1;
    int nGeoIndex = 0;
    Point ptNearestPos = {};
    Point ptFootPos = {};
    if (true == bIsChkFuture) {
        double dCurLength = 0;
        int nIndex = 0;
        for (; nIndex < vctvdGeoLink.size() - 1; ++nIndex) {
            dCurLength += PointsDis(Point(vctvdGeoLink.at(nIndex)), Point(vctvdGeoLink.at(nIndex + 1)));
            if (dCurLength >= dLength) {
                break;
            }
        }
        ptOutLenPoint = vctvdGeoLink.at(nIndex);
        nOutNearestIndex = nIndex;
    } else {
        double dCurLength = 0;
        int nIndex = vctvdGeoLink.size() - 1;
        for (; nIndex > 0; --nIndex) {
            dCurLength += PointsDis(Point(vctvdGeoLink.at(nIndex)), Point(vctvdGeoLink.at(nIndex - 1)));
            if (dCurLength >= dLength) {
                break;
            }
        }
        ptOutLenPoint = vctvdGeoLink.at(nIndex - 1);
        nOutNearestIndex = nIndex - 1;
    }
}

void MapInterface::GetPosInHdLane(const Point& ptPos, const std::vector<std::vector<double>>& vctvdGeoLink, double& dOutToHeadDis, double& dOutToTailDis) {}

double MapInterface::GetLaneAngle(const HdLinkId& liLinkId, const HdLaneId& liLaneId, bool bIsStart) {
    const HdLink* cphlLink = m_spHdLib->GetLinkData(liLinkId);
    if (cphlLink != nullptr) {
        for (const HdLane& hlLane : cphlLink->vcthlLane) {
            if (hlLane.liId == liLaneId) {
                double dAngle = 0;
                if (true == bIsStart) {
                    dAngle = PointsAngle(hlLane.vctvdGeoLink.at(0), hlLane.vctvdGeoLink.at(1));
                } else {
                    dAngle = PointsAngle(hlLane.vctvdGeoLink.at(hlLane.vctvdGeoLink.size() - 2), hlLane.vctvdGeoLink.at(hlLane.vctvdGeoLink.size() - 1));
                }
                return dAngle;
            }
        }
    }

    return 0.0;
}

bool MapInterface::GetNextLinkIdLaneId(const HdLinkId& chlCurLinkId, const HdLaneId& chlCurLaneId, double dLastAngle, LanePriority& nInOutLanePrior, HdLinkId& liOutNxtLinkId, HdLaneId& liOutNxtLaneId) {
    auto func_GetLaneType = [this](const HdLinkId& liLinkId, const HdLaneId& liLaneId) {
        const HdLink* cphlLink = m_spHdLib->GetLinkData(liLinkId);
        for (const HdLane& hlLane : cphlLink->vcthlLane) {
            if (hlLane.liId == liLaneId) {
                return hlLane.nType;
            }
        }
    };

    const HdLink* cphlCurLink = m_spHdLib->GetLinkData(chlCurLinkId);
    if (cphlCurLink != nullptr) {
        // 于当前Lane相连接的下一组Lane
        std::vector<HdLaneId> vctliConnectedLaneId = {};
        for (const HdLane& chlCurLane : cphlCurLink->vcthlLane) {
            if (chlCurLane.liId == chlCurLaneId) {
                vctliConnectedLaneId = chlCurLane.vctliNxtLaneId;
                break;
            }
        }

        std::vector<HdLinkId> vctliNxtLinkIds = {};
        // 相连接的下一组link
        if (true == m_spHdLib->GetLinkNext(chlCurLinkId, vctliNxtLinkIds)) {
            std::map<HdLaneId, HdLinkId> hasTrunArrowsMap;
            std::map<HdLaneId, HdLinkId> noTurnArrowsMap;
            for (const HdLinkId& cliNxtLinkId : vctliNxtLinkIds) {
                const HdLink* cphlNxtLink = m_spHdLib->GetLinkData(cliNxtLinkId);
                if (nullptr != cphlNxtLink) {
                    // 下一组Link中的每一个车道是否与当前车道相连
                    for (const HdLane& chlNxtLane : cphlNxtLink->vcthlLane) {
                        for (const HdLaneId& cliConnectedLaneId : vctliConnectedLaneId) {
                            // 相连
                            if (cliConnectedLaneId == chlNxtLane.liId) {
                                std::string strNxtLaneArrows = "";
                                m_spHdLib->GetLaneArrow(cphlNxtLink->liId, chlNxtLane.liId, strNxtLaneArrows);
                                switch (nInOutLanePrior) {
                                    case LanePriority::LeftFirst:
                                        // 优先左转
                                        if ((chlNxtLane.nType != HdLane::UTurnLane) && (strNxtLaneArrows.npos == strNxtLaneArrows.find(sc_cArrow_LeftUTurn)) &&
                                            ((strNxtLaneArrows.npos != strNxtLaneArrows.find(sc_cArrow_LeftTurn)) || (strNxtLaneArrows.npos != strNxtLaneArrows.find(sc_cArrow_LeftConf)))) {
                                            liOutNxtLinkId = cphlNxtLink->liId;
                                            liOutNxtLaneId = chlNxtLane.liId;
                                            nInOutLanePrior = LanePriority::Straight;
                                            return true;
                                        }
                                        // 只能直行
                                        else {
                                            noTurnArrowsMap[chlNxtLane.liId] = cphlNxtLink->liId;
                                        }
                                        break;
                                    case LanePriority::RightFirst:
                                        // 优先右转
                                        if ((chlNxtLane.nType != HdLane::UTurnLane) && (strNxtLaneArrows.npos == strNxtLaneArrows.find(sc_cArrow_RightUTurn)) &&
                                            ((strNxtLaneArrows.npos != strNxtLaneArrows.find(sc_cArrow_RightTurn)) || (strNxtLaneArrows.npos != strNxtLaneArrows.find(sc_cArrow_RightConf)))) {
                                            liOutNxtLinkId = cphlNxtLink->liId;
                                            liOutNxtLaneId = chlNxtLane.liId;
                                            nInOutLanePrior = LanePriority::Straight;
                                            return true;
                                        }
                                        // 只能直行
                                        else {
                                            noTurnArrowsMap[chlNxtLane.liId] = cphlNxtLink->liId;
                                        }
                                        break;
                                    case LanePriority::Straight:
                                    default:
                                        // 对于直行优先的情况，过滤掉存在左右转的情况
                                        if ((strNxtLaneArrows.npos == strNxtLaneArrows.find(sc_cArrow_LeftUTurn)) && (strNxtLaneArrows.npos == strNxtLaneArrows.find(sc_cArrow_LeftTurn)) &&
                                            (strNxtLaneArrows.npos == strNxtLaneArrows.find(sc_cArrow_LeftConf)) && (strNxtLaneArrows.npos == strNxtLaneArrows.find(sc_cArrow_RightUTurn)) &&
                                            (strNxtLaneArrows.npos == strNxtLaneArrows.find(sc_cArrow_RightTurn)) && (strNxtLaneArrows.npos == strNxtLaneArrows.find(sc_cArrow_RightConf))) {
                                            noTurnArrowsMap[chlNxtLane.liId] = cphlNxtLink->liId;
                                        } else {
                                            hasTrunArrowsMap[chlNxtLane.liId] = cphlNxtLink->liId;
                                        }
                                        break;
                                }
                            }
                        }
                    }
                }
            }

            bool bNeedChangeToStraight = false;
            HdLaneId hdLaneIdTmp;
            HdLinkId hdLinkIdTmp;
            double dMaxAngleSub = std::numeric_limits<double>::min();
            double dMinAngleSub = std::numeric_limits<double>::max();
            for (std::map<HdLaneId, HdLinkId>::iterator it = noTurnArrowsMap.begin(); it != noTurnArrowsMap.end(); it++) {
                double dLaneAngle = GetLaneAngle(it->second, it->first, true);
                double dCurLaneAngleSub = AngleSub(dLaneAngle, dLastAngle);
                switch (nInOutLanePrior) {
                    case LanePriority::LeftFirst:
                    case LanePriority::RightFirst:
                        // 需要转向，选择角度偏差大的
                        if ((HdLane::UTurnLane != func_GetLaneType(it->second, it->first)) && (dMaxAngleSub < dCurLaneAngleSub)) {
                            dMaxAngleSub = dCurLaneAngleSub;
                            hdLinkIdTmp = it->second;
                            hdLaneIdTmp = it->first;
                            bNeedChangeToStraight = true;  // 不能在这里变更优先方向，否则会影响下一次循环
                        }
                        break;
                    case LanePriority::Straight:
                    default:
                        // 直行，选择角度偏差小的
                        if ((HdLane::UTurnLane != func_GetLaneType(it->second, it->first)) && (dMinAngleSub > dCurLaneAngleSub)) {
                            dMinAngleSub = dCurLaneAngleSub;
                            hdLinkIdTmp = it->second;
                            hdLaneIdTmp = it->first;
                        }
                        break;
                }
            }
            if (true == bNeedChangeToStraight) {
                nInOutLanePrior = LanePriority::Straight;
            }
            if (hdLaneIdTmp.lValue != 0) {
                liOutNxtLinkId = hdLinkIdTmp;
                liOutNxtLaneId = hdLaneIdTmp;
                return true;
            } else {
                bNeedChangeToStraight = false;
                for (std::map<HdLaneId, HdLinkId>::iterator it2 = hasTrunArrowsMap.begin(); it2 != hasTrunArrowsMap.end(); it2++) {
                    double dCurLaneAngleSub = AngleSub(GetLaneAngle(it2->second, it2->first, true), dLastAngle);
                    switch (nInOutLanePrior) {
                        case LanePriority::LeftFirst:
                        case LanePriority::RightFirst:
                            // 需要转向，选择角度偏差大的
                            if ((HdLane::UTurnLane != func_GetLaneType(it2->second, it2->first)) && (dMaxAngleSub < dCurLaneAngleSub)) {
                                dMaxAngleSub = dCurLaneAngleSub;
                                hdLinkIdTmp = it2->second;
                                hdLaneIdTmp = it2->first;
                                bNeedChangeToStraight = true;  // 不能在这里变更优先方向，否则会影响下一次循环
                            }
                            break;
                        case LanePriority::Straight:
                        default:
                            // 直行，选择角度偏差小的
                            if ((HdLane::UTurnLane != func_GetLaneType(it2->second, it2->first)) && (dMinAngleSub > dCurLaneAngleSub)) {
                                dMinAngleSub = dCurLaneAngleSub;
                                hdLinkIdTmp = it2->second;
                                hdLaneIdTmp = it2->first;
                            }
                    }
                }
                liOutNxtLinkId = hdLinkIdTmp;
                liOutNxtLaneId = hdLaneIdTmp;
                if (true == bNeedChangeToStraight) {
                    nInOutLanePrior = LanePriority::Straight;
                }
                return true;
            }
            return true;
        }
    }

    return false;
}

/******客户需求：简化LaneEdge.edgeGeometry坐标点的个数，结果小于50个*****/
void MapInterface::SimplifyPoint(std::vector<Point3D>& vctp3GeoLink, int nMaxPoints) {
    int num = vctp3GeoLink.size() / (vctp3GeoLink.size() - nMaxPoints) == 1 ? 2 : vctp3GeoLink.size() / (vctp3GeoLink.size() - nMaxPoints);
    if (num > vctp3GeoLink.size() - 2) {
        num = vctp3GeoLink.size() - 2;
    }
    int count = 1;
    for (std::vector<Point3D>::iterator it = vctp3GeoLink.begin() + 1; it != vctp3GeoLink.end() - 1;)  //遍历第2个到倒数第2个
    {
        if (count % num == 0) {
            it = vctp3GeoLink.erase(it);
        } else {
            it++;
        }
        count++;
    }

    return;
}

}  // namespace jf