#include "topology/TunnelPairing.h"

#include <algorithm>
#include <cmath>
#include <queue>
#include <unordered_map>
#include <unordered_set>
#include <cstdio>
#include <cstdarg>

namespace {
// Lightweight stderr-style logger to a fixed file. We deliberately use a
// global FILE* rather than wxLog (MarklinCommon doesn't link wx). Disabled
// when MC_TUNNEL_DEBUG is not defined.
#define MC_TUNNEL_DEBUG 1
#if MC_TUNNEL_DEBUG
static void TLog(const char* fmt, ...) {
    static FILE* f = std::fopen("C:/Users/Public/trackdesigner_tunnel.log", "a");
    if (!f) return;
    va_list ap;
    va_start(ap, fmt);
    std::vfprintf(f, fmt, ap);
    std::fputc('\n', f);
    std::fflush(f);
    va_end(ap);
}
#else
static void TLog(const char*, ...) {}
#endif
} // namespace

namespace MC {

// ── Portal id helper ──────────────────────────────────────────────────────────
bool IsTunnelPortalId(const std::string& itemId) {
    return itemId == "tunnel_portal_single";
}

// ── Anchor a portal to its nearest piece ──────────────────────────────────────
// Returns -1 if no piece is within the anchor radius. The radius is generous
// enough to cover portals placed up to ~halfway along a long straight piece
// (188 mm / 3 = ~62 mm from one end). Manually-dragged portals also anchor
// over this distance, which is fine -- a portal that far from any track will
// rarely be intentional.
static int AnchorPortal(const PlacedScenery& portal,
                         const std::vector<PlacedPiece>& pieces)
{
    const double kAnchorRadius = 100.0;
    int bestId = -1;
    double bestDistSq = kAnchorRadius * kAnchorRadius;
    double overallBestSq = 1e18;
    int    overallBestId = -1;
    for (const auto& p : pieces) {
        double dx = p.transform.pos.x - portal.transform.pos.x;
        double dy = p.transform.pos.y - portal.transform.pos.y;
        double d2 = dx*dx + dy*dy;
        if (d2 < overallBestSq) { overallBestSq = d2; overallBestId = p.id; }
        if (d2 <= bestDistSq) {
            bestDistSq = d2;
            bestId = p.id;
        }
    }
    TLog("AnchorPortal: portal id=%d pos=(%.1f,%.1f) -> anchor=%d (best=%d dist=%.1f, radius=%.1f)",
         portal.id, portal.transform.pos.x, portal.transform.pos.y,
         bestId, overallBestId, std::sqrt(overallBestSq), kAnchorRadius);
    return bestId;
}

// ── BFS distances and predecessors from a starting piece ──────────────────────
// `dist[pieceId]` = number of edges (pieces traversed) from `startPieceId`.
// `pred[pieceId]` = piece id of the predecessor on the shortest path.
// Pieces not reachable are absent from both maps.
static void BfsTopology(int startPieceId,
                         const std::vector<PlacedPiece>& pieces,
                         const std::unordered_map<int,size_t>& idxOf,
                         std::unordered_map<int,int>& dist,
                         std::unordered_map<int,int>& pred)
{
    dist[startPieceId] = 0;
    pred[startPieceId] = -1;
    std::queue<int> q;
    q.push(startPieceId);

    while (!q.empty()) {
        int cur = q.front(); q.pop();
        int curDist = dist[cur];
        auto it = idxOf.find(cur);
        if (it == idxOf.end()) continue;
        const auto& piece = pieces[it->second];
        for (const auto& joint : piece.joints) {
            int neighbor = joint.pieceId;
            if (neighbor == -1) continue;
            if (dist.find(neighbor) != dist.end()) continue;
            dist[neighbor] = curDist + 1;
            pred[neighbor] = cur;
            q.push(neighbor);
        }
    }
}

// ── Reconstruct shortest path A -> B ──────────────────────────────────────────
// Returns the list of piece IDs forming the path, including both endpoints.
// Empty if B is not reachable from A.
static std::vector<int> ReconstructPath(int aId, int bId,
                                         const std::unordered_map<int,int>& pred)
{
    std::vector<int> out;
    if (pred.find(bId) == pred.end()) return out;   // unreachable
    int cur = bId;
    while (cur != -1) {
        out.push_back(cur);
        auto it = pred.find(cur);
        if (it == pred.end()) break;
        cur = it->second;
    }
    std::reverse(out.begin(), out.end());
    if (out.empty() || out.front() != aId) return {};
    return out;
}

// ── Find connected component of a piece via BFS ───────────────────────────────
static void FindComponent(int seedPieceId,
                           const std::vector<PlacedPiece>& pieces,
                           const std::unordered_map<int,size_t>& idxOf,
                           std::unordered_set<int>& visited,
                           std::unordered_set<int>& component)
{
    std::queue<int> q;
    q.push(seedPieceId);
    visited.insert(seedPieceId);
    component.insert(seedPieceId);
    while (!q.empty()) {
        int cur = q.front(); q.pop();
        auto it = idxOf.find(cur);
        if (it == idxOf.end()) continue;
        const auto& piece = pieces[it->second];
        for (const auto& joint : piece.joints) {
            int n = joint.pieceId;
            if (n == -1) continue;
            if (visited.insert(n).second) {
                component.insert(n);
                q.push(n);
            }
        }
    }
}

// ── Main pairing entry point ──────────────────────────────────────────────────
std::vector<TunnelPair> ComputeTunnelPairs(
    const std::vector<PlacedPiece>& pieces,
    const std::vector<PlacedScenery>& scenery)
{
    std::vector<TunnelPair> result;
    if (pieces.empty() || scenery.empty()) {
        TLog("ComputeTunnelPairs: pieces=%zu scenery=%zu -> early exit",
             pieces.size(), scenery.size());
        return result;
    }
    TLog("ComputeTunnelPairs: pieces=%zu scenery=%zu", pieces.size(), scenery.size());

    // Index lookup
    std::unordered_map<int,size_t> pieceIdxOf;
    for (size_t i = 0; i < pieces.size(); ++i) pieceIdxOf[pieces[i].id] = i;

    // Anchor each portal. Collect (portalId, anchorPieceId, portalIdx).
    struct Anchored { int portalId; int pieceId; size_t scIdx; };
    std::vector<Anchored> anchored;
    for (size_t i = 0; i < scenery.size(); ++i) {
        const auto& s = scenery[i];
        if (!IsTunnelPortalId(s.itemId)) continue;
        int a = AnchorPortal(s, pieces);
        if (a < 0) continue;
        anchored.push_back({s.id, a, i});
    }
    if (anchored.size() < 2) return result;

    // Group portals by connected component. We discover components on the fly:
    // for each anchored portal whose anchor piece hasn't been visited yet,
    // run a component BFS and collect all anchored portals whose anchor pieces
    // fall in that component.
    std::unordered_set<int> globalVisited;
    for (size_t i = 0; i < anchored.size(); ++i) {
        if (globalVisited.count(anchored[i].pieceId)) continue;

        std::unordered_set<int> component;
        FindComponent(anchored[i].pieceId, pieces, pieceIdxOf,
                      globalVisited, component);

        // Collect portals whose anchor piece is in this component.
        std::vector<Anchored> inComp;
        for (const auto& a : anchored) {
            if (component.count(a.pieceId)) inComp.push_back(a);
        }
        if (inComp.size() < 2) continue;

        // Pick a deterministic "first" portal: the one with smallest portalId.
        std::sort(inComp.begin(), inComp.end(),
                  [](const Anchored& x, const Anchored& y){
                      return x.portalId < y.portalId;
                  });

        // BFS from the first portal's anchor to learn distances of all others.
        std::unordered_map<int,int> distFromFirst, predFromFirst;
        BfsTopology(inComp[0].pieceId, pieces, pieceIdxOf,
                    distFromFirst, predFromFirst);

        // Order the remaining portals by chain distance, tiebreaker = portalId.
        struct Ordered { int portalId; int pieceId; int dist; };
        std::vector<Ordered> ordered;
        ordered.push_back({inComp[0].portalId, inComp[0].pieceId, 0});
        for (size_t j = 1; j < inComp.size(); ++j) {
            auto it = distFromFirst.find(inComp[j].pieceId);
            if (it == distFromFirst.end()) continue; // shouldn't happen
            ordered.push_back({inComp[j].portalId, inComp[j].pieceId, it->second});
        }
        std::sort(ordered.begin()+1, ordered.end(),
                  [](const Ordered& x, const Ordered& y){
                      if (x.dist != y.dist) return x.dist < y.dist;
                      return x.portalId < y.portalId;
                  });

        // Pair (0,1), (2,3), ... and compute the covered path for each pair.
        // We need a fresh BFS from each pair's A anchor to reconstruct the
        // path to B.
        for (size_t j = 0; j + 1 < ordered.size(); j += 2) {
            const auto& A = ordered[j];
            const auto& B = ordered[j+1];
            std::unordered_map<int,int> dA, pA;
            BfsTopology(A.pieceId, pieces, pieceIdxOf, dA, pA);
            std::vector<int> path = ReconstructPath(A.pieceId, B.pieceId, pA);
            if (path.empty()) continue;
            TunnelPair tp;
            tp.portalIdA       = A.portalId;
            tp.portalIdB       = B.portalId;
            tp.anchorPieceA    = A.pieceId;
            tp.anchorPieceB    = B.pieceId;
            tp.coveredPieceIds = std::move(path);
            result.push_back(std::move(tp));
        }
    }

    return result;
}

// ── Flatten covered piece ids ─────────────────────────────────────────────────
std::vector<int> CoveredPieceIds(const std::vector<TunnelPair>& pairs) {
    std::unordered_set<int> seen;
    std::vector<int> out;
    for (const auto& p : pairs)
        for (int id : p.coveredPieceIds)
            if (seen.insert(id).second) out.push_back(id);
    std::sort(out.begin(), out.end());
    return out;
}

} // namespace MC
