// openmw-0002-test.cpp
// CWE-407: A* pathfinding std::find(openset) O(V*E) in pathgrid.cpp
//
// The A* implementation in PathgridGraph::aStarSearch uses
// std::find(openset.begin(), openset.end(), dest) to check if a point is in
// the open set. The openset is a std::list<size_t>, so this is O(N) per edge
// check. The code even has a TODO: "if this causes performance problems a
// hash table may help".
//
// Fix: add an unordered_set<size_t> opensetMembership for O(1) membership.
// This test validates correctness and measures operation counts.

#include <algorithm>
#include <cassert>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <deque>
#include <list>
#include <set>
#include <unordered_set>
#include <vector>

struct Edge {
    size_t index;
    float cost;
};

struct Point {
    int mX, mY, mZ;
};

static constexpr size_t NoIndex = static_cast<size_t>(-1);

float costAStar(const Point& a, const Point& b) {
    return 300.0f * (std::abs(a.mX - b.mX) + std::abs(a.mY - b.mY) + std::abs(a.mZ - b.mZ));
}

struct Node {
    std::vector<Edge> edges;
};

static size_t gProbeCount = 0;

// ---------- DEFECTIVE: std::find on list, counting probes ----------
namespace Defective {
    std::deque<size_t> aStarSearch(const std::vector<Node>& graph,
                                    const std::vector<Point>& points,
                                    size_t start, size_t goal) {
        std::deque<size_t> path;
        size_t graphSize = graph.size();
        std::vector<float> gScore(graphSize, -1);
        std::vector<float> fScore(graphSize, -1);
        std::vector<size_t> graphParent(graphSize, NoIndex);

        gScore[start] = 0;
        fScore[start] = costAStar(points[start], points[goal]);

        std::list<size_t> openset;
        std::set<size_t> closedset;
        openset.push_back(start);

        size_t current = start;

        while (!openset.empty()) {
            current = openset.front();
            openset.pop_front();

            if (current == goal) break;
            closedset.insert(current);

            for (const auto& edge : graph[current].edges) {
                if (!closedset.contains(edge.index)) {
                    size_t dest = edge.index;
                    float tentativeG = gScore[current] + edge.cost;
                    // DEFECT: O(N) linear scan. Count each element comparison.
                    bool isInOpenSet = false;
                    for (auto it = openset.begin(); it != openset.end(); ++it) {
                        gProbeCount++;
                        if (*it == dest) { isInOpenSet = true; break; }
                    }
                    if (!isInOpenSet || tentativeG < gScore[dest]) {
                        graphParent[dest] = current;
                        gScore[dest] = tentativeG;
                        fScore[dest] = tentativeG + costAStar(points[dest], points[goal]);
                        if (!isInOpenSet) {
                            auto it = openset.begin();
                            for (; it != openset.end(); ++it)
                                if (fScore[*it] > fScore[dest]) break;
                            openset.insert(it, dest);
                        }
                    }
                }
            }
        }

        if (current != goal) return path;
        while (graphParent[current] != NoIndex) {
            path.push_front(current);
            current = graphParent[current];
        }
        path.push_front(start);
        return path;
    }
}

// ---------- PATCHED: unordered_set for membership ----------
namespace Patched {
    std::deque<size_t> aStarSearch(const std::vector<Node>& graph,
                                    const std::vector<Point>& points,
                                    size_t start, size_t goal) {
        std::deque<size_t> path;
        size_t graphSize = graph.size();
        std::vector<float> gScore(graphSize, -1);
        std::vector<float> fScore(graphSize, -1);
        std::vector<size_t> graphParent(graphSize, NoIndex);

        gScore[start] = 0;
        fScore[start] = costAStar(points[start], points[goal]);

        std::list<size_t> openset;
        std::set<size_t> closedset;
        std::unordered_set<size_t> opensetMembership;
        openset.push_back(start);
        opensetMembership.insert(start);

        size_t current = start;

        while (!openset.empty()) {
            current = openset.front();
            openset.pop_front();
            opensetMembership.erase(current);

            if (current == goal) break;
            closedset.insert(current);

            for (const auto& edge : graph[current].edges) {
                if (!closedset.contains(edge.index)) {
                    size_t dest = edge.index;
                    float tentativeG = gScore[current] + edge.cost;
                    // PATCHED: O(1) hash set lookup (1 probe counted)
                    bool isInOpenSet = opensetMembership.count(dest) > 0;
                    gProbeCount++; // just 1 probe
                    if (!isInOpenSet || tentativeG < gScore[dest]) {
                        graphParent[dest] = current;
                        gScore[dest] = tentativeG;
                        fScore[dest] = tentativeG + costAStar(points[dest], points[goal]);
                        if (!isInOpenSet) {
                            opensetMembership.insert(dest);
                            auto it = openset.begin();
                            for (; it != openset.end(); ++it)
                                if (fScore[*it] > fScore[dest]) break;
                            openset.insert(it, dest);
                        }
                    }
                }
            }
        }

        if (current != goal) return path;
        while (graphParent[current] != NoIndex) {
            path.push_front(current);
            current = graphParent[current];
        }
        path.push_front(start);
        return path;
    }
}

// Build a dense random-weight graph. All nodes at same position (0,0,0) so
// heuristic is zero and A* degenerates to Dijkstra, maximizing openset size.
void buildDenseGraph(size_t N, size_t edgesPerNode, std::vector<Node>& graph, std::vector<Point>& points) {
    graph.resize(N);
    points.resize(N);
    for (size_t i = 0; i < N; i++) {
        points[i] = {0, 0, 0}; // zero heuristic = Dijkstra
        for (size_t j = 1; j <= edgesPerNode && i + j < N; j++) {
            float cost = 100.0f + (float)(i * 7 + j * 13) * 0.1f; // varying costs
            graph[i].edges.push_back({i + j, cost});
            graph[i + j].edges.push_back({i, cost});
        }
    }
}

int main() {
    const size_t N = 2000;
    const size_t edgesPerNode = 8;

    std::vector<Node> graph;
    std::vector<Point> points;
    buildDenseGraph(N, edgesPerNode, graph, points);

    size_t start = 0;
    size_t goal = N - 1;

    // Correctness: paths must be identical
    gProbeCount = 0;
    auto p1 = Defective::aStarSearch(graph, points, start, goal);
    gProbeCount = 0;
    auto p2 = Patched::aStarSearch(graph, points, start, goal);
    assert(p1 == p2);
    printf("PASS correctness: paths match, length=%zu (N=%zu)\n", p1.size(), N);

    // Operation count comparison
    gProbeCount = 0;
    Defective::aStarSearch(graph, points, start, goal);
    size_t defOps = gProbeCount;

    gProbeCount = 0;
    Patched::aStarSearch(graph, points, start, goal);
    size_t patOps = gProbeCount;

    double opRatio = (double)defOps / (double)patOps;
    printf("Defective probes: %zu  Patched probes: %zu  Op-count ratio: %.1fx\n",
           defOps, patOps, opRatio);
    fflush(stdout);
    assert(opRatio > 2.0 && "Patched should have at least 2x fewer probes");
    printf("PASS performance: %.1fx fewer membership probes\n", opRatio);

    return 0;
}