using Microsoft.JSInterop;
using RobotNet10.RobotApp.Client.Clients;
using RobotNet10.RobotApp.Client.Shared.SLAM;
using RobotNet10.RobotApp.Shared.Enums;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Numbers;
using QuaternionGeometry = RobotNet10.Shared.Geometry.Quaternion;
using QuaternionNumbers = RobotNet10.Shared.Numbers.Quaternion;

namespace RobotNet10.RobotApp.Client.Components.SLAM;

public partial class MapLocalization
{
    private const int GridPollIntervalMs = 3000;
    private const int PoseLaserPollIntervalMs = 500;
    private const int GridRequestMaxRetries = 10;
    private const int GridRequestRetryDelayMs = 500;
    private const double MinFitScale = 0.5;

    private IJSObjectReference _jsModule = null!;
    private DotNetObjectReference<MapLocalization> _dotNetObj = null!;

    // Container rect state
    private double _containerRectX = 0.0;
    private double _containerRectY = 0.0;
    private double _containerRectWidth = 0.0;
    private double _containerRectHeight = 0.0;
    private double _containerRectTop = 0.0;
    private double _containerRectRight = 0.0;
    private double _containerRectBottom = 0.0;
    private double _containerRectLeft = 0.0;

    // View state
    public double CursorX { get; private set; } = 0.0;
    public double CursorY { get; private set; } = 0.0;
    private double _clientOriginX = 0.0;
    private double _clientOriginY = 0.0;
    private double _scale = 1.0;
    private double _left = 0.0;
    private double _top = 0.0;
    private double _fitScale = 1.0;

    // Map data
    private double _resolution = 1.0;
    private double _originX = 0.0;
    private double _originY = 0.0; // Transformed origin Y (like MapContainer.OriginY)
    private double _mapOriginY = 0.0; // Original origin Y from grid (like MapContainer MapData.OriginY)
    private double _imageWidth = 0.0;
    private double _imageHeight = 0.0;

    // Grid data
    public OccupancyGridDto? CurrentGrid { get; private set; }
    private OccupancyGridDto? _lastDrawnGrid; // Track last drawn grid
    private DateTime _lastGridUpdateTime = DateTime.MinValue;

    // Trajectory path for polyline (ScanMapping only) - world coordinates for SVG viewBox
    private List<Vector2> _trajectoryPath = [];
    
    // SVG viewBox origin (similar to MapContainer.OriginX and OriginY)
    private double _svgOriginX = 0.0; // SVG viewBox X origin (world coordinates)
    private double _svgOriginY = 0.0; // SVG viewBox Y origin (world coordinates)

    // State
    private SLAMState? _currentState;
    /// <summary>True when grid/pose/laser should be requested and displayed (Localizing or ScanMapping).</summary>
    private bool IsMapDisplayActive => _currentState == SLAMState.Relocalizing || _currentState == SLAMState.Localizing || _currentState == SLAMState.ScanMapping;
    /// <summary>Reference grid for view.</summary>
    private OccupancyGridDto? ReferenceGrid => CurrentGrid;
    private System.Timers.Timer? _updateTimer;
    private System.Timers.Timer? _poseLaserTimer; // Timer for pose and laser scan updates (0.5s)
    private readonly Lock _timerLock = new();
    private readonly Lock _poseLaserTimerLock = new();
    private bool _hasRequestedGridForLocalization = false;
    
    // Robot pose and laser scan data
    private PoseDto _currentPose = new()
    {
        Position = new RobotNet10.Shared.Numbers.Vector3 { X = 0, Y = 0, Z = 0 },
        Orientation = new QuaternionGeometry { W = 1, X = 0, Y = 0, Z = 0 },
        Timestamp = DateTime.UtcNow
    };
    private RobotNet10.Shared.Numbers.Vector3[]? _currentLaserScanPoints;

    #region Lifecycle

