// #include "MapMatching.hpp"

// #include <unistd.h>

// #include <fstream>

// #include "core/BindRoad.h"

// namespace zone {

// namespace fusion_localization {

// namespace map {

// MapMatching::MapMatching() : Thread("MapMatching thread"), map_lib_(), is_running_(false), /*gnss_mutex_(), vision_mutex_(),*/ le_(location_error::no_error), getGpsData_exec_num(0) {
//     // pthread_mutexattr_t mutex_attr;
//     // pthread_mutexattr_init(&mutex_attr);
//     // pthread_mutexattr_settype(&mutex_attr, PTHREAD_MUTEX_RECURSIVE);
//     // pthread_mutex_init(&gnss_mutex_, &mutex_attr);
//     // pthread_mutex_init(&vision_mutex_, &mutex_attr);
//     // pthread_mutexattr_destroy(&mutex_attr);

//     // begin run bind road thread
//     // map_matching_thread_ = new std::thread(&MapMatching::MapMatchingProc, this);

//     // 创建csv文件
//     std::ofstream ofsQgisInput;
//     std::ofstream ofsQgisResult;
//     strQgisFileInput_ = "source.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"
//                   << ", "
//                   << "num"
//                   << ","
//                   << "link"
//                   << ","
//                   << "lane" << std::endl;
//     ofsQgisInput.close();
//     ofsQgisResult.close();
// }

// MapMatching::~MapMatching() {
//     // pthread_mutex_destroy(&gnss_mutex_);
//     // pthread_mutex_destroy(&vision_mutex_);
// }

// void MapMatching::Init(const std::string& projectDir, const std::string& configJsonPath) {
//     map_lib_.init(projectDir, configJsonPath);
//     // begin run bind road thread
//     // map_matching_thread_ = new std::thread(&MapMatching::MapMatchingProc, this);
//     Start();
// }

// bool MapMatching::IsRunning() { return is_running_; }

// bool MapMatching::Start() {
//     std::cout << "MapMatching::Start() is called. current is_running_ = " << is_running_ << std::endl;

//     if (true == is_running_) {
//         return true;
//     }

//     return Thread::Start();
// }

// bool MapMatching::Stop() {
//     std::cout << "MapMatching::Stop() is called." << std::endl;

//     if (false == is_running_) {
//         return true;
//     }

//     // unsigned uTimeout = INFINITE;
//     bool bForce = true;
//     return Thread::Stop(1000, bForce);
// }

// void MapMatching::RegPubAheadMapCb(cbPubAheadMap func) {
//     ahead_map_pub_ = move(func);
//     return;
// }

// void MapMatching::RegPubGnssFootLocCb(cbPubGnssFootLoc func) {
//     gnss_foot_pub_ = move(func);
//     return;
// }
// void MapMatching::SaveGnssLocationData(const IGnssLocationData& data) {
//     AutoLock locker(gnss_locker_);
//     // pthread_mutex_lock(&gnss_mutex_);
//     gnss_location_data_list_.push_back(data);
//     // pthread_mutex_unlock(&gnss_mutex_);
// }

// bool MapMatching::GetGnssLocationData(IGnssLocationData& outGnssLoc) {
//     AutoLock locker(gnss_locker_);
//     // pthread_mutex_lock(&gnss_mutex_);
//     // Just use the newest data
//     while (gnss_location_data_list_.size() > 1) {
//         gnss_location_data_list_.pop_front();
//     }
//     if (!gnss_location_data_list_.empty()) {
//         outGnssLoc = gnss_location_data_list_.front();
//         gnss_location_data_list_.pop_front();
//         // pthread_mutex_unlock(&gnss_mutex_);
//         return true;
//         // gnss_location_data_list_.pop_front();
//     } else {
//         // pthread_mutex_unlock(&gnss_mutex_);
//         return false;
//     }
// }

// void MapMatching::SaveVisionMapData(const IVisionMap& data) {
//     AutoLock locker(vision_locker_);
//     // pthread_mutex_lock(&vision_mutex_);
//     vision_map_list_.push_back(data);
//     // pthread_mutex_unlock(&vision_mutex_);
// }

// bool MapMatching::GetVisionMapData(IVisionMap& outVisionMap) {
//     AutoLock locker(vision_locker_);
//     // pthread_mutex_lock(&vision_mutex_);
//     // Just use the newest data
//     while (vision_map_list_.size() > 1) {
//         vision_map_list_.pop_front();
//     }
//     if (!vision_map_list_.empty()) {
//         outVisionMap = vision_map_list_.front();
//         vision_map_list_.pop_front();
//         // pthread_mutex_unlock(&vision_mutex_);
//         // vision_map_list_.pop_front();
//         return true;
//     } else {
//         // pthread_mutex_unlock(&vision_mutex_);
//         return false;
//     }
// }

// uint32_t MapMatching::Run() {
//     // prctl(std::PR_SET_NAME, "TcpClient");
//     is_running_ = true;
//     // loop
//     while (false == Signalled()) {
//         MapMatchingOnce();
//     }
// }
// void MapMatching::MapMatchingProc() {
//     is_running_ = true;
//     while (is_running_) {
//         MapMatchingOnce();
//     }
// }

// void MapMatching::MapMatchingOnce() {
//     IGnssLocationData gnssLoc = {};
//     while (false == GetGnssLocationData(gnssLoc)) {
//         usleep(10);
//     }

//     PrintQgis(strQgisFileInput_, gnssLoc.gnss_point.x, gnssLoc.gnss_point.y, getGpsData_exec_num, 0, 0);
//     getGpsData_exec_num++;

//     do {
//         IVisionMap vision = {};
//         while (false == GetVisionMapData(vision)) {
//             usleep(10);
//         }
//         jf::HdLinkId currentLink = {};
//         bool laneChange = false;
//         int lane_num;
//         jf::Point foot_loc = {0, 0, 0};
//         jf::Point v_horizontal_loc = {0, 0, 0};
//         jf::Point v_vertical_loc = {0, 0, 0};
//         if (true == StepUpdateConfidence(gnssLoc, vision, currentLink, laneChange, lane_num, foot_loc, v_horizontal_loc, v_vertical_loc)) {
//             PrintQgis(strQgisFileResult_, foot_loc.lon, foot_loc.lat, getGpsData_exec_num, currentLink.lValue, lane_num);
//         } else {
//             // trust gnss input
//             if (true == LocateByGnss(gnssLoc, currentLink, lane_num)) {
//                 foot_loc = {gnssLoc.gnss_point.x, gnssLoc.gnss_point.y, gnssLoc.gnss_point.z};
//                 v_horizontal_loc = foot_loc;
//                 v_vertical_loc = foot_loc;
//                 PrintQgis(strQgisFileResult_, foot_loc.lon, foot_loc.lat, 0, currentLink.lValue, lane_num);
//             } else {
//                 PrintQgis(strQgisFileResult_, foot_loc.lon, foot_loc.lat, -1, -1, -1);
//                 continue;
//             }
//         }

