using RobotNet10.Shared.Sensor;
using System.Runtime.InteropServices;

namespace RobotNet10.RobotApp.Xloc;

/// <summary>
/// High-level wrapper for xloc localization library
/// Provides thread-safe access to xloc functionality with automatic resource management
/// </summary>
public class XlocClient : IDisposable
{
    private volatile IntPtr _handle = IntPtr.Zero;
    // public IntPtr _tfBuffer = IntPtr.Zero;  // TF3 buffer for xloc
    private readonly object _lock = new object();
    private readonly object _dispatchLock = new object();
    private bool _disposed = false;
    private readonly ILogger? _logger;
    private IntPtr _tfBuffer = IntPtr.Zero;

    /// <summary>
    /// Public property to access the TF3 buffer for use in navigation and other components
    /// </summary>
    public IntPtr TfBuffer => _tfBuffer;

    public XlocClient(IntPtr tfBuffer, ILogger? logger = null)
    {
        _logger = logger;
        _tfBuffer = tfBuffer;  // TF3 buffer for xloc
    }

    /// <summary>
    /// Initialize xloc instance
    /// Note: TF3BufferManager must be initialized before calling this method
    /// </summary>
    public void Initialize()
    {
        lock (_lock)
        {
            if (_handle != IntPtr.Zero)
            {
                _logger?.LogWarning("XlocClient already initialized");
                return;
            }

            try
            {
                // Verify TF buffer is available from TF3BufferManager
                if (_tfBuffer == IntPtr.Zero)
                {
                    throw new InvalidOperationException("TF buffer not initialized. Ensure TF3BufferManager is initialized first.");
                }
                
                // _logger?.LogInformation("[XLOC] ═══════════════════════════════════════════");
                // _logger?.LogInformation("[XLOC] Initializing XlocClient...");
                // _logger?.LogInformation("[XLOC] TF buffer available: {Buffer}", _tfBuffer);
                
                // Wait a moment to ensure TF3 transforms are fully propagated
                // This is critical - the native xloc library needs time to index the transforms
                _logger?.LogInformation("[XLOC] Waiting for TF3 transforms to be ready (500ms)...");
                System.Threading.Thread.Sleep(500);
                
                // _logger?.LogInformation("[XLOC] Creating xloc instance with TF buffer...");
                
                // Create xloc instance with TF buffer (initialized and populated with static transforms)
                _handle = XlocNativeInterface.xloc_create(_tfBuffer);
                
                _logger?.LogInformation("[XLOC] xloc_create() returned handle: {Handle}", _handle);
                
                if (_handle == IntPtr.Zero)
                {
                    throw new InvalidOperationException("Failed to create xloc instance. xloc_create returned null.");
                }

                // _logger?.LogInformation("[XLOC] ═══════════════════════════════════════════");
                // _logger?.LogInformation("[XLOC] ✅ XlocClient initialized successfully");
                // _logger?.LogInformation("[XLOC] Ready to serve: localization, mapping, odometry dispatch");
            }
            catch (Exception ex)
            {
                _logger?.LogError(ex, "[XLOC] ❌ Exception in Initialize: {Message}", ex.Message);
                throw;
            }
        }
    }

    /// <summary>
    /// Check if xloc is initialized
    /// </summary>
    public bool IsInitialized
    {
        get
        {
            lock (_lock)
            {
                return _handle != IntPtr.Zero;
            }
        }
    }