    protected override async Task OnAfterRenderAsync(bool firstRender)
    {
        await base.OnAfterRenderAsync(firstRender);
        if (!firstRender) return;

        // Load JavaScript module (canvas + view/events)
        _jsModule = await JSRuntime.InvokeAsync<IJSObjectReference>("import", "/js/mapLocalization.js");

        // Setup zoom and pan event listeners
        _dotNetObj = DotNetObjectReference.Create(this);
        await _jsModule.InvokeVoidAsync("updateContainerRect", _dotNetObj, containerRef, nameof(OnContainerResize));
        await _jsModule.InvokeVoidAsync("registerResizeObserver", _dotNetObj, containerRef, nameof(OnContainerResize));
        await _jsModule.InvokeVoidAsync("addMouseWheelEventListener", _dotNetObj, containerRef, nameof(OnMouseWheel));
        await _jsModule.InvokeVoidAsync("addMouseMoveEventListener", _dotNetObj, containerRef, nameof(OnMouseMove));
        await _jsModule.InvokeVoidAsync("addMouseDownEventListener", _dotNetObj, containerRef, nameof(OnMouseDown));
        await _jsModule.InvokeVoidAsync("addMouseUpEventListener", _dotNetObj, containerRef, nameof(OnMouseUp));

        await CartographerClient.StartAsync();

        // Get initial state and trigger OnStateChanged to handle grid loading and timer setup
        var state = await CartographerClient.GetCurrentStateAsync();
        OnStateChanged(state);

        // Subscribe to events
        CartographerClient.StateChanged += OnStateChanged;
    }

    #endregion

    #region State & Grid

    private void OnStateChanged(SLAMState state)
    {
        _currentState = state;
        UpdateTimerBasedOnState();

        // When entering InitializingLocalizing, Localizing or ScanMapping state, request OccupancyGrid once
        if ((state == SLAMState.Relocalizing || state == SLAMState.Localizing || state == SLAMState.ScanMapping) && !_hasRequestedGridForLocalization)
        {
            _hasRequestedGridForLocalization = true;
            _ = InvokeAsync(async () =>
            {
                await RequestOccupancyGridForLocalizationAsync();
                // Start pose/laser scan timer after requesting grid
                UpdatePoseLaserTimerBasedOnState();
            });
        }
        else if (state == SLAMState.Relocalizing || state == SLAMState.Localizing || state == SLAMState.ScanMapping)
        {
            // Already requested grid, only update timer if grid is available
            // This handles state transitions like Relocalizing -> Localizing where grid request is still in progress
            if (CurrentGrid != null)
            {
                UpdatePoseLaserTimerBasedOnState();
            }
            // If CurrentGrid is null, the timer will be started by the InvokeAsync above when grid is loaded
        }
        else
        {
            UpdatePoseLaserTimerBasedOnState();
        }

        // Reset flag when leaving map-display states (InitializingLocalizing, Localizing, ScanMapping)
        if (state != SLAMState.Relocalizing && state != SLAMState.Localizing && state != SLAMState.ScanMapping)
        {
            _hasRequestedGridForLocalization = false;
            _trajectoryPath.Clear();
            _ = InvokeAsync(async () =>
            {
                await ClearLaserScanAsync();
                await UpdateTrajectoryPolylineAsync(); // Clear polyline via JsInvoke
                RobotPoseRef.UpdatePose(0, 0, 0);
                RobotPoseInfoRef.Update(0, 0, 0, 0);
            });
        }
        else if (state == SLAMState.ScanMapping)
        {
            // Entering ScanMapping: clear trajectory until timer fetches nodes
            _trajectoryPath.Clear();
            _ = InvokeAsync(UpdateTrajectoryPolylineAsync);
        }
    }

    #endregion

    #region Grid & Map

    private async Task LoadMapFromGrid(OccupancyGridDto grid)
    {
        CurrentGrid = grid;
        _resolution = grid.Resolution;
        _originX = grid.Origin.Position.X;
        _mapOriginY = grid.Origin.Position.Y; // Original origin Y from grid
        // Similar to MapContainer: OriginY = -ImageHeight * Resolution - mapData.OriginY
        _originY = -grid.Height * grid.Resolution - grid.Origin.Position.Y;
        _imageWidth = grid.Width * grid.Resolution;
        _imageHeight = grid.Height * grid.Resolution;
        
        // Update SVG origin from grid origin
        UpdateSvgOrigin();
        
        // Draw the grid on canvas
        await DrawOccupancyGrid();
    }
    
    private void UpdateSvgOrigin()
    {
        var referenceGrid = ReferenceGrid;
        if (referenceGrid == null)
        {
            _svgOriginX = 0.0;
            _svgOriginY = 0.0;
            return;
        }

        // SVG viewBox origin uses world coordinates from grid origin
        _svgOriginX = referenceGrid.Origin.Position.X;
        _svgOriginY = referenceGrid.Origin.Position.Y;
    }

