#include "MapInfo.hpp"

namespace jf {

    MapInfo::MapInfo(){}

    MapInfo::~MapInfo() {}

/**
 * 获取车辆实际行驶的link和lane
 *
 * @param pointIn  车辆GPS坐标点
 * @param Agdir  车辆航向角
 * @param hdLinkId
 * @param hdLaneId
 * @return 主要排除问题：（1）坐标点位于多个link的交叉区域的时候判定车辆真正行驶的link
 *                    （2）车辆拐弯的时候暂时偏离实际想走的车道
 */
    bool MapInfo::locateLinkandLane(Point pointIn, float Agdir, HdLinkId &hdLinkId, HdLaneId &hdLaneId) {

        static HdLinkId s_liLastLinkId = {};
        static HdLaneId s_liLastLaneId = {}; //记录车辆上一个周期经过的link和lane

        std::vector<HdLinkId> vctliLocateLinkIds = {};
        std::vector<HdLaneId> vctliLocateLaneIds = {};
        if (false == m_spHdLib->GetLocateLanes(pointIn, vctliLocateLinkIds, vctliLocateLaneIds)) { //定位失败，那么使用上次的结果

            if (s_liLastLaneId.lValue == 0) {  // 上次结果不可用
                return false;
            } else {   // 上次结果可以用
                vctliLocateLinkIds.push_back(s_liLastLinkId);
                vctliLocateLaneIds.push_back(s_liLastLaneId);
            }
        }

        if (vctliLocateLaneIds.size() >= 2) {
            // 首先，选择航向角匹配的车道
            std::vector<int> vctnAngleMatchedLanes = {};
            for (int i = 0; i < vctliLocateLaneIds.size(); ++i) {
                double dAzimuth = 0.0;
                m_spHdLib->GetLaneAzimuth(vctliLocateLinkIds.at(i), vctliLocateLaneIds.at(i), pointIn, dAzimuth);
                double dAngleSub = std::min(std::abs(dAzimuth - Agdir), std::abs(360 - (dAzimuth - Agdir)));
                if (dAngleSub < 15) {
                    vctnAngleMatchedLanes.push_back(i);
                }
            }
            // 多个车道符合条件或者没有符合条件的车道
            if (vctnAngleMatchedLanes.size() >= 2 || vctnAngleMatchedLanes.size() == 0) {
                // 应该尝试使用INS的高程作为判断，但INS的高程并不是很精确，所以不使用
                hdLinkId = s_liLastLinkId;
                hdLaneId = s_liLastLaneId;
            } else {
                hdLinkId = vctliLocateLinkIds.at(vctnAngleMatchedLanes.at(0));
                hdLaneId = vctliLocateLaneIds.at(vctnAngleMatchedLanes.at(0));
            }
        } else {
//            // 即使只有一条车道符合条件，也可能存在坐标点与实际车道不符合的情况
//            double dAzimuth = 0.0;
//            m_spHdLib->GetLaneAzimuth(vctliLocateLinkIds.at(0), vctliLocateLaneIds.at(0), pointIn, dAzimuth);
//            double dAngleSub = std::min(std::abs(dAzimuth - Agdir), std::abs(360 - (dAzimuth - Agdir)));
//            if (dAngleSub < 15) {
                hdLinkId = vctliLocateLinkIds.at(0);
                hdLaneId = vctliLocateLaneIds.at(0);
//            }
//                // 角度不匹配，使用上次结果
//            else {
//                hdLinkId = s_liLastLinkId;
//                hdLaneId = s_liLastLaneId;
//            }
        }
        s_liLastLinkId = hdLinkId;
        s_liLastLaneId = hdLaneId;
        return true;
    }

    /**
     * 输出自车当前车道的道路信息
     *
     * 信息包括：
     *  （1）当前车道的限速信息
     *  （2）左右相邻车道的车到边界线ID
     *  （3）是否能变道到左右相邻车道
     * @return
     */

