#include "MapLib.h"

#include "HdLibMath.h"

using namespace jf;

MapLib::MapLib() { m_spHdLib = HdLib::GetInstance(); }

void MapLib::Init(const std::string &strProjectPath, const std::string &strMapConfigPath) { return m_spHdLib->Init(strProjectPath, strMapConfigPath); }

bool MapLib::BFSLinks(const HdLinkId &from, double &ret, bool backward, bool &outFound, double outIterMetre, HdLinkId &outLink) {
    auto func_GetTunnelInfo = [this, &outFound, backward, &outIterMetre, &outLink](bool visited, bool first, const jf::HdLinkId &id, double &outLength, std::set<jf::HdLinkId> &outNexts) {
        static const int sc_nRoadType_Channel = 3;
        if (visited) {
            return false;
        }
        if (outLength >= outIterMetre) {
            return false;
        }
        if (!first) {
            double link_length = 0;
            m_spHdLib->GetLinkLength(id, link_length);
            outLength += link_length;
        }
        int road_type = 0;
        m_spHdLib->GetLinkType(id, road_type);
        if (road_type == sc_nRoadType_Channel) {
            outFound = true;
            outLink = id;
            return true;
        }
        std::vector<jf::HdLinkId> connected_lks = {};
        if (backward) {
            m_spHdLib->GetLinkNext(id, connected_lks);
        } else {
            m_spHdLib->GetLinkPre(id, connected_lks);
        }
        for (const jf::HdLinkId &id : connected_lks) outNexts.insert(id);
        return false;
    };

    std::set<HdLinkId> nexts;
    if (func_GetTunnelInfo(false, true, from, ret, nexts)) {
        return true;
    };
    std::set<HdLinkId> visited{from};
    while (nexts.size() != 0) {
        std::set<HdLinkId> current(nexts.begin(), nexts.end());
        nexts.clear();
        for (const HdLinkId &c : current) {
            if (func_GetTunnelInfo(jf::Utils::Contains(visited, c), false, c, ret, nexts)) {
                visited.insert(c);
                return true;
            };
            visited.insert(c);
        }
    }
    return true;
}

bool MapLib::GetLinkPreNxt(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctliOutPreLinkIds, std::vector<HdLinkId> &vctliOutNxtLinkdIds) { return m_spHdLib->GetLinkPreNxt(lLinkId, vctliOutPreLinkIds, vctliOutNxtLinkdIds); }

void MapLib::HmmBindRoad(const BindRoadInput &input, BindRoadOutput &output) { return m_brdBindroad.BindRoadHmm(input, output); }

// bool MapLib::GetDataNearby(const Point &ptPoint, float fLonIntervalHalfMetre, float fLatIntervalHalfMetre, std::vector<HdLinkId> &vcthlOutLink) { return m_spHdLib->GetDataNearby(ptPoint, fLonIntervalHalfMetre, fLatIntervalHalfMetre, vcthlOutLink); }
bool MapLib::GetIsInCross(const Point& ptPoint)
{
    return m_spHdLib->GetIsInCross(ptPoint);
}

bool MapLib::GetNearEndpoints(const Point &ptPoint, std::vector<Point> &vctptOutPoint, std::vector<LinkEndpoint> &vctleOutLinkEp, std::vector<double> &vctdOutDistance) {
    return m_spHdLib->GetNearEndpoints(ptPoint, MapConfig::route_rect_lon, MapConfig::route_rect_lat, vctptOutPoint, vctleOutLinkEp, vctdOutDistance);
}

bool MapLib::GetNearEndpoints(const Point &ptPoint, std::vector<Point> &vctptOutPoint, std::vector<LaneEndpoint> &vctleOutLaneEp, std::vector<double> &vctdOutDistance) {
    return m_spHdLib->GetNearEndpoints(ptPoint, MapConfig::route_rect_lon, MapConfig::route_rect_lat, vctptOutPoint, vctleOutLaneEp, vctdOutDistance);
}