//         // make ahead map data
//         if ((0 != currentLink.lValue) && (0 != lane_num)) {
//             IAheadMap ahead_map = {};
//             MakeAheadMapData(currentLink, lane_num, ahead_map);
//             ahead_map_pub_(ahead_map);
//         }

//         // make ekf message data
//         if ((0 != currentLink.lValue) && (0 != lane_num)) {
//             IGnssFootLocation gnss_foot = {};
//             MakeGnssFootData(gnssLoc, vision, currentLink, laneChange, lane_num, foot_loc, v_horizontal_loc, v_vertical_loc, gnss_foot);
//             gnss_foot_pub_(gnss_foot);
//         }
//     } while (0);
// }

// bool MapMatching::StepUpdateConfidence(const IGnssLocationData& gnssLoc, const IVisionMap& vision, jf::HdLinkId& outCurrentLink, bool& outLaneChange, int& outLaneNum, jf::Point& ptOutFootPoint, jf::Point& ptOutVisionHorizontalPoint,
//                                        jf::Point& ptOutVisionVerticalPoint) {
//     static int match_cnt = 0;

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

//     static bool update_start_lane = true;

//     do {
//         jf::HdLinkId currentLink = {};
//         static int laneNum = 0;
//         bool forceTotalFalse = false;
//         while (true == update_start_lane) {
//             if (false == UpdateStartLaneConfidence(gnssLoc, vision, currentLink, laneNum)) {
//                 return false;
//             }

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

//             forceTotalFalse = true;

//             update_start_lane = false;
//         };

//         if (false == UpdateHorizontalConfidence(gnssLoc, vision, currentLink, outLaneChange, laneNum, ptOutFootPoint, ptOutVisionHorizontalPoint)) {
//             update_start_lane = true;
//             continue;
//         }

//         if (false == UpdateVerticalConfidence(gnssLoc, vision, currentLink, ptOutVisionVerticalPoint)) {
//             update_start_lane = true;
//             continue;
//         }

//         UpdateTotalConfidence(forceTotalFalse);

//         outCurrentLink = currentLink;
//         outLaneNum = laneNum;

//         return true;
//     } while (true);

//     return true;
// }

// bool MapMatching::UpdateTotalConfidence(bool forceTotalFalse) {
//     if (forceTotalFalse == true) {
//         confidence_.total = false;
//     } else if (confidence_.horizontal >= 90 && confidence_.vertical >= 90) {
//         confidence_.total = true;
//     } else {
//         confidence_.total = false;
//     }

//     return true;
// }

// bool MapMatching::UpdateStartLaneConfidence(const IGnssLocationData& gnssLoc, const IVisionMap& vision, jf::HdLinkId& outCurrentLink, int& outLaneNum) {
//     // Only check the possilbe lane number
//     auto func_GetLaneNum = [this](const jf::HdLinkId& currentLink, const IVisionMap& vision) {
//         int lane_cnt = 0;
//         map_lib_.GetLaneCnt(currentLink, lane_cnt);
//         std::cout << "MapMatching::UpdateStartLaneConfidence(), current link: " << currentLink.lValue << ", lane count: " << lane_cnt << ", ";
//         if (lane_cnt == 1) {
//             std::cout << "possible lane num: 1." << std::endl;
//             return 1;
//         } else if (lane_cnt == 2) {
//             if (false == vision.left_link_marking.empty()) {
//                 std::cout << "possible lane num: 1." << std::endl;
//                 return 1;
//             } else if (false == vision.right_link_marking.empty()) {
//                 std::cout << "possible lane num: 2." << std::endl;
//                 return 2;
//             }
//         } else if (lane_cnt == 3) {
//             if (false == vision.left_link_marking.empty()) {
//                 std::cout << "possible lane num: 1." << std::endl;
//                 return 1;
//             } else if (vision.lane_markings.size() == 4) {
//                 std::cout << "possible lane num: 2." << std::endl;
//                 return 2;
//             } else if (false == vision.right_link_marking.empty()) {
//                 std::cout << "possible lane num: 3." << std::endl;
//                 return 3;
//             }
//         } else if (lane_cnt >= 4) {
//             if (false == vision.left_link_marking.empty()) {
//                 std::cout << "possible lane num: 1." << std::endl;
//                 return 1;
//             } else if (false == vision.right_link_marking.empty()) {
//                 std::cout << "possible lane num: " << lane_cnt << "." << std::endl;
//                 return lane_cnt;
//             }
//         }

//         std::cout << "could not get the lane num." << std::endl;
//         return 0;
//     };

//     // try to match, only worked when lane markings in vision is not too many
//     auto func_LuckyMatch = [this](const IVisionMap& vision, const jf::HdLinkId& currentLink) {
//         std::vector<int> possible_lanes = {};
//         std::vector<jf::HdEdgeId> checking_edges = {};
//         int lane_cnt = 0;
//         map_lib_.GetLaneCnt(currentLink, lane_cnt);
//         for (const ILaneMarking& lane_marking : vision.lane_markings) {
//             std::vector<LaneMarkingSide> checking_poses = {};
//             switch (lane_marking.pos) {
//                 case LaneMarkingSide::LANE_MARKING_SIDE_LEFT:
//                     checking_poses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT};
//                     for (int i = 1; i <= lane_cnt; i++) {
//                         if (true == MatchEdgeInfo(vision, currentLink, checking_poses, i)) {
//                             possible_lanes.push_back(i);
//                         };
//                     }
//                     break;
//                 case LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT:
//                     checking_poses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT};
//                     for (int i = 2; i <= lane_cnt; i++) {
//                         if (true == MatchEdgeInfo(vision, currentLink, checking_poses, i)) {
//                             possible_lanes.push_back(i);
//                         };
//                     }
//                     break;
//                 case LaneMarkingSide::LANE_MARKING_SIDE_RIGHT:
//                     checking_poses = {LaneMarkingSide::LANE_MARKING_SIDE_RIGHT};
//                     for (int i = 1; i <= lane_cnt; i++) {
//                         if (true == MatchEdgeInfo(vision, currentLink, checking_poses, i)) {
//                             possible_lanes.push_back(i);
//                         };
//                     }
//                     break;
//                 case LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT:
//                     checking_poses = {LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT};
//                     for (int i = 1; i <= lane_cnt - 1; i++) {
//                         if (true == MatchEdgeInfo(vision, currentLink, checking_poses, i)) {
//                             possible_lanes.push_back(i);
//                         };
//                     }
//                     break;
//                 default:
//                     break;
//             }
//         }

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

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

