#include "graph_map.h"
#include <algorithm>
#include <cmath>
#include <queue>

// A*路径规划实现（简化版，不考虑其他AGV）
std::vector<Path*> GraphMap::findPath(Point* start, Point* end, const std::vector<AGV*>& avoid_agvs) {
    if (start == end) {
        return {};
    }

    // 注意：avoid_agvs参数保留以保持接口兼容性，但不再使用
    // 路径规划专注于找到理论最优路径，避让由资源管理器处理

    // A*算法
    struct Node {
        Point* point;
        Path* path_from_parent;  // 到达这个点的路径
        double g_score;          // 从起点到当前点的实际代价
        double f_score;          // g_score + 启发式代价

        Node(Point* p, Path* path, double g, double f)
            : point(p), path_from_parent(path), g_score(g), f_score(f) {}

        bool operator<(const Node& other) const {
            return f_score > other.f_score;  // 最小堆
        }
    };

    std::priority_queue<Node> open_set;
    std::unordered_map<Point*, Path*> came_from;
    std::unordered_map<Point*, double> g_score;

    // 初始化起点
    open_set.emplace(start, nullptr, 0, start->distanceTo(*end));
    g_score[start] = 0;

    while (!open_set.empty()) {
        Node current = open_set.top();
        open_set.pop();

        // 到达终点
        if (current.point == end) {
            std::vector<Path*> path_list;
            Point* curr = end;

            // 回溯构建路径
            while (curr != start && came_from.find(curr) != came_from.end()) {
                Path* path = came_from[curr];
                path_list.insert(path_list.begin(), path);
                curr = path->start == curr ? path->end : path->start;
            }

            return path_list;
        }

        // 探索相邻路径
        const std::vector<Path*>& adj_paths = getAdjacentPaths(current.point);
        for (Path* path : adj_paths) {
            // 简化版：不再检查路径占用，由资源管理器处理
            // 获取相邻节点
            Point* neighbor = path->getOtherEnd(current.point);
            if (!neighbor) {
                continue;
            }

            // 计算代价
            double tentative_g = current.g_score + path->length;

            // 如果这是更好的路径或者首次访问
            if (g_score.find(neighbor) == g_score.end() || tentative_g < g_score[neighbor]) {
                came_from[neighbor] = path;
                g_score[neighbor] = tentative_g;

                // 计算启发式代价（到终点的直线距离）
                double f_score = tentative_g + neighbor->distanceTo(*end);

                open_set.emplace(neighbor, path, tentative_g, f_score);
            }
        }
    }

    // 无法找到路径
    return {};
}

bool GraphMap::isPathAvailable(Path* path, const AGV* requester) {
    std::lock_guard<std::mutex> lock(map_mutex_);

    // 如果路径未被占用，则可用
    if (!path->isOccupied()) {
        return true;
    }

    // 如果被请求者自己占用，也认为可用（已在路上）
    return path->occupied_by == requester;
}

void GraphMap::reservePath(Path* path, AGV* agv) {
    std::lock_guard<std::mutex> lock(map_mutex_);
    path->occupied_by = agv;
}

void GraphMap::releasePath(Path* path, AGV* agv) {
    std::lock_guard<std::mutex> lock(map_mutex_);
    if (path->occupied_by == agv) {
        path->occupied_by = nullptr;
    }
}

// K最短路径实现 (简化版Yen's算法变体)
std::vector<std::vector<Path*>> GraphMap::findKShortestPaths(
    Point* start,
    Point* end,
    int K,
    const std::vector<AGV*>& avoid_agvs
) {
    std::vector<std::vector<Path*>> results;

    if (K <= 0 || start == nullptr || end == nullptr || start == end) {
        return results;
    }

    // 1. 找到最短路径 (使用标准A*)
    std::vector<Path*> first_path = findPath(start, end, avoid_agvs);
    if (first_path.empty()) {
        return results;  // 无法找到任何路径
    }
    results.push_back(first_path);

    if (K == 1) {
        return results;
    }

    // 2. 寻找K-1条备选路径 (通过边惩罚方法)
    std::unordered_map<Path*, double> edge_penalties;

    for (int k = 1; k < K; ++k) {
        std::vector<Path*> best_alternative;
        double best_cost = 1e9;

        // 尝试惩罚前一条路径中的每条边，寻找替代方案
        const std::vector<Path*>& prev_path = results[k-1];

        // 随机选择一些偏离点来探索不同路径 (简化版)
        // 完整Yen's算法会比较慢，这里使用贪心偏离策略
        for (size_t deviate_idx = 0; deviate_idx < prev_path.size() && deviate_idx < 3; ++deviate_idx) {
            Path* banned_path = prev_path[deviate_idx];

            // 临时增加这条边的代价
            double original_penalty = edge_penalties[banned_path];
            edge_penalties[banned_path] += 1000.0;  // 大惩罚值

            // 使用修改后的代价重新运行A*
            std::vector<Path*> alternative = _findPathWithPenalties(start, end, edge_penalties, avoid_agvs);

            // 恢复代价
            edge_penalties[banned_path] = original_penalty;

            if (!alternative.empty()) {
                // 计算路径总长度
                double total_length = 0.0;
                for (Path* p : alternative) {
                    total_length += p->length;
                }

                // 检查是否与已有路径重复
                bool is_duplicate = false;
                for (const auto& existing : results) {
                    if (_arePathsSame(alternative, existing)) {
                        is_duplicate = true;
                        break;
                    }
                }

                if (!is_duplicate && total_length < best_cost) {
                    best_cost = total_length;
                    best_alternative = alternative;
                }
            }
        }

        if (best_alternative.empty()) {
            break;  // 无法找到更多路径
        }

        results.push_back(best_alternative);
    }

    return results;
}