bool MapLib::GetNearestEndpoint(const Point &ptPoint, Point &ptOutPoint, LinkEndpoint &leOutEndpoint, double &dOutDistance) {
    return m_spHdLib->GetNearestEndpoint(ptPoint, MapConfig::route_rect_lon, MapConfig::route_rect_lat, ptOutPoint, leOutEndpoint, dOutDistance);
}

bool MapLib::GetNearestEndpoint(const Point &ptPoint, Point &ptOutPoint, LaneEndpoint &leOutEndpoint, double &dOutDistance) {
    return m_spHdLib->GetNearestEndpoint(ptPoint, MapConfig::route_rect_lon, MapConfig::route_rect_lat, ptOutPoint, leOutEndpoint, dOutDistance);
}

bool MapLib::GetLocateEndpoints(const Point &ptPoint, std::vector<LinkEndpoint> &vctleOutLinkEp) { return m_spHdLib->GetLocateEndpoints(ptPoint, MapConfig::route_rect_lon, MapConfig::route_rect_lat, vctleOutLinkEp); }

bool MapLib::GetLocateEndpoints(const Point &ptPoint, std::vector<LaneEndpoint> &vctleOutLaneEp) { return m_spHdLib->GetLocateEndpoints(ptPoint, MapConfig::route_rect_lon, MapConfig::route_rect_lat, vctleOutLaneEp); }

bool MapLib::GetPreEndpoints(const LaneEndpoint &leCurEndpoint, std::vector<LaneEndpoint> &vctleOutPreEndpoint) { return m_spHdLib->GetPreEndpoints(leCurEndpoint, vctleOutPreEndpoint); }

// bool MapLib::GetNextEndpoints(const LinkEndpoint &leCurEndpoint, std::vector<LinkEndpoint> &vctleOutNextEndpoints) { return m_spHdLib->GetNextEndpoints(leCurEndpoint, vctleOutNextEndpoints); }

// bool MapLib::GetNextEndpoints(const LaneEndpoint &leCurEndpoint, std::vector<LaneEndpoint> &vctleOutNextEndpoints) { return m_spHdLib->GetNextEndpoints(leCurEndpoint, vctleOutNextEndpoints); }

bool MapLib::GetLocateLanes(const Point &ptPoint, std::vector<HdLinkId> &vcthlOutLink, std::vector<HdLaneId> &vcthlOutLane) { return m_spHdLib->GetLocateLanes(ptPoint, vcthlOutLink, vcthlOutLane); }

// bool MapLib::GetPerpendicularFoot(const HdLinkId &lLinkId, int nLaneNum, const Point &ptPoint, Point &ptOutPoint) { return HdLibMath::GetPerpendicularFoot(lLinkId, nLaneNum, ptPoint, ptOutPoint); }

bool MapLib::GetPerpendicularFoot(const Point &ptPoint, const std::vector<std::vector<double>> &vctvdGeoLink, Point &ptOutPoint, double &dOutDistance) { return HdLibMath::GetPerpendicularFoot(ptPoint, vctvdGeoLink, ptOutPoint, dOutDistance); }

// bool MapLib::GetNearestDistance(const Point &ptPoint, const std::vector<std::vector<double>> &vctvdGeoLink, Point &ptOutPoint, double &dOutDistance, int &nOutIndex) {
//     return HdLibMath::GetNearestDistance(ptPoint, vctvdGeoLink, ptOutPoint, dOutDistance, nOutIndex);
// }

bool MapLib::GetLinkLength(const HdLinkId &lLinkId, double &dOutLinkLength) { return m_spHdLib->GetLinkLength(lLinkId, dOutLinkLength); }

bool MapLib::GetLinkType(const HdLinkId &lLinkId, int &nOutLinkType) { return m_spHdLib->GetLinkType(lLinkId, nOutLinkType); }

bool MapLib::GetLinkStartAngle(const HdLinkId &lLinkId, double &dOutStartAngle) { return m_spHdLib->GetLinkStartAngle(lLinkId, dOutStartAngle); }

