#ifndef _JFXMAP_TRAVERSAL_H_
#define _JFXMAP_TRAVERSAL_H_

#include "SdTypes.h"

namespace jf {

struct Connection {
    SdNodeId from;
    SdNodeId to;
    double to_acc_length;
    inline bool operator<(const Connection &o) const {
        if (from < o.from) {
            return true;
        } else if (from == o.from && to < o.to) {
            return true;
        }
        return false;
    }
    inline bool operator==(const Connection &o) const { return from == o.from && to == o.to; }
};

class Traversal {
   public:
    // 注意：这个接口包含from
    static std::set<SdNodeId> IterNodesInMetre(const SdNodeId &from, double distance, bool backward = false /*, const visitHookFuncType& hookFunc = default_parameter*/) {
        std::set<SdNodeId> ret;
        ret.insert(from);
        SdWorld::GetSingleton()->BFS(Connection{from, from, 0}, ret, [&distance, &backward](bool visited_node, bool is_first, const Connection &c, std::set<SdNodeId> &ret, std::set<Connection> &nexts) {
            if (visited_node) {
                return false;
            }
            const SdNode *nd_to = SdWorld::GetSingleton()->GetNodeData(c.to);
            if (!nd_to) {
                return false;
            }
            std::vector<SdNodeId> degrees;
            if (backward) {
                degrees = nd_to->inDegree;
            } else {
                degrees = nd_to->outDegree;
            }
            ret.insert(c.to);
            for (int i = 0; i < degrees.size(); ++i) {
                double length = c.to_acc_length + PointsDis(c.from.p, c.to.p);
                if (length <= distance) {
                    nexts.insert({c.to, degrees[i], length});
                }
            }

            return false;
        });
        return ret;
    }
    // 注意：这个接口包含from
    static std::set<RoadId> IterRoadIdInMetre(const SdNodeId &from, double distance, bool backward = false) {
        std::set<RoadId> ret;
        SdWorld::GetSingleton()->BFS(Connection{from, from, 0}, ret, [&distance, &backward](bool visited_node, bool is_first, const Connection &c, std::set<RoadId> &ret, std::set<Connection> &nexts) {
            if (visited_node) {
                return false;
            }
            const SdNode *nd_from = SdWorld::GetSingleton()->GetNodeData(c.from);
            if (!nd_from) {
                return false;
            }
            const SdNode *nd_to = SdWorld::GetSingleton()->GetNodeData(c.to);
            if (!nd_to) {
                return false;
            }
            std::vector<SdNodeId> degrees;
            if (backward) {
                degrees = nd_to->inDegree;
                ret.insert(nd_from->inRids.begin(), nd_from->inRids.end());
            } else {
                degrees = nd_to->outDegree;
                ret.insert(nd_to->outRids.begin(), nd_to->outRids.end());
            }
            for (int i = 0; i < degrees.size(); ++i) {
                double length = c.to_acc_length + PointsDis(c.from.p, c.to.p);
                if (length <= distance) {
                    nexts.insert({c.to, degrees[i], length});
                }
            }

            return false;
        });
        return ret;
    }
    // 注意：这个接口包含from
    static void GetNextNNodes(int n, const SdNode *from, std::set<const SdNode *> &ret) {
        SdWorld::GetSingleton()->BFS(from, ret, [n](bool visited, bool isFirst, const SdNode *visitingNd, std::set<const SdNode *> &ret, std::set<const SdNode *> &nexts) {
            if (visited) {
                return false;
            }
            ret.insert(visitingNd);
            if (ret.size() >= n) {
                return true;
            }
            std::vector<SdNodeId> out;
            visitingNd->GetNexts(out);
            for (SdNodeId id : out) {
                const SdNode *nd = SdWorld::GetSingleton()->GetNodeData(id);
                nexts.insert(nd);
            }
            return false;
        });
    }
};

}  // namespace jf

#endif