#include "MapInterface.hpp"

#include <cmath>
#include <iostream>
#include <limits>
namespace jf {
MapInterface::MapInterface() {  }

MapInterface::~MapInterface() {}

void MapInterface::Init(const std::string& strPrjPath, const std::string strCfgPath)
{
    m_mlMapLib.Init(strPrjPath, strCfgPath);
}

bool MapInterface::GetMapData(const Point &ptInLoc, double dCarAngle, int &nOutLaneCnt, int &nOutLaneNum, LaneType &nOutLaneType, EdgeCrossType &nOutLeftEdgeCrossType, EdgeCrossType &nOutRightEdgeCrossType, int &nOutSpeedLimit, double &dOutLaneAngle,
                              Point &ptOutFoot) {
    long lRoadId;
    std::vector<long> vctlPreRoadId = {};
    std::vector<long> vctlNxtRoadId = {};

    return GetMapData(ptInLoc, dCarAngle, lRoadId, vctlPreRoadId, vctlNxtRoadId, nOutLaneCnt, nOutLaneNum, nOutLaneType, nOutLeftEdgeCrossType, nOutRightEdgeCrossType, nOutSpeedLimit, dOutLaneAngle, ptOutFoot);
}

bool MapInterface::GetMapData(const Point &ptInLoc, double dCarAngle, long &lOutRaodId, std::vector<long> &vctlOutPreRoadId, std::vector<long> &vctlOutNxtRoadId, int &nOutLaneCnt, int &nOutLaneNum, LaneType &nOutLaneType,
                              EdgeCrossType &nOutLeftEdgeCrossType, EdgeCrossType &nOutRightEdgeCrossType, int &nOutSpeedLimit, double &dOutLaneAngle, Point &ptOutFoot) {
    std::vector<HdLinkId> vcthlLinkIds = {};
    std::vector<HdLaneId> vcthlLaneIds = {};
    // Locate lane failed
    if (false == m_mlMapLib.GetLocateLanes(ptInLoc, vcthlLinkIds, vcthlLaneIds)) {
        nOutLaneCnt = 0;
        nOutLaneNum = 0;
        nOutLaneType = LaneType::NotInvestigated;
        nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
        nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
        nOutSpeedLimit = 0;
        dOutLaneAngle = 0.0;
        ptOutFoot = ptInLoc;
        //std::cout << "MapInterface::GetMapData(), Locate lane failed." << std::endl;
        return false;
    }

    int nIndex = 0;
    double dMinAngleDiff = std::numeric_limits<double>::max();
    auto func_GetAngleDiff = [](double dAngle1, double dAngle2) { return std::min(std::abs(dAngle1 - dAngle2), 360 - std::abs((dAngle1 - dAngle2))); };
    for (int i = 0; i < vcthlLinkIds.size(); i++) {
        const HdLinkId &hlLinkId = vcthlLinkIds[i];
        const HdLaneId &hlLaneId = vcthlLaneIds[i];
        double dAzimuth = 0;
        if (vcthlLinkIds.size() == 1) {
            nIndex = i;
        } else {
            if (true == m_mlMapLib.GetLaneAzimuth(hlLinkId, hlLaneId, ptInLoc, dAzimuth) || true == m_mlMapLib.GetLinkStartAngle(hlLinkId, dAzimuth)) {
                double dAngleDiff = func_GetAngleDiff(dCarAngle, dAzimuth);
                if (dMinAngleDiff >= dAngleDiff) {
                    dMinAngleDiff = dAngleDiff;
                    nIndex = i;
                }
            }
        }
    }

    lOutRaodId = vcthlLinkIds[nIndex].lValue;
    std::vector<HdLinkId> vctliPLinkIds = {};
    std::vector<HdLinkId> vctliNLinkIds = {};
    m_mlMapLib.GetLinkPreNxt(vcthlLinkIds[nIndex], vctliPLinkIds, vctliNLinkIds);
    for (int i = 0; i < vctliPLinkIds.size(); ++i) {
        vctlOutPreRoadId.push_back(vctliPLinkIds.at(i).lValue);
    }
    for (int i = 0; i < vctliNLinkIds.size(); ++i) {
        vctlOutNxtRoadId.push_back(vctliNLinkIds.at(i).lValue);
    }

    if (false == m_mlMapLib.GetLaneCnt(vcthlLinkIds[nIndex], nOutLaneCnt)) {
        nOutLaneCnt = 0;
        nOutLaneNum = 0;
        nOutLaneType = LaneType::NotInvestigated;
        nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
        nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
        nOutSpeedLimit = 0;
        dOutLaneAngle = 0.0;
        ptOutFoot = {};
        std::cout << "MapInterface::GetMapData(), get lane count failed." << std::endl;
        return false;
    }

    if (false == m_mlMapLib.GetLaneNum(vcthlLinkIds[nIndex], vcthlLaneIds[nIndex], nOutLaneNum)) {
        nOutLaneNum = 0;
        nOutLaneType = LaneType::NotInvestigated;
        nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
        nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
        nOutSpeedLimit = 0;
        dOutLaneAngle = 0.0;
        ptOutFoot = {};
        std::cout << "MapInterface::GetMapData(), get lane num failed." << std::endl;
        return false;
    }

    if (false == m_mlMapLib.GetLaneType(vcthlLinkIds[nIndex], nOutLaneNum, (int &)nOutLaneType)) {
        nOutLaneType = LaneType::NotInvestigated;
        nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
        nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
        nOutSpeedLimit = 0;
        dOutLaneAngle = 0.0;
        ptOutFoot = {};
        std::cout << "MapInterface::GetMapData(), get lane type failed." << std::endl;
        return false;
    }

    if (false == m_mlMapLib.GetLaneEdgeCrossType(vcthlLinkIds[nIndex], nOutLaneNum, true, (int &)nOutLeftEdgeCrossType)) {
        nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
        nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
        nOutSpeedLimit = 0;
        dOutLaneAngle = 0.0;
        ptOutFoot = {};
        std::cout << "MapInterface::GetMapData(), get left lane edge cross failed." << std::endl;
        return false;
    }

    if (false == m_mlMapLib.GetLaneEdgeCrossType(vcthlLinkIds[nIndex], nOutLaneNum, false, (int &)nOutRightEdgeCrossType)) {
        nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
        nOutSpeedLimit = 0;
        dOutLaneAngle = 0.0;
        ptOutFoot = {};
        std::cout << "MapInterface::GetMapData(), get right lane cross failed." << std::endl;
        return false;
    }

    if (false == m_mlMapLib.GetLinkSpeedLimit(vcthlLinkIds[nIndex], nOutSpeedLimit)) {
        nOutSpeedLimit = 0;
        dOutLaneAngle = 0.0;
        ptOutFoot = {};
        std::cout << "MapInterface::GetMapData(), get link speed failed." << std::endl;
        return false;
    }

    if (false == m_mlMapLib.GetLaneAzimuth(vcthlLinkIds[nIndex], vcthlLaneIds[nIndex], ptInLoc, dOutLaneAngle)) {
        dOutLaneAngle = 0.0;
        ptOutFoot = {};
        std::cout << "MapInterface::GetMapData(), get lane angle failed." << std::endl;
        return false;
    }

    int nSeqNum = 0;
    if (false == m_mlMapLib.GetLaneNum(vcthlLinkIds[nIndex], vcthlLaneIds[nIndex], nSeqNum)) {
        ptOutFoot = {};
        std::cout << "MapInterface::GetMapData(), get lane num failed." << std::endl;
        return false;
    } else {
        std::vector<std::vector<double>> vctvdGeoLink = {};
        double dDistance = 0;
        if (false == m_mlMapLib.GetLaneGeoLink(vcthlLinkIds[nIndex], nSeqNum, vctvdGeoLink)) {
            ptOutFoot = {};
            //std::cout << "MapInterface::GetMapData(), get lane geolink failed." << std::endl;
            return false;
        } else {
            if (false == m_mlMapLib.GetPerpendicularFoot(ptInLoc, vctvdGeoLink, ptOutFoot, dDistance)) {
                ptOutFoot = {};
                std::cout << "MapInterface::GetMapData(), get lane foot point failed." << std::endl;
                return false;
            }
        };
    }

    return true;
}

}  // namespace jf