    bool MapInfo::getCurrentLocationInfo(Point pointIn, float Agdir, HdLinkId &hdLinkId, HdLaneId &hdLaneId,
                                              int& speedLimitInfo,
                                              int64_t& LeftLaneEdgeId1, int64_t& LeftLaneEdgeId2, bool& LeftLane_Crossable,
                                              int64_t& RightLaneEdgeId1, int64_t& RightLaneEdgeId2, bool& RightLane_Crossable){

        const HdLink *linkPtr = m_spHdLib->GetLinkData(hdLinkId);
        int CurrentLaneNum;
        for(HdLane hdLane :linkPtr->vcthlLane){
            if(hdLane.liId.lValue == hdLaneId.lValue){
                speedLimitInfo = hdLane.nMaxSpeed; //限速信息输出
                CurrentLaneNum = hdLane.nSeqNum;
                break;
            }
        }

        for (const HdLane hdLane : linkPtr->vcthlLane) {
            if(CurrentLaneNum - 1 > 0){
                if (hdLane.nSeqNum == CurrentLaneNum-1) {
                    LeftLaneEdgeId1 = hdLane.lLeftEdgeId;
                    LeftLaneEdgeId2 = hdLane.lRightEdgeId;
                    LeftLane_Crossable =  m_spHdLib->IsReachable(linkPtr, hdLaneId,hdLane.liId);
                }
            }else if(CurrentLaneNum + 1 <= linkPtr->vcthlLane.size()){
                if(hdLane.nSeqNum == CurrentLaneNum + 1){
                    RightLaneEdgeId1 =  hdLane.lLeftEdgeId;
                    RightLaneEdgeId2 =  hdLane.lRightEdgeId;
                    RightLane_Crossable = m_spHdLib->IsReachable(linkPtr, hdLaneId,hdLane.liId);
                }
            }

        }

        return true;
    }


/**
 *  计算需要输出车道边界线的Link
 *
 * std::deque<HdLinkId> hdLinkIdQueue;                          //最终需要维护的数据结构1
 * std::map<HdLinkId, std::map<int, Point>> hdLinkIdQueueTable; //最终需要维护的数据结构2
 * @return
 */
    bool MapInfo::CreateMapInfo_First( const HdLinkId &hdLinkId, const HdLaneId &hdLaneId, const Point pointIn,
                                            std::deque<HdLinkId>& hdLinkIdQueue, std::map<HdLinkId, std::map<int, Point>>& hdLinkIdQueueTable) {

        double BehindDistanceTarget = 10.0; //车辆后方10m
        double FontDistanceTarget = 200; //车辆前方100m
        const HdLink *hdLinkPtr = m_spHdLib->GetLinkData(hdLinkId);//自车所在的Link
        static std::queue<HdLinkId> hdLinkIdLastQueue; //自辆经过的上一个Link和本周期的link队列
        Point footPointOfCar; //自车垂足点
        double distanceTmp;
        int indexOfCarPoint; //自车所在的Index
        HdLibMath::GetPerpendicularFoot(pointIn, hdLinkPtr->vctvdGeoLink, footPointOfCar, distanceTmp, indexOfCarPoint);

        /**
         * 生成车前的信息
         */
        auto createFont = [this](Point pointInGeoLink, const std::vector<std::vector<double>> &vctvdGeoLink,
                                 int indexOfPoint,
                                 double &distanceTarget, std::map<int, Point> &resultGeoMap) {
            double dRealLength = 0; //本周期累加距离总和
            static double dRealLengthLast = 0; //上一周期累加距离总和
            for (int i = indexOfPoint; i < vctvdGeoLink.size(); i++) {
                if (i == indexOfPoint) {
                    dRealLength += PointsDis(Point(vctvdGeoLink[i]), pointInGeoLink);
                } else {
                    dRealLength += PointsDis(Point(vctvdGeoLink[i]), Point(vctvdGeoLink[i - 1]));
                }
                if (dRealLength >= distanceTarget) { //如果累加距离超过了目标距离
                    std::vector<Point> vctPointTmp = jf::InterP(vctvdGeoLink[i - 1], vctvdGeoLink[i],distanceTarget - dRealLengthLast);
                    resultGeoMap[i] = vctPointTmp[1];
                    return true;
                }
                dRealLengthLast = dRealLength;
            }
            distanceTarget = distanceTarget - dRealLength;
            return false;
        };

        /**
         * 生成车后的信息
         */
        auto createBehind = [this](Point pointInGeoLink, const std::vector<std::vector<double>> &vctvdGeoLink,
                                   int indexOfPoint,
                                   double &distanceTarget, std::map<int, Point> &resultGeoMap) {
            double dRealLength = 0; //本周期累加距离总和
            static double dRealLengthLast = 0; //上一周期累加距离总和
            for (int i = indexOfPoint - 1; i >= 0; i--) {
                if (i == indexOfPoint - 1) {
                    dRealLength += PointsDis(Point(vctvdGeoLink[i]), pointInGeoLink);
                } else {
                    dRealLength += PointsDis(Point(vctvdGeoLink[i]), Point(vctvdGeoLink[i + 1]));
                }
                if (dRealLength >= distanceTarget) { //如果累加距离超过了目标距离
                    std::vector<Point> vctPointTmp = jf::InterP(vctvdGeoLink[i + 1], vctvdGeoLink[i],
                                                                distanceTarget - dRealLengthLast);
                    resultGeoMap[i] = vctPointTmp[1];
                    distanceTarget = 0;
                    return true;
                }
                dRealLengthLast = dRealLength;
            }
            distanceTarget = distanceTarget - dRealLength;
            return false;
        };

        /**
        * 递归寻找前方具有联通关系的Link
        */
        auto findFontLinksFunc = [this, &createFont, &hdLinkIdQueue, &hdLinkIdQueueTable](HdLinkId hdLinkId1,
                                                                                          double FontDistanceTarget) {
            std::function<void(HdLinkId, double)> func;
            func = [this, &createFont, &hdLinkIdQueue, &hdLinkIdQueueTable, &func](HdLinkId hdLinkId1,
                                                                                   double FontDistanceTarget) {
                const HdLink *hdLinkPtrTmp = m_spHdLib->GetLinkData(hdLinkId1);
                for (HdLinkId hdLinkIdTmp: hdLinkPtrTmp->vctliNxtLinkId) {
                    std::map<int, Point> resultGeoMap_FontTmp;
                    double FontDistanceTargetTmp1 = FontDistanceTarget;
                    const HdLink *hdLinkPtrTmp = m_spHdLib->GetLinkData(hdLinkIdTmp);
                    if (!createFont(hdLinkPtrTmp->vctvdGeoLink[0], hdLinkPtrTmp->vctvdGeoLink, 1,
                                    FontDistanceTargetTmp1, resultGeoMap_FontTmp)) {
                        hdLinkIdQueue.push_front(hdLinkPtrTmp->liId);
                        func(hdLinkPtrTmp->liId, FontDistanceTargetTmp1);
                    } else {
                        hdLinkIdQueue.push_front(hdLinkPtrTmp->liId);
                        hdLinkIdQueueTable[hdLinkPtrTmp->liId] = resultGeoMap_FontTmp;
                    }
                }
            };
            return func(hdLinkId1, FontDistanceTarget);
        };

        /******车辆后方*******/
        std::map<int, Point> resultGeoMap_behind;
        double BehindDistanceTargetTmp = BehindDistanceTarget;
        if (createBehind(footPointOfCar, hdLinkPtr->vctvdGeoLink, indexOfCarPoint, BehindDistanceTargetTmp,
                         resultGeoMap_behind)) { //计算自车link
            hdLinkIdQueue.push_back(hdLinkPtr->liId);
            hdLinkIdQueueTable[hdLinkPtr->liId] = resultGeoMap_behind;
        } else {//计算以前走过的link
            hdLinkIdQueue.push_back(hdLinkPtr->liId);
            HdLinkId hdLinkIdLast;
            if (hdLinkIdLastQueue.size() < 2) { //如果没有以前的linkId记录
                if (!hdLinkPtr->vctliPreLinkId.empty()) {
                    hdLinkIdLast = hdLinkPtr->vctliPreLinkId[0];
                }
            } else { //如果有以前的linkId记录
                hdLinkIdLast = hdLinkIdLastQueue.front();
            }
            if (hdLinkIdLast.lValue != 0) {
                const HdLink *hdLinkLastPtr = m_spHdLib->GetLinkData(hdLinkIdLast);
                if (createBehind(hdLinkLastPtr->vctvdGeoLink[hdLinkLastPtr->vctvdGeoLink.size() - 1],
                                 hdLinkLastPtr->vctvdGeoLink, hdLinkLastPtr->vctvdGeoLink.size() - 1,
                                 BehindDistanceTargetTmp, resultGeoMap_behind)) {
                    hdLinkIdQueue.push_back(hdLinkLastPtr->liId);
                    hdLinkIdQueueTable[hdLinkLastPtr->liId] = resultGeoMap_behind;
                } else {
                    hdLinkIdQueue.push_back(hdLinkLastPtr->liId);
                }
            }
        }
        if (hdLinkIdLastQueue.empty()) {
            hdLinkIdLastQueue.push(hdLinkPtr->liId);
        } else if (hdLinkIdLastQueue.size() == 1) {
            if (hdLinkIdLastQueue.back().lValue != hdLinkPtr->liId.lValue) {
                hdLinkIdLastQueue.push(hdLinkPtr->liId);
            }
        } else {
            if (hdLinkIdLastQueue.back().lValue != hdLinkPtr->liId.lValue) {
                hdLinkIdLastQueue.push(hdLinkPtr->liId);
            }
            hdLinkIdLastQueue.pop();
        }

        /******车辆前方*******/
        std::map<int, Point> resultGeoMap_Font;
        double FontDistanceTargetTmp = FontDistanceTarget;
        if (createFont(footPointOfCar, hdLinkPtr->vctvdGeoLink, indexOfCarPoint, FontDistanceTargetTmp,
                       resultGeoMap_Font)) { //计算自车link
            hdLinkIdQueueTable[hdLinkPtr->liId].insert(resultGeoMap_Font.begin(), resultGeoMap_Font.end());
        } else {//计算前方具有联通关系的link
            findFontLinksFunc(hdLinkPtr->liId, FontDistanceTargetTmp);
        }
        return true;
    }

/**
 * 计算需要输出的地图信息
 * @return
 */
    bool MapInfo::CreateMapInfo_Second(const HdLinkId &hdLinkId, const HdLaneId &hdLaneId, const Point pointIn,
                                            std::deque<HdLinkId> &hdLinkIdQueue,
                                            std::map<HdLinkId, std::map<int, Point>> &hdLinkIdQueueTable,
                                            std::vector<HdEdge>& vctheEdgeResult) {

        //(1)队列尾部第一个元素 特殊处理：从Index开始向数组尾部方向输出边界线
        if((hdLinkIdQueueTable.count(hdLinkIdQueue.back())!=0)&&
           hdLinkIdQueueTable[hdLinkIdQueue.back()].size()==1){

            Point pointTmp;
            for(auto it:hdLinkIdQueueTable[hdLinkIdQueue.back()]){
                pointTmp = it.second;
            }
            const HdLink *hdLinkPtr = m_spHdLib->GetLinkData(hdLinkIdQueue.back());
            for(HdEdge hdEdge : hdLinkPtr->vctheEdge){
                Point footPoint;
                double dis;
                int footIndex;
                HdLibMath::GetPerpendicularFoot(pointTmp,hdEdge.vctvdGeoLink,footPoint,dis,footIndex);
                int targetEdgePointSize = hdEdge.vctvdGeoLink.size()-footIndex;
                //删除数组中前面的元素，一直到元素个数到达目标个数。
                for(std::vector<std::vector<double>>::iterator it = hdEdge.vctvdGeoLink.begin();hdEdge.vctvdGeoLink.size()>targetEdgePointSize;){
                    it = hdEdge.vctvdGeoLink.erase(it);
                }
                std::vector<double> Vectpoint = {footPoint.dLon,footPoint.dLat,footPoint.dAlt};
                hdEdge.vctvdGeoLink.insert(hdEdge.vctvdGeoLink.begin(),Vectpoint);
                vctheEdgeResult.push_back(hdEdge);
            }
            hdLinkIdQueue.pop_back();
        }
        //(2)遍历队列中所有link
        while(!hdLinkIdQueue.empty()){
            /**table里面没有记录信息，则表示link的全部的边界线都要**/
            if(hdLinkIdQueueTable.find(hdLinkIdQueue.back())==hdLinkIdQueueTable.end()){

                const HdLink *hdLinkPtr = m_spHdLib->GetLinkData(hdLinkIdQueue.back());
                for(HdEdge hdEdge : hdLinkPtr->vctheEdge){
                    vctheEdgeResult.push_back(hdEdge);
                }
                hdLinkIdQueue.pop_back();

            }
                /**table里面记录一条数据，则从Index开始向数组头部方向输出边界线**/
            else if(hdLinkIdQueueTable[hdLinkIdQueue.back()].size()==1){

                Point pointTmp;
                for(auto it:hdLinkIdQueueTable[hdLinkIdQueue.back()]){
                    pointTmp = it.second;
                }
                const HdLink *hdLinkPtr = m_spHdLib->GetLinkData(hdLinkIdQueue.back());
                for(HdEdge hdEdge : hdLinkPtr->vctheEdge){
                    Point footPoint;
                    double dis;
                    int footIndex;
                    HdLibMath::GetPerpendicularFoot(pointTmp,hdEdge.vctvdGeoLink,footPoint,dis,footIndex);
                    int targetEdgePointSize = footIndex;
                    //删除数组中后面的元素，一直到元素个数到达目标个数。
                    while( hdEdge.vctvdGeoLink.size()>targetEdgePointSize){
                        hdEdge.vctvdGeoLink.pop_back();
                    }
                    std::vector<double> Vectpoint = {footPoint.dLon,footPoint.dLat,footPoint.dAlt};
                    hdEdge.vctvdGeoLink.push_back(Vectpoint);
                    vctheEdgeResult.push_back(hdEdge);
                }
                hdLinkIdQueue.pop_back();

            }
                /**table里面记录两条数据，则从小Index向大Index方向输出边界线**/
            else{

                std::vector<Point>  VctpointTmp;
                for(auto it:hdLinkIdQueueTable[hdLinkIdQueue.back()]){
                    VctpointTmp.push_back(it.second);
                }
                const HdLink *hdLinkPtr = m_spHdLib->GetLinkData(hdLinkIdQueue.back());
                for(HdEdge hdEdge : hdLinkPtr->vctheEdge){
                    Point footPoint1,footPoint2;
                    double dis1,dis2;
                    int footIndex1,footIndex2;

                    HdLibMath::GetPerpendicularFoot(VctpointTmp[0],hdEdge.vctvdGeoLink,footPoint1,dis1,footIndex1);
                    HdLibMath::GetPerpendicularFoot(VctpointTmp[1],hdEdge.vctvdGeoLink,footPoint2,dis2,footIndex2);

                    if(footIndex1>=footIndex2){
                        /**首先删除尾部元素*/
                        int targetEdgePointSize = footIndex1;
                        //删除数组中后面的元素，一直到元素个数到达目标个数。
                        while( hdEdge.vctvdGeoLink.size()>targetEdgePointSize){
                            hdEdge.vctvdGeoLink.pop_back();
                        }
                        std::vector<double> Vectpoint = {footPoint1.dLon,footPoint1.dLat,footPoint1.dAlt};
                        hdEdge.vctvdGeoLink.push_back(Vectpoint);
                        /***删除头部元素****/
                        int targetEdgePointSize2 = footIndex2;
                        for(std::vector<std::vector<double>>::iterator it = hdEdge.vctvdGeoLink.begin();targetEdgePointSize2 > 0;){
                            it = hdEdge.vctvdGeoLink.erase(it);
                            targetEdgePointSize2--;
                        }
                        std::vector<double> Vectpoint2 = {footPoint2.dLon,footPoint2.dLat,footPoint2.dAlt};
                        hdEdge.vctvdGeoLink.insert(hdEdge.vctvdGeoLink.begin(),Vectpoint2);
                        vctheEdgeResult.push_back(hdEdge);

                    }else{

                        /**首先删除尾部元素*/
                        int targetEdgePointSize = footIndex2;
                        //删除数组中后面的元素，一直到元素个数到达目标个数。
                        while( hdEdge.vctvdGeoLink.size()>targetEdgePointSize){
                            hdEdge.vctvdGeoLink.pop_back();
                        }
                        std::vector<double> Vectpoint = {footPoint2.dLon,footPoint2.dLat,footPoint2.dAlt};
                        hdEdge.vctvdGeoLink.push_back(Vectpoint);
                        /***删除头部元素****/
                        int targetEdgePointSize2 = footIndex1;
                        for(std::vector<std::vector<double>>::iterator it = hdEdge.vctvdGeoLink.begin();targetEdgePointSize2 > 0;){
                            it = hdEdge.vctvdGeoLink.erase(it);
                            targetEdgePointSize2--;
                        }
                        std::vector<double> Vectpoint2 = {footPoint1.dLon,footPoint1.dLat,footPoint1.dAlt};
                        hdEdge.vctvdGeoLink.insert(hdEdge.vctvdGeoLink.begin(),Vectpoint2);
                        vctheEdgeResult.push_back(hdEdge);
                    }
                }
                hdLinkIdQueue.pop_back();

            }
        }
        return true;
    }


}  // namespace jf