using System.Diagnostics;
using OpenCvSharp;
using Serilog;

namespace Poe2Trade.Navigation;

public class WorldMap : IDisposable
{
    private readonly MinimapConfig _config;
    private Mat _canvas;
    private Mat _confidence; // CV_16SC1: per-pixel wall confidence counter
    private int _canvasSize;
    private MapPosition _position;
    private int _frameCount;
    private int _consecutiveMatchFails;
    private Mat? _prevWallMask; // for frame deduplication
    private readonly PathFinder _pathFinder = new();

    // Checkpoint tracking (canvas coordinates)
    private readonly List<(Point Pos, long LastSeenMs)> _checkpointsOff = [];
    private readonly List<(Point Pos, long LastSeenMs)> _checkpointsOn = [];
    private const int CheckpointDedupRadius = 20;

    public MapPosition Position => _position;
    public bool LastMatchSucceeded { get; private set; }
    public int CanvasSize => _canvasSize;
    internal List<Point>? LastBfsPath => _pathFinder.LastResult?.Path;

    private const int GrowMargin = 500;
    private const int GrowAmount = 2000; // 1000px added per side

    public WorldMap(MinimapConfig config)
    {
        _config = config;
        _canvasSize = config.CanvasSize;
        _canvas = new Mat(_canvasSize, _canvasSize, MatType.CV_8UC1, Scalar.Black);
        _confidence = new Mat(_canvasSize, _canvasSize, MatType.CV_16SC1, Scalar.Black);
        _position = new MapPosition(_canvasSize / 2.0, _canvasSize / 2.0);
    }

    private void EnsureCapacity()
    {
        var x = (int)Math.Round(_position.X);
        var y = (int)Math.Round(_position.Y);
        if (x >= GrowMargin && y >= GrowMargin &&
            x < _canvasSize - GrowMargin && y < _canvasSize - GrowMargin)
            return;

        var oldSize = _canvasSize;
        var newSize = oldSize + GrowAmount;
        var offset = GrowAmount / 2;

        var newCanvas = new Mat(newSize, newSize, MatType.CV_8UC1, Scalar.Black);
        var newConf = new Mat(newSize, newSize, MatType.CV_16SC1, Scalar.Black);
        using (var dst = new Mat(newCanvas, new Rect(offset, offset, oldSize, oldSize)))
            _canvas.CopyTo(dst);
        using (var dst = new Mat(newConf, new Rect(offset, offset, oldSize, oldSize)))
            _confidence.CopyTo(dst);

        _canvas.Dispose();
        _confidence.Dispose();
        _canvas = newCanvas;
        _confidence = newConf;
        _canvasSize = newSize;
        _position = new MapPosition(_position.X + offset, _position.Y + offset);
        Log.Information("Canvas grown: {Old}→{New}, offset={Offset}", oldSize, newSize, offset);
    }

