// XLOC Map Renderer - Canvas-based 2D visualization for robot localization
// Renders occupancy grid maps, robot pose, and LIDAR scans

(function () {
    window.robotnet = window.robotnet || {};
    if (!window.robotnet.triggerFileDownload) {
        window.robotnet.triggerFileDownload = function (url) {
            const a = document.createElement("a");
            a.href = url;
            a.rel = "noopener";
            document.body.appendChild(a);
            a.click();
            document.body.removeChild(a);
        };
    }
    if (!window.robotnet.clickElementById) {
        window.robotnet.clickElementById = function (elementId) {
            const el = document.getElementById(elementId);
            if (el) el.click();
        };
    }
})();

window.xlocMapRenderer = {
    canvas: null,
    ctx: null,
    gridMap: null,
    onlineMapOverlay: null, // Online map overlay for update map mode
    mapCacheCanvas: null,
    mapCacheCtx: null,
    mapCacheReady: false,
    globalPathData: null,
    showGlobalPath: false,
    localPathData: null,
    showLocalPath: true,
    costMapData: null,
    costMapCacheCanvas: null,
    costMapCacheCtx: null,
    costMapCacheReady: false,
    costMapPoseAnchor: null,
    costMapOdomAnchor: null,
    costMapFixedYaw: null,
    costMapMapFromOdomLock: null,
    costMapLocalizationActive: false,
    costMapLastMatchingScore: -1,
    showCostMap: false,
    showRobotPose: true,
    robotFootprint: null, // Array of {x, y} points defining robot footprint
    showRobotFootprint: true,
    robotPose: null,
    laserScanData: null,
    lidarDisplayOptions: {
        lidar1: { visible: true, mode: 'minimal' },
        lidar2: { visible: true, mode: 'minimal' },
        lidar3: { visible: true, mode: 'minimal' }
    },
    cursorTooltipEl: null,
    cursorStyle: "url('data:image/svg+xml;utf8,<svg xmlns=\"http://www.w3.org/2000/svg\" width=\"24\" height=\"24\" viewBox=\"0 0 24 24\"><line x1=\"0\" y1=\"12\" x2=\"24\" y2=\"12\" stroke=\"%23000000\" stroke-width=\"2\"/><line x1=\"12\" y1=\"0\" x2=\"12\" y2=\"24\" stroke=\"%23000000\" stroke-width=\"2\"/></svg>') 12 12, crosshair",
    initialPoseCursor: "url('data:image/svg+xml;utf8,<svg xmlns=\"http://www.w3.org/2000/svg\" width=\"24\" height=\"24\" viewBox=\"0 0 24 24\"><line x1=\"0\" y1=\"12\" x2=\"24\" y2=\"12\" stroke=\"%23000000\" stroke-width=\"2\"/><line x1=\"12\" y1=\"0\" x2=\"12\" y2=\"24\" stroke=\"%23000000\" stroke-width=\"2\"/></svg>') 12 12, crosshair",
    resizeObserver: null,
    renderPending: false,
    dotNetHelper: null,
    
    // Camera/view parameters
    offsetX: 0,
    offsetY: 0,
    scale: 20, // pixels per meter (zoom level)
    
    // LOCKED values - NEVER change after map is loaded
    lockedMapOrigin: null,  // {x, y, z} - Map origin is FIXED
    lockedOffsetX: null,    // View offset X is FIXED after initial pose
    lockedOffsetY: null,    // View offset Y is FIXED after initial pose
    
    // Drag state
    isDragging: false,
    dragStartX: 0,
    dragStartY: 0,
    lastOffsetX: 0,
    lastOffsetY: 0,
    
    // Initial pose selection state
    initialPose: null,
    initialPoseMode: false,
    initialPoseDragging: false,
    initialPoseDragBase: null,
    
    /**
     * Initialize the map renderer with a canvas element
     */
    init: function(canvasElement) {
        this.canvas = canvasElement;
        if (!this.canvas) {
            console.error('Canvas element not found');
            return;
        }
        
        this.ctx = this.canvas.getContext('2d');
        if (!this.ctx) {
            console.error('Failed to get 2D context');
            return;
        }

        this.cursorTooltipEl = document.getElementById('xloc-map-cursor-tooltip');
        this.setupResizeObserver();
        
        // Set canvas size to match container
        this.resize();
        
        // Setup mouse event handlers for pan/zoom
        this.setupEventHandlers();
        
        // Initial render
        this.render();
    },
    
    /**
     * Setup mouse event handlers for panning
     */
    setupEventHandlers: function() {
        if (!this.canvas) return;
        
        // Mouse down - start dragging
        this.canvas.addEventListener('mousedown', (e) => {
            if (e.button !== 0) return; // Only handle left mouse button
            
            // CRITICAL: Check initial pose mode FIRST to prevent panning conflict
            if (this.initialPoseMode) {
                console.log("Initial pose mode: mouse down at", e.offsetX, e.offsetY);
                const mapCoords = this.getMapCoordsFromEvent(e);
                console.log("Map coordinates:", mapCoords.mapX, mapCoords.mapY);
                
                this.initialPoseDragging = true;
                this.initialPoseDragBase = { x: mapCoords.mapX, y: mapCoords.mapY };
                
                // Initialize pose at click position with default yaw (0 = pointing right/+X)
                const existingYaw = this.initialPose ? this.initialPose.yaw : 0;
                this.initialPose = { x: mapCoords.mapX, y: mapCoords.mapY, yaw: existingYaw };
                
                // Update pose from current mouse position (will set yaw based on drag)
                this.updateInitialPoseFromEvent(e);
                this.requestRender();
                
                e.preventDefault(); // Prevent default behavior
                e.stopPropagation(); // Stop event propagation
                return;
            }
            
            // Normal panning mode
            this.isDragging = true;
            this.dragStartX = e.offsetX;
            this.dragStartY = e.offsetY;
            this.lastOffsetX = this.offsetX;
            this.lastOffsetY = this.offsetY;
            this.canvas.style.cursor = this.cursorStyle;
        });
        
        // Mouse move - pan view
        this.canvas.addEventListener('mousemove', (e) => {
            this.updateCursorTooltip(e);
            
            // CRITICAL: Check initial pose dragging FIRST to prevent panning conflict
            if (this.initialPoseDragging) {
                this.updateInitialPoseFromEvent(e);
                this.requestRender();
                e.preventDefault(); // Prevent default behavior during drag
                e.stopPropagation(); // Stop event propagation
                return;
            }
            
            // Normal panning mode
            if (this.isDragging) {
                const dx = e.offsetX - this.dragStartX;
                const dy = e.offsetY - this.dragStartY;
                this.offsetX = this.lastOffsetX + dx;
                this.offsetY = this.lastOffsetY + dy;
                
                // CRITICAL: Update locked offsets when user pans (user-initiated action)
                if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
                    this.lockedOffsetX = this.offsetX;
                    this.lockedOffsetY = this.offsetY;
                }
                
                this.render();
            }
        });
        
        // Mouse up - stop dragging
        this.canvas.addEventListener('mouseup', (e) => {
            if (this.initialPoseDragging) {
                console.log("Initial pose drag ended");
                // Update pose one final time to ensure it's current
                this.updateInitialPoseFromEvent(e);
                console.log("Final initial pose:", this.initialPose);
                this.initialPoseDragging = false;
                this.notifyInitialPoseSelected();
                e.preventDefault();
                e.stopPropagation();
                return;
            }
            this.isDragging = false;
            this.canvas.style.cursor = this.cursorStyle;
        });
        
        // Mouse leave - stop dragging
        this.canvas.addEventListener('mouseleave', () => {
            if (this.initialPoseDragging) {
                this.initialPoseDragging = false;
                this.notifyInitialPoseSelected();
            }
            this.isDragging = false;
            this.canvas.style.cursor = this.cursorStyle;
            this.hideCursorTooltip();
        });
        
        // Mouse wheel - zoom
        this.canvas.addEventListener('wheel', (e) => {
            e.preventDefault();
            
            // Zoom factor
            const zoomFactor = e.deltaY > 0 ? 0.9 : 1.1;
            const newScale = this.scale * zoomFactor;
            
            // Clamp zoom level
            if (newScale >= 1 && newScale <= 100) {
                // Zoom towards mouse position
                const mouseX = e.offsetX - this.canvas.width / 2 - this.offsetX;
                const mouseY = e.offsetY - this.canvas.height / 2 - this.offsetY;
                
                this.scale = newScale;
                
                // Adjust offset to zoom towards mouse
                this.offsetX -= mouseX * (zoomFactor - 1);
                this.offsetY -= mouseY * (zoomFactor - 1);
                
                // CRITICAL: Update locked offsets when user zooms (user-initiated action)
                if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
                    this.lockedOffsetX = this.offsetX;
                    this.lockedOffsetY = this.offsetY;
                }
                
                this.render();
            }
        });
        
        // Set initial cursor
        this.canvas.style.cursor = this.cursorStyle;
    },

    setupResizeObserver: function() {
        if (!this.canvas) return;
        const container = this.canvas.parentElement;
        if (!container || typeof ResizeObserver === 'undefined') return;
        if (this.resizeObserver) {
            this.resizeObserver.disconnect();
        }
        this.resizeObserver = new ResizeObserver(() => {
            this.resize();
        });
        this.resizeObserver.observe(container);
    },

    updateCursorTooltip: function(event) {
        if (!this.canvas || !this.cursorTooltipEl) return;
        const { cssX, cssY, mapX, mapY } = this.getMapCoordsFromEvent(event);
        this.cursorTooltipEl.textContent = `Map: (${mapX.toFixed(3)}, ${mapY.toFixed(3)})m`;
        this.cursorTooltipEl.style.left = `${cssX + 12}px`;
        this.cursorTooltipEl.style.top = `${cssY + 12}px`;
        this.cursorTooltipEl.style.display = 'block';
    },

    hideCursorTooltip: function() {
        if (!this.cursorTooltipEl) return;
        this.cursorTooltipEl.style.display = 'none';
    },

    getMapCoordsFromEvent: function(event) {
        const rect = this.canvas.getBoundingClientRect();
        const cssX = event.clientX - rect.left;
        const cssY = event.clientY - rect.top;
        const scaleX = this.canvas.width / rect.width;
        const scaleY = this.canvas.height / rect.height;
        const canvasX = cssX * scaleX;
        const canvasY = cssY * scaleY;
        const { mapFrameX, mapFrameY } = this.getFramePositions();
        const mapX = (canvasX - mapFrameX) / this.scale;
        const mapY = (mapFrameY - canvasY) / this.scale;
        return { cssX, cssY, canvasX, canvasY, mapX, mapY };
    },

    setDotNetHelper: function(dotNetHelper) {
        this.dotNetHelper = dotNetHelper;
    },

    setInitialPoseMode: function(enabled) {
        const wasEnabled = this.initialPoseMode;
        this.initialPoseMode = !!enabled;
        console.log("Initial pose mode changed:", wasEnabled ? "ON" : "OFF", "→", this.initialPoseMode ? "ON" : "OFF");
        
        if (!this.initialPoseMode) {
            // Mode disabled - stop any dragging and reset cursor
            this.initialPoseDragging = false;
            this.initialPoseDragBase = null;
            if (this.canvas) {
                this.canvas.style.cursor = this.cursorStyle;
            }
            console.log("Initial pose mode disabled - cursor reset");
        } else {
            // Mode enabled - set crosshair cursor (black)
            if (this.canvas) {
                this.canvas.style.cursor = this.initialPoseCursor;
                console.log("Initial pose mode enabled - cursor set to crosshair");
            }
            // Ensure initial pose is visible when mode is enabled
            // If no initial pose exists, create a default one at origin
            if (!this.initialPose) {
                this.initialPose = { x: 0, y: 0, yaw: 0 };
                console.log("Created default initial pose at origin:", this.initialPose);
            } else {
                console.log("Using existing initial pose:", this.initialPose);
            }
        }
        this.requestRender();
    },

    setInitialPose: function(pose) {
        if (!pose) {
            console.warn('setInitialPose called with null/undefined pose');
            return;
        }
        
        const x = Number(pose.x ?? pose.X ?? 0);
        const y = Number(pose.y ?? pose.Y ?? 0);
        const yaw = Number(pose.yaw ?? pose.Yaw ?? 0);
        this.initialPose = { x, y, yaw };
        
        // CRITICAL: Use locked view offsets if available (prevents map frame from moving)
        if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
            // Force use locked offsets - map frame MUST stay fixed
            if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 || 
                Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
                console.error('⚠️ CRITICAL ERROR: View offsets changed when setting initial pose!');
                console.error('  Locked:', { offsetX: this.lockedOffsetX, offsetY: this.lockedOffsetY });
                console.error('  Current:', { offsetX: this.offsetX, offsetY: this.offsetY });
                console.error('  → Restoring locked offsets to prevent map frame movement!');
                // Restore locked offsets
                this.offsetX = this.lockedOffsetX;
                this.offsetY = this.lockedOffsetY;
            }
        }
        
        // CRITICAL: Verify map origin has NOT changed
        if (this.lockedMapOrigin) {
            const currentOrigin = this.gridMap?.origin;
            if (currentOrigin) {
                const dx = Math.abs(currentOrigin.x - this.lockedMapOrigin.x);
                const dy = Math.abs(currentOrigin.y - this.lockedMapOrigin.y);
                const dz = Math.abs(currentOrigin.z - this.lockedMapOrigin.z);
                
                if (dx > 0.001 || dy > 0.001 || dz > 0.001) {
                    console.error('⚠️ CRITICAL ERROR: Map origin changed when setting initial pose!');
                    console.error('  Locked:', this.lockedMapOrigin);
                    console.error('  Current:', currentOrigin);
                    console.error('  → Forcing use of locked origin!');
                    // Force use locked origin
                    if (this.gridMap) {
                        this.gridMap.origin = {
                            x: this.lockedMapOrigin.x,
                            y: this.lockedMapOrigin.y,
                            z: this.lockedMapOrigin.z
                        };
                    }
                }
            }
        }
        
        
        // IMPORTANT: Only update initial pose visualization, DO NOT reload map!
        // Map origin and view offsets are locked and will not change.
        this.requestRender();
    },

    updateInitialPoseFromEvent: function(event) {
        if (!this.initialPoseDragging || !this.initialPoseDragBase) return;
        const mapCoords = this.getMapCoordsFromEvent(event);
        const dx = mapCoords.mapX - this.initialPoseDragBase.x;
        const dy = mapCoords.mapY - this.initialPoseDragBase.y;
        const distance = Math.hypot(dx, dy);
        // Calculate yaw from drag direction: atan2(dy, dx) gives angle from +X axis
        // If distance is too small, keep previous yaw or default to 0
        const yaw = distance > 0.01 ? Math.atan2(dy, dx) : (this.initialPose ? this.initialPose.yaw : 0);
        // Position stays at click point, yaw follows drag direction
        this.initialPose = { x: this.initialPoseDragBase.x, y: this.initialPoseDragBase.y, yaw: yaw };
    },

    notifyInitialPoseSelected: function() {
        if (!this.dotNetHelper || !this.initialPose) {
            console.warn("Cannot notify initial pose: dotNetHelper or initialPose is null");
            return;
        }
        console.log("Notifying initial pose selected:", {
            x: this.initialPose.x,
            y: this.initialPose.y,
            yaw: this.initialPose.yaw,
            yawDeg: this.initialPose.yaw * 180 / Math.PI
        });
        this.dotNetHelper.invokeMethodAsync("OnInitialPoseSelected", {
            x: this.initialPose.x,
            y: this.initialPose.y,
            yaw: this.initialPose.yaw
        }).catch((error) => {
            console.error("Failed to notify initial pose selection:", error);
        });
    },
    
    /**
     * Get map origin x,y in meters (supports camelCase and PascalCase from C# serialization).
     * Map (0,0) = World (0,0) + (originX, originY). When negative, Map is bottom-left of World.
     * 
     * CRITICAL: Always use LOCKED origin if available to prevent map frame from moving.
     */
    getMapOriginXY: function() {
        // Always use locked origin if available (prevents map frame from moving)
        if (this.lockedMapOrigin) {
            return {
                x: this.lockedMapOrigin.x,
                y: this.lockedMapOrigin.y,
                z: this.lockedMapOrigin.z
            };
        }
        
        // Fallback to gridMap origin if locked origin not available yet
        if (!this.gridMap) return { x: 0, y: 0 };
        const o = this.gridMap.origin != null ? this.gridMap.origin : this.gridMap.Origin;
        if (o == null) return { x: 0, y: 0 };
        const x = typeof o.x === 'number' ? o.x : (typeof o.X === 'number' ? o.X : (Number(o.x) || Number(o.X) || 0));
        const y = typeof o.y === 'number' ? o.y : (typeof o.Y === 'number' ? o.Y : (Number(o.y) || Number(o.Y) || 0));
        return { x: x, y: y };
    },
    
    /**
     * Map frame = canvas center (single origin). Robot and occupancy are computed in Map frame.
     * 
     * Map frame position = canvas center + view offsets (for panning/zooming)
     * Map origin (in world coordinates) is LOCKED and NEVER changes.
     * 
     * CRITICAL: Always use LOCKED view offsets if available to prevent map frame from moving!
     */
    getFramePositions: function() {
        // CRITICAL: Use locked view offsets if available (prevents map frame from moving)
        let offsetX = this.offsetX;
        let offsetY = this.offsetY;
        
        if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
            // Force use locked offsets - map frame MUST stay fixed
            if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 || 
                Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
                console.error('⚠️ CRITICAL ERROR: View offsets changed! Restoring locked offsets in getFramePositions().');
                console.error('  Locked:', { offsetX: this.lockedOffsetX, offsetY: this.lockedOffsetY });
                console.error('  Current:', { offsetX: this.offsetX, offsetY: this.offsetY });
                // Restore locked offsets
                this.offsetX = this.lockedOffsetX;
                this.offsetY = this.lockedOffsetY;
            }
            // Always use locked offsets for frame position calculation
            offsetX = this.lockedOffsetX;
            offsetY = this.lockedOffsetY;
        }
        
        const mapFrameX = this.canvas.width / 2 + offsetX;
        const mapFrameY = this.canvas.height / 2 + offsetY;
        return { mapFrameX, mapFrameY };
    },
    
    /**
     * Set the occupancy grid map data (object - may have origin serialization issues)
     */
    setGridMap: function(gridMapData) {
        if (!gridMapData) return;
        var o = gridMapData.origin != null ? gridMapData.origin : gridMapData.Origin;
        if (gridMapData.dataBase64 && o == null) {
            console.warn('xlocMapRenderer: setGridMap received data but no origin - map will render at World frame');
        }
        this.gridMap = gridMapData;
        this.buildMapCache();
        this.requestRender();
    },
    
    /**
     * Set grid map with explicit origin (avoids C#->JS object serialization issues).
     * Call this from C# with numeric originX, originY so map renders at Map frame.
     * 
     * CRITICAL: Map origin is LOCKED on first call and NEVER changes!
     * This ensures that when robot pose changes, the map frame itself stays fixed.
     */
    setGridMapWithOrigin: function(width, height, resolution, originX, originY, originZ, dataBase64, isMapping) {
        if (width == null || height == null || !dataBase64) {
            console.warn('[setGridMapWithOrigin] Invalid parameters', { width, height, hasData: !!dataBase64, dataLength: dataBase64 ? dataBase64.length : 0 });
            return;
        }
        
        // Only log when map changes significantly
        const isNewMap = this.lockedMapOrigin === null || 
                        this.gridMap === null || 
                        this.gridMap.width !== width || 
                        this.gridMap.height !== height;
        
        if (isNewMap) {
            console.log(`[setGridMapWithOrigin] Setting map - Width: ${width}, Height: ${height}, Resolution: ${resolution}, Origin: (${originX}, ${originY}, ${originZ}), Data length: ${dataBase64.length}`);
        }
        
        const newOrigin = {
            x: Number(originX),
            y: Number(originY),
            z: Number(originZ)
        };
        
        // When mapping, allow origin to update from online map to ensure alignment with LIDAR
        // When not mapping, lock origin to prevent map frame movement
        const isMappingMode = isMapping === true; // Check if currently mapping
        
        if (this.lockedMapOrigin === null) {
            // First time: lock origin
            this.lockedMapOrigin = newOrigin;
            console.log(`[setGridMapWithOrigin] Locked map origin at (${newOrigin.x}, ${newOrigin.y}, ${newOrigin.z})`);
            // CRITICAL: Also lock view offsets when map is first loaded
            // This ensures map frame position stays fixed
            if (this.lockedOffsetX === null && this.lockedOffsetY === null) {
                this.lockedOffsetX = this.offsetX;
                this.lockedOffsetY = this.offsetY;
            }
        } else {
            // Check if origin changed
            const dx = Math.abs(newOrigin.x - this.lockedMapOrigin.x);
            const dy = Math.abs(newOrigin.y - this.lockedMapOrigin.y);
            const dz = Math.abs(newOrigin.z - this.lockedMapOrigin.z);
            
            if (dx > 0.001 || dy > 0.001 || dz > 0.001) {
                // Origin changed - during mapping, use new origin to align with LIDAR
                // During localization, keep locked origin to prevent map frame movement
                if (isMappingMode) {
                    console.log(`[setGridMapWithOrigin] Origin updated during mapping: (${newOrigin.x.toFixed(3)}, ${newOrigin.y.toFixed(3)}, ${newOrigin.z.toFixed(3)})`);
                    console.log(`[setGridMapWithOrigin] Previous: (${this.lockedMapOrigin.x.toFixed(3)}, ${this.lockedMapOrigin.y.toFixed(3)}, ${this.lockedMapOrigin.z.toFixed(3)})`);
                    // Update locked origin to match online map (ensures alignment with LIDAR)
                    this.lockedMapOrigin = newOrigin;
                } else {
                    console.warn('[setGridMapWithOrigin] Origin changed but not mapping - keeping locked origin');
                    // Use locked origin
                    originX = this.lockedMapOrigin.x;
                    originY = this.lockedMapOrigin.y;
                    originZ = this.lockedMapOrigin.z;
                }
            }
        }
        
        // Use current origin (may be updated during mapping)
        this.gridMap = {
            width: width,
            height: height,
            resolution: resolution,
            origin: { x: this.lockedMapOrigin.x, y: this.lockedMapOrigin.y, z: this.lockedMapOrigin.z },
            dataBase64: dataBase64
        };
        
        if (isNewMap) {
            console.log('[setGridMapWithOrigin] Building map cache...');
        }
        this.buildMapCache();
        if (isNewMap) {
            console.log('[setGridMapWithOrigin] Requesting render...');
        }
        this.requestRender();
        if (isNewMap) {
            console.log('[setGridMapWithOrigin] Map set and render requested');
        }
    },
    setGridMapWithMerge: function(width, height, resolution, originX, originY, originZ, dataBase64) {
        if (width == null || height == null || !dataBase64) {
            console.warn('[setGridMapWithMerge] Invalid parameters', { width, height, hasData: !!dataBase64 });
            return;
        }
        
        // If no existing map, just set it normally
        if (!this.gridMap || !this.gridMap.dataBase64) {
            console.log('[setGridMapWithMerge] No existing map, setting new map');
            this.setGridMapWithOrigin(width, height, resolution, originX, originY, originZ, dataBase64, false);
            return;
        }
        
        const newOrigin = {
            x: Number(originX),
            y: Number(originY),
            z: Number(originZ)
        };
        
        // Use locked origin (don't allow origin to change during update map)
        const originToUse = this.lockedMapOrigin || newOrigin;
        
        // Decode existing map data
        const existingBytes = Uint8Array.from(atob(this.gridMap.dataBase64), c => c.charCodeAt(0));
        const existingWidth = this.gridMap.width;
        const existingHeight = this.gridMap.height;
        const existingResolution = this.gridMap.resolution;
        
        // Decode new online map data
        const newBytes = Uint8Array.from(atob(dataBase64), c => c.charCodeAt(0));
        
        // Calculate bounds for merged map
        // Both maps should use the same origin, so we need to calculate pixel offsets
        const resolutionToUse = resolution;
        const existingOrigin = this.gridMap.origin || originToUse;
        const existingOffsetX = (existingOrigin.x - originToUse.x) / resolutionToUse;
        const existingOffsetY = (existingOrigin.y - originToUse.y) / resolutionToUse;
        const newOffsetX = 0; // New map uses the same origin
        const newOffsetY = 0;
        
        // Calculate bounds
        const existingMinX = Math.round(existingOffsetX);
        const existingMinY = Math.round(existingOffsetY);
        const existingMaxX = existingMinX + existingWidth;
        const existingMaxY = existingMinY + existingHeight;
        const newMinX = Math.round(newOffsetX);
        const newMinY = Math.round(newOffsetY);
        const newMaxX = newMinX + width;
        const newMaxY = newMinY + height;
        
        const mergedMinX = Math.min(existingMinX, newMinX);
        const mergedMinY = Math.min(existingMinY, newMinY);
        const mergedMaxX = Math.max(existingMaxX, newMaxX);
        const mergedMaxY = Math.max(existingMaxY, newMaxY);
        
        const mergedWidth = mergedMaxX - mergedMinX;
        const mergedHeight = mergedMaxY - mergedMinY;
        
        // Create merged map data
        const mergedBytes = new Uint8Array(mergedWidth * mergedHeight);
        // Initialize with unknown (255)
        mergedBytes.fill(255);
        
        // Copy existing map data
        for (let y = 0; y < existingHeight; y++) {
            for (let x = 0; x < existingWidth; x++) {
                const existingIdx = y * existingWidth + x;
                const mergedX = x + existingMinX - mergedMinX;
                const mergedY = y + existingMinY - mergedMinY;
                
                if (mergedX >= 0 && mergedX < mergedWidth && mergedY >= 0 && mergedY < mergedHeight) {
                    const mergedIdx = mergedY * mergedWidth + mergedX;
                    const existingValue = existingBytes[existingIdx];
                    // Copy existing value (including unknown = 255)
                    mergedBytes[mergedIdx] = existingValue;
                }
            }
        }
        
        // Merge new online map data (overwrite unknown areas and add new data)
        for (let y = 0; y < height; y++) {
            for (let x = 0; x < width; x++) {
                const newIdx = y * width + x;
                const mergedX = x + newMinX - mergedMinX;
                const mergedY = y + newMinY - mergedMinY;
                
                if (mergedX >= 0 && mergedX < mergedWidth && mergedY >= 0 && mergedY < mergedHeight) {
                    const mergedIdx = mergedY * mergedWidth + mergedX;
                    const newValue = newBytes[newIdx];
                    // Merge: use new value if it's not unknown, otherwise keep existing
                    if (newValue !== 255) {
                        mergedBytes[mergedIdx] = newValue;
                    } 
                }
            }
        }
        
        // Convert merged bytes to base64
        let mergedBase64 = '';
        for (let i = 0; i < mergedBytes.length; i++) {
            mergedBase64 += String.fromCharCode(mergedBytes[i]);
        }
        mergedBase64 = btoa(mergedBase64);
        
        // Update grid map with merged data
        this.gridMap = {
            width: mergedWidth,
            height: mergedHeight,
            resolution: resolution, // Use new map resolution (should be same)
            origin: originToUse,
            dataBase64: mergedBase64
        };
        
        console.log(`[setGridMapWithMerge] Merged map - Width: ${mergedWidth}, Height: ${mergedHeight}, Resolution: ${resolution}`);
        this.buildMapCache();
        this.requestRender();
    },
    /**
     * Set grid map with overlay: static map as base, online map as overlay (for update map mode)
     */
    setGridMapWithOverlay: function(staticWidth, staticHeight, staticResolution, originX, originY, originZ, staticDataBase64, onlineWidth, onlineHeight, onlineResolution, onlineDataBase64) {
        if (staticWidth == null || staticHeight == null || !staticDataBase64 || onlineWidth == null || onlineHeight == null || !onlineDataBase64) {
            console.warn('[setGridMapWithOverlay] Invalid parameters');
            return;
        }
        
        const newOrigin = {
            x: Number(originX),
            y: Number(originY),
            z: Number(originZ)
        };
        
        // Lock origin if not already locked
        if (this.lockedMapOrigin === null) {
            this.lockedMapOrigin = newOrigin;
            console.log(`[setGridMapWithOverlay] Locked map origin at (${newOrigin.x}, ${newOrigin.y}, ${newOrigin.z})`);
            if (this.lockedOffsetX === null && this.lockedOffsetY === null) {
                this.lockedOffsetX = this.offsetX;
                this.lockedOffsetY = this.offsetY;
            }
        }
        
        // Decode static map data
        const staticBytes = Uint8Array.from(atob(staticDataBase64), c => c.charCodeAt(0));
        
        // Decode online map data
        const onlineBytes = Uint8Array.from(atob(onlineDataBase64), c => c.charCodeAt(0));
        
        // Create static map object
        const staticMap = {
            width: staticWidth,
            height: staticHeight,
            resolution: staticResolution,
            origin: newOrigin,
            data: staticBytes
        };
        
        // Create online map object (overlay)
        const onlineMap = {
            width: onlineWidth,
            height: onlineHeight,
            resolution: onlineResolution,
            origin: newOrigin,
            data: onlineBytes
        };
        
        // Store both maps
        this.gridMap = staticMap;
        this.onlineMapOverlay = onlineMap; // Store online map as overlay
        
        // Build map cache with overlay
        this.buildMapCacheWithOverlay();
        
        this.requestRender();
    },
    
    /**
     * Build map cache with overlay: static map as base, online map overlaid on top
     */
    buildMapCacheWithOverlay: function() {
        if (!this.mapCacheCanvas || !this.gridMap) return;
        
        try {
            const staticMap = this.gridMap;
            const onlineMap = this.onlineMapOverlay;
            
            if (!onlineMap) {
                // Fallback to regular map cache if no overlay
                this.buildMapCache();
                return;
            }
            
            // Use the larger dimensions
            const maxWidth = Math.max(staticMap.width, onlineMap.width);
            const maxHeight = Math.max(staticMap.height, onlineMap.height);
            
            this.mapCacheCanvas.width = maxWidth;
            this.mapCacheCanvas.height = maxHeight;
            
            const imageData = this.mapCacheCtx.createImageData(maxWidth, maxHeight);
            const pixels = imageData.data;
            
            // First, render static map as base
            for (let y = 0; y < staticMap.height; y++) {
                for (let x = 0; x < staticMap.width; x++) {
                    const idx = y * staticMap.width + x;
                    const byte = staticMap.data[idx];
                    const pixelIdx = (y * maxWidth + x) * 4;
                    
                    // Convert occupancy value to color
                    if (byte === 0) {
                        // Free space - white
                        pixels[pixelIdx] = 255;
                        pixels[pixelIdx + 1] = 255;
                        pixels[pixelIdx + 2] = 255;
                        pixels[pixelIdx + 3] = 255;
                    } else if (byte === 100) {
                        // Occupied - black
                        pixels[pixelIdx] = 0;
                        pixels[pixelIdx + 1] = 0;
                        pixels[pixelIdx + 2] = 0;
                        pixels[pixelIdx + 3] = 255;
                    } else {
                        // Unknown - gray
                        pixels[pixelIdx] = 128;
                        pixels[pixelIdx + 1] = 128;
                        pixels[pixelIdx + 2] = 128;
                        pixels[pixelIdx + 3] = 255;
                    }
                }
            }
            
            // Then, overlay online map on top (only where online map has data)
            for (let y = 0; y < onlineMap.height; y++) {
                for (let x = 0; x < onlineMap.width; x++) {
                    const idx = y * onlineMap.width + x;
                    const byte = onlineMap.data[idx];
                    
                    // Only overlay if online map has valid data (not unknown)
                    if (byte !== 205) { // 205 = unknown in occupancy grid
                        const pixelIdx = (y * maxWidth + x) * 4;
                        
                        // Overlay online map data (with slight transparency to show both)
                        if (byte === 0) {
                            // Free space - light blue tint
                            pixels[pixelIdx] = Math.min(255, pixels[pixelIdx] * 0.7 + 200 * 0.3);
                            pixels[pixelIdx + 1] = Math.min(255, pixels[pixelIdx + 1] * 0.7 + 220 * 0.3);
                            pixels[pixelIdx + 2] = Math.min(255, pixels[pixelIdx + 2] * 0.7 + 255 * 0.3);
                        } else if (byte === 100) {
                            // Occupied - red tint
                            pixels[pixelIdx] = Math.min(255, pixels[pixelIdx] * 0.5 + 255 * 0.5);
                            pixels[pixelIdx + 1] = Math.min(255, pixels[pixelIdx + 1] * 0.5 + 0 * 0.5);
                            pixels[pixelIdx + 2] = Math.min(255, pixels[pixelIdx + 2] * 0.5 + 0 * 0.5);
                        }
                    }
                }
            }
            
            this.mapCacheCtx.putImageData(imageData, 0, 0);
            this.mapCacheReady = true;
            console.log('[buildMapCacheWithOverlay] Map cache with overlay built successfully');
        } catch (error) {
            console.error('[buildMapCacheWithOverlay] Error building map cache with overlay:', error);
            this.mapCacheReady = false;
        }
    },
    
    /**
     * Set the robot pose
     * CRITICAL: This should NEVER change view offsets or map origin!
     * Only robot pose visualization changes - map frame stays FIXED!
     */
    setRobotPose: function(poseData) {
        // CRITICAL: Use locked view offsets if available (prevents map frame from moving)
        if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
            // Force use locked offsets - map frame MUST stay fixed
            if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 || 
                Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
                console.error('⚠️ CRITICAL ERROR: View offsets changed! Restoring locked offsets.');
                console.error('  Locked:', { offsetX: this.lockedOffsetX, offsetY: this.lockedOffsetY });
                console.error('  Current:', { offsetX: this.offsetX, offsetY: this.offsetY });
                this.offsetX = this.lockedOffsetX;
                this.offsetY = this.lockedOffsetY;
            } else {
                // Even if they match, force use locked offsets to be absolutely sure
                this.offsetX = this.lockedOffsetX;
                this.offsetY = this.lockedOffsetY;
            }
        }
        
        // Update robot pose (ONLY this changes - map frame stays fixed)
        this.robotPose = poseData;
        
        // CRITICAL: Verify map origin has NOT changed
        if (this.lockedMapOrigin && this.gridMap?.origin) {
            const dx = Math.abs(this.gridMap.origin.x - this.lockedMapOrigin.x);
            const dy = Math.abs(this.gridMap.origin.y - this.lockedMapOrigin.y);
            const dz = Math.abs(this.gridMap.origin.z - this.lockedMapOrigin.z);
            
            if (dx > 0.001 || dy > 0.001 || dz > 0.001) {
                console.error('⚠️ CRITICAL ERROR: Map origin changed when setting robot pose!');
                console.error('  Locked:', this.lockedMapOrigin);
                console.error('  Current:', this.gridMap.origin);
                console.error('  → Forcing use of locked origin!');
                // Force use locked origin
                this.gridMap.origin = {
                    x: this.lockedMapOrigin.x,
                    y: this.lockedMapOrigin.y,
                    z: this.lockedMapOrigin.z
                };
            }
        }
        
        this.requestRender();
    },
    
    /**
     * Set laser scan data for all LIDARs
     */
    setLaserScanData: function(allLidarData) {
        this.laserScanData = allLidarData;
        this.requestRender();
    },

    /**
     * Batch update pose + laser in one call to reduce round-trips and render once (smoother updates).
     */
    updatePoseAndLaser: function(poseData, allLidarData) {
        if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
            if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 ||
                Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
                this.offsetX = this.lockedOffsetX;
                this.offsetY = this.lockedOffsetY;
            }
        }
        if (poseData) this.robotPose = poseData;
        if (allLidarData) this.laserScanData = allLidarData;
        if (this.lockedMapOrigin && this.gridMap && this.gridMap.origin) {
            const dx = Math.abs(this.gridMap.origin.x - this.lockedMapOrigin.x);
            const dy = Math.abs(this.gridMap.origin.y - this.lockedMapOrigin.y);
            const dz = Math.abs(this.gridMap.origin.z - this.lockedMapOrigin.z);
            if (dx > 0.001 || dy > 0.001 || dz > 0.001) {
                this.gridMap.origin = { x: this.lockedMapOrigin.x, y: this.lockedMapOrigin.y, z: this.lockedMapOrigin.z };
            }
        }
        this.requestRender();
    },

    setLidarDisplayOptions: function(options) {
        if (!options || typeof options !== 'object') {
            return;
        }

        const normalizeMode = (mode) => (mode === 'full' ? 'full' : 'minimal');
        const normalizeOne = (source, fallback) => ({
            visible: source && typeof source.visible === 'boolean' ? source.visible : fallback.visible,
            mode: normalizeMode(source && source.mode)
        });

        this.lidarDisplayOptions = {
            lidar1: normalizeOne(options.lidar1, this.lidarDisplayOptions.lidar1),
            lidar2: normalizeOne(options.lidar2, this.lidarDisplayOptions.lidar2),
            lidar3: normalizeOne(options.lidar3, this.lidarDisplayOptions.lidar3)
        };

        this.requestRender();
    },

    setGlobalPathVisible: function(visible) {
        this.showGlobalPath = !!visible;
        console.log(`🔄 Global Path visibility: ${this.showGlobalPath ? 'ENABLED' : 'DISABLED'}`);
        this.requestRender();
    },

    /**
     * Explicitly override the locked map origin (used when the user changes
     * the map origin via the Change Map Origin UI). This bypasses the
     * "locked origin" guard so that the next map reload can apply the new origin.
     */
    setMapOrigin: function(originX, originY, originZ) {
        const newOrigin = {
            x: Number(originX),
            y: Number(originY),
            z: Number(originZ)
        };
        if (!Number.isFinite(newOrigin.x) || !Number.isFinite(newOrigin.y) || !Number.isFinite(newOrigin.z)) {
            console.warn('[setMapOrigin] Invalid origin values, ignored', newOrigin);
            return;
        }
        console.log(`[setMapOrigin] Updating locked origin -> (${newOrigin.x.toFixed(3)}, ${newOrigin.y.toFixed(3)}, ${newOrigin.z.toFixed(3)})`);
        this.lockedMapOrigin = newOrigin;
        if (this.gridMap) {
            this.gridMap.origin = { x: newOrigin.x, y: newOrigin.y, z: newOrigin.z };
        }
        this.requestRender();
    },

    /**
     * Clear the locked origin so the next setGridMapWithOrigin call re-locks it.
     */
    clearMapOriginLock: function() {
        console.log('[clearMapOriginLock] Locked origin cleared; next map load will relock.');
        this.lockedMapOrigin = null;
    },

    setLocalPathVisible: function(visible) {
        this.showLocalPath = !!visible;
        console.log(`🔄 Local Path visibility: ${this.showLocalPath ? 'ENABLED' : 'DISABLED'}`);
        this.requestRender();
    },

    setGlobalPathDataJson: function(jsonString) {
        console.log('🔍 setGlobalPathDataJson called with JSON string of length:', jsonString ? jsonString.length : 'null');
        
        if (!jsonString) {
            console.warn('⚠️ Global Path: jsonString is null/undefined');
            this.globalPathData = [];
            this.requestRender();
            return;
        }
        
        try {
            const pathPoints = JSON.parse(jsonString);
            console.log('✅ JSON parsed successfully:', pathPoints);
            this.setGlobalPathData(pathPoints);
        } catch (error) {
            console.error('❌ Error parsing JSON:', error);
            console.error('   Raw string:', jsonString);
            this.globalPathData = [];
            this.requestRender();
        }
    },

    setLocalPathDataJson: function(jsonString) {
        if (!jsonString) {
            this.localPathData = [];
            this.requestRender();
            return;
        }

        try {
            const pathPoints = JSON.parse(jsonString);
            this.setLocalPathData(pathPoints);
        } catch (error) {
            console.error('❌ Local Path: Error parsing JSON:', error);
            this.localPathData = [];
            this.requestRender();
        }
    },

    setGlobalPathData: function(pathPoints) {
        console.log('\ud83d\udd0d setGlobalPathData called with:', pathPoints);
        console.log('   Type:', typeof pathPoints);
        console.log('   Is Array:', Array.isArray(pathPoints));
        console.log('   Length:', pathPoints ? pathPoints.length : 'N/A');
        
        if (!pathPoints) {
            console.warn('\u274c Global Path: pathPoints is null/undefined');
            this.globalPathData = [];
            this.requestRender();
            return;
        }
        
        if (!Array.isArray(pathPoints)) {
            console.warn('\u274c Global Path: pathPoints is not an array, type:', typeof pathPoints);
            console.warn('   Value:', pathPoints);
            this.globalPathData = [];
            this.requestRender();
            return;
        }
        
        if (pathPoints.length === 0) {
            console.warn('\u26a0\ufe0f Global Path: Received empty array (0 points)');
            this.globalPathData = [];
            this.requestRender();
            return;
        }
        
        console.log(`\ud83d\udd0d Raw data sample (first 3 points):`);
        for (let i = 0; i < Math.min(3, pathPoints.length); i++) {
            console.log(`   Point ${i}:`, pathPoints[i]);
        }

        const mappedPoints = pathPoints.map((point, idx) => {
            const mapped = {
                x: Number(point?.x ?? point?.X ?? 0),
                y: Number(point?.y ?? point?.Y ?? 0),
                theta: Number(point?.theta ?? point?.Theta ?? 0)
            };
            if (idx < 3) {
                console.log(`   Mapped ${idx}:`, mapped, '(finite:', Number.isFinite(mapped.x) && Number.isFinite(mapped.y), ')');
            }
            return mapped;
        });
        
        this.globalPathData = mappedPoints.filter(point => Number.isFinite(point.x) && Number.isFinite(point.y));

        console.log(`\u2705 Global Path: Processed ${this.globalPathData.length}/${pathPoints.length} valid points`);
        if (this.globalPathData.length > 0) {
            const first = this.globalPathData[0];
            const last = this.globalPathData[this.globalPathData.length - 1];
            console.log(`   First point: (${first.x.toFixed(2)}, ${first.y.toFixed(2)})`);
            console.log(`   Last point: (${last.x.toFixed(2)}, ${last.y.toFixed(2)})`);
        } else {
            console.warn('\u26a0\ufe0f All points were filtered out! Check if coordinates are valid.');
        }

        this.requestRender();
    },

    setLocalPathData: function(pathPoints) {
        if (!Array.isArray(pathPoints) || pathPoints.length === 0) {
            this.localPathData = [];
            this.requestRender();
            return;
        }

        const mappedPoints = pathPoints.map((point) => ({
            x: Number(point?.x ?? point?.X ?? 0),
            y: Number(point?.y ?? point?.Y ?? 0),
            theta: Number(point?.theta ?? point?.Theta ?? 0)
        }));

        this.localPathData = mappedPoints.filter(point => Number.isFinite(point.x) && Number.isFinite(point.y));
        this.requestRender();
    },

    /**
     * Debug helper: Get current path rendering status
     */
    getPathDebugInfo: function() {
        const info = {
            showGlobalPath: this.showGlobalPath,
            pathDataExists: !!this.globalPathData,
            pathPointCount: this.globalPathData ? this.globalPathData.length : 0,
            scale: this.scale,
            offsetX: this.offsetX,
            offsetY: this.offsetY,
            canvasSize: this.canvas ? `${this.canvas.width}x${this.canvas.height}` : 'no canvas'
        };
        
        if (this.globalPathData && this.globalPathData.length > 0) {
            info.firstPoint = this.globalPathData[0];
            info.lastPoint = this.globalPathData[this.globalPathData.length - 1];
        }
        
        console.log('🔍 Path Debug Info:', info);
        return info;
    },

    /**
     * Debug helper: Force path render with test data
     */
    testPathRender: function() {
        console.log('🧪 Testing path render with sample data...');
        
        // Use current robot position as starting point if available
        const startX = this.robotPose ? this.robotPose.x : 0;
        const startY = this.robotPose ? this.robotPose.y : 0;
        
        // Create a simple test path (square pattern)
        const testPath = [
            { x: startX, y: startY, theta: 0 },
            { x: startX + 1, y: startY, theta: 0 },
            { x: startX + 1, y: startY + 1, theta: 1.57 },
            { x: startX, y: startY + 1, theta: 3.14 },
            { x: startX, y: startY, theta: -1.57 }
        ];
        
        this.showGlobalPath = true;
        this.setGlobalPathData(testPath);
        console.log('✅ Test path set. Should see green square path on canvas.');
    },
    
    /**
     * Resize canvas to match container
     */
    resize: function() {
        if (!this.canvas) return;
        
        const container = this.canvas.parentElement;
        if (container) {
            this.canvas.width = container.clientWidth;
            this.canvas.height = container.clientHeight;
            this.requestRender();
        }
    },

    requestRender: function() {
        if (this.renderPending) return;
        this.renderPending = true;
        requestAnimationFrame(() => {
            this.renderPending = false;
            this.render();
        });
    },

    /**
     * Force immediate render (for debugging)
     */
    forceRender: function() {
        this.renderPending = false;
        this.render();
    },

    buildMapCache: function() {
        if (!this.gridMap || !this.gridMap.dataBase64) {
            console.warn('[buildMapCache] No grid map data or dataBase64 missing');
            this.mapCacheReady = false;
            return;
        }

        try {
            // Decode base64 data once and cache as a canvas
            const binaryString = atob(this.gridMap.dataBase64);
            const bytes = new Uint8Array(binaryString.length);
            for (let i = 0; i < binaryString.length; i++) {
                bytes[i] = binaryString.charCodeAt(i);
            }

            const width = this.gridMap.width;
            const height = this.gridMap.height;

            // Only log first time or when significant changes
            const shouldLog = !this.mapCacheReady || (width !== this.mapCacheCanvas.width || height !== this.mapCacheCanvas.height);
            if (shouldLog) {
                console.log(`[buildMapCache] Building map cache - Width: ${width}, Height: ${height}, Data length: ${bytes.length}, Resolution: ${this.gridMap.resolution}`);
            }

            if (!this.mapCacheCanvas) {
                this.mapCacheCanvas = document.createElement('canvas');
                this.mapCacheCtx = this.mapCacheCanvas.getContext('2d');
            }

            this.mapCacheCanvas.width = width;
            this.mapCacheCanvas.height = height;

            const imageData = this.mapCacheCtx.createImageData(width, height);

            // Count occupancy values for debugging (only when logging)
            let occupiedCount = 0;
            let freeCount = 0;
            let unknownCount = 0;

            // Optimize: process in batches for better performance
            for (let i = 0; i < bytes.length; i++) {
                // Occupancy values: int8_t from C API
                // -1 (0xFF as unsigned) = Unknown
                // 0 = Free
                // 1-99 = Probability of occupancy (higher = more likely occupied)
                // 100 = Occupied
                // When read as unsigned byte: -1 becomes 255
                const occupancy = bytes[i];
                let r, g, b, a = 0;

                // Render occupied cells: 100 (definitely occupied) or >= 50 (likely occupied)
                // This ensures we see boundaries even if probability is not exactly 100
                if (occupancy === 100 || (occupancy >= 50 && occupancy < 100)) {
                    r = g = b = 0;  // Black
                    a = 255;        // Fully opaque
                    if (shouldLog) {
                        if (occupancy === 100) {
                            occupiedCount++;
                        } else {
                            occupiedCount++; // Count probability cells as occupied too
                        }
                    }
                } else {
                    r = g = b = 0;
                    a = 0;          // Transparent
                    if (shouldLog) {
                        if (occupancy === 0) {
                            freeCount++;
                        } else if (occupancy === 255) {
                            unknownCount++;
                        } else {
                            unknownCount++; // Other values treated as unknown
                        }
                    }
                }

                const idx = i * 4;
                imageData.data[idx] = r;
                imageData.data[idx + 1] = g;
                imageData.data[idx + 2] = b;
                imageData.data[idx + 3] = a;
            }

            if (shouldLog) {
                console.log(`[buildMapCache] Occupancy stats - Occupied: ${occupiedCount}, Free: ${freeCount}, Unknown: ${unknownCount}`);
            }

            this.mapCacheCtx.putImageData(imageData, 0, 0);
            this.mapCacheReady = true;
            console.log('[buildMapCache] Map cache built successfully');
        } catch (error) {
            console.error('[buildMapCache] Error building map cache:', error);
            this.mapCacheReady = false;
        }
    },
    
    /**
     * Main render function - draws everything
     * 
     * CRITICAL: This function should NEVER change map origin or view offsets.
     * Map frame position is calculated from locked origin + current view offsets.
     */
    render: function() {
        if (!this.ctx || !this.canvas) return;
        
        // CRITICAL: Use locked view offsets if available (prevents map frame from moving)
        if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
            // Force use locked offsets - map frame MUST stay fixed
            if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 || 
                Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
                console.error('⚠️ CRITICAL ERROR: View offsets changed at start of render! Restoring locked offsets.');
                console.error('  Locked:', { offsetX: this.lockedOffsetX, offsetY: this.lockedOffsetY });
                console.error('  Current:', { offsetX: this.offsetX, offsetY: this.offsetY });
                this.offsetX = this.lockedOffsetX;
                this.offsetY = this.lockedOffsetY;
            } else {
                // Even if they match, force use locked offsets to be absolutely sure
                this.offsetX = this.lockedOffsetX;
                this.offsetY = this.lockedOffsetY;
            }
        }
        
        // Save view state at start of render
        const renderStartOffsetX = this.offsetX;
        const renderStartOffsetY = this.offsetY;
        const renderStartScale = this.scale;
        
        // Verify map origin hasn't changed during render
        if (this.lockedMapOrigin && this.gridMap?.origin) {
            const dx = Math.abs(this.gridMap.origin.x - this.lockedMapOrigin.x);
            const dy = Math.abs(this.gridMap.origin.y - this.lockedMapOrigin.y);
            const dz = Math.abs(this.gridMap.origin.z - this.lockedMapOrigin.z);
            
            if (dx > 0.001 || dy > 0.001 || dz > 0.001) {
                console.error('⚠️ CRITICAL: Map origin changed during render! Restoring locked origin.');
                console.error('  Locked:', this.lockedMapOrigin);
                console.error('  Current:', this.gridMap.origin);
                this.gridMap.origin = {
                    x: this.lockedMapOrigin.x,
                    y: this.lockedMapOrigin.y,
                    z: this.lockedMapOrigin.z
                };
            }
        }
        
        // Clear canvas - NO white background, only grid will show
        this.ctx.clearRect(0, 0, this.canvas.width, this.canvas.height);
        
        // Draw in order: grid -> grid map -> map frame axes -> cost map -> global path -> laser scans -> initial pose -> robot pose
        // Cost map and path should be drawn BEFORE laser scans so they're visible underneath
        this.drawGrid();
        this.drawGridMap();
        this.drawCoordinateAxes(); // Draw map frame axes (X=red, Y=green) at origin
        this.drawWorldCoordinateAxes(); // Draw world coordinate axes (X=red, Y=green) at origin
        this.drawCostMap(); // Draw cost map overlay
        this.drawGlobalPath(); // Draw path BEFORE laser scans
        this.drawLocalPath(); // Draw local path BEFORE laser scans
        this.drawLaserScans();
        this.drawInitialPose();
        if (this.showRobotPose) {
            this.drawRobotPose();
        }
        this.drawRobotAxes();
        this.drawRobotFootprint();
        
        // CRITICAL: Use locked view offsets if available (prevents map frame from moving)
        if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
            // Force use locked offsets - map frame MUST stay fixed
            if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 || 
                Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
                console.error('⚠️ CRITICAL ERROR: View offsets changed during render!');
                console.error('  Locked:', { offsetX: this.lockedOffsetX, offsetY: this.lockedOffsetY });
                console.error('  Current:', { offsetX: this.offsetX, offsetY: this.offsetY });
                console.error('  Robot pose:', this.robotPose);
                console.error('  → Restoring locked offsets!');
                // Restore locked offsets
                this.offsetX = this.lockedOffsetX;
                this.offsetY = this.lockedOffsetY;
            }
        }
    },
    
    /**
     * Draw background grid (1 meter squares). Grid origin = Map frame = canvas center.
     */
    drawGrid: function() {
        const { mapFrameX, mapFrameY } = this.getFramePositions();
        
        const gridSpacing = this.scale;
        const snapToPixel = (value) => Math.round(value) + 0.5;
        
        // Calculate visible range in map frame coordinates
        // For X: canvas X = mapFrameX + (mapX * scale), so mapX = (canvasX - mapFrameX) / scale
        const minX = (-mapFrameX) / this.scale;
        const maxX = (this.canvas.width - mapFrameX) / this.scale;
        
        // For Y: canvas Y = mapFrameY - (mapY * scale), so mapY = (mapFrameY - canvasY) / scale
        // Top of canvas (y=0) corresponds to: mapY = (mapFrameY - 0) / scale = mapFrameY / scale
        // Bottom of canvas (y=height) corresponds to: mapY = (mapFrameY - height) / scale
        const minY = (mapFrameY - this.canvas.height) / this.scale;
        const maxY = mapFrameY / this.scale;
        
        // Add margin to ensure all visible grid lines are drawn
        const margin = 3; // meters - increased to ensure coverage
        const startGridX = Math.floor(minX - margin);
        const endGridX = Math.ceil(maxX + margin);
        const startGridY = Math.floor(minY - margin);
        const endGridY = Math.ceil(maxY + margin);
        
        // Adjust line width based on zoom level for better visibility
        const baseLineWidth = 1;
        const adjustedLineWidth = Math.max(0.5, Math.min(2, baseLineWidth / (this.scale / 20)));
        
        // Draw vertical lines (1 meter spacing) - relative to World frame
        this.ctx.strokeStyle = '#d0d0d0'; // Darker gray
        this.ctx.lineWidth = adjustedLineWidth;
        this.ctx.globalAlpha = 0.6; // More opaque
        
        for (let x = startGridX; x <= endGridX; x += 1) {
            const canvasX = snapToPixel(mapFrameX + (x * gridSpacing));
            // Use larger margin to ensure lines are drawn even when slightly outside viewport
            if (canvasX >= -100 && canvasX <= this.canvas.width + 100) {
                this.ctx.beginPath();
                this.ctx.moveTo(canvasX, 0);
                this.ctx.lineTo(canvasX, this.canvas.height);
                this.ctx.stroke();
            }
        }
        
        // Draw horizontal lines (1 meter spacing) - relative to World frame
        // Important: y increases upward in map frame, but canvas Y increases downward
        // So for map frame y, canvas Y = mapFrameY - (y * gridSpacing)
        for (let y = startGridY; y <= endGridY; y += 1) {
            const canvasY = snapToPixel(mapFrameY - (y * gridSpacing));
            // Use much larger margin and remove strict bounds checking to ensure all lines are drawn
            // The key is to draw all lines in the calculated range, regardless of exact canvas position
            if (!isNaN(canvasY) && isFinite(canvasY)) {
                this.ctx.beginPath();
                this.ctx.moveTo(0, canvasY);
                this.ctx.lineTo(this.canvas.width, canvasY);
                this.ctx.stroke();
            }
        }
        
        // Draw 5-meter grid lines (darker and more visible)
        this.ctx.strokeStyle = '#b0b0b0'; // Even darker
        this.ctx.lineWidth = Math.max(1, adjustedLineWidth * 2);
        this.ctx.globalAlpha = 0.7;
        
        for (let x = startGridX; x <= endGridX; x += 1) {
            if (x % 5 === 0) {
                const canvasX = snapToPixel(mapFrameX + (x * gridSpacing));
                if (canvasX >= -100 && canvasX <= this.canvas.width + 100) {
                    this.ctx.beginPath();
                    this.ctx.moveTo(canvasX, 0);
                    this.ctx.lineTo(canvasX, this.canvas.height);
                    this.ctx.stroke();
                }
            }
        }
        
        for (let y = startGridY; y <= endGridY; y += 1) {
            if (y % 5 === 0) {
                const canvasY = snapToPixel(mapFrameY - (y * gridSpacing));
                // Remove bounds checking for 5-meter lines too - just check for valid number
                if (!isNaN(canvasY) && isFinite(canvasY)) {
                    this.ctx.beginPath();
                    this.ctx.moveTo(0, canvasY);
                    this.ctx.lineTo(this.canvas.width, canvasY);
                    this.ctx.stroke();
                }
            }
        }
        
        // Reset alpha for other drawing
        this.ctx.globalAlpha = 1.0;
    },
    
    /**
     * Draw the occupancy grid map - only renders occupied cells (100) as black boundaries
     */
    drawGridMap: function() {
        if (!this.gridMap) {
            console.debug('[drawGridMap] No gridMap data');
            return;
        }
        if (!this.mapCacheReady) {
            console.debug('[drawGridMap] Map cache not ready');
            return;
        }
        if (!this.mapCacheCanvas) {
            console.debug('[drawGridMap] Map cache canvas not available');
            return;
        }
        
        try {
            const width = this.gridMap.width;
            const height = this.gridMap.height;
            const resolution = this.gridMap.resolution;
            
            // Map frame (0,0) = canvas center
            // Origin is the position of cell [0,0] (bottom-left corner) in map frame coordinates
            // In ROS occupancy grid: origin is the world position of the bottom-left corner of cell [0,0]
            const { mapFrameX, mapFrameY } = this.getFramePositions();
            const originXY = this.getMapOriginXY();
            
            // Calculate where cell [0,0] should be drawn in canvas coordinates
            // Map frame origin (0,0) is at (mapFrameX, mapFrameY) in canvas
            // Cell [0,0] is at (origin.x, origin.y) in map frame
            // Canvas coordinates: mapFrameX + origin.x * scale, mapFrameY - origin.y * scale (flip Y)
            const cell00CanvasX = mapFrameX + (originXY.x * this.scale);
            const cell00CanvasY = mapFrameY - (originXY.y * this.scale);
            
            const mapWidthPixels = width * resolution * this.scale;
            const mapHeightPixels = height * resolution * this.scale;
            
            this.ctx.save();
            // Translate to where cell [0,0] should be drawn
            this.ctx.translate(cell00CanvasX, cell00CanvasY);
            // Flip Y axis (map Y increases upward, canvas Y increases downward)
            // After flip, cell [0,0] is at bottom-left, map extends right and up
            this.ctx.scale(1, -1);
            // Draw map: cell (0,0) will be at the translate point after flip
            // Map extends rightward (positive X) and upward (positive Y in map frame, downward in canvas after flip)
            this.ctx.drawImage(this.mapCacheCanvas, 0, 0, mapWidthPixels, mapHeightPixels);
            this.ctx.restore();
            
        } catch (error) {
            console.error('[drawGridMap] Error drawing grid map:', error);
        }
    },
    
    /**
     * Draw robot pose icon (position and orientation) as a translucent purple arrowhead.
     */
    drawRobotPose: function() {
        if (!this.robotPose) return;
        
        const { mapFrameX, mapFrameY } = this.getFramePositions();
        // Robot pose in MAP coordinates -> canvas = mapFrame + (robot.x*scale, -robot.y*scale)
        const robotX = mapFrameX + (this.robotPose.x * this.scale);
        const robotY = mapFrameY - (this.robotPose.y * this.scale);
        
        const arrowLength = 35;
        const headAngle = Math.PI / 6;
        const angle = -this.robotPose.yaw;
        const arrowTipX = robotX + arrowLength * Math.cos(angle);
        const arrowTipY = robotY + arrowLength * Math.sin(angle);
        
        this.ctx.fillStyle = 'rgba(156, 39, 176, 0.45)';
        this.ctx.strokeStyle = 'rgba(106, 27, 154, 0.85)';
        this.ctx.lineWidth = 3;
        
        this.ctx.beginPath();
        this.ctx.moveTo(arrowTipX, arrowTipY);
        this.ctx.lineTo(
            robotX - arrowLength * 0.5 * Math.cos(angle - headAngle),
            robotY - arrowLength * 0.5 * Math.sin(angle - headAngle)
        );
        this.ctx.lineTo(
            robotX - arrowLength * 0.5 * Math.cos(angle + headAngle),
            robotY - arrowLength * 0.5 * Math.sin(angle + headAngle)
        );
        this.ctx.closePath();
        this.ctx.fill();
        this.ctx.stroke();
    },

    /**
     * Draw robot-local coordinate axes independently from the robot icon toggle.
     */
    drawRobotAxes: function() {
        if (!this.robotPose) return;

        const { mapFrameX, mapFrameY } = this.getFramePositions();
        const robotX = mapFrameX + (this.robotPose.x * this.scale);
        const robotY = mapFrameY - (this.robotPose.y * this.scale);
        this.drawRobotCoordinateAxes(robotX, robotY, this.robotPose.yaw || 0);
    },

    /**
     * Draw robot-local coordinate axes (x: red, y: green) at robot center.
     */
    drawRobotCoordinateAxes: function(robotX, robotY, yaw) {
        const axisLength = 42;
        const arrowHeadSize = 8;

        // Convert robot yaw (map convention) to canvas angle convention.
        const xAxisAngle = -yaw;
        const yAxisAngle = xAxisAngle - Math.PI / 2;

        const xEndX = robotX + axisLength * Math.cos(xAxisAngle);
        const xEndY = robotY + axisLength * Math.sin(xAxisAngle);
        const yEndX = robotX + axisLength * Math.cos(yAxisAngle);
        const yEndY = robotY + axisLength * Math.sin(yAxisAngle);

        // X axis
        this.ctx.strokeStyle = '#F44336';
        this.ctx.lineWidth = 2.5;
        this.ctx.beginPath();
        this.ctx.moveTo(robotX, robotY);
        this.ctx.lineTo(xEndX, xEndY);
        this.ctx.stroke();

        this.ctx.fillStyle = '#F44336';
        this.ctx.beginPath();
        this.ctx.moveTo(xEndX, xEndY);
        this.ctx.lineTo(
            xEndX - arrowHeadSize * Math.cos(xAxisAngle - Math.PI / 6),
            xEndY - arrowHeadSize * Math.sin(xAxisAngle - Math.PI / 6)
        );
        this.ctx.lineTo(
            xEndX - arrowHeadSize * Math.cos(xAxisAngle + Math.PI / 6),
            xEndY - arrowHeadSize * Math.sin(xAxisAngle + Math.PI / 6)
        );
        this.ctx.closePath();
        this.ctx.fill();

        // Y axis
        this.ctx.strokeStyle = '#4CAF50';
        this.ctx.lineWidth = 2.5;
        this.ctx.beginPath();
        this.ctx.moveTo(robotX, robotY);
        this.ctx.lineTo(yEndX, yEndY);
        this.ctx.stroke();

        this.ctx.fillStyle = '#4CAF50';
        this.ctx.beginPath();
        this.ctx.moveTo(yEndX, yEndY);
        this.ctx.lineTo(
            yEndX - arrowHeadSize * Math.cos(yAxisAngle - Math.PI / 6),
            yEndY - arrowHeadSize * Math.sin(yAxisAngle - Math.PI / 6)
        );
        this.ctx.lineTo(
            yEndX - arrowHeadSize * Math.cos(yAxisAngle + Math.PI / 6),
            yEndY - arrowHeadSize * Math.sin(yAxisAngle + Math.PI / 6)
        );
        this.ctx.closePath();
        this.ctx.fill();

        // Axis labels
        this.ctx.font = 'bold 12px Arial';
        this.ctx.fillStyle = '#F44336';
        this.ctx.fillText('x', xEndX + 6, xEndY + 4);
        this.ctx.fillStyle = '#4CAF50';
        this.ctx.fillText('y', yEndX + 6, yEndY + 4);
    },

    /**
     * Draw robot footprint (base outline) at current robot pose.
     * Transforms footprint points from robot frame to map frame.
     */
    drawRobotFootprint: function() {
        if (!this.showRobotFootprint || !this.robotPose || !this.robotFootprint || this.robotFootprint.length === 0) {
            return;
        }

        const { mapFrameX, mapFrameY } = this.getFramePositions();
        const robotX = this.robotPose.x;
        const robotY = this.robotPose.y;
        const yaw = this.robotPose.yaw || 0;

        const cos = Math.cos(yaw);
        const sin = Math.sin(yaw);

        // Transform footprint points from robot frame to map frame
        const transformedPoints = this.robotFootprint.map(point => {
            // Rotate and translate point
            const rotatedX = point.x * cos - point.y * sin;
            const rotatedY = point.x * sin + point.y * cos;
            const mapX = robotX + rotatedX;
            const mapY = robotY + rotatedY;

            // Convert to canvas coordinates
            const canvasX = mapFrameX + (mapX * this.scale);
            const canvasY = mapFrameY - (mapY * this.scale);
            return { x: canvasX, y: canvasY, origX: point.x, origY: point.y };
        });

        // Sort points by angle from center to ensure correct square drawing order
        if (transformedPoints.length >= 4) {
            // Calculate center of footprint in robot frame
            const centerX = this.robotFootprint.reduce((sum, p) => sum + p.x, 0) / this.robotFootprint.length;
            const centerY = this.robotFootprint.reduce((sum, p) => sum + p.y, 0) / this.robotFootprint.length;
            
            // Sort by angle from center (counterclockwise)
            transformedPoints.sort((a, b) => {
                const angleA = Math.atan2(a.origY - centerY, a.origX - centerX);
                const angleB = Math.atan2(b.origY - centerY, b.origX - centerX);
                return angleA - angleB;
            });
        }

        // Draw footprint as a square
        this.ctx.fillStyle = 'rgba(0, 188, 212, 0.2)'; // Cyan with transparency
        this.ctx.strokeStyle = '#00bcd4'; // Cyan color
        this.ctx.lineWidth = 2;
        this.ctx.beginPath();
        
        // Draw square by connecting all points in sorted order
        if (transformedPoints.length >= 4) {
            this.ctx.moveTo(transformedPoints[0].x, transformedPoints[0].y);
            for (let i = 1; i < transformedPoints.length; i++) {
                this.ctx.lineTo(transformedPoints[i].x, transformedPoints[i].y);
            }
            this.ctx.closePath();
            this.ctx.fill();
            this.ctx.stroke();
        }
    },

    /**
     * Draw initial pose (position + heading) as a bright blue arrow
     */
    drawInitialPose: function() {
        // Only draw if mode is enabled
        if (!this.initialPoseMode) return;
        
        // If no initial pose exists, don't draw (should not happen if mode is enabled)
        if (!this.initialPose) {
            console.warn("Initial pose mode is ON but no initial pose exists");
            return;
        }
        
        const { mapFrameX, mapFrameY } = this.getFramePositions();
        const baseX = mapFrameX + (this.initialPose.x * this.scale);
        const baseY = mapFrameY - (this.initialPose.y * this.scale);

        const arrowLength = 50; // Larger arrow
        const headAngle = Math.PI / 6;
        const angle = -this.initialPose.yaw;

        const tipX = baseX + arrowLength * Math.cos(angle);
        const tipY = baseY + arrowLength * Math.sin(angle);

        // Draw a glowing effect (outer glow) - more visible when dragging
        if (this.initialPoseDragging) {
            this.ctx.shadowColor = 'rgba(33, 150, 243, 1.0)';
            this.ctx.shadowBlur = 20;
        } else {
            this.ctx.shadowColor = 'rgba(33, 150, 243, 0.8)';
            this.ctx.shadowBlur = 15;
        }

        // Base marker (larger and bright blue)
        this.ctx.fillStyle = '#2196F3'; // Material Blue
        this.ctx.strokeStyle = '#1976D2';
        this.ctx.lineWidth = this.initialPoseDragging ? 4 : 3;
        this.ctx.beginPath();
        this.ctx.arc(baseX, baseY, this.initialPoseDragging ? 10 : 8, 0, 2 * Math.PI);
        this.ctx.fill();
        this.ctx.stroke();

        // Arrow shaft (thicker)
        this.ctx.strokeStyle = '#2196F3';
        this.ctx.lineWidth = this.initialPoseDragging ? 5 : 4;
        this.ctx.beginPath();
        this.ctx.moveTo(baseX, baseY);
        this.ctx.lineTo(tipX, tipY);
        this.ctx.stroke();

        // Arrow head (larger)
        this.ctx.fillStyle = '#2196F3';
        this.ctx.strokeStyle = '#1976D2';
        this.ctx.lineWidth = this.initialPoseDragging ? 3 : 2;
        this.ctx.beginPath();
        this.ctx.moveTo(tipX, tipY);
        this.ctx.lineTo(
            tipX - arrowLength * 0.5 * Math.cos(angle - headAngle),
            tipY - arrowLength * 0.5 * Math.sin(angle - headAngle)
        );
        this.ctx.lineTo(
            tipX - arrowLength * 0.5 * Math.cos(angle + headAngle),
            tipY - arrowLength * 0.5 * Math.sin(angle + headAngle)
        );
        this.ctx.closePath();
        this.ctx.fill();
        this.ctx.stroke();

        // Reset shadow
        this.ctx.shadowBlur = 0;
    },
    
    /**
     * Draw laser scan points from all LIDARs
     */
    drawLaserScans: function() {
        if (!this.laserScanData) return;

        const lidar1Options = this.lidarDisplayOptions?.lidar1 || { visible: true, mode: 'minimal' };
        const lidar2Options = this.lidarDisplayOptions?.lidar2 || { visible: true, mode: 'minimal' };
        const lidar3Options = this.lidarDisplayOptions?.lidar3 || { visible: true, mode: 'minimal' };
        
        // Draw each LIDAR's scan with different colors (darker for white background)
        if (lidar1Options.visible && this.laserScanData.lidar1) {
            this.drawSingleLidarScan(this.laserScanData.lidar1, '#cc0000', this.getLidarTransform('scan_1'), lidar1Options.mode); // Dark Red
        }
        if (lidar2Options.visible && this.laserScanData.lidar2) {
            this.drawSingleLidarScan(this.laserScanData.lidar2, '#00aa00', this.getLidarTransform('scan_2'), lidar2Options.mode); // Dark Green
        }
        if (lidar3Options.visible && this.laserScanData.lidar3) {
            this.drawSingleLidarScan(this.laserScanData.lidar3, '#0000cc', this.getLidarTransform('scan_3'), lidar3Options.mode); // Dark Blue
        }
    },

    drawGlobalPath: function() {
        if (!this.showGlobalPath) return;
        if (!this.globalPathData || this.globalPathData.length === 0) return;

        const { mapFrameX, mapFrameY } = this.getFramePositions();
        
        const pathPoints = this.globalPathData.map(point => ({
            x: mapFrameX + (point.x * this.scale),
            y: mapFrameY - (point.y * this.scale)
        }));

        // Check if any points are visible
        const visiblePoints = pathPoints.filter(p =>
            p.x >= 0 && p.x <= this.canvas.width &&
            p.y >= 0 && p.y <= this.canvas.height
        );

        if (visiblePoints.length === 0) {
            console.warn('Global path: no points visible in current view (pan/zoom or path off-screen).');
        }

        this.ctx.save();

        // Draw path line - RViz style with green color
        if (pathPoints.length > 1) {
            // Background white outline for visibility
            this.ctx.beginPath();
            this.ctx.moveTo(pathPoints[0].x, pathPoints[0].y);
            for (let i = 1; i < pathPoints.length; i++) {
                this.ctx.lineTo(pathPoints[i].x, pathPoints[i].y);
            }
            this.ctx.strokeStyle = '#FFFFFF';
            this.ctx.lineWidth = 8;
            this.ctx.lineJoin = 'round';
            this.ctx.lineCap = 'round';
            this.ctx.globalAlpha = 0.9;
            this.ctx.stroke();

            // Main path line - bright green like RViz
            this.ctx.beginPath();
            this.ctx.moveTo(pathPoints[0].x, pathPoints[0].y);
            for (let i = 1; i < pathPoints.length; i++) {
                this.ctx.lineTo(pathPoints[i].x, pathPoints[i].y);
            }
            this.ctx.strokeStyle = '#00FF00'; // Bright green like RViz
            this.ctx.lineWidth = 4;
            this.ctx.globalAlpha = 0.85;
            this.ctx.stroke();
        }

        // Draw path points for short paths or better visibility
        this.ctx.globalAlpha = 0.8;
        const pointStep = Math.max(1, Math.floor(pathPoints.length / 50));
        this.ctx.fillStyle = '#00FF00';
        for (let i = 0; i < pathPoints.length; i += pointStep) {
            const point = pathPoints[i];
            this.ctx.beginPath();
            this.ctx.arc(point.x, point.y, 3, 0, 2 * Math.PI);
            this.ctx.fill();
        }

        // Directional arrows along the path (heading indicators)
        if (pathPoints.length > 1) {
            const arrowSpacingPx = Math.max(30, this.scale * 0.8); // ~1 arrow per 0.8 m (min 30px)
            const arrowSize      = Math.max(5, Math.min(12, this.scale * 0.18));

            this.ctx.globalAlpha = 0.95;
            this.ctx.fillStyle   = '#FF0000';
            this.ctx.strokeStyle = '#FF0000';
            this.ctx.lineWidth   = 1;

            let distAccum = arrowSpacingPx * 0.5; // first arrow offset

            for (let i = 0; i < pathPoints.length - 1; i++) {
                const p0 = pathPoints[i];
                const p1 = pathPoints[i + 1];
                const dx = p1.x - p0.x;
                const dy = p1.y - p0.y;
                const segLen = Math.sqrt(dx * dx + dy * dy);
                if (segLen < 1e-6) continue;

                let remaining = segLen;
                let traveled  = 0;

                while (distAccum <= remaining) {
                    const t  = (traveled + distAccum) / segLen;
                    const px = p0.x + dx * t;
                    const py = p0.y + dy * t;

                    // Prefer theta from path data (world frame -> flip Y for screen)
                    const worldTheta = this.globalPathData[i]?.theta;
                    const angle = (worldTheta !== undefined && isFinite(worldTheta))
                        ? -worldTheta
                        : Math.atan2(dy, dx);

                    const L = arrowSize;
                    const W = arrowSize * 0.55;

                    this.ctx.save();
                    this.ctx.translate(px, py);
                    this.ctx.rotate(angle);

                    this.ctx.beginPath();
                    this.ctx.moveTo( L,        0);     // tip
                    this.ctx.lineTo(-L * 0.6, -W);     // left wing
                    this.ctx.lineTo(-L * 0.25, 0);     // tail notch
                    this.ctx.lineTo(-L * 0.6,  W);     // right wing
                    this.ctx.closePath();
                    this.ctx.fill();
                    this.ctx.stroke();

                    this.ctx.restore();

                    traveled  += distAccum;
                    remaining -= distAccum;
                    distAccum  = arrowSpacingPx;
                }

                distAccum -= remaining;
            }
        }

        // Start marker - bright green circle
        const firstPoint = pathPoints[0];
        this.ctx.globalAlpha = 1.0;
        this.ctx.fillStyle = '#FFFFFF';
        this.ctx.beginPath();
        this.ctx.arc(firstPoint.x, firstPoint.y, 8, 0, 2 * Math.PI);
        this.ctx.fill();
        this.ctx.fillStyle = '#00FF00';
        this.ctx.beginPath();
        this.ctx.arc(firstPoint.x, firstPoint.y, 6, 0, 2 * Math.PI);
        this.ctx.fill();

        // End marker - bright red circle
        const lastPoint = pathPoints[pathPoints.length - 1];
        this.ctx.fillStyle = '#FFFFFF';
        this.ctx.beginPath();
        this.ctx.arc(lastPoint.x, lastPoint.y, 8, 0, 2 * Math.PI);
        this.ctx.fill();
        this.ctx.fillStyle = '#FF0000';
        this.ctx.beginPath();
        this.ctx.arc(lastPoint.x, lastPoint.y, 6, 0, 2 * Math.PI);
        this.ctx.fill();

        this.ctx.restore();
    },

    drawLocalPath: function() {
        if (!this.showLocalPath) return;
        if (!this.localPathData || this.localPathData.length < 2) return;

        const { mapFrameX, mapFrameY } = this.getFramePositions();
        const pathPoints = this.localPathData.map(point => ({
            x: mapFrameX + (point.x * this.scale),
            y: mapFrameY - (point.y * this.scale)
        }));

        this.ctx.save();
        // White outline for contrast
        this.ctx.beginPath();
        this.ctx.moveTo(pathPoints[0].x, pathPoints[0].y);
        for (let i = 1; i < pathPoints.length; i++) {
            this.ctx.lineTo(pathPoints[i].x, pathPoints[i].y);
        }
        this.ctx.strokeStyle = '#FFFFFF';
        this.ctx.lineWidth = 6;
        this.ctx.lineJoin = 'round';
        this.ctx.lineCap = 'round';
        this.ctx.globalAlpha = 0.85;
        this.ctx.stroke();

        // Main local path line (orange)
        this.ctx.beginPath();
        this.ctx.moveTo(pathPoints[0].x, pathPoints[0].y);
        for (let i = 1; i < pathPoints.length; i++) {
            this.ctx.lineTo(pathPoints[i].x, pathPoints[i].y);
        }
        this.ctx.strokeStyle = '#ff9800';
        this.ctx.lineWidth = 3;
        this.ctx.globalAlpha = 0.9;
        this.ctx.stroke();

        // Directional arrows along the local path
        if (pathPoints.length > 1) {
            const arrowSpacingPx = Math.max(25, this.scale * 0.5);
            const arrowSize      = Math.max(4, Math.min(10, this.scale * 0.15));

            this.ctx.globalAlpha = 0.95;
            this.ctx.fillStyle   = '#ff9800';
            this.ctx.strokeStyle = '#b26a00';
            this.ctx.lineWidth   = 1;

            let distAccum = arrowSpacingPx * 0.5;

            for (let i = 0; i < pathPoints.length - 1; i++) {
                const p0 = pathPoints[i];
                const p1 = pathPoints[i + 1];
                const dx = p1.x - p0.x;
                const dy = p1.y - p0.y;
                const segLen = Math.sqrt(dx * dx + dy * dy);
                if (segLen < 1e-6) continue;

                let remaining = segLen;
                let traveled  = 0;

                while (distAccum <= remaining) {
                    const t  = (traveled + distAccum) / segLen;
                    const px = p0.x + dx * t;
                    const py = p0.y + dy * t;

                    const worldTheta = this.localPathData[i]?.theta;
                    const angle = (worldTheta !== undefined && isFinite(worldTheta))
                        ? -worldTheta
                        : Math.atan2(dy, dx);

                    const L = arrowSize;
                    const W = arrowSize * 0.55;

                    this.ctx.save();
                    this.ctx.translate(px, py);
                    this.ctx.rotate(angle);

                    this.ctx.beginPath();
                    this.ctx.moveTo( L,        0);
                    this.ctx.lineTo(-L * 0.6, -W);
                    this.ctx.lineTo(-L * 0.25, 0);
                    this.ctx.lineTo(-L * 0.6,  W);
                    this.ctx.closePath();
                    this.ctx.fill();
                    this.ctx.stroke();

                    this.ctx.restore();

                    traveled  += distAccum;
                    remaining -= distAccum;
                    distAccum  = arrowSpacingPx;
                }

                distAccum -= remaining;
            }
        }

        this.ctx.restore();
    },

    // ==================== COST MAP FUNCTIONS ====================
    
    setCostMapVisible: function(visible) {
        this.showCostMap = !!visible;
        console.log(`🔄 Cost Map visibility: ${this.showCostMap ? 'ENABLED' : 'DISABLED'}`);
        this.requestRender();
    },

    setRobotPoseVisible: function(visible) {
        this.showRobotPose = !!visible;
        console.log(`🔄 Robot icon visibility: ${this.showRobotPose ? 'ENABLED' : 'DISABLED'}`);
        this.requestRender();
    },

    setRobotFootprint: function(footprintJson) {
        try {
            const footprintData = typeof footprintJson === 'string' ? JSON.parse(footprintJson) : footprintJson;
            this.robotFootprint = footprintData;
            console.log('📐 Robot footprint set:', this.robotFootprint);
            this.requestRender();
        } catch (error) {
            console.error('❌ Failed to parse robot footprint:', error);
            this.robotFootprint = null;
        }
    },

    setRobotFootprintVisible: function(visible) {
        this.showRobotFootprint = !!visible;
        console.log(`🔄 Robot footprint visibility: ${this.showRobotFootprint ? 'ENABLED' : 'DISABLED'}`);
        this.requestRender();
    },

    setCostMapData: function(data) {
        if (!data) {
            return;
        }

        try {
            const hasFullMap = data.hasFullMap === true || (!!data.dataBase64 && data.width > 0 && data.height > 0);
            const isCostmapUpdated = data.isCostmapUpdated === true || !!data.costmapUpdate;

            if (!hasFullMap && !isCostmapUpdated) {
                return;
            }

            if (hasFullMap) {
                const width = Number(data.width) || 0;
                const height = Number(data.height) || 0;
                const resolution = Number(data.resolution) || 0;
                const originX = Number(data.originX) || 0;
                const originY = Number(data.originY) || 0;
                const originTheta = Number(data.originTheta) || 0;
                if (width <= 0 || height <= 0 || resolution <= 0) {
                    console.warn('[setCostMapData] Invalid full costmap payload');
                    return;
                }

                const fullData = this.decodeCostMapBase64(data.dataBase64);
                if (!fullData || fullData.length === 0) {
                    console.warn('[setCostMapData] Missing full costmap bytes');
                    return;
                }
                const hasExistingSnapshot = !!(this.costMapData && this.costMapData.data);
                const dimensionsChanged = !hasExistingSnapshot ||
                    this.costMapData.width !== width ||
                    this.costMapData.height !== height ||
                    Math.abs(this.costMapData.resolution - resolution) > 1e-9;

                // Always update costmap data for continuous updates (lidar frequency)
                this.costMapData = {
                    frameId: data.frameId || 'map',
                    width: width,
                    height: height,
                    resolution: resolution,
                    originX: originX,
                    originY: originY,
                    originTheta: originTheta,
                    poseX: dimensionsChanged ? originX : this.costMapData.poseX,
                    poseY: dimensionsChanged ? originY : this.costMapData.poseY,
                    fixedYaw: dimensionsChanged 
                        ? (Number.isFinite(this.costMapFixedYaw) ? this.costMapFixedYaw : originTheta)
                        : this.costMapData.fixedYaw,
                    data: fullData
                };

                if (dimensionsChanged) {
                    this.costMapPoseAnchor = { x: originX, y: originY };
                    this.costMapOdomAnchor = null;
                }
                
                this.rebuildCostMapCache();
            }

            if (this.costMapData && isCostmapUpdated) {
                this.applyCostMapUpdate(this.costMapData, data.costmapUpdate);
            }

            this.updateCostMapPoseFromOdometry(
                data.odometry,
                data.localizationActive === true,
                data.xlocPose,
                Number(data.matchingScore)
            );

            if (this.costMapData && Number.isFinite(this.costMapFixedYaw)) {
                this.costMapData.fixedYaw = this.costMapFixedYaw;
            }

            if (data.xlocPose && Number(data.matchingScore) !== -1) {
                this.setRobotPose(data.xlocPose);
            }

            this.requestRender();
        } catch (error) {
            console.error('❌ Error processing cost map data:', error);
        }
    },

    decodeCostMapBase64: function(dataBase64) {
        if (!dataBase64) {
            return null;
        }
        const binaryString = atob(dataBase64);
        const bytes = new Uint8Array(binaryString.length);
        for (let i = 0; i < binaryString.length; i++) {
            bytes[i] = binaryString.charCodeAt(i);
        }
        return bytes;
    },

    rebuildCostMapCache: function() {
        if (!this.costMapData || !this.costMapData.data) {
            this.costMapCacheReady = false;
            return;
        }

        const { width, height, data } = this.costMapData;

        if (!this.costMapCacheCanvas) {
            this.costMapCacheCanvas = document.createElement('canvas');
            this.costMapCacheCtx = this.costMapCacheCanvas.getContext('2d');
        }

        // Rotate 90° clockwise: swap width and height
        this.costMapCacheCanvas.width = height;
        this.costMapCacheCanvas.height = width;

        const imageData = this.costMapCacheCtx.createImageData(height, width);
        const pixels = imageData.data;

        // Rotate 90° clockwise: original (x, y) -> rotated (height - 1 - y, x)
        for (let y = 0; y < height; y++) {
            for (let x = 0; x < width; x++) {
                const srcIdx = y * width + x;
                const costValue = data[srcIdx];
                
                // Rotated coordinates
                const rotX = height - 1 - y;
                const rotY = x;
                const dstIdx = rotY * height + rotX;
                const pxIdx = dstIdx * 4;

                // Unknown and free space should stay transparent so inflation gradient is visible.
                if (costValue === 255 || costValue === 0) {
                    pixels[pxIdx] = 0;
                    pixels[pxIdx + 1] = 0;
                    pixels[pxIdx + 2] = 0;
                    pixels[pxIdx + 3] = 0;
                    continue;
                }

                let r = 0;
                let g = 0;
                let b = 0;
                let a = 0;

                if (costValue >= 253) {
                    // Inscribed/lethal obstacles.
                    r = 255;
                    g = 30;
                    b = 30;
                    a = 235;
                } else {
                    // Inflation area (1..252): green/yellow -> orange -> red with increasing alpha.
                    const t = Math.max(0, Math.min(1, costValue / 252));
                    r = Math.round(60 + 195 * t);
                    g = Math.round(255 - 220 * t);
                    b = Math.round(40 * (1 - t));
                    a = Math.round(35 + 190 * Math.pow(t, 0.65));
                }

                pixels[pxIdx] = r;
                pixels[pxIdx + 1] = g;
                pixels[pxIdx + 2] = b;
                pixels[pxIdx + 3] = a;
            }
        }

        this.costMapCacheCtx.putImageData(imageData, 0, 0);
        this.costMapCacheReady = true;
    },

    applyCostMapUpdate: function(costMapData, costmapUpdate) {
        if (!costMapData || !costMapData.data || !costmapUpdate) {
            return;
        }

        const patchWidth = Number(costmapUpdate.width) || 0;
        const patchHeight = Number(costmapUpdate.height) || 0;
        const patchX = Number(costmapUpdate.x) || 0;
        const patchY = Number(costmapUpdate.y) || 0;

        if (patchWidth <= 0 || patchHeight <= 0) {
            return;
        }

        const patchBytes = this.decodeCostMapBase64(costmapUpdate.dataBase64 || costmapUpdate.data);
        if (!patchBytes || patchBytes.length === 0) {
            return;
        }

        const maxPatchCells = patchWidth * patchHeight;
        const patchCellCount = Math.min(maxPatchCells, patchBytes.length);
        let applied = 0;

        for (let i = 0; i < patchCellCount; i++) {
            const localX = i % patchWidth;
            const localY = Math.floor(i / patchWidth);
            const mapX = patchX + localX;
            const mapY = patchY + localY;

            if (mapX < 0 || mapX >= costMapData.width || mapY < 0 || mapY >= costMapData.height) {
                continue;
            }

            const mapIdx = mapY * costMapData.width + mapX;
            costMapData.data[mapIdx] = patchBytes[i];
            applied++;
        }

        if (applied > 0) {
            this.rebuildCostMapCache();
        }
    },

    normalizeFrameId: function(frameId) {
        return (frameId || '').toString().replace(/^\/+/, '').toLowerCase();
    },

    lockMapFromOdomTransform: function(odometry, xlocPose) {
        const odomX = Number(odometry?.x);
        const odomY = Number(odometry?.y);
        const odomYaw = Number(odometry?.yaw);
        const mapX = Number(xlocPose?.x);
        const mapY = Number(xlocPose?.y);
        const mapYaw = Number(xlocPose?.yaw);

        const hasOdom = Number.isFinite(odomX) && Number.isFinite(odomY) && Number.isFinite(odomYaw);
        const hasMapPose = Number.isFinite(mapX) && Number.isFinite(mapY) && Number.isFinite(mapYaw);
        if (!hasOdom || !hasMapPose) {
            return false;
        }

        // map<-odom = map<-base * inverse(odom<-base)
        const c = Math.cos(mapYaw);
        const s = Math.sin(mapYaw);
        const tx = mapX - (c * odomX - s * odomY);
        const ty = mapY - (s * odomX + c * odomY);

        this.costMapMapFromOdomLock = {
            tx: tx,
            ty: ty,
            yaw: mapYaw - odomYaw
        };
        return true;
    },

    transformPointFromOdomToMap: function(x, y) {
        if (!this.costMapMapFromOdomLock) {
            return { x: x, y: y };
        }

        const t = this.costMapMapFromOdomLock;
        const c = Math.cos(t.yaw);
        const s = Math.sin(t.yaw);
        return {
            x: t.tx + c * x - s * y,
            y: t.ty + s * x + c * y
        };
    },

    updateCostMapPoseFromOdometry: function(odometry, localizationActive, xlocPose, matchingScore) {
        if (!this.costMapData) {
            return;
        }

        const odom = odometry || null;
        const odomX = Number(odom?.x);
        const odomY = Number(odom?.y);
        const odomYaw = Number(odom?.yaw);
        const hasOdom = Number.isFinite(odomX) && Number.isFinite(odomY);

        const frameId = this.normalizeFrameId(this.costMapData.frameId);
        const isOdomFrame = frameId === 'odom';
        const hasValidLocalization = Number.isFinite(matchingScore) && matchingScore !== -1;
        const localizationJustSucceeded = hasValidLocalization && this.costMapLastMatchingScore === -1;

        // Lock costmap orientation and map<-odom transform when localization starts.
        if (localizationActive && !this.costMapLocalizationActive) {
            if (isOdomFrame && hasValidLocalization) {
                this.lockMapFromOdomTransform(odom, xlocPose);
            }
        }

        // Update costmap yaw to match robot yaw when localization successfully calculates (matching score != -1)
        if (hasValidLocalization && (localizationJustSucceeded || localizationActive && !this.costMapLocalizationActive)) {
            const oldYaw = this.costMapFixedYaw;
            let baseYaw = 0;
            if (xlocPose && Number.isFinite(Number(xlocPose.yaw))) {
                baseYaw = Number(xlocPose.yaw);
            } else if (Number.isFinite(odomYaw)) {
                baseYaw = odomYaw;
            } else {
                baseYaw = Number(this.costMapData.originTheta) || 0;
            }
            
            // Rotate costmap 45° clockwise to align inflation with wall boundaries
            const offsetDegrees = 190;  // Negative = clockwise
            const offsetRadians = offsetDegrees * Math.PI / 180;
            this.costMapFixedYaw = baseYaw + offsetRadians;
            
            if (Number.isFinite(this.costMapFixedYaw)) {
                const baseYawDeg = baseYaw * 180 / Math.PI;
                const finalYawDeg = this.costMapFixedYaw * 180 / Math.PI;
                const oldYawDeg = Number.isFinite(oldYaw) ? oldYaw * 180 / Math.PI : null;
                const offsetLabel = offsetDegrees > 0 ? `+${offsetDegrees}° CCW` : `${Math.abs(offsetDegrees)}° CW`;
                console.log(`[Costmap] Locked yaw: Robot=${baseYawDeg.toFixed(1)}° ${offsetLabel} = ${finalYawDeg.toFixed(1)}° (matching score: ${matchingScore.toFixed(2)})` + 
                           (oldYawDeg !== null ? `, was ${oldYawDeg.toFixed(1)}°` : ''));
            }
        }

        this.costMapLocalizationActive = localizationActive;
        this.costMapLastMatchingScore = matchingScore;

        if (!Number.isFinite(this.costMapFixedYaw)) {
            this.costMapFixedYaw = Number(this.costMapData.originTheta) || 0;
        }

        if (isOdomFrame) {
            if (!this.costMapMapFromOdomLock && hasValidLocalization) {
                this.lockMapFromOdomTransform(odom, xlocPose);
            }

            const transformed = this.transformPointFromOdomToMap(
                Number(this.costMapData.originX) || 0,
                Number(this.costMapData.originY) || 0
            );
            this.costMapData.poseX = transformed.x;
            this.costMapData.poseY = transformed.y;
        } else if (hasOdom && this.costMapPoseAnchor && this.costMapOdomAnchor) {
            // Fallback for non-odom frame sources.
            this.costMapData.poseX = this.costMapPoseAnchor.x + (odomX - this.costMapOdomAnchor.x);
            this.costMapData.poseY = this.costMapPoseAnchor.y + (odomY - this.costMapOdomAnchor.y);
        } else {
            this.costMapData.poseX = Number(this.costMapData.originX) || 0;
            this.costMapData.poseY = Number(this.costMapData.originY) || 0;
        }

        this.costMapData.fixedYaw = this.costMapFixedYaw;
    },

    drawCostMap: function() {
        if (!this.showCostMap || !this.costMapData || !this.costMapData.data || !this.costMapCacheReady) {
            return;
        }

        const { mapFrameX, mapFrameY } = this.getFramePositions();
        const { width, height, resolution, poseX, poseY, fixedYaw } = this.costMapData;

        // After 90° CW rotation, dimensions are swapped in the cache canvas
        const mapWidthPixels = height * resolution * this.scale;
        const mapHeightPixels = width * resolution * this.scale;

        // Calculate angle difference between costmap and robot
        if (this.robotPose && Number.isFinite(this.robotPose.yaw) && Number.isFinite(fixedYaw)) {
            const angleDiffRad = fixedYaw - this.robotPose.yaw;
            const angleDiffDeg = angleDiffRad * 180 / Math.PI;
            
            // Log once every 2 seconds to avoid spam
            if (!this._lastCostMapAngleLog || Date.now() - this._lastCostMapAngleLog > 2000) {
                console.log(`[Costmap] Offset from robot: ${angleDiffDeg.toFixed(1)}° (Robot: ${(this.robotPose.yaw * 180 / Math.PI).toFixed(1)}°, Costmap: ${(fixedYaw * 180 / Math.PI).toFixed(1)}°)`);
                this._lastCostMapAngleLog = Date.now();
            }
        }

        this.ctx.save();
        this.ctx.globalAlpha = 0.75;

        // Use robot position as center of rotation (robot should be at center of costmap)
        let centerX, centerY;
        if (this.robotPose) {
            // Robot is the center of costmap
            centerX = mapFrameX + (this.robotPose.x * this.scale);
            centerY = mapFrameY - (this.robotPose.y * this.scale);
        } else {
            // Fallback to costmap origin + half dimensions
            const halfWidthMeters = (height * resolution) / 2;  // After 90° rotation
            const halfHeightMeters = (width * resolution) / 2;
            centerX = mapFrameX + ((poseX + halfWidthMeters) * this.scale);
            centerY = mapFrameY - ((poseY + halfHeightMeters) * this.scale);
        }

        // Translate to center (robot position), rotate, then draw costmap centered
        this.ctx.translate(centerX, centerY);
        this.ctx.rotate(-(fixedYaw || 0));
        
        // Translate costmap in robot frame: X=forward, Y=left
        const xOffsetMeters = -0.285;  // Forward offset
        const yOffsetMeters = -0.707;  // Left offset
        this.ctx.translate(xOffsetMeters * this.scale, -yOffsetMeters * this.scale);
        
        this.ctx.scale(1, -1);
        
        // Draw costmap centered at origin (robot at center + offsets)
        this.ctx.drawImage(this.costMapCacheCanvas, -mapWidthPixels / 2, -mapHeightPixels / 2, mapWidthPixels, mapHeightPixels);

        // Draw costmap frame so its boundary is always visible.
        this.ctx.globalAlpha = 1.0;
        this.ctx.strokeStyle = 'rgba(0, 188, 212, 0.95)';
        this.ctx.lineWidth = 2;
        this.ctx.strokeRect(-mapWidthPixels / 2, -mapHeightPixels / 2, mapWidthPixels, mapHeightPixels);

        this.ctx.restore();
    },

    /**
     * Get LIDAR transform relative to base_link (from XlocClient.cs static transforms)
     */
    getLidarTransform: function(frameId) {
        // Defaults to identity if unknown
        const defaultTransform = { x: 0, y: 0, yaw: 0 };
        switch (frameId) {
            case 'scan_1': {
                // base_link -> scan_1
                return {
                    x: 0.2985,
                    y: 0.0,
                    yaw: this.quaternionToYaw(0.0, 0.0, 0.0, 1.0)
                };
            }
            case 'scan_2': {
                // base_link -> scan_2
                return {
                    x: -0.2985,
                    y: -0.0,
                    yaw: this.quaternionToYaw(0.0, 0.0, 1.0, 0.0)
                };
            }
            case 'scan_3': {
                // base_link -> scan_3
                return {
                    x: 0.707,
                    y: 0.2825,
                    yaw: this.quaternionToYaw(0.0, 0.0, 0.3826834, 0.9238795)
                };
            }
            default:
                return defaultTransform;
        }
    },

    /**
     * Convert quaternion to yaw (radians) for Z rotation only.
     */
    quaternionToYaw: function(qx, qy, qz, qw) {
        const sinyCosp = 2.0 * (qw * qz + qx * qy);
        const cosyCosp = 1.0 - 2.0 * (qy * qy + qz * qz);
        return Math.atan2(sinyCosp, cosyCosp);
    },
    
    /**
     * Draw a single LIDAR scan
     * LIDAR points are transformed from robot frame -> map frame -> canvas
     */
    drawSingleLidarScan: function(scanData, color, lidarTransform, renderMode) {
        if (!scanData || !scanData.points || scanData.points.length === 0) return;
        
        const { mapFrameX, mapFrameY } = this.getFramePositions();
        // Get robot pose in MAP coordinates for transformation
        const robotX = this.robotPose ? this.robotPose.x : 0;
        const robotY = this.robotPose ? this.robotPose.y : 0;
        const robotYaw = this.robotPose ? this.robotPose.yaw : 0;
        
        this.ctx.fillStyle = color;
        
        const lidar = lidarTransform || { x: 0, y: 0, yaw: 0 };
        const cosLidarYaw = Math.cos(lidar.yaw);
        const sinLidarYaw = Math.sin(lidar.yaw);

        // Backend now handles sampling based on mode: minimal = 120 points, full = all points
        // Frontend just draws all points received from backend
        for (let i = 0; i < scanData.points.length; i++) {
            const point = scanData.points[i];
            let pointX, pointY;
            
            // Check if point has x,y (cartesian) or angle,range (polar)
            if (point.x !== undefined && point.y !== undefined) {
                // Already in cartesian coordinates (robot frame)
                pointX = point.x;
                pointY = point.y;
            } else if (point.angle !== undefined && point.range !== undefined) {
                // Convert from polar to cartesian (robot frame)
                // LIDAR convention: angle 0 = forward (positive X)
                pointX = point.range * Math.cos(point.angle);
                pointY = point.range * Math.sin(point.angle);
            } else {
                continue; // Skip invalid point
            }
            
            // Transform from LIDAR frame -> robot frame
            const robotPointX = lidar.x + (pointX * cosLidarYaw - pointY * sinLidarYaw);
            const robotPointY = lidar.y + (pointX * sinLidarYaw + pointY * cosLidarYaw);

            // Transform from robot frame to MAP frame
            const cosYaw = Math.cos(robotYaw);
            const sinYaw = Math.sin(robotYaw);
            const mapX = robotX + (robotPointX * cosYaw - robotPointY * sinYaw);
            const mapY = robotY + (robotPointX * sinYaw + robotPointY * cosYaw);
            
            // Convert MAP coordinates to canvas coordinates
            // Same transform as map: mapFrameX + mapX * scale, mapFrameY - mapY * scale (flip Y)
            const canvasX = mapFrameX + (mapX * this.scale);
            const canvasY = mapFrameY - (mapY * this.scale);
            
            // Draw point (larger size: 3px radius)
            this.ctx.beginPath();
            this.ctx.arc(canvasX, canvasY, 3, 0, 2 * Math.PI);
            this.ctx.fill();
        }
    },
    
    /**
     * Draw map frame coordinate axes (fixed at origin)
     */
    drawCoordinateAxes: function() {
        const { mapFrameX, mapFrameY } = this.getFramePositions();
        const axisLength = 60;
        
        // Origin marker
        this.ctx.fillStyle = '#FF9800';
        this.ctx.strokeStyle = '#F57C00';
        this.ctx.lineWidth = 2;
        this.ctx.beginPath();
        this.ctx.arc(mapFrameX, mapFrameY, 6, 0, 2 * Math.PI);
        this.ctx.fill();
        this.ctx.stroke();
        
        // X axis (red)
        this.ctx.strokeStyle = '#F44336';
        this.ctx.lineWidth = 3;
        this.ctx.beginPath();
        this.ctx.moveTo(mapFrameX, mapFrameY);
        this.ctx.lineTo(mapFrameX + axisLength, mapFrameY);
        this.ctx.stroke();
        
        // X arrow
        this.ctx.fillStyle = '#F44336';
        this.ctx.beginPath();
        this.ctx.moveTo(mapFrameX + axisLength, mapFrameY);
        this.ctx.lineTo(mapFrameX + axisLength - 8, mapFrameY - 5);
        this.ctx.lineTo(mapFrameX + axisLength - 8, mapFrameY + 5);
        this.ctx.closePath();
        this.ctx.fill();
        
        // X label
        this.ctx.fillStyle = '#F44336';
        this.ctx.font = 'bold 12px Arial';
        this.ctx.fillText('X', mapFrameX + axisLength + 8, mapFrameY + 5);
        
        // Y axis (green)
        this.ctx.strokeStyle = '#4CAF50';
        this.ctx.lineWidth = 3;
        this.ctx.beginPath();
        this.ctx.moveTo(mapFrameX, mapFrameY);
        this.ctx.lineTo(mapFrameX, mapFrameY - axisLength);
        this.ctx.stroke();
        
        // Y arrow
        this.ctx.fillStyle = '#4CAF50';
        this.ctx.beginPath();
        this.ctx.moveTo(mapFrameX, mapFrameY - axisLength);
        this.ctx.lineTo(mapFrameX - 5, mapFrameY - axisLength + 8);
        this.ctx.lineTo(mapFrameX + 5, mapFrameY - axisLength + 8);
        this.ctx.closePath();
        this.ctx.fill();
        
        // Y label
        this.ctx.fillStyle = '#4CAF50';
        this.ctx.font = 'bold 12px Arial';
        this.ctx.fillText('Y', mapFrameX + 8, mapFrameY - axisLength - 5);
    },
    drawWorldCoordinateAxes: function() {
        if (!this.lockedMapOrigin) return; // Only draw if map origin is known
        
        const { mapFrameX, mapFrameY } = this.getFramePositions();
        const originXY = this.getMapOriginXY();
        
        // Calculate world (0,0) position in canvas coordinates
        // Map origin is the world position of map (0,0), so world (0,0) = map (0,0) - origin
        // In canvas: worldX = mapFrameX + (-originX) * scale, worldY = mapFrameY - (-originY) * scale
        const worldOriginX = mapFrameX - (originXY.x * this.scale);
        const worldOriginY = mapFrameY + (originXY.y * this.scale);
        
        const axisLength = 50;
        
        // Origin marker (blue for world frame)
        this.ctx.fillStyle = '#2196F3';
        this.ctx.strokeStyle = '#1976D2';
        this.ctx.lineWidth = 2;
        this.ctx.beginPath();
        this.ctx.arc(worldOriginX, worldOriginY, 5, 0, 2 * Math.PI);
        this.ctx.fill();
        this.ctx.stroke();
        
        // X axis (red, dashed for world frame)
        this.ctx.strokeStyle = '#F44336';
        this.ctx.lineWidth = 2.5;
        this.ctx.setLineDash([5, 5]);
        this.ctx.beginPath();
        this.ctx.moveTo(worldOriginX, worldOriginY);
        this.ctx.lineTo(worldOriginX + axisLength, worldOriginY);
        this.ctx.stroke();
        this.ctx.setLineDash([]);
        
        // X arrow
        this.ctx.fillStyle = '#F44336';
        this.ctx.beginPath();
        this.ctx.moveTo(worldOriginX + axisLength, worldOriginY);
        this.ctx.lineTo(worldOriginX + axisLength - 8, worldOriginY - 5);
        this.ctx.lineTo(worldOriginX + axisLength - 8, worldOriginY + 5);
        this.ctx.closePath();
        this.ctx.fill();
        
        // X label
        this.ctx.fillStyle = '#F44336';
        this.ctx.font = 'bold 11px Arial';
        this.ctx.fillText('X_world', worldOriginX + axisLength + 8, worldOriginY + 5);
        
        // Y axis (green, dashed for world frame)
        this.ctx.strokeStyle = '#4CAF50';
        this.ctx.lineWidth = 2.5;
        this.ctx.setLineDash([5, 5]);
        this.ctx.beginPath();
        this.ctx.moveTo(worldOriginX, worldOriginY);
        this.ctx.lineTo(worldOriginX, worldOriginY - axisLength);
        this.ctx.stroke();
        this.ctx.setLineDash([]);
        
        // Y arrow
        this.ctx.fillStyle = '#4CAF50';
        this.ctx.beginPath();
        this.ctx.moveTo(worldOriginX, worldOriginY - axisLength);
        this.ctx.lineTo(worldOriginX - 5, worldOriginY - axisLength + 8);
        this.ctx.lineTo(worldOriginX + 5, worldOriginY - axisLength + 8);
        this.ctx.closePath();
        this.ctx.fill();
        
        // Y label
        this.ctx.fillStyle = '#4CAF50';
        this.ctx.font = 'bold 11px Arial';
        this.ctx.fillText('Y_world', worldOriginX + 8, worldOriginY - axisLength - 5);
    },
    /**
     * Set zoom level (scale)
     */
    setZoom: function(newScale) {
        this.scale = Math.max(1, Math.min(100, newScale)); // Clamp between 1 and 100
        this.render();
    },
    
    /**
     * Zoom in (increase scale by 20%)
     */
    zoomIn: function() {
        const newScale = this.scale * 1.2;
        if (newScale <= 100) {
            this.scale = newScale;
            this.render();
        }
    },
    
    /**
     * Zoom out (decrease scale by 20%)
     */
    zoomOut: function() {
        const newScale = this.scale / 1.2;
        if (newScale >= 1) {
            this.scale = newScale;
            this.render();
        }
    },
    
    /**
     * Reset zoom to default
     */
    resetZoom: function() {
        this.scale = 20;
        this.render();
    },
    
    /**
     * Reset view to default (zoom + pan)
     */
    resetView: function() {
        this.offsetX = 0;
        this.offsetY = 0;
        this.scale = 20;
        this.render();
    },
    
    /**
     * Reset locked values (call when loading a new map)
     */
    resetLocks: function() {
        this.lockedMapOrigin = null;
        this.lockedOffsetX = null;
        this.lockedOffsetY = null;
        this.costMapData = null;
        this.costMapCacheReady = false;
        this.costMapPoseAnchor = null;
        this.costMapOdomAnchor = null;
        this.costMapFixedYaw = null;
        this.costMapMapFromOdomLock = null;
        this.costMapLocalizationActive = false;
        this.costMapLastMatchingScore = -1;
    },
    
    /**
     * Get current view state (for debugging/verification)
     * Returns JSON string for C# deserialization
     */
    getViewState: function() {
        const state = {
            offsetX: this.offsetX,
            offsetY: this.offsetY,
            scale: this.scale,
            lockedMapOrigin: this.lockedMapOrigin,
            gridMapOrigin: this.gridMap?.origin
        };
        return JSON.stringify(state);
    },
    
    /**
     * Set view offsets (for restoring after verification)
     */
    setViewOffsets: function(offsetX, offsetY) {
        this.offsetX = Number(offsetX);
        this.offsetY = Number(offsetY);
        
        // CRITICAL: If locked offsets exist, update them to match (for consistency)
        if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
            this.lockedOffsetX = this.offsetX;
            this.lockedOffsetY = this.offsetY;
        }
        
        this.requestRender();
    }
};

// Global wrapper functions for JSInterop debugging
function debugSetGlobalPathData(pathPoints) {
    console.log('🎯 Global wrapper function called: debugSetGlobalPathData');
    console.log('   Received:', pathPoints);
    if (window.xlocMapRenderer) {
        console.log('   ✅ xlocMapRenderer exists, calling setGlobalPathData...');
        return window.xlocMapRenderer.setGlobalPathData(pathPoints);
    } else {
        console.error('   ❌ xlocMapRenderer NOT FOUND!');
        return null;
    }
}