#include "MapMatching.hpp"

#include <unistd.h>

#include <algorithm>
#include <fstream>

namespace jf {

MapMatching::MapMatching() : m_spHdLib(HdLib::GetInstance()), m_spBindRoad(BindRoad::GetInstance()), m_lckGnss(), m_lckVision(), m_lckEkfResult(), m_lckMatchResult(), m_nLastLaneSeqNum(1) {
    // 创建csv文件, for test
    std::ofstream ofsQgisInput;
    std::ofstream ofsQgisResult;
    strQgisFileInput_ = "gnss.csv";
    strQgisFileResult_ = "result.csv";
    ofsQgisInput.open(strQgisFileInput_.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisResult.open(strQgisFileResult_.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisInput.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisResult.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisInput.precision(15);
    ofsQgisResult.precision(15);

    // 插入标签
    ofsQgisInput << "lon, lat, num, link, lane" << std::endl;
    ofsQgisResult << "lon, lat, cross, leftdis, rightdis, left, right" << std::endl;
    ofsQgisInput.close();
    ofsQgisResult.close();
}

MapMatching::~MapMatching() {}

void MapMatching::Init() {
    // init map
    // m_spHdLib->Init(projectDir, configJsonPath);
    // begin run map matching thread
    m_thdMatching = new std::thread(&MapMatching::MapMatchingProc, this);
}

void MapMatching::SaveMatchResultData(const IMatchResult& mrMatchResult) {
    // save gnss foot data
    AutoLock alLocker(m_lckMatchResult);
    m_lstmrMatchResult.push_back(mrMatchResult);
}

bool MapMatching::GetMatchResultData(IMatchResult& mrOutMatchResult) {
    AutoLock alLocker(m_lckMatchResult);
    // Just use the newest data
    while (m_lstmrMatchResult.size() > 1) {
        m_lstmrMatchResult.pop_front();
    }
    if (!m_lstmrMatchResult.empty()) {
        // get the newest data
        mrOutMatchResult = m_lstmrMatchResult.front();
        m_lstmrMatchResult.pop_front();
        return true;
    } else {
        return false;
    }
}

void MapMatching::SaveGnssLocData(const IGnssLoc& glGnssLoc) {
    // save gnss data
    AutoLock alLocker(m_lckGnss);
    m_lstglGnss.push_back(glGnssLoc);
}

bool MapMatching::GetGnssLocData(IGnssLoc& glOutGnssLoc) {
    AutoLock alLocker(m_lckGnss);
    // Just use the newest data
    while (m_lstglGnss.size() > 1) {
        m_lstglGnss.pop_front();
    }
    if (!m_lstglGnss.empty()) {
        // get the newest data
        glOutGnssLoc = m_lstglGnss.front();
        m_lstglGnss.pop_front();
        return true;
    } else {
        return false;
    }
}

void MapMatching::SaveVisionMapData(const IVisionMap& vmVisionMap) {
    // save eyeq4 data
    AutoLock alLocker(m_lckVision);
    m_lstvmVision.push_back(vmVisionMap);
}

bool MapMatching::GetVisionMapData(IVisionMap& vmOutVisionMap) {
    AutoLock alLocker(m_lckVision);
    // Just use the newest data
    while (m_lstvmVision.size() > 1 && m_lstvmVision.front().crossing == MarkingCrossing::EDGE_CROSSING_NO_CROSSING) {
        m_lstvmVision.pop_front();
    }
    if (!m_lstvmVision.empty()) {
        // get the newest data
        vmOutVisionMap = m_lstvmVision.front();
        m_vmCurVision = vmOutVisionMap;
        m_lstvmVision.pop_front();
        return true;
    } else {
        return false;
    }
}

void MapMatching::SaveEkfResultData(const IEkfResult& erEkfResult) {
    // save the localization result
    AutoLock alLocker(m_lckEkfResult);
    m_lsterEkfResult.push_back(erEkfResult);
}

bool MapMatching::GetEkfResultData(IEkfResult& erOutEkfResult) {
    AutoLock alLocker(m_lckEkfResult);
    // Just use the newest data
    while (m_lsterEkfResult.size() > 1) {
        m_lsterEkfResult.pop_front();
    }
    if (!m_lsterEkfResult.empty()) {
        // get the newest data
        erOutEkfResult = m_lsterEkfResult.front();
        m_lsterEkfResult.pop_front();
        return true;
    } else {
        return false;
    }
}

void MapMatching::MapMatchingProc() {
    while (true) {
        MapMatchingOnce();
    }
}

void MapMatching::MapMatchingOnce() {
    IGnssLoc glGnssLoc = {};
    while (false == GetGnssLocData(glGnssLoc)) {
        // got gnss data failed, sleep
        usleep(5);
    }

    // for test
    PrintQgis(strQgisFileInput_, glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.heading, 0, 0);

    IVisionMap vmVision = {};
    while (false == GetVisionMapData(vmVision)) {
        // got vmVision map data failed sleep
        // usleep(5);
        // make ekf message data
        Point ptCurLoc = Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z);
        jf::HdLinkId liCurrentLinkId = {};
        int nLaneSeqNum = 0;
        LocateByGnss(ptCurLoc, liCurrentLinkId, nLaneSeqNum);
        IMatchResult mrMatchResult = {};
        Point ptFootLoc = {0, 0, 0};
        Point ptHorizontalLoc = {0, 0, 0};
        Point ptVerticalLoc = {0, 0, 0};
        MakeMatchResult(glGnssLoc, vmVision, liCurrentLinkId, false, nLaneSeqNum, ptFootLoc, ptHorizontalLoc, ptVerticalLoc, mrMatchResult);
        SaveMatchResultData(mrMatchResult);
        return;
    }

    // convert cross type
    auto func_ConvertCross = [](HMC::LaneMarkingType vcVisionCross, bool bIsLeft) {
        switch (vcVisionCross) {
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_SINGLE_DASHED:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_DOUBLE_DASHED:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_MIXED_SINGLE_DASHED:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_MIXED_DOUBLE_DASHED:
                return "D";  // dashed line
                break;
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_SINGLE_SOLID:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_DOUBLE_SOLID:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_MIXED_SINGLE_SOLID:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_MIXED_DOUBLE_SOLID:
                return "S";  // solid line
                break;
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_SHORT_THICK_DASHED:
                return "T";
                break;
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_RIGHT_SOLID_LEFT_DASHED:
                if (true == bIsLeft) {
                    return "D";  // dashed line
                } else {
                    return "S";  // solid line
                }
                break;
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_LEFT_SOLID_RIGHT_DASHED:
                if (true == bIsLeft) {
                    return "S";  // solid line
                } else {
                    return "D";  // dashed line
                }
                break;
            default:
                return "U";
                break;
        }
        return "U";
    };

    // convert color value
    auto func_ConvertColor = [](HMC::LaneMarkingColor vcVisionColor) {
        switch (vcVisionColor) {
            case HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_WHITE:
                return "W";  // white
                break;
            case HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_YELLOW:
                return "Y";  // yellow
                break;
            case HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_RAD:
                return "R";  // red
                break;
            case HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_BLUE:
                return "B";  // blue
            case HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_GREEN:
                return "G";  // green
            default:
                return "W";  // white
                break;
        }
        return "W";  // white
    };

    auto func_FindEdge = [](IVisionMap vmVision, LaneMarkingSide lsSide) {
        for (auto lmEdge : vmVision.lane_markings) {
            if (lmEdge.pos == lsSide) {
                return lmEdge;
            }
        }
        ILaneMarking lmEdge = {};
        return lmEdge;
    };

    static jf::HdLinkId liCurrentLinkId = {};
    bool bLaneChange = false;
    int nLaneSeqNum = 0;
    Point ptFootLoc = {0, 0, 0};
    Point ptHorizontalLoc = {0, 0, 0};
    Point ptVerticalLoc = {0, 0, 0};
    // set confidence
    if (true == UpdateConfidence(glGnssLoc, vmVision, liCurrentLinkId, bLaneChange, nLaneSeqNum, ptFootLoc, ptHorizontalLoc, ptVerticalLoc)) {
        // for test
        // PrintQgis(strQgisFileResult_, ptHorizontalLoc.lon, ptHorizontalLoc.lat, confidence_.horizontal, (int)vmVision.crossing, nLaneSeqNum);
    }
    // update confidence failed, use rtk data
    else {
        // // trust gnss input, check where the car is
        // Point in_point = Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z);
        // if (true == LocateByGnss(in_point, liCurrentLinkId, nLaneSeqNum)) {
        //     ptFootLoc = {glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z};
        //     PrintQgis(strQgisFileResult_, ptFootLoc.lon, ptFootLoc.lat, 0, liCurrentLinkId.lValue, nLaneSeqNum);
        // } else {
        //     PrintQgis(strQgisFileResult_, ptFootLoc.lon, ptFootLoc.lat, -1, -1, -1);
        // }
    }

    ILaneMarking lmLeftEdge = func_FindEdge(vmVision, LaneMarkingSide::LANE_MARKING_SIDE_LEFT);
    ILaneMarking lmRightEdge = func_FindEdge(vmVision, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT);
    std::string strLeftInfo = func_ConvertColor(lmLeftEdge.lane_marking_color);
    strLeftInfo += func_ConvertCross(lmLeftEdge.cross_type, true);
    std::string strRightInfo = func_ConvertColor(lmRightEdge.lane_marking_color);
    strRightInfo += func_ConvertCross(lmRightEdge.cross_type, false);
    PrintQgis(strQgisFileResult_, ptHorizontalLoc.dLon, ptHorizontalLoc.dLat, (int)vmVision.crossing, lmLeftEdge.dis, lmRightEdge.dis, strLeftInfo, strRightInfo);

    // make ekf message data
    IMatchResult mrMatchResult = {};
    MakeMatchResult(glGnssLoc, vmVision, liCurrentLinkId, bLaneChange, nLaneSeqNum, ptFootLoc, ptHorizontalLoc, ptVerticalLoc, mrMatchResult);
    SaveMatchResultData(mrMatchResult);
}

bool MapMatching::UpdateConfidence(const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, jf::HdLinkId& liOutCurrentLinkId, bool& bOutIsLaneChange, int& nOutLaneSeqNum, Point& ptOutFootLoc, Point& ptOutVisionHorizontalLoc, Point& ptOutVisionVerticalLoc) {
    static int s_nMatchCnt = 0;

    // total confidence
    if (false == UpdateTotalConfidence(false)) {
        return false;
    }

    static bool s_bIsUpdateStartLane = true;

    jf::HdLinkId liCurrentLinkId = {};
    static int s_nLaneSeqNum = 0;
    bool bIsForceTotalFalse = false;

    // try to match start lane until success
    while (true == s_bIsUpdateStartLane) {
        // check start lane
        if (false == UpdateStartLaneConfidence(glGnssLoc, vmVision, liCurrentLinkId, s_nLaneSeqNum)) {
            // 如果匹配失败，可能是EyeQ4数据不对，则继续使用上次结果
            // nLaneSeqNum = 0;
            return false;
        }

        if (false == UpdateStartStatusConfidence()) {
            continue;
        }

        bIsForceTotalFalse = true;

        s_bIsUpdateStartLane = false;

        m_cfConfidence.nHorizontal = 100;
        m_cfConfidence.nVertical = 100;
    };

    // horizontal confidence
    if (false == UpdateHorizontalConfidence(glGnssLoc, vmVision, liCurrentLinkId, bOutIsLaneChange, s_nLaneSeqNum, ptOutFootLoc, ptOutVisionHorizontalLoc)) {
        // failed, update parameters for next use
        s_nLaneSeqNum = 0;
        s_bIsUpdateStartLane = true;
        return false;
    }

    // vertical confidence
    if (false == UpdateVerticalConfidence(glGnssLoc, vmVision, liCurrentLinkId, ptOutVisionVerticalLoc)) {
        // failed, update parameters for next use
        s_nLaneSeqNum = 0;
        s_bIsUpdateStartLane = true;
        return false;
    }

    UpdateTotalConfidence(bIsForceTotalFalse);

    liOutCurrentLinkId = liCurrentLinkId;
    nOutLaneSeqNum = s_nLaneSeqNum;

    return true;
}

bool MapMatching::UpdateTotalConfidence(bool bIsForceTotalFalse) {
    if (bIsForceTotalFalse == true) {
        m_cfConfidence.nTotal = false;
    } else if (m_cfConfidence.nHorizontal >= 90 && m_cfConfidence.nVertical >= 90) {
        m_cfConfidence.nTotal = true;
    } else {
        m_cfConfidence.nTotal = false;
    }

    return true;
}

bool MapMatching::UpdateStartLaneConfidence(const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, jf::HdLinkId& liOutCurrentLinkId, int& nOutLaneSeqNum) {
    // Only check the possilbe lane number
    auto func_GetLaneNum = [this](const jf::HdLinkId& liCurrentLinkId, const IVisionMap& vmVision) {
        int nLanesCnt = 0;
        int nLaneSeqNum = 0;
        m_spHdLib->GetLaneCnt(liCurrentLinkId, nLanesCnt);
        std::cout << "MapMatching::UpdateStartLaneConfidence(), current link: " << liCurrentLinkId.lValue << ", lane count: " << nLanesCnt << ", ";
        // just one lane, not need to match
        if (nLanesCnt == 1) {
            std::cout << "possible lane num: 1." << std::endl;
            nLaneSeqNum = 1;
        }
        // two lanes, try to match with road eadge
        else if (nLanesCnt == 2) {
            if (false == vmVision.left_link_marking.empty()) {
                std::cout << "possible lane num: 1." << std::endl;
                nLaneSeqNum = 1;
            } else if (false == vmVision.right_link_marking.empty()) {
                std::cout << "possible lane num: 2." << std::endl;
                nLaneSeqNum = 2;
            }
        }
        // three lanes, try to match with the road edge and lane edge
        else if (nLanesCnt == 3) {
            if (false == vmVision.left_link_marking.empty()) {
                std::cout << "possible lane num: 1." << std::endl;
                nLaneSeqNum = 1;
            } else if (vmVision.lane_markings.size() == 4) {
                std::cout << "possible lane num: 2." << std::endl;
                nLaneSeqNum = 2;
            } else if (false == vmVision.right_link_marking.empty()) {
                std::cout << "possible lane num: 3." << std::endl;
                nLaneSeqNum = 3;
            }
        }
        // more than 4 lanes, only the first and last lane could be matched
        else if (nLanesCnt >= 4) {
            if (false == vmVision.left_link_marking.empty()) {
                std::cout << "possible lane num: 1." << std::endl;
                nLaneSeqNum = 1;
            } else if (false == vmVision.right_link_marking.empty()) {
                std::cout << "possible lane num: " << nLanesCnt << "." << std::endl;
                nLaneSeqNum = nLanesCnt;
            }
        }
        // failed
        else {
            std::cout << "could not get the lane num." << std::endl;
        }

        return nLaneSeqNum;
    };

    // try to match, only worked when lane markings in vmVision is not too many
    auto func_LuckyMatch = [this](const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId) {
        std::vector<int> vctnPossibleLaneSeqNum = {};
        std::vector<jf::HdEdgeId> vctlCheckingEdgeId = {};
        int nLaneCnt = 0;
        m_spHdLib->GetLaneCnt(liCurrentLinkId, nLaneCnt);
        // 遍历所有识别出的车道线
        for (const ILaneMarking& lane_marking : vmVision.lane_markings) {
            std::vector<LaneMarkingSide> vctlsCheckingPoses = {};
            switch (lane_marking.pos) {
                case LaneMarkingSide::LANE_MARKING_SIDE_LEFT:
                    // match with the left edge of the current lane
                    vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT};
                    // 用当前link中的所有车道与之尝试匹配
                    for (int i = 1; i <= nLaneCnt; i++) {
                        if (true == MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, i)) {
                            // 保存成功的结果
                            vctnPossibleLaneSeqNum.push_back(i);
                        };
                    }
                    break;
                case LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT:
                    // match with the left edge of the lane left the current lane
                    vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT};
                    // 用当前link中的所有车道与之尝试匹配
                    for (int i = 2; i <= nLaneCnt; i++) {
                        if (true == MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, i)) {
                            // 保存成功的结果
                            vctnPossibleLaneSeqNum.push_back(i);
                        };
                    }
                    break;
                case LaneMarkingSide::LANE_MARKING_SIDE_RIGHT:
                    // match with the right edge of the current lane
                    vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_RIGHT};
                    // 用当前link中的所有车道与之尝试匹配
                    for (int i = 1; i <= nLaneCnt; i++) {
                        if (true == MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, i)) {
                            // 保存成功的结果
                            vctnPossibleLaneSeqNum.push_back(i);
                        };
                    }
                    break;
                case LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT:
                    // match with the right edge of the lane right the current lane
                    vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT};
                    // 用当前link中的所有车道与之尝试匹配
                    for (int i = 1; i <= nLaneCnt - 1; i++) {
                        if (true == MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, i)) {
                            // 保存成功的结果
                            vctnPossibleLaneSeqNum.push_back(i);
                        };
                    }
                    break;
                default:
                    break;
            }
        }

        // 去重
        std::sort(vctnPossibleLaneSeqNum.begin(), vctnPossibleLaneSeqNum.end());
        vctnPossibleLaneSeqNum.erase(unique(vctnPossibleLaneSeqNum.begin(), vctnPossibleLaneSeqNum.end()), vctnPossibleLaneSeqNum.end());

        // only one result
        int nLaneSeqNum = 0;
        if (1 == vctnPossibleLaneSeqNum.size()) {
            std::cout << "MapMatching::UpdateStartLaneConfidence(), lucky match success, lane num: " << vctnPossibleLaneSeqNum.at(0) << std::endl;
            nLaneSeqNum = vctnPossibleLaneSeqNum.at(0);
        } else {
            std::cout << "MapMatching::UpdateStartLaneConfidence(), lucky match failed, lane num: 0" << std::endl;
            nLaneSeqNum = 0;
        }

        return nLaneSeqNum;
    };

    // step 1
    // bind road
    auto func_BindRoad = [this](const IGnssLoc& glGnssLoc, jf::HdLinkId& liOutCurrentLinkId) { return StepBindRoad(glGnssLoc, liOutCurrentLinkId); };

    // step 2
    // 尝试匹配车辆当前所在的车道
    auto func_CheckCurrentLane = [this, &func_GetLaneNum, &func_LuckyMatch](const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId) {
        int nLaneSeqNum = func_GetLaneNum(liCurrentLinkId, vmVision);
        if (nLaneSeqNum == 0) {
            // failed
            m_cfConfidence.nStartLane = 0;
        } else {
            // set match lanes
            // 匹配左右车道线
            std::vector<LaneMarkingSide> vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT};
            if (MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, nLaneSeqNum)) {
                std::cout << "MapMatching::UpdateStartLaneConfidence(), match current lane successed." << std::endl;
                return nLaneSeqNum;
            } else {
                std::cout << "MapMatching::UpdateStartLaneConfidence(), match current lane failed." << std::endl;
            }
        }

        // failed befor, try one more time
        nLaneSeqNum = func_LuckyMatch(vmVision, liCurrentLinkId);
        if (0 == nLaneSeqNum) {
            int nLaneCnt = 0;
            m_spHdLib->GetLaneCnt(liCurrentLinkId, nLaneCnt);

            // 车辆变道的可能性很低，所以继续尝试使用上一次匹配车道进行匹配
            std::vector<LaneMarkingSide> vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT};
            if (MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, m_nLastLaneSeqNum)) {
                std::cout << "MapMatching::UpdateStartLaneConfidence(), match current lane successed." << std::endl;
                nLaneSeqNum = m_nLastLaneSeqNum;
                return nLaneSeqNum;
            }

            // 从上一次匹配车道开始，由近及远逐个匹配左右两侧的车道
            for (int i = 1; i <= nLaneCnt; i++) {
                if (m_nLastLaneSeqNum + i <= nLaneCnt) {
                    std::vector<LaneMarkingSide> vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT};
                    if (MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, m_nLastLaneSeqNum + i)) {
                        std::cout << "MapMatching::UpdateStartLaneConfidence(), match current lane successed." << std::endl;
                        nLaneSeqNum = m_nLastLaneSeqNum + i;
                        return nLaneSeqNum;
                    }
                }
                if (m_nLastLaneSeqNum - i >= 1) {
                    std::vector<LaneMarkingSide> vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT};
                    if (MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, m_nLastLaneSeqNum - i)) {
                        std::cout << "MapMatching::UpdateStartLaneConfidence(), match current lane successed." << std::endl;
                        nLaneSeqNum = m_nLastLaneSeqNum - i;
                        return nLaneSeqNum;
                    }
                }
            }
        }

        return nLaneSeqNum;
    };

    // step 3
    // 尝试匹配当前车道的相邻车道
    auto func_CheckNearLink = [this](const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, int nLaneSeqNum) {
        int nLaneCnt = 0;
        m_spHdLib->GetLaneCnt(liCurrentLinkId, nLaneCnt);

        std::vector<LaneMarkingSide> vctlsCheckingPoses = {};
        // 第一车道，仅仅有右右车道可以匹配
        if (nLaneSeqNum == 1) {
            vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT};
        }
        // 最末车道，仅仅有左左车道可以匹配
        else if (nLaneSeqNum == nLaneCnt) {
            vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT};
        }
        // 中间车道，左左和右右车道都可以匹配
        else {
            vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT};
        }
        // try to match
        if (false == MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, nLaneSeqNum, false)) {
            // just reduce the confidence
            std::cout << "MapMatching::UpdateStartLaneConfidence(), match near by lane failed." << std::endl;
            m_cfConfidence.nStartLane -= 10;
        } else {
            std::cout << "MapMatching::UpdateStartLaneConfidence(), match near by lane success." << std::endl;
        }
    };

    // step 4
    // 检查角度是否匹配
    auto func_CheckAngle = [this](const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId) {
        // 车辆与车道夹角大于30度？
        if (false == vmVision.lane_markings.empty() && std::abs(vmVision.lane_markings[0].angle) <= 30) {
            // goto step 5
            return true;
        } else {
            // goto step 1
            return false;
        }
    };

    // step 5
    // 更新匹配次数
    auto func_UpdateMatchCnt = [this](bool bIsAdd) {
        static int s_nMatchCnt = 0;

        // 是否连续成功次数增加
        if (true == bIsAdd) {
            s_nMatchCnt++;
        } else {
            s_nMatchCnt = 0;
        }

        // 匹配成功
        if (s_nMatchCnt >= 1) {
            if (m_cfConfidence.nStartLane >= 90) {
                return true;
            }
        }
        // 成功次数不够
        else {
            // repeat step 1
            return false;
        }

        // 如果不返回true，则会出现明明匹配成功却输出中断的情况
        return true;
    };

    bool bIsReport = false;

    // step 1
    // 绑路
    if (false == func_BindRoad(glGnssLoc, liOutCurrentLinkId)) {
        // 绑路失败，返回
        m_cfConfidence.nStartLane = 0;

        // step 5
        bIsReport = func_UpdateMatchCnt(false);

        std::cout << "MapMatching::UpdateStartLaneConfidence(), bind road failed." << std::endl;
        return false;
    }

    // 绑路成功，初始车道置信度设置为100
    m_cfConfidence.nStartLane = 100;

    // step 2
    // 检查当前车道是否匹配成功
    int nLaneSeqNum = func_CheckCurrentLane(vmVision, liOutCurrentLinkId);
    if (0 == nLaneSeqNum) {
        std::vector<int> vctnLocateLaneSeqNums = {};
        // 匹配失败，尝试使用所在车道
        if (false == GetLocateLaneNum(glGnssLoc, liOutCurrentLinkId, vctnLocateLaneSeqNums)) {
            m_cfConfidence.nStartLane = 0;

            // step 5
            bIsReport = func_UpdateMatchCnt(false);

            std::cout << "MapMatching::UpdateStartLaneConfidence(), check current lane num failed." << std::endl;
            return false;
        }
    }
    if (1 == nLaneSeqNum) {
        // step 4
        // check angle
        if (true == func_CheckAngle(vmVision, liOutCurrentLinkId)) {
            // step 5
            bIsReport = func_UpdateMatchCnt(true);
            nOutLaneSeqNum = nLaneSeqNum;
            std::cout << "MapMatching::UpdateStartLaneConfidence(), angle has no problem." << std::endl;
            return true;
        } else {
            // step 5
            bIsReport = func_UpdateMatchCnt(false);
            std::cout << "MapMatching::UpdateStartLaneConfidence(), check angle failed." << std::endl;
            return false;
        }
    } else {
        // step 3
        // check nearby lanes
        func_CheckNearLink(vmVision, liOutCurrentLinkId, nLaneSeqNum);

        // step 4
        // check angle
        if (true == func_CheckAngle(vmVision, liOutCurrentLinkId)) {
            // step 5
            bIsReport = func_UpdateMatchCnt(true);
            nOutLaneSeqNum = nLaneSeqNum;
            std::cout << "MapMatching::UpdateStartLaneConfidence(), angle has no problem." << std::endl;
            return true;
        } else {
            // step 5
            bIsReport = func_UpdateMatchCnt(false);
            return false;
        }
    }
}