    /// <summary>
    /// Dispatch Odometry data to xloc
    /// </summary>
    public void DispatchOdometry(Odometry odom, string sensorId)
    {
        ThrowIfNotInitializedFast();

        xloc_odometry_t xlocOdom = default;
        try
        {
            var timestamp = DateTime.Now.ToString("HH:mm:ss.fff");
            _logger?.LogTrace("[{Time}] [DISPATCH-START] Odometry from {SensorId}", timestamp, sensorId);

            xlocOdom = odom.ToXlocOdometry();
            lock (_dispatchLock)
            {
                _logger?.LogTrace("[{Time}] [NATIVE-CALL] xloc_dispatch_odometry...", timestamp);
                XlocNativeInterface.xloc_dispatch_odometry(_handle, sensorId, ref xlocOdom);
                _logger?.LogTrace("[{Time}] [NATIVE-DONE] xloc_dispatch_odometry OK", timestamp);
            }

            _logger?.LogTrace("[{Time}] [DISPATCH-DONE] Odometry from {SensorId}", timestamp, sensorId);
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DISPATCH-ERROR] Odometry dispatch failed: {Message}", ex.Message);
            throw;
        }
        finally
        {
            // Free allocated memory
            XlocConversionExtensions.FreeXlocOdometry(ref xlocOdom);
        }
    }

    /// <summary>
    /// Dispatch IMU data to xloc
    /// </summary>
    public void DispatchImu(Imu imu, string sensorId)
    {
        ThrowIfNotInitializedFast();

        xloc_imu_t xlocImu = default;
        try
        {
            var timestamp = DateTime.Now.ToString("HH:mm:ss.fff");
            _logger?.LogTrace("[{Time}] [DISPATCH-START] IMU from {SensorId}", timestamp, sensorId);

            xlocImu = imu.ToXlocImu();

            lock (_dispatchLock)
            {
                _logger?.LogTrace("[{Time}] [NATIVE-CALL] xloc_dispatch_imu...", timestamp);
                XlocNativeInterface.xloc_dispatch_imu(_handle, sensorId, ref xlocImu);
                _logger?.LogTrace("[{Time}] [NATIVE-DONE] xloc_dispatch_imu OK", timestamp);
            }

            _logger?.LogTrace("[{Time}] [DISPATCH-DONE] IMU from {SensorId}", timestamp, sensorId);
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DISPATCH-ERROR] IMU dispatch failed: {Message}", ex.Message);
            throw;
        }
        finally
        {
            // Free allocated memory
            XlocConversionExtensions.FreeXlocImu(ref xlocImu);
        }
    }

    /// <summary>
    /// Dispatch LaserScan data to xloc
    /// </summary>
    public void DispatchLaserScan(LaserScan scan, string sensorId)
    {
        ThrowIfNotInitializedFast();

        if (string.IsNullOrWhiteSpace(sensorId))
        {
            throw new ArgumentException("Sensor ID must not be null or empty.", nameof(sensorId));
        }

        if (scan.Ranges == null || scan.Ranges.Length == 0)
        {
            _logger?.LogWarning("[DISPATCH-SKIP] LaserScan from {SensorId} has no ranges", sensorId);
            return;
        }

        xloc_laserscan_t xlocScan = default;
        try
        {
            var timestamp = DateTime.Now.ToString("HH:mm:ss.fff");

            xlocScan = scan.ToXlocLaserScan();

            var lockWaitStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
            lock (_dispatchLock)
            {
                var lockWaitMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - lockWaitStart;
                var nativeStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();

                XlocNativeInterface.xloc_dispatch_laserscan(_handle, sensorId, ref xlocScan);

                var nativeMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - nativeStart;

                if (lockWaitMs > 5 || nativeMs > 10)
                {
                    // _logger?.LogWarning(
                    //     "[XLOC-DIAG] DispatchLaserScan {SensorId} lock_wait={LockWaitMs}ms native={NativeMs}ms",
                    //     sensorId, lockWaitMs, nativeMs);
                }
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DISPATCH-ERROR] LaserScan dispatch failed: {Message}", ex.Message);
            throw;
        }
        finally
        {
            // Free allocated memory
            XlocConversionExtensions.FreeXlocLaserScan(ref xlocScan);
        }
    }

    /// <summary>
    /// Activate a map file
    /// </summary>
    public bool ActivateMap(string mapFileName)
    {
        ThrowIfNotInitialized();

        try
        {
            xloc_status_response_t response;
            lock (_lock)
            {
                response = XlocNativeInterface.xloc_activate_map(_handle, mapFileName);
            }

            string message = XlocConversionExtensions.GetMessageAndFree(ref response);
            
            if (response.code == 0)
            {
                _logger?.LogInformation("Map activated successfully: {MapFile}", mapFileName);
                return true;
            }
            else
            {
                _logger?.LogWarning("Failed to activate map {MapFile}: Code={Code}, Message={Message}", 
                    mapFileName, response.code, message);
                return false;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error activating map {MapFile}", mapFileName);
            throw;
        }
    }

    /// <summary>
    /// Start localization
    /// Automatically resets SLAM error state before starting to clear any previous trajectory state
    /// </summary>
    public bool StartLocalization()
    {
        ThrowIfNotInitialized();

        try
        {
            // Reset SLAM error state first to clear any previous trajectory state
            // This is required when starting a new localization session after stopping a previous one
            _logger?.LogInformation("Resetting SLAM error state before starting localization...");
            ResetSlamError();
            
            // Small delay to ensure reset is processed
            System.Threading.Thread.Sleep(100);

            xloc_status_response_t response;
            lock (_lock)
            {
                response = XlocNativeInterface.xloc_start_localization(_handle);
            }

            string message = XlocConversionExtensions.GetMessageAndFree(ref response);
            
            if (response.code == 0)
            {
                _logger?.LogInformation("Localization started successfully");
                return true;
            }
            else
            {
                _logger?.LogWarning("Failed to start localization: Code={Code}, Message={Message}", 
                    response.code, message);
                return false;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error starting localization");
            throw;
        }
    }

    /// <summary>
    /// Stop localization
    /// </summary>
    public bool StopLocalization()
    {
        ThrowIfNotInitialized();

        try
        {
            xloc_status_response_t response;
            lock (_lock)
            {
                response = XlocNativeInterface.xloc_stop_localization(_handle);
            }

            string message = XlocConversionExtensions.GetMessageAndFree(ref response);
            
            if (response.code == 0)
            {
                _logger?.LogInformation("Localization stopped successfully");
                return true;
            }
            else
            {
                _logger?.LogWarning("Failed to stop localization: Code={Code}, Message={Message}", 
                    response.code, message);
                return false;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error stopping localization");
            throw;
        }
    }

    /// <summary>
    /// Start mapping
    /// Automatically resets SLAM error state before starting to clear any previous trajectory state
    /// </summary>
    public bool StartMapping()
    {
        ThrowIfNotInitialized();

        try
        {
            // Reset SLAM error state first to clear any previous trajectory state
            // This is required when starting a new mapping session after stopping a previous one
            _logger?.LogInformation("Resetting SLAM error state before starting mapping...");
            ResetSlamError();
            
            // Small delay to ensure reset is processed
            System.Threading.Thread.Sleep(100);

            xloc_status_response_t response;
            lock (_lock)
            {
                response = XlocNativeInterface.xloc_start_mapping(_handle);
            }

            string message = XlocConversionExtensions.GetMessageAndFree(ref response);
            
            if (response.code == 0)
            {
                _logger?.LogInformation("Mapping started successfully");
                return true;
            }
            else
            {
                _logger?.LogWarning("Failed to start mapping: Code={Code}, Message={Message}", 
                    response.code, message);
                return false;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error starting mapping");
            throw;
        }
    }

    /// <summary>
    /// Stop mapping and save to file
    /// </summary>
    public bool StopMapping(string mapFileName)
    {
        ThrowIfNotInitialized();

        try
        {
            xloc_status_response_t response;
            lock (_lock)
            {
                response = XlocNativeInterface.xloc_stop_mapping(_handle, mapFileName);
            }

            string message = XlocConversionExtensions.GetMessageAndFree(ref response);
            
            if (response.code == 0)
            {
                _logger?.LogInformation("Mapping stopped and saved to {MapFile}", mapFileName);
                return true;
            }
            else
            {
                _logger?.LogWarning("Failed to stop mapping: Code={Code}, Message={Message}", 
                    response.code, message);
                return false;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error stopping mapping");
            throw;
        }
    }

    /// <summary>
    /// Switch to a different map with optional initial pose
    /// </summary>
    public bool SwitchMap(string mapFileName, (double x, double y, double z, double qx, double qy, double qz, double qw)? initialPose = null)
    {
        ThrowIfNotInitialized();

        try
        {
            xloc_status_response_t response;
            lock (_lock)
            {
                if (initialPose.HasValue)
                {
                    var pose = new xloc_pose_t
                    {
                        position = new double[] { initialPose.Value.x, initialPose.Value.y, initialPose.Value.z },
                        orientation = new double[] { initialPose.Value.qx, initialPose.Value.qy, initialPose.Value.qz, initialPose.Value.qw }
                    };
                    response = XlocNativeInterface.xloc_switch_map(_handle, mapFileName, 1, ref pose);
                }
                else
                {
                    var dummyPose = new xloc_pose_t();
                    response = XlocNativeInterface.xloc_switch_map(_handle, mapFileName, 0, ref dummyPose);
                }
            }

            string message = XlocConversionExtensions.GetMessageAndFree(ref response);

            if (response.code == 0)
            {
                _logger?.LogInformation("Switched to map: {MapFile}", mapFileName);
                return true;
            }
            else
            {
                _logger?.LogWarning("Failed to switch map: Code={Code}, Message={Message}", response.code, message);
                return false;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error switching map");
            throw;
        }
    }

    /// <summary>
    /// Change map origin (re-center the map coordinate system)
    /// </summary>
    public bool ChangeMapOrigin(double x, double y, double z, double qx, double qy, double qz, double qw)
    {
        ThrowIfNotInitialized();

        try
        {
            var newOrigin = new xloc_pose_t
            {
                position = new double[] { x, y, z },
                orientation = new double[] { qx, qy, qz, qw }
            };

            xloc_status_response_t response;
            lock (_lock)
            {
                response = XlocNativeInterface.xloc_change_map_origin(_handle, ref newOrigin);
            }

            string message = XlocConversionExtensions.GetMessageAndFree(ref response);

            if (response.code == 0)
            {
                _logger?.LogInformation("Map origin changed to: ({X}, {Y}, {Z})", x, y, z);
                return true;
            }
            else
            {
                _logger?.LogWarning("Failed to change map origin: Code={Code}, Message={Message}", response.code, message);
                return false;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error changing map origin");
            throw;
        }
    }

    /// <summary>
    /// Start updating existing map (allow SLAM to modify the loaded map)
    /// </summary>
    public bool StartUpdateMap()
    {
        ThrowIfNotInitialized();

        try
        {
            xloc_status_response_t response;
            lock (_lock)
            {
                response = XlocNativeInterface.xloc_start_update_map(_handle);
            }

            string message = XlocConversionExtensions.GetMessageAndFree(ref response);

            if (response.code == 0)
            {
                _logger?.LogInformation("Started updating map");
                return true;
            }
            else
            {
                _logger?.LogWarning("Failed to start map update: Code={Code}, Message={Message}", response.code, message);
                return false;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error starting map update");
            throw;
        }
    }

    /// <summary>
    /// Stop updating map and optionally save the updated map
    /// </summary>
    public bool StopUpdateMap(bool saveUpdatedMap = true)
    {
        ThrowIfNotInitialized();

        try
        {
            xloc_status_response_t response;
            lock (_lock)
            {
                response = XlocNativeInterface.xloc_stop_update_map(_handle, saveUpdatedMap ? 1 : 0);
            }

            string message = XlocConversionExtensions.GetMessageAndFree(ref response);

            if (response.code == 0)
            {
                _logger?.LogInformation("Stopped updating map (save={Save})", saveUpdatedMap);
                return true;
            }
            else
            {
                _logger?.LogWarning("Failed to stop map update: Code={Code}, Message={Message}", response.code, message);
                return false;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error stopping map update");
            throw;
        }
    }

    /// <summary>
    /// Reset SLAM error state (attempt recovery from error)
    /// </summary>
    public bool ResetSlamError()
    {
        ThrowIfNotInitialized();

        try
        {
            xloc_status_response_t response;
            lock (_lock)
            {
                response = XlocNativeInterface.xloc_reset_slam_error(_handle);
            }

            string message = XlocConversionExtensions.GetMessageAndFree(ref response);

            if (response.code == 0)
            {
                _logger?.LogInformation("SLAM error reset successfully");
                return true;
            }
            else
            {
                _logger?.LogWarning("Failed to reset SLAM error: Code={Code}, Message={Message}", response.code, message);
                return false;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error resetting SLAM error");
            throw;
        }
    }

    /// <summary>
    /// Set initial pose for localization
    /// </summary>
    public bool SetInitialPose(double x, double y, double z, double qx, double qy, double qz, double qw)
    {
        ThrowIfNotInitialized();

        try
        {
            // Get map origin BEFORE setting initial pose
            var mapBefore = GetStaticGridMap(reloadFromFile: false);
            _logger?.LogWarning("=== XlocClient.SetInitialPose - BEFORE ===");
            if (mapBefore != null)
            {
                _logger?.LogWarning("Map origin BEFORE: ({X}, {Y}, {Z})", 
                    mapBefore.Origin.X, mapBefore.Origin.Y, mapBefore.Origin.Z);
            }
            else
            {
                _logger?.LogWarning("Map origin BEFORE: NULL (no map available)");
            }

            var pose = new xloc_pose_t
            {
                position = new double[] { x, y, z },
                orientation = new double[] { qx, qy, qz, qw }
            };

            _logger?.LogWarning("Calling xloc_set_initial_pose: ({X}, {Y}, {Z}) Q:({Qx}, {Qy}, {Qz}, {Qw})", 
                x, y, z, qx, qy, qz, qw);

            xloc_status_response_t response;
            lock (_lock)
            {
                response = XlocNativeInterface.xloc_set_initial_pose(_handle, ref pose);
            }

            string message = XlocConversionExtensions.GetMessageAndFree(ref response);

            if (response.code == 0)
            {
                _logger?.LogInformation("Initial pose set to: ({X}, {Y}, {Z})", x, y, z);
                
                // Get map origin AFTER setting initial pose
                System.Threading.Thread.Sleep(50); // Small delay to let XLOC process
                var mapAfter = GetStaticGridMap(reloadFromFile: false);
                _logger?.LogWarning("=== XlocClient.SetInitialPose - AFTER ===");
                if (mapAfter != null)
                {
                    _logger?.LogWarning("Map origin AFTER: ({X}, {Y}, {Z})", 
                        mapAfter.Origin.X, mapAfter.Origin.Y, mapAfter.Origin.Z);
                    
                    // Compare with before
                    if (mapBefore != null)
                    {
                        var dx = Math.Abs(mapBefore.Origin.X - mapAfter.Origin.X);
                        var dy = Math.Abs(mapBefore.Origin.Y - mapAfter.Origin.Y);
                        var dz = Math.Abs(mapBefore.Origin.Z - mapAfter.Origin.Z);
                        
                        if (dx > 0.001 || dy > 0.001 || dz > 0.001)
                        {
                            _logger?.LogError("⚠️ CRITICAL ERROR: Map origin changed after xloc_set_initial_pose!");
                            _logger?.LogError("  Before: ({X}, {Y}, {Z})", mapBefore.Origin.X, mapBefore.Origin.Y, mapBefore.Origin.Z);
                            _logger?.LogError("  After: ({X}, {Y}, {Z})", mapAfter.Origin.X, mapAfter.Origin.Y, mapAfter.Origin.Z);
                            _logger?.LogError("  Difference: ΔX={DX:F3}m, ΔY={DY:F3}m, ΔZ={DZ:F3}m", dx, dy, dz);
                        }
                        else
                        {
                            _logger?.LogWarning("✓ Map origin unchanged after xloc_set_initial_pose ✓");
                        }
                    }
                }
                else
                {
                    _logger?.LogWarning("Map origin AFTER: NULL (no map available)");
                }
                
                return true;
            }
            else
            {
                _logger?.LogWarning("Failed to set initial pose: Code={Code}, Message={Message}", response.code, message);
                return false;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error setting initial pose");
            throw;
        }
    }

    /// <summary>
    /// Get current pose estimate from xloc
    /// </summary>
    public (double x, double y, double z, double qx, double qy, double qz, double qw)? GetCurrentPose()
    {
        ThrowIfNotInitialized();

        IntPtr posePtr = IntPtr.Zero;
        try
        {
            _logger?.LogTrace("[DEBUG] About to call xloc_get_current_pose()...");
            lock (_lock)
            {
                posePtr = XlocNativeInterface.xloc_get_current_pose(_handle);
            }
            _logger?.LogTrace("[DEBUG] xloc_get_current_pose() returned: {PosePtr}", posePtr);

            if (posePtr == IntPtr.Zero)
            {
                _logger?.LogWarning("xloc_get_current_pose returned null");
                return null;
            }

            _logger?.LogTrace("[DEBUG] Marshaling pose structure...");
            // Marshal the pose from unmanaged memory
            var pose = Marshal.PtrToStructure<xloc_pose_t>(posePtr);
            _logger?.LogTrace("[DEBUG] Pose marshaled successfully");
            
            return (
                pose.position[0], pose.position[1], pose.position[2],
                pose.orientation[0], pose.orientation[1], pose.orientation[2], pose.orientation[3]
            );
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DEBUG] Exception in GetCurrentPose: {Message}", ex.Message);
            return null;
        }
        finally
        {
            if (posePtr != IntPtr.Zero)
                XlocNativeInterface.xloc_free_pose(posePtr);
        }
    }

    /// <summary>
    /// Get static grid map (from loaded map file)
    /// </summary>
    public OccupancyGridData? GetStaticGridMap(bool reloadFromFile = false)
    {
        ThrowIfNotInitialized();

        IntPtr gridPtr = IntPtr.Zero;
        try
        {
            // _logger?.LogTrace("[DEBUG] About to call xloc_get_static_grid_map()...");
            lock (_lock)
            {
                gridPtr = XlocNativeInterface.xloc_get_static_grid_map(_handle, reloadFromFile ? 1 : 0);
            }
            // _logger?.LogTrace("[DEBUG] xloc_get_static_grid_map() returned: {GridPtr}", gridPtr);

            if (gridPtr == IntPtr.Zero)
            {
                // _logger?.LogWarning("xloc_get_static_grid_map returned null");
                return null;
            }

            // _logger?.LogTrace("[DEBUG] Marshaling occupancy grid structure...");
            var grid = Marshal.PtrToStructure<xloc_occupancy_grid_t>(gridPtr);
            // _logger?.LogInformation("[XLOC] GetStaticGridMap marshaled: width={Width}, height={Height}, resolution={Resolution}, data_length={DataLength}, data_ptr={DataPtr}",
            //     grid.width, grid.height, grid.resolution, grid.data_length, grid.data);

            // Marshal frame_id string
            string frameId = string.Empty;
            if (grid.header.frame_id != IntPtr.Zero)
            {
                frameId = Marshal.PtrToStringAnsi(grid.header.frame_id) ?? string.Empty;
            }

            // Copy occupancy data
            byte[] data = new byte[grid.data_length];
            if (grid.data != IntPtr.Zero && grid.data_length > 0)
            {
                Marshal.Copy(grid.data, data, 0, (int)grid.data_length);
            }

            var result = new OccupancyGridData
            {
                Resolution = grid.resolution,
                Width = grid.width,
                Height = grid.height,
                Origin = new PoseData
                {
                    X = grid.origin.position[0],
                    Y = grid.origin.position[1],
                    Z = grid.origin.position[2],
                    Qx = grid.origin.orientation[0],
                    Qy = grid.origin.orientation[1],
                    Qz = grid.origin.orientation[2],
                    Qw = grid.origin.orientation[3]
                },
                Data = data,
                FrameId = frameId
            };
            
            // _logger?.LogInformation("[XLOC] GetStaticGridMap returning: width={Width}, height={Height}, resolution={Resolution}, data_length={DataLength}",
            //     result.Width, result.Height, result.Resolution, result.Data?.Length ?? 0);
            
            return result;
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DEBUG] Exception in GetStaticGridMap: {Message}", ex.Message);
            return null;
        }
        finally
        {
            if (gridPtr != IntPtr.Zero)
                XlocNativeInterface.xloc_free_occupancy_grid(gridPtr);
        }
    }

    /// <summary>
    /// Get static grid map pointer (from loaded map file)
    /// Returns IntPtr to xloc_occupancy_grid_t (must be freed with xloc_free_occupancy_grid)
    /// </summary>
    public IntPtr GetStaticGridMapPtr(bool reloadFromFile = false)
    {
        ThrowIfNotInitialized();
        
        lock (_lock)
        {
            return XlocNativeInterface.xloc_get_static_grid_map(_handle, reloadFromFile ? 1 : 0);
        }
    }

    /// <summary>
    /// Get online grid map pointer (from SLAM)
    /// Returns IntPtr to xloc_occupancy_grid_t (must be freed with xloc_free_occupancy_grid)
    /// </summary>
    public IntPtr GetOnlineGridMapPtr()
    {
        ThrowIfNotInitialized();
        
        lock (_lock)
        {
            return XlocNativeInterface.xloc_get_online_grid_map(_handle);
        }
    }

    /// <summary>
    /// Get online grid map (from SLAM)
    /// </summary>
    public OccupancyGridData? GetOnlineGridMap()
    {
        ThrowIfNotInitialized();

        IntPtr gridPtr = IntPtr.Zero;
        try
        {
            _logger?.LogTrace("[DEBUG] About to call xloc_get_online_grid_map()...");
            lock (_lock)
            {
                gridPtr = XlocNativeInterface.xloc_get_online_grid_map(_handle);
            }
            _logger?.LogTrace("[DEBUG] xloc_get_online_grid_map() returned: {GridPtr}", gridPtr);

            if (gridPtr == IntPtr.Zero)
            {
                _logger?.LogWarning("xloc_get_online_grid_map returned null");
                return null;
            }

            _logger?.LogTrace("[DEBUG] Marshaling occupancy grid structure...");
            var grid = Marshal.PtrToStructure<xloc_occupancy_grid_t>(gridPtr);
            _logger?.LogTrace("[DEBUG] Occupancy grid marshaled successfully");

            // Marshal frame_id string
            string frameId = string.Empty;
            if (grid.header.frame_id != IntPtr.Zero)
            {
                frameId = Marshal.PtrToStringAnsi(grid.header.frame_id) ?? string.Empty;
            }

            // Copy occupancy data
            byte[] data = new byte[grid.data_length];
            if (grid.data != IntPtr.Zero && grid.data_length > 0)
            {
                Marshal.Copy(grid.data, data, 0, (int)grid.data_length);
            }

            return new OccupancyGridData
            {
                Resolution = grid.resolution,
                Width = grid.width,
                Height = grid.height,
                Origin = new PoseData
                {
                    X = grid.origin.position[0],
                    Y = grid.origin.position[1],
                    Z = grid.origin.position[2],
                    Qx = grid.origin.orientation[0],
                    Qy = grid.origin.orientation[1],
                    Qz = grid.origin.orientation[2],
                    Qw = grid.origin.orientation[3]
                },
                Data = data,
                FrameId = frameId
            };
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DEBUG] Exception in GetOnlineGridMap: {Message}", ex.Message);
            return null;
        }
        finally
        {
            if (gridPtr != IntPtr.Zero)
                XlocNativeInterface.xloc_free_occupancy_grid(gridPtr);
        }
    }

    public XlocDiagnosticsData? GetDiagnostics()
    {
        ThrowIfNotInitialized();

        IntPtr diagPtr = IntPtr.Zero;
        try
        {
            diagPtr = XlocNativeInterface.xloc_get_diagnostics(_handle);
            if (diagPtr == IntPtr.Zero)
            {
                _logger?.LogWarning("xloc_get_diagnostics returned null");
                return null;
            }

            var diag = Marshal.PtrToStructure<xloc_diagnostics_t>(diagPtr);
            
            // Marshal map name string
            string mapName = string.Empty;
            if (diag.current_active_map != IntPtr.Zero)
            {
                mapName = Marshal.PtrToStringAnsi(diag.current_active_map) ?? string.Empty;
            }

            // Marshal frame_id string from header
            string frameId = string.Empty;
            if (diag.header.frame_id != IntPtr.Zero)
            {
                frameId = Marshal.PtrToStringAnsi(diag.header.frame_id) ?? string.Empty;
            }

            return new XlocDiagnosticsData
            {
                HeaderSeq = diag.header.seq,
                HeaderStampSec = diag.header.stamp.sec,
                HeaderStampNsec = diag.header.stamp.nsec,
                HeaderFrameId = frameId,
                XlocState = diag.xloc_state,
                CurrentActiveMap = mapName,
                Reliability = diag.reliability,
                MatchingScore = diag.matching_score
            };
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error getting diagnostics");
            return null;
        }
        finally
        {
            if (diagPtr != IntPtr.Zero)
            {
                XlocNativeInterface.xloc_free_diagnostics(diagPtr);
            }
        }
    }



    private void ThrowIfNotInitialized()
    {
        lock (_lock)
        {
            if (_handle == IntPtr.Zero)
            {
                throw new InvalidOperationException("XlocClient is not initialized. Call Initialize() first.");
            }
        }
    }

    /// <summary>
    /// Lock-free initialization check for high-frequency dispatch paths.
    /// Uses volatile read of _handle to avoid lock contention with admin operations.
    /// </summary>
    private void ThrowIfNotInitializedFast()
    {
        if (_handle == IntPtr.Zero)
        {
            throw new InvalidOperationException("XlocClient is not initialized. Call Initialize() first.");
        }
    }

    public void Dispose()
    {
        Dispose(true);
        GC.SuppressFinalize(this);
    }

    protected virtual void Dispose(bool disposing)
    {
        if (_disposed)
            return;

        lock (_lock)
        {
            if (_handle != IntPtr.Zero)
            {
                try
                {
                    XlocNativeInterface.xloc_destroy(_handle);
                    _logger?.LogInformation("XlocClient disposed successfully");
                }
                catch (Exception ex)
                {
                    _logger?.LogError(ex, "Error disposing XlocClient");
                }
                finally
                {
                    _handle = IntPtr.Zero;
                }
            }
            
            // Note: TF buffer is managed by TF3BufferManager, do not destroy it here
            // Only reset the reference
            _tfBuffer = IntPtr.Zero;
        }

        _disposed = true;
    }

    ~XlocClient()
    {
        Dispose(false);
    }
}

/// <summary>
/// Occupancy grid data for display
/// </summary>
public class OccupancyGridData
{
    public float Resolution { get; set; }
    public uint Width { get; set; }
    public uint Height { get; set; }
    public PoseData Origin { get; set; } = new();
    public byte[] Data { get; set; } = Array.Empty<byte>();
    public string FrameId { get; set; } = string.Empty;
}

/// <summary>
/// Pose data
/// </summary>
public class PoseData
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Z { get; set; }
    public double Qx { get; set; }
    public double Qy { get; set; }
    public double Qz { get; set; }
    public double Qw { get; set; }
}

/// <summary>
/// XLOC diagnostics data
/// </summary>
public class XlocDiagnosticsData
{
    public uint HeaderSeq { get; set; }
    public uint HeaderStampSec { get; set; }
    public uint HeaderStampNsec { get; set; }
    public string HeaderFrameId { get; set; } = string.Empty;
    public byte XlocState { get; set; }  // 0=MAPPING, 1=LOCALIZATION, 2=PROCESSING, 3=READY, 4=ERROR
    public string CurrentActiveMap { get; set; } = string.Empty;
    public double Reliability { get; set; }  // 0.0 to 1.0
    public double MatchingScore { get; set; }  // SLAM matching quality
    
    public string StateString => XlocState switch
    {
        0 => "MAPPING",
        1 => "LOCALIZATION",
        2 => "PROCESSING",
        3 => "READY",
        4 => "ERROR",
        _ => $"Unknown ({XlocState})"
    };
}