#ifndef route_h
#define route_h

#include <vector>
#include "AStar.h"

namespace jf {
template <typename EndpointType>
class Route {
   private:
    int roaD_Plan_min_waypoint_dis;
    int roaD_Plan_max_links;

   public:
    Route() {}
    Route(int in_roaD_Plan_min_waypoint_dis, int in_roaD_Plan_max_links) : roaD_Plan_max_links(in_roaD_Plan_max_links), roaD_Plan_min_waypoint_dis(in_roaD_Plan_min_waypoint_dis) {}
    /**
     * @brief 寻路
     * @param vctptWayNode：起点，途径点（如果有），终点的集合，必须是顺序的
     * @param vcthlOutLinkPath：寻路结果
     * @param bIsThrowWhenFail：如果失败是否放弃
     * @return 寻路是否成功
     */
    bool FindRoute(const std::vector<Point> &vctptWayNode, std::vector<HdLinkId> &vcthlOutLinkPath, bool bIsThrowWhenFail = false) {
        std::vector<EndpointType> vctleOutRoute;
        std::vector<EndpointType> vctleOutWayPoint;
        if (!FindRoute(vctptWayNode, vctleOutRoute, vctleOutWayPoint, bIsThrowWhenFail, true)) {
            return false;
        }
        vcthlOutLinkPath.resize(vctleOutRoute.size());
        std::transform(vctleOutRoute.begin(), vctleOutRoute.end(), vcthlOutLinkPath.begin(), [](const EndpointType &e) { return e.lLinkId; });
        return true;
    }