bool MapMatching::UpdateStartStatusConfidence() {
    if (m_cfConfidence.nStartLane >= 90) {
        m_cfConfidence.nStartStatus++;
    } else {
        m_cfConfidence.nStartStatus = 0;
    }

    return true;
}

bool MapMatching::UpdateHorizontalConfidence(const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, jf::HdLinkId& liOutCurrentLinkId, bool& bOutLaneChanged, int& nInOutLaneSeqNum, Point& ptOutFootLoc, Point& ptOutHorizontalLoc) {
    static jf::HdLinkId s_liLastLinkId = 0;

    // 更新横向置信度
    auto func_UpdateHorizontalValue = [this](bool bIsMatch, const jf::HdLinkId& liCurrentLinkId, int nLaneSeqNum) {
        static std::list<bool> s_lstbMatchResult = {};

        // 路口车道
        static const int sc_nLaneType_Crossing = 11;
        // 对于路口车道没有办法确认EyeQ4的识别结果是否正确，所以忽略
        int nLaneType = 0;
        m_spHdLib->GetLaneType(liCurrentLinkId, nLaneSeqNum, nLaneType);
        if (sc_nLaneType_Crossing == nLaneType) {
            return;
        }

        int nUnmatchCnt = 0;
        int nMatchCnt = 0;

        // 缓存当前结果
        s_lstbMatchResult.push_back(bIsMatch);
        // 最大5个, 较旧的抛弃
        if (s_lstbMatchResult.size() > 6) {
            s_lstbMatchResult.pop_front();
        }
        // 检查成功和失败的次数
        for (bool bIsMatch : s_lstbMatchResult) {
            if (true == bIsMatch) {
                nMatchCnt++;
            } else {
                nUnmatchCnt++;
            }
        }

        // 连续五次成功，置信度+5
        if (nMatchCnt >= 5 && m_cfConfidence.nHorizontal < 100) {
            m_cfConfidence.nHorizontal += 5;
        }

        // 连续五次失败，置信度-5
        if (nUnmatchCnt >= 5) {
            m_cfConfidence.nHorizontal -= 5;
        }
    };

    // step 1
    // 绑路
    auto func_BindRoad = [this](const IGnssLoc& glGnssLoc, jf::HdLinkId& liOutCurrentLinkId) { return StepBindRoad(glGnssLoc, liOutCurrentLinkId); };

    // step 2
    // 尝试匹配车辆所在的
    auto func_CheckCurrentLane = [this, &func_UpdateHorizontalValue](const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, const std::vector<int>& vctnLaneSeqNums, int& nOutLaneNum) {
        // 逐个匹配所给的车道
        for (int nLaneSeqNum : vctnLaneSeqNums) {
            // 所给的车道序号有问题
            int nLaneCnt = 0;
            m_spHdLib->GetLaneCnt(liCurrentLinkId, nLaneCnt);
            if (0 == nLaneSeqNum || nLaneSeqNum > nLaneCnt) {
                continue;
            }

            // 匹配当前车道的左右车道
            std::vector<LaneMarkingSide> vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT};
            if (true == MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, nLaneSeqNum)) {
                // 成功，记录一次匹配成功，注意失败次数的更新不在这里进行
                nOutLaneNum = nLaneSeqNum;
                func_UpdateHorizontalValue(true, liCurrentLinkId, nLaneSeqNum);

                return true;
            }
        }

        return false;
    };

    // step 3
    // 检查变道信息
    auto func_CheckCrossing = [this](const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, bool& bOutLaneChanged, const std::vector<int>& vctnLaneSeqNums, int& nOutLaneNum) {
        for (int nLaneSeqNum : vctnLaneSeqNums) {
            // 左变道
            if (MarkingCrossing::EDGE_CROSSING_LEFT_CROSSING == vmVision.crossing) {
                bOutLaneChanged = true;
                // 非第一车道
                if (nLaneSeqNum > 1) {
                    // 车道数-1
                    nLaneSeqNum--;
                    // goto step 2
                    nOutLaneNum = nLaneSeqNum;
                    return true;
                } else {
                    // goto start lane matching
                    continue;
                }
            }
            // 右变道
            else if (MarkingCrossing::EDGE_CROSSING_RIGHT_CROSSING == vmVision.crossing) {
                bOutLaneChanged = true;
                // vmVision.crossing = LaneCrossing::None;
                int nLaneCnt = 0;
                m_spHdLib->GetLaneCnt(liCurrentLinkId, nLaneCnt);
                // 非最末车道
                if (nLaneSeqNum < nLaneCnt) {
                    // 车道数+1
                    nLaneSeqNum++;
                    // goto step 2
                    nOutLaneNum = nLaneSeqNum;
                    return true;
                } else {
                    // goto start lane matching
                    continue;
                }
            }
            // 无变道
            else {
                int nLaneCnt = 0;
                m_spHdLib->GetLaneCnt(liCurrentLinkId, nLaneCnt);
                if (0 == nLaneSeqNum || nLaneSeqNum > nLaneCnt) {
                    continue;
                }

                return true;
            }
        }

        return false;
    };

    // step 4
    // 匹配相邻车道
    auto func_CheckNearLink = [this, &func_UpdateHorizontalValue](const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, int nLaneSeqNum) {
        static std::list<bool> s_lstbUnmatchResult = {};

        int nLaneCnt = 0;
        m_spHdLib->GetLaneCnt(liCurrentLinkId, nLaneCnt);

        bool bUnmatch = false;
        // 第一车道，仅检查右右车道
        if (nLaneSeqNum == 1) {
            std::vector<LaneMarkingSide> vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT};
            // 仅检查车道的虚实而不检查颜色
            if (false == MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, nLaneSeqNum, false)) {
                bUnmatch = true;
            }
        }
        // 最末车道，仅检查左左车道
        else if (nLaneSeqNum == nLaneCnt) {
            std::vector<LaneMarkingSide> vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT};
            // 仅检查车道的虚实而不检查颜色
            if (false == MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, nLaneSeqNum, false)) {
                bUnmatch = true;
            }
        }
        // 左左右右都检查
        else {
            std::vector<LaneMarkingSide> vctlsCheckingPoses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT};
            // 仅检查车道的虚实而不检查颜色
            if (false == MatchEdgeInfo(vmVision, liCurrentLinkId, vctlsCheckingPoses, nLaneSeqNum, false)) {
                bUnmatch = true;
            }
        }

        // 即使失败也仅仅记录一次匹配失败
        func_UpdateHorizontalValue(!bUnmatch, liCurrentLinkId, nLaneSeqNum);

        return true;
    };

    // step 5
    auto func_CheckAngle = [this](const IVisionMap& vmVision) {
        // 车辆与道路夹角需小于30度
        if (false == vmVision.lane_markings.empty() && std::abs(vmVision.lane_markings[0].angle) < 30) {
            if (m_cfConfidence.nHorizontal % 10 != 0) {
                m_cfConfidence.nHorizontal += 5;
            }
            return true;
        } else {
            // goto step 1
            return false;
        }
    };

    // get horizontal vmVision point
    auto func_GetHorizontalPoint = [this](const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, jf::HdLinkId& nInOutCurrentLinkId, int& nInOutLaneSeqNum, Point& ptOutHorizontalLoc) {
        static double s_dLastLeftLaneDis = 0.0;
        static double s_dLastRightLaneDis = 0.0;
        double dCurLeftLaneDis = 0.0;
        double dCurRightLaneDis = 0.0;

        // only the first two lane marking may be the current lane
        // 获取到左侧车道线和右侧车道线的距离
        for (int i = 0; i < 2 && i < vmVision.lane_markings.size(); i++) {
            const ILaneMarking& lmLaneMarking = vmVision.lane_markings[i];
            if ((lmLaneMarking.pos == LaneMarkingSide::LANE_MARKING_SIDE_LEFT) && (0 != lmLaneMarking.dis)) {
                dCurLeftLaneDis = std::abs(lmLaneMarking.dis);
            }
            if ((lmLaneMarking.pos == LaneMarkingSide::LANE_MARKING_SIDE_RIGHT) && (0 != lmLaneMarking.dis)) {
                dCurRightLaneDis = std::abs(lmLaneMarking.dis);
            }
        }

        // 获取车道宽度（单位毫米）
        int nLaneWidth = 0;
        m_spHdLib->GetLaneWidth(nInOutCurrentLinkId, nInOutLaneSeqNum, nLaneWidth);
        // 如果到车道线的距离大于一个车道的宽度，则认为该距离不合理
        // if (left_lane_dis * 1000 > dLaneWidht * 1.25) {
        //     left_lane_dis = last_left_lane_dis;
        // }
        // if (right_lane_dis * 1000 > dLaneWidht * 1.25) {
        //     right_lane_dis = last_right_lane_dis;
        // }

        // 判断是否有距离突变（以定位变道），如果有则记录变道，以便受到变道信息时重复变道
        IVisionMap vmTmpVision = vmVision;
        IVisionMap vmMergedVision = {};
        // 距离左边界线距离突然变大，或者距离右边界线距离突然变小
        if ((s_dLastLeftLaneDis < 0.5 && dCurLeftLaneDis * 1000 > nLaneWidth * 0.9) || (s_dLastRightLaneDis * 1000 > nLaneWidth * 0.9 && dCurRightLaneDis != 0 && dCurRightLaneDis < 0.5)) {
            vmTmpVision.crossing = MarkingCrossing::EDGE_CROSSING_LEFT_CROSSING;
            MergeVisionMapData(vmTmpVision, vmMergedVision);
            if (vmMergedVision.crossing == MarkingCrossing::EDGE_CROSSING_LEFT_CROSSING) {
                nInOutLaneSeqNum = nInOutLaneSeqNum - 1;
            }
        }
        // 距离右边界线距离突然变大，或者距离左边界线距离突然变小
        if ((s_dLastRightLaneDis < 0.5 && dCurRightLaneDis * 1000 > nLaneWidth * 0.9) || (s_dLastLeftLaneDis * 1000 > nLaneWidth * 0.9 && dCurLeftLaneDis != 0 && dCurLeftLaneDis < 0.5)) {
            vmTmpVision.crossing = MarkingCrossing::EDGE_CROSSING_RIGHT_CROSSING;
            MergeVisionMapData(vmTmpVision, vmMergedVision);
            if (vmMergedVision.crossing == MarkingCrossing::EDGE_CROSSING_RIGHT_CROSSING) {
                nInOutLaneSeqNum = nInOutLaneSeqNum + 1;
            }
        }

        // 没有识别出距离，使用上次的距离
        if (0 == dCurLeftLaneDis && s_dLastLeftLaneDis != 0) {
            dCurLeftLaneDis = s_dLastLeftLaneDis;
        }
        if (0 == dCurRightLaneDis && s_dLastRightLaneDis != 0) {
            dCurRightLaneDis = s_dLastRightLaneDis;
        }

        Point ptLeftLoc = {};
        Point ptRightLoc = {};
        if (0 != dCurLeftLaneDis) {
            // get perpendicular foot point on the left edge
            std::vector<std::vector<double>> vctvdEdgeGeolink = {};
            m_spHdLib->GetLaneEdgeGeoLink(nInOutCurrentLinkId, nInOutLaneSeqNum, true, vctvdEdgeGeolink);
            Point ptCurGnssLoc = Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z);
            Point ptFootLoc = {};
            double dDistance = 0;
            HdLibMath::GetPerpendicularFoot(ptCurGnssLoc, vctvdEdgeGeolink, ptFootLoc, dDistance);
            Point ptNearPos = {};
            double dNearDis = 0.0;
            int nNearIdx = 0;
            HdLibMath::GetNearestPointFromArray(Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, 0), vctvdEdgeGeolink, ptNearPos, dNearDis, nNearIdx);
            double dAngle = 0;
            if (false == vctvdEdgeGeolink.empty()) {
                if (nNearIdx == 0) {
                    dAngle = PointsAngle(Point(vctvdEdgeGeolink.at(0)), Point(vctvdEdgeGeolink.at(1)));
                } else {
                    dAngle = PointsAngle(Point(vctvdEdgeGeolink.at(nNearIdx - 1)), Point(vctvdEdgeGeolink.at(nNearIdx)));
                }
            } else {
                m_spHdLib->GetLinkStartAngle(nInOutCurrentLinkId, dAngle);
            }

            // 根据到车道线的距离和航向角，判断在经度和纬度方向上的距离
            double dLonDis = cos(dAngle * PI / 180) * dCurLeftLaneDis;
            double dLatDis = 0 - sin(dAngle * PI / 180) * dCurLeftLaneDis;

            // 计算从垂足起，偏移经度距离和纬度距离后的坐标
            PointDis2NewPoint(ptFootLoc, dLonDis, dLatDis, ptLeftLoc);
        }
        if (0 != dCurRightLaneDis) {
            // get perpendicular foot point on the right edge
            std::vector<std::vector<double>> vctvdEdgeGeolink = {};
            m_spHdLib->GetLaneEdgeGeoLink(nInOutCurrentLinkId, nInOutLaneSeqNum, false, vctvdEdgeGeolink);
            Point ptCurGnssLoc = Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z);
            Point ptFootLoc = {};
            double dDistance = 0;
            HdLibMath::GetPerpendicularFoot(ptCurGnssLoc, vctvdEdgeGeolink, ptFootLoc, dDistance);
            Point ptNearPos = {};
            double dNearDis = 0.0;
            int nNearIdx = 0;
            HdLibMath::GetPerpendicularFoot(Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, 0), vctvdEdgeGeolink, ptNearPos, dNearDis, nNearIdx);
            double dAngle = 0;
            if (false == vctvdEdgeGeolink.empty()) {
                if (nNearIdx == 0) {
                    dAngle = PointsAngle(Point(vctvdEdgeGeolink.at(0)), Point(vctvdEdgeGeolink.at(1)));
                } else {
                    dAngle = PointsAngle(Point(vctvdEdgeGeolink.at(nNearIdx - 1)), Point(vctvdEdgeGeolink.at(nNearIdx)));
                }
            } else {
                m_spHdLib->GetLinkStartAngle(nInOutCurrentLinkId, dAngle);
            }

            // 根据到车道线的距离和航向角，判断在经度和纬度方向上的距离
            double dLonDis = 0 - cos(dAngle * PI / 180) * dCurRightLaneDis;
            double dLatDis = sin(dAngle * PI / 180) * dCurRightLaneDis;

            // 计算从垂足起，偏移经度距离和纬度距离后的坐标
            PointDis2NewPoint(ptFootLoc, dLonDis, dLatDis, ptRightLoc);
        }

        // 将左侧距离计算出的坐标和右侧距离计算出的坐标取平均
        if (dCurLeftLaneDis != 0 && dCurRightLaneDis != 0) {
            ptOutHorizontalLoc.dLon = (ptLeftLoc.dLon + ptRightLoc.dLon) / 2;
            ptOutHorizontalLoc.dLat = (ptLeftLoc.dLat + ptRightLoc.dLat) / 2;
        } else if (dCurLeftLaneDis != 0) {
            ptOutHorizontalLoc = ptLeftLoc;
        } else if (dCurRightLaneDis != 0) {
            ptOutHorizontalLoc = ptRightLoc;
        } else {
            ptOutHorizontalLoc = Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z);
        }

        // 更新距离以做下次之用
        if (0 != dCurLeftLaneDis) {
            s_dLastLeftLaneDis = dCurLeftLaneDis;
        }
        if (0 != dCurRightLaneDis) {
            s_dLastRightLaneDis = dCurRightLaneDis;
        }
    };

    // 计算Eyeq4识别出的左侧和右侧距离的坐标
    auto func_GetVisionDis = [](const IVisionMap& vmVision, double& dOutLeftDis, double& dOutRightDis) {
        for (int i = 0; i < 2 && i < vmVision.lane_markings.size(); ++i) {
            if (vmVision.lane_markings.at(i).pos == LaneMarkingSide::LANE_MARKING_SIDE_LEFT) {
                dOutLeftDis = std::abs(vmVision.lane_markings.at(i).dis) * 1000;
            } else if (vmVision.lane_markings.at(i).pos == LaneMarkingSide::LANE_MARKING_SIDE_RIGHT) {
                dOutRightDis = std::abs(vmVision.lane_markings.at(i).dis) * 1000;
            }
        }
    };

    auto func_GetLaneNext = [this](const HdLinkId& lPreLinkId, const HdLinkId& lCurLinkId, int nPreLaneSeqNum, std::vector<int>& vctnOutNSeqNums) {
        const HdLink* cphlPreLink = m_spHdLib->GetLinkData(lPreLinkId);
        const HdLink* cphlCurLink = m_spHdLib->GetLinkData(lCurLinkId);
        if ((cphlPreLink != NULL) && (cphlCurLink != NULL) && (nPreLaneSeqNum >= 1 && nPreLaneSeqNum <= cphlPreLink->vcthlLane.size())) {
            HdLane hlPreLane = cphlPreLink->vcthlLane.at(nPreLaneSeqNum - 1);
            for (int i = 0; i < hlPreLane.vctliNxtLaneId.size(); i++) {
                for (int j = 0; j < cphlCurLink->vcthlLane.size(); j++) {
                    if (hlPreLane.vctliNxtLaneId.at(i).lValue == cphlCurLink->vcthlLane.at(j).liId.lValue) {
                        vctnOutNSeqNums.push_back(j + 1);
                    }
                }
            }
        }

        if (false == vctnOutNSeqNums.empty()) {
            // 去重
            sort(vctnOutNSeqNums.begin(), vctnOutNSeqNums.end());
            vctnOutNSeqNums.erase(unique(vctnOutNSeqNums.begin(), vctnOutNSeqNums.end()), vctnOutNSeqNums.end());
            return true;
        }

        return false;
    };

    // step 1
    jf::HdLinkId liCurrentLinkId = {};
    if (false == func_BindRoad(glGnssLoc, liCurrentLinkId)) {
        // 即使失败也不返回，使用坐标点所在link和车道
        // return false;
    }

    // get locate lanes
    // 如果失败，则使用绑路结果
    std::vector<int> vctnLocateLaneSeqNums = {};
    if (false == GetLocateLaneNum(glGnssLoc, liCurrentLinkId, vctnLocateLaneSeqNums)) {
        return false;
    }

    // get connect lanes
    m_nLastLaneSeqNum = nInOutLaneSeqNum != 0 ? nInOutLaneSeqNum : !vctnLocateLaneSeqNums.empty() ? vctnLocateLaneSeqNums.at(0) : 1;
    std::vector<int> vctnConnectLaneSeqNums = {};
    // link有变化，计算和上次匹配车道相连接的车道
    if (s_liLastLinkId != liCurrentLinkId) {
        if (false == func_GetLaneNext(s_liLastLinkId, liCurrentLinkId, m_nLastLaneSeqNum, vctnConnectLaneSeqNums)) {
            // 长时间EyeQ4中断，已经没有办法判断连接关系，采用GNSS所在车道
            // int nLaneCnt = 0;
            // map_lib_.GetLaneCnt(liCurrentLinkId, nLaneCnt);
            // if (nLaneCnt < last_lane_num_) {
            //     last_lane_num_ = nLaneCnt;
            // }
            vctnConnectLaneSeqNums.push_back(vctnLocateLaneSeqNums.at(0));
        } else {
            // 如果有多个选择，优先选择与车辆航向角偏差最小的。
            if (vctnConnectLaneSeqNums.size() >= 2) {
                for (int i = 0; i < vctnConnectLaneSeqNums.size(); i++) {
                    for (int j = i + 1; j < vctnConnectLaneSeqNums.size(); j++) {
                        std::vector<std::vector<double>> vctvdGeoLinkI = {};
                        std::vector<std::vector<double>> vctvdGeoLinkJ = {};
                        m_spHdLib->GetLaneGeoLink(liCurrentLinkId, vctnConnectLaneSeqNums.at(i), vctvdGeoLinkI);
                        m_spHdLib->GetLaneGeoLink(liCurrentLinkId, vctnConnectLaneSeqNums.at(j), vctvdGeoLinkJ);
                        Point ptNearPos = {};
                        double dNearDis = 0.0;
                        int nNearIdx = 0;
                        HdLibMath::GetPerpendicularFoot(Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, 0), vctvdGeoLinkI, ptNearPos, dNearDis, nNearIdx);
                        double dStartAngleI = 0;
                        if (nNearIdx == 0) {
                            dStartAngleI = PointsAngle(Point(vctvdGeoLinkI.at(0)), Point(vctvdGeoLinkI.at(1)));
                        } else {
                            dStartAngleI = PointsAngle(Point(vctvdGeoLinkI.at(nNearIdx - 1)), Point(vctvdGeoLinkI.at(nNearIdx)));
                        }
                        HdLibMath::GetPerpendicularFoot(Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, 0), vctvdGeoLinkJ, ptNearPos, dNearDis, nNearIdx);
                        double dStartAngleJ = 0;
                        if (nNearIdx == 0) {
                            dStartAngleJ = PointsAngle(Point(vctvdGeoLinkJ.at(0)), Point(vctvdGeoLinkJ.at(1)));
                        } else {
                            dStartAngleJ = PointsAngle(Point(vctvdGeoLinkJ.at(nNearIdx - 1)), Point(vctvdGeoLinkJ.at(nNearIdx)));
                        }
                        double dAngleSubI = AngleSub(glGnssLoc.heading, dStartAngleI);
                        double dAngleSubJ = AngleSub(glGnssLoc.heading, dStartAngleJ);
                        if (dAngleSubI > dAngleSubJ) {
                            int nTempLaneSeqNum = vctnConnectLaneSeqNums[i];
                            vctnConnectLaneSeqNums[i] = vctnConnectLaneSeqNums[j];
                            vctnConnectLaneSeqNums[j] = nTempLaneSeqNum;
                        }
                    }
                }
            } else {
                // do nothing
            }
        }
        m_nLastLaneSeqNum = vctnConnectLaneSeqNums.at(vctnConnectLaneSeqNums.size() - 1);
    } else {
        vctnConnectLaneSeqNums.push_back(m_nLastLaneSeqNum);
    };
    s_liLastLinkId = liCurrentLinkId;

    // step 2
    // 匹配当前车道
    // 尝试使用相连接车道匹配
    if (false == func_CheckCurrentLane(vmVision, liCurrentLinkId, vctnConnectLaneSeqNums, nInOutLaneSeqNum)) {
        // 尝试使用坐标点所在车道匹配
        if (false == func_CheckCurrentLane(vmVision, liCurrentLinkId, vctnLocateLaneSeqNums, nInOutLaneSeqNum)) {
            // 两次尝试均失败，则使用上次匹配结果
            // nInOutLaneSeqNum = !vctnLocateLaneSeqNums.empty() ? vctnLocateLaneSeqNums.at(0) : last_lane_num_;
            nInOutLaneSeqNum = m_nLastLaneSeqNum;
            func_UpdateHorizontalValue(false, liCurrentLinkId, m_nLastLaneSeqNum);
        }
    }

    // 根据车道变化已经判断出了变道，记录，以防止重复计算
    // 向左变道
    if (nInOutLaneSeqNum == m_nLastLaneSeqNum - 1) {
        IVisionMap vmTmpVision = vmVision;
        IVisionMap vmMergedVision = {};
        vmTmpVision.crossing = MarkingCrossing::EDGE_CROSSING_LEFT_CROSSING;
        MergeVisionMapData(vmTmpVision, vmMergedVision);
    }
    // 向右变道
    else if (nInOutLaneSeqNum == m_nLastLaneSeqNum + 1) {
        IVisionMap vmTmpVision = vmVision;
        IVisionMap vmMergedVision = {};
        vmTmpVision.crossing = MarkingCrossing::EDGE_CROSSING_RIGHT_CROSSING;
        MergeVisionMapData(vmTmpVision, vmMergedVision);
    }

    // step 3
    // 注意可能有一次变道，发送多次变道信息的情况，此处做了过滤
    IVisionMap vmMergedVision = {};
    MergeVisionMapData(vmVision, vmMergedVision);
    //尝试使用相连车道
    if (false == func_CheckCrossing(vmMergedVision, liCurrentLinkId, bOutLaneChanged, vctnConnectLaneSeqNums, nInOutLaneSeqNum)) {
        // 尝试使用坐标点所在车道
        if (false == func_CheckCrossing(vmMergedVision, liCurrentLinkId, bOutLaneChanged, vctnLocateLaneSeqNums, nInOutLaneSeqNum)) {
            // goto start lane matching
            std::cout << "MapMatching::UpdateHorizontalConfidence(), check crossing failed, goto start lane matching." << std::endl;
            return false;
        }
    }

    // step 4
    // 检查相邻车道
    if (false == func_CheckNearLink(vmVision, liCurrentLinkId, nInOutLaneSeqNum)) {
        std::cout << "MapMatching::UpdateHorizontalConfidence(), match nearby lane failed, use last result." << std::endl;
    }

    // step 5
    // 检查车辆与道路夹角
    if (false == func_CheckAngle(vmVision)) {
        std::cout << "MapMatching::UpdateHorizontalConfidence(), match nearby lane failed, use last result." << std::endl;
    }

    static const int sc_nFrequently = 10;
    static const int sc_nUpdateTime = 3;  // seconds
    static int s_nLowAccuracyCnt = 0;
    // 检查低置信度是否过多
    if (m_cfConfidence.nHorizontal < 50) {
        s_nLowAccuracyCnt++;
        if (s_nLowAccuracyCnt >= sc_nFrequently * sc_nUpdateTime / 6) {
            // goto start lane matching
            std::cout << "MapMatching::UpdateHorizontalConfidence(), confidence is too low, goto start lane matching." << std::endl;
            return false;
        }
    } else {
        s_nLowAccuracyCnt = 0;
    }

    // std::cout << "MapMatching::UpdateHorizontalConfidence(), current horizontal confidence: " << confidence_.horizontal << std::endl;

    // 计算垂足
    std::vector<std::vector<double>> vctvdLaneGeolink = {};
    m_spHdLib->GetLaneGeoLink(liCurrentLinkId, nInOutLaneSeqNum, vctvdLaneGeolink);
    Point ptCurGnssLoc = Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z);
    Point ptFootLoc = {};
    double dDistance = 0;
    HdLibMath::GetPerpendicularFoot(ptCurGnssLoc, vctvdLaneGeolink, ptOutFootLoc, dDistance);

    // 根据到车道线的距离计算横向坐标
    func_GetHorizontalPoint(glGnssLoc, vmVision, liCurrentLinkId, nInOutLaneSeqNum, ptOutHorizontalLoc);

    // output
    m_nLastLaneSeqNum = nInOutLaneSeqNum;
    liOutCurrentLinkId = liCurrentLinkId;

    return true;
}

