#ifndef HdLib_h
#define HdLib_h

#include <vector>

// #include "ajson.hpp"
#include <algorithm>

#include "CoordSys.h"
#include "HdLibMath.h"
#include "HdTypes.h"
#include "MapConfig.h"
#include "Save.h"
#include "SdTypes.h"
#include "World.h"

namespace jf {
// 获取数据接口
enum class GetDataNearby_param {
    none,                                       // 效率最高
    only_filter_in_rect,                        // 只严格过滤处在rect区域中的点
    only_current_link_first,                    // 只将当前link作为第一个元素
    both_filter_in_rect_and_current_link_first  // 同时做上两条
};

class HdLib;
typedef std::shared_ptr<HdLib> ShPtr_HdLib;

class HdLib {
   public:
    static ShPtr_HdLib GetInstance();
    ~HdLib() = default;

   private:
    HdLib() = default;  // 单例模式，不允许随便构造
    static ShPtr_HdLib m_spInstance;

   public:
    // 集成接口
    void Init(const std::string &strProjectPath, const std::string &strMapConfigPath);

    bool GetLinkPreNxt(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctliOutPreLinkIds, std::vector<HdLinkId> &vctliOutNxtLinkdIds);

    // 获取范围内和指定点接近的所有的Endpoint(按照距离排序)
    bool GetNearEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<Point> &vctptOutPoint, std::vector<LinkEndpoint> &vctleOutLinkEp, std::vector<double> &vctdOutDistance);
    bool GetNearEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<Point> &vctptOutPoint, std::vector<LaneEndpoint> &vctleOutLaneEp, std::vector<double> &vctdOutDistance);

    // 获取和指定点最近的std::vector<Endpoint
    bool GetNearestEndpoint(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, Point &ptOutPoint, LinkEndpoint &leOutEp, double &dOutDistance);
    bool GetNearestEndpoint(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, Point &ptOutPoint, LaneEndpoint &leOutEp, double &dOutDistance);

    bool GetLocateLanes(const Point &ptPoint, std::vector<HdLinkId> &vcthlOutLinkId, std::vector<HdLaneId> &vcthlOutLaneId);
    // 指定点所属于的Endpoint
    bool GetLocateEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<LinkEndpoint> &vctleOutLinkEp);
    bool GetLocateEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<LaneEndpoint> &vctleOutLaneEp);

    // 获得和当前点相连接的上一组Endpoint
    bool GetPreEndpoints(const LaneEndpoint &leCurEndpoint, std::vector<LaneEndpoint> &vctleOutPreEndpoint);

    /**根据当前的 LinkEndpoint获取下一组LinkEndpoint*/
    bool GetNextLinkEndpoints(const LinkEndpoint &leCurEndpoint, std::vector<LinkEndpoint> &vctleOutNextEndpoints);
    /**根据当前的LaneEndpoint获取下一组LaneEndpoint*/
    bool GetNextLaneEndpoints(const LaneEndpoint &leCurEndpoint, std::vector<LaneEndpoint> &vctleOutNextEndpoints);

    // 求某个点在轨迹点上的位置
    void GetPosInGeoLink(const Point &ptPos, const std::vector<std::vector<double>> &vctvdGeoLink, double &dOutToHeadDis, double &dOutToTailDis);

    bool GetNearestPointfromLaneGeo(const Point &ptPoint, Point &ptOutPoint, double &dOutDistance);
    // 获取lane上的数据
    bool GetLinkLength(const HdLinkId &lLinkId, double &dOutLinkLength);
    bool GetLinkType(const HdLinkId &lLinkId, int &nOutLinkType);
    bool GetLinkStartAngle(const HdLinkId &lLinkId, double &dOutStartAngle);
    bool GetLinkSpeedLimit(const HdLinkId &lLinkId, int &nOutSpeedLimit);
    bool GetLinkLimitValue(const HdLinkId &lLinkId, int nLimitType, int &nOutLimitValue);
    bool GetLinkRank(const HdLinkId &lLinkId, int &nOutRank);
    bool GetLinkPre(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctlOutPLinkId);
    bool GetLinkNext(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctlOutNLinkId);
    bool GetLinkNext(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<HdLinkId> &vctlOutNLinkId);
    bool GetLaneId(const HdLinkId &lLinkId, int nLaneSeqNum, HdLaneId &lOutLaneId);
    bool GetLaneNum(const HdLinkId &lLinkId, const HdLaneId &lLaneId, int &nOutLaneSeqNum);
    bool GetLaneType(const HdLinkId &lLinkId, int nLaneSeqNum, int &nOutLaneType);
    bool GetLaneWidth(const HdLinkId &lLinkId, int nLaneSeqNum, int &nOutLaneWidth);
    bool GetLaneEdgeId(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, HdEdgeId &lOutEdgeId);
    bool GetLaneNext(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<HdLaneId> &vctlOutNLaneId);
    bool GetLaneNext(const HdLinkId &lLinkId, const HdLaneId &lLaneId, std::vector<HdLaneId> &vctlOutNLaneId);
    bool GetLaneGeoLink(const HdLinkId &lLinkId, const HdLaneId &lLaneId, std::vector<std::vector<double>> &vctvdOutGeoLink);
    bool GetLaneGeoLink(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<std::vector<double>> &vctvdOutGeoLink);
    bool GetLaneCurvature(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutCurvature);
    bool GetLaneRoll(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutRoll);
    bool GetLanePitch(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutPitch);
    bool GetLaneAzimuth(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutAzimuth);
    bool GetLaneEdgeCrossType(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, int &nOutCrossType);
    bool GetLaneEdgeGeoLink(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, std::vector<std::vector<double>> &vctvdOutGeolink);
    bool GetEdgeCnt(const HdLinkId &lLinkId, int &nOutEdgeCnt);
    bool GetEdgeId(const HdLinkId &lLinkId, int nEdgeIdx, HdEdgeId &lOutEdgeId);
    bool GetEdgeCross(const HdLinkId &lLinkId, HdEdgeId lId, int &nOutEdgeCross);
    bool GetEdgeColor(const HdLinkId &lLinkId, HdEdgeId lId, int &nOutEdgeColor);
    bool GetEdgeGeoLink(const HdLinkId &lLinkId, HdEdgeId lId, std::vector<std::vector<double>> &vctvdOutGeolink);
    bool GetStopLineGeoLink(const HdLinkId &clLinkId, int nType, std::vector<std::vector<double>> &vctvdOutGeolink);
    bool GetTrafficLightGeoLink(const HdLinkId &clLinkId, int nLaneSeqNum, int nType, std::vector<std::vector<double>> &vctvdOutGeolink);
    bool GetOverHeadGeoLink(const HdLinkId &clLinkId, int nLaneSeqNum, int nType, std::vector<std::vector<double>> &vctvdOutGeolink);
    bool GetTrafficSign(const HdLinkId &clLinkId, std::vector<int> &vctnOutShape, std::vector<int> &vctnOutType1, std::vector<int> &vctnOutType2, std::vector<std::string> &vctstOutText, std::vector<std::vector<std::vector<double>>> &vctvvOutGeoArea);
    bool GetTrafficSign(const Point &cptPoint, std::vector<int> &vctnOutShape, std::vector<int> &vctnOutType1, std::vector<int> &vctnOutType2, std::vector<std::string> &vctstOutText, std::vector<std::vector<std::vector<double>>> &vctvvOutGeoArea);
    bool GetTrafficSignGeoArea(const HdLinkId &clLinkId, int nShape, std::vector<std::vector<double>> &vctvdOutGeometry);

    /****获取标志物对象数据**/
    template <typename T>
    bool QueryObjDataByLinkIdAndLaneNum(const HdLink *ObjHdLink, int laneNum, const std::vector<T> &In_VctObj, std::vector<T> &Out_VctObj) {
        Out_VctObj.clear();
        HdLaneId hdLaneIdTmp;
        for (HdLane hdLane : ObjHdLink->vcthlLane) {
            if (hdLane.nSeqNum == laneNum) {
                hdLaneIdTmp = hdLane.liId;
                break;
            }
        }
        if (hdLaneIdTmp == 0) {
            std::cout << "======ERROR===== QueryObjDataByLinkIdAndLaneNum方法laneNum输入有误！" << std::endl;
        }
        if (ObjHdLink != NULL) {
            for (T obj : In_VctObj) {
                for (HdLaneId hdLaneId : obj.vctlLaneId) {
                    if (hdLaneId == hdLaneIdTmp) {
                        Out_VctObj.push_back(obj);
                    }
                }
            }
        }
        return !Out_VctObj.empty();
    }

    bool IsLaneContainArrow(const HdLinkId &lLinkId, int nLaneSeqNum, char cArrowType);
    bool GetLaneArrow(const HdLinkId &lLinkId, const HdLaneId &lLaneId, std::string &strOutArrows);
    bool ConvertGeoLink(const std::vector<std::vector<double>> &vctvdGeolink, std::vector<Point> &vctptOutGeolink);
    bool ConvertGeoLink(const std::vector<Point> &vctptGeolink, std::vector<std::vector<double>> &vctvdOutGeolink);

    // 获取lane上的数据
    int GetLinkSpeedLimit(const LaneEndpoint &leCurEndpoint);
    bool HasStopLine(const LaneEndpoint &leCurEndpoint);
    bool GetGeoLink(const LaneEndpoint &leEndpoint, std::vector<std::vector<double>> &vctvdOutGeolink, bool bManageTile = false);
    bool GetGeoLink(const HdLinkId &lLinkId, std::vector<std::vector<double>> &vctvdOutGeolink);

    // 获取到左右边边缘线的距离
    double GetDistanceToLeftEdge(const LaneEndpoint &leEndpoint, const Point &ptPoint);
    double GetDistanceToRightEdge(const LaneEndpoint &leEndpoint, const Point &ptPoint);

    // 这个接口比较快
    // TODO : 为了兼容旧版本，hdlib不做tile管理
    bool GetLinkIdsInRect(const Point &ptPoint, float lon_interval_half_metre, float flatintervalhalfmetre, std::vector<HdLinkId> &vctlOutLinkId, bool bManageTile = true);

    // 注意这个接口可能会返回比lon_interval_half_metre更大范围的区域，因为地图上的点是被稀释过的
    // 可以选择GetDataNearby_param参数过滤和排序
    // TODO : 为了兼容旧版本，hdlib不做tile管理
    bool GetDataNearby(const Point &ptPoint, float lon_interval_half_metre, float flatintervalhalfmetre, std::vector<const HdLink *> &vcthlOutLink, GetDataNearby_param p = GetDataNearby_param::none, bool bManageTile = true);

    // 查看link是否可达
    bool IsLinkArrivable(const HdLink *hlFrom, const HdLink *hlTo, int nMaxDepth = 5);
    bool LoadTile(const Point &ptPoint, bool nManageTile = true);
    const HdLink *GetLinkData(const HdLinkId &lLinkId);
    bool GetLaneData(const HdLinkId &lLinkId, int lane_no, HdLane &hdLaneOut);
    std::set<HdLinkId> GetNodeIdsInRect(const Rect2D &rc2Rect);

    // 获取某个点在道路参考线上距离开始结束点的距离(dOutToHeadDis: 到开始点的距离，dOutToTailDis：到结束点的距离)
    bool GetPosInHdLink(const Point &ptCurPos, const HdLinkId &lLinkId, double &dOutToHeadDis, double &dOutToTailDis);

    // 根据车道类型获取车道的集合
    bool GetLanesInTypes(std::vector<int> &vctnLaneType, const HdLink *hlLink, std::vector<const HdLane *> &vcthlOutLanes);
    bool GetLanesOutTypes(std::vector<int> &vctnLaneType, const HdLink *hlLink, std::vector<const HdLane *> &vcthlOutLanes);

    int GetEdgeIndexById(const HdLink *cphlLink, HdEdgeId lId);
    bool IsReachable(const HdLink *hlLink, HdLaneId lLane1, HdLaneId lLane2);
    bool GetLaneCnt(const HdLinkId &lLinkId, int &nOutLaneCnt);

    // 测试用
    std::vector<Point> LoadGpses();

   private:
    // 用于计算一组link内，从一个lane的起点可以到达的其他lane的终点（link内全联通）的数据结构
    struct LaneEndpointExt : LaneEndpoint {
        Point ptSpNode;                     // 坐标点（取车道的起点）
        bool bLChangable;                   // 是否可以向左变道
        bool bRChangable;                   // 是否可以向右变道
        int nLaneNum;                       // 当前车道的序号
        std::vector<HdLaneId> vctPreLanes;  // 可以到达该车道的所有前车道的集合
        std::vector<HdLaneId> vctNxtLanes;  // 可以到达该车道的所有后车道的集合
    };
    HdLib(const HdLib &obj) = delete;
    HdLib &operator=(const HdLib &obj) = delete;

    void GenCrossable(const HdLink *hlLink, std::map<int, const HdLane *> &mapOutIdxLane, std::vector<std::vector<int>> &vctvnOutRightCrossable, std::vector<std::vector<int>> &vctvnOutLeftCrossable);
    void GenPreEndpoints(HdLaneId lCurLaneId, const std::vector<const HdLink *> &vcthlPreLink, std::vector<LaneEndpoint> &vctleOutPreEps);
    int GetEdgeCross(const HdLink *hlLink, const HdEdgeId &lId);
    void GenPreRLanes(HdLaneId lCurLaneId, const HdLink *cphlPreLink, const std::map<int, LaneEndpointExt> &mapLaneNumEpExt, std::vector<LaneEndpoint> &vctleOutPreEps);
    bool IsPointInLane(const Point &point, const HdLink *cphlLink, const HdLane &hdLane);

    HdWorld m_hwHdWorld;
};
};  // namespace jf

#endif
