
#include "LanePlan.h"

#include <iostream>

#include "Bezier.h"

using namespace jf;

LanePlan::LanePlan(const std::string &cstrProjectPath, const std::string &cstrMapConfigPath) { HdLib::GetInstance()->Init(cstrProjectPath, cstrMapConfigPath); }

bool LanePlan::SetPath(const std::vector<Point> &cvctptWayNode, std::vector<LaneEndpoint> &vctleOutWayPoint, bool bRenowResult /* = true*/) {
    // 刷新现有的寻路结果
    if (true == bRenowResult) {
        m_vctleRoute.clear();
        m_vctlRouteEndLaneId.clear();
    }

    // 尝试寻路
    bool bFindResult = m_rtleRoute.FindRoute(cvctptWayNode, m_vctleRoute, vctleOutWayPoint, false, true);
    if (true == bFindResult) {
        // 将终点的车道ID保存
        m_vctlRouteEndLaneId.push_back(m_vctleRoute[m_vctleRoute.size() - 1].lLaneId);
    }

    // 生成轨迹点
    GenInterpGeometry(cvctptWayNode, vctleOutWayPoint, m_vctleRoute);
    PrintQGIS("FullRoute.csv", m_vctleRoute);

    return bFindResult;
}

void LanePlan::SetUnpassableLane(const std::vector<HdLaneId> &cvctlUnpassableLane) {
    // 手动设置不可通行的车道
    m_vctlUnpassableLane = cvctlUnpassableLane;
}

int LanePlan::GetLaneCnt() {
    // 返回寻路结果中的车道数目
    return m_vctleRoute.size();
}

bool LanePlan::GetLaneEndpoint(int nIndex, LaneEndpoint &leOutEndpoint) {
    // 判断参数是否合法
    if ((nIndex >= 0) && (nIndex < m_vctleRoute.size())) {
        leOutEndpoint = m_vctleRoute[nIndex];
        return true;
    }
    // 如果参数不对
    return false;
}

bool LanePlan::GetNearEndpoints(const Point &cptInPoint, std::vector<Point> &vctptOutPoint, std::vector<LaneEndpoint> &vctleOutLaneEp, std::vector<double> &vctdOutDistance) {
    std::vector<Point> vctptPoint = {};
    std::vector<LaneEndpoint> vctleLaneEp = {};
    std::vector<double> vctdDistance = {};
    // 获取指定点附近的车道集合
    if (true == HdLib::GetInstance()->GetNearEndpoints(cptInPoint, MapConfig::route_rect_lon, MapConfig::route_rect_lat, vctptPoint, vctleLaneEp, vctdDistance)) {
        static const int sc_nMaxResult = 6;
        vctptOutPoint.insert(vctptOutPoint.end(), vctptPoint.begin(), vctptPoint.begin() + std::min(int(vctleLaneEp.size()), sc_nMaxResult));
        vctleOutLaneEp.insert(vctleOutLaneEp.end(), vctleLaneEp.begin(), vctleLaneEp.begin() + std::min(int(vctleLaneEp.size()), sc_nMaxResult));
        vctdOutDistance.insert(vctdOutDistance.end(), vctdDistance.begin(), vctdDistance.begin() + std::min(int(vctleLaneEp.size()), sc_nMaxResult));
        return true;
    }
    return false;
}

bool LanePlan::GetIndex(const std::vector<LaneEndpoint> &cvctleEndpoint, int &nOutIndex, int nPastIndex /* = 0*/, int nFindCnt /* = 65536*/) {
    // 获取所给车道在寻路结果中的索引值
    for (const LaneEndpoint leEndpoint : cvctleEndpoint) {
        if (true == GetIndex(leEndpoint, nOutIndex, nPastIndex, nFindCnt)) {
            return true;
        }
    }
    return false;
}