    /// <summary>
    /// Apply origin and image size from a grid.
    /// </summary>
    private void ApplyOriginFromGrid(OccupancyGridDto grid)
    {
        _resolution = grid.Resolution;
        _originX = grid.Origin.Position.X;
        _mapOriginY = grid.Origin.Position.Y;
        _originY = -grid.Height * grid.Resolution - grid.Origin.Position.Y;
        _imageWidth = grid.Width * grid.Resolution;
        _imageHeight = grid.Height * grid.Resolution;
        UpdateSvgOrigin();
    }

    /// <summary>
    /// Apply trajectory from grid DTO to _trajectoryPath and update polyline if changed.
    /// </summary>
    /// <returns>True if trajectory was updated.</returns>
    private async Task<bool> TryApplyTrajectoryFromGrid(OccupancyGridDto? grid)
    {
        if (grid?.TrajectoryNodes == null || grid.TrajectoryNodes.Length == 0)
            return false;
        var newPath = grid.TrajectoryNodes.Select(n => new Vector2(n.Pose.Position.X, n.Pose.Position.Y)).ToList();
        if (newPath.Count == _trajectoryPath.Count && newPath.SequenceEqual(_trajectoryPath))
            return false;
        _trajectoryPath = newPath;
        await UpdateTrajectoryPolylineAsync();
        return true;
    }

    #endregion

    #region Drawing

    /// <summary>
    /// Create a snapshot of grid for _lastDrawnGrid* tracking.
    /// </summary>
    private static OccupancyGridDto CreateDrawnGridSnapshot(OccupancyGridDto grid)
    {
        return new OccupancyGridDto
        {
            Resolution = grid.Resolution,
            Width = grid.Width,
            Height = grid.Height,
            Origin = grid.Origin,
            Version = grid.Version,
            KnownCells = grid.KnownCells ?? [],
            LastBaseUpdated = grid.LastBaseUpdated,
            LastUpdated = grid.LastUpdated,
            TrajectoryNodes = grid.TrajectoryNodes
        };
    }

    private async Task DrawOccupancyGrid()
    {
        var referenceGrid = ReferenceGrid;
        if (referenceGrid == null) return;

        var grid = CurrentGrid;
        
        // Check if we need to redraw grid
        bool needsRedraw = false;
        if (grid != null && (_lastDrawnGrid == null || _lastDrawnGrid.Version != grid.Version))
        {
            needsRedraw = true;
        }
        
        if (!needsRedraw)
        {
            return;
        }
        
        try
        {
            // Get container size to calculate scale for auto-fit
            await Task.Delay(10);
            var containerSize = await _jsModule.InvokeAsync<double[]>("getElementSize", containerRef);
            var containerWidth = containerSize[0];
            var containerHeight = containerSize[1];
            
            if (containerWidth <= 0 || containerHeight <= 0)
            {
                return;
            }
            
            // Use reference grid for fit scale (base or updating)
            var refW = referenceGrid.Width;
            var refH = referenceGrid.Height;
            if (_imageWidth <= 0 || _imageHeight <= 0)
            {
                _imageWidth = refW * referenceGrid.Resolution;
                _imageHeight = refH * referenceGrid.Resolution;
                _originX = referenceGrid.Origin.Position.X;
                _mapOriginY = referenceGrid.Origin.Position.Y;
                _originY = -referenceGrid.Height * referenceGrid.Resolution - referenceGrid.Origin.Position.Y;
            }
            if (_imageWidth > 0 && _imageHeight > 0)
            {
                _fitScale = Math.Min(containerWidth / _imageWidth, containerHeight / _imageHeight);
            }
            else
            {
                var scaleX = containerWidth / refW;
                var scaleY = containerHeight / refH;
                _fitScale = Math.Min(scaleX, scaleY);
            }
            
            if (_fitScale < MinFitScale)
                _fitScale = MinFitScale;
            if (_scale <= 0)
                _scale = _fitScale;
            
            // Draw grid
            if (needsRedraw && grid != null)
            {
                await _jsModule.InvokeVoidAsync("setCanvasSize", mapCanvasBaseRef, grid.Width, grid.Height);
                if (grid.KnownCells != null && grid.KnownCells.Length > 0)
                {
                    await _jsModule.InvokeVoidAsync("drawOccupancyGrid",
                        mapCanvasBaseRef, grid.Width, grid.Height, grid.KnownCells);
                }
                else
                {
                    await _jsModule.InvokeVoidAsync("clearCanvas", mapCanvasBaseRef);
                }
                _lastDrawnGrid = CreateDrawnGridSnapshot(grid);
            }
            
            var svgWidth = referenceGrid.Width * referenceGrid.Resolution;
            var svgHeight = referenceGrid.Height * referenceGrid.Resolution;
            await _jsModule.InvokeVoidAsync("setSvgConfig", mapContainerRef, svgWidth, svgHeight, _svgOriginX, _svgOriginY);
            
            await ScaleFitContentAsync();
            
            if (IsMapDisplayActive)
            {
                await UpdateRobotPoseSvgAsync();
                if (_currentLaserScanPoints != null && _currentLaserScanPoints.Length > 0)
                    await DrawLaserScanAsync();
                if (_currentState == SLAMState.ScanMapping)
                    await UpdateTrajectoryPolylineAsync();
            }
        }
        catch (Exception ex)
        {
            Console.Error.WriteLine($"[MapLocalization] DrawOccupancyGrid: Exception - {ex.Message}");
        }
    }