    /// <summary>
    /// Match current wall mask against the accumulated map to find position,
    /// then stitch walls and paint explored area.
    /// </summary>
    public MapPosition MatchAndStitch(Mat classifiedMat, Mat wallMask, MinimapMode mode = MinimapMode.Overlay,
        List<Point>? checkpointsOff = null, List<Point>? checkpointsOn = null)
    {
        EnsureCapacity();
        var sw = Stopwatch.StartNew();
        _frameCount++;

        var isCorner = mode == MinimapMode.Corner;
        var warmupFrames = isCorner ? 2 : _config.WarmupFrames;
        var needsBootstrap = _frameCount <= warmupFrames || _consecutiveMatchFails >= 30;

        Log.Debug("MatchAndStitch: frame#{N} mode={Mode} frameSize={W}x{H} pos=({X:F1},{Y:F1}) bootstrap={Boot} prevWallMask={HasPrev}",
            _frameCount, mode, wallMask.Width, wallMask.Height,
            _position.X, _position.Y, needsBootstrap, _prevWallMask != null);

        var wallCountBefore = Cv2.CountNonZero(wallMask);

        if (ShouldSkipFrame(classifiedMat, wallMask, isCorner, needsBootstrap, sw))
            return _position;

        var wallCountAfter = Cv2.CountNonZero(wallMask);
        var dedupMs = sw.Elapsed.TotalMilliseconds;

        // Store current wall mask for next frame's dedup check
        _prevWallMask?.Dispose();
        _prevWallMask = wallMask.Clone();

        // Warmup / re-bootstrap: stitch at current position to seed the canvas
        if (needsBootstrap)
        {
            // Don't consume warmup slots on empty frames (game still loading minimap)
            if (wallCountAfter < 50)
            {
                _frameCount--;
                Log.Information("Warmup waiting for minimap ({Ms:F1}ms)", sw.Elapsed.TotalMilliseconds);
                return _position;
            }

            StitchWithConfidence(classifiedMat, _position, boosted: true, mode: mode);
            PaintExploredCircle(_position);
            MergeCheckpoints(_position, classifiedMat.Width, checkpointsOff, checkpointsOn);
            LastMatchSucceeded = true; // signal caller to update viewport
            if (_consecutiveMatchFails >= 30)
            {
                Log.Information("Re-bootstrap: mode={Mode} pos=({X:F1},{Y:F1}) frameSize={FS} walls={W} stitch={Ms:F1}ms",
                    mode, _position.X, _position.Y, classifiedMat.Width, wallCountAfter, sw.Elapsed.TotalMilliseconds);
                _consecutiveMatchFails = 0;
            }
            else
            {
                Log.Information("Warmup frame {N}/{Total}: walls={WallsBefore}→{WallsAfter}(filtered) frameSize={FS} stitch={Ms:F1}ms",
                    _frameCount, _config.WarmupFrames, wallCountBefore, wallCountAfter,
                    wallMask.Width, sw.Elapsed.TotalMilliseconds);
            }
            return _position;
        }

        // Match wallMask against canvas to find best position
        var matchStart = sw.Elapsed.TotalMilliseconds;
        var matched = MatchPosition(wallMask, _position);
        var matchMs = sw.Elapsed.TotalMilliseconds - matchStart;

        if (matched == null)
        {
            _consecutiveMatchFails++;
            LastMatchSucceeded = false;
            Log.Information("MatchAndStitch: dedup={Dedup:F1}ms match={Match:F1}ms (FAILED x{Fails}) total={Total:F1}ms",
                dedupMs, matchMs, _consecutiveMatchFails, sw.Elapsed.TotalMilliseconds);
            return _position; // don't stitch — wrong position would corrupt the canvas
        }

        _consecutiveMatchFails = 0;
        LastMatchSucceeded = true;
        var prevPos = _position;
        _position = matched;
        var stitchStart = sw.Elapsed.TotalMilliseconds;
        StitchWithConfidence(classifiedMat, _position, boosted: false, mode: mode);
        PaintExploredCircle(_position);
        MergeCheckpoints(_position, classifiedMat.Width, checkpointsOff, checkpointsOn);
        var stitchMs = sw.Elapsed.TotalMilliseconds - stitchStart;

        var posDx = _position.X - prevPos.X;
        var posDy = _position.Y - prevPos.Y;
        Log.Debug("MatchAndStitch: mode={Mode} pos=({X:F1},{Y:F1}) moved=({Dx:F1},{Dy:F1}) dedup={Dedup:F1}ms match={Match:F1}ms stitch={Stitch:F1}ms total={Total:F1}ms",
            mode, _position.X, _position.Y, posDx, posDy, dedupMs, matchMs, stitchMs, sw.Elapsed.TotalMilliseconds);
        return _position;
    }

    /// <summary>
    /// Noise filter + frame deduplication. Returns true if the frame should be skipped.
    /// </summary>
    private bool ShouldSkipFrame(Mat classifiedMat, Mat wallMask, bool isCorner, bool needsBootstrap, Stopwatch sw)
    {
        // Block-based noise filter: only needed for overlay (game effects bleed through)
        // Skip during warmup — we need walls to seed the canvas, confidence handles noise
        if (!isCorner && !needsBootstrap)
        {
            var cleanFraction = FilterNoisyBlocks(wallMask, classifiedMat);
            if (cleanFraction < 0.25)
            {
                Log.Information("Noise filter: {Clean:P0} clean, skipping ({Ms:F1}ms)",
                    cleanFraction, sw.Elapsed.TotalMilliseconds);
                return true;
            }
        }

        // Frame deduplication: skip if minimap hasn't scrolled yet (but always allow warmup through)
        if (!needsBootstrap && _prevWallMask != null && _frameCount > 1)
        {
            using var xor = new Mat();
            Cv2.BitwiseXor(wallMask, _prevWallMask, xor);
            var changedPixels = Cv2.CountNonZero(xor);
            if (changedPixels < _config.FrameChangeThreshold)
            {
                Log.Debug("Frame dedup: {Changed} changed pixels, skipping ({Ms:F1}ms)",
                    changedPixels, sw.Elapsed.TotalMilliseconds);
                return true;
            }
        }

        return false;
    }

