
#include "MapInterface.hpp"
#include "OdrManager.h"
#include "Component.h"

using namespace OpenDrive;

OpenDrive::OdrManager g_map;
int g_load = 0;

namespace jf {
    MapInterface::MapInterface(const std::string& strPrjPath, const std::string strCfgPath)
    {
        std::string filename = strPrjPath + strCfgPath;
        bool bRtn = g_map.LoadFile(filename.c_str());
        if (bRtn)
            g_load = 1;
        std::cout << "load map file = " << filename << " ret = " << bRtn << std::endl;
    }

    MapInterface::~MapInterface()
    {
    }

    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 = {};
		int id;
        return GetMapData(ptInLoc, dCarAngle, lRoadId, vctlPreRoadId, vctlNxtRoadId, nOutLaneCnt, nOutLaneNum, nOutLaneType, nOutLeftEdgeCrossType, nOutRightEdgeCrossType, nOutSpeedLimit, dOutLaneAngle, ptOutFoot,id);
    }

    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,int& id)
    {
        if (g_load == 0)
            return false;
		uint64_t begin = GetCurTime();
		uint64_t start = GetCurTime();
		uint64_t end1,end2,end3,end4,end5;
#ifdef _DEBUG_API_
        std::vector<OdrInfo> odr = g_map.GetOdrInfoByXY(ptInLoc.dLon, ptInLoc.dLat);
        printf("odr size = %d\n", odr.size());
        for (auto iter : odr)
        {
            printf("roadID = %s,sectionID = %s,laneID = %s, _s = %f. _t = %f \n", iter._roadID, iter._sectionID, iter._laneID, iter._s, iter._t);
        }
#else
        std::vector<OdrInfo> odr = g_map.GetOdrInfo(ptInLoc.dLon, ptInLoc.dLat);
#endif
		end1 = GetCurTime() - begin;
		begin = GetCurTime();
        if (odr.size() == 0)
        {
            printf("not 2 in map lon = %.8f, lat = %.8f\n", ptInLoc.dLon, ptInLoc.dLat);
            return false;
        }
        std::string roadId;
        std::string sectionId;
        std::string laneId;
#ifdef _DEBUG_API_
        double angle = g_map.GetHdgByXY(3926.381879, 11180.870534, dCarAngle, roadId);
        std::cout << "GetHdgByXY comValue = " << dCarAngle << ",return = " << angle << ",roadId = " << roadId << std::endl;
#else
        double carAngle = (90 - dCarAngle)/180*3.1415926;
		//double angle = g_map.GetHdg(ptInLoc.dLon, ptInLoc.dLat, dCarAngle, roadId);
		long int roadLaneId = 0;
		int rett = g_map.GetHdg(odr,carAngle,roadLaneId);
		if(rett)
			id = roadLaneId;
#endif
		double angle = 0;
		double min_angle = 3.14159265;
		for (auto its : odr)
        {
            //std::string id = its._roadID;
			double roadAngle = 0;
			roadAngle = g_map.GetDirection(its._roadID,its._s);
			printf("GetDirection roadAngle = %f, carAngle = %f\n",roadAngle,carAngle);
			double tmp = roadAngle - carAngle;
			if(tmp < -3.14159265)
				tmp += 2*3.14159265;
			if(tmp > 3.14159265)
				tmp -= 2*3.14159265;
			double deta = abs(tmp);
			if(deta < min_angle)
			{
				min_angle = deta;
				angle = roadAngle;
				roadId = its._roadID;
				sectionId = its._sectionID;
				laneId = its._laneID;
			}
           // if (id == roadId)
            //{
            //    sectionId = its._sectionID;
            //    laneId = its._laneID;
            //    break;
            //}
        }

        dOutLaneAngle = -(angle*180/3.1415926 - 90);
		if(dOutLaneAngle < 0)
			dOutLaneAngle += 360;
        end2 = GetCurTime() - begin;
        begin = GetCurTime();
		if(roadId == "")
			return false;
        lOutRaodId = std::stol(roadId);
        int _sectionId = sectionId == "" ? 0 : std::stol(sectionId);
        int _laneId = laneId == "" ? 0:  std::stol(laneId);
		printf("GetHdg laneId = %d, dCarAngle = %f, carAngle = %f, angle = %f, dOutLandAngle = %f,ptInLoc.dLon = %.8f, ptInLoc.dLat = %.8f\n",_laneId,dCarAngle,carAngle,angle,dOutLaneAngle,ptInLoc.dLon, ptInLoc.dLat);
#ifdef _DEBUG_API_
        std::cout << "lOutRaodId = " << lOutRaodId << ",_sectionId = " << _sectionId << ", _laneId = " << _laneId << std::endl;
#endif
        char** preRoadId = nullptr;
        int preSize = 0;
        bool retPre = g_map.GetPreRoadID(roadId.c_str(), preRoadId, preSize);
        if (retPre)
        {
#ifdef _DEBUG_API_
            std::cout << "vctlOutPreRoadId size = " << preSize << std::endl;
#endif
            for (int i = 0; i < preSize; i++)
            {
                std::string prdId = preRoadId[i];
                long preIdx = std::stol(prdId);
                vctlOutPreRoadId.push_back(preIdx);
#ifdef _DEBUG_API_
                std::cout << "vctlOutPreRoadId i = " << i << ", roadId = " << preIdx << std::endl;
#endif
            }
        }
        char** nextRoadId = nullptr;
        int nextSize = 0;
        bool retNext = g_map.GetSucRoadID(roadId.c_str(), nextRoadId, nextSize);
        if (retNext)
        {
#ifdef _DEBUG_API_
            std::cout << "vctlOutNxtRoadId size = " << nextSize << std::endl;
#endif
            for (int i = 0; i < nextSize; i++)
            {
                std::string nextId = nextRoadId[i];
                long nextIdx = std::stol(nextId);
                vctlOutNxtRoadId.push_back(nextIdx);
#ifdef _DEBUG_API_
                std::cout << "vctlOutNxtRoadId i = " << i << ", roadId = " << nextIdx << std::endl;
#endif
            }
        }
        char** allLaneIds = nullptr;
        int allLaneSize = 0;
        bool retALane = g_map.GetAllDrivingLaneIDS(roadId.c_str(), sectionId.c_str(), allLaneIds, allLaneSize);
        if (retALane)
        {
#ifdef _DEBUG_API_
            std::cout << "GetAllDrivingLaneIDS size = " << allLaneSize << std::endl;
            for (int i = 0; i < allLaneSize; i++)
            {
                std::cout << "GetAllDrivingLaneIDS i = " << i << ", laneId = " << allLaneIds[i] << std::endl;
            }
#endif
        }
        nOutLaneCnt = allLaneSize;
        nOutLaneNum = abs(_laneId);
        end3 = GetCurTime() - begin;
        begin = GetCurTime();
        std::string typeString = g_map.GetLaneTypeString(roadId.c_str(), sectionId.c_str(), laneId.c_str());
#ifdef _DEBUG_API_
        std::cout << "GetLaneTypeString  type = " << typeString << std::endl;
#endif
        // "Driving" "Biking" "Shoulder" "SideWalk" "Parking" "Restricted" "Border" "none"

        if ("Driving" == typeString)
        {
            nOutLaneType = LaneType::NormalLane;
        }
        else if ("Biking" == typeString)
        {
            nOutLaneType = LaneType::NonMotorizedLane;
        }
        else if ("Shoulder" == typeString)
        {
            nOutLaneType = LaneType::NormalLane;
        }
        else if ("SideWalk" == typeString)
        {
            nOutLaneType = LaneType::Sidewalk;
        }
        else if ("Parking" == typeString)
        {
            nOutLaneType = LaneType::StopLane;
        }
        else if ("Restricted" == typeString)
        {
            nOutLaneType = LaneType::NormalLane;
        }
        else if ("Border" == typeString)
        {
            nOutLaneType = LaneType::NormalLane;
        }
        else
        {
            nOutLaneType = LaneType::Ohters;
        }
#ifdef _DEBUG_API_
        std::cout << "nOutLaneType = " << static_cast<int>(nOutLaneType) << std::endl;
#endif
        bool isJunction = g_map.bJunctionRoad(roadId.c_str());
#ifdef _DEBUG_API_
        std::cout << "bJunctionRoad = " << isJunction << std::endl;
#endif
        if (isJunction)
        {
            std::cout << "bJunctionRoad is true" << std::endl;
			nOutLaneType = LaneType::JunctionLane;
        }
		end4 = GetCurTime() - begin;
        begin = GetCurTime();
#ifdef _DEBUG_API_
        NearestRoadMark mark = g_map.GetNeartestLaneRoadMarkByXY(ptInLoc.dLon, ptInLoc.dLat);
#else
        //NearestRoadMark mark = g_map.GetNeartestLaneRoadMark(ptInLoc.dLon, ptInLoc.dLat);
        NearestRoadMark mark = g_map.GetNeartestLaneRoadMarkByID(roadId.c_str(), sectionId.c_str(), laneId.c_str());
#endif
        if (mark._leftRoadMark)
        {
            if (mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID || mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID_SOLID)
                nOutLeftEdgeCrossType = EdgeCrossType::Unable;
            else if (mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_BROKEN)
                nOutLeftEdgeCrossType = EdgeCrossType::BidirecAble;
            else if (mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID_BROKEN)
                nOutLeftEdgeCrossType = EdgeCrossType::LeftAble;
            else if (mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_BROKEN_SOLID)
                nOutLeftEdgeCrossType = EdgeCrossType::RightAble;
            else
                nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
#ifdef _DEBUG_API_

	  std::cout << "_leftRoadMark _type = " << mark._leftRoadMark->_type << std::endl;
            std::cout << "nOutLeftEdgeCrossType = " << static_cast<int>(nOutLeftEdgeCrossType) << std::endl;
#endif
        }
        if (mark._rightRoadMark)
        {
            if (mark._rightRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID || mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID_SOLID)
                nOutRightEdgeCrossType = EdgeCrossType::Unable;
            else if (mark._rightRoadMark->_type == ODR_ROAD_MARK_TYPE_BROKEN)
                nOutRightEdgeCrossType = EdgeCrossType::BidirecAble;
            else if (mark._rightRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID_BROKEN)
                nOutRightEdgeCrossType = EdgeCrossType::LeftAble;
            else if (mark._rightRoadMark->_type == ODR_ROAD_MARK_TYPE_BROKEN_SOLID)
                nOutRightEdgeCrossType = EdgeCrossType::RightAble;
            else
                nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
#ifdef _DEBUG_API_
            std::cout << "_rightRoadMark _type = " << mark._rightRoadMark->_type << std::endl;
            std::cout << "nOutRightEdgeCrossType = " << static_cast<int>(nOutRightEdgeCrossType) << std::endl;
#endif
        }
		end5 = GetCurTime() - begin;
        begin = GetCurTime();
		uint64_t ddd = begin - start;
		static int count_t = 0;
		static int count_1 = 0;
		static int count_5 = 0;
		static int count_30 = 0;
		static int count_100 = 0;
		static int count_more = 0;
		count_t++;
		if(ddd <= 1000)
			count_1++;
		else if( ddd < 5000)
			count_5++;
		else if( ddd < 30000)
			count_30++;
		else if(ddd < 100000)
			count_100++;
		else 
			count_more++;
		if(count_t >= 1000)
		{
			printf("GetMapDataTime frequency totel = %d, <1 = %d, 1-5 = %d, 5-30 = %d, 30-100 = %d, >100 = %d \n",count_t,count_1,count_5,count_30,count_100,count_more);
			count_t = 0;
			count_1 = 0;
			count_5 = 0;
			count_30 = 0;
			count_100 = 0;
			count_more = 0;
		}
		if(ddd  > 0)
			printf("GetMapDataTime lon=%.8f,lat=%.8f,carAngle=%f,toteltime=%llu, GetOdrInfo=%llu,GetHdg=%llu,GetPreRoadID&GetSucRoadID&GetAllDrivingLaneIDS=%llu,GetLaneTypeString&bJunctionRoad=%llu,GetNeartestLaneRoadMark=%llu\n",ptInLoc.dLon, ptInLoc.dLat,carAngle,begin - start,end1,end2,end3,end4,end5);
        nOutSpeedLimit = 0;
        // angle = g_map.GetHdgByXY(ptInLoc.dLon,ptInLoc.dLat,90.0);
        // std::cout << "GetHdgByXY comValue = " << 90 << ",return = " << angle << std::endl;
        // angle = g_map.GetHdgByXY(ptInLoc.dLon,ptInLoc.dLat,180.0);
        // std::cout << "GetHdgByXY comValue = " << 180 << ",return = " << angle << std::endl;

        return true;
    }