bool LanePlan::GetIndex(const LaneEndpoint &cleEndpoint, int &nOutIndex, int nPastIndex /* = 0*/, int nFindCnt /* = 65536*/) {
    // 计算到终点的距离
    size_t nEndIndex = std::min(m_vctleRoute.size(), (size_t)(nPastIndex + nFindCnt));
    // 在寻路结果中，从当前节点开始，向后找，指导找到与指定节点相同的节点开始
    for (size_t i = nPastIndex; i < nEndIndex; ++i) {
        LaneEndpoint leCurEp = m_vctleRoute[i];
        if (cleEndpoint.lLinkId == leCurEp.lLinkId) {
            if (((leCurEp.nStartLaneNum >= cleEndpoint.nStartLaneNum) && (leCurEp.nEndLaneNum <= cleEndpoint.nStartLaneNum)) || ((leCurEp.nStartLaneNum <= cleEndpoint.nStartLaneNum) && (leCurEp.nEndLaneNum >= cleEndpoint.nStartLaneNum))) {
                nOutIndex = i;
                return true;
            } else {
                return false;
            }
        }
    }
    return false;
}

int LanePlan::GetPosIdxInLane(const Point &cptInPoint, int nIndex) {
    // 判断参数是否合法
    if (nIndex < 0 || nIndex >= m_vctleRoute.size()) {
        return -1;
    }

    // 获取指定的车道
    const LaneEndpoint &leLaneEp = m_vctleRoute[nIndex];
    std::vector<std::vector<double>> vctvdGeoLink = {};
    HdLib::GetInstance()->ConvertGeoLink(leLaneEp.vctptGeoLink, vctvdGeoLink);
    Point ptGeoPos = {};
    double dGeoDistance = -1;
    int nGeoIndex = 0;
    // 获取指定的点，在车道轨迹点上的索引（取距离最近的点的索引）
    if (false == HdLibMath::GetNearestPointFromArray(cptInPoint, vctvdGeoLink, ptGeoPos, dGeoDistance, nGeoIndex)) {
        return -1;
    }

    return nGeoIndex;
}

void LanePlan::GenInterpGeometry(const std::vector<Point> &cvctptWayNode, std::vector<LaneEndpoint> &vctleOutWayPoint, std::vector<LaneEndpoint> &vctleInOutLanePath) {
    // 处理每一个车道
    int nClosedListCnt = vctleInOutLanePath.size();
    for (size_t i = 0; i < nClosedListCnt; ++i) {
        // 获取该车道的link
        std::vector<std::vector<double>> vctvdGeoLink = {};
        HdLib::GetInstance()->GetLaneGeoLink(vctleInOutLanePath[i].lLinkId, vctleInOutLanePath[i].nEndLaneNum, vctvdGeoLink);
#if true
        // 将double类型的二维数组转换为Point型的一维数组
        HdLib::GetInstance()->ConvertGeoLink(vctvdGeoLink, vctleInOutLanePath[i].vctptGeoLink);

#else
        std::vector<Point> vctptGeoLink = {};
        // 将double类型的二维数组转换为Point型的一维数组
        m_mlMapLib.ConvertGeoLink(vctvdGeoLink, vctptGeoLink);

        int nStatGeoIndex = 0;
        int nEndGeoIndex = vctptGeoLink.size() - 1;
        Point ptGeoPos = {};
        // 第一车道和最后车道单独处理
        if ((i == 0) || (i == nClosedListCnt - 1)) {
            double dGeoDistance = -1;
            int nGeoIndex = 0;
            if (i == 0) {
                // 第一车道，所给点到车道的垂足点到距离车道最近的点的下一点（第二个点），以及从第二个点到最后的点分别处理
                GetNearestDistance(cvctptWayNode.at(0), vctvdGeoLink, ptGeoPos, dGeoDistance, nGeoIndex);
                GetPerpendicularFoot(cvctptWayNode.at(0), vctvdGeoLink, ptGeoPos, dGeoDistance);
                nStatGeoIndex = nGeoIndex == 0 ? nGeoIndex : nGeoIndex - 1;
            }
            if (i == nClosedListCnt - 1) {
                // 第末车道，最初的点到所给点的前一点，以及从前一点到所给点到车道的垂足点分别处理
                GetNearestDistance(cvctptWayNode.at(cvctptWayNode.size() - 1), vctvdGeoLink, ptGeoPos, dGeoDistance, nGeoIndex);
                GetPerpendicularFoot(cvctptWayNode.at(cvctptWayNode.size() - 1), vctvdGeoLink, ptGeoPos, dGeoDistance);
                nEndGeoIndex = nGeoIndex >= vctvdGeoLink.size() - 1 ? nGeoIndex : nGeoIndex + 1;
            }
        }

        if (vctleInOutLanePath[i].nStartLaneNum == vctleInOutLanePath[i].nEndLaneNum) {
            // 针对原来的点，做插值处理（间隔0.5米）
            for (int j = nStatGeoIndex, k = j + 1; k <= nEndGeoIndex; ++j, ++k) {
                std::vector<Point> ret = jf::InterP(vctptGeoLink[j], vctptGeoLink[k], 0.5);
                vctleInOutLanePath[i].vctptGeoLink.insert(vctleInOutLanePath[i].vctptGeoLink.end(), ret.begin(), ret.end());
            }
        } else {
            // 平滑
            std::vector<Point> vctptCurve = {};
            SmoothChangeRoad(vctleInOutLanePath[i].lLinkId, vctleInOutLanePath[i].nStartLaneNum, vctleInOutLanePath[i].nEndLaneNum, vctptCurve);
            vctleInOutLanePath[i].vctptGeoLink = vctptCurve;
        }
#endif
    }
}