    #endregion

    #region View & Scale

    /// <summary>
    /// Public method to fit and center the map view.
    /// Can be called from external components/pages.
    /// </summary>
    public async Task FitViewAsync()
    {
        if (ReferenceGrid == null) return;

        // Recalculate fit scale based on current container and image dimensions
        if (_imageWidth > 0 && _imageHeight > 0 && _containerRectWidth > 0 && _containerRectHeight > 0)
        {
            _fitScale = Math.Min(_containerRectWidth / _imageWidth, _containerRectHeight / _imageHeight);
            if (_fitScale < MinFitScale)
                _fitScale = MinFitScale;
        }

        await ScaleFitContentAsync();
    }

    private void UpdateClientOrigin()
    {
        if (ReferenceGrid == null) return;

        // Calculate client origin (exactly like MapContainer.SetViewMovement and ViewContainerResize)
        // MapContainer: ClientOriginX = ViewContainerRectLeft + Left - OriginX * Scale
        // MapContainer: ClientOriginY = ViewContainerRectTop + Top - OriginY * Scale
        // 
        // MapLocalization now has the same structure as MapContainer:
        // - scaleY(-1) is on SVG (class="map-editor"), not on wrapper div
        // - This matches MapContainer's structure exactly
        _clientOriginX = _containerRectLeft + _left - _originX * _scale;
        _clientOriginY = _containerRectTop + _top - _originY * _scale;
    }

    public async Task ScaleFitContentAsync()
    {
        if (ReferenceGrid == null) return;
        
        _scale = _fitScale;
        
        var wrapperWidth = _imageWidth * _scale;
        var wrapperHeight = _imageHeight * _scale;
        
        var centerLeft = (_containerRectWidth - wrapperWidth) / 2;
        var centerTop = (_containerRectHeight - wrapperHeight) / 2;
        
        await SetViewMovement(centerLeft, centerTop);
        
        await _jsModule.InvokeVoidAsync("setSvgRect", mapContainerRef, wrapperWidth, wrapperHeight);
        await _jsModule.InvokeVoidAsync("setCanvasRect", mapCanvasBaseRef, wrapperWidth, wrapperHeight);
        await _jsModule.InvokeVoidAsync("setCanvasRect", laserScanCanvasRef, wrapperWidth, wrapperHeight);
    }

    private async Task SetViewMovement(double left, double top)
    {
        _top = top;
        _left = left;
        
        // Update client origin (exactly like MapContainer.SetViewMovement)
        UpdateClientOrigin();
        
        if (_jsModule != null && ReferenceGrid != null)
        {
            var width = _imageWidth * _scale;
            var height = _imageHeight * _scale;
            await _jsModule.InvokeVoidAsync("setMapMovement", viewMovementRef, _top, _left, width, height);
        }
    }

    #endregion

    #region Event Handlers (JSInvokable)

    [JSInvokable]
    public void OnContainerResize(double x, double y, double width, double height, double top, double right, double bottom, double left)
    {
        _containerRectX = x;
        _containerRectY = y;
        _containerRectWidth = width;
        _containerRectHeight = height;
        _containerRectTop = top;
        _containerRectRight = right;
        _containerRectBottom = bottom;
        _containerRectLeft = left;

        // Update client origin (exactly like MapContainer.ViewContainerResize)
        UpdateClientOrigin();

        // Recalculate fit scale (exactly like MapContainer.ViewContainerResize)
        // MapContainer: FitScale = Math.Min(ViewContainerRectWidth / ImageWidth, ViewContainerRectHeight / ImageHeight)
        if (_imageWidth > 0 && _imageHeight > 0)
        {
            _fitScale = Math.Min(_containerRectWidth / _imageWidth, _containerRectHeight / _imageHeight);
        }
    }