/*    bool MapInterface::GetMapIDS(const Point& ptInLoc, double dCarAngle,int& id)
    {
        std::vector<OdrInfo> odr = g_map.GetOdrInfo(ptInLoc.dLon, ptInLoc.dLat);
        if (odr.size() == 0)
        {
            printf("not 3 in map lon = %.8f, lat = %.8f\n", ptInLoc.dLon, ptInLoc.dLat);
            return false;
        }
        //std::string roadId;
        double carAngle = (90 - dCarAngle)/180*3.1415926;
		long int roadLaneId = 0;
        bool result = g_map.GetHdg(ptInLoc.dLon, ptInLoc.dLat, dCarAngle, roadLaneId);
		printf("GetHdg result = %d, roadLaneId = %d, lon = %.8f,lat = %.8f,dCarAngle = %f\n",result,roadLaneId,ptInLoc.dLon, ptInLoc.dLat,dCarAngle);
		if(result)
            id = roadLaneId;
		else
			id = 0;
//        id = 0;
//        char** vecRoad = nullptr;
//        int nSize = 0;
//        auto result = g_map.GetAllJunctionRoadIDS(ptInLoc.dLon, ptInLoc.dLat, vecRoad, nSize);
//		if (result)
//        {
//            if (nSize == 1)
//            {
//                std::string ids = vecRoad[0];
//                id = std::stol(ids);
//            }
//        }
//		printf("GetMapIDS result = %d, nSize = %d, id = %d,ptInLoc.dLon = %.8f, ptInLoc.dLat = %.8f\n",result,nSize,id,ptInLoc.dLon, ptInLoc.dLat);
        return true;
    }
*/

}  // namespace jf