bool LanePlan::SmoothChangeRoad(const HdLinkId &clLinkId, int nFromLaneNum, int nToLaneNum, std::vector<Point> &vctptOutCurve) {
    int nChangeRoadNum = abs(nFromLaneNum - nToLaneNum);
    auto func_Calc = [this](const HdLinkId &clLinkId, int nChangeRoadNum, int nFromLane, int nToLane, float fPercent, int nPastIndex, bool bIsFirstSegment, bool bIsLastSegment, std::vector<Point> &vctptOutCurve) -> int {
        std::vector<std::vector<double>> vctvdFromGeoLink = {};
        HdLib::GetInstance()->GetLaneGeoLink(clLinkId, nFromLane, vctvdFromGeoLink);
        int nFromControlIndex = fPercent * vctvdFromGeoLink.size();
        std::vector<std::vector<double>> vctvdToGeoLink = {};
        HdLib::GetInstance()->GetLaneGeoLink(clLinkId, nToLane, vctvdToGeoLink);
        int nToControlIdx = fPercent * vctvdToGeoLink.size();
        // 变道半长度，根据index来的，index每隔0.5米一个
        int nSmoothValue = std::min(std::max(int(vctvdFromGeoLink.size() / float(nChangeRoadNum) / 2), 1), 30);
        // 上一次变道到本nFromLane的index，到本nFromLane到to_lane的起始
        // int nCurveSeg1FromIdx = nPastIndex + (bIsFirstSegment ? 0 : nSmoothValue);
        // int nCurveSeg1ToIdx = nFromControlIndex - nSmoothValue;
        // if (nCurveSeg1FromIdx < nCurveSeg1ToIdx) {
        // vctptOutCurve.insert(vctptOutCurve.end(), vctvdFromGeoLink.begin() + nCurveSeg1FromIdx, vctvdFromGeoLink.begin() + nCurveSeg1ToIdx);
        // }
        // 贝塞尔曲线
        std::vector<Point> vctptCs{Point(vctvdFromGeoLink[std::max(nFromControlIndex - nSmoothValue, 0)][0], vctvdFromGeoLink[std::max(nFromControlIndex - nSmoothValue, 0)][1], vctvdFromGeoLink[std::max(nFromControlIndex - nSmoothValue, 0)][2]),
                                   Point(vctvdFromGeoLink[nFromControlIndex - 0][0], vctvdFromGeoLink[nFromControlIndex - 0][1], vctvdFromGeoLink[nFromControlIndex - 0][2]),
                                   Point(vctvdToGeoLink[nToControlIdx + 0][0], vctvdToGeoLink[nToControlIdx + 0][1], vctvdToGeoLink[nToControlIdx + 0][2]),
                                   Point(vctvdToGeoLink[std::min(nToControlIdx + nSmoothValue, int(vctvdToGeoLink.size() - 1))][0], vctvdToGeoLink[std::min(nToControlIdx + nSmoothValue, int(vctvdToGeoLink.size() - 1))][1],
                                         vctvdToGeoLink[std::min(nToControlIdx + nSmoothValue, int(vctvdToGeoLink.size() - 1))][2])};
        std::vector<Point> vctptBs = Bezier(vctptCs, nSmoothValue * 6);
        vctptOutCurve.insert(vctptOutCurve.end(), vctptBs.begin(), vctptBs.end());

        // 连到终点
        if (bIsLastSegment) {
            vctptOutCurve.insert(vctptOutCurve.end(), vctvdToGeoLink.begin() + std::min(nToControlIdx + nSmoothValue, int(vctvdToGeoLink.size() - 1)), vctvdToGeoLink.end());
        }

        return nToControlIdx;
    };
    // 循环需要并道的所有index
    // 找比例点，贝塞尔曲线控制点在比例点附近
    // 比如变3条车道，比例点是1/6,3/6,5/6
    // 比如变2条车道，比例点是1/4,3/4
    int nToControlIdx = 0;
    // 从右向左变道
    if (nFromLaneNum > nToLaneNum) {
        int nPercentIdx = 1;
        for (int i = nFromLaneNum, j = nFromLaneNum - 1; j >= nToLaneNum; --i, --j) {
            float fPercent = float(nPercentIdx) / (nChangeRoadNum * 2);
            nToControlIdx = func_Calc(clLinkId, nChangeRoadNum, i, j, fPercent, nToControlIdx, i == nFromLaneNum, j == nToLaneNum, vctptOutCurve);
            nPercentIdx += 2;
        }
    }
    // 从左向右变道
    else {
        int nPercentIdx = 1;
        for (int i = nFromLaneNum, j = nFromLaneNum + 1; j <= nToLaneNum; ++i, ++j) {
            float fPercent = float(nPercentIdx) / (nChangeRoadNum * 2);
            nToControlIdx = func_Calc(clLinkId, nChangeRoadNum, i, j, fPercent, nToControlIdx, i == nFromLaneNum, j == nToLaneNum, vctptOutCurve);
            nPercentIdx += 2;
        }
    }

    // 插值
    std::vector<Point> vctptInterP = {};
    for (int i = 0, j = 1; j < vctptOutCurve.size(); ++i, ++j) {
        std::vector<Point> vctptRet = jf::InterP(vctptOutCurve[i], vctptOutCurve[j], 0.5);
        vctptInterP.insert(vctptInterP.end(), vctptRet.begin(), vctptRet.end());
    }
    vctptOutCurve = vctptInterP;

    return true;
}

void LanePlan::PrintQGIS(const std::string &cstrFileName, const std::vector<LaneEndpoint> &cvctleClosedList) {
    static int64_t IndexGlobal;
    std::ofstream ofsQgisResult;
    // 创建csv文件
    ofsQgisResult.open(cstrFileName.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisResult.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisResult.precision(15);

    // 插入标签
    ofsQgisResult << "lon"
                  << ", "
                  << "lat"
                  << ", "
                  << "num" << std::endl;
    // 写入点
    for (size_t i = 0; i < cvctleClosedList.size(); ++i) {
        for (size_t j = 0; j < cvctleClosedList[i].vctptGeoLink.size(); j++) {
            ofsQgisResult << cvctleClosedList[i].vctptGeoLink[j].dLon << ", " << cvctleClosedList[i].vctptGeoLink[j].dLat << ", " << IndexGlobal << std::endl;
            IndexGlobal++;
        }
    }
    ofsQgisResult.close();
}