    [JSInvokable]
    public async Task OnMouseWheel(double deltaY, double clientX, double clientY)
    {
        if (ReferenceGrid == null) return;

        // Calculate scale change (exactly like MapContainer.MouseWheelOnMapContainer)
        double scaleChange;
        if (deltaY > 0)
        {
            if (_scale <= _fitScale / 2) return;
            scaleChange = _scale > _fitScale ? -(_scale / _fitScale) : -0.1;
        }
        else
        {
            if (_scale >= _fitScale * 100) return;
            scaleChange = _scale < _fitScale ? 0.5 : (_scale / _fitScale);
        }

        // Store old scale before updating (exactly like MapContainer)
        double oldScale = _scale;

        // Update scale (exactly like MapContainer: Scale += scaleChange)
        _scale += scaleChange;
        
        // Set SVG rect first (exactly like MapContainer: ImageWidth * Scale, ImageHeight * Scale)
        await _jsModule.InvokeVoidAsync("setSvgRect", mapContainerRef, _imageWidth * _scale, _imageHeight * _scale);
        
        await _jsModule.InvokeVoidAsync("setCanvasRect", mapCanvasBaseRef, _imageWidth * _scale, _imageHeight * _scale);
        await _jsModule.InvokeVoidAsync("setCanvasRect", laserScanCanvasRef, _imageWidth * _scale, _imageHeight * _scale);

        // Calculate cursor position in world coordinates (exactly like MapContainer)
        // MapContainer: CursorX = (clientX - ClientOriginX) / Scale
        // MapContainer: CursorY = (ClientOriginY - clientY) / Scale
        // Update CursorX/CursorY (they are in world coordinates)
        CursorX = (clientX - _clientOriginX) / oldScale;
        CursorY = (_clientOriginY - clientY) / oldScale;
        MapMousePositionRef.Update(CursorX, CursorY);

        // Calculate mouse position relative to map origin (exactly like MapContainer)
        // MapContainer: mouseX = CursorX - OriginX
        // MapContainer: mouseY = CursorY - MapData.OriginY (use original origin Y, not transformed)
        double mouseX = CursorX - _originX;
        double mouseY = CursorY - _mapOriginY;

        // Calculate movement adjustment (exactly like MapContainer.MouseWheelOnMapContainer)
        // MapContainer: Left - mouseX * scaleChange, Top - (ImageHeight - mouseY) * scaleChange
        await SetViewMovement(_left - mouseX * scaleChange, _top - (_imageHeight - mouseY) * scaleChange);
        
        // Update robot pose after scale change
        if (IsMapDisplayActive)
        {
            await UpdateRobotPoseSvgAsync();
        }
    }

    [JSInvokable]
    public async Task OnMouseMove(double clientX, double clientY, long buttons, bool ctrlKey, double movementX, double movementY)
    {
        // Calculate cursor position in world coordinates (exactly like MapContainer.MouseMoveOnMapContainer)
        // MapContainer: CursorX = (clientX - ClientOriginX) / Scale (world coordinates in meters)
        // MapContainer: CursorY = (ClientOriginY - clientY) / Scale (world coordinates in meters)
        CursorX = (clientX - _clientOriginX) / _scale;
        CursorY = (_clientOriginY - clientY) / _scale;
        MapMousePositionRef.Update(CursorX, CursorY);
        
        // Update RobotGoalPose mouse position (line goal->mouse and circle)
        if (buttons == 1) // Right mouse button down: update goal pose to current mouse
        {
            GoalPoseRef?.SetMousePosition(CursorX, CursorY);
        }
        else if (buttons == 4) // Middle mouse button
        {
            await SetViewMovement(_left + movementX, _top + movementY);
            
            // Update robot pose after pan (pose position in SVG doesn't change, but we update to ensure consistency)
            if (IsMapDisplayActive)
            {
                await UpdateRobotPoseSvgAsync();
            }
        }
    }

    [JSInvokable]
    public async Task OnMouseDown(int button, bool altKey, bool ctrlKey, bool shiftKey)
    {
        Console.WriteLine($"OnMouseDown: button={button}, altKey={altKey}, ctrlKey={ctrlKey}, shiftKey={shiftKey}");
        if (button == 0) // Right mouse button: set robot pose and goal pose for RobotGoalPose
        {
            var robotX = _currentPose.Position.X;
            var robotY = _currentPose.Position.Y;
            GoalPoseRef?.SetRobotPosition(robotX, robotY);
            GoalPoseRef?.SetGoalPosition(CursorX, CursorY);
            GoalPoseRef?.SetMousePosition(CursorX, CursorY);
            GoalPoseRef?.Show();
        }

        await Task.CompletedTask;
    }