    private MapPosition? MatchPosition(Mat wallMask, MapPosition estimate)
    {
        var frameSize = wallMask.Width;
        var searchPad = _config.MatchSearchRadius;
        var searchSize = frameSize + 2 * searchPad;

        var cx = (int)Math.Round(estimate.X);
        var cy = (int)Math.Round(estimate.Y);

        // Search region on canvas (centered on estimate)
        var sx = cx - searchSize / 2;
        var sy = cy - searchSize / 2;

        // Clamp to canvas bounds
        var sx0 = Math.Max(0, sx);
        var sy0 = Math.Max(0, sy);
        var sx1 = Math.Min(_canvasSize, sx + searchSize);
        var sy1 = Math.Min(_canvasSize, sy + searchSize);
        var sw = sx1 - sx0;
        var sh = sy1 - sy0;

        // Search region must be larger than template
        if (sw <= frameSize || sh <= frameSize)
            return null;

        // Extract search ROI and convert to binary wall mask
        using var searchRoi = new Mat(_canvas, new Rect(sx0, sy0, sw, sh));
        using var canvasWalls = new Mat();
        Cv2.Compare(searchRoi, new Scalar((byte)MapCell.Wall), canvasWalls, CmpType.EQ);

        // Check if canvas has enough walls to match against
        var canvasWallCount = Cv2.CountNonZero(canvasWalls);
        var frameWallCount = Cv2.CountNonZero(wallMask);
        if (frameWallCount < 50)
        {
            Log.Information("Match fail: too few frame walls ({FrameWalls}) frameSize={FS}",
                frameWallCount, frameSize);
            return null;
        }

        if (canvasWallCount < 50)
        {
            Log.Information("Match fail: too few canvas walls ({CanvasWalls}) frame walls={FrameWalls} frameSize={FS} searchROI={SW}x{SH}",
                canvasWallCount, frameWallCount, frameSize, sw, sh);
            return null;
        }

        // Template match: find where frame's walls best align with canvas walls
        using var result = new Mat();
        Cv2.MatchTemplate(canvasWalls, wallMask, result, TemplateMatchModes.CCoeffNormed);
        Cv2.MinMaxLoc(result, out _, out var maxVal, out _, out var maxLoc);

        if (maxVal < _config.MatchConfidence)
        {
            Log.Information("Match fail: low confidence {Conf:F3} (need {Min:F2}) frameSize={FS} searchROI={SW}x{SH} canvas={CanvasWalls} frame={FrameWalls}",
                maxVal, _config.MatchConfidence, frameSize, sw, sh, canvasWallCount, frameWallCount);
            return null;
        }

        // Convert match position to world coordinates
        // maxLoc is the top-left of the template in the search ROI
        var matchX = sx0 + maxLoc.X + frameSize / 2.0;
        var matchY = sy0 + maxLoc.Y + frameSize / 2.0;

        var deltaX = matchX - estimate.X;
        var deltaY = matchY - estimate.Y;
        Log.Information("Match OK: conf={Conf:F3} pos=({X:F1},{Y:F1}) delta=({Dx:F1},{Dy:F1}) frameSize={FS} searchROI={SW}x{SH} canvas={CanvasWalls} frame={FrameWalls}",
            maxVal, matchX, matchY, deltaX, deltaY, frameSize, sw, sh, canvasWallCount, frameWallCount);

        return new MapPosition(matchX, matchY);
    }

