#ifndef _MAP_MATCHING_HPP_
#define _MAP_MATCHING_HPP_

#include <pthread.h>
#include <functional>
#include <iostream>
#include <list>
#include <thread>

#include "BindRoad.h"
#include "HdLib.h"
#include "Locker.h"
#include "input_hd_map_define.h"

namespace jf {

typedef IGnssFootLocation IMatchResult;
typedef ILocalization IEkfResult;
typedef IGnssLocationData IGnssLoc;

struct Confidence {
    Confidence() : nTotal(100), nStartLane(100), nStartStatus(100), nHorizontal(100), nVertical(100) {}

    int nTotal;
    int nStartLane;
    int nStartStatus;
    int nHorizontal;
    int nVertical;
};

class MapMatching {
   public:
    MapMatching();
    ~MapMatching();

    // 初始化
    void Init();

    // 返回地图匹配结果
    bool GetMatchResultData(IMatchResult& mrOutMatchResult);

    // 保存GNSS数据
    void SaveGnssLocData(const IGnssLoc& glGnssLoc);

    // 保存EyeQ4数据
    void SaveVisionMapData(const IVisionMap& vmVisionMap);

    // 保存egomotion结果
    void SaveEkfResultData(const IEkfResult& erEkfResult);

   private:
    // 定位流程线程
    void MapMatchingProc();
    void MapMatchingOnce();

    // 保存地图匹配结果
    void SaveMatchResultData(const IMatchResult& mrMatchResult);
    // 返回GNSS数据
    bool GetGnssLocData(IGnssLoc& glOutGnssLoc);
    // 返回EyeQ4数据
    bool GetVisionMapData(IVisionMap& vmOutVisionMap);
    // 返回egomotion结果
    bool GetEkfResultData(IEkfResult& erOutEkfResult);

    // 计算更新整体置信度
    bool UpdateTotalConfidence(bool bForceTotalFalse);
    // 计算置信度
    bool UpdateConfidence(const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, jf::HdLinkId& liOutCurrentLinkId, bool& bOutLaneChange, int& nOutLaneNum, Point& ptOutFootLoc, Point& ptOutVisionHorizontalLoc, Point& ptOutVisionVerticalLoc);
    // 计算初始车道
    bool UpdateStartLaneConfidence(const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, jf::HdLinkId& liOutCurrentLinkId, int& nOutLaneSeqNum);
    bool UpdateStartStatusConfidence();
    // 横向定位
    bool UpdateHorizontalConfidence(const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, jf::HdLinkId& liOutCurrentLinkId, bool& bOutLaneChange, int& nOutLaneSeqNum, Point& ptOutFootLoc, Point& ptOutVisionHorizontalLoc);
    // 纵向定位
    bool UpdateVerticalConfidence(const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, Point& ptOutVisionVerticalLoc);

    // 用GNSS坐标点定位车道
    bool LocateByGnss(const Point& ptCurLoc, jf::HdLinkId& liOutCurrentLinkId, int& nOutLaneSeqNum);

    // 绑路流程
    bool StepBindRoad(const IGnssLoc& glGnssLoc, jf::HdLinkId& liOutCurrentLinkId);

    // 获取Edge信息
    void GetEdgeFromMap(const jf::HdLinkId& liCurrentLinkId, int nLaneSeqNum, int& nOutLeftCross, int& nOutRightCross, int& nOutLeftColor, int& nOutRightColor);

    // 匹配Edge信息
    bool MatchEdgeInfo(const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, const std::vector<LaneMarkingSide>& vctlsCheckingPos, int nLaneSeqNum, bool bStrongMatch = true);

    // 根据绑路点得到当前link
    // void MakeAheadMapData(const jf::HdLinkId& currentLink, int lane_num, IAheadMap& outAheadMap);
    void MakeMatchResult(const IGnssLoc& glGnssLoc, const IVisionMap& vmVision, const jf::HdLinkId& liCurrentLinkId, bool bLaneChange, int nLaneSeqNum, const Point& ptFootLoc, const Point& ptVisionHorizontalLoc, const Point& ptVisionVerticalLoc,
                         IMatchResult& mrOutMatchResult);

    // 用GNSS坐标点定位车道
    bool GetLocateLaneNum(const IGnssLoc& glGnssLoc, jf::HdLinkId& liOutCurrentLinkId, std::vector<int>& vctnOutLocateLaneSeqNum);

    // 坐标点到特殊道路点(隧道，高架下，城市峡谷中)的距离
    bool GetSpecialRoadDis(const Point& ptCurLoc, const jf::HdLinkId& liCurrentLinkId, float& fOutDisToEntry, float& fOutDisToExit, int& nOutSpecialType);
    // 坐标点到特殊道路点的入口或出口的距离
    bool GetSpecialToEntryExit(const Point& ptCurLoc, const jf::HdLinkId& from, bool forward, float& out_to_entry, float& out_to_exit, int& out_special_type);
    bool GetFirstSpecialMetre(const Point& current_pos, const jf::HdLinkId& from, float iter_metre, bool backward, float& out_metre, jf::HdLinkId& out_link);
    // 计算某点距离link起点和终点的距离
    bool GetPosInHdLink(const Point& ptCurPos, const jf::HdLinkId& id, float& dOutToHeadDis, float& dOutToTailDis);
    // 过滤多余的变道信息
    void MergeVisionMapData(const IVisionMap& vmVision, IVisionMap& vmOutMergedVision);

    // for test
    void PrintQgis(const std::string& strQgisFile, double dLon, double dLat, int nNum, long lId, int nLane = 0);
    void PrintQgis(const std::string& strQgisFile, double dLon, double dLat, int nCross, double dLeftDis, double dRightDis, const std::string& strLeftInfo, const std::string& strRightInfo);

   private:
    Locker m_lckGnss;
    Locker m_lckVision;
    Locker m_lckEkfResult;
    Locker m_lckMatchResult;

    std::thread* m_thdMatching;

    std::list<IGnssLoc> m_lstglGnss;
    std::list<IVisionMap> m_lstvmVision;
    std::list<IEkfResult> m_lsterEkfResult;
    std::list<IMatchResult> m_lstmrMatchResult;

    IVisionMap m_vmCurVision;

    Confidence m_cfConfidence;

    int m_nLastLaneSeqNum;

    std::string strQgisFileInput_;
    std::string strQgisFileResult_;

    ShPtr_HdLib m_spHdLib;
    ShPtr_BindRoad m_spBindRoad;
};

}  // namespace jf
#endif  // _MAP_MATCHING_HPP_