    [JSInvokable]
    public async Task OnMouseUp(int button, bool altKey, bool ctrlKey, bool shiftKey)
    {
        if (button == 0) // Right mouse button up: áp dụng Optimize (ẩn nếu goal–mouse < 1 m, ngược lại clamp 1 m)
            GoalPoseRef?.Optimize();
        await Task.CompletedTask;
    }

    #endregion

    #region Timers

    /// <summary>
    /// Localizing: grid base requested once only (no timer).
    /// ScanMapping: timer 3s to poll grid base + grid updating + trajectory nodes.
    /// </summary>
    private void UpdateTimerBasedOnState()
    {
        lock (_timerLock)
        {
            if (_currentState == SLAMState.ScanMapping)
            {
                if (_updateTimer == null)
                {
                    _updateTimer = new System.Timers.Timer(GridPollIntervalMs)
                    {
                        AutoReset = false
                    };
                    _updateTimer.Elapsed += OnTimerElapsed;
                    _updateTimer.Start();
                }
                else if (!_updateTimer.Enabled)
                {
                    _updateTimer.Start();
                }
            }
            else
            {
                if (_updateTimer != null)
                {
                    _updateTimer.Stop();
                    _updateTimer.Elapsed -= OnTimerElapsed;
                    _updateTimer.Dispose();
                    _updateTimer = null;
                }
            }
        }
    }

    /// <summary>
    private void OnTimerElapsed(object? sender, System.Timers.ElapsedEventArgs e)
    {
        _ = InvokeAsync(async () =>
        {
            try
            {
                if (_currentState != SLAMState.ScanMapping)
                {
                    lock (_timerLock)
                    {
                        if (_updateTimer != null && _currentState == SLAMState.ScanMapping)
                            _updateTimer.Start();
                    }
                    return;
                }

                bool hasUpdates = false;

                var grid = await CartographerClient.GetOccupancyGridAsync(_lastGridUpdateTime);
                if (grid != null)
                {
                    CurrentGrid = grid;
                    _lastGridUpdateTime = grid.LastBaseUpdated;
                    hasUpdates = true;
                    UpdateSvgOrigin();
                    await TryApplyTrajectoryFromGrid(grid);
                }

                if (hasUpdates && ReferenceGrid != null)
                {
                    // BUG FIX: Always update origin when grid changes, not just when dimensions change significantly
                    // Grid origin can change when grid grows even if dimensions stay similar
                    if (CurrentGrid != null)
                    {
                        // Check if origin changed (which indicates grid grow/shift)
                        var originChanged = Math.Abs(_originX - CurrentGrid.Origin.Position.X) > 0.0001 ||
                                           Math.Abs(_mapOriginY - CurrentGrid.Origin.Position.Y) > 0.0001;

                        // Check if dimensions changed significantly
                        var dimensionsChanged = _imageWidth == 0 || _imageHeight == 0 ||
                            Math.Abs(_imageWidth - CurrentGrid.Width * CurrentGrid.Resolution) > 0.001 ||
                            Math.Abs(_imageHeight - CurrentGrid.Height * CurrentGrid.Resolution) > 0.001;

                        if (dimensionsChanged || originChanged)
                        {
                            Console.WriteLine($"[MapLocalization] Grid changed: dims={dimensionsChanged}, origin={originChanged}, " +
                                $"size={CurrentGrid.Width}x{CurrentGrid.Height}, " +
                                $"origin=[{CurrentGrid.Origin.Position.X:F3},{CurrentGrid.Origin.Position.Y:F3}], " +
                                $"prevOrigin=[{_originX:F3},{_mapOriginY:F3}]");
                            await LoadMapFromGrid(CurrentGrid);
                        }
                        else
                            await DrawOccupancyGrid();
                    }
                    StateHasChanged();
                }

                lock (_timerLock)
                {
                    if (_updateTimer != null && _currentState == SLAMState.ScanMapping)
                        _updateTimer.Start();
                }
            }
            catch (Exception ex)
            {
                Console.Error.WriteLine($"[MapLocalization] Error in OnTimerElapsed: {ex.Message}");
            }
        });
    }

