RCS-3000/include/dispatch/graph_map.h

#ifndef GRAPH_MAP_H
#define GRAPH_MAP_H

#include <string>
#include <vector>
#include <unordered_map>
#include <unordered_set>
#include <memory>
#include <mutex>
#include <cmath>

// 前向声明
class AGV;

/**
 * @brief 地图节点（Point）
 */
class Point {
public:
    int id;                     // 节点唯一ID
    double x, y;               // 坐标位置（米）
    std::string name;          // 节点名称

    // 资源占用管理
    AGV* occupied_by;          // 当前占用该节点的AGV

    Point(int id_, double x_, double y_, const std::string& name_ = "")
        : id(id_), x(x_), y(y_), name(name_), occupied_by(nullptr) {}

    bool isOccupied() const { return occupied_by != nullptr; }

    double distanceTo(const Point& other) const {
        double dx = x - other.x;
        double dy = y - other.y;
        return sqrt(dx * dx + dy * dy);
    }
};

/**
 * @brief 路径边（Path）
 */
class Path {
public:
    int id;                     // 路径唯一ID
    Point* start;              // 起始节点
    Point* end;                // 终点节点
    double length;             // 路径长度（米）
    double max_speed;          // 最大允许速度（米/秒）
    bool bidirectional;        // 是否双向通行

    // 资源占用管理
    AGV* occupied_by;          // 当前占用该路径的AGV

    Path(int id_, Point* start_, Point* end_, bool bidirectional_ = true)
        : id(id_), start(start_), end(end_), bidirectional(bidirectional_), occupied_by(nullptr) {
        length = start->distanceTo(*end);
        max_speed = 2.0;  // 默认最大速度2m/s
    }

    bool isOccupied() const { return occupied_by != nullptr; }

    Point* getOtherEnd(const Point* p) {
        if (p == start) return end;
        if (p == end) return start;
        return nullptr;
    }
};

/**
 * @brief 地图图结构
 */
class GraphMap {
private:
    std::unordered_map<int, std::unique_ptr<Point>> points_;
    std::unordered_map<int, std::unique_ptr<Path>> paths_;
    std::unordered_map<Point*, std::vector<Path*>> adj_paths_;
    mutable std::mutex map_mutex_;

public:
    GraphMap() = default;

    Point* addPoint(int id, double x, double y, const std::string& name = "") {
        std::lock_guard<std::mutex> lock(map_mutex_);
        auto point = std::make_unique<Point>(id, x, y, name);
        Point* ptr = point.get();
        points_[id] = std::move(point);
        return ptr;
    }

    Path* addPath(int id, int start_id, int end_id, bool bidirectional = true) {
        std::lock_guard<std::mutex> lock(map_mutex_);

        auto start_it = points_.find(start_id);
        auto end_it = points_.find(end_id);

        if (start_it == points_.end() || end_it == points_.end()) {
            return nullptr;
        }

        auto path = std::make_unique<Path>(id, start_it->second.get(), end_it->second.get(), bidirectional);
        Path* ptr = path.get();

        paths_[id] = std::move(path);

        adj_paths_[start_it->second.get()].push_back(ptr);
        adj_paths_[end_it->second.get()].push_back(ptr);

        return ptr;
    }

    Point* getPoint(int id) {
        auto it = points_.find(id);
        return it != points_.end() ? it->second.get() : nullptr;
    }

    Point* getPointById(int id) {
        return getPoint(id);  // 别名，保持接口一致性
    }

    Path* getPath(int id) {
        auto it = paths_.find(id);
        return it != paths_.end() ? it->second.get() : nullptr;
    }

    const std::vector<Path*>& getAdjacentPaths(Point* point) {
        static std::vector<Path*> empty;
        auto it = adj_paths_.find(point);
        return it != adj_paths_.end() ? it->second : empty;
    }

    // 获取所有路径（用于清理AGV占用）
    std::vector<Path*> getAllPaths() {
        std::lock_guard<std::mutex> lock(map_mutex_);
        std::vector<Path*> all_paths;
        for (const auto& pair : paths_) {
            all_paths.push_back(pair.second.get());
        }
        return all_paths;
    }

    // A*路径规划
    std::vector<Path*> findPath(Point* start, Point* end, const std::vector<AGV*>& avoid_agvs);

    // K最短路径规划 (Yen's算法或简化变体)
    // 用于DQN强化学习: 生成K条候选路径供智能体选择
    std::vector<std::vector<Path*>> findKShortestPaths(
        Point* start,
        Point* end,
        int K,
        const std::vector<AGV*>& avoid_agvs = {}
    );

    // 计算路径特征 (用于DQN状态编码)
    struct PathFeatures {
        double length;
        int conflicts;
        double avg_speed;
        double smoothness;
    };
    PathFeatures calculatePathFeatures(const std::vector<Path*>& path);

private:
    // 内部辅助函数
    std::vector<Path*> _findPathWithPenalties(
        Point* start,
        Point* end,
        const std::unordered_map<Path*, double>& penalties,
        const std::vector<AGV*>& avoid_agvs
    );

    bool _arePathsSame(const std::vector<Path*>& path1, const std::vector<Path*>& path2);

public:
    bool isPathAvailable(Path* path, const AGV* requester);
    void reservePath(Path* path, AGV* agv);
    void releasePath(Path* path, AGV* agv);

    void getStatistics(size_t& num_points, size_t& num_paths) const {
        std::lock_guard<std::mutex> lock(map_mutex_);
        num_points = points_.size();
        num_paths = paths_.size();
    }
};

#endif // GRAPH_MAP_H