#include "geometry/SnapEngine.h"
#include "io/CatalogLoader.h"
#include <cmath>
#include <limits>

namespace MC {

// ── Helpers ───────────────────────────────────────────────────────────────────
bool SnapEngine::AreCompatible(const ConnectionPoint& a,
                                const ConnectionPoint& b,
                                double snapRadiusMm)
{
    // 1. Must be spatially close
    if (a.pos.distanceTo(b.pos) > snapRadiusMm)
        return false;

    // 2. Must be facing each other (headings ~180° apart)
    if (!ConnectionPoint::headingsCompatible(a.heading, b.heading, 2.0))
        return false;

    // 3. Both must be open
    return a.isOpen && b.isOpen;
}

std::vector<ConnectionPoint> SnapEngine::WorldConnections(
    const PlacedPiece& placed, const TrackPiece& def)
{
    std::vector<ConnectionPoint> result;
    result.reserve(def.connections.size());
    for (int i = 0; i < (int)def.connections.size(); ++i)
        result.push_back(placed.worldConnection(def, i));
    return result;
}

// ── ComputeSnapTransform ──────────────────────────────────────────────────────
//
// We want to place 'piece' such that its connection[sourceConnIdx] exactly
// meets 'targetWorldConn'.
//
// Strategy:
//   1. The target connection has world pos T and heading H_t.
//   2. The source connection, in piece-local space, is at pos S and heading H_s.
//   3. For the two to join, the source must arrive at T facing OPPOSITE to H_t,
//      i.e. the source exit heading must equal H_t + 180°.
//   4. This means the piece must be rotated by:
//        pieceRotation = (H_t + 180°) - H_s
//   5. The piece origin in world space:
//        origin = T - Rotate(S, pieceRotation)
//
Transform2D SnapEngine::ComputeSnapTransform(
    const TrackPiece&      piece,
    int                    sourceConnIdx,
    const ConnectionPoint& targetWorldConn)
{
    if (sourceConnIdx < 0 || sourceConnIdx >= (int)piece.connections.size())
        return {};

    const ConnectionPoint& srcLocal = piece.connections[sourceConnIdx];

    // Required piece rotation
    double requiredHeading =
        Transform2D::normalizeAngle(targetWorldConn.heading + 180.0);
    double pieceRotation = Transform2D::normalizeAngle(
        requiredHeading - srcLocal.heading);

    // Piece origin in world space
    Vec2 rotatedSrc = srcLocal.pos.rotated(pieceRotation);
    Vec2 origin     = targetWorldConn.pos - rotatedSrc;

    return Transform2D{ origin, pieceRotation };
}

// ── FindSnap ──────────────────────────────────────────────────────────────────
SnapResult SnapEngine::FindSnap(
    const TrackPiece&               dragged,
    const Transform2D&              draggedTransform,
    const std::vector<PlacedPiece>& canvas,
    const CatalogLoader&            catalog)
{
    SnapResult best;
    double     bestDist = std::numeric_limits<double>::max();

    // Build world-space connections for the dragged piece
    // We create a temporary PlacedPiece just to reuse worldConnection()
    PlacedPiece temp;
    temp.transform = draggedTransform;
    temp.joints.resize(dragged.connections.size());
    auto draggedWorldConns = WorldConnections(temp, dragged);

    // Iterate all canvas pieces
    for (const auto& canvasPiece : canvas) {
        const TrackPiece* canvasDef = catalog.Find(canvasPiece.catalogNumber);
        if (!canvasDef) continue;

        auto canvasWorldConns = WorldConnections(canvasPiece, *canvasDef);

        // Try all pairs of open connections
        for (int ci = 0; ci < (int)canvasWorldConns.size(); ++ci) {
            if (!canvasWorldConns[ci].isOpen) continue;

            for (int di = 0; di < (int)draggedWorldConns.size(); ++di) {
                if (!draggedWorldConns[di].isOpen) continue;

                // Per-connection series override (adapter tracks like 24922)
                TrackSeries dragConnSeries = (di < (int)dragged.connSeries.size())
                    ? dragged.connSeries[di] : dragged.series;
                TrackSeries canvConnSeries = (ci < (int)canvasDef->connSeries.size())
                    ? canvasDef->connSeries[ci] : canvasDef->series;

                // Series must be compatible both ways
                if (!SeriesCompatible(dragConnSeries, canvConnSeries))
                    continue;

                double dist = canvasWorldConns[ci].pos.distanceTo(
                              draggedWorldConns[di].pos);

                if (dist > kSnapRadiusMm) continue;
                // Reject only true U-turns — connections pointing the same way
                // (headings within ~20° of each other). Everything else is valid.
                double h1 = canvasWorldConns[ci].heading;
                double h2 = draggedWorldConns[di].heading;
                double hdiff = std::fmod(std::abs(h1 - h2), 360.0);
                if (hdiff > 180.0) hdiff = 360.0 - hdiff;
                if (hdiff < 20.0) continue; // same direction — genuine U-turn, skip

                if (dist < bestDist) {
                    bestDist = dist;
                    best.found           = true;
                    best.targetPieceId   = canvasPiece.id;
                    best.targetConnIdx   = ci;
                    best.sourceConnIdx   = di;
                    best.distanceMm      = dist;
                    best.snappedTransform = ComputeSnapTransform(
                        dragged, di, canvasWorldConns[ci]);
                }
            }
        }
    }

    return best;
}

SnapEngine::IncompatibleSnapResult SnapEngine::FindIncompatibleSnap(
    const TrackPiece&               dragged,
    const Transform2D&              draggedTransform,
    const std::vector<PlacedPiece>& canvas,
    const CatalogLoader&            catalog)
{
    PlacedPiece temp;
    temp.transform = draggedTransform;
    temp.joints.resize(dragged.connections.size());
    auto draggedWorldConns = WorldConnections(temp, dragged);

    for (const auto& canvasPiece : canvas) {
        const TrackPiece* canvasDef = catalog.Find(canvasPiece.catalogNumber);
        if (!canvasDef) continue;

        auto canvasWorldConns = WorldConnections(canvasPiece, *canvasDef);

        for (int ci = 0; ci < (int)canvasWorldConns.size(); ++ci) {
            if (!canvasWorldConns[ci].isOpen) continue;
            for (int di = 0; di < (int)draggedWorldConns.size(); ++di) {
                if (!draggedWorldConns[di].isOpen) continue;

                double dist = canvasWorldConns[ci].pos.distanceTo(
                              draggedWorldConns[di].pos);
                if (dist > kSnapRadiusMm) continue;

                double h1 = canvasWorldConns[ci].heading;
                double h2 = draggedWorldConns[di].heading;
                double hdiff = std::fmod(std::abs(h1 - h2), 360.0);
                if (hdiff > 180.0) hdiff = 360.0 - hdiff;
                if (hdiff < 20.0) continue;

                // Geometrically compatible — now check series
                TrackSeries dragConnSeries = (di < (int)dragged.connSeries.size())
                    ? dragged.connSeries[di] : dragged.series;
                TrackSeries canvConnSeries = (ci < (int)canvasDef->connSeries.size())
                    ? canvasDef->connSeries[ci] : canvasDef->series;

                if (!SeriesCompatible(dragConnSeries, canvConnSeries)) {
                    return { true, dragConnSeries, canvConnSeries };
                }
            }
        }
    }
    return {};
}

} // namespace MC