    private void StitchWithConfidence(Mat classifiedMat, MapPosition position, bool boosted,
        MinimapMode mode = MinimapMode.Overlay)
    {
        var isCorner = mode == MinimapMode.Corner;
        var frameSize = classifiedMat.Width;
        var halfSize = frameSize / 2;
        var canvasX = (int)Math.Round(position.X) - halfSize;
        var canvasY = (int)Math.Round(position.Y) - halfSize;

        // Clamp to canvas bounds
        var srcX = Math.Max(0, -canvasX);
        var srcY = Math.Max(0, -canvasY);
        var dstX = Math.Max(0, canvasX);
        var dstY = Math.Max(0, canvasY);
        var w = Math.Min(frameSize - srcX, _canvasSize - dstX);
        var h = Math.Min(frameSize - srcY, _canvasSize - dstY);

        if (w <= 0 || h <= 0) return;

        var srcRect = new Rect(srcX, srcY, w, h);
        var dstRect = new Rect(dstX, dstY, w, h);

        var srcRoi = new Mat(classifiedMat, srcRect);
        var dstRoi = new Mat(_canvas, dstRect);
        var confRoi = new Mat(_confidence, dstRect);

        // Corner minimap is clean — trust walls immediately, lower threshold
        var confInc = isCorner ? (short)_config.ConfidenceMax : (short)_config.ConfidenceInc;
        var confDec = (short)_config.ConfidenceDec;
        var confThreshold = isCorner ? (short)2 : (short)_config.ConfidenceThreshold;
        var confMax = (short)_config.ConfidenceMax;

        for (var row = 0; row < h; row++)
        for (var col = 0; col < w; col++)
        {
            var srcVal = srcRoi.At<byte>(row, col);
            var conf = confRoi.At<short>(row, col);

            if (srcVal == (byte)MapCell.Wall)
            {
                conf = boosted
                    ? confMax
                    : Math.Min((short)(conf + confInc), confMax);
            }
            else if (conf > 0)
            {
                conf = Math.Max((short)(conf - confDec), (short)0);
            }
            else
            {
                continue;
            }

            confRoi.Set(row, col, conf);

            var current = dstRoi.At<byte>(row, col);
            // Corner mode: no double-evidence needed (clean data)
            var needed = !isCorner && current == (byte)MapCell.Explored
                ? (short)(confThreshold * 2)
                : confThreshold;

            if (conf >= needed)
                dstRoi.Set(row, col, (byte)MapCell.Wall);
            else if (current == (byte)MapCell.Wall && conf < confThreshold)
                dstRoi.Set(row, col, (byte)MapCell.Explored);
        }

        // Mark fog on canvas (on top of Unknown or Explored — not Wall)
        if (isCorner)
        {
            for (var row = 0; row < h; row++)
            for (var col = 0; col < w; col++)
            {
                if (srcRoi.At<byte>(row, col) != (byte)MapCell.Fog) continue;
                var dst = dstRoi.At<byte>(row, col);
                if (dst == (byte)MapCell.Unknown || dst == (byte)MapCell.Explored)
                    dstRoi.Set(row, col, (byte)MapCell.Fog);
            }
        }
        else
        {
            // Overlay: exclude small area near player center (spell effect noise)
            const int fogInner = 30;
            const int fogInner2 = fogInner * fogInner;

            for (var row = 0; row < h; row++)
            for (var col = 0; col < w; col++)
            {
                if (srcRoi.At<byte>(row, col) != (byte)MapCell.Fog) continue;

                var fx = srcX + col - halfSize;
                var fy = srcY + row - halfSize;
                if (fx * fx + fy * fy < fogInner2) continue;

                var dst = dstRoi.At<byte>(row, col);
                if (dst == (byte)MapCell.Unknown || dst == (byte)MapCell.Explored)
                    dstRoi.Set(row, col, (byte)MapCell.Fog);
            }
        }

    }