bool MapMatching::UpdateVerticalConfidence(const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, Point& ptOutVisionPoint) {
    // update confidence
    auto func_UpdateVerticalValue = [this](bool bIsAdd) {
        if (bIsAdd == true && m_cfConfidence.nVertical < 100) {
            m_cfConfidence.nVertical += 5;
        }
        if (bIsAdd == false && m_cfConfidence.nVertical > 0) {
            m_cfConfidence.nVertical -= 5;
        }
    };

    // 所给的字符串是否是数字
    auto func_IsNum = [](std::string strInput) {
        for (int i = 0; i < strInput.size(); i++) {
            int tmp = (int)strInput[i];
            if (tmp >= 48 && tmp <= 57) {
                continue;
            } else {
                return false;
            }
        }
        return true;
    };

    // 计算纵向定位点的位置
    auto func_GetLoc = [this, vmVision, liCurrentLinkId, glGnssLoc](const ITrafsignMap& tsrVisionTsr, const Point& inTsrPoint, Point& ptOutVisionPoint) {
        // update location
        // 车辆左边为正，右边为负
        double dStartAngle = 0;
        m_spHdLib->GetLinkStartAngle(liCurrentLinkId, dStartAngle);
        double dAngle = dStartAngle * PI / 180;

        // 根据到标牌的距离和航向角计算经度方向上的偏移和纬度方向上的偏移
        double dLonDis = (tsrVisionTsr.lat_dis - tsrVisionTsr.lon_dis / tan(dAngle)) * sin(dAngle);
        double dLatDis = tsrVisionTsr.lon_dis / sin(dAngle) + (tsrVisionTsr.lat_dis - tsrVisionTsr.lon_dis / tan(dAngle)) * cos(dAngle);

        // 根据从标牌坐标的偏移计算车辆坐标
        PointDis2NewPoint(inTsrPoint, -dLonDis, -dLatDis, ptOutVisionPoint);

        // 返回计算出的坐标到原始GNSS的距离
        return PointsDis(ptOutVisionPoint, Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z));
    };

    // step 1
    // 检查GNSS置信度
    auto func_CheckAccuracy = [this, &func_UpdateVerticalValue](const IGnssLoc& glGnssLoc) {
        if (glGnssLoc.coefficient > 0.7) {
            func_UpdateVerticalValue(true);
        } else {
            func_UpdateVerticalValue(false);
        }
    };

    // step 2
    // 检查TSR
    auto func_CheckTsr = [this, &func_GetLoc, &func_UpdateVerticalValue, &func_IsNum](const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, Point& ptOutVisionPoint) {
        // 没有检测到标牌
        if (true == vmVision.traffic_signs.empty()) {
            return true;
        }

        // just one tranffic sign will be used
        const ITrafsignMap& tmTsr = vmVision.traffic_signs.at(0);

        // 获取坐标附近的标牌
        std::vector<int> vctnShapes = {};
        std::vector<int> vctnMainClasses = {};
        std::vector<int> vctnSubClasses = {};
        std::vector<std::string> vctstValues = {};
        std::vector<std::vector<std::vector<double>>> vctvvdGeometries = {};
        Point ptCurLoc = Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z);
        m_spHdLib->GetTrafficSign(ptCurLoc, vctnShapes, vctnMainClasses, vctnSubClasses, vctstValues, vctvvdGeometries);

        if (vctnMainClasses.size() > 0) {
            double dMinDis = std::numeric_limits<double>::max();
            for (int i = 0; i < vctnMainClasses.size(); i++) {
                // 标牌匹配
                // if ((mainClasses[i] == (int)tsr.traffic_sign_class) && (subClasses[i] == tsr.traffic_sign_sub_class) && (atoi(values[i].c_str()) == tsr.speed_limit_value)) {
                if (func_IsNum(vctstValues[i]) && (atoi(vctstValues[i].c_str()) == tmTsr.speed_limit_value)) {
                    Point ptOutPoint = {};
                    if ((vctvvdGeometries[i].size() >= 1) && (vctvvdGeometries[i][0].size() >= 3)) {
                        // 可能有多个匹配结果，取结果与原始GNSS最近的
                        double dCurDis = func_GetLoc(tmTsr, Point(vctvvdGeometries[i][0][0], vctvvdGeometries[i][0][1], vctvvdGeometries[i][0][2]), ptOutPoint);
                        if (dMinDis > dCurDis) {
                            dMinDis = dCurDis;
                            ptOutVisionPoint = ptOutPoint;
                        }
                    }
                }
            }

            // 更新置信度
            func_UpdateVerticalValue(true);
            return true;
        }

        // 更新置信度
        func_UpdateVerticalValue(false);
        return true;
    };

    func_CheckAccuracy(glGnssLoc);
    func_CheckTsr(glGnssLoc, vmVision, liCurrentLinkId, ptOutVisionPoint);

    // std::cout << "MapMatching::UpdateVerticalConfidence(), current vertical confidence: " << confidence_.vertical << std::endl;

    return true;
}