//     // step 1
//     auto func_BindRoad = [this, &func_LuckyMatch](const IGnssLocationData& gnssLoc, jf::HdLinkId& outCurrentLink) { return StepBindRoad(gnssLoc, outCurrentLink); };

//     // step 2
//     auto func_CheckCurrentLane = [this, &func_GetLaneNum, &func_LuckyMatch](const IVisionMap& vision, const jf::HdLinkId& currentLink) {
//         int lane_num = func_GetLaneNum(currentLink, vision);
//         if (lane_num == 0) {
//             confidence_.start_lane = 0;
//         } else {
//             std::vector<LaneMarkingSide> checking_poses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT};
//             if (MatchEdgeInfo(vision, currentLink, checking_poses, lane_num)) {
//                 std::cout << "MapMatching::UpdateStartLaneConfidence(), match current lane successed." << std::endl;
//                 return lane_num;
//             } else {
//                 std::cout << "MapMatching::UpdateStartLaneConfidence(), match current lane failed." << std::endl;
//             }
//         }

//         // failed befor, try one more time
//         lane_num = func_LuckyMatch(vision, currentLink);

//         return lane_num;
//     };

//     // step 3
//     auto func_CheckNearLink = [this](const IVisionMap& vision, const jf::HdLinkId& currentLink, int lane_num) {
//         int lane_cnt = 0;
//         map_lib_.GetLaneCnt(currentLink, lane_cnt);

//         std::vector<LaneMarkingSide> checking_poses = {};
//         if (lane_num == 1) {
//             checking_poses = {LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT};
//         } else if (lane_num == lane_cnt) {
//             checking_poses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT};
//         } else {
//             checking_poses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT};
//         }
//         if (false == MatchEdgeInfo(vision, currentLink, checking_poses, lane_num, false)) {
//             std::cout << "MapMatching::UpdateStartLaneConfidence(), match near by lane failed." << std::endl;
//             confidence_.start_lane -= 10;
//         } else {
//             std::cout << "MapMatching::UpdateStartLaneConfidence(), match near by lane success." << std::endl;
//         }
//     };

//     // step 4
//     auto func_CheckAngle = [this](const IVisionMap& vision, const jf::HdLinkId& currentLink) {
//         if (std::abs(vision.lane_markings[0].angle) <= 30) {
//             // goto step 5
//             return true;
//         } else {
//             // goto step 1
//             return false;
//         }
//     };

//     // step 5
//     auto func_UpdateMatchCnt = [this](bool is_add) {
//         static int match_cnt = 0;

//         if (true == is_add) {
//             match_cnt++;
//         } else {
//             match_cnt = 0;
//         }

//         if (match_cnt >= 6) {
//             if (confidence_.start_lane >= 90) {
//                 return true;
//             }
//         } else {
//             // repeat step 1
//             return false;
//         }

//         return true;
//     };

//     bool is_report = false;

//     do {
//         // step 1
//         if (false == func_BindRoad(gnssLoc, outCurrentLink)) {
//             confidence_.start_lane = 0;
//             // step 5
//             is_report = func_UpdateMatchCnt(false);

//             std::cout << "MapMatching::UpdateStartLaneConfidence(), bind road failed." << std::endl;
//             return false;
//         }
//         confidence_.start_lane = 100;

//         // step 2
//         int lane_num = func_CheckCurrentLane(vision, outCurrentLink);
//         if (0 == lane_num) {
//             confidence_.start_lane = 0;

//             // step 5
//             is_report = func_UpdateMatchCnt(false);

//             std::cout << "MapMatching::UpdateStartLaneConfidence(), check current lane num failed." << std::endl;
//             return false;
//         } else if (1 == lane_num) {
//             // step 4
//             if (true == func_CheckAngle(vision, outCurrentLink)) {
//                 // step 5
//                 is_report = func_UpdateMatchCnt(true);
//                 outLaneNum = lane_num;
//                 std::cout << "MapMatching::UpdateStartLaneConfidence(), angle has no problem." << std::endl;
//                 return true;
//             } else {
//                 // step 5
//                 is_report = func_UpdateMatchCnt(false);
//                 std::cout << "MapMatching::UpdateStartLaneConfidence(), check angle failed." << std::endl;
//                 continue;
//             }
//         } else {
//             // step 3
//             func_CheckNearLink(vision, outCurrentLink, lane_num);

//             // step 4
//             if (true == func_CheckAngle(vision, outCurrentLink)) {
//                 // step 5
//                 is_report = func_UpdateMatchCnt(true);
//                 outLaneNum = lane_num;
//                 std::cout << "MapMatching::UpdateStartLaneConfidence(), angle has no problem." << std::endl;
//                 return true;
//             } else {
//                 // step 5
//                 is_report = func_UpdateMatchCnt(false);
//                 continue;
//             }
//         }
//     } while (true);
// }

// bool MapMatching::UpdateStartStatusConfidence() {
//     if (confidence_.start_lane >= 90) {
//         confidence_.start_status++;
//     } else {
//         confidence_.start_status = 0;
//     }

//     return true;
// }

// bool MapMatching::UpdateHorizontalConfidence(const IGnssLocationData& gnssLoc, const IVisionMap& vision, jf::HdLinkId& outCurrentLink, bool& outLaneChange, int& inoutLaneNum, jf::Point& outFootPoint, jf::Point& outHorizontalPoint) {
//     // step 1
//     auto func_BindRoad = [this](const IGnssLocationData& gnssLoc, jf::HdLinkId& outCurrentLink) { return StepBindRoad(gnssLoc, outCurrentLink); };

//     // step 2
//     auto func_CheckCurrentLane = [this](const IVisionMap& vision, const jf::HdLinkId& currentLink, int lane_num) {
//         static std::list<bool> match_results = {};
//         static int unmatch_cnt = 0;
//         static int match_cnt = 0;

//         std::vector<LaneMarkingSide> checking_poses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT};
//         if (true == MatchEdgeInfo(vision, currentLink, checking_poses, lane_num)) {
//             match_results.push_back(true);
//             if (match_results.size() > 6) {
//                 match_results.pop_front();
//             }
//             for (bool match : match_results) {
//                 if (true == match) {
//                     match_cnt++;
//                 }
//             }
//             if (match_cnt >= 5 && confidence_.horizontal < 100) {
//                 confidence_.horizontal += 5;
//             }
//         } else {
//             match_results.push_back(false);
//             if (match_results.size() > 6) {
//                 match_results.pop_front();
//             }
//             unmatch_cnt++;
//             if (unmatch_cnt >= 6) {
//                 confidence_.horizontal -= 5;
//                 if (confidence_.horizontal <= 0) {
//                     // goto start lane matching
//                     return false;
//                 }
//             }
//         }