// 带边惩罚的A*算法 (内部辅助函数)
std::vector<Path*> GraphMap::_findPathWithPenalties(
    Point* start,
    Point* end,
    const std::unordered_map<Path*, double>& penalties,
    const std::vector<AGV*>& avoid_agvs
) {
    if (start == end) {
        return {};
    }

    struct Node {
        Point* point;
        Path* path_from_parent;
        double g_score;
        double f_score;

        Node(Point* p, Path* path, double g, double f)
            : point(p), path_from_parent(path), g_score(g), f_score(f) {}

        bool operator<(const Node& other) const {
            return f_score > other.f_score;
        }
    };

    std::priority_queue<Node> open_set;
    std::unordered_map<Point*, Path*> came_from;
    std::unordered_map<Point*, double> g_score;

    open_set.emplace(start, nullptr, 0, start->distanceTo(*end));
    g_score[start] = 0;

    while (!open_set.empty()) {
        Node current = open_set.top();
        open_set.pop();

        if (current.point == end) {
            std::vector<Path*> path_list;
            Point* curr = end;

            while (curr != start && came_from.find(curr) != came_from.end()) {
                Path* path = came_from[curr];
                path_list.insert(path_list.begin(), path);
                curr = path->start == curr ? path->end : path->start;
            }

            return path_list;
        }

        const std::vector<Path*>& adj_paths = getAdjacentPaths(current.point);
        for (Path* path : adj_paths) {
            Point* neighbor = path->getOtherEnd(current.point);
            if (!neighbor) {
                continue;
            }

            // 计算带惩罚的代价
            double penalty = 0.0;
            auto it = penalties.find(path);
            if (it != penalties.end()) {
                penalty = it->second;
            }

            double tentative_g = current.g_score + path->length + penalty;

            if (g_score.find(neighbor) == g_score.end() || tentative_g < g_score[neighbor]) {
                came_from[neighbor] = path;
                g_score[neighbor] = tentative_g;

                double f_score = tentative_g + neighbor->distanceTo(*end);
                open_set.emplace(neighbor, path, tentative_g, f_score);
            }
        }
    }

    return {};
}

// 检查两条路径是否相同 (内部辅助函数)
bool GraphMap::_arePathsSame(const std::vector<Path*>& path1, const std::vector<Path*>& path2) {
    if (path1.size() != path2.size()) {
        return false;
    }

    for (size_t i = 0; i < path1.size(); ++i) {
        if (path1[i] != path2[i]) {
            return false;
        }
    }

    return true;
}

// 计算路径特征
GraphMap::PathFeatures GraphMap::calculatePathFeatures(const std::vector<Path*>& path) {
    PathFeatures features;

    if (path.empty()) {
        features.length = 0.0;
        features.conflicts = 0;
        features.avg_speed = 0.0;
        features.smoothness = 0.0;
        return features;
    }

    // 计算总长度和平均速度
    double total_length = 0.0;
    double total_speed = 0.0;
    int conflicts = 0;

    for (Path* p : path) {
        total_length += p->length;
        total_speed += p->max_speed;
        if (p->isOccupied()) {
            conflicts++;
        }
    }

    features.length = total_length;
    features.avg_speed = path.empty() ? 0.0 : total_speed / path.size();
    features.conflicts = conflicts;

    // 计算平滑度 (基于角度变化)
    double curvature_changes = 0.0;
    for (size_t i = 1; i < path.size() - 1; ++i) {
        // 计算三个连续点形成的角度
        Point* p1 = path[i-1]->start == path[i]->start ? path[i-1]->end : path[i-1]->start;
        Point* p2 = path[i]->start;
        Point* p3 = path[i+1]->end == path[i]->end ? path[i+1]->start : path[i+1]->end;

        if (p1 && p2 && p3) {
            // 计算向量
            double v1x = p2->x - p1->x;
            double v1y = p2->y - p1->y;
            double v2x = p3->x - p2->x;
            double v2y = p3->y - p2->y;

            // 归一化
            double norm1 = std::sqrt(v1x * v1x + v1y * v1y);
            double norm2 = std::sqrt(v2x * v2x + v2y * v2y);

            if (norm1 > 1e-6 && norm2 > 1e-6) {
                v1x /= norm1;
                v1y /= norm1;
                v2x /= norm2;
                v2y /= norm2;

                // 点积
                double dot = v1x * v2x + v1y * v2y;
                dot = std::max(-1.0, std::min(1.0, dot));  // 裁剪到[-1, 1]

                // 角度
                double angle = std::acos(dot);
                curvature_changes += angle;
            }
        }
    }

    features.smoothness = 1.0 / (1.0 + curvature_changes);

    return features;
}