bool MapMatching::LocateByGnss(const Point& ptCurLoc, jf::HdLinkId& liOutCurrentLinkId, int& nOutLaneNum) {
    std::vector<jf::HdLinkId> vctliLocateLinkId = {};
    std::vector<jf::HdLaneId> vctliLocateLaneId = {};
    m_spHdLib->GetLocateLanes(ptCurLoc, vctliLocateLinkId, vctliLocateLaneId);
    // more than one lane may be located
    for (const jf::HdLinkId& liLocateLinkId : vctliLocateLinkId) {
        int nLaneCnt = 0;
        m_spHdLib->GetLaneCnt(liLocateLinkId, nLaneCnt);
        for (int i = 1; i <= nLaneCnt; i++) {
            jf::HdLaneId liLaneId = {};
            m_spHdLib->GetLaneId(liLocateLinkId, i, liLaneId);
            for (jf::HdLaneId liLocateLaneId : vctliLocateLaneId) {
                if (liLocateLaneId == liLaneId) {
                    liOutCurrentLinkId = liLocateLinkId;
                    nOutLaneNum = i;
                    return true;
                }
            }
        }
    }

    std::cout << "MapMatching::LocateByGnss(), failed, current lon: " << ptCurLoc.dLon << ", lat: " << ptCurLoc.dLat << ", alt: " << ptCurLoc.dAlt << std::endl;
    return false;
}