//         return true;
//     };

//     // step 3
//     auto func_CheckCrossing = [this](const IVisionMap& vision, const jf::HdLinkId& currentLink, bool& outLaneChange, int& inoutLaneNum) {
//         if (MarkingCrossing::EDGE_CROSSING_LEFT_CROSSING == vision.crossing) {
//             outLaneChange = true;
//             // vision.crossing = LaneCrossing::None;
//             if (inoutLaneNum > 1) {
//                 inoutLaneNum--;
//                 // goto step 2
//                 return true;
//             } else {
//                 // goto start lane matching
//                 return false;
//             }
//         } else if (MarkingCrossing::EDGE_CROSSING_RIGHT_CROSSING == vision.crossing) {
//             outLaneChange = true;
//             // vision.crossing = LaneCrossing::None;
//             int lane_cnt = 0;
//             map_lib_.GetLaneCnt(currentLink, lane_cnt);
//             if (inoutLaneNum < lane_cnt) {
//                 inoutLaneNum++;
//                 // goto step 2
//                 return true;
//             } else {
//                 // goto start lane matching
//                 return false;
//             }
//         }

//         return true;
//     };

//     // step 4
//     auto func_CheckNearLink = [this](const IVisionMap& vision, const jf::HdLinkId& currentLink, int lane_num) {
//         static std::list<bool> unmatch_results = {};

//         int lane_cnt = 0;
//         map_lib_.GetLaneCnt(currentLink, lane_cnt);

//         bool unmatch = false;
//         if (lane_num == 1) {
//             std::vector<LaneMarkingSide> checking_poses = {};
//             if (false == MatchEdgeInfo(vision, currentLink, checking_poses, lane_num, false)) {
//                 unmatch = true;
//             }
//         } else if (lane_num == lane_cnt) {
//             std::vector<LaneMarkingSide> checking_poses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT};
//             if (false == MatchEdgeInfo(vision, currentLink, checking_poses, lane_num, false)) {
//                 unmatch = true;
//             }
//         } else {
//             std::vector<LaneMarkingSide> checking_poses = {LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT, LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT};
//             if (false == MatchEdgeInfo(vision, currentLink, checking_poses, lane_num, false)) {
//                 unmatch = true;
//             }
//         }

//         unmatch_results.push_back(unmatch);
//         if (unmatch_results.size() > 6) {
//             unmatch_results.pop_front();
//         }
//         for (bool unmatch : unmatch_results) {
//             if (true == unmatch) {
//                 confidence_.horizontal -= 5;
//                 break;
//             }
//         }
//         if (confidence_.horizontal <= 0) {
//             confidence_.horizontal = 0;
//             // goto start lane matching
//             return false;
//         }

//         return true;
//     };

//     // step 5
//     auto func_CheckAngle = [this](const IVisionMap& vision) {
//         if (std::abs(vision.lane_markings[0].angle) < 30) {
//             if (confidence_.horizontal % 10 != 0) {
//                 confidence_.horizontal += 5;
//             }
//             return true;
//         } else {
//             // goto step 1
//             return false;
//         }
//     };

//     // get horizontal vision Point
//     auto func_GetHorizontalPoint = [this](const IGnssLocationData& gnssLoc, const IVisionMap& vision, jf::HdLinkId& inoutCurrentLink, int& inoutLaneNum, jf::Point& outHorizontalPoint) {
//         double lane_dis = std::numeric_limits<double>::max();
//         bool is_left = true;
//         // only the first two lane marking is current lane
//         for (int i = 0; i < 2; i++) {
//             const ILaneMarking& lane_marking = vision.lane_markings[i];
//             // more little and valid
//             if ((lane_dis > lane_marking.dis) && (0 != lane_marking.dis)) {
//                 lane_dis = lane_marking.dis;
//                 if (LaneMarkingSide::LANE_MARKING_SIDE_RIGHT == lane_marking.pos) {
//                     is_left = false;
//                 }
//             }
//         }

//         if (std::numeric_limits<double>::max() != lane_dis) {
//             // get perpendicular foot Point on the left/right edge
//             std::vector<std::vector<double>> edge_geolink = {};
//             map_lib_.GetLaneEdgeGeoLink(inoutCurrentLink, inoutLaneNum, is_left, edge_geolink);
//             jf::Point in_point = jf::Point(gnssLoc.gnss_point.x, gnssLoc.gnss_point.y, gnssLoc.gnss_point.z);
//             jf::Point perpendicular = {};
//             double distance = 0;
//             map_lib_.GetPerpendicularFoot(in_point, edge_geolink, perpendicular, distance);
//             double angle = 0;
//             map_lib_.GetLinkStartAngle(inoutCurrentLink, angle);

//             double lon_dis = 0;
//             double lat_dis = 0;
//             if (true == is_left) {
//                 lon_dis = cos(angle) * lane_dis;
//                 lat_dis = sin(angle) * lane_dis;
//             } else {
//                 lon_dis = 0 - cos(angle) * lane_dis;
//                 lat_dis = 0 - sin(angle) * lane_dis;
//             }

//             // get Point, if the distance is too long, trust the rtk data
//             jf::PointDis2NewPoint(perpendicular, lon_dis, lat_dis, outHorizontalPoint);
//             if ((true == gnssLoc.credible) && (0.2 <= std::abs(jf::D_P(perpendicular, outHorizontalPoint) - distance))) {
//                 LocateByGnss(gnssLoc, inoutCurrentLink, inoutLaneNum);
//                 outHorizontalPoint = {gnssLoc.gnss_point.x, gnssLoc.gnss_point.y, gnssLoc.gnss_point.z};
//             }
//             std::cout << "MapMatching::UpdateHorizontalConfidence(), func_GetHorizontalPoint, difference between vision Point and gnss: " << std::abs(jf::D_P(perpendicular, outHorizontalPoint) - distance) << std::endl;
//         }
//     };

//     // step 1
//     jf::HdLinkId currentLink = {};
//     if (false == func_BindRoad(gnssLoc, currentLink)) {
//         return false;
//     }
//     confidence_.horizontal = 100;

//     do {
//         static const int frequently = 10;
//         static const int update_time = 3;  // seconds
//         static int visionMapCnt = 0;
//         visionMapCnt++;
//         static int low_accuracy_cnt = 0;
//         IVisionMap mergedVision = {};
//         MergeVisionMapData(vision, mergedVision);
//         if ((mergedVision.crossing == MarkingCrossing::EDGE_CROSSING_LEFT_CROSSING || mergedVision.crossing == MarkingCrossing::EDGE_CROSSING_RIGHT_CROSSING || false == mergedVision.traffic_signs.empty()) || (visionMapCnt % 6 == 0)) {
//             // step 2
//             if (false == func_CheckCurrentLane(mergedVision, currentLink, inoutLaneNum)) {
//                 // goto start lane matching
//                 std::cout << "MapMatching::UpdateHorizontalConfidence(), match current lane failed, use last result" << std::endl;
//                 // return false;
//             }