    /**
     * @brief 寻路
     * @param vctptWayNode：起点，途径点（如果有），终点的集合，必须是顺序的
     * @param vctleOutRoute：寻路结果
     * @param vctleOutWayPoint：该次寻路处理中，使用的端点（起点，途径点（如果有），终点都有对应）
     * @param bIsThrowWhenFail：如果失败是否放弃
     * @param bHighPrecision：是否将终点集中的终点按照距离优先使用
     * @return 寻路是否成功
     */
    bool FindRoute(const std::vector<Point> &vctptWayNode, std::vector<EndpointType> &vctleOutRoute,
                   std::vector<EndpointType> &vctleOutWayPoint, bool bIsThrowWhenFail = false, bool bHighPrecision = false) {
        static const int sc_nMaxNearbyLinkCnt = 6;

        // 如果所给的点太少
        int nSize = vctptWayNode.size();
        if (nSize <= 1) {
            return false;
        }

        // 找到所有的临近LinkEndpoint
        std::vector<std::vector<EndpointType>> vctvlNodes = {};
        if (false == GetEndPoint(vctptWayNode, bIsThrowWhenFail, sc_nMaxNearbyLinkCnt, vctvlNodes)) {
            return false;
        }

        // 寻路，如果失败，则认为上一段寻路的开始或结束的端点不对，删除掉距离给定坐标点最近的端点重新处理
        int nStartNodeIndex = 0;
        // 起点
        EndpointType leStartNode = vctvlNodes[0][nStartNodeIndex];
        // 分段寻路结果
        std::vector<std::vector<EndpointType>> vctvlPath = {};
        // 已经使用过的起点
        std::vector<EndpointType> vctleUsedStartNode = {};
        // 已经使用过的终点
        std::vector<EndpointType> vctleNgEndNode = {};
        nSize = vctvlNodes.size();
        for (int i = 0; i < nSize - 1;) {
            std::vector<EndpointType> vctlePath = {};
            std::vector<EndpointType> vctleEndNode = {};
            // 寻找当前起点的下一个节点集合作为终点
            for (EndpointType leEndNode : vctvlNodes[i + 1]) {
                bool bSameFlag = false;
                // 如果可以重试
                if (false == bIsThrowWhenFail) {
                    // 从已经使用过的终点中寻找当前节点是否已经被使用过
                    for (EndpointType leNgEndNode : vctleNgEndNode) {
                        if (((leEndNode.lLaneId != 0) && (leEndNode.lLaneId == leNgEndNode.lLaneId)) || ((leEndNode.lLaneId == 0) && (leEndNode.lLinkId.lValue == leNgEndNode.lLinkId.lValue))) {
                            bSameFlag = true;
                        }
                    }
                }
                // 如果当前节点未被使用，则将其放入终点集合中
                if (false == bSameFlag) {
                    vctleEndNode.push_back(leEndNode);
                }
            }
            // 如果终点集合为空，则失败
            if (true == vctleEndNode.empty()) {
                return false;
            }

            // jf_log_info("[roadPath::findPath] get path from {0} to {1}", leStartNode.lLinkId.liId, vctleEndNode[0].lLinkId.liId);
            EndpointType leUsedEndNode = {};
            // 尝试一段寻路
            if (true == m_astar.GetPath(leStartNode, vctleEndNode, vctlePath, leUsedEndNode, 5000 /*MapConfig::roaD_Plan_max_links*/, bHighPrecision)) {
                // 将终点集合清空
                vctleEndNode.clear();

                // 更新已经使用过的起点集合
                vctleUsedStartNode.push_back(leStartNode);
                // 将当前段的寻路结果保存
                vctvlPath.push_back(vctlePath);
                // 将当前的寻路终点作为下一段寻路的起点
                leStartNode = leUsedEndNode;
                ++i;
            } else {
                // 将终点集合清空
                vctleEndNode.clear();

                // 回退重新寻找结束点
                if (false == bIsThrowWhenFail) {
                    // 之前的结果有问题，退回重新寻路
                    if (false == vctvlPath.empty()) {
                        // 更新不可使用的终点
                        vctleNgEndNode.push_back(leStartNode);
                        // 更新起点
                        leStartNode = vctleUsedStartNode.at(vctleUsedStartNode.size() - 1);
                        vctleUsedStartNode.pop_back();

                        // 回退
                        vctvlPath.pop_back();
                        --i;
                    }
                    // 起点有问题，重新寻找起点
                    else {
                        ++nStartNodeIndex;
                        if ((sc_nMaxNearbyLinkCnt >= nStartNodeIndex) && (vctvlNodes[0].size() > nStartNodeIndex)) {
                            leStartNode = vctvlNodes[0][nStartNodeIndex];
                        } else {
                            return false;
                        }
                    }
                } else {
                    // 之前的结果有问题，退回重新寻路
                    if (false == vctvlPath.empty()) {
                        // vctleNgEndNode.push_back(leStartNode);
                        leStartNode = vctleUsedStartNode.at(vctleUsedStartNode.size() - 1);
                        vctleUsedStartNode.pop_back();

                        vctvlPath.pop_back();
                        ++i;
                    }
                    // 起点有问题，放弃起点
                    else {
                        ++nStartNodeIndex;
                        if (nSize - 1 > nStartNodeIndex) {
                            leStartNode = vctvlNodes[nStartNodeIndex][0];
                        } else {
                            return false;
                        }
                    }
                }
            }
        }

        // 寻路失败
        if (true == vctvlPath.empty()) {
            return false;
        }

        // 保存寻路结果
        nSize = vctvlPath.size();
        // 将数段寻路结果顺序保存
        for (int i = 0; i < nSize; ++i) {
            vctleOutWayPoint.push_back(vctvlPath[i][0]);
            for (EndpointType leAsNode : vctvlPath[i]) {
                vctleOutRoute.push_back(leAsNode);
            }
            // 去重
            if ((i != nSize - 1) && (false == vctvlPath[i].empty())) {
                vctleOutRoute.pop_back();
            }
        }
        // 更新所使用的节点
        vctleOutWayPoint.push_back(vctvlPath[nSize - 1][vctvlPath[nSize - 1].size() - 1]);

        return true;
    }