bool MapLib::GetLinkSpeedLimit(const HdLinkId &lLinkId, int &nOutSpeedLimit) { return m_spHdLib->GetLinkSpeedLimit(lLinkId, nOutSpeedLimit); }

// bool MapLib::GetLinkLimitValue(const HdLinkId &lLinkId, int nLimitType, int &nOutLimitValue) { return m_spHdLib->GetLinkLimitValue(lLinkId, nLimitType, nOutLimitValue); }

bool MapLib::GetLinkRank(const HdLinkId &lLinkId, int &nOutRank) { return m_spHdLib->GetLinkRank(lLinkId, nOutRank); }

bool MapLib::GetLinkPre(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctlOutPLinkId) { return m_spHdLib->GetLinkPre(lLinkId, vctlOutPLinkId); }

bool MapLib::GetLinkNext(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctlOutNLinkId) { return m_spHdLib->GetLinkNext(lLinkId, vctlOutNLinkId); }

bool MapLib::GetLinkNext(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<HdLinkId> &vctlOutNLinkId) { return m_spHdLib->GetLinkNext(lLinkId, nLaneSeqNum, vctlOutNLinkId); }

bool MapLib::GetLaneCnt(const HdLinkId &lLinkId, int &nOutLanesCnt) { return m_spHdLib->GetLaneCnt(lLinkId, nOutLanesCnt); }

bool MapLib::GetLaneId(const HdLinkId &lLinkId, int nLaneSeqNum, HdLaneId &lOutLaneId) { return m_spHdLib->GetLaneId(lLinkId, nLaneSeqNum, lOutLaneId); }

bool MapLib::GetLaneNum(const HdLinkId &lLinkId, const HdLaneId &lLaneId, int &nOutLaneSeqNum) { return m_spHdLib->GetLaneNum(lLinkId, lLaneId, nOutLaneSeqNum); }

bool MapLib::GetLaneType(const HdLinkId &lLinkId, int nLaneSeqNum, int &nOutLaneType) { return m_spHdLib->GetLaneType(lLinkId, nLaneSeqNum, nOutLaneType); }

bool MapLib::GetLaneNext(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<HdLaneId> &vctlOutNLaneId) { return m_spHdLib->GetLaneNext(lLinkId, nLaneSeqNum, vctlOutNLaneId); }

bool MapLib::GetLaneWidth(const HdLinkId &lLinkId, int nLaneSeqNum, int &nOutLaneWidth) { return m_spHdLib->GetLaneWidth(lLinkId, nLaneSeqNum, nOutLaneWidth); }

bool MapLib::GetLaneGeoLink(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<std::vector<double>> &vctvdOutGeoLink) { return m_spHdLib->GetLaneGeoLink(lLinkId, nLaneSeqNum, vctvdOutGeoLink); }

bool MapLib::GetLaneAzimuth(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutAzimuth) { return m_spHdLib->GetLaneAzimuth(lLinkId, lLaneId, ptInPoint, dOutAzimuth); }

bool MapLib::GetLaneEdgeId(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, HdEdgeId &lOutEdgeId) { return m_spHdLib->GetLaneEdgeId(lLinkId, nLaneSeqNum, bIsLeft, lOutEdgeId); }

bool MapLib::GetEdgeCnt(const HdLinkId &lLinkId, int &nOutEdgeCnt) { return m_spHdLib->GetEdgeCnt(lLinkId, nOutEdgeCnt); }

bool MapLib::GetEdgeId(const HdLinkId &lLinkId, int nEdgeIdx, HdEdgeId &lOutEdgeId) { return m_spHdLib->GetEdgeId(lLinkId, nEdgeIdx, lOutEdgeId); }

bool MapLib::GetEdgeCross(const HdLinkId &lLinkId, HdEdgeId lId, int &nOutEdgeCross) { return m_spHdLib->GetEdgeCross(lLinkId, lId, nOutEdgeCross); }