//             // step 3
//             if (true == func_CheckCrossing(mergedVision, currentLink, outLaneChange, inoutLaneNum)) {
//                 map_lib_.GetPerpendicularFoot(currentLink, inoutLaneNum, jf::Point(gnssLoc.gnss_point.x, gnssLoc.gnss_point.y, gnssLoc.gnss_point.z), outFootPoint);
//             } else {
//                 // 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(mergedVision, currentLink, inoutLaneNum)) {
//                 std::cout << "MapMatching::UpdateHorizontalConfidence(), match nearby lane failed, use last result." << std::endl;
//             }

//             // step 5
//             if (false == func_CheckAngle(mergedVision)) {
//                 std::cout << "MapMatching::UpdateHorizontalConfidence(), match nearby lane failed, use last result." << std::endl;
//             }

//             if (confidence_.horizontal < 50) {
//                 low_accuracy_cnt++;
//                 if (low_accuracy_cnt >= frequently * update_time / 6) {
//                     visionMapCnt = 0;
//                     // goto start lane matching
//                     std::cout << "MapMatching::UpdateHorizontalConfidence(), confidence is too low, goto start lane matching." << std::endl;
//                     return false;
//                 }
//             } else {
//                 low_accuracy_cnt = 0;
//             }
//         } else {
//             std::cout << "MapMatching::UpdateHorizontalConfidence(), current horizontal confidence: " << confidence_.horizontal << std::endl;
//             func_GetHorizontalPoint(gnssLoc, vision, currentLink, inoutLaneNum, outHorizontalPoint);
//             return true;
//         }

//         std::cout << "MapMatching::UpdateHorizontalConfidence(), current horizontal confidence: " << confidence_.horizontal << std::endl;
//         func_GetHorizontalPoint(gnssLoc, vision, currentLink, inoutLaneNum, outHorizontalPoint);
//         return true;
//     } while (true);
// }

// bool MapMatching::UpdateVerticalConfidence(const IGnssLocationData& gnssLoc, const IVisionMap& vision, const jf::HdLinkId& currentLink, jf::Point& ptOutVisionPoint) {
//     // step 1
//     auto func_Init = [this]() { confidence_.vertical = 100; };

//     // step 2
//     auto func_CheckAccuracy = [this](const IGnssLocationData& gnssLoc) {
//         if (gnssLoc.coefficient <= 70) {
//             confidence_.vertical -= 5;
//         }
//     };

//     // step 3
//     auto func_CheckTsr = [this](const IGnssLocationData& gnssLoc, const IVisionMap& vision, const jf::HdLinkId& currentLink, jf::Point& ptOutVisionPoint) {
//         static int tsr_cnt_gnss = 0;
//         if (true == vision.traffic_signs.empty()) {
//             tsr_cnt_gnss = 0;
//             return true;
//         }

//         tsr_cnt_gnss++;

//         if (tsr_cnt_gnss % 5 == 0) {
//             bool matched = false;

//             std::vector<int> shapes = {};
//             std::vector<int> mainClasses = {};
//             std::vector<int> subClasses = {};
//             std::vector<std::string> values = {};
//             std::vector<std::vector<std::vector<double>>> geometries = {};
//             jf::Point pt = jf::Point(gnssLoc.gnss_point.x, gnssLoc.gnss_point.y, gnssLoc.gnss_point.z);
//             map_lib_.GetTrafficSign(pt, shapes, mainClasses, subClasses, values, geometries);

//             int matchedIndex = -1;
//             if (mainClasses.size() > 0 && vision.traffic_signs.size() > 0) {
//                 for (int i = 0; i < mainClasses.size(); i++) {
//                     for (int j = 0; j < vision.traffic_signs.size(); j++) {
//                         if ((mainClasses[i] == (int)vision.traffic_signs.at(j).traffic_sign_class) && (subClasses[i] == vision.traffic_signs[j].traffic_sign_sub_class) && (atoi(values[i].c_str()) == vision.traffic_signs[j].speed_limit_value)) {
//                             matchedIndex = j;
//                             break;
//                         }
//                     }
//                 }
//             }

//             if (matchedIndex >= 0) {
//                 // update location
//                 double lonDis = vision.traffic_signs.at(matchedIndex).lon_dis;
//                 double latDis = vision.traffic_signs.at(matchedIndex).lat_dis;
//                 // update lonDis and latDis by heading
//                 if (gnssLoc.heading >= 0 && gnssLoc.heading < 90) {
//                     // do nothing
//                 } else if (gnssLoc.heading >= 90 && gnssLoc.heading < 180) {
//                     latDis = 0 - latDis;
//                 } else if (gnssLoc.heading >= 180 && gnssLoc.heading < 270) {
//                     lonDis = 0 - lonDis;
//                     latDis = 0 - latDis;
//                 } else {
//                     lonDis = 0 - lonDis;
//                 }
//                 jf::Point inPoint = jf::Point(gnssLoc.gnss_point.x, gnssLoc.gnss_point.y, gnssLoc.gnss_point.z);
//                 jf::PointDis2NewPoint(inPoint, lonDis, latDis, ptOutVisionPoint);
//             } else {
//                 confidence_.vertical -= 5;
//             }
//         }

//         return true;
//     };

//     func_Init();
//     func_CheckAccuracy(gnssLoc);
//     func_CheckTsr(gnssLoc, vision, currentLink, ptOutVisionPoint);

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

//     return true;
// }

// bool MapMatching::LocateByGnss(const IGnssLocationData& gnssLoc, jf::HdLinkId& outCurrentLink, int& outLaneNum) {
//     jf::Point in_point = jf::Point(gnssLoc.gnss_point.x, gnssLoc.gnss_point.y, gnssLoc.gnss_point.z);
//     std::vector<jf::HdLinkId> links = {};
//     std::vector<jf::HdLaneId> lanes = {};
//     map_lib_.GetLocateLanes(in_point, links, lanes);
//     for (const jf::HdLinkId& link : links) {
//         int lane_cnt = 0;
//         map_lib_.GetLaneCnt(link, lane_cnt);
//         for (int i = 1; i <= lane_cnt; i++) {
//             jf::HdLaneId link_lane = {};
//             map_lib_.GetLaneId(link, i, link_lane);
//             for (jf::HdLaneId lane : lanes) {
//                 if (lane == link_lane) {
//                     outCurrentLink = link;
//                     outLaneNum = i;
//                     return true;
//                 }
//             }
//         }
//     }

