using System.Runtime.InteropServices;
using System.IO;
using System.Linq;
using Microsoft.Extensions.Logging;
using RobotNet10.RobotApp.Xloc;
using RobotNet10.Shared.Geometry;
using System.Security.AccessControl;
using System.Formats.Tar;

namespace RobotNet10.RobotApp.Navigation;

/// <summary>
/// High-level wrapper for Navigation library
/// Provides thread-safe access to Navigation functionality with automatic resource management
/// </summary>
public class NavigationClient : IDisposable
{
    [DllImport("libc", EntryPoint = "free", CallingConvention = CallingConvention.Cdecl)]
    private static extern void NativeFree(IntPtr ptr);

    private IntPtr _handle = IntPtr.Zero;
    private IntPtr _tfBuffer = IntPtr.Zero;
    private readonly object _lock = new object();
    private bool _disposed = false;
    private readonly ILogger? _logger;
    private uint _sequenceNumber = 0;

    public NavigationClient(IntPtr tfBuffer, ILogger? logger = null)
    {
        _logger = logger;
        _tfBuffer = tfBuffer;
    }

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

            string navCorePath = "/usr/local/pnkx_nav_core";
            string originalDir = Directory.GetCurrentDirectory();
            bool directoryChanged = false;

            try
            {
                if (Directory.Exists(navCorePath))
                {
                    try
                    {
                        Directory.SetCurrentDirectory(navCorePath);
                        directoryChanged = true;
                        _logger?.LogInformation("Changed working directory to: {Path} (original: {Original})", navCorePath, originalDir);
                    }
                    catch (Exception ex)
                    {
                        _logger?.LogWarning("Failed to change working directory: {Message}. Continuing with current directory.", ex.Message);
                    }
                }
                else
                {
                    _logger?.LogWarning("Navigation core path not found: {Path}. Config files may not be found.", navCorePath);
                }

                if (_tfBuffer == IntPtr.Zero)
                {
                    throw new InvalidOperationException("TF buffer not initialized. Ensure TF3BufferManager is initialized first.");
                }

                _logger?.LogInformation("[NAV] ═══════════════════════════════════════════");
                _logger?.LogInformation("[NAV] Initializing NavigationClient...");
                _logger?.LogInformation("[NAV] TF buffer available: {TfBuffer}", _tfBuffer);

                _logger?.LogInformation("[NAV] Waiting for TF3 transforms to be indexed (500ms)...");
                System.Threading.Thread.Sleep(500);

                _handle = NavigationNativeInterface.navigation_create();

                if (_handle == IntPtr.Zero)
                {
                    throw new InvalidOperationException("Failed to create Navigation instance. navigation_create returned null.");
                }

                bool initResult = NavigationNativeInterface.navigation_initialize(_handle, _tfBuffer);
            }
            catch (Exception ex)
            {
                _logger?.LogError(ex, "[NAV] ❌ Exception in Initialize: {Message}", ex.Message);
                throw;
            }
            finally
            {
                if (directoryChanged)
                {
                    try
                    {
                        Directory.SetCurrentDirectory(originalDir);
                        _logger?.LogInformation("Restored working directory to: {Path}", originalDir);
                    }
                    catch (Exception ex)
                    {
                        _logger?.LogWarning("Failed to restore working directory: {Message}", ex.Message);
                    }
                }
            }
        }
    }

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

    /// <summary>
    /// Get navigation handle (for direct API calls)
    /// </summary>
    internal IntPtr Handle
    {
        get
        {
            lock (_lock)
            {
                return _handle;
            }
        }
    }

    /// <summary>
    /// Update map -> odom transform from Xloc pose
    /// </summary>
    public bool UpdateMapToOdomTransform(double x, double y, double z, double qx, double qy, double qz, double qw)
    {
        if (_tfBuffer == IntPtr.Zero)
            return false;

        try
        {
            double quatNorm = Math.Sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
            double finalQx = qx;
            double finalQy = qy;
            double finalQz = qz;
            double finalQw = qw;

            if (quatNorm < 1e-6)
            {
                finalQx = 0.0;
                finalQy = 0.0;
                finalQz = 0.0;
                finalQw = 1.0;
                _logger?.LogWarning("Invalid quaternion (norm={Norm:F6}), using identity quaternion for map->odom transform", quatNorm);
            }
            else if (Math.Abs(quatNorm - 1.0) > 1e-3)
            {
                finalQx = qx / quatNorm;
                finalQy = qy / quatNorm;
                finalQz = qz / quatNorm;
                finalQw = qw / quatNorm;
                _logger?.LogTrace("Quaternion normalized from norm {Norm:F6} to 1.0", quatNorm);
            }

            var now = DateTime.UtcNow;
            long sec = ((DateTimeOffset)now).ToUnixTimeSeconds();
            long nsec = (now.Ticks % TimeSpan.TicksPerSecond) * 100;

            var mapToOdom = new TF3.TF3_Transform
            {
                timestamp_sec = sec,
                timestamp_nsec = nsec,
                frame_id = "map",
                child_frame_id = "odom",
                translation_x = x,
                translation_y = y,
                translation_z = z,
                rotation_x = finalQx,
                rotation_y = finalQy,
                rotation_z = finalQz,
                rotation_w = finalQw
            };

            lock (_lock)
            {
                // Dynamic transform: costmap checks stamp freshness (transform_tolerance ~1s).
                bool success = TF3.TF3NativeInterface.tf3_set_transform(
                    _tfBuffer, ref mapToOdom, "navigation_client", false);

                if (success)
                {
                    _logger?.LogTrace("Updated transform: map → odom ({X:F2}, {Y:F2}, {Z:F2})", x, y, z);
                }
                return success;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error updating map->odom transform");
            return false;
        }
    }

    /// <summary>
    /// Dispatch static map to navigation system
    /// </summary>
    public bool DispatchStaticMap(OccupancyGridData mapData, string mapName)
    {
        ThrowIfNotInitialized();
        if (mapData == null)
        {
            throw new ArgumentNullException(nameof(mapData));
        }

        if (!IsNavigationReady())
        {
            _logger?.LogWarning("Navigation is not ready (is_ready = false). Cannot add static map {MapName}", mapName);
            return false;
        }

        OccupancyGrid navGrid = default;
        IntPtr navGridPtr = IntPtr.Zero;
        try
        {
            var timestamp = DateTime.Now.ToString("HH:mm:ss.fff");

            var xlocGrid = new xloc_occupancy_grid_t
            {
                header = new xloc_header_t
                {
                    seq = 0,
                    stamp = new xloc_unix_time_t { sec = 0, nsec = 0 },
                    frame_id = Marshal.StringToHGlobalAnsi(mapData.FrameId ?? "map")
                },
                resolution = mapData.Resolution,
                width = mapData.Width,
                height = mapData.Height,
                origin = new xloc_pose_t
                {
                    position = new double[] { mapData.Origin.X, mapData.Origin.Y, mapData.Origin.Z },
                    orientation = new double[] { mapData.Origin.Qx, mapData.Origin.Qy, mapData.Origin.Qz, mapData.Origin.Qw }
                },
                data = IntPtr.Zero,
                data_length = (nuint)(mapData.Data?.Length ?? 0)
            };

            if (mapData.Data != null && mapData.Data.Length > 0)
            {
                xlocGrid.data = Marshal.AllocHGlobal(mapData.Data.Length);
                Marshal.Copy(mapData.Data, 0, xlocGrid.data, mapData.Data.Length);
            }

            try
            {
                navGrid = xlocGrid.ToNavigationOccupancyGrid();
                navGridPtr = NavigationConversionExtensions.AllocateNavigationOccupancyGrid(navGrid);

                bool result;
                lock (_lock)
                {
                    result = NavigationNativeInterface.navigation_add_static_map(_handle, mapName, navGrid);
                }

                if (result)
                {
                    _logger?.LogInformation("[{Time}] [DISPATCH-DONE] Static map {MapName} added successfully", timestamp, mapName);
                }
                else
                {
                    _logger?.LogError("[{Time}] [DISPATCH-ERROR] Failed to add static map {MapName}", timestamp, mapName);
                }

                return result;
            }
            finally
            {
                if (xlocGrid.data != IntPtr.Zero)
                {
                    Marshal.FreeHGlobal(xlocGrid.data);
                }
                if (xlocGrid.header.frame_id != IntPtr.Zero)
                {
                    Marshal.FreeHGlobal(xlocGrid.header.frame_id);
                }
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DISPATCH-ERROR] Static map dispatch failed: {Message}", ex.Message);
            return false;
        }
        finally
        {
            if (navGridPtr != IntPtr.Zero)
            {
                NavigationConversionExtensions.FreeNavigationOccupancyGridPtr(navGridPtr);
            }
            else
            {
                NavigationConversionExtensions.FreeNavigationOccupancyGrid(ref navGrid);
            }
        }
    }

    /// <summary>
    /// Dispatch laser scan to navigation system
    /// NOTE: This method does NOT free the memory allocated in the LaserScan struct.
    /// Caller is responsible for freeing memory using FreeNavigationLaserScan after dispatch.
    /// </summary>
    public void DispatchLaserScan(LaserScan scan, string laserScanName)
    {
        ThrowIfNotInitialized();
        try
        {
            lock (_lock)
            {
                NavigationNativeInterface.navigation_add_laser_scan(_handle, laserScanName, scan);
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DISPATCH-ERROR] LaserScan dispatch failed: {Message}", ex.Message);
            throw;
        }
    }

    /// <summary>
    /// Dispatch odometry to navigation system
    /// NOTE: This method does NOT free the memory allocated in the Odometry struct.
    /// Caller is responsible for freeing memory using FreeNavigationOdometry after dispatch.
    /// </summary>
    public void DispatchOdometry(Odometry odom, string odometryName)
    {
        ThrowIfNotInitialized();
        try
        {
            lock (_lock)
            {
                var now = DateTime.UtcNow;
                long sec = ((DateTimeOffset)now).ToUnixTimeSeconds();
                long nsec = (now.Ticks % TimeSpan.TicksPerSecond) * 100;
                var odomTobaselink = new TF3.TF3_Transform
                {
                    timestamp_sec = sec,
                    timestamp_nsec = nsec,
                    frame_id = "odom",
                    child_frame_id = "base_footprint",
                    translation_x = odom.pose.pose.position.x,
                    translation_y = odom.pose.pose.position.y,
                    translation_z = odom.pose.pose.position.z,
                    rotation_x = odom.pose.pose.orientation.x,
                    rotation_y = odom.pose.pose.orientation.y,
                    rotation_z = odom.pose.pose.orientation.z,
                    rotation_w = odom.pose.pose.orientation.w
                };
                TF3.TF3NativeInterface.tf3_set_transform(_tfBuffer, ref odomTobaselink, "navigation_client", false);
                NavigationNativeInterface.navigation_add_odometry(_handle, odometryName, odom);
                var errorCode = new TF3.TF3_ErrorCode();
                TF3.TF3_Transform result = new TF3.TF3_Transform();
                bool lookup = TF3.TF3NativeInterface.tf3_lookup_transform(
                    _tfBuffer,
                    "map",
                    "base_footprint",
                    sec,
                    nsec,
                    ref result,
                    ref errorCode);
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DISPATCH-ERROR] Odometry dispatch failed: {Message}", ex.Message);
            throw;
        }
    }

    /// <summary>
    /// Dispatch gridmap to navigation system
    /// NOTE: This method does NOT free the memory allocated in the OccupancyGrid struct.
    /// Caller is responsible for freeing memory using FreeNavigationOccupancyGrid after dispatch.
    /// </summary>
    public void DispatchGridMap(OccupancyGrid grid, string gridMapName)
    {
        ThrowIfNotInitialized();
        try
        {
            lock (_lock)
            {
                NavigationNativeInterface.navigation_add_static_map(_handle, gridMapName, grid);
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DISPATCH-ERROR] GridMap dispatch failed: {Message}", ex.Message);
            throw;
        }
    }

    /// <summary>
    /// Check if navigation is ready to receive data
    /// </summary>
    private bool IsNavigationReady()
    {
        NavFeedback feedback = default;
        bool feedbackRetrieved = false;
        try
        {
            lock (_lock)
            {
                bool success = NavigationNativeInterface.navigation_get_feedback(_handle, ref feedback);
                if (!success)
                {
                    return false;
                }
                feedbackRetrieved = true;
                return feedback.is_ready;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogTrace(ex, "Failed to check navigation ready status");
            return false;
        }
    }

    /// <summary>
    /// Set the robot's footprint (outline shape)
    /// </summary>
    public bool SetRobotFootprint(Point[] points)
    {
        ThrowIfNotInitialized();

        if (points == null || points.Length == 0)
        {
            throw new ArgumentException("Points array cannot be null or empty", nameof(points));
        }

        try
        {
            GCHandle pointsHandle = GCHandle.Alloc(points, GCHandleType.Pinned);
            try
            {
                IntPtr pointsPtr = pointsHandle.AddrOfPinnedObject();

                lock (_lock)
                {
                    return NavigationNativeInterface.navigation_set_robot_footprint(
                        _handle,
                        pointsPtr,
                        (nuint)points.Length);
                }
            }
            finally
            {
                if (pointsHandle.IsAllocated)
                {
                    pointsHandle.Free();
                }
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error setting robot footprint");
            throw;
        }
    }

    /// <summary>
    /// Get the robot's footprint (outline shape)
    /// </summary>
    public Point[]? GetRobotFootprint()
    {
        ThrowIfNotInitialized();

        try
        {
            IntPtr pointsPtr = IntPtr.Zero;
            nuint count = 0;

            lock (_lock)
            {
                bool success = NavigationNativeInterface.navigation_get_robot_footprint(
                    _handle,
                    out pointsPtr,
                    out count);

                if (!success || pointsPtr == IntPtr.Zero)
                {
                    return null;
                }
            }

            try
            {
                int pointCount = (int)count.ToUInt32();
                Point[] points = new Point[pointCount];

                for (int i = 0; i < pointCount; i++)
                {
                    IntPtr pointPtr = IntPtr.Add(pointsPtr, i * Marshal.SizeOf<Point>());
                    points[i] = Marshal.PtrToStructure<Point>(pointPtr);
                }

                return points;
            }
            finally
            {
                if (pointsPtr != IntPtr.Zero)
                {
                    NavigationNativeInterface.navigation_free_points(pointsPtr);
                }
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error getting robot footprint");
            return null;
        }
    }

    /// <summary>
    /// Send a goal for the robot to navigate to
    /// </summary>
    public bool MoveTo(double x, double y, double z, double qx, double qy, double qz, double qw,
                       string frameId = "map",
                       double xyTolerance = 0.1,
                       double yawTolerance = 0.1)
    {
        ThrowIfNotInitialized();

        try
        {
            var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);

            lock (_lock)
            {
                bool result = NavigationNativeInterface.navigation_move_to(_handle, goal);
                FreePoseStamped(ref goal);
                return result;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error sending move_to command");
            throw;
        }
    }

    /// <summary>
    /// Send a goal for the robot to navigate to with order
    /// </summary>
    public bool MoveToOrder(IntPtr orderHandle, double x, double y, double z, double qx, double qy, double qz, double qw,
                            string frameId = "map",
                            double xyTolerance = 0.1,
                            double yawTolerance = 0.1)
    {
        ThrowIfNotInitialized();

        if (orderHandle == IntPtr.Zero)
        {
            throw new ArgumentException("Order handle cannot be zero", nameof(orderHandle));
        }

        long handleValue = orderHandle.ToInt64();
        if (handleValue < 0x1000 || handleValue > 0x7FFFFFFFFFFF)
        {
            string errorMsg = $"Invalid order handle value: {handleValue} (0x{handleValue:X}). Order handle must be a valid pointer to an Order object created by the navigation system. Value must be between 0x1000 and 0x7FFFFFFFFFFF.";
            _logger?.LogDebug(errorMsg);
            throw new ArgumentException(errorMsg, nameof(orderHandle));
        }

        if (IsLikelyTestValue(handleValue))
        {
            string errorMsg = $"Order handle {handleValue} (0x{handleValue:X}) appears to be a test value. Order handle must be a valid pointer to an Order object created by the navigation system, not a test value.";
            _logger?.LogDebug(errorMsg);
            throw new ArgumentException(errorMsg, nameof(orderHandle));
        }

        try
        {
            var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);

            lock (_lock)
            {
                try
                {
                    Order order = Marshal.PtrToStructure<Order>(orderHandle);

                    bool result = NavigationNativeInterface.navigation_move_to_order(_handle, order, goal);

                    FreePoseStamped(ref goal);

                    if (result)
                    {
                        _logger?.LogInformation("navigation_move_to_order returned true. Command sent successfully with order_handle: {OrderHandle}", orderHandle.ToInt64());
                    }
                    else
                    {
                        _logger?.LogWarning("navigation_move_to_order returned false. Order handle may be invalid: {OrderHandle}", orderHandle.ToInt64());
                    }

                    return result;
                }
                catch (AccessViolationException avex)
                {
                    FreePoseStamped(ref goal);
                    _logger?.LogError(avex, "Access violation when calling navigation_move_to_order. Order handle may be invalid pointer: {OrderHandle}", orderHandle.ToInt64());
                    throw new ArgumentException($"Invalid order handle: {orderHandle.ToInt64()}. Order handle must be a valid pointer to Order object.", nameof(orderHandle), avex);
                }
                catch (SEHException sehex)
                {
                    FreePoseStamped(ref goal);
                    _logger?.LogError(sehex, "Structured exception (SEH) when calling navigation_move_to_order. Order handle may be invalid pointer: {OrderHandle}", orderHandle.ToInt64());
                    throw new ArgumentException($"Invalid order handle: {orderHandle.ToInt64()}. Order handle must be a valid pointer to Order object.", nameof(orderHandle), sehex);
                }
            }
        }
        catch (ArgumentException)
        {
            throw;
        }
        catch (AccessViolationException ex)
        {
            _logger?.LogError(ex, "Access violation when calling navigation_move_to_order. Order handle may be invalid pointer: {OrderHandle}", orderHandle.ToInt64());
            throw new ArgumentException($"Invalid order handle: {orderHandle.ToInt64()}. Order handle must be a valid pointer to Order object.", nameof(orderHandle), ex);
        }
        catch (SEHException ex)
        {
            _logger?.LogError(ex, "Structured exception (SEH) when calling navigation_move_to_order. Order handle may be invalid pointer: {OrderHandle}", orderHandle.ToInt64());
            throw new ArgumentException($"Invalid order handle: {orderHandle.ToInt64()}. Order handle must be a valid pointer to Order object.", nameof(orderHandle), ex);
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error sending move_to_order command with order handle: {OrderHandle}", orderHandle.ToInt64());
            throw;
        }
    }

    /// <summary>
    /// Check if a handle value looks like a test value (common patterns like 0x12345678, 0xDEADBEEF, etc.)
    /// </summary>
    private static bool IsLikelyTestValue(long handleValue)
    {
        long[] testValues = {
            0x12345678, 0x123456789, 0x1234567890, 0x12345678901, 0x123456789012,
            0xDEADBEEF, unchecked((long)0xDEADBEEFDEADBEEFUL),
            0xCAFEBABE, unchecked((long)0xCAFEBABECAFEBABEUL),
            0x00000000, unchecked((long)0xFFFFFFFFUL), unchecked((long)0xFFFFFFFFFFFFFFFFUL),
            0x11111111, 0x22222222, 0x33333333, 0x44444444,
            0x55555555, 0x66666666, 0x77777777, 0x88888888,
            0x99999999, unchecked((long)0xAAAAAAAAUL), unchecked((long)0xBBBBBBBBUL), unchecked((long)0xCCCCCCCCUL),
            unchecked((long)0xDDDDDDDDUL), unchecked((long)0xEEEEEEEEUL), unchecked((long)0xFFFFFFFFUL)
        };

        foreach (var testVal in testValues)
        {
            if (handleValue == testVal)
                return true;
        }

        string hexStr = handleValue.ToString("X");
        if (hexStr.Length >= 4)
        {
            if (hexStr.All(c => c == hexStr[0]) ||
                (hexStr.Length >= 8 && hexStr.Substring(0, 4) == hexStr.Substring(4, 4)))
            {
                return true;
            }
        }

        return false;
    }

    /// <summary>
    /// Send a goal for the robot to navigate to with order (using OrderHandle wrapper)
    /// </summary>
    public bool MoveToOrder(OrderHandle orderHandle, double x, double y, double z, double qx, double qy, double qz, double qw,
                            string frameId = "map",
                            double xyTolerance = 0.1,
                            double yawTolerance = 0.1)
    {
        if (orderHandle == null)
        {
            throw new ArgumentNullException(nameof(orderHandle));
        }

        if (!orderHandle.IsValid)
        {
            throw new ArgumentException("Order handle is not valid (zero pointer)", nameof(orderHandle));
        }

        return MoveToOrder(orderHandle.Handle, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
    }

    /// <summary>
    /// Send a goal for the robot to navigate to with order (using OrderData)
    /// </summary>
    public bool MoveToOrder(OrderData orderData, double x, double y, double z, double qx, double qy, double qz, double qw,
                            string frameId = "map",
                            double xyTolerance = 0.1,
                            double yawTolerance = 0.1)
    {
        ThrowIfNotInitialized();

        if (orderData == null)
        {
            throw new ArgumentNullException(nameof(orderData));
        }

        Order nativeOrder = default;
        try
        {
            nativeOrder = OrderConverter.ConvertToNativeOrder(orderData);

            var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);

            lock (_lock)
            {
                bool result = NavigationNativeInterface.navigation_move_to_order(_handle, nativeOrder, goal);

                FreePoseStamped(ref goal);

                if (result)
                {
                    _logger?.LogInformation("navigation_move_to_order returned true. Command sent successfully with order: {OrderId}", orderData.OrderId);
                }
                else
                {
                    _logger?.LogWarning("navigation_move_to_order returned false for order: {OrderId}", orderData.OrderId);
                }

                return result;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error sending move_to_order command with order: {OrderId}", orderData.OrderId);
            throw;
        }
        finally
        {
            OrderConverter.FreeNativeOrder(ref nativeOrder);
        }
    }

    /// <summary>
    /// Send a docking goal to a predefined marker
    /// </summary>
    public bool DockTo(string marker, double x, double y, double z, double qx, double qy, double qz, double qw,
                      string frameId = "map",
                      double xyTolerance = 0.05,
                      double yawTolerance = 0.05)
    {
        ThrowIfNotInitialized();

        try
        {
            var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);

            lock (_lock)
            {
                bool result = NavigationNativeInterface.navigation_dock_to(_handle, marker, goal);
                FreePoseStamped(ref goal);
                return result;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error sending dock_to command");
            throw;
        }
    }

    /// <summary>
    /// Send a docking goal with VDA5050 order payload.
    /// </summary>
    public bool DockToOrder(OrderData orderData, string marker, double x, double y, double z, double qx, double qy, double qz, double qw,
        string frameId = "map")
    {
        ThrowIfNotInitialized();

        if (orderData == null)
            throw new ArgumentNullException(nameof(orderData));

        Order nativeOrder = default;
        try
        {
            nativeOrder = OrderConverter.ConvertToNativeOrder(orderData);
            var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);

            lock (_lock)
            {
                bool result = NavigationNativeInterface.navigation_dock_to_order(_handle, nativeOrder, marker, goal);
                FreePoseStamped(ref goal);
                return result;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error sending dock_to_order command with order: {OrderId}", orderData.OrderId);
            throw;
        }
        finally
        {
            OrderConverter.FreeNativeOrder(ref nativeOrder);
        }
    }

    /// <summary>
    /// Move straight toward the target position
    /// </summary>
    public bool MoveStraightTo(double distance)
    {
        ThrowIfNotInitialized();

        try
        {
            lock (_lock)
            {
                bool result = NavigationNativeInterface.navigation_move_straight_to(_handle, distance);
                Console.WriteLine($"navigation_move_straight_to returned: success");
                return result;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error sending move_straight_to command");
            throw;
        }
    }

    /// <summary>
    /// Rotate in place to align with target orientation
    /// </summary>
    public bool RotateTo(double x, double y, double z, double qx, double qy, double qz, double qw,
                        string frameId = "map",
                        double yawTolerance = 0.1)
    {
        ThrowIfNotInitialized();

        try
        {
            var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);

            lock (_lock)
            {
                bool result = NavigationNativeInterface.navigation_rotate_to(_handle, goal);
                FreePoseStamped(ref goal);
                return result;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error sending rotate_to command");
            throw;
        }
    }

    /// <summary>
    /// Pause the robot's movement
    /// </summary>
    public void Pause()
    {
        ThrowIfNotInitialized();

        try
        {
            lock (_lock)
            {
                NavigationNativeInterface.navigation_pause(_handle);
            }
            _logger?.LogInformation("Navigation paused");
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error pausing navigation");
            throw;
        }
    }

    /// <summary>
    /// Resume motion after a pause
    /// </summary>
    public void Resume()
    {
        ThrowIfNotInitialized();

        try
        {
            lock (_lock)
            {
                NavigationNativeInterface.navigation_resume(_handle);
            }
            _logger?.LogInformation("Navigation resumed");
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error resuming navigation");
            throw;
        }
    }

    /// <summary>
    /// Cancel the current goal and stop the robot
    /// </summary>
    public void Cancel()
    {
        ThrowIfNotInitialized();

        try
        {
            lock (_lock)
            {
                NavigationNativeInterface.navigation_cancel(_handle);
            }
            _logger?.LogInformation("Navigation cancelled");
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error cancelling navigation");
            throw;
        }
    }

    /// <summary>
    /// Send limited linear velocity command
    /// </summary>
    public bool SetTwistLinear(double linearX, double linearY, double linearZ)
    {
        ThrowIfNotInitialized();

        try
        {
            lock (_lock)
            {
                bool success = NavigationNativeInterface.navigation_set_twist_linear(
                    _handle,
                    linearX, linearY, linearZ);
                _logger?.LogInformation("Set linear twist: ({LinearX:F2}, {LinearY:F2}, {LinearZ:F2}), success: {Success}", linearX, linearY, linearZ, success);
                return success;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error setting linear twist");
            throw;
        }
    }

    /// <summary>
    /// Send limited angular velocity command
    /// </summary>
    public bool SetTwistAngular(double angularX, double angularY, double angularZ)
    {
        ThrowIfNotInitialized();

        NavFeedback feedback = default;
        bool feedbackRetrieved = false;
        try
        {
            lock (_lock)
            {
                bool success = NavigationNativeInterface.navigation_get_feedback(_handle, ref feedback);
                if (!success)
                {
                    return false;
                }
                feedbackRetrieved = true;
                if (!feedback.is_ready)
                {
                    _logger?.LogTrace("Navigation is not ready (is_ready = false). Cannot set twist angular");
                    return false;
                }
            }
        }
        catch (Exception ex)
        {
            _logger?.LogTrace(ex, "Failed to check navigation ready status. Proceeding with caution.");
        }
        finally
        {
            if (feedbackRetrieved)
            {
                if (feedback.feed_back_str != IntPtr.Zero)
                {
                    NavigationNativeInterface.nav_c_api_free_string(feedback.feed_back_str);
                }
            }
        }

        try
        {
            lock (_lock)
            {
                return NavigationNativeInterface.navigation_set_twist_angular(
                    _handle,
                    angularX, angularY, angularZ);
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error setting angular twist");
            throw;
        }
    }

    /// <summary>
    /// Get the robot's pose as a PoseStamped
    /// </summary>
    public (double x, double y, double z, double qx, double qy, double qz, double qw, string frameId)? GetRobotPoseStamped()
    {
        ThrowIfNotInitialized();

        try
        {
            PoseStamped pose = default;
            lock (_lock)
            {
                bool success = NavigationNativeInterface.navigation_get_robot_pose_stamped(_handle, ref pose);
                if (!success)
                {
                    return null;
                }
            }

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

                return (
                    pose.pose.position.x,
                    pose.pose.position.y,
                    pose.pose.position.z,
                    pose.pose.orientation.x,
                    pose.pose.orientation.y,
                    pose.pose.orientation.z,
                    pose.pose.orientation.w,
                    frameId
                );
            }
            finally
            {
                if (pose.header.frame_id != IntPtr.Zero)
                {
                    NavigationNativeInterface.nav_c_api_free_string(pose.header.frame_id);
                }
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error getting robot pose");
            return null;
        }
    }

    /// <summary>
    /// Get the robot's pose as a 2D pose
    /// </summary>
    public (double x, double y, double theta)? GetRobotPose2D()
    {
        ThrowIfNotInitialized();

        try
        {
            Pose2D pose = default;
            lock (_lock)
            {
                bool success = NavigationNativeInterface.navigation_get_robot_pose_2d(_handle, ref pose);
                if (!success)
                {
                    return null;
                }
            }

            return (pose.x, pose.y, pose.theta);
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error getting robot pose 2D");
            return null;
        }
    }

    /// <summary>
    /// Get the robot's current twist
    /// </summary>
    public (double x, double y, double theta, string frameId)? GetTwist()
    {
        ThrowIfNotInitialized();

        try
        {
            var twist = new Twist2DStamped
            {
                header = new Header
                {
                    seq = _sequenceNumber++,
                    sec = (uint)DateTime.Now.Second,
                    nsec = (uint)(DateTime.Now.Millisecond * 1_000_000),
                    frame_id = Marshal.StringToHGlobalAnsi("base_footprint")
                },
                velocity = new Twist2D
                {
                    x = default,
                    y = default,
                    theta = default
                }
            };
            bool success = false;

            lock (_lock)
            {
                success = NavigationNativeInterface.navigation_get_twist(_handle, ref twist);

                if (!success)
                {
                    return null;
                }
            }
            try
            {
                string frameId = string.Empty;
                if (twist.header.frame_id != IntPtr.Zero)
                {
                    frameId = Marshal.PtrToStringAnsi(twist.header.frame_id) ?? string.Empty;
                }

                return (twist.velocity.x, twist.velocity.y, twist.velocity.theta, frameId);
            }
            finally
            {
                if (success && twist.header.frame_id != IntPtr.Zero)
                {
                    NavigationNativeInterface.nav_c_api_free_string(twist.header.frame_id);
                }
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error getting twist");
            return null;
        }
    }

    /// <summary>
    /// Get the robot's current twist using struct reference (thread-safe)
    /// Caller is responsible for freeing frame_id string using nav_c_api_free_string
    /// </summary>
    internal bool GetTwistStruct(ref Twist2DStamped twist)
    {
        ThrowIfNotInitialized();

        lock (_lock)
        {
            return NavigationNativeInterface.navigation_get_twist(_handle, ref twist);
        }
    }

    /// <summary>
    /// Get navigation feedback
    /// </summary>
    public NavigationFeedbackData? GetFeedback()
    {
        ThrowIfNotInitialized();

        NavFeedback feedback = default;
        bool feedbackRetrieved = false;
        try
        {
            lock (_lock)
            {
                bool success = NavigationNativeInterface.navigation_get_feedback(_handle, ref feedback);
                if (!success)
                {
                    return null;
                }
                feedbackRetrieved = true;
            }

            try
            {
                string feedbackStr = string.Empty;
                if (feedback.feed_back_str != IntPtr.Zero)
                {
                    feedbackStr = Marshal.PtrToStringAnsi(feedback.feed_back_str) ?? string.Empty;
                }

                return new NavigationFeedbackData
                {
                    NavigationState = feedback.navigation_state,
                    FeedbackString = feedbackStr,
                    CurrentPose = new Pose2DData
                    {
                        X = feedback.current_pose.x,
                        Y = feedback.current_pose.y,
                        Theta = feedback.current_pose.theta
                    },
                    GoalChecked = feedback.goal_checked,
                    IsReady = feedback.is_ready
                };
            }
            catch (Exception ex)
            {
                _logger?.LogError(ex, "Error getting feedback");
                return null;
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error getting feedback");
            return null;
        }
        finally
        {
            if (feedbackRetrieved)
            {
                if (feedback.feed_back_str != IntPtr.Zero)
                {
                    NavigationNativeInterface.nav_c_api_free_string(feedback.feed_back_str);
                }
            }
        }
    }

    /// <summary>
    /// Get global planner path data from navigation_get_global_data.
    /// </summary>
    public GlobalPathData? GetGlobalPathData()
    {
        ThrowIfNotInitialized();

        PlannerDataOutput plannerData = default;
        bool fetched = false;

        try
        {
            lock (_lock)
            {
                fetched = NavigationNativeInterface.navigation_get_global_data(_handle, ref plannerData);
            }

            if (!fetched)
            {
                return null;
            }

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

            var points = new List<GlobalPathPointData>();
            int pointCount = (int)plannerData.plan.poses_count;
            if (plannerData.plan.poses != IntPtr.Zero && pointCount > 0)
            {
                int poseSize = Marshal.SizeOf<Pose2DStamped>();
                for (int index = 0; index < pointCount; index++)
                {
                    IntPtr posePtr = IntPtr.Add(plannerData.plan.poses, index * poseSize);
                    Pose2DStamped pose = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
                    points.Add(new GlobalPathPointData
                    {
                        X = pose.pose.x,
                        Y = pose.pose.y,
                        Theta = pose.pose.theta
                    });
                }
            }

            return new GlobalPathData
            {
                FrameId = frameId,
                Points = points
            };
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error getting global planner data");
            return null;
        }
        finally
        {
            if (fetched)
            {
                FreePlannerDataOutput(ref plannerData);
            }
        }
    }

    /// <summary>
    /// Get local planner path data from navigation_get_local_data.
    /// </summary>
    public GlobalPathData? GetLocalPathData()
    {
        ThrowIfNotInitialized();

        PlannerDataOutput plannerData = default;
        bool fetched = false;

        try
        {
            lock (_lock)
            {
                fetched = NavigationNativeInterface.navigation_get_local_data(_handle, ref plannerData);
            }

            if (!fetched)
            {
                return null;
            }

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

            var points = new List<GlobalPathPointData>();
            int pointCount = (int)plannerData.plan.poses_count;
            if (plannerData.plan.poses != IntPtr.Zero && pointCount > 0)
            {
                int poseSize = Marshal.SizeOf<Pose2DStamped>();
                for (int index = 0; index < pointCount; index++)
                {
                    IntPtr posePtr = IntPtr.Add(plannerData.plan.poses, index * poseSize);
                    Pose2DStamped pose = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
                    points.Add(new GlobalPathPointData
                    {
                        X = pose.pose.x,
                        Y = pose.pose.y,
                        Theta = pose.pose.theta
                    });
                }
            }

            return new GlobalPathData
            {
                FrameId = frameId,
                Points = points
            };
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error getting local planner path data");
            return null;
        }
        finally
        {
            if (fetched)
            {
                FreePlannerDataOutput(ref plannerData);
            }
        }
    }

    /// <summary>
    /// Get cost map data from local planner (navigation_get_local_data)
    /// Returns occupancy grid with cost values (0-254 where 254=obstacle, 0=free)
    /// </summary>
    public CostMapData? GetCostMapData()
    {
        ThrowIfNotInitialized();

        PlannerDataOutput plannerData = default;
        bool fetched = false;

        try
        {
            lock (_lock)
            {
                fetched = NavigationNativeInterface.navigation_get_local_data(_handle, ref plannerData);
            }

            if (!fetched)
            {
                return null;
            }

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

            string updateFrameId = string.Empty;
            if (plannerData.costmap_update.header.frame_id != IntPtr.Zero)
            {
                updateFrameId = Marshal.PtrToStringAnsi(plannerData.costmap_update.header.frame_id) ?? string.Empty;
            }

            bool hasFullMap = plannerData.costmap.data != IntPtr.Zero
                && plannerData.costmap.data_count != 0
                && plannerData.costmap.info.width > 0
                && plannerData.costmap.info.height > 0;

            bool isCostmapUpdated = plannerData.is_costmap_updated
                && plannerData.costmap_update.data != IntPtr.Zero
                && plannerData.costmap_update.data_count != 0
                && plannerData.costmap_update.width > 0
                && plannerData.costmap_update.height > 0;

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

            byte[] costData = Array.Empty<byte>();
            if (hasFullMap)
            {
                int dataCount = (int)plannerData.costmap.data_count;
                costData = new byte[dataCount];
                if (dataCount > 0)
                {
                    Marshal.Copy(plannerData.costmap.data, costData, 0, dataCount);
                }
            }

            byte[] updateData = Array.Empty<byte>();
            if (isCostmapUpdated)
            {
                int updateCount = (int)plannerData.costmap_update.data_count;
                updateData = new byte[updateCount];
                if (updateCount > 0)
                {
                    Marshal.Copy(plannerData.costmap_update.data, updateData, 0, updateCount);
                }
            }

            double originTheta = hasFullMap
                ? QuaternionToYaw(plannerData.costmap.info.origin.orientation)
                : 0.0;

            return new CostMapData
            {
                FrameId = frameId,
                Resolution = hasFullMap ? plannerData.costmap.info.resolution : 0.0,
                Width = hasFullMap ? (int)plannerData.costmap.info.width : 0,
                Height = hasFullMap ? (int)plannerData.costmap.info.height : 0,
                OriginX = hasFullMap ? plannerData.costmap.info.origin.position.x : 0.0,
                OriginY = hasFullMap ? plannerData.costmap.info.origin.position.y : 0.0,
                OriginTheta = originTheta,
                Data = costData,
                HasFullMap = hasFullMap,
                UpdateFrameId = updateFrameId,
                IsCostmapUpdated = isCostmapUpdated,
                UpdateX = plannerData.costmap_update.x,
                UpdateY = plannerData.costmap_update.y,
                UpdateWidth = (int)plannerData.costmap_update.width,
                UpdateHeight = (int)plannerData.costmap_update.height,
                UpdateData = updateData
            };
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error getting cost map data");
            return null;
        }
        finally
        {
            if (fetched)
            {
                FreePlannerDataOutput(ref plannerData);
            }
        }
    }

    private static void FreePlannerDataOutput(ref PlannerDataOutput plannerData)
    {
        if (plannerData.plan.poses != IntPtr.Zero)
        {
            int pointCount = (int)plannerData.plan.poses_count;
            int poseSize = Marshal.SizeOf<Pose2DStamped>();
            for (int index = 0; index < pointCount; index++)
            {
                IntPtr posePtr = IntPtr.Add(plannerData.plan.poses, index * poseSize);
                Pose2DStamped pose = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
                if (pose.header.frame_id != IntPtr.Zero)
                {
                    NavigationNativeInterface.nav_c_api_free_string(pose.header.frame_id);
                }
            }

            NativeFree(plannerData.plan.poses);
            plannerData.plan.poses = IntPtr.Zero;
            plannerData.plan.poses_count = 0;
        }

        if (plannerData.plan.header.frame_id != IntPtr.Zero)
        {
            NavigationNativeInterface.nav_c_api_free_string(plannerData.plan.header.frame_id);
            plannerData.plan.header.frame_id = IntPtr.Zero;
        }

        if (plannerData.costmap.header.frame_id != IntPtr.Zero)
        {
            NavigationNativeInterface.nav_c_api_free_string(plannerData.costmap.header.frame_id);
            plannerData.costmap.header.frame_id = IntPtr.Zero;
        }

        if (plannerData.costmap.data != IntPtr.Zero)
        {
            NativeFree(plannerData.costmap.data);
            plannerData.costmap.data = IntPtr.Zero;
            plannerData.costmap.data_count = 0;
        }

        if (plannerData.costmap_update.header.frame_id != IntPtr.Zero)
        {
            NavigationNativeInterface.nav_c_api_free_string(plannerData.costmap_update.header.frame_id);
            plannerData.costmap_update.header.frame_id = IntPtr.Zero;
        }

        if (plannerData.costmap_update.data != IntPtr.Zero)
        {
            NativeFree(plannerData.costmap_update.data);
            plannerData.costmap_update.data = IntPtr.Zero;
            plannerData.costmap_update.data_count = 0;
        }

        if (plannerData.footprint.header.frame_id != IntPtr.Zero)
        {
            NavigationNativeInterface.nav_c_api_free_string(plannerData.footprint.header.frame_id);
            plannerData.footprint.header.frame_id = IntPtr.Zero;
        }

        if (plannerData.footprint.polygon.points != IntPtr.Zero)
        {
            NativeFree(plannerData.footprint.polygon.points);
            plannerData.footprint.polygon.points = IntPtr.Zero;
            plannerData.footprint.polygon.points_count = 0;
        }
    }

    /// <summary>
    /// Convert quaternion (x, y, z, w) to yaw angle (theta) in radians
    /// </summary>
    private static double QuaternionToYaw(Quaternion q)
    {
        double yaw = Math.Atan2(
            2.0 * (q.w * q.z + q.x * q.y),
            1.0 - 2.0 * (q.y * q.y + q.z * q.z)
        );
        return yaw;
    }

    public (double x, double y, double z, double qx, double qy, double qz, double qw, string frameId)? OffsetGoal2D(
        double poseX, double poseY, double poseTheta,
        string frameId,
        double offsetDistance)
    {
        ThrowIfNotInitialized();

        try
        {
            PoseStamped outGoal;
            lock (_lock)
            {
                bool success = NavigationNativeInterface.navigation_offset_goal_2d(
                    poseX, poseY, poseTheta,
                    frameId,
                    offsetDistance,
                    out outGoal);

                if (!success)
                {
                    return null;
                }
            }

            try
            {
                string resultFrameId = string.Empty;
                if (outGoal.header.frame_id != IntPtr.Zero)
                {
                    resultFrameId = Marshal.PtrToStringAnsi(outGoal.header.frame_id) ?? string.Empty;
                }

                return (
                    outGoal.pose.position.x,
                    outGoal.pose.position.y,
                    outGoal.pose.position.z,
                    outGoal.pose.orientation.x,
                    outGoal.pose.orientation.y,
                    outGoal.pose.orientation.z,
                    outGoal.pose.orientation.w,
                    resultFrameId
                );
            }
            finally
            {
                if (outGoal.header.frame_id != IntPtr.Zero)
                {
                    NavigationNativeInterface.nav_c_api_free_string(outGoal.header.frame_id);
                }
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error offsetting goal 2D");
            return null;
        }
    }

    /// <summary>
    /// Publish static transforms to TF buffer
    /// Sets up the robot's coordinate frame tree
    /// </summary>
    private void PublishStaticTransforms()
    {
        try
        {
            bool success1 = NavigationNativeInterface.tf_listener_set_static_transform(
                _tfBuffer, "map", "odom",
                0.0, 0.0, 0.0,
                0.0, 0.0, 0.0, 1.0);

            if (success1)
                _logger?.LogInformation("[DEBUG] Published static transform: map → odom");
            else
                _logger?.LogWarning("[DEBUG] Failed to publish static transform: map → odom");

            bool success2 = NavigationNativeInterface.tf_listener_set_static_transform(
                _tfBuffer, "odom", "base_footprint",
                0.0, 0.0, 0.0,
                0.0, 0.0, 0.0, 1.0);

            if (success2)
                _logger?.LogInformation("[DEBUG] Published static transform: odom → base_footprint");
            else
                _logger?.LogWarning("[DEBUG] Failed to publish static transform: odom → base_footprint");

            bool success3 = NavigationNativeInterface.tf_listener_set_static_transform(
                _tfBuffer, "base_footprint", "base_link",
                0.0, 0.0, 0.0,
                0.0, 0.0, 0.0, 1.0);

            if (success3)
                _logger?.LogInformation("[DEBUG] Published static transform: base_footprint → base_link");
            else
                _logger?.LogWarning("[DEBUG] Failed to publish static transform: base_footprint → base_link");

            var now = DateTime.UtcNow;
            long sec = ((DateTimeOffset)now).ToUnixTimeSeconds();
            long nsec = (now.Ticks % TimeSpan.TicksPerSecond) * 100;

            var baseToLidar1 = new TF3.TF3_Transform
            {
                timestamp_sec = sec,
                timestamp_nsec = nsec,
                frame_id = "base_link",
                child_frame_id = "scan_1",
                translation_x = -0.15,
                translation_y = 0.0,
                translation_z = 0.0,
                rotation_x = 0.0,
                rotation_y = 0.0,
                rotation_z = 1.0,
                rotation_w = 0.0
            };

            TF3.TF3NativeInterface.tf3_set_transform(_tfBuffer, ref baseToLidar1, "navigation_client", true);
            _logger?.LogInformation("[DEBUG] Published static transform: base_link → scan_1");

            bool success6 = NavigationNativeInterface.tf_listener_set_static_transform(
                _tfBuffer, "base_link", "scan_2",
                0.707, -0.2825, 0.0,
                0.0, 0.0, -0.3826834, 0.9238795);

            if (success6)
                _logger?.LogInformation("[DEBUG] Published static transform: base_link → scan_2 (right lidar, -45°)");
            else
                _logger?.LogWarning("[DEBUG] Failed to publish static transform: base_link → scan_2");

            bool success7 = NavigationNativeInterface.tf_listener_set_static_transform(
                _tfBuffer, "base_link", "scan_3",
                0.707, 0.2825, 0.0,
                0.0, 0.0, 0.3802634, 0.9248782);

            if (success7)
                _logger?.LogInformation("[DEBUG] Published static transform: base_link → scan_3 (left lidar, +45°)");
            else
                _logger?.LogWarning("[DEBUG] Failed to publish static transform: base_link → scan_3");
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DEBUG] Error publishing static transforms: {Message}", ex.Message);
        }
    }

    /// <summary>
    /// Create a PoseStamped structure from parameters
    /// </summary>
    private PoseStamped CreatePoseStamped(double x, double y, double z, double qx, double qy, double qz, double qw, string frameId)
    {
        var now = DateTime.UtcNow;
        long sec = ((DateTimeOffset)now).ToUnixTimeSeconds();
        long nsec = (now.Ticks % TimeSpan.TicksPerSecond) * 100;

        return new PoseStamped
        {
            header = new Header
            {
                seq = 0,
                sec = (uint)sec,
                nsec = (uint)nsec,
                frame_id = Marshal.StringToHGlobalAnsi(frameId)
            },
            pose = new Pose
            {
                position = new Point { x = x, y = y, z = z },
                orientation = new Quaternion { x = qx, y = qy, z = qz, w = qw }
            }
        };
    }

    /// <summary>
    /// Free memory allocated for PoseStamped
    /// </summary>
    private void FreePoseStamped(ref PoseStamped poseStamped)
    {
        if (poseStamped.header.frame_id != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(poseStamped.header.frame_id);
            poseStamped.header.frame_id = IntPtr.Zero;
        }
    }

    private void ThrowIfNotInitialized()
    {
        lock (_lock)
        {
            if (_handle == IntPtr.Zero)
            {
                throw new InvalidOperationException("NavigationClient 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)
        {
            _disposed = true;
            if (_handle != IntPtr.Zero)
            {
                try
                {
                    NavigationNativeInterface.navigation_destroy(_handle);
                    _logger?.LogInformation("NavigationClient disposed successfully");
                }
                catch (Exception ex)
                {
                    _logger?.LogError(ex, "Error disposing NavigationClient");
                }
                finally
                {
                    _handle = IntPtr.Zero;
                }
            }

            _tfBuffer = IntPtr.Zero;
        }
    }

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

/// <summary>
/// Navigation feedback data
/// </summary>
public class NavigationFeedbackData
{
    public NavigationState NavigationState { get; set; }
    public string FeedbackString { get; set; } = string.Empty;
    public Pose2DData CurrentPose { get; set; } = new();
    public bool GoalChecked { get; set; }
    public bool IsReady { get; set; }

    public string StateString => NavigationState switch
    {
        NavigationState.Pending => "Pending",
        NavigationState.Active => "Active",
        NavigationState.Preempted => "Preempted",
        NavigationState.Succeeded => "Succeeded",
        NavigationState.Aborted => "Aborted",
        NavigationState.Rejected => "Rejected",
        NavigationState.Preempting => "Preempting",
        NavigationState.Recalling => "Recalling",
        NavigationState.Recalled => "Recalled",
        NavigationState.Lost => "Lost",
        NavigationState.Planning => "Planning",
        NavigationState.Controlling => "Controlling",
        NavigationState.Clearing => "Clearing",
        NavigationState.Paused => "Paused",
        _ => $"Unknown ({(int)NavigationState})"
    };
}

/// <summary>
/// Pose2D data
/// </summary>
public class Pose2DData
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Theta { get; set; }
}

public class GlobalPathData
{
    public string FrameId { get; set; } = string.Empty;
    public List<GlobalPathPointData> Points { get; set; } = new();
}

public class GlobalPathPointData
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Theta { get; set; }
}

public class CostMapData
{
    public string FrameId { get; set; } = string.Empty;
    public double Resolution { get; set; }
    public int Width { get; set; }
    public int Height { get; set; }
    public double OriginX { get; set; }
    public double OriginY { get; set; }
    public double OriginTheta { get; set; }
    public byte[] Data { get; set; } = Array.Empty<byte>();
    public bool HasFullMap { get; set; }
    public string UpdateFrameId { get; set; } = string.Empty;
    public bool IsCostmapUpdated { get; set; }
    public int UpdateX { get; set; }
    public int UpdateY { get; set; }
    public int UpdateWidth { get; set; }
    public int UpdateHeight { get; set; }
    public byte[] UpdateData { get; set; } = Array.Empty<byte>();
}