bool MapLib::GetEdgeColor(const HdLinkId &lLinkId, HdEdgeId lId, int &nOutEdgeColor) { return m_spHdLib->GetEdgeColor(lLinkId, lId, nOutEdgeColor); }

bool MapLib::GetLaneEdgeCrossType(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, int &nOutCrossType) { return m_spHdLib->GetLaneEdgeCrossType(lLinkId, nLaneSeqNum, bIsLeft, nOutCrossType); }

bool MapLib::GetLaneEdgeGeoLink(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, std::vector<std::vector<double>> &vctvdOutGeoLink) { return m_spHdLib->GetLaneEdgeGeoLink(lLinkId, nLaneSeqNum, bIsLeft, vctvdOutGeoLink); }

bool MapLib::GetEdgeGeoLink(const HdLinkId &lLinkId, HdEdgeId lId, std::vector<std::vector<double>> &vctvdOutGeoLink) { return m_spHdLib->GetEdgeGeoLink(lLinkId, lId, vctvdOutGeoLink); }

bool MapLib::GetStopLineGeoLink(const HdLinkId &clLinkId, int nType, std::vector<std::vector<double>> &vctvdOutGeoLink) { return m_spHdLib->GetStopLineGeoLink(clLinkId, nType, vctvdOutGeoLink); }

// bool MapLib::GetTrafficLightGeoLink(const HdLinkId &clLinkId, int nLaneSeqNum, int nType, std::vector<std::vector<double>> &vctvdOutGeoLink) { return m_spHdLib->GetTrafficLightGeoLink(clLinkId, nLaneSeqNum, nType, vctvdOutGeoLink); }

bool MapLib::GetOverHeadGeoLink(const HdLinkId &clLinkId, int nLaneSeqNum, int nType, std::vector<std::vector<double>> &vctvdOutGeoLink) { return m_spHdLib->GetOverHeadGeoLink(clLinkId, nLaneSeqNum, nType, vctvdOutGeoLink); }

bool MapLib::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) {
    return m_spHdLib->GetTrafficSign(clLinkId, vctnOutShape, vctnOutType1, vctnOutType2, vctstOutText, vctvvOutGeoArea);
}

bool MapLib::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) {
    return m_spHdLib->GetTrafficSign(cptPoint, vctnOutShape, vctnOutType1, vctnOutType2, vctstOutText, vctvvOutGeoArea);
}

bool MapLib::GetTrafficSignGeoArea(const HdLinkId &clLinkId, int nShape, std::vector<std::vector<double>> &vctvdOutGeometry) { return m_spHdLib->GetTrafficSignGeoArea(clLinkId, nShape, vctvdOutGeometry); }

bool MapLib::GetGeoLink(const LaneEndpoint &leEndpoint, std::vector<std::vector<double>> &vctvdOutGeoLink, bool bManageTile /* = false*/) { return m_spHdLib->GetGeoLink(leEndpoint, vctvdOutGeoLink, bManageTile /* = false*/); }

bool MapLib::GetGeoLink(const HdLinkId &lLinkId, std::vector<std::vector<double>> &vctvdOutGeoLink) { return m_spHdLib->GetGeoLink(lLinkId, vctvdOutGeoLink); }

bool MapLib::IsLaneContainArrow(const HdLinkId &lLinkId, int nLaneSeqNum, char cArrowType) { return m_spHdLib->IsLaneContainArrow(lLinkId, nLaneSeqNum, cArrowType); }

bool MapLib::ConvertGeoLink(const std::vector<std::vector<double>> &vctvdGeoLink, std::vector<Point> &vctptOutGeoLink) { return m_spHdLib->ConvertGeoLink(vctvdGeoLink, vctptOutGeoLink); }

bool MapLib::ConvertGeoLink(const std::vector<Point> &vctptGeoLink, std::vector<std::vector<double>> &vctvdOutGeoLink) { return m_spHdLib->ConvertGeoLink(vctptGeoLink, vctvdOutGeoLink); }