//     std::cout << "MapMatching::LocateByGnss(), failed, current lon: " << gnssLoc.gnss_point.x << ", lat: " << gnssLoc.gnss_point.y << ", alt: " << gnssLoc.gnss_point.z << std::endl;
//     return false;
// }

// bool MapMatching::StepBindRoad(const IGnssLocationData& gnssLoc, jf::HdLinkId& outCurrentLink) {
//     jf::BindRoadOutput output(1);
//     map_lib_.HmmBindRoad({jf::Point(gnssLoc.gnss_point.x, gnssLoc.gnss_point.y, gnssLoc.gnss_point.z)}, output);
//     if (output.e != jf::BindRoadOutput::no_error && output.e != jf::BindRoadOutput::pending_after_init) {
//         le_ = std::max(le_, location_error::map_error);
//         std::cout << "MapMatching::StepBindRoad(), HmmBindRoad" << std::endl;
//         return false;
//     }

//     const jf::Point& bindroad_output_gnss = output.nodeInfos[0].outp;
//     std::vector<jf::HdLinkId> outNearbyLinks = {};
//     if (!map_lib_.GetDataNearby(bindroad_output_gnss,
//                                 40,  // 标牌测距需要前方80米的
//                                 40, outNearbyLinks)) {
//         // JFLOG(JFERROR,
//         //       "[Offline][step_GetCurrentLink] exec_num %llu, GetDataNearyBy "
//         //       "failed, BindRoad out gnss : %s",
//         //       getGnssData_exec_num, jf::to_string(bindroad_output_gnss).c_str());
//         le_ = std::max(le_, location_error::map_error);
//         std::cout << "MapMatching::StepBindRoad(), GetDataNearby" << std::endl;
//         return false;
//     }

//     bool foundCurrentLinkId = false;
//     for (const jf::HdLinkId l : outNearbyLinks) {
//         if (l.lValue == output.nodeInfos[0].rid) {
//             outCurrentLink = l;
//             foundCurrentLinkId = true;
//             break;
//         }
//     }
//     if (!foundCurrentLinkId) {
//         // JFLOG(JFERROR, "[Offline][step_GetCurrentLink] exec_num %llu, GetCurrentLinkId failed", getGnssData_exec_num);
//         le_ = std::max(le_, location_error::map_error);
//         std::cout << "MapMatching::StepBindRoad(), foundCurrentLinkId" << std::endl;
//         return false;
//     }

//     return true;
// }

// void MapMatching::GetEdgeFromMap(const jf::HdLinkId& currentLink, int lane_num, int& outLeftCross, int& outRightCross, int& outLeftColor, int& outRightColor) {
//     jf::HdEdgeId left_id = 0;
//     jf::HdEdgeId right_id = 0;
//     map_lib_.GetEdgeId(currentLink, lane_num - 1, left_id);
//     map_lib_.GetEdgeId(currentLink, lane_num, right_id);

//     map_lib_.GetEdgeCross(currentLink, left_id, outLeftCross);
//     map_lib_.GetEdgeCross(currentLink, right_id, outRightCross);
//     map_lib_.GetEdgeColor(currentLink, left_id, outLeftColor);
//     map_lib_.GetEdgeColor(currentLink, right_id, outRightColor);

//     return;
// }

// bool MapMatching::MatchEdgeInfo(const IVisionMap& vision, const jf::HdLinkId& currentLink, const std::vector<LaneMarkingSide>& checking_poses, int lane_num, bool strong_match /* = true */) {
//     auto func_ConvertCross = [](HMC::LaneMarkingType vision_cross, bool is_left) {
//         int edge_cross = 0;
//         switch (vision_cross) {
//             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:
//                 edge_cross = 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:
//                 edge_cross = 1;  // solid line
//                 break;
//             case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_RIGHT_SOLID_LEFT_DASHED:
//                 if (true == is_left) {
//                     edge_cross = 2;  // dashed line
//                 } else {
//                     edge_cross = 1;  // solid line
//                 }
//                 break;
//             case HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_LEFT_SOLID_RIGHT_DASHED:
//                 if (true == is_left) {
//                     edge_cross = 1;  // solid line
//                 } else {
//                     edge_cross = 2;  // dashed line
//                 }
//                 break;
//             default:
//                 edge_cross = 0;
//                 break;
//         }
//         return edge_cross;
//     };

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

//     auto fucn_MatchOnePos = [this, &func_ConvertCross, &func_ConvertColor](const IVisionMap& vision, const jf::HdLinkId& currentLink, LaneMarkingSide pos, int lane_num, bool strong_match /* = true */) {
//         // Get edge info by pos
//         HMC::LaneMarkingColor vision_color = HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_WHITE;
//         HMC::LaneMarkingType vision_cross = HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_NOT_INVESTIGATED;
//         float64_t vision_prob = 0;
//         bool found = false;
//         for (ILaneMarking lane_marking : vision.lane_markings) {
//             if (lane_marking.pos == pos) {
//                 vision_color = lane_marking.lane_marking_color;
//                 vision_cross = lane_marking.cross_type;
//                 vision_prob = lane_marking.exist_prob;
//                 found = true;
//                 break;
//             }
//         }

//         if (false == found) {
//             // not found, ignore
//             return true;
//         }

//         // Edge info by lane sequence number
//         int left_cross = 0;
//         int left_color = 0;
//         int right_cross = 0;
//         int right_color = 0;

//         // Check target
//         bool is_left = true;
//         int map_cross = 0;
//         int map_color = 0;
//         switch (pos) {
//             case LaneMarkingSide::LANE_MARKING_SIDE_LEFT:
//                 GetEdgeFromMap(currentLink, lane_num, left_cross, right_cross, left_color, right_color);
//                 is_left = true;
//                 map_cross = left_cross;
//                 map_color = left_color;
//                 break;
//             case LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT:
//                 GetEdgeFromMap(currentLink, lane_num - 1, left_cross, right_cross, left_color, right_color);
//                 is_left = true;
//                 map_cross = left_cross;
//                 map_color = left_color;
//                 break;
//             case LaneMarkingSide::LANE_MARKING_SIDE_RIGHT:
//                 GetEdgeFromMap(currentLink, lane_num, left_cross, right_cross, left_color, right_color);
//                 is_left = false;
//                 map_cross = right_cross;
//                 map_color = right_color;
//                 break;
//             case LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT:
//                 GetEdgeFromMap(currentLink, lane_num + 1, left_cross, right_cross, left_color, right_color);
//                 is_left = false;
//                 map_cross = right_cross;
//                 map_color = right_color;
//                 break;
//             default:
//                 break;
//         };