    private async Task RequestOccupancyGridForLocalizationAsync()
    {
        try
        {
            var requestTime = DateTime.MinValue;
            for (int i = 0; i < GridRequestMaxRetries; i++)
            {
                if (!IsMapDisplayActive)
                    break;

                var grid = await CartographerClient.GetOccupancyGridAsync(requestTime);
                if (grid == null)
                {
                    await Task.Delay(GridRequestRetryDelayMs);
                    continue;
                }

                CurrentGrid = grid;
                _lastGridUpdateTime = grid.LastBaseUpdated;
                await LoadMapFromGrid(grid);
                await TryApplyTrajectoryFromGrid(grid);
                break;
            }
        }
        catch (Exception ex)
        {
            Console.Error.WriteLine($"[MapLocalization] Error requesting OccupancyGrid for localization: {ex.Message}");
        }
    }    
    #endregion

    private void UpdatePoseLaserTimerBasedOnState()
    {
        lock (_poseLaserTimerLock)
        {
            if (IsMapDisplayActive)
            {
                if (_poseLaserTimer == null)
                {
                    _poseLaserTimer = new System.Timers.Timer(PoseLaserPollIntervalMs)
                    {
                        AutoReset = false
                    };
                    _poseLaserTimer.Elapsed += OnPoseLaserTimerElapsed;
                    _poseLaserTimer.Start();
                }
                else if (!_poseLaserTimer.Enabled)
                {
                    _poseLaserTimer.Start();
                }
            }
            else
            {
                if (_poseLaserTimer != null)
                {
                    _poseLaserTimer.Stop();
                    _poseLaserTimer.Elapsed -= OnPoseLaserTimerElapsed;
                    _poseLaserTimer.Dispose();
                    _poseLaserTimer = null;
                }
            }
        }
    }

    private void OnPoseLaserTimerElapsed(object? sender, System.Timers.ElapsedEventArgs e)
    {
        _ = InvokeAsync(async () =>
        {
            try
            {
                // Get robot pose
                var pose = await CartographerClient.GetCurrentPoseAsync();
                if (pose != null)
                {
                    _currentPose = pose;
                    await UpdateRobotPoseSvgAsync();
                }

                // Get laser scan
                var laserScanPoints = await CartographerClient.GetSamplePointCloudAsync();
                if (laserScanPoints != null && laserScanPoints.Length > 0)
                {
                    _currentLaserScanPoints = laserScanPoints;
                    var refGrid = ReferenceGrid;
                    if (refGrid != null && refGrid.Width > 0 && refGrid.Height > 0)
                        await DrawLaserScanAsync();
                }

                lock (_poseLaserTimerLock)
                {
                    if (_poseLaserTimer != null && IsMapDisplayActive)
                    {
                        _poseLaserTimer.Start();
                    }
                }
            }
            catch (Exception ex)
            {
                Console.Error.WriteLine($"[MapLocalization] Error in OnPoseLaserTimerElapsed: {ex.Message}");
            }
        });
    }

    #region Helpers

    /// <summary>
    /// Lấy goal pose từ RobotGoalPose (GoalX, GoalY, GoalYaw) dưới dạng PoseDto để gửi SetInitialPose.
    /// </summary>
    public PoseDto? GetGoalPoseDto()
    {
        if (GoalPoseRef == null)
            return null;
        var q = QuaternionNumbers.FromYawRadian(GoalPoseRef.GoalYaw);
        return new PoseDto
        {
            Position = new RobotNet10.Shared.Numbers.Vector3 { X = GoalPoseRef.GoalX, Y = GoalPoseRef.GoalY, Z = 0 },
            Orientation = new QuaternionGeometry(q.X, q.Y, q.Z, q.W),
            Timestamp = DateTime.UtcNow
        };
    }

    private async Task UpdateRobotPoseSvgAsync()
    {
        try
        {
            // Hide robot pose if scale is invalid or no grid (base or updating for ScanMapping)
            var referenceGrid = CurrentGrid;
            if (_scale <= 0 || referenceGrid == null)
            {
                return;
            }

            var robotX = _currentPose.Position.X;
            var robotY = _currentPose.Position.Y;
            var yaw = _currentPose.Orientation.ToYawRadian();

            RobotPoseRef.UpdatePose(robotX, robotY, yaw);
            RobotPoseInfoRef.Update(robotX, robotY, yaw, _currentPose.Score);
        }
        catch (Exception ex)
        {
            Console.Error.WriteLine($"[MapLocalization] UpdateRobotPoseSvgAsync: Exception - {ex.Message}");
        }
    }