    /// <summary>
    /// Mark explored area: circle around player position.
    /// Unknown cells are marked Explored within the full radius.
    /// Fog cells are only cleared in the inner portion (fogClearRadius),
    /// preserving fog at the edge so it stays visible on the viewport.
    /// </summary>
    private void PaintExploredCircle(MapPosition position)
    {
        var pcx = (int)Math.Round(position.X);
        var pcy = (int)Math.Round(position.Y);
        var r = _config.ExploredRadius;
        var r2 = r * r;
        var fogClear = r - 20;
        var fogClear2 = fogClear * fogClear;

        var y0 = Math.Max(0, pcy - r);
        var y1 = Math.Min(_canvasSize - 1, pcy + r);
        var x0 = Math.Max(0, pcx - r);
        var x1 = Math.Min(_canvasSize - 1, pcx + r);

        for (var y = y0; y <= y1; y++)
        for (var x = x0; x <= x1; x++)
        {
            var dx = x - pcx;
            var dy = y - pcy;
            var d2 = dx * dx + dy * dy;
            if (d2 > r2) continue;
            var cell = _canvas.At<byte>(y, x);
            if (cell == (byte)MapCell.Unknown)
                _canvas.Set(y, x, (byte)MapCell.Explored);
            else if (cell == (byte)MapCell.Fog && d2 <= fogClear2)
                _canvas.Set(y, x, (byte)MapCell.Explored);
        }
    }

    /// <summary>
    /// Zero out 50×50 blocks where wall density exceeds 25%.
    /// Modifies wallMask and classifiedMat in-place.
    /// Returns fraction of blocks that are clean (0.0–1.0).
    /// </summary>
    private static double FilterNoisyBlocks(Mat wallMask, Mat classifiedMat,
        int blockSize = 50, double blockMaxDensity = 0.25)
    {
        var rows = wallMask.Rows;
        var cols = wallMask.Cols;
        var totalBlocks = 0;
        var cleanBlocks = 0;

        for (var by = 0; by < rows; by += blockSize)
        for (var bx = 0; bx < cols; bx += blockSize)
        {
            var bw = Math.Min(blockSize, cols - bx);
            var bh = Math.Min(blockSize, rows - by);
            totalBlocks++;

            var blockRect = new Rect(bx, by, bw, bh);
            using var blockRoi = new Mat(wallMask, blockRect);
            var wallCount = Cv2.CountNonZero(blockRoi);

            if ((double)wallCount / (bw * bh) > blockMaxDensity)
            {
                // Zero out this noisy block in both mats
                blockRoi.SetTo(Scalar.Black);
                using var classBlock = new Mat(classifiedMat, blockRect);
                classBlock.SetTo(Scalar.Black);
            }
            else
            {
                cleanBlocks++;
            }
        }

        return totalBlocks > 0 ? (double)cleanBlocks / totalBlocks : 1.0;
    }

    public (double dirX, double dirY)? FindNearestUnexplored(MapPosition pos, int searchRadius = 400)
        => _pathFinder.FindNearestUnexplored(_canvas, _canvasSize, pos, searchRadius);

    public (double dirX, double dirY)? FindPathToTarget(MapPosition pos, Point target, int searchRadius = 400)
        => _pathFinder.FindPathToTarget(_canvas, _canvasSize, pos, target, searchRadius);

    public byte[] GetMapSnapshot()
    {
        Cv2.ImEncode(".png", _canvas, out var buf);
        return buf;
    }