//         // Is matched
//         if (true == strong_match) {
//             if ((map_cross == func_ConvertCross(vision_cross, is_left)) && (map_color == func_ConvertColor(vision_color))) {
//                 return true;
//             } else {
//                 return false;
//             }
//         } else {
//             if (map_cross == func_ConvertCross(vision_cross, is_left)) {
//                 return true;
//             } else {
//                 return false;
//             }
//         }

//         return false;
//     };

//     for (LaneMarkingSide pos : checking_poses) {
//         if (false == fucn_MatchOnePos(vision, currentLink, pos, lane_num, strong_match)) {
//             return false;
//         }
//     }

//     return true;
// }

// void MapMatching::MakeAheadMapData(const jf::HdLinkId& currentLink, int lane_num, IAheadMap& outAheadMap) {
//     jf::HdEdgeId left_id = 0;
//     jf::HdEdgeId right_id = 0;
//     map_lib_.GetLaneEdgeId(currentLink, lane_num, true, left_id);
//     map_lib_.GetLaneEdgeId(currentLink, lane_num, false, right_id);
//     map_lib_.GetEdgeColor(currentLink, left_id, (int&)outAheadMap.cur_lane_left_color);
//     map_lib_.GetEdgeCross(currentLink, left_id, (int&)outAheadMap.cur_lane_left_type);
//     map_lib_.GetEdgeColor(currentLink, right_id, (int&)outAheadMap.cur_lane_right_color);
//     map_lib_.GetEdgeCross(currentLink, right_id, (int&)outAheadMap.cur_lane_right_type);
//     std::vector<jf::HdLinkId> next_links = {};
//     if (true == map_lib_.GetLinkNext(currentLink, lane_num, next_links) && next_links.empty() == false) {
//         map_lib_.GetLaneEdgeId(next_links[0], lane_num, true, left_id);
//         map_lib_.GetLaneEdgeId(next_links[0], lane_num, false, right_id);
//         map_lib_.GetEdgeColor(next_links[0], left_id, (int&)outAheadMap.next_lane_left_color);
//         map_lib_.GetEdgeCross(next_links[0], left_id, (int&)outAheadMap.next_lane_left_type);
//         map_lib_.GetEdgeColor(next_links[0], right_id, (int&)outAheadMap.next_lane_right_color);
//         map_lib_.GetEdgeCross(next_links[0], right_id, (int&)outAheadMap.next_lane_right_type);
//     } else {
//         map_lib_.GetLinkNext(currentLink, lane_num, next_links);
//     }
//     std::vector<std::vector<double>> geolink = {};
//     if (lane_num > 1) {
//         map_lib_.GetLaneGeoLink(currentLink, lane_num - 1, geolink);
//         for (auto geo : geolink) {
//             outAheadMap.left_lane_geo_lon.push_back(geo[0]);
//             outAheadMap.left_lane_geo_lat.push_back(geo[1]);
//             outAheadMap.left_lane_geo_alt.push_back(geo[2]);
//         }
//     }
//     int lane_cnt = 0;
//     map_lib_.GetLaneCnt(currentLink, lane_cnt);
//     if (lane_num < lane_cnt) {
//         map_lib_.GetLaneGeoLink(currentLink, lane_num + 1, geolink);
//         for (auto geo : geolink) {
//             outAheadMap.right_lane_geo_lon.push_back(geo[0]);
//             outAheadMap.right_lane_geo_lat.push_back(geo[1]);
//             outAheadMap.right_lane_geo_alt.push_back(geo[2]);
//         }
//     }
//     std::vector<int> shapes = {};
//     std::vector<int> mainClasses = {};
//     std::vector<int> subClasses = {};
//     std::vector<std::string> values = {};
//     std::vector<std::vector<std::vector<double>>> geometries = {};
//     map_lib_.GetTrafficSign(currentLink, shapes, mainClasses, subClasses, values, geometries);
//     std::vector<ITrafsign> trafsigns = {};
//     for (int i = 0; i < mainClasses.size(); ++i) {
//         ITrafsign trafsign = {};
//         trafsign.traffic_sign_class = mainClasses[i];
//         trafsign.traffic_sign_sub_class = subClasses[i];
//         trafsign.speed_limit_value = atoi(values[i].c_str());
//         trafsigns.push_back(trafsign);
//     }

//     return;
// }

// void MapMatching::MakeGnssFootData(const IGnssLocationData& gnssLoc, const IVisionMap& vision, const jf::HdLinkId& currentLink, bool laneChange, int lane_num, const jf::Point& footPoint, const jf::Point& visionHorizontalPoint,
//                                    const jf::Point& visionVerticalPoint, IGnssFootLocation& outGnssFoot) {
//     auto func_GetLaneMarkingAngle = [](const IVisionMap& vision) {
//         for (int i = 0; i < 2; i++) {
//             const ILaneMarking& lane_marking = vision.lane_markings[i];
//             if (0 != lane_marking.angle) {
//                 return lane_marking.angle;
//             }
//         }

//         return (float)0.0;
//     };

//     auto func_GetLaneMarkingDis = [this](const IGnssLocationData& gnssLoc, const IVisionMap& vision, const jf::HdLinkId& currentLink, int lane_num, bool isLeft) {
//         // try to get by vision
//         for (int i = 0; i < 2; i++) {
//             const ILaneMarking& lane_marking = vision.lane_markings[i];
//             if ((true == isLeft) && (LaneMarkingSide::LANE_MARKING_SIDE_LEFT == lane_marking.pos)) {
//                 return lane_marking.dis;
//             } else if ((false == isLeft) && (LaneMarkingSide::LANE_MARKING_SIDE_RIGHT == lane_marking.pos)) {
//                 return lane_marking.dis;
//             }
//         }

//         // try to get by gnss
//         std::vector<std::vector<double>> edge_geolink = {};
//         map_lib_.GetLaneEdgeGeoLink(currentLink, lane_num, isLeft, edge_geolink);
//         jf::Point in_point = jf::Point(gnssLoc.gnss_point.x, gnssLoc.gnss_point.y, gnssLoc.gnss_point.z);
//         jf::Point perpendicular = {};
//         double distance = 0;
//         map_lib_.GetPerpendicularFoot(in_point, edge_geolink, perpendicular, distance);

//         return distance;
//     };

