Graph.h

#ifndef _OVERSEER_GRAPH_H_
#define _OVERSEER_GRAPH_H_

#include "Map.h"
#include "ChokePoint.h"
#include "Region.h"
#include <algorithm>

namespace Overseer{

    class ChokePoint;

    typedef std::vector<ChokePoint> CPPath;
    typedef std::tuple<size_t, size_t, size_t> ChokePointId;

    struct AstarNode {
        AstarNode(ChokePointId cp_id, float f);

        ChokePointId choke_point_id;
        float f_score;

        bool operator==(const AstarNode& rhs) const;
    };

    struct LowerScore {
        bool operator()(const AstarNode& lhs, const AstarNode& rhs) const;
    };

    class Graph {

        public:

            Graph();
            Graph(Map* map);

            std::vector<ChokePoint> getChokePoints(size_t region_id_a, size_t region_id_b) const;

            std::vector<ChokePoint> getChokePoints() const;

            ChokePoint getChokePoint(ChokePointId cp_id) const;

            CPPath& getPath(ChokePoint cp_a, ChokePoint cp_b);

            void createChokePoints();

            void computePaths();

            void setMap(Map *map);

        private:
            size_t num_regions;
            Map *p_map;
            std::vector<std::vector<std::vector<ChokePoint>>> m_ChokePointsMatrix;
            std::map<ChokePointId, std::map<ChokePointId, float>> m_ChokePointDistanceMap;
            std::map<ChokePointId, std::vector<ChokePoint>> m_ChokePointNeighbors;
            std::map<size_t, std::vector<ChokePoint>> m_RegionChokePoints;
            std::map<ChokePointId, std::map<ChokePointId, CPPath>> m_ChokePointPaths;

            // Check if region id is valid
            bool validId(size_t id_arg) const;
            // Cluster the point within a chokepoint
            std::vector<std::deque<TilePosition>>* createClustersFromFrontiers(std::pair<std::pair<size_t,size_t>, std::vector<TilePosition>> frontierByRegionPair);
            // Caculate the distances between adjacent chokepoints
            void initializeChokePointDistanceMap();
            // Sets the distance between cp_a and cp_b, if not adjacent distance -> infinity.
            void setChokePointDistance(ChokePointId cp_a, ChokePointId cp_b, float d);
            // Gets the distance between chokepoints cp_a and cp_b
            float getChokePointDistance(ChokePoint cp_a, ChokePoint cp_b);
            // Sets the CPPath path to the chokepoints cp_a and cp_b
            void setChokePointPath(ChokePoint cp_a, ChokePoint cp_b, CPPath path);
            // Uses A* to calculate the shortest path between Chokepoints a and b
            CPPath aStar(ChokePoint cp_a, ChokePoint cp_b);
            // calculate the shortest euclidean distance between cp_a and cp_b
            float bestHeuristic(ChokePoint cp_a, ChokePoint cp_b);
            // Reconstructs the path from A* output
            CPPath reconstructPath(std::map<ChokePointId, ChokePointId> came_from, ChokePoint current);
            // Returns the chokepoints id is the output from A*
            std::vector<ChokePointId> extractKeys(std::map<ChokePointId, ChokePointId> const& input_map);
    };
}

#endif /* _GRAPH_H_ */