    private (int x, int y) WorldToGrid(double worldX, double worldY)
    {
        var grid = ReferenceGrid;
        if (grid == null)
            return (0, 0);

        var relativeX = worldX - grid.Origin.Position.X;
        var relativeY = worldY - grid.Origin.Position.Y;

        var gridX = (int)Math.Floor(relativeX / grid.Resolution);
        var gridY = (int)Math.Floor(relativeY / grid.Resolution);

        return (gridX, gridY);
    }

    /// <summary>
    /// Update trajectory polyline via JsInvoke (ElementReference, no StateHasChanged).
    /// Only updates points data; other attributes (fill, stroke, stroke-width, opacity) are fixed in markup.
    /// </summary>
    private async Task UpdateTrajectoryPolylineAsync()
    {
        var pointsStr = "";
        if (_trajectoryPath.Count > 1)
        {
            var worldPoints = _trajectoryPath.Select(p => $"{p.X},{p.Y}");
            pointsStr = string.Join(" ", worldPoints);
        }

        try
        {
            await _jsModule.InvokeVoidAsync("setPolylinePointsOnly",
                trajectoryPolylineRef,
                pointsStr);
        }
        catch (Exception ex)
        {
            Console.Error.WriteLine($"[MapLocalization] UpdateTrajectoryPolylineAsync: {ex.Message}");
        }
    }

    private async Task DrawLaserScanAsync()
    {
        var referenceGrid = ReferenceGrid;
        if (_currentLaserScanPoints == null || _currentLaserScanPoints.Length == 0 || referenceGrid == null)
        {
            return;
        }

        try
        {
            if (referenceGrid.Width <= 0 || referenceGrid.Height <= 0)
            {
                return;
            }

            // Set canvas size
            await _jsModule.InvokeVoidAsync("setCanvasSize", laserScanCanvasRef, referenceGrid.Width, referenceGrid.Height);

            // laserScanPoints from GetSamplePointCloudAsync() are already in global (map) coordinates.
            // Only need to convert to grid coordinates via WorldToGrid (uses grid.Origin).
            // laserScanCanvasRef uses class "map-canvas" with CSS transform: scale(1, -1), so the canvas
            // is rendered with Y flipped: buffer y=0 (top) appears at bottom, buffer y=height-1 (bottom) at top.
            // We pass (gridX, gridY) directly: grid Y increases upward. Drawing at (gridX, gridY) puts
            // world-Y-up at large buffer y; after scale(1,-1) that displays at visual top. Correct.
            var mapPoints = new List<(double X, double Y)>();

            foreach (var point in _currentLaserScanPoints)
            {
                // point.X, point.Y are already world coordinates; WorldToGrid applies Origin transform
                var (gridX, gridY) = WorldToGrid(point.X, point.Y);

                if (gridX >= 0 && gridX < referenceGrid.Width && gridY >= 0 && gridY < referenceGrid.Height)
                {
                    // Pass grid coords (Y-up); map-canvas CSS scale(1,-1) handles display flip
                    mapPoints.Add((gridX, gridY));
                }
            }

            if (mapPoints.Count > 0)
            {
                var pointsArray = mapPoints.Select(p => new double[] { p.X, p.Y }).ToArray();
                await _jsModule.InvokeVoidAsync("drawPointsOnCanvas", laserScanCanvasRef, pointsArray, "#FF0000", 0.5);
            }
        }
        catch (Exception ex)
        {
            Console.Error.WriteLine($"[MapLocalization] DrawLaserScanAsync: Exception - {ex.Message}");
        }
    }

    private async Task ClearLaserScanAsync()
    {
        await _jsModule.InvokeVoidAsync("clearCanvas", laserScanCanvasRef);
    }

    #endregion

    #region Dispose

    public async ValueTask DisposeAsync()
    {
        CartographerClient.StateChanged -= OnStateChanged;

        lock (_timerLock)
        {
            if (_updateTimer != null)
            {
                _updateTimer.Stop();
                _updateTimer.Elapsed -= OnTimerElapsed;
                _updateTimer.Dispose();
                _updateTimer = null;
            }
        }

        lock (_poseLaserTimerLock)
        {
            if (_poseLaserTimer != null)
            {
                _poseLaserTimer.Stop();
                _poseLaserTimer.Elapsed -= OnPoseLaserTimerElapsed;
                _poseLaserTimer.Dispose();
                _poseLaserTimer = null;
            }
        }

        await _jsModule.DisposeAsync();
        _dotNetObj?.Dispose();
    }

    #endregion
}