//     // make data
//     outGnssFoot.change_lanes = laneChange;
//     outGnssFoot.lane_seq_no = lane_num;
//     map_lib_.GetLaneCnt(currentLink, (int&)outGnssFoot.lane_num);
//     map_lib_.GetLinkType(currentLink, (int&)outGnssFoot.road_type);
//     outGnssFoot.angle_with_lane = func_GetLaneMarkingAngle(vision);
//     StepGetTunnelInfo(jf::Point(gnssLoc.gnss_point.x, gnssLoc.gnss_point.y, gnssLoc.gnss_point.z), currentLink, outGnssFoot.road_type_begin_dis, outGnssFoot.road_type_end_dis);
//     outGnssFoot.in_road_type = outGnssFoot.road_type != 1 ? true : false;
//     outGnssFoot.left_lane_edge_dis = func_GetLaneMarkingDis(gnssLoc, vision, currentLink, lane_num, true);
//     outGnssFoot.right_lane_edge_dis = func_GetLaneMarkingDis(gnssLoc, vision, currentLink, lane_num, false);
//     outGnssFoot.foot_point.x = footPoint.lon;
//     outGnssFoot.foot_point.y = footPoint.lat;
//     outGnssFoot.foot_point.z = footPoint.alt;
//     outGnssFoot.vision_horizontal_point.x = visionHorizontalPoint.lon;
//     outGnssFoot.vision_horizontal_point.y = visionHorizontalPoint.lat;
//     outGnssFoot.vision_horizontal_point.z = visionHorizontalPoint.alt;
//     outGnssFoot.vision_vertical_point.x = visionVerticalPoint.lon;
//     outGnssFoot.vision_vertical_point.y = visionVerticalPoint.lat;
//     outGnssFoot.vision_vertical_point.z = visionVerticalPoint.alt;

//     return;
// }

// bool MapMatching::StepGetTunnelInfo(const jf::Point& ingps, const jf::HdLinkId currentLink, float& to_entry, float& to_exit) {
//     if (!GetTunnelToEntryExit(ingps, currentLink, true, to_entry, to_exit)) {
//         GetTunnelToEntryExit(ingps, currentLink, false, to_entry, to_exit);
//     }
//     return true;
// }

// bool MapMatching::GetTunnelToEntryExit(const jf::Point& ingps, const jf::HdLinkId& from, bool forward, float& out_to_entry, float& out_to_exit) {
//     float metre1 = 0;
//     float metre2 = 0;
//     jf::HdLinkId outLinkId = {};
//     bool found = false;
//     if (GetFirstTunnelMetre(ingps, from, 5000, forward, metre1, outLinkId)) {
//         static const int sc_nRoadType_Channel = 3;
//         found = true;
//         int road_type = 0;
//         map_lib_.GetLinkType(outLinkId, road_type);
//         // 不在隧道中
//         if (road_type != sc_nRoadType_Channel) {
//             std::vector<jf::HdLinkId> next_or_prev = {};
//             if (forward) {
//                 map_lib_.GetLinkNext(outLinkId, next_or_prev);
//             } else {
//                 map_lib_.GetLinkPre(outLinkId, next_or_prev);
//             }
//             if (next_or_prev.size() > 0 && GetFirstTunnelMetre(ingps, next_or_prev[0], 5000, forward, metre2, outLinkId)) {
//             }
//             if (forward) {
//                 out_to_entry = metre1;
//                 out_to_exit = metre1 + metre2;
//             } else {
//                 out_to_entry = -(metre1 + metre2);
//                 out_to_exit = -metre1;
//             }
//         }
//         // 在隧道中
//         if (road_type == sc_nRoadType_Channel) {
//             if (GetFirstTunnelMetre(ingps, from, 5000, !forward, metre2, outLinkId)) {
//             }
//             if (!forward) {
//                 out_to_entry = -metre1;
//                 out_to_exit = metre2;
//             } else {
//                 out_to_entry = -metre2;
//                 out_to_exit = metre1;
//             }
//         }
//     }
//     return found;
// }

// bool MapMatching::GetFirstTunnelMetre(const jf::Point& current_pos, const jf::HdLinkId from, float iter_metre, bool backward, float& out_metre, jf::HdLinkId& out_link) {
//     double length = 0;
//     bool found = false;
//     map_lib_.BFSLinks(from, length, backward, found, iter_metre, out_link);
//     float to_head, to_tail;
//     GetPosInHdLink(current_pos, from, to_head, to_tail);
//     length += backward ? to_tail : to_head;
//     if (found) out_metre = length;
//     return found;
// }

// bool MapMatching::GetPosInHdLink(const jf::Point& ptCurPos, const jf::HdLinkId& id, float& dOutToHeadDis, float& dOutToTailDis) {
//     std::vector<std::vector<double>> geolink = {};
//     map_lib_.GetGeoLink(id, geolink);
//     jf::Point ptGeoPos = {};
//     double dGeoDistance = -1;
//     int nGeoIndex = -1;
//     if (false == map_lib_.GetNearestDistance(ptCurPos, geolink, ptGeoPos, dGeoDistance, nGeoIndex)) {
//         return false;
//     }

//     double dRealLength = 0;
//     for (size_t i = nGeoIndex; i > 0; --i) {
//         dRealLength += jf::D_P(jf::Point(geolink[i]), jf::Point(geolink[i - 1]));
//     }
//     dOutToHeadDis = dRealLength;

//     dRealLength = 0;
//     for (size_t i = nGeoIndex; i < geolink.size() - 1; ++i) {
//         dRealLength += jf::D_P(jf::Point(geolink[i]), jf::Point(geolink[i + 1]));
//     }
//     dOutToTailDis = dRealLength;

//     return true;
// }

// void MapMatching::MergeVisionMapData(const IVisionMap& vision, IVisionMap& outVision) {
//     outVision = vision;
//     static std::vector<IVisionMap> visions = {};
//     visions.push_back(vision);
//     while (visions.size() > 30) {
//         visions.erase(visions.begin());
//     }
//     for (IVisionMap v : visions) {
//         if (v.crossing == MarkingCrossing::EDGE_CROSSING_LEFT_CROSSING || v.crossing == MarkingCrossing::EDGE_CROSSING_RIGHT_CROSSING) {
//             if (v.crossing == vision.crossing) {
//                 outVision.crossing = MarkingCrossing::EDGE_CROSSING_NO_CROSSING;
//             }
//         }
//     }

//     return;
// }

// void MapMatching::PrintQgis(const std::string& cstrQgisFile, double lon, double lat, int num, long id, int lane) {
//     std::ofstream ofsQgisFile;
//     ofsQgisFile.open(cstrQgisFile.c_str(), std::ios::out | std::ios::app);
//     ofsQgisFile.setf(std::ios::fixed, std::ios::floatfield);
//     ofsQgisFile.precision(15);

//     // 写入点
//     ofsQgisFile << lon << ", " << lat << ", " << num << ", " << lane << ", " << id << std::endl;
//     ofsQgisFile.close();
// }
// }  // namespace map

// }  // namespace fusion_localization

// }  // namespace zone