bool MapMatching::StepBindRoad(const IGnssLoc& glGnssLoc, jf::HdLinkId& liOutCurrentLinkId) {
    BindRoadOutput boOutput(1);
    BindRoadOutput::error beError = BindRoadOutput::error::no_error;

    // 绑路
    m_spBindRoad->BindRoadHmm({Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z)}, boOutput);
    if (boOutput.e != BindRoadOutput::no_error && boOutput.e != BindRoadOutput::pending_after_init) {
        beError = std::max(beError, BindRoadOutput::error::no_candidates_in_init);
        std::cout << "MapMatching::StepBindRoad(), HmmBindRoad" << std::endl;
        return false;
    }

    // 获取附近数据
    const Point& bindroad_output_gnss = boOutput.nodeInfos[0].outp;
    std::vector<jf::HdLinkId> outNearbyLinks = {};
    std::vector<const HdLink*> vcthlOutLinkPtr;
    if (!m_spHdLib->GetDataNearby(bindroad_output_gnss,
                                  40,  // 标牌测距需要前方80米的
                                  40, vcthlOutLinkPtr)) {
        beError = std::max(beError, BindRoadOutput::error::no_candidates_in_init);
        std::cout << "MapMatching::StepBindRoad(), GetDataNearby" << std::endl;
        return false;
    }

    // 两者批评过滤
    bool foundCurrentLinkId = false;
    for (const HdLink* hdLinkptr : vcthlOutLinkPtr) {
        if (hdLinkptr->liId.lValue == boOutput.nodeInfos[0].rid) {
            liOutCurrentLinkId = hdLinkptr->liId;
            foundCurrentLinkId = true;
            break;
        }
    }
    if (!foundCurrentLinkId) {
        beError = std::max(beError, BindRoadOutput::error::no_candidates_in_init);
        std::cout << "MapMatching::StepBindRoad(), foundCurrentLinkId" << std::endl;
        return false;
    }

    return true;
}