    public byte[] GetViewportSnapshot(MapPosition center, int viewSize = 400)
    {
        var cx = (int)Math.Round(center.X);
        var cy = (int)Math.Round(center.Y);
        var half = viewSize / 2;

        var x0 = Math.Clamp(cx - half, 0, _canvasSize - viewSize);
        var y0 = Math.Clamp(cy - half, 0, _canvasSize - viewSize);
        var roi = new Mat(_canvas, new Rect(x0, y0, viewSize, viewSize));

        using var colored = new Mat(viewSize, viewSize, MatType.CV_8UC3, new Scalar(23, 17, 13));
        for (var r = 0; r < viewSize; r++)
        for (var c = 0; c < viewSize; c++)
        {
            var v = roi.At<byte>(r, c);
            if (v == (byte)MapCell.Explored)
                colored.Set(r, c, new Vec3b(104, 64, 31));
            else if (v == (byte)MapCell.Wall)
                colored.Set(r, c, new Vec3b(26, 45, 61));
            else if (v == (byte)MapCell.Fog)
                colored.Set(r, c, new Vec3b(120, 70, 40));
        }

        // BFS overlay: frontier cells + direction line
        var bfs = _pathFinder.LastResult;
        if (bfs != null)
        {
            // Other frontier directions (dim cyan)
            foreach (var pt in bfs.OtherFrontier)
            {
                var vx = pt.X - x0;
                var vy = pt.Y - y0;
                if (vx >= 0 && vx < viewSize && vy >= 0 && vy < viewSize)
                    colored.Set(vy, vx, new Vec3b(100, 80, 0));
            }

            // Best frontier direction (bright green)
            foreach (var pt in bfs.BestFrontier)
            {
                var vx = pt.X - x0;
                var vy = pt.Y - y0;
                if (vx >= 0 && vx < viewSize && vy >= 0 && vy < viewSize)
                    colored.Set(vy, vx, new Vec3b(0, 220, 0));
            }

            // Direction line from player
            var px2 = bfs.PlayerCx - x0;
            var py2 = bfs.PlayerCy - y0;
            var lineLen = 60;
            var ex = (int)(px2 + bfs.DirX * lineLen);
            var ey = (int)(py2 + bfs.DirY * lineLen);
            Cv2.ArrowedLine(colored, new Point(px2, py2), new Point(ex, ey),
                new Scalar(0, 220, 0), 2, tipLength: 0.3);

            // BFS path polyline (bright green)
            if (bfs.Path.Count >= 2)
            {
                var viewPath = new Point[bfs.Path.Count];
                for (var i = 0; i < bfs.Path.Count; i++)
                    viewPath[i] = new Point(bfs.Path[i].X - x0, bfs.Path[i].Y - y0);
                Cv2.Polylines(colored, [viewPath], isClosed: false,
                    color: new Scalar(0, 255, 0), thickness: 1);
            }
        }

        // Checkpoint markers
        foreach (var (cp, _) in _checkpointsOff)
        {
            var cpx = cp.X - x0;
            var cpy = cp.Y - y0;
            if (cpx >= 0 && cpx < viewSize && cpy >= 0 && cpy < viewSize)
                Cv2.Circle(colored, new Point(cpx, cpy), 5, new Scalar(100, 100, 100), -1); // gray
        }
        foreach (var (cp, _) in _checkpointsOn)
        {
            var cpx = cp.X - x0;
            var cpy = cp.Y - y0;
            if (cpx >= 0 && cpx < viewSize && cpy >= 0 && cpy < viewSize)
                Cv2.Circle(colored, new Point(cpx, cpy), 5, new Scalar(220, 200, 0), -1); // bright cyan (BGR)
        }

        // Player dot (orange, on top)
        var px = cx - x0;
        var py = cy - y0;
        if (px >= 0 && px < viewSize && py >= 0 && py < viewSize)
            Cv2.Circle(colored, new Point(px, py), 4, new Scalar(0, 140, 255), -1);

        Cv2.ImEncode(".png", colored, out var buf);
        return buf;
    }

    /// <summary>
    /// Convert frame-relative checkpoint positions to canvas coords and merge with existing lists.
    /// When a checkpoint-on appears near a checkpoint-off, remove the off entry.
    /// </summary>
    private void MergeCheckpoints(MapPosition position, int frameSize,
        List<Point>? offPoints, List<Point>? onPoints)
    {
        var halfSize = frameSize / 2;
        var canvasX = (int)Math.Round(position.X) - halfSize;
        var canvasY = (int)Math.Round(position.Y) - halfSize;
        var now = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
        var r2 = CheckpointDedupRadius * CheckpointDedupRadius;

        if (offPoints != null)
        {
            foreach (var fp in offPoints)
            {
                var cp = new Point(canvasX + fp.X, canvasY + fp.Y);
                if (!IsDuplicate(_checkpointsOff, cp, r2))
                    _checkpointsOff.Add((cp, now));
            }
        }

        if (onPoints != null)
        {
            foreach (var fp in onPoints)
            {
                var cp = new Point(canvasX + fp.X, canvasY + fp.Y);

                // Remove matching off-checkpoint (it got activated)
                for (var i = _checkpointsOff.Count - 1; i >= 0; i--)
                {
                    var dx = _checkpointsOff[i].Pos.X - cp.X;
                    var dy = _checkpointsOff[i].Pos.Y - cp.Y;
                    if (dx * dx + dy * dy <= r2)
                    {
                        _checkpointsOff.RemoveAt(i);
                        break;
                    }
                }

                if (!IsDuplicate(_checkpointsOn, cp, r2))
                    _checkpointsOn.Add((cp, now));
            }
        }
    }