    // 保存寻路结果，如果寻路结果需要保存成文件
    void SaveJson(const std::vector<EndpointType> &vctleOutRoute, const std::string &strFileName) {
        std::stringstream ss;
        ajson::save_to(ss, vctleOutRoute);
        FileSys::Save(MapConfig::road_plan_file_dir + strFileName, ss.str(), MapConfig::tile_encrypt);
    }

    // 读取寻路结果，如果寻路结果需要保存成文件
    void ReadJson(std::vector<EndpointType> &vctleOutRoute, const std::string &strFileName) {
        std::string s = {};
        FileSys::Read(MapConfig::road_plan_file_dir + strFileName, s, MapConfig::tile_encrypt);
        ajson::load_from_buff(vctleOutRoute, s.c_str());
    }

   private:
    // 获取节点
    bool GetEndPoint(const std::vector<Point> &vctptWayNode, bool bIsThrowWhenFail, int nMaxNearbyLinkCnt, std::vector<std::vector<EndpointType>> &vctvlOutEndpoints) {
        int nSize = vctptWayNode.size();
        int nLastValidIndex = 0;
        for (int i = 0; i < nSize; ++i) {
            Point ptNode = vctptWayNode.at(i);
            std::vector<Point> vctptNearNode = {};
            std::vector<double> vctdDistanceNode = {};
            std::vector<EndpointType> vctleNode = {};
            // 获取和指定点范围内的所有节点
            if (false == HdLib::GetInstance()->GetNearEndpoints(ptNode, MapConfig::route_rect_lon, MapConfig::route_rect_lat, vctptNearNode, vctleNode, vctdDistanceNode)) {
                if (true == bIsThrowWhenFail) {
                    // jf_log_error("[Route::findPath] get nearest node for failed");
                    continue;
                } else {
                    // jf_log_error("[Route::findPath] get nearest node for failed");
                    return false;
                }
            } else {
                // 优先使用点所在的车道
                std::vector<EndpointType> vctleLocEp = {};
                if (true == HdLib::GetInstance()->GetLocateEndpoints(ptNode, MapConfig::route_rect_lon, MapConfig::route_rect_lat, vctleLocEp)) {
                    std::vector<EndpointType> vctleTmpEp = vctleLocEp;
                    for (const EndpointType &cleNearEp : vctleNode) {
                        bool bIsAlreadyIn = false;
                        for (const EndpointType &cleLocEp : vctleLocEp) {
                            if (((cleNearEp.lLaneId != 0) && (cleNearEp.lLaneId == cleLocEp.lLaneId)) || ((cleNearEp.lLaneId == 0) && (cleNearEp.lLinkId.lValue == cleLocEp.lLinkId.lValue))) {
                                bIsAlreadyIn = true;
                            }
                        }
                        if (false == bIsAlreadyIn) {
                            vctleTmpEp.push_back(cleNearEp);
                        }
                    }
                    vctleNode.swap(vctleTmpEp);
                }

                // 去掉过多的结果，只取最近的10条link
                while (vctleNode.size() > nMaxNearbyLinkCnt) {
                    vctleNode.pop_back();
                };

                // 去掉过于密集的点
                if ((true == vctvlOutEndpoints.empty()) || ((false == vctvlOutEndpoints.empty()) && (PointsDis(vctptWayNode.at(nLastValidIndex), vctptWayNode.at(i)) >= 10 /*MapConfig::roaD_Plan_min_waypoint_dis*/) &&
                                                            (vctvlOutEndpoints[vctvlOutEndpoints.size() - 1][0].lLinkId.lValue != vctleNode[0].lLinkId.lValue))) {
                    nLastValidIndex = i;
                    vctvlOutEndpoints.push_back(vctleNode);
                }
            }
        }

        return true;
    }

   private:
    //    MapLib m_mlMapLib;
    AStar m_astar;
};
}  // namespace jf

#endif