void MapMatching::GetEdgeFromMap(const jf::HdLinkId& liCurrentLinkId, int nLaneSeqNum, int& outLeftType, int& outRightType, int& outLeftColor, int& outRightColor) {
    jf::HdEdgeId left_id = 0;
    jf::HdEdgeId right_id = 0;

    // get edge id
    m_spHdLib->GetLaneEdgeId(liCurrentLinkId, nLaneSeqNum, true, left_id);
    m_spHdLib->GetLaneEdgeId(liCurrentLinkId, nLaneSeqNum, false, right_id);

    // get edge cross
    m_spHdLib->GetEdgeCross(liCurrentLinkId, left_id, outLeftType);
    m_spHdLib->GetEdgeCross(liCurrentLinkId, right_id, outRightType);

    // edge color
    m_spHdLib->GetEdgeColor(liCurrentLinkId, left_id, outLeftColor);
    m_spHdLib->GetEdgeColor(liCurrentLinkId, right_id, outRightColor);

    return;
}

bool MapMatching::MatchEdgeInfo(const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, const std::vector<LaneMarkingSide>& vctlsCheckingPoses, int nLaneSeqNum, bool bIsStrongMatch /* = true */) {
    // 检查跨越属性
    auto func_CheckCross = [](HMC::LaneMarkingType ltVisionCross) {
        if (ltVisionCross != HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_NONE) {
            return true;
        }

        return false;
    };

    // 检查颜色属性
    auto func_CheckColor = [](HMC::LaneMarkingColor ltVisionColor) {
        if (ltVisionColor != HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_OTHERS) {
            return true;
        }

        return false;
    };

    // convert cross type
    auto func_ConvertCross = [](HMC::LaneMarkingType ltVisionCross, bool bIsLeft) {
        int nEdgeCross = 0;
        switch (ltVisionCross) {
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_SINGLE_DASHED:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_DOUBLE_DASHED:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_MIXED_SINGLE_DASHED:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_MIXED_DOUBLE_DASHED:
                nEdgeCross = 2;  // dashed line
                break;
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_SINGLE_SOLID:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_DOUBLE_SOLID:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_MIXED_SINGLE_SOLID:
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_MIXED_DOUBLE_SOLID:
                nEdgeCross = 1;  // solid line
                break;
            case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_SHORT_THICK_DASHED:
                nEdgeCross = 3;
                break;
            // case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_RIGHT_SOLID_LEFT_DASHED:
            //     if (true == bIsLeft) {
            //         nEdgeCross = 2;  // dashed line
            //     } else {
            //         nEdgeCross = 1;  // solid line
            //     }
            //     break;
            // case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_LEFT_SOLID_RIGHT_DASHED:
            //     if (true == bIsLeft) {
            //         nEdgeCross = 1;  // solid line
            //     } else {
            //         nEdgeCross = 2;  // dashed line
            //     }
            //     break;
            default:
                nEdgeCross = 0;
                break;
        }
        return nEdgeCross;
    };

    // convert color value
    auto func_ConvertColor = [](HMC::LaneMarkingColor ltVisionColor) {
        int nEdgeColor = 0;
        switch (ltVisionColor) {
            case HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_WHITE:
                nEdgeColor = 1;  // white
                break;
            case HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_YELLOW:
                nEdgeColor = 2;  // yellow
                break;
            case HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_RAD:
                nEdgeColor = 3;  // red
                break;
            case HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_BLUE:
                nEdgeColor = 4;  // blue
                break;
            case HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_GREEN:
                nEdgeColor = 5;  // green
                break;
            default:
                nEdgeColor = 1;  // white
                break;
        }
        return nEdgeColor;
    };

    // One edge matching
    auto fucn_MatchOnePos = [this, &func_CheckCross, &func_CheckColor, &func_ConvertCross, &func_ConvertColor](const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, LaneMarkingSide lsPos, int nLaneSeqNum, bool bIstrongMatch /* = true */) {
        // Get edge info by pos
        HMC::LaneMarkingColor ltVisionColor = HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_WHITE;
        HMC::LaneMarkingType ltVisionCross = HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_NOT_INVESTIGATED;
        float64_t dVisionProb = 0;
        bool bIsFound = false;
        for (ILaneMarking lmLaneMarking : vmVision.lane_markings) {
            if (lmLaneMarking.pos == lsPos) {
                ltVisionColor = lmLaneMarking.lane_marking_color;
                ltVisionCross = lmLaneMarking.cross_type;
                dVisionProb = lmLaneMarking.exist_prob;
                bIsFound = true;
                break;
            }
        }

        bool bMatched = true;
        if (false == bIsFound || dVisionProb <= 80) {
            // not found, ignore
            return bMatched;
        }

        // Edge info by lane sequence number
        int nLeftCross = 0;
        int nLeftColor = 0;
        int nRightCross = 0;
        int nRightColor = 0;

        // 根据车道位置，获取相应的地图中的车道的虚实和颜色
        bool bIsLeft = true;
        int nMapCross = 0;
        int nMapColor = 0;
        switch (lsPos) {
            case LaneMarkingSide::LANE_MARKING_SIDE_LEFT:
                GetEdgeFromMap(liCurrentLinkId, nLaneSeqNum, nLeftCross, nRightCross, nLeftColor, nRightColor);
                bIsLeft = true;
                nMapCross = nLeftCross;
                nMapColor = nLeftColor;
                break;
            case LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT:
                GetEdgeFromMap(liCurrentLinkId, nLaneSeqNum - 1, nLeftCross, nRightCross, nLeftColor, nRightColor);
                bIsLeft = true;
                nMapCross = nLeftCross;
                nMapColor = nLeftColor;
                break;
            case LaneMarkingSide::LANE_MARKING_SIDE_RIGHT:
                GetEdgeFromMap(liCurrentLinkId, nLaneSeqNum, nLeftCross, nRightCross, nLeftColor, nRightColor);
                bIsLeft = false;
                nMapCross = nRightCross;
                nMapColor = nRightColor;
                break;
            case LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT:
                GetEdgeFromMap(liCurrentLinkId, nLaneSeqNum + 1, nLeftCross, nRightCross, nLeftColor, nRightColor);
                bIsLeft = false;
                nMapCross = nRightCross;
                nMapColor = nRightColor;
                break;
            default:
                break;
        };

        // 检查是否匹配
        if (true == func_CheckCross(ltVisionCross)) {
            // cross
            if (nMapCross == func_ConvertCross(ltVisionCross, bIsLeft)) {
                bMatched &= true;
            }
            // 存在eyeq4将短粗虚线误识别为虚线的情况，允许匹配通过
            else if ((3 == nMapCross) && (2 == func_ConvertCross(ltVisionCross, bIsLeft))) {
                bMatched &= true;
            }
            //
            else {
                bMatched &= false;
            }
        } else {
            bMatched &= true;
        }

        // Is matched: both color and cross should be matched
        if (true == bIstrongMatch) {
            // color
            if (true == func_CheckColor(ltVisionColor)) {
                if (nMapColor == func_ConvertColor(ltVisionColor)) {
                    bMatched &= true;
                }
                // 有可能地图中没有颜色选项，默认匹配
                else if (nMapColor == 0) {
                    bMatched &= true;
                } else {
                    bMatched &= false;
                }
            } else {
                bMatched &= true;
            }
        }

        return bMatched;
    };

    // 逐个批评所给的匹配位置
    for (LaneMarkingSide pos : vctlsCheckingPoses) {
        if (false == fucn_MatchOnePos(vmVision, liCurrentLinkId, pos, nLaneSeqNum, bIsStrongMatch)) {
            return false;
        }
    }

    return true;
}