    private static bool IsDuplicate(List<(Point Pos, long LastSeenMs)> list, Point pt, int r2)
    {
        foreach (var (pos, _) in list)
        {
            var dx = pos.X - pt.X;
            var dy = pos.Y - pt.Y;
            if (dx * dx + dy * dy <= r2)
                return true;
        }
        return false;
    }

    /// <summary>
    /// Returns the nearest unactivated checkpoint within maxDist canvas pixels, or null.
    /// </summary>
    public Point? GetNearestCheckpointOff(MapPosition pos, int maxDist = 200)
    {
        var px = (int)Math.Round(pos.X);
        var py = (int)Math.Round(pos.Y);
        var maxDist2 = maxDist * maxDist;

        Point? best = null;
        var bestDist2 = int.MaxValue;
        foreach (var (cp, _) in _checkpointsOff)
        {
            var dx = cp.X - px;
            var dy = cp.Y - py;
            var d2 = dx * dx + dy * dy;
            if (d2 <= maxDist2 && d2 < bestDist2)
            {
                bestDist2 = d2;
                best = cp;
            }
        }
        return best;
    }

    public void Reset()
    {
        _canvas.Dispose();
        _confidence.Dispose();
        _canvasSize = _config.CanvasSize;
        _canvas = new Mat(_canvasSize, _canvasSize, MatType.CV_8UC1, Scalar.Black);
        _confidence = new Mat(_canvasSize, _canvasSize, MatType.CV_16SC1, Scalar.Black);
        _prevWallMask?.Dispose();
        _prevWallMask = null;
        _position = new MapPosition(_canvasSize / 2.0, _canvasSize / 2.0);
        _frameCount = 0;
        _consecutiveMatchFails = 0;
        LastMatchSucceeded = false;
        _checkpointsOff.Clear();
        _checkpointsOn.Clear();
    }

    /// <summary>
    /// Mode switch: halve confidence around current position so old mode's weak walls
    /// decay quickly while strong walls provide matching reference for the new mode.
    /// No re-bootstrap stitch — let the first match find the correct position.
    /// </summary>
    public void Rebootstrap()
    {
        Log.Information("Rebootstrap: frameCount={N} pos=({X:F1},{Y:F1}) matchFails={Fails} prevWallMask={Size}",
            _frameCount, _position.X, _position.Y, _consecutiveMatchFails,
            _prevWallMask != null ? $"{_prevWallMask.Width}x{_prevWallMask.Height}" : "null");

        // Halve confidence around current position — weak walls get demoted,
        // strong walls survive to help the new mode's first match find position
        var halfClear = 250; // slightly larger than largest frame half (200)
        var cx = (int)Math.Round(_position.X);
        var cy = (int)Math.Round(_position.Y);
        var x0 = Math.Max(0, cx - halfClear);
        var y0 = Math.Max(0, cy - halfClear);
        var w = Math.Min(_canvasSize, cx + halfClear) - x0;
        var h = Math.Min(_canvasSize, cy + halfClear) - y0;
        if (w > 0 && h > 0)
        {
            var rect = new Rect(x0, y0, w, h);
            var confThreshold = (short)_config.ConfidenceThreshold;

            using var confRoi = new Mat(_confidence, rect);
            using var canvasRoi = new Mat(_canvas, rect);

            var demoted = 0;
            for (var row = 0; row < h; row++)
            for (var col = 0; col < w; col++)
            {
                var conf = confRoi.At<short>(row, col);
                if (conf <= 0) continue;
                conf = (short)(conf / 2);
                confRoi.Set(row, col, conf);
                if (conf < confThreshold && canvasRoi.At<byte>(row, col) == (byte)MapCell.Wall)
                {
                    canvasRoi.Set(row, col, (byte)MapCell.Explored);
                    demoted++;
                }
            }
            Log.Information("Rebootstrap: halved confidence in {W}x{H} area, demoted {Demoted} weak walls", w, h, demoted);
        }

        _prevWallMask?.Dispose();
        _prevWallMask = null;
        _frameCount = 0; // force re-warmup with new mode's data
    }

    public void Dispose()
    {
        _canvas.Dispose();
        _confidence.Dispose();
        _prevWallMask?.Dispose();
    }
}