#include "MapInterface.hpp"

#include <time.h>

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

namespace jf {

MapInterface::MapInterface(const std::string strPrjPath, const std::string strCfgPath, const double sc_dLMReportDis) : m_lpPlan(strPrjPath, strCfgPath), m_nLastIdx(-1), statusFlagOfPathPlanning(0), sc_dLMReportDis(sc_dLMReportDis) {
    std::cout << "播报距离已设置为： " << this->sc_dLMReportDis << "米" << std::endl;
}

MapInterface::~MapInterface() {}

/**
 * 设置全局路径规划的起始点和目标点
 * @param cspmWayNode
 * @param ppmOutPath
 * @return
 */
bool MapInterface::setGlobalPathPlanning(const SetPathMsg_ &cspmWayNode, PathPlanMsg_ &ppmOutPath) {
    // 生成消息
    ppmOutPath.ptCurLoc = cspmWayNode.arrptWayPoints.at(0);
    // 清除原有的寻路信息
    m_nLastIdx = 0;
    m_vctleStartEndpoint.clear();
    m_vctptWayPoint.clear();
    m_vctleWayPoint.clear();
    m_vctnWayPointIdxInPath.clear();

    // 转换座标点的数据类型
    for (point_ jfPoint : cspmWayNode.arrptWayPoints) {
        Point ptInPoint {};
        ptInPoint.dLon = jfPoint.dLon;
        ptInPoint.dLat = jfPoint.dLat;
        ptInPoint.dAlt = jfPoint.dAlt;
        m_vctptWayPoint.push_back(ptInPoint);
    }
    Point pointTmp;
    if (filterTargetPoint(m_vctptWayPoint[m_vctptWayPoint.size() - 1], pointTmp)) {
        m_vctptWayPoint[m_vctptWayPoint.size() - 1] = pointTmp;
    } else {
        ppmOutPath.bIsSuccess = false;
        ppmOutPath.bIsChanged = false;
        return false;
    }

    // 记录起点所在的车道
    std::vector<Point> vctptPoint = {};
    std::vector<double> vctdDistance = {};
    HdLib::GetInstance()->GetLocateEndpoints(m_vctptWayPoint[0], 10,10,m_vctleStartEndpoint);
    m_lpPlan.GetNearEndpoints(m_vctptWayPoint[0], vctptPoint, m_vctleStartEndpoint, vctdDistance);
    // 寻路
    if (calculatePathPlanning(ppmOutPath)) {
        statusFlagOfPathPlanning = 1;
        // 计算结束点
        m_nEndPosIdxInLastLane = m_lpPlan.GetPosIdxInLane(m_vctptWayPoint[m_vctptWayPoint.size() - 1], m_lpPlan.GetLaneCnt() - 1);
        m_ptLastPos = m_vctptWayPoint[0];
        return true;
    } else {
        return false;
    }
}

/**
 * 实时路径规划（以全局目标路径为前提）
 * @param cgdCurLoc
 * @param ppmOutPath
 * @return
 */
bool MapInterface::dynamicPathPlanning(const GpsData_ &cgdCurLoc, PathPlanMsg_ &ppmOutPath) {
    ppmOutPath.ptCurLoc.dLon = cgdCurLoc.longitude;
    ppmOutPath.ptCurLoc.dLat = cgdCurLoc.latitude;
    ppmOutPath.ptCurLoc.dAlt = cgdCurLoc.altitude;
    //如果未设置全局目标路径，则不进行实时路径规划
    if (statusFlagOfPathPlanning == 0) {
        ppmOutPath.bIsSuccess = false;
        ppmOutPath.bIsChanged = false;
        return false;
    }
    // 转换座标点的数据类型
    Point ptInPoint = {cgdCurLoc.longitude, cgdCurLoc.latitude, cgdCurLoc.altitude};

    int nCurIdx = 0;

    // 计算该点在全局路径规划结果中的位置索引（从0开始）
    if (true == GetIndex(ptInPoint, nCurIdx, m_nLastIdx)) {
        m_nLastIdx = nCurIdx;
        if (nCurIdx < m_lpPlan.GetLaneCnt() - 1) {
            ppmOutPath.bIsSuccess = true;
            ppmOutPath.bIsChanged = false;
        } else {
            if (IsReachedEnd(ptInPoint)) {  //到达终点
                statusFlagOfPathPlanning = 0;
                ppmOutPath.bIsSuccess = false;
                ppmOutPath.bIsChanged = false;
            } else {  //到达最后一个endpoint，但是还没有到终点坐标
                ppmOutPath.bIsSuccess = true;
                ppmOutPath.bIsChanged = false;
            }
        }
    } else {  // 已经偏离路线，重新寻路
        std::vector<Point> vctptWayPoint = {};
        vctptWayPoint.push_back(ptInPoint);
        // 从当前点开始，利用已经设置的途径点和终点，重新寻路，但已经路过的途径点，不再重新寻路
        for (int i = 0; i < m_vctnWayPointIdxInPath.size(); ++i) {
            if (m_vctnWayPointIdxInPath[i] > m_nLastIdx) {
                vctptWayPoint.push_back(m_vctptWayPoint[i]);
            }
        }
        m_nLastIdx = 0;
        m_vctptWayPoint.swap(vctptWayPoint);
        m_vctleWayPoint.clear();
        m_vctnWayPointIdxInPath.clear();
        // 记录起点所在的车道
        std::vector<Point> vctptPoint = {};
        std::vector<double> vctdDistance = {};
        if ((false == HdLib::GetInstance()->GetLocateEndpoints(m_vctptWayPoint[0],10,10, m_vctleStartEndpoint)) && (false == m_lpPlan.GetNearEndpoints(m_vctptWayPoint[0], vctptPoint, m_vctleStartEndpoint, vctdDistance))) {
        }
        calculatePathPlanning(ppmOutPath);
        if (!ppmOutPath.bIsSuccess) {  //局部路径规划失败后，则需要重新设置全局目标路径
            statusFlagOfPathPlanning = 0;
        }
        ppmOutPath.bIsChanged = true;
    }
    m_ptLastPos = ptInPoint;
    return true;
}

/**
 * 实时获取道路前方数据  //注意！必须先进行dynamicPathPlanning获取有效的定位m_nLastIdx，再进行道路前方信息的获取GetPathAhead
 * @param cgdCurLoc
 * @param pamOutAhead
 * @return
 */
bool MapInterface::GetPathAhead(const GpsData_ &cgdCurLoc, PathAheadMsg_ &pamOutAhead) {
    pamOutAhead.ptCurLoc.dLon = cgdCurLoc.longitude;
    pamOutAhead.ptCurLoc.dLat = cgdCurLoc.latitude;
    pamOutAhead.ptCurLoc.dAlt = cgdCurLoc.altitude;
    //如果未设置全局目标路径，则不获取道路前方数据
    if (statusFlagOfPathPlanning == 0) {
        pamOutAhead.bIsSuccess = false;
        return false;
    }else{
        pamOutAhead.bIsSuccess = true;
    }
    // 转换座标点的数据类型
    Point ptInPoint = {cgdCurLoc.longitude, cgdCurLoc.latitude, cgdCurLoc.altitude};
    LaneEndpoint leCurEndpoint = {};
    m_lpPlan.GetLaneEndpoint(m_nLastIdx, leCurEndpoint);
    std::vector<double> vctdOutSegDistance;
    handleReportDistance(ptInPoint, leCurEndpoint, vctdOutSegDistance);

    //生成landmark数据
    std::vector<landmark_> vctjlLandmark = {};
    std::map<int64_t ,int32_t> HeightLimitMap;
    std::map<int64_t, int32_t> weightLimitMap;
    ConvertLandmark(ptInPoint, leCurEndpoint, vctjlLandmark, vctdOutSegDistance,HeightLimitMap,weightLimitMap);
    for (landmark_ jlLandmark : vctjlLandmark) {
        pamOutAhead.arrlmAheadLandmark.push_back(jlLandmark);
    }
    //生成播报的link数据
    for (size_t i = 0; i < vctdOutSegDistance.size(); i++) {
        LaneEndpoint laneEndpointTmp = {};
        m_lpPlan.GetLaneEndpoint(m_nLastIdx + i, laneEndpointTmp);
        link_ linkTmp = {};
        ConvertLink(laneEndpointTmp, HeightLimitMap,weightLimitMap,linkTmp);
        pamOutAhead.arrlkAheadPath.push_back(linkTmp);
        if (i == 0) {
            std::vector<LaneEndpoint> vctleLaneEp = {};
            HdLib::GetInstance()->GetLocateEndpoints(ptInPoint,10,10, vctleLaneEp);
            // 默认
            pamOutAhead.nCurBlockIndex = 1;
            int nLaneChange = 0;
            int nFixIdx = 0;
            GetLaneChange(leCurEndpoint, nLaneChange, nFixIdx);
            bool bFound = false;
            for (int m = 0; (m <= abs(nLaneChange)) && (false == bFound); ++m) {
                HdLaneId lLaneId = 0;
                if (true == HdLib::GetInstance()->GetLaneId(leCurEndpoint.lLinkId, leCurEndpoint.nStartLaneNum + m * nFixIdx, lLaneId)) {
                    // 可能有多个，只有和当前车道号相同的序号才可以使用
                    for (LaneEndpoint leEndpoint : vctleLaneEp) {
                        // 可能有多个，只有和当前车道号相同的序号才可以使用
                        if (leEndpoint.lLaneId == lLaneId) {
                            pamOutAhead.nCurBlockIndex = leEndpoint.nStartLaneNum;
                            bFound = true;
                            break;
                        }
                    }
                }
            }
        }
    }
    //生成link的转角
    for (int j = 0; j < pamOutAhead.arrlkAheadPath.size(); j++) {
        if (j + 1 < pamOutAhead.arrlkAheadPath.size()) {
            pamOutAhead.arrlkAheadPath[j].fEndChangedAngle = pamOutAhead.arrlkAheadPath[j].fStartAngle - pamOutAhead.arrlkAheadPath[j + 1].fStartAngle;
        } else {
            pamOutAhead.arrlkAheadPath[j].fEndChangedAngle = 0;
        }
    }

    return true;
}

/**
 * 路径规划 核心计算模块
 *
 * @param msgOutPpmPlan
 * @return 路径规划是否成功
 */
bool MapInterface::calculatePathPlanning(PathPlanMsg_ &msgOutPpmPlan) {
    // 寻路
    if (!m_lpPlan.SetPath(m_vctptWayPoint, m_vctleWayPoint, true)) {
        // 失败
        msgOutPpmPlan.bIsSuccess = false;
        msgOutPpmPlan.bIsChanged = false;
        return false;
    }
    // 生成消息
    msgOutPpmPlan.bIsSuccess = true;
    msgOutPpmPlan.bIsChanged = true;

    // 获取寻路结果中的LaneEndpoint的数量
    int nLaneCnt = m_lpPlan.GetLaneCnt();
    // 逐条车道生成数据
    for (int i = 0; i < nLaneCnt; ++i) {
        LaneEndpoint leCurEndpoint = {};
        // 获取一个车道信息
        m_lpPlan.GetLaneEndpoint(i, leCurEndpoint);
        // 重新计算途径点的内部信息
        for (size_t j = 0; j < m_vctleWayPoint.size(); ++j) {
            if (m_vctleWayPoint[j].lLaneId == leCurEndpoint.lLaneId) {
                m_vctnWayPointIdxInPath.push_back(i);
            }
        }

        // 不是最后一条车道
        if (i != nLaneCnt - 1) {
            LaneEndpoint leNxtEndpoint = {};
            // 获取下一条车道的信息
            m_lpPlan.GetLaneEndpoint(i + 1, leNxtEndpoint);
            // 生成Block信息
            std::vector<block_> vctjbBlock = {};
            ConvertBlock(leCurEndpoint, leNxtEndpoint, vctjbBlock);
            // 添加到消息队列中（如果有变道，可能有多个Block）
            for (block_ jbBlock : vctjbBlock) {
                msgOutPpmPlan.arrblPath.push_back(jbBlock);
            }
        }
        // 最后一条车道
        else {
            // 生成Block信息
            std::vector<block_> vctjbBlock = {};
            // 添加到消息队列中（如果有变道，可能有多个Block）
            ConvertBlock(leCurEndpoint, leCurEndpoint, vctjbBlock);
            for (block_ jbBlock : vctjbBlock) {
                msgOutPpmPlan.arrblPath.push_back(jbBlock);
            }
        }
    }

    return true;
}

/**
 * 播报距离处理模块（必须要有路径规划结果才能调用）
 * @param cptInPoint
 * @param cleCurLaneEndpoint
 * @param vctdOutSegDistance 车辆前方涉及到的laneEndpoint的距当前点的距离队列
 * @return
 */
bool MapInterface::handleReportDistance(const Point &cptInPoint, const LaneEndpoint &cleCurLaneEndpoint, std::vector<double> &vctdOutSegDistance) {
    int nIndex = 0;
    Point ptPerFootPoint = {};
    // 获取垂足信息
    GetPerFootInfo(cptInPoint, cleCurLaneEndpoint, nIndex, ptPerFootPoint);

    // 假设当前点为CP1，垂足为FP2，距离当前点最近的点为NP3，先计算垂足点FP2到最近点NP3的下一点的距离
    // 如果NP3是最后的点，则计算FP2到NP3的距离
    // 这样做是因为FP2可能位于NP3的前面，也可能位于NP3的后面，为了减少误差，直接计算FP2到NP3的下一点的距离
    // 如果NP3是最后一个点，则不存在垂足点在NP3后面的情况
    int nNextPerFootIndex = nIndex >= cleCurLaneEndpoint.vctptGeoLink.size() - 1 ? nIndex : nIndex + 1;
    double dRealDistance = 0;
    double dSegmentDis = PointsDis(ptPerFootPoint, cleCurLaneEndpoint.vctptGeoLink[nNextPerFootIndex]);
    // 从最近点NP3的下一点开始，计算当前LaneEndpoint的距离
    for (size_t i = nNextPerFootIndex; i < cleCurLaneEndpoint.vctptGeoLink.size() - 1; i++) {
        dSegmentDis += PointsDis(cleCurLaneEndpoint.vctptGeoLink[i], cleCurLaneEndpoint.vctptGeoLink[i + 1]);
    }
    // 到当前段终点的长度无论如何都要保存，因为当前段里可能会有播报物体
    dRealDistance = dSegmentDis;
    vctdOutSegDistance.push_back(dRealDistance);

    // 如果到当前段终点的长度还没有超过播报距离，则继续往下找
    int nParseEndpointCnt = 0;
    while (dRealDistance <= sc_dLMReportDis) {
        ++nParseEndpointCnt;
        dSegmentDis = 0;
        // 计算下一段LaneEndpoint的长度
        // 将其保存（即使超过播报距离也暂时保存，待到使用时过滤掉）
        LaneEndpoint leNxtEndpoint = {};
        if (true == m_lpPlan.GetLaneEndpoint(m_nLastIdx + nParseEndpointCnt, leNxtEndpoint)) {
            for (size_t i = 0; i < leNxtEndpoint.vctptGeoLink.size() - 1; i++) {
                dSegmentDis += PointsDis(leNxtEndpoint.vctptGeoLink[i], leNxtEndpoint.vctptGeoLink[i + 1]);
            }
            dRealDistance += dSegmentDis;
            vctdOutSegDistance.push_back(dRealDistance);
        } else {
            break;
        }
    }
    return true;
}

/**
 * 目标终点的过滤
 * （1、如果终点在车道上，不作处理 2、终点不在车道上，在一定范围内）
 * @param inTargetPoint
 * @param outTargetPoint
 * @return
 */
 bool MapInterface::filterTargetPoint(const Point &inTargetPoint, Point &outTargetPoint){
        std::vector<LaneEndpoint> vctLaneEndpoint;
        if(!HdLib::GetInstance()->GetLocateEndpoints(inTargetPoint,10,10,vctLaneEndpoint)){
            double distanceTmp;
            return HdLib::GetInstance()->HdLib::GetNearestPointfromLaneGeo(inTargetPoint,outTargetPoint,distanceTmp);
        }else{
            outTargetPoint = inTargetPoint;
            return  true;
        }

}

/**
 * 生成link数据
 * @param cleCurLaneEndpoint
 * @param cleNxtLaneEndpoint
 * @param jlOutLink
 * @return
 */
bool MapInterface::ConvertLink(const LaneEndpoint &cleCurLaneEndpoint, std::map<int64_t,int32_t> &HeightLimitMap,std::map<int64_t,int32_t>& weightLimitMap, link_ &jlOutLink) {
    // 获取当前link里面的车道总数针对每个车道分别处理
    for (HdLane hdLaneTmp : HdLib::GetInstance()->GetLinkData(cleCurLaneEndpoint.lLinkId)->vcthlLane) {
        // 生成lane数据
        lane_ jlLane = {};
        // 车道ID
        jlLane.lId = hdLaneTmp.liId;
        // 车道总数
        jlLane.nLanesCnt = HdLib::GetInstance()->GetLinkData(cleCurLaneEndpoint.lLinkId)->vcthlLane.size();
        // 当前车道序号
        jlLane.nLaneSeqNum = hdLaneTmp.nSeqNum;

        // 车道起始坐标
        jlLane.ptLaneStart.dLon = hdLaneTmp.vctvdGeoLink[0][0];
        jlLane.ptLaneStart.dLat = hdLaneTmp.vctvdGeoLink[0][1];
        jlLane.ptLaneStart.dAlt = hdLaneTmp.vctvdGeoLink[0][2];
        // 车道终止坐标
        jlLane.ptLaneEnd.dLon = hdLaneTmp.vctvdGeoLink[hdLaneTmp.vctvdGeoLink.size() - 1][0];
        jlLane.ptLaneEnd.dLat = hdLaneTmp.vctvdGeoLink[hdLaneTmp.vctvdGeoLink.size() - 1][1];
        jlLane.ptLaneEnd.dAlt = hdLaneTmp.vctvdGeoLink[hdLaneTmp.vctvdGeoLink.size() - 1][2];
        // 车道宽度
        jlLane.nLaneWidth = hdLaneTmp.nWidth;
        jlLane.lLeftEdgeId = hdLaneTmp.lLeftEdgeId;
        jlLane.lRightEdgeId = hdLaneTmp.lRightEdgeId;
        // 与该车道相连的下一组车道序号
        for (HdLaneId lLaneId : hdLaneTmp.vctliNxtLaneId) {
            jlLane.arrlNxtLanes.push_back(lLaneId);
        }
        // 车道类型
        jlLane.nLaneType = hdLaneTmp.nType;
        jlLane.nSpeedLimit = hdLaneTmp.nMaxSpeed;
        jlOutLink.arrlnLanes.push_back(jlLane);
    }

    // 获取当前link里面的车道边界线总数，针对每个边界线作分别处理
    int nEdgeCnt = 0;
    HdLib::GetInstance()->GetEdgeCnt(cleCurLaneEndpoint.lLinkId, nEdgeCnt);
    for (int i = 0; i < nEdgeCnt; ++i) {
        // 生成edge数据
        edge_ jeEdge = {};
        // 获取边界线序号
        HdLib::GetInstance()->GetEdgeId(cleCurLaneEndpoint.lLinkId, i, jeEdge.lId);
        // 获取边界线座标点
        std::vector<std::vector<double>> vctvdEdgeGeo = {};
        HdLib::GetInstance()->GetEdgeGeoLink(cleCurLaneEndpoint.lLinkId, jeEdge.lId, vctvdEdgeGeo);
        for (int i = 0; i < vctvdEdgeGeo.size(); ++i) {
            point_ jpPoint = point_();
            jpPoint.dLon = vctvdEdgeGeo[i][0];
            jpPoint.dLat = vctvdEdgeGeo[i][1];
            jpPoint.dAlt = vctvdEdgeGeo[i][2];
            jeEdge.arrptGeo.push_back(jpPoint);
        }
        // 获取边界线跨越属性
        static const int sc_nEdgeCross_Uncrossable = 1;
        static const int sc_nEdgeCross_BidirecAble = 2;
        static const int sc_nEdgeCross_LeftAble = 3;
        static const int sc_nEdgeCross_RightAble = 4;
        static const int sc_nEdgeAttrib_Uncrossable = 0;
        static const int sc_nEdgeAttrib_BidirecAble = 1;
        static const int sc_nEdgeAttrib_LeftAble = 2;
        static const int sc_nEdgeAttrib_RightAble = 3;
        static const int sc_nEdgeAttrib_Unknown = 99;
        int nEdgeAttrib = 0;
        HdLib::GetInstance()->GetEdgeCross(cleCurLaneEndpoint.lLinkId, jeEdge.lId, nEdgeAttrib);
        switch (nEdgeAttrib) {
            case sc_nEdgeCross_Uncrossable:  // 无法跨越
                jeEdge.nAttrib = sc_nEdgeAttrib_Uncrossable;
                break;
            case sc_nEdgeCross_BidirecAble:  // 双方相跨越
                jeEdge.nAttrib = sc_nEdgeAttrib_BidirecAble;
                break;
            case sc_nEdgeCross_LeftAble:  // 向左跨越
                jeEdge.nAttrib = sc_nEdgeAttrib_LeftAble;
                break;
            case sc_nEdgeCross_RightAble:  // 向右跨越
                jeEdge.nAttrib = sc_nEdgeAttrib_RightAble;
                break;
            default:
                jeEdge.nAttrib = sc_nEdgeAttrib_Unknown;
                break;
        };

        jlOutLink.arregEdges.push_back(jeEdge);
    }
    if(HeightLimitMap.find(cleCurLaneEndpoint.lLinkId.lValue)!=HeightLimitMap.end()){
        jlOutLink.nHighLimit = HeightLimitMap[cleCurLaneEndpoint.lLinkId.lValue];
    }
    if(weightLimitMap.find(cleCurLaneEndpoint.lLinkId.lValue)!=weightLimitMap.end()){
        jlOutLink.nWeightLimit = weightLimitMap[cleCurLaneEndpoint.lLinkId.lValue];
    }
//    // 获取link限速
//    m_lpPlan.GetLinkSpeedLimit(cleCurLaneEndpoint.lLinkId, jlOutLink.nSpeedLimit);
//
//    // 获取link限高限重信息
//    static const int sc_nIndFaciType2_HeightLimit = 8;
//    static const int sc_nIndFaciType2_WeightLimit = 9;
//    m_lpPlan.GetLinkLimitValue(cleCurLaneEndpoint.lLinkId, sc_nIndFaciType2_HeightLimit, jlOutLink.nHighLimit);
//    m_lpPlan.GetLinkLimitValue(cleCurLaneEndpoint.lLinkId, sc_nIndFaciType2_WeightLimit, jlOutLink.nWeightLimit);

    // 获取道路等级
    static const int sc_nRoadRank_Unknown = 0;
    static const int sc_nRoadRank_Highway = 1;
    static const int sc_nRoadRank_NationalRoad = 2;
    static const int sc_nRoadRank_ProvincialRoad = 3;
    static const int sc_nRoadRank_CountyRoad = 4;
    static const int sc_nRoadRank_CityHighway = 7;
    static const int sc_nRoadRank_CityMainRoad = 8;
    static const int sc_nRoadRank_CitySecondaryRoad = 9;
    static const int sc_nRoadRank_CityInnerRoad = 10;
    static const int sc_nRoadRank_CityPath = 11;
    static const int sc_nRoadLevel_Unknown = 0;
    static const int sc_nRoadLevel_NationalHighway = 1;
    static const int sc_nRoadLevel_ProvincialRoad = 2;
    static const int sc_nRoadLevel_CountyRoad = 3;
    static const int sc_nRoadLevel_CityRoad = 4;
    static const int sc_nRoadLevel_OtherRoad = 5;
    int nRoadLevel = 0;
    HdLib::GetInstance()->GetLinkRank(cleCurLaneEndpoint.lLinkId, nRoadLevel);
    switch (nRoadLevel) {
        case sc_nRoadRank_Unknown:  // 未调查
            jlOutLink.nRoadLevel = sc_nRoadLevel_Unknown;
            break;
        case sc_nRoadRank_Highway:       // 高速道路
        case sc_nRoadRank_NationalRoad:  // 国道
            jlOutLink.nRoadLevel = sc_nRoadLevel_NationalHighway;
            break;
        case sc_nRoadRank_ProvincialRoad:  // 省道
            jlOutLink.nRoadLevel = sc_nRoadLevel_ProvincialRoad;
            break;
        case sc_nRoadRank_CountyRoad:  // 县道
            jlOutLink.nRoadLevel = sc_nRoadLevel_CountyRoad;
            break;
        case sc_nRoadRank_CityHighway:        // 城市快速
        case sc_nRoadRank_CityMainRoad:       // 城市主干道
        case sc_nRoadRank_CitySecondaryRoad:  // 城市次干道
        case sc_nRoadRank_CityInnerRoad:      // 城市内部路
        case sc_nRoadRank_CityPath:           // 城市小路
            jlOutLink.nRoadLevel = sc_nRoadLevel_CityRoad;
            break;
        default:
            jlOutLink.nRoadLevel = sc_nRoadLevel_OtherRoad;
            break;
    };

    // 获取道路起点的方向角度
    double dStartAngle = 0;
    HdLib::GetInstance()->GetLinkStartAngle(cleCurLaneEndpoint.lLinkId, dStartAngle);
    jlOutLink.fStartAngle = dStartAngle;

    return true;
}

bool MapInterface::ConvertLandmark(const Point &cptInPoint, const LaneEndpoint &cleCurLaneEndpoint, std::vector<landmark_> &vctjlOutLandmark,
                                   std::vector<double> &vctdSegmentDis,std::map<int64_t,int32_t>& heightLimitMap,std::map<int64_t,int32_t> &weightLimitMap) {
    static const int nType_StopLine = 0;
    static const int nType_TrafficLight = 1;
    static const int nType_CrossPole = 2;
    static const int nType_TrafficSign = 3;
    static const int nType_EndPoint = 4;
    static const int nType_BusStop = 5;
    static const int nType_Hydrant = 6;
    static const int nType_Arrow = 7;
    static const int nType_Text = 8;
    static const int nType_Symbol = 9;
    static const int nType_NoParking = 10;
    static const int nType_SpeedBump = 11;
    static const int nType_OverHead = 12;

    /**在当前车辆的LaneEndpoint的基础上，向道路前方递增LaneEndpoint的个数遍历**/
    for (size_t i = 0; i < vctdSegmentDis.size(); i++) {
        LaneEndpoint laneEndpointTmp = {};
        m_lpPlan.GetLaneEndpoint(m_nLastIdx + i, laneEndpointTmp);
        /**获取当前lane车道的中心线的轨迹点vctvdGeolink**/
        std::vector<std::vector<double>> vctvdGeolink = {};
        HdLib::GetInstance()->GetLaneGeoLink(laneEndpointTmp.lLinkId, laneEndpointTmp.nStartLaneNum, vctvdGeolink);
        /***获取当前lane终点到车辆的距离****/
        double dLastSegmentDis = 0;
        if (i == 0) {
            // 如果是第0个LaneEndpoint，即车辆所在的LaneEndpoint，距离的计算起点就是车辆所在的点
            // 经过的距离是0
            dLastSegmentDis = 0;
        } else {
            // 经过的距离是他前面所有分段的距离总和
            dLastSegmentDis = vctdSegmentDis[i - 1];
        }
        /**获取变道过程中车辆经过的所有车道**/
        int nLaneChang = 0;  //变道次数
        int nFixIdx = 0;     //变道方向
        GetLaneChange(laneEndpointTmp, nLaneChang, nFixIdx);

        std::vector<std::vector<double>> vctGeolinkOfLaneEndpointTmp;  // laneEndPoint所在的车道 的车道中心线轨迹点
        HdLib::GetInstance()->ConvertGeoLink(laneEndpointTmp.vctptGeoLink, vctGeolinkOfLaneEndpointTmp);
        /***遍历车道所属的link内所有类型的标志物对象****/
        const HdLink *hdLink = HdLib::GetInstance()->GetLinkData(laneEndpointTmp.lLinkId);
        for (int nTypeTmp = 0; nTypeTmp <= 12; nTypeTmp++) {
            switch (nTypeTmp) {
                case 0: {  //停止线
                    for (ObjStopLine objStopLine : hdLink->vctslStopLine) {
                        landmark_ landmark;
                        landmark.Id = objStopLine.lId;
                        landmark.nType = nType_StopLine;
                        for (std::vector<double> raw_point : objStopLine.vctvdGeoLink) {
                            point_ point(raw_point[0], raw_point[1], raw_point[2]);
                            landmark.ptPos.push_back(point);
                        }

                        double tmp;
                        //获取标志物的垂点到所属的lane的起点距离
                        HdLib::GetInstance()->GetPosInGeoLink(objStopLine.vctvdGeoLink[0], vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                        if (i == 0) {  //如果车辆和标志物处于同一个lane内
                            double distanceTmp = landmark.dDistance;
                            HdLib::GetInstance()->GetPosInGeoLink(cptInPoint, vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                            landmark.dDistance = distanceTmp - landmark.dDistance;
                            if (landmark.dDistance >= 0 && sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        } else {
                            landmark.dDistance = landmark.dDistance + dLastSegmentDis;
                            if (sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        }
                    }
                } break;

                case 1: {  //红绿灯
                    std::vector<ObjTrafficLight> vctTrafficLight;
                    for (int j = 0; j <= abs(nLaneChang); ++j) {
                        HdLane hdLaneTmp;
                        HdLib::GetInstance()->GetLaneData(laneEndpointTmp.lLinkId, laneEndpointTmp.nStartLaneNum + j * nFixIdx, hdLaneTmp);
                        vctTrafficLight.insert(vctTrafficLight.end(), hdLaneTmp.vcttlTrafficLight.begin(), hdLaneTmp.vcttlTrafficLight.end());
                    }
                    for (ObjTrafficLight objTrafficLight : vctTrafficLight) {
                        landmark_ landmark;
                        landmark.Id = objTrafficLight.lId;
                        landmark.nType = nType_TrafficLight;
                        for (std::vector<double> raw_point : objTrafficLight.vctvdGeoArea) {
                            point_ point(raw_point[0], raw_point[1], raw_point[2]);
                            landmark.ptPos.push_back(point);
                        }
                        if (i == 0) {
                            double tmp1, tmp2, tmp3;
                            HdLib::GetInstance()->GetPosInGeoLink(cptInPoint, vctGeolinkOfLaneEndpointTmp, tmp1, tmp3);                //车辆到lane起始点的距离
                            HdLib::GetInstance()->GetPosInGeoLink(laneEndpointTmp.ptEpNode, vctGeolinkOfLaneEndpointTmp, tmp2, tmp3);  // lane终点到lane起始点的距离
                            landmark.dDistance = tmp2 - tmp1;
                        } else {
                            landmark.dDistance = vctdSegmentDis[i];
                        }
                        if (sc_dLMReportDis >= landmark.dDistance && landmark.dDistance >= 0) {
                            vctjlOutLandmark.push_back(landmark);
                        }
                    }

                } break;
                case 2: {  //横杆

                }

                break;
                case 3: {  //交通标牌
                    for (ObjTrafficSign objTrafficSign : hdLink->vctTrafficSign) {
                        landmark_ landmark;
                        landmark.Id = objTrafficSign.lId;
                        landmark.nType = nType_TrafficSign;
                        for (std::vector<double> raw_point : objTrafficSign.vctvdGeoArea) {
                            point_ point(raw_point[0], raw_point[1], raw_point[2]);
                            landmark.ptPos.push_back(point);
                        }
                        landmark.shape = objTrafficSign.nShape;
                        landmark.type1 = objTrafficSign.nType1;
                        landmark.type2 = objTrafficSign.nType2;
                        //生成限高map表数据
                        if(landmark.type1 == 3 && landmark.type2 == 8){
                            if(!objTrafficSign.strText.empty()) {
                                heightLimitMap[hdLink->liId.lValue] = atoi(objTrafficSign.strText.c_str());
                            }
                        }
                        //限制重量
                        if(landmark.type1 == 3 && landmark.type2 == 9){
                            if (!objTrafficSign.strText.empty()){
                                weightLimitMap[hdLink->liId.lValue] = atoi(objTrafficSign.strText.c_str());
                            }
                        }
                        double tmp;
                        //获取标志物的垂点到所属的lane的起点距离
                        HdLib::GetInstance()->GetPosInGeoLink(objTrafficSign.vctvdGeoArea[0], vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                        if (i == 0) {  //如果车辆和标志物处于同一个lane内
                            double distanceTmp = landmark.dDistance;
                            HdLib::GetInstance()->GetPosInGeoLink(cptInPoint, vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                            landmark.dDistance = distanceTmp - landmark.dDistance;
                            if (landmark.dDistance >= 0 && sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        } else {
                            landmark.dDistance = landmark.dDistance + dLastSegmentDis;
                            if (sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        }
                    }
                } break;
                case 4: {  //终点
                    LaneEndpoint leLastEndpoint = {};
                    m_lpPlan.GetLaneEndpoint(m_lpPlan.GetLaneCnt() - 1, leLastEndpoint);
                    if (laneEndpointTmp.lLinkId.lValue == leLastEndpoint.lLinkId.lValue) {
                        std::map<int64_t, landmark_> NewObjDataMap;
                        landmark_ landmark;
                        landmark.Id = 999999;
                        landmark.nType = nType_EndPoint;
                        point_ point(leLastEndpoint.vctptGeoLink[m_nEndPosIdxInLastLane].dLon, leLastEndpoint.vctptGeoLink[m_nEndPosIdxInLastLane].dLat, leLastEndpoint.vctptGeoLink[m_nEndPosIdxInLastLane].dAlt);
                        landmark.ptPos.push_back(point);

                        double tmp;
                        HdLib::GetInstance()->GetPosInGeoLink({leLastEndpoint.vctptGeoLink[m_nEndPosIdxInLastLane].dLon, leLastEndpoint.vctptGeoLink[m_nEndPosIdxInLastLane].dLat, leLastEndpoint.vctptGeoLink[m_nEndPosIdxInLastLane].dAlt},
                                                              vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                        if (i == 0) {
                            double distanceTmp = landmark.dDistance;
                            HdLib::GetInstance()->GetPosInGeoLink(cptInPoint, vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                            landmark.dDistance = distanceTmp - landmark.dDistance;
                            if (landmark.dDistance >= 0 && sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        } else {
                            landmark.dDistance = landmark.dDistance + dLastSegmentDis;
                            if (sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        }
                    }
                }

                break;
                case 5: {  //公交站
                    std::vector<ObjBusStop> vctBusStop;
                    for (int j = 0; j <= abs(nLaneChang); ++j) {
                        std::vector<ObjBusStop> vctBusStopTmp;
                        HdLib::GetInstance()->QueryObjDataByLinkIdAndLaneNum(hdLink, laneEndpointTmp.nStartLaneNum + j * nFixIdx, hdLink->vctBusStop, vctBusStopTmp);
                        vctBusStop.insert(vctBusStop.end(), vctBusStopTmp.begin(), vctBusStopTmp.end());
                    }
                    for (ObjBusStop objBusStop : vctBusStop) {
                        landmark_ landmark;
                        landmark.Id = objBusStop.lId;
                        landmark.nType = nType_BusStop;
                        for (std::vector<double> raw_point : objBusStop.vctvdGeoArea) {
                            point_ point(raw_point[0], raw_point[1], raw_point[2]);
                            landmark.ptPos.push_back(point);
                        }

                        double tmp;
                        //获取标志物的垂点到所属的lane的起点距离
                        HdLib::GetInstance()->GetPosInGeoLink(objBusStop.vctvdGeoArea[0], vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                        if (i == 0) {  //如果车辆和标志物处于同一个lane内
                            double distanceTmp = landmark.dDistance;
                            HdLib::GetInstance()->GetPosInGeoLink(cptInPoint, vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                            landmark.dDistance = distanceTmp - landmark.dDistance;
                            if (landmark.dDistance >= 0 && sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        } else {
                            landmark.dDistance = landmark.dDistance + dLastSegmentDis;
                            if (sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        }
                    }

                }

                break;
                case 6: {  //消防栓
                    std::vector<ObjHydrant> vctHydrant;
                    for (int j = 0; j <= abs(nLaneChang); ++j) {
                        std::vector<ObjHydrant> vctHydrantTmp;
                        HdLib::GetInstance()->QueryObjDataByLinkIdAndLaneNum(hdLink, laneEndpointTmp.nStartLaneNum + j * nFixIdx, hdLink->vctHydrant, vctHydrantTmp);
                        vctHydrant.insert(vctHydrant.end(), vctHydrantTmp.begin(), vctHydrantTmp.end());
                    }
                    for (ObjHydrant objHydrant : vctHydrant) {
                        landmark_ landmark;
                        landmark.Id = objHydrant.lId;
                        landmark.nType = nType_Hydrant;
                        for (std::vector<double> raw_point : objHydrant.vctvdGeoArea) {
                            point_ point(raw_point[0], raw_point[1], raw_point[2]);
                            landmark.ptPos.push_back(point);
                        }

                        double tmp;
                        //获取标志物的垂点到所属的lane的起点距离
                        HdLib::GetInstance()->GetPosInGeoLink(objHydrant.vctvdGeoArea[0], vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                        if (i == 0) {  //如果车辆和标志物处于同一个lane内
                            double distanceTmp = landmark.dDistance;
                            HdLib::GetInstance()->GetPosInGeoLink(cptInPoint, vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                            landmark.dDistance = distanceTmp - landmark.dDistance;
                            if (landmark.dDistance >= 0 && sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        } else {
                            landmark.dDistance = landmark.dDistance + dLastSegmentDis;
                            if (sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        }
                    }
                } break;
                case 7: {  //地面箭头
                    HdLane hdLaneTmp;
                    HdLib::GetInstance()->GetLaneData(laneEndpointTmp.lLinkId, laneEndpointTmp.nEndLaneNum, hdLaneTmp);
                    for (ObjArrow objArrow : hdLaneTmp.vctarArrow) {
                        landmark_ landmark;
                        landmark.Id = objArrow.lId;
                        landmark.nType = nType_Arrow;
                        for (std::vector<double> raw_point : objArrow.vctvdGeoArea) {
                            point_ point(raw_point[0], raw_point[1], raw_point[2]);
                            landmark.ptPos.push_back(point);
                        }

                        double tmp;
                        //获取标志物的垂点到所属的lane的起点距离
                        HdLib::GetInstance()->GetPosInGeoLink(objArrow.vctvdGeoArea[0], vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                        if (i == 0) {  //如果车辆和标志物处于同一个lane内
                            double distanceTmp = landmark.dDistance;
                            HdLib::GetInstance()->GetPosInGeoLink(cptInPoint, vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                            landmark.dDistance = distanceTmp - landmark.dDistance;
                            if (landmark.dDistance >= 0 && sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        } else {
                            landmark.dDistance = landmark.dDistance + dLastSegmentDis;
                            if (sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        }
                    }
                }

                break;
                case 8: {  //地面文字

                }

                break;
                case 9: {  //地面符号

                }

                break;
                case 10: {  //禁止停车区域
                    std::vector<ObjNoParking> vctNoParking;
                    for (int j = 0; j <= abs(nLaneChang); ++j) {
                        std::vector<ObjNoParking> vctNoParkingTmp;
                        HdLib::GetInstance()->QueryObjDataByLinkIdAndLaneNum(hdLink, laneEndpointTmp.nStartLaneNum + j * nFixIdx, hdLink->vctNoParking, vctNoParkingTmp);
                        vctNoParking.insert(vctNoParking.end(), vctNoParkingTmp.begin(), vctNoParkingTmp.end());
                    }
                    for (ObjNoParking objNoParking : vctNoParking) {
                        landmark_ landmark;
                        landmark.Id = objNoParking.lId;
                        landmark.nType = nType_NoParking;
                        for (std::vector<double> raw_point : objNoParking.vctvdGeoArea) {
                            point_ point(raw_point[0], raw_point[1], raw_point[2]);
                            landmark.ptPos.push_back(point);
                        }
                        double tmp;
                        //获取标志物的垂点到所属的lane的起点距离
                        HdLib::GetInstance()->GetPosInGeoLink(objNoParking.vctvdGeoArea[0], vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                        if (i == 0) {  //如果车辆和标志物处于同一个lane内
                            double distanceTmp = landmark.dDistance;
                            HdLib::GetInstance()->GetPosInGeoLink(cptInPoint, vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                            landmark.dDistance = distanceTmp - landmark.dDistance;
                            if (landmark.dDistance >= 0 && sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        } else {
                            landmark.dDistance = landmark.dDistance + dLastSegmentDis;
                            if (sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        }
                    }
                }

                break;
                case 11: {  //减速带
                    std::vector<ObjSpeedBump> vctSpeedBump;
                    for (int j = 0; j <= abs(nLaneChang); ++j) {
                        std::vector<ObjSpeedBump> vctSpeedBumpTmp;
                        HdLib::GetInstance()->QueryObjDataByLinkIdAndLaneNum(hdLink, laneEndpointTmp.nStartLaneNum + j * nFixIdx, hdLink->vctSpeedBump, vctSpeedBumpTmp);
                        vctSpeedBump.insert(vctSpeedBump.end(), vctSpeedBumpTmp.begin(), vctSpeedBumpTmp.end());
                    }
                    for (ObjSpeedBump objSpeedBump : vctSpeedBump) {
                        landmark_ landmark;
                        landmark.Id = objSpeedBump.lId;
                        landmark.nType = nType_SpeedBump;
                        for (std::vector<double> raw_point : objSpeedBump.vctvdGeoArea) {
                            point_ point(raw_point[0], raw_point[1], raw_point[2]);
                            landmark.ptPos.push_back(point);
                        }
                        double tmp;
                        //获取标志物的垂点到所属的lane的起点距离
                        HdLib::GetInstance()->GetPosInGeoLink(objSpeedBump.vctvdGeoArea[0], vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                        if (i == 0) {  //如果车辆和标志物处于同一个lane内
                            double distanceTmp = landmark.dDistance;
                            HdLib::GetInstance()->GetPosInGeoLink(cptInPoint, vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                            landmark.dDistance = distanceTmp - landmark.dDistance;
                            if (landmark.dDistance >= 0 && sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        } else {
                            landmark.dDistance = landmark.dDistance + dLastSegmentDis;
                            if (sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        }
                    }
                }

                break;
                case 12: {  //上方障碍物
                    std::vector<ObjOverHead> vctOverHead;
                    for (int j = 0; j <= abs(nLaneChang); ++j) {
                        std::vector<ObjOverHead> vctOverHeadTmp;
                        HdLib::GetInstance()->QueryObjDataByLinkIdAndLaneNum(hdLink, laneEndpointTmp.nStartLaneNum + j * nFixIdx, hdLink->vctOverHead, vctOverHeadTmp);
                        vctOverHead.insert(vctOverHead.end(), vctOverHeadTmp.begin(), vctOverHeadTmp.end());
                    }
                    for (ObjOverHead objOverHead : vctOverHead) {
                        landmark_ landmark;
                        landmark.Id = objOverHead.lId;
                        for (std::vector<double> raw_point : objOverHead.vctvdGeoLink) {
                            point_ point(raw_point[0], raw_point[1], raw_point[2]);
                            landmark.ptPos.push_back(point);
                        }
                        if (objOverHead.OH_type == 3) {  //横杆
                            landmark.nType = nType_CrossPole;
                        } else {
                            landmark.nType = nType_OverHead;
                        }
                        double tmp;
                        //获取标志物的垂点到所属的lane的起点距离
                        HdLib::GetInstance()->GetPosInGeoLink(objOverHead.vctvdGeoLink[0], vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                        if (i == 0) {  //如果车辆和标志物处于同一个lane内
                            double distanceTmp = landmark.dDistance;
                            HdLib::GetInstance()->GetPosInGeoLink(cptInPoint, vctGeolinkOfLaneEndpointTmp, landmark.dDistance, tmp);
                            landmark.dDistance = distanceTmp - landmark.dDistance;
                            if (landmark.dDistance >= 0 && sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        } else {
                            landmark.dDistance = landmark.dDistance + dLastSegmentDis;
                            if (sc_dLMReportDis >= landmark.dDistance) {
                                vctjlOutLandmark.push_back(landmark);
                            }
                        }
                    }

                } break;
                default:
                    break;
            }
        }
    }
    return true;
}

/**
 * 生成路径规划的block数据
 * @param cleCurLaneEndpoint
 * @param cleNxtLaneEndpoint
 * @param vctjbOutBlock
 * @return
 */
bool MapInterface::ConvertBlock(const LaneEndpoint &cleCurLaneEndpoint, const LaneEndpoint &cleNxtLaneEndpoint, std::vector<block_> &vctjbOutBlock) {
    static const int sc_nDriveDirect_Straight = 0;         // 直行
    static const int sc_nDriveDirect_LeftTurn = 1;         // 左转
    static const int sc_nDriveDirect_RightTurn = 2;        // 右转
    static const int sc_nDriveDirect_UTurn = 3;            // 掉头
    static const int sc_nDriveDirect_LeftChangeLane = 4;   // 左变道
    static const int sc_nDriveDirect_RightChangeLane = 5;  // 右变道
    static const int sc_nDriveDirect_Others = 6;           // 其他

    static const char sc_cArrow_LeftUTurn = 'A';
    static const char sc_cArrow_LeftTurn = 'B';
    static const char sc_cArrow_Straight = 'C';
    static const char sc_cArrow_RightTurn = 'D';
    static const char sc_cArrow_RightUTurn = 'E';
    static const char sc_cArrow_LeftConf = 'G';
    static const char sc_cArrow_RightConf = 'H';

    // 转换坐标点数据类型
    auto func_ConvertPoint = [](const Point &cptSrcPoint, point_ &jpOutTgtPoint) {
        jpOutTgtPoint.dLon = cptSrcPoint.dLon;
        jpOutTgtPoint.dLat = cptSrcPoint.dLat;
        jpOutTgtPoint.dAlt = cptSrcPoint.dAlt;
    };

    // 获取车道内的轨迹点数据
    auto func_GetGeolinkInBlock = [this, func_ConvertPoint](const LaneEndpoint &cleLaneEndpoint, int nLaneSeqNum, std::vector<point_> &vctjpOutGeolink, double &dOutRealLength) {
        std::vector<std::vector<double>> vctvdLaneGeolink = {};
        HdLib::GetInstance()->GetLaneGeoLink(cleLaneEndpoint.lLinkId, nLaneSeqNum, vctvdLaneGeolink);
        int nStatGeoIndex = 0;
        int nEndGeoIndex = vctvdLaneGeolink.size() - 1;
        LaneEndpoint leFirstLaneEndpoint = {};
        LaneEndpoint leLastLaneEndpoint = {};
        m_lpPlan.GetLaneEndpoint(0, leFirstLaneEndpoint);
        m_lpPlan.GetLaneEndpoint(m_lpPlan.GetLaneCnt() - 1, leLastLaneEndpoint);
        // 第一车道和最后车道单独处理
        if ((cleLaneEndpoint.lLinkId.lValue == leFirstLaneEndpoint.lLinkId.lValue) || (cleLaneEndpoint.lLinkId.lValue == leLastLaneEndpoint.lLinkId.lValue)) {
            double dGeoDistance = -1;
            int nGeoIndex = 0;
            Point ptGeoPos = {};
            if (cleLaneEndpoint.lLinkId.lValue == leFirstLaneEndpoint.lLinkId.lValue) {
                // 第一车道，所给点到车道的垂足点到距离车道最近的点的下一点（第二个点），以及从第二个点到最后的点分别处理
                HdLib::GetInstance()->GetNearestDistance(m_vctptWayPoint.at(0), vctvdLaneGeolink, ptGeoPos, dGeoDistance, nGeoIndex);
                HdLibMath::GetPerpendicularFoot(m_vctptWayPoint.at(0), vctvdLaneGeolink, ptGeoPos, dGeoDistance);
                nStatGeoIndex = nGeoIndex == 0 ? nGeoIndex : nGeoIndex - 1;
            }
            if (cleLaneEndpoint.lLinkId.lValue == leLastLaneEndpoint.lLinkId.lValue) {
                // 第末车道，最初的点到所给点的前一点，以及从前一点到所给点到车道的垂足点分别处理
                HdLib::GetInstance()->GetNearestDistance(m_vctptWayPoint.at(m_vctptWayPoint.size() - 1), vctvdLaneGeolink, ptGeoPos, dGeoDistance, nGeoIndex);
                HdLibMath::GetPerpendicularFoot(m_vctptWayPoint.at(m_vctptWayPoint.size() - 1), vctvdLaneGeolink, ptGeoPos, dGeoDistance);
                nEndGeoIndex = nGeoIndex >= vctvdLaneGeolink.size() - 1 ? nGeoIndex : nGeoIndex + 1;
            }
        }

        // 针对原来的点，做插值处理（间隔0.5米）
        std::vector<Point> vctptLaneGeolink = {};
        std::vector<Point> vctptDensityLaneGeolink = {};
        HdLib::GetInstance()->ConvertGeoLink(vctvdLaneGeolink, vctptLaneGeolink);
        for (int i = nStatGeoIndex, j = i + 1; j <= nEndGeoIndex; ++i, ++j) {
            std::vector<Point> ret = jf::InterP(vctptLaneGeolink[i], vctptLaneGeolink[j], 0.5);
            vctptDensityLaneGeolink.insert(vctptDensityLaneGeolink.end(), ret.begin(), ret.end());
        }
        for (size_t i = 0; (false == vctptDensityLaneGeolink.empty()) && (i < vctptDensityLaneGeolink.size() - 1); ++i) {
            point_ jpTgtPoint = point_();
            func_ConvertPoint(vctptDensityLaneGeolink[i], jpTgtPoint);
            vctjpOutGeolink.push_back(jpTgtPoint);
        }

        // 计算行驶长度
        if ((cleLaneEndpoint.lLinkId.lValue == leFirstLaneEndpoint.lLinkId.lValue) || (cleLaneEndpoint.lLinkId.lValue == leLastLaneEndpoint.lLinkId.lValue)) {
            double dRealLength = 0;
            for (int i = nStatGeoIndex, j = i + 1; j <= nEndGeoIndex; ++i, ++j) {
                dRealLength += PointsDis(vctptLaneGeolink[i], vctptLaneGeolink[j]);
            }
            dOutRealLength = dRealLength;
        } else {
            HdLib::GetInstance()->GetLinkLength(cleLaneEndpoint.lLinkId, dOutRealLength);
        }
    };

    // 计算行驶方向
    auto func_GetDriveDirect = [this](const LaneEndpoint &cleCurLaneEndpoint, const LaneEndpoint &cleNxtLaneEndpoint, int nFromSeqNum, int nToSeqNum, int nIndex) {
        // 获取起点的方向角度
        double dCurLinkStartAngle = 0;
        double dNxtLinkStartAngle = 0;
        HdLib::GetInstance()->GetLinkStartAngle(cleCurLaneEndpoint.lLinkId, dCurLinkStartAngle);
        HdLib::GetInstance()->GetLinkStartAngle(cleNxtLaneEndpoint.lLinkId, dNxtLinkStartAngle);
        // 向左变道
        if (nFromSeqNum > nToSeqNum) {
            if (nFromSeqNum - nIndex > nToSeqNum) {
                return sc_nDriveDirect_LeftChangeLane;
            } else {
                return sc_nDriveDirect_Straight;
            }
        }
        // 向右变道
        else if (nFromSeqNum < nToSeqNum) {
            if (nFromSeqNum + nIndex < nToSeqNum) {
                return sc_nDriveDirect_RightChangeLane;
            } else {
                return sc_nDriveDirect_Straight;
            }
        } else {
            if ((abs(dCurLinkStartAngle - dNxtLinkStartAngle) >= 45) && (abs(dCurLinkStartAngle - dNxtLinkStartAngle) <= 315)) {
                double dLeftTurnEndAngle = (dCurLinkStartAngle - 90) < 0 ? (dCurLinkStartAngle - 90 + 360) : (dCurLinkStartAngle - 90);
                double dRightTurnEndAngle = (dCurLinkStartAngle + 90) >= 360 ? (dCurLinkStartAngle + 90 - 360) : (dCurLinkStartAngle + 90);
                double dUTurnEndAngle = (dCurLinkStartAngle + 180) >= 360 ? (dCurLinkStartAngle + 180 - 360) : (dCurLinkStartAngle + 180);
                // 左转
                if ((abs(dLeftTurnEndAngle - dNxtLinkStartAngle) <= 45) || (abs(dLeftTurnEndAngle - dNxtLinkStartAngle) >= 315)) {
                    return sc_nDriveDirect_LeftTurn;
                }
                // 右转
                else if ((abs(dRightTurnEndAngle - dNxtLinkStartAngle) <= 45) || (abs(dRightTurnEndAngle - dNxtLinkStartAngle) >= 315)) {
                    return sc_nDriveDirect_RightTurn;
                }
                // 掉头
                else if ((abs(dUTurnEndAngle - dNxtLinkStartAngle) <= 45) || (abs(dUTurnEndAngle - dNxtLinkStartAngle) >= 315)) {
                    return sc_nDriveDirect_UTurn;
                }
                // 无法判断
                else {
                    return sc_nDriveDirect_Others;
                }
            } else {
                // 直行
                return sc_nDriveDirect_Straight;
            }
        }
    };

    //组装block数据
    auto func_GenBlockInfo = [this, func_GetGeolinkInBlock, func_GetDriveDirect](const LaneEndpoint &cleCurLaneEndpoint, const LaneEndpoint &cleNxtLaneEndpoint, int nLaneSeqNum, int nFromSeqNum, int nToSeqNum, int nIndex, block_ &jbOutBlock) {
        // 道路序号
        jbOutBlock.nLaneSeqNum = nLaneSeqNum;

        // 车道ID
        HdLaneId lLaneId = {};

        HdLib::GetInstance()->GetLaneId(cleCurLaneEndpoint.lLinkId, nLaneSeqNum, lLaneId);

        jbOutBlock.lId = lLaneId;

        // 轨迹点信息
        func_GetGeolinkInBlock(cleCurLaneEndpoint, nLaneSeqNum, jbOutBlock.arrptTrack, jbOutBlock.dDriveLen);

        // 行使方向
        jbOutBlock.nDriveDirect = func_GetDriveDirect(cleCurLaneEndpoint, cleNxtLaneEndpoint, abs(nFromSeqNum), abs(nToSeqNum), nIndex);
    };

    if (cleCurLaneEndpoint.nMidLaneNum == 0) {
        for (size_t i = 0; i <= abs(cleCurLaneEndpoint.nStartLaneNum - cleCurLaneEndpoint.nEndLaneNum); i++) {
            block_ jbBlock = {};

            // 道路序号
            int nLaneSeqNum = cleCurLaneEndpoint.nStartLaneNum >= cleCurLaneEndpoint.nEndLaneNum ? cleCurLaneEndpoint.nStartLaneNum - i : cleCurLaneEndpoint.nStartLaneNum + i;

            func_GenBlockInfo(cleCurLaneEndpoint, cleNxtLaneEndpoint, nLaneSeqNum, cleCurLaneEndpoint.nStartLaneNum, cleCurLaneEndpoint.nEndLaneNum, i, jbBlock);

            vctjbOutBlock.push_back(jbBlock);
        }
    } else {
        for (size_t i = 0; i < abs(cleCurLaneEndpoint.nStartLaneNum - cleCurLaneEndpoint.nMidLaneNum); i++) {
            block_ jbBlock = {};

            // 道路序号
            int nLaneSeqNum = cleCurLaneEndpoint.nStartLaneNum >= cleCurLaneEndpoint.nMidLaneNum ? cleCurLaneEndpoint.nStartLaneNum - i : cleCurLaneEndpoint.nStartLaneNum + i;

            func_GenBlockInfo(cleCurLaneEndpoint, cleNxtLaneEndpoint, nLaneSeqNum, cleCurLaneEndpoint.nStartLaneNum, cleCurLaneEndpoint.nMidLaneNum, i, jbBlock);

            vctjbOutBlock.push_back(jbBlock);
        }
        for (size_t i = 0; i <= abs(cleCurLaneEndpoint.nMidLaneNum - cleCurLaneEndpoint.nEndLaneNum); i++) {
            block_ jbBlock = {};

            // 道路序号
            int nLaneSeqNum = cleCurLaneEndpoint.nMidLaneNum >= cleCurLaneEndpoint.nEndLaneNum ? cleCurLaneEndpoint.nMidLaneNum - i : cleCurLaneEndpoint.nMidLaneNum + i;

            func_GenBlockInfo(cleCurLaneEndpoint, cleNxtLaneEndpoint, nLaneSeqNum, cleCurLaneEndpoint.nMidLaneNum, cleCurLaneEndpoint.nEndLaneNum, i, jbBlock);

            vctjbOutBlock.push_back(jbBlock);
        }
    }

    return true;
}

/**
 * 获取当前坐标点 在路径规划LaneEndpoint序列中的Index
 *
 * @param cptPoint
 * @param nOutIndex
 * @param nPastIndex
 * @return
 */
bool MapInterface::GetIndex(const Point &cptPoint, int &nOutIndex, int nPastIndex /* = 0*/) {
    static const double sc_dMaxGpsJumpDis = 10;

    std::vector<Point> vctptPoint = {};
    std::vector<double> vctdDistance = {};
    std::vector<LaneEndpoint> vctleEndpoint = {};
    // 优先使用输入点所在的车道，如果没有则使用输入点附近的车道
    if ((false == HdLib::GetInstance()->GetLocateEndpoints(cptPoint,10,10, vctleEndpoint)) && (false == m_lpPlan.GetNearEndpoints(cptPoint, vctptPoint, vctleEndpoint, vctdDistance))) {
        return false;
    }
    // 如果尚在第一车道，则允许当前点所在的车道和实际的路径规划的第一车道不同。
    if (0 == m_nLastIdx) {
        for (const LaneEndpoint &cleCurLaneEndpoint : vctleEndpoint) {
            for (const LaneEndpoint &cleStartLaneEndpoint : m_vctleStartEndpoint) {
                if (cleStartLaneEndpoint.lLaneId == cleCurLaneEndpoint.lLaneId) {
                    nOutIndex = 0;
                    return true;
                }
            }
        }
    }

    if ((m_ptLastPos.dLon == 0 || m_ptLastPos.dLat == 0) || (PointsDis(m_ptLastPos, cptPoint) >= sc_dMaxGpsJumpDis)) {
        // 如果上一点距离当前点太远，则忽视寻找范围的限制
        return m_lpPlan.GetIndex(vctleEndpoint, nOutIndex);
    } else {
        /**这个值至少为3
         * 例如sc_nGetIndex_FindCount = 3，表示的是以上一周期车辆所属的lane在路径规划中的Index作为基准,这一周期的寻找范围是[Index-1，Index+3]
         **/
        static const int sc_nGetIndex_FindCount = 4;
        return m_lpPlan.GetIndex(vctleEndpoint, nOutIndex, nPastIndex == 0 ? nPastIndex : nPastIndex - 1, sc_nGetIndex_FindCount);
    }
}

bool MapInterface::GetPerFootInfo(const Point &cptInPoint, const LaneEndpoint &cleCurLaneEndpoint, int &nOutIndex, Point &ptOutPerFootPoint) {
    std::vector<std::vector<double>> vctvdGeolink = {};
    // 转换坐标类型
    HdLib::GetInstance()->ConvertGeoLink(cleCurLaneEndpoint.vctptGeoLink, vctvdGeolink);
    Point ptNearPoint = {};
    double dDistance = 0;
    // 获取到指定点最近的点
    HdLib::GetInstance()->GetNearestDistance(cptInPoint, vctvdGeolink, ptNearPoint, dDistance, nOutIndex);
    // 获取垂足点
    HdLibMath::GetPerpendicularFoot(cptInPoint, vctvdGeolink, ptOutPerFootPoint, dDistance);

    return true;
}

bool MapInterface::IsReachedEnd(const Point &ptPoint) {
    // 判断当前点在轨迹点里面的索引是否已经超过终点的索引
    if (m_nEndPosIdxInLastLane < m_lpPlan.GetPosIdxInLane(ptPoint, m_lpPlan.GetLaneCnt() - 1)) {
        return true;
    }
    return false;
}

bool MapInterface::GetLaneChange(const LaneEndpoint cleEndpoint, int &nOutLaneChange, int &nOutFixIdx) {
    if (cleEndpoint.nMidLaneNum == 0) {
        nOutLaneChange = cleEndpoint.nEndLaneNum - cleEndpoint.nStartLaneNum;
    } else {
        // assert(cleEndpoint.nMidLaneNum != cleEndpoint.nEndLaneNum);
        nOutLaneChange = abs(cleEndpoint.nEndLaneNum - cleEndpoint.nStartLaneNum) > abs(cleEndpoint.nMidLaneNum - cleEndpoint.nStartLaneNum) ? cleEndpoint.nEndLaneNum - cleEndpoint.nStartLaneNum : cleEndpoint.nMidLaneNum - cleEndpoint.nStartLaneNum;
    }

    nOutFixIdx = nOutLaneChange == 0 ? 0 : nOutLaneChange / abs(nOutLaneChange);

    return true;
}
}  // namespace jf