void MapMatching::MakeMatchResult(const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, bool bIsLaneChanged, int nLaneSeqNum, const Point& ptFootLoc, const Point& ptVisionHorizontalLoc,
                                  const Point& ptVisionVerticalLoc, IMatchResult& mrOutMatchResult) {
    // 获取距离车道边界线的角度
    auto func_GetLaneMarkingAngle = [](const IVisionMap& vmVision) {
        for (int i = 0; i < 2 && i < vmVision.lane_markings.size(); i++) {
            const ILaneMarking& lane_marking = vmVision.lane_markings[i];
            if (0 != lane_marking.angle) {
                return lane_marking.angle;
            }
        }

        return (float)0.0;
    };

    // 获取距离车道边界线的距离
    auto func_GetLaneMarkingDis = [this](const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, int nLaneSeqNum, bool bIsLeft) {
        // try to get by vmVision
        for (int i = 0; i < 2 && i < vmVision.lane_markings.size(); i++) {
            const ILaneMarking& lmLaneMarking = vmVision.lane_markings[i];
            if ((true == bIsLeft) && (LaneMarkingSide::LANE_MARKING_SIDE_LEFT == lmLaneMarking.pos)) {
                return lmLaneMarking.dis;
            } else if ((false == bIsLeft) && (LaneMarkingSide::LANE_MARKING_SIDE_RIGHT == lmLaneMarking.pos)) {
                return lmLaneMarking.dis;
            }
        }

        // try to get by gnss
        std::vector<std::vector<double>> vctvdEdgeGeolink = {};
        double dDistance = 0;
        if (liCurrentLinkId.lValue != 0 && nLaneSeqNum != 0) {
            m_spHdLib->GetLaneEdgeGeoLink(liCurrentLinkId, nLaneSeqNum, bIsLeft, vctvdEdgeGeolink);
            Point ptCurLoc = Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z);
            Point ptFootLoc = {};
            HdLibMath::GetPerpendicularFoot(ptCurLoc, vctvdEdgeGeolink, ptFootLoc, dDistance);
        }
        return dDistance;
    };

    // make data
    mrOutMatchResult.timestamp = vmVision.timestamp;
    mrOutMatchResult.change_lanes = bIsLaneChanged;
    mrOutMatchResult.lane_seq_no = nLaneSeqNum;
    if (liCurrentLinkId.lValue != 0) {
        m_spHdLib->GetLaneCnt(liCurrentLinkId, (int&)mrOutMatchResult.lane_num);
        m_spHdLib->GetLinkType(liCurrentLinkId, (int&)mrOutMatchResult.road_type);
    }
    mrOutMatchResult.angle_with_lane = func_GetLaneMarkingAngle(vmVision);
    //    if (liCurrentLinkId.lValue != 0) {
    //        GetSpecialRoadDis(Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z), liCurrentLinkId, mrOutMatchResult.special_type_begin_dis, mrOutMatchResult.special_type_end_dis, (int32_t&)mrOutMatchResult.special_type);
    //    }
    int lane_type = 0;
    if (liCurrentLinkId.lValue != 0 && nLaneSeqNum != 0) {
        m_spHdLib->GetLaneType(liCurrentLinkId, nLaneSeqNum, lane_type);
    }
    static const int sc_lane_type_crossing = 11;
    mrOutMatchResult.in_road_crossing = lane_type == sc_lane_type_crossing ? true : false;
    mrOutMatchResult.left_lane_edge_dis = func_GetLaneMarkingDis(glGnssLoc, vmVision, liCurrentLinkId, nLaneSeqNum, true);
    mrOutMatchResult.right_lane_edge_dis = func_GetLaneMarkingDis(glGnssLoc, vmVision, liCurrentLinkId, nLaneSeqNum, false);
    mrOutMatchResult.foot_point.x = ptFootLoc.dLon;
    mrOutMatchResult.foot_point.y = ptFootLoc.dLat;
    mrOutMatchResult.foot_point.z = ptFootLoc.dAlt;
    mrOutMatchResult.foot_prob = glGnssLoc.coefficient;
    mrOutMatchResult.vision_horizontal_point.x = ptVisionHorizontalLoc.dLon;
    mrOutMatchResult.vision_horizontal_point.y = ptVisionHorizontalLoc.dLat;
    mrOutMatchResult.vision_horizontal_point.z = ptVisionHorizontalLoc.dAlt;
    mrOutMatchResult.horizal_prob = m_cfConfidence.nHorizontal;
    // if (vmVision.lane_markings.size() >= 2) {
    //     outGnssFoot.horizal_prob = sqrt(pow(vmVision.lane_markings[0].exist_prob, 2) + pow(vmVision.lane_markings[1].exist_prob, 2));
    // } else if (vmVision.lane_markings.size() == 1) {
    //     outGnssFoot.horizal_prob = vmVision.lane_markings[0].exist_prob;
    // } else {
    //     outGnssFoot.horizal_prob = 0;
    // }
    mrOutMatchResult.vision_vertical_point.x = ptVisionVerticalLoc.dLon;
    mrOutMatchResult.vision_vertical_point.y = ptVisionVerticalLoc.dLat;
    mrOutMatchResult.vision_vertical_point.z = ptVisionVerticalLoc.dAlt;
    // float32_t mid_prob = 0;
    // for (auto tsr : vmVision.traffic_signs) {
    //     mid_prob += pow(tsr.exist_prob, 2);
    // }
    // outGnssFoot.vertical_prob = sqrt(mid_prob);
    mrOutMatchResult.vertical_prob = m_cfConfidence.nVertical;
    mrOutMatchResult.gnss_time = glGnssLoc.gnss_time;
    mrOutMatchResult.gnss_point = glGnssLoc.gnss_point;                         // lon, lat, alt
    mrOutMatchResult.gnss_heading = glGnssLoc.heading;                          // heading angle
    mrOutMatchResult.gnss_speed = glGnssLoc.speed;                              // vehicle speed
    mrOutMatchResult.gnss_longitude_deviation = glGnssLoc.longitude_deviation;  // standard deviation of position of longitude in WGS84 coordinate system, m
    mrOutMatchResult.gnss_latitude_deviation = glGnssLoc.latitude_deviation;    // standard deviation of position of latitude in WGS84 coordinate system, m
    mrOutMatchResult.gnss_height_deviation = glGnssLoc.height_deviation;        // standard deviation of position of height in WGS84 coordinate system, m

    return;
}

bool MapMatching::GetLocateLaneNum(const IGnssLoc& glGnssLoc, jf::HdLinkId& liInOutCurrentLinkId, std::vector<int>& vctnOutLocateLaneSeqNum) {
    auto func_GetLaneSeqNum = [this](const HdLinkId& liLinkId, const HdLaneId& liLaneId, int& nOutLaneSeqNum) {
        const HdLink* cphlLink = m_spHdLib->GetLinkData(liLinkId);
        if (cphlLink == nullptr) return false;

        for (const HdLane& hlLane : cphlLink->vcthlLane) {
            if (hlLane.liId == liLaneId) {
                nOutLaneSeqNum = hlLane.nSeqNum;
                break;
            }
        }
        return true;
    };
    // 获取当前Link的车道数量
    auto func_GetLaneNum = [this, &func_GetLaneSeqNum](const jf::HdLinkId& liCurrentLinkId, const jf::HdLaneId& liCurrentLaneId, std::vector<int>& vctnOutLocateLaneSeqNum) {
        int nLaneSeqNum = 0;
        if (true == func_GetLaneSeqNum(liCurrentLinkId, liCurrentLaneId, nLaneSeqNum)) {
            vctnOutLocateLaneSeqNum.push_back(nLaneSeqNum);
        }
    };

    // 记录上次结果，以便出现多个选项时过滤
    static jf::HdLinkId s_liLastLinkId = liInOutCurrentLinkId;

    Point ptCurLoc = Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, glGnssLoc.gnss_point.z);
    std::vector<jf::HdLinkId> vctliLocateLinks = {};
    std::vector<jf::HdLaneId> vctliLocateLanes = {};
    // try to update lane num with locate gnss if gnss is credible
    if (true == m_spHdLib->GetLocateLanes(ptCurLoc, vctliLocateLinks, vctliLocateLanes)) {
        if (vctliLocateLinks.size() >= 2) {
            std::vector<jf::HdLinkId> vctliUsableLinks = {};
            std::vector<jf::HdLaneId> vctliUsableLanes = {};
            // 优先选择上次结果
            for (int i = 0; i < vctliLocateLinks.size(); ++i) {
                if (s_liLastLinkId == vctliLocateLinks.at(i)) {
                    vctliUsableLinks.push_back(vctliLocateLinks.at(i));
                    vctliUsableLanes.push_back(vctliLocateLanes.at(i));
                    break;
                }
            }
            // 选择上次link相连接的结果
            if (vctnOutLocateLaneSeqNum.empty()) {
                std::vector<jf::HdLinkId> vctliNextLinks = {};
                m_spHdLib->GetLinkNext(s_liLastLinkId, vctliNextLinks);
                for (int i = 0; i < vctliLocateLinks.size(); ++i) {
                    for (int j = 0; j < vctliNextLinks.size(); j++) {
                        if (vctliNextLinks.at(j) == vctliLocateLinks.at(i)) {
                            vctliUsableLinks.push_back(vctliLocateLinks.at(i));
                            vctliUsableLanes.push_back(vctliLocateLanes.at(i));
                        }
                    }
                }
            }

            // 如果有多个符合条件的，选择一个最近的
            if (vctliUsableLinks.size() >= 2) {
                double dMinDis = std::numeric_limits<double>::max();
                int nBestIndex = 0;
                for (int i = 0; i < vctliUsableLinks.size(); i++) {
                    int nLaneSeqNum = 0;
                    func_GetLaneSeqNum(vctliUsableLinks.at(i), vctliUsableLanes.at(i), nLaneSeqNum);
                    std::vector<std::vector<double>> vctvdGeoLink = {};
                    m_spHdLib->GetLaneGeoLink(vctliUsableLinks.at(i), nLaneSeqNum, vctvdGeoLink);
                    Point ptNearPos = {};
                    double dNearDis = 0.0;
                    int nNearIdx = 0;
                    HdLibMath::GetNearestPointFromArray(ptCurLoc, vctvdGeoLink, ptNearPos, dNearDis, nNearIdx);
                    if (dMinDis > dNearDis) {
                        dMinDis = dNearDis;
                        nBestIndex = i;
                    }
                }
                liInOutCurrentLinkId = vctliUsableLinks.at(nBestIndex);
                func_GetLaneNum(liInOutCurrentLinkId, vctliUsableLanes.at(nBestIndex), vctnOutLocateLaneSeqNum);
            } else if (vctliUsableLinks.size() == 1) {
                liInOutCurrentLinkId = vctliUsableLinks.at(0);
                func_GetLaneNum(liInOutCurrentLinkId, vctliUsableLanes.at(0), vctnOutLocateLaneSeqNum);
            } else {
                // 都不符合，取一个角度偏差最小的
                double dMinAngle = std::numeric_limits<double>::max();
                int nBestIndex = -1;
                for (int i = 0; i < vctliLocateLinks.size(); ++i) {
                    double dCurAngle = 0.0;
                    m_spHdLib->GetLinkStartAngle(vctliLocateLinks.at(i), dCurAngle);
                    double dAngleSub = std::min(abs(dCurAngle - glGnssLoc.heading), 360 - abs((dCurAngle - glGnssLoc.heading)));
                    if (dMinAngle > dAngleSub && dAngleSub < 30) {
                        dMinAngle = dAngleSub;
                        nBestIndex = i;
                    }
                }
                if (nBestIndex >= 0) {
                    liInOutCurrentLinkId = vctliLocateLinks.at(nBestIndex);
                    func_GetLaneNum(liInOutCurrentLinkId, vctliLocateLanes.at(nBestIndex), vctnOutLocateLaneSeqNum);
                }
            }
        } else {
            int nLaneSeqNum = 0;
            func_GetLaneSeqNum(vctliLocateLinks.at(0), vctliLocateLanes.at(0), nLaneSeqNum);
            std::vector<std::vector<double>> geolink = {};
            m_spHdLib->GetLaneGeoLink(vctliLocateLinks.at(0), nLaneSeqNum, geolink);
            Point ptNearPos = {};
            double dNearDis = 0.0;
            int nNearIdx = 0;
            HdLibMath::GetNearestPointFromArray(Point(glGnssLoc.gnss_point.x, glGnssLoc.gnss_point.y, 0), geolink, ptNearPos, dNearDis, nNearIdx);
            double dAngle = 0;
            if (nNearIdx == 0) {
                dAngle = PointsAngle(Point(geolink.at(0)), Point(geolink.at(1)));
            } else {
                dAngle = PointsAngle(Point(geolink.at(nNearIdx - 1)), Point(geolink.at(nNearIdx)));
            }
            // 即使在车道里，角度偏差也不能大于30度
            double dAngleSub = std::min(abs(dAngle - glGnssLoc.heading), 360 - abs((dAngle - glGnssLoc.heading)));
            if (dAngleSub < 30) {
                liInOutCurrentLinkId = vctliLocateLinks.at(0);
                func_GetLaneNum(liInOutCurrentLinkId, vctliLocateLanes.at(0), vctnOutLocateLaneSeqNum);
            }
        }
    }

    if (vctnOutLocateLaneSeqNum.empty()) {
        return false;
    } else {
        s_liLastLinkId = liInOutCurrentLinkId;
        return true;
    }
}

void MapMatching::MergeVisionMapData(const IVisionMap& vmVision, IVisionMap& vmOutVision) {
    vmOutVision = vmVision;
    static std::vector<IVisionMap> s_vctvmVisions = {};
    // 缓存数据
    s_vctvmVisions.push_back(vmVision);
    // 多于10个就抛弃旧数据
    while (s_vctvmVisions.size() > 10) {
        s_vctvmVisions.erase(s_vctvmVisions.begin());
    }
    for (int i = 0; i < s_vctvmVisions.size() - 1; ++i) {
        const IVisionMap& vmVision = s_vctvmVisions.at(i);
        // 是否出现过变道 如果有，则忽视该变道信息
        if (vmVision.crossing == MarkingCrossing::EDGE_CROSSING_LEFT_CROSSING || vmVision.crossing == MarkingCrossing::EDGE_CROSSING_RIGHT_CROSSING) {
            if (vmVision.crossing == vmVision.crossing) {
                vmOutVision.crossing = MarkingCrossing::EDGE_CROSSING_NO_CROSSING;
            }
        }
    }

    return;
}

void MapMatching::PrintQgis(const std::string& strQgisFile, double dLon, double dLat, int nNum, long lId, int nLane /* = 0*/) {
    std::ofstream ofsQgisFile;
    ofsQgisFile.open(strQgisFile.c_str(), std::ios::out | std::ios::app);
    ofsQgisFile.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisFile.precision(15);

    // 写入点
    ofsQgisFile << dLon << ", " << dLat << ", " << nNum << ", " << lId << ", " << nLane << std::endl;
    ofsQgisFile.close();
}

void MapMatching::PrintQgis(const std::string& strQgisFile, double dLon, double dLat, int nCross, double dLeftDis, double dRightDis, const std::string& strLeftInfo, const std::string& strRightInfo) {
    std::ofstream ofsQgisFile;
    ofsQgisFile.open(strQgisFile.c_str(), std::ios::out | std::ios::app);
    ofsQgisFile.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisFile.precision(15);

    // 写入点
    ofsQgisFile << dLon << ", " << dLat << ", " << nCross << ", " << dLeftDis << ", " << dRightDis << ", " << strLeftInfo << ", " << strRightInfo << std::endl;
    ofsQgisFile.close();
}

}  // namespace jf
