using OpenCvSharp;
using Serilog;

namespace Poe2Trade.Navigation;

public class PositionTracker : IDisposable
{
    private readonly MinimapConfig _config;
    private Mat? _prevGray;
    private Mat? _hanningWindow;
    private double _worldX;
    private double _worldY;
    private int _stuckCounter;

    public MapPosition Position => new(_worldX, _worldY);
    public bool IsStuck => _stuckCounter >= _config.StuckFrameCount;

    public PositionTracker(MinimapConfig config)
    {
        _config = config;
        _worldX = config.CanvasSize / 2.0;
        _worldY = config.CanvasSize / 2.0;
    }

    public MapPosition UpdatePosition(Mat currentGray)
    {
        if (_prevGray == null || _hanningWindow == null)
        {
            _prevGray = currentGray.Clone();
            _hanningWindow = new Mat();
            Cv2.CreateHanningWindow(_hanningWindow, currentGray.Size(), MatType.CV_64F);
            return Position;
        }

        // Convert to float64 for phase correlation
        using var prev64 = new Mat();
        using var curr64 = new Mat();
        _prevGray.ConvertTo(prev64, MatType.CV_64F);
        currentGray.ConvertTo(curr64, MatType.CV_64F);

        var shift = Cv2.PhaseCorrelate(prev64, curr64, _hanningWindow, out var confidence);

        if (confidence < _config.ConfidenceThreshold)
        {
            Log.Debug("Phase correlation low confidence: {Confidence:F3}", confidence);
            _stuckCounter++;
            _prevGray.Dispose();
            _prevGray = currentGray.Clone();
            return Position;
        }

        // Negate: minimap scrolls opposite to player movement
        var dx = -shift.X;
        var dy = -shift.Y;
        var displacement = Math.Sqrt(dx * dx + dy * dy);

        if (displacement < _config.StuckThreshold)
        {
            _stuckCounter++;
        }
        else
        {
            _stuckCounter = 0;
            _worldX += dx;
            _worldY += dy;
        }

        Log.Debug("Position: ({X:F1}, {Y:F1}) dx={Dx:F1} dy={Dy:F1} conf={Conf:F3} stuck={Stuck}",
            _worldX, _worldY, dx, dy, confidence, _stuckCounter);

        _prevGray.Dispose();
        _prevGray = currentGray.Clone();
        return Position;
    }

    public void Reset()
    {
        _prevGray?.Dispose();
        _prevGray = null;
        _hanningWindow?.Dispose();
        _hanningWindow = null;
        _worldX = _config.CanvasSize / 2.0;
        _worldY = _config.CanvasSize / 2.0;
        _stuckCounter = 0;
        Log.Information("Position tracker reset");
    }

    public void Dispose()
    {
        _prevGray?.Dispose();
        _hanningWindow?.Dispose();
    }
}