using System.Runtime.InteropServices;
using System.Text.Json;

namespace RobotNet10.RobotApp.Navigation;

/// <summary>
/// C-compatible structs for Navigation C API interop
/// Based on /home/robotics/sonvh/pnkx_nav_core/src/APIs/c_api/include/nav_c_api.h
/// </summary>

/// <summary>
/// Navigation states, including planning and controller status
/// </summary>
public enum NavigationState : int
{
    Pending = 0,
    Active = 1,
    Preempted = 2,
    Succeeded = 3,
    Aborted = 4,
    Rejected = 5,
    Preempting = 6,
    Recalling = 7,
    Recalled = 8,
    Lost = 9,
    Planning = 10,
    Controlling = 11,
    Clearing = 12,
    Paused = 13
}

/// <summary>
/// Point structure (x, y, z)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct Point
{
    public double x;
    public double y;
    public double z;
}

/// <summary>
/// Pose2D structure (x, y, theta)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct Pose2D
{
    public double x;
    public double y;
    public double theta;
}

[StructLayout(LayoutKind.Sequential)]
public struct Pose2DStamped
{
    public Header header;
    public Pose2D pose;
}

[StructLayout(LayoutKind.Sequential)]
public struct Path2D
{
    public Header header;
    public IntPtr poses;
    public nuint poses_count;
}

/// <summary>
/// Twist2D structure (x, y, theta velocities)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct Twist2D
{
    public double x;
    public double y;
    public double theta;
}

/// <summary>
/// Quaternion structure
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct Quaternion
{
    public double x;
    public double y;
    public double z;
    public double w;
}

/// <summary>
/// Position structure (alias for Point to match C API)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct Position
{
    public double x;
    public double y;
    public double z;
}

// Point is the same as Position in C API
// public using Point = Position;

/// <summary>
/// Pose structure
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct Pose
{
    public Point position;  // Use Point to match C API exactly
    public Quaternion orientation;
}


/// <summary>
/// Header structure
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct Header
{
    public uint seq;
    public uint sec;  // uint32_t
    public uint nsec;
    public IntPtr frame_id;  // char* - must be allocated and freed
}

/// <summary>
/// PoseStamped structure
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct PoseStamped
{
    public Header header;
    public Pose pose;
}

/// <summary>
/// Twist2DStamped structure
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct Twist2DStamped
{
    public Header header;
    public Twist2D velocity;
}

/// <summary>
/// Vector3 structure
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct Vector3
{
    public double x;
    public double y;
    public double z;
}

/// <summary>
/// OccupancyGrid structure
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct OccupancyGrid
{
    public Header header;
    public MapMetaData info;
    public IntPtr data;
    public nuint data_count;
}

[StructLayout(LayoutKind.Sequential)]
public struct OccupancyGridUpdate
{
    public Header header;
    public int x;
    public int y;
    public uint width;
    public uint height;
    public IntPtr data;
    public nuint data_count;
}

[StructLayout(LayoutKind.Sequential)]
public struct Point32
{
    public float x;
    public float y;
    public float z;
}

[StructLayout(LayoutKind.Sequential)]
public struct Polygon
{
    public IntPtr points;
    public nuint points_count;
}

[StructLayout(LayoutKind.Sequential)]
public struct PolygonStamped
{
    public Header header;
    public Polygon polygon;
}

[StructLayout(LayoutKind.Sequential)]
public struct PlannerDataOutput
{
    public Path2D plan;
    public OccupancyGrid costmap;
    public OccupancyGridUpdate costmap_update;
    [MarshalAs(UnmanagedType.I1)]
    public bool is_costmap_updated;
    public PolygonStamped footprint;
}

/// <summary>
/// Time structure (sec, nsec)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct Time
{
    public uint sec;
    public uint nsec;
}

[StructLayout(LayoutKind.Sequential)]
public struct MapMetaData
{
    public Time map_load_time;  // Time when map was loaded
    public float resolution;  // meters per cell
    public uint width;  // cells
    public uint height;  // cells
    public Pose origin;  // pose of cell (0,0) in map frame
}
/// <summary>
/// Navigation feedback structure
/// </summary>

[StructLayout(LayoutKind.Sequential)]
public struct NavFeedback
    {
        public NavigationState navigation_state;
        public IntPtr feed_back_str;  // char*; free with nav_c_api_free_string
        public Pose2D current_pose;
        [MarshalAs(UnmanagedType.I1)]
        public bool goal_checked;
        [MarshalAs(UnmanagedType.I1)]
        public bool is_ready;
    }
[StructLayout(LayoutKind.Sequential)]
public struct Odometry
{
    public Header header;
    public IntPtr child_frame_id; // char* - must be allocated and freed
    public PoseWithCovariance pose;
    public TwistWithCovariance twist;
}
[StructLayout(LayoutKind.Sequential)]
public struct PoseWithCovariance{
    public Pose pose;
    public IntPtr covariance;  // double* - array of 36 doubles (6x6 matrix)
    public nuint covariance_count;  // size_t - should be 36
}
// [StructLayout(LayoutKind.Sequential)]
// public struct Position{
//     public double x;
//     public double y;
//     public double z;
// }
[StructLayout(LayoutKind.Sequential)]
public struct TwistWithCovariance{
    public Twist twist;
    public IntPtr covariance;  // double* - array of 36 doubles (6x6 matrix)
    public nuint covariance_count;  // size_t - should be 36
}
[StructLayout(LayoutKind.Sequential)]
public struct Twist{
    public Vector3 linear;
    public Vector3 angular;
}
// [StructLayout(LayoutKind.Sequential)]
// public struct Vector3{
//     public double x;
//     public double y;
//     public double z;
// }
  [StructLayout(LayoutKind.Sequential)]
   public struct LaserScan
    {
        public Header header;
        public float angle_min;
        public float angle_max;
        public float angle_increment;
        public float time_increment;
        public float scan_time;
        public float range_min;
        public float range_max;
        public IntPtr ranges;
        public nuint ranges_count;
        public IntPtr intensities;
        public nuint intensities_count;
    }
     [StructLayout(LayoutKind.Sequential)]
    public struct ControlPoint
    {
        public double x;
        public double y;
        public double weight;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct ActionParameter
    {
        public IntPtr key;   // char*
        public IntPtr value; // char*
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct NodePosition
    {
        public double x;
        public double y;
        public double theta;
        public float allowedDeviationXY;
        public float allowedDeviationTheta;
        public IntPtr mapId;          // char*
        public IntPtr mapDescription; // char*
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Trajectory
    {
        public uint degree;
        public IntPtr knotVector;           // double*
        public nuint knotVector_count;
        public IntPtr controlPoints;        // ControlPoint*
        public nuint controlPoints_count;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Action
    {
        public IntPtr actionType;       // char*
        public IntPtr actionId;         // char*
        public IntPtr actionDescription;// char*
        public IntPtr blockingType;     // char*
        public IntPtr actionParameters; // ActionParameter*
        public nuint actionParameters_count;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Node
    {
        public IntPtr nodeId;           // char*
        public int sequenceId;
        public IntPtr nodeDescription;  // char*
        public byte released;
        public NodePosition nodePosition;
        public IntPtr actions;          // Action*
        public nuint actions_count;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Edge
    {
        public IntPtr edgeId;           // char*
        public int sequenceId;
        public IntPtr edgeDescription; // char*
        public byte released;
        public IntPtr startNodeId;     // char*
        public IntPtr endNodeId;       // char*
        public double maxSpeed;
        public double maxHeight;
        public double minHeight;
        public double orientation;
        public IntPtr orientationType; // char*
        public IntPtr direction;       // char*
        public byte rotationAllowed;
        public double maxRotationSpeed;
        public Trajectory trajectory;
        public double length;
        public IntPtr actions;          // Action*
        public nuint actions_count;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Order
    {
        public int headerId;
        public IntPtr timestamp;        // char*
        public IntPtr version;         // char*
        public IntPtr manufacturer;    // char*
        public IntPtr serialNumber;    // char*
        public IntPtr orderId;         // char*
        public uint orderUpdateId;
        public IntPtr nodes;           // Node*
        public nuint nodes_count;
        public IntPtr edges;           // Edge*
        public nuint edges_count;
        public IntPtr zoneSetId;      // char*
    }
/// <summary>
/// OrderHandle - wrapper for OrderHandle (void* in C)
/// Represents a pointer to a robot_protocol_msgs::Order object
/// </summary>
public class OrderHandle : IDisposable
{
    private IntPtr _handle;
    private bool _disposed = false;
    private readonly bool _ownsHandle;

    /// <summary>
    /// Create OrderHandle from IntPtr
    /// </summary>
    /// <param name="handle">Pointer to Order object</param>
    /// <param name="ownsHandle">Whether this handle owns the memory (will free on dispose)</param>
    public OrderHandle(IntPtr handle, bool ownsHandle = false)
    {
        _handle = handle;
        _ownsHandle = ownsHandle;
    }

    /// <summary>
    /// Get the underlying IntPtr
    /// </summary>
    public IntPtr Handle => _handle;

    /// <summary>
    /// Check if handle is valid (not zero)
    /// </summary>
    public bool IsValid => _handle != IntPtr.Zero;

    /// <summary>
    /// Implicit conversion to IntPtr
    /// </summary>
    public static implicit operator IntPtr(OrderHandle orderHandle) => orderHandle?._handle ?? IntPtr.Zero;

    /// <summary>
    /// Explicit conversion from IntPtr
    /// </summary>
    public static explicit operator OrderHandle(IntPtr handle) => new OrderHandle(handle);

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

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

        if (disposing && _ownsHandle && _handle != IntPtr.Zero)
        {
            // Free the Order object if we own it
            // Note: This requires navigation_free_order function
            // NavigationNativeInterface.navigation_free_order(_handle);
            _handle = IntPtr.Zero;
        }

        _disposed = true;
    }

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

/// <summary>
/// Order data structure (C# representation of robot_protocol_msgs::Order)
/// This is a simplified version for API usage
/// </summary>
public class OrderData
{
    public int HeaderId { get; set; }
    public string Timestamp { get; set; } = string.Empty;
    public string Version { get; set; } = string.Empty;
    public string Manufacturer { get; set; } = string.Empty;
    public string SerialNumber { get; set; } = string.Empty;
    public string OrderId { get; set; } = string.Empty;
    public uint OrderUpdateId { get; set; }
    public string? ZoneSetId { get; set; }
    public List<OrderNodeData> Nodes { get; set; } = new();
    public List<OrderEdgeData> Edges { get; set; } = new();
}

/// <summary>
/// Order Node data
/// </summary>
public class OrderNodeData
{
    public string NodeId { get; set; } = string.Empty;
    public int SequenceId { get; set; }
    public string NodeDescription { get; set; } = string.Empty;
    public bool Released { get; set; }
    public OrderNodePositionData? NodePosition { get; set; }
    public List<OrderActionData> Actions { get; set; } = new();
}

/// <summary>
/// Order Node Position data
/// </summary>
public class OrderNodePositionData
{
    public double X { get; set; }
    public double Y { get; set; }
    public double? Theta { get; set; }
    public double? AllowedDeviationXY { get; set; }
    public double? AllowedDeviationTheta { get; set; }
    public string? MapId { get; set; }
    public string? MapDescription { get; set; }
}

/// <summary>
/// Order Edge data
/// </summary>
public class OrderEdgeData
{
    public string EdgeId { get; set; } = string.Empty;
    public int SequenceId { get; set; }
    public string? EdgeDescription { get; set; }
    public bool Released { get; set; }
    public string StartNodeId { get; set; } = string.Empty;
    public string EndNodeId { get; set; } = string.Empty;
    public double? MaxSpeed { get; set; }
    public double? MaxHeight { get; set; }
    public double? MinHeight { get; set; }
    public double Orientation { get; set; }
    /// <summary>VDA5050 can send number (e.g. 1) or string; we accept both via JsonElement.</summary>
    public JsonElement? OrientationType { get; set; }
    public string Direction { get; set; } = "forward";
    public bool? RotationAllowed { get; set; }
    public double? MaxRotationSpeed { get; set; }
    public double Length { get; set; }
    public OrderTrajectoryData? Trajectory { get; set; }
    public List<OrderActionData> Actions { get; set; } = new();
}

/// <summary>
/// Order Trajectory data
/// </summary>
public class OrderTrajectoryData
{
    public uint Degree { get; set; }
    public List<double> KnotVector { get; set; } = new();
    public List<OrderControlPointData> ControlPoints { get; set; } = new();
}

/// <summary>
/// Order Control Point data
/// </summary>
public class OrderControlPointData
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Weight { get; set; } = 1.0;
}

/// <summary>
/// Order Action data
/// </summary>
public class OrderActionData
{
    public string ActionType { get; set; } = string.Empty;
    public string ActionId { get; set; } = string.Empty;
    public string ActionDescription { get; set; } = string.Empty;
    public string BlockingType { get; set; } = "NONE";
    public List<OrderActionParameterData> ActionParameters { get; set; } = new();
}

/// <summary>
/// Order Action Parameter data
/// </summary>
public class OrderActionParameterData
{
    public string Key { get; set; } = string.Empty;
    public string Value { get; set; } = string.Empty;
}

/// <summary>
/// Helper class to convert OrderData to native Order struct
/// </summary>
public static class OrderConverter
{
    /// <summary>
    /// Convert OrderData (managed) to Order (native struct)
    /// </summary>
    /// <param name="orderData">Managed order data</param>
    /// <returns>Native Order struct with allocated memory</returns>
    public static Order ConvertToNativeOrder(OrderData orderData)
    {
        var order = new Order
        {
            headerId = orderData.HeaderId,
            timestamp = Marshal.StringToHGlobalAnsi(orderData.Timestamp),
            version = Marshal.StringToHGlobalAnsi(orderData.Version),
            manufacturer = Marshal.StringToHGlobalAnsi(orderData.Manufacturer),
            serialNumber = Marshal.StringToHGlobalAnsi(orderData.SerialNumber),
            orderId = Marshal.StringToHGlobalAnsi(orderData.OrderId),
            orderUpdateId = orderData.OrderUpdateId,
            zoneSetId = Marshal.StringToHGlobalAnsi(orderData.ZoneSetId ?? string.Empty),
            nodes_count = (nuint)orderData.Nodes.Count,
            edges_count = (nuint)orderData.Edges.Count
        };

        // Allocate and convert nodes
        if (orderData.Nodes.Count > 0)
        {
            int nodeSize = Marshal.SizeOf<Node>();
            order.nodes = Marshal.AllocHGlobal(nodeSize * orderData.Nodes.Count);
            
            for (int i = 0; i < orderData.Nodes.Count; i++)
            {
                var nodeData = orderData.Nodes[i];
                var node = ConvertToNativeNode(nodeData);
                IntPtr nodePtr = IntPtr.Add(order.nodes, i * nodeSize);
                Marshal.StructureToPtr(node, nodePtr, false);
            }
        }
        else
        {
            order.nodes = IntPtr.Zero;
        }

        // Allocate and convert edges
        if (orderData.Edges.Count > 0)
        {
            int edgeSize = Marshal.SizeOf<Edge>();
            order.edges = Marshal.AllocHGlobal(edgeSize * orderData.Edges.Count);
            
            for (int i = 0; i < orderData.Edges.Count; i++)
            {
                var edgeData = orderData.Edges[i];
                var edge = ConvertToNativeEdge(edgeData);
                IntPtr edgePtr = IntPtr.Add(order.edges, i * edgeSize);
                Marshal.StructureToPtr(edge, edgePtr, false);
            }
        }
        else
        {
            order.edges = IntPtr.Zero;
        }

        return order;
    }

    /// <summary>
    /// Free memory allocated for native Order struct
    /// </summary>
    public static void FreeNativeOrder(ref Order order)
    {
        // Free string fields
        if (order.timestamp != IntPtr.Zero) Marshal.FreeHGlobal(order.timestamp);
        if (order.version != IntPtr.Zero) Marshal.FreeHGlobal(order.version);
        if (order.manufacturer != IntPtr.Zero) Marshal.FreeHGlobal(order.manufacturer);
        if (order.serialNumber != IntPtr.Zero) Marshal.FreeHGlobal(order.serialNumber);
        if (order.orderId != IntPtr.Zero) Marshal.FreeHGlobal(order.orderId);
        if (order.zoneSetId != IntPtr.Zero) Marshal.FreeHGlobal(order.zoneSetId);

        // Free nodes
        if (order.nodes != IntPtr.Zero)
        {
            int nodeSize = Marshal.SizeOf<Node>();
            for (int i = 0; i < (int)order.nodes_count; i++)
            {
                IntPtr nodePtr = IntPtr.Add(order.nodes, i * nodeSize);
                Node node = Marshal.PtrToStructure<Node>(nodePtr);
                FreeNativeNode(ref node);
            }
            Marshal.FreeHGlobal(order.nodes);
        }

        // Free edges
        if (order.edges != IntPtr.Zero)
        {
            int edgeSize = Marshal.SizeOf<Edge>();
            for (int i = 0; i < (int)order.edges_count; i++)
            {
                IntPtr edgePtr = IntPtr.Add(order.edges, i * edgeSize);
                Edge edge = Marshal.PtrToStructure<Edge>(edgePtr);
                FreeNativeEdge(ref edge);
            }
            Marshal.FreeHGlobal(order.edges);
        }
    }

    private static Node ConvertToNativeNode(OrderNodeData nodeData)
    {
        var node = new Node
        {
            nodeId = Marshal.StringToHGlobalAnsi(nodeData.NodeId),
            sequenceId = nodeData.SequenceId,
            nodeDescription = Marshal.StringToHGlobalAnsi(nodeData.NodeDescription),
            released = (byte)(nodeData.Released ? 1 : 0),
            actions_count = (nuint)nodeData.Actions.Count
        };

        // Convert node position
        if (nodeData.NodePosition != null)
        {
            node.nodePosition = new NodePosition
            {
                x = nodeData.NodePosition.X,
                y = nodeData.NodePosition.Y,
                theta = nodeData.NodePosition.Theta ?? 0.0,
                allowedDeviationXY = (float)(nodeData.NodePosition.AllowedDeviationXY ?? 0.0),
                allowedDeviationTheta = (float)(nodeData.NodePosition.AllowedDeviationTheta ?? 0.0),
                mapId = Marshal.StringToHGlobalAnsi(nodeData.NodePosition.MapId ?? string.Empty),
                mapDescription = Marshal.StringToHGlobalAnsi(nodeData.NodePosition.MapDescription ?? string.Empty)
            };
        }

        // Convert actions
        if (nodeData.Actions.Count > 0)
        {
            int actionSize = Marshal.SizeOf<Action>();
            node.actions = Marshal.AllocHGlobal(actionSize * nodeData.Actions.Count);
            
            for (int i = 0; i < nodeData.Actions.Count; i++)
            {
                var actionData = nodeData.Actions[i];
                var action = ConvertToNativeAction(actionData);
                IntPtr actionPtr = IntPtr.Add(node.actions, i * actionSize);
                Marshal.StructureToPtr(action, actionPtr, false);
            }
        }
        else
        {
            node.actions = IntPtr.Zero;
        }

        return node;
    }

    private static void FreeNativeNode(ref Node node)
    {
        if (node.nodeId != IntPtr.Zero) Marshal.FreeHGlobal(node.nodeId);
        if (node.nodeDescription != IntPtr.Zero) Marshal.FreeHGlobal(node.nodeDescription);
        if (node.nodePosition.mapId != IntPtr.Zero) Marshal.FreeHGlobal(node.nodePosition.mapId);
        if (node.nodePosition.mapDescription != IntPtr.Zero) Marshal.FreeHGlobal(node.nodePosition.mapDescription);

        if (node.actions != IntPtr.Zero)
        {
            int actionSize = Marshal.SizeOf<Action>();
            for (int i = 0; i < (int)node.actions_count; i++)
            {
                IntPtr actionPtr = IntPtr.Add(node.actions, i * actionSize);
                Action action = Marshal.PtrToStructure<Action>(actionPtr);
                FreeNativeAction(ref action);
            }
            Marshal.FreeHGlobal(node.actions);
        }
    }

    private static string OrientationTypeToString(JsonElement? je)
    {
        if (!je.HasValue) return string.Empty;
        var e = je.Value;
        if (e.ValueKind == JsonValueKind.Number) return e.GetDouble().ToString(System.Globalization.CultureInfo.InvariantCulture);
        if (e.ValueKind == JsonValueKind.String) return e.GetString() ?? string.Empty;
        return string.Empty;
    }

    private static Edge ConvertToNativeEdge(OrderEdgeData edgeData)
    {
        var edge = new Edge
        {
            edgeId = Marshal.StringToHGlobalAnsi(edgeData.EdgeId),
            sequenceId = edgeData.SequenceId,
            edgeDescription = Marshal.StringToHGlobalAnsi(edgeData.EdgeDescription ?? string.Empty),
            released = (byte)(edgeData.Released ? 1 : 0),
            startNodeId = Marshal.StringToHGlobalAnsi(edgeData.StartNodeId),
            endNodeId = Marshal.StringToHGlobalAnsi(edgeData.EndNodeId),
            maxSpeed = edgeData.MaxSpeed ?? 0.0,
            maxHeight = edgeData.MaxHeight ?? 0.0,
            minHeight = edgeData.MinHeight ?? 0.0,
            orientation = edgeData.Orientation,
            orientationType = Marshal.StringToHGlobalAnsi(OrientationTypeToString(edgeData.OrientationType)),
            direction = Marshal.StringToHGlobalAnsi(edgeData.Direction ?? string.Empty),
            rotationAllowed = (byte)((edgeData.RotationAllowed ?? false) ? 1 : 0),
            maxRotationSpeed = edgeData.MaxRotationSpeed ?? 0.0,
            length = edgeData.Length,
            actions_count = (nuint)edgeData.Actions.Count
        };

        // Convert trajectory
        if (edgeData.Trajectory != null)
        {
            edge.trajectory = ConvertToNativeTrajectory(edgeData.Trajectory);
        }

        // Convert actions
        if (edgeData.Actions.Count > 0)
        {
            int actionSize = Marshal.SizeOf<Action>();
            edge.actions = Marshal.AllocHGlobal(actionSize * edgeData.Actions.Count);
            
            for (int i = 0; i < edgeData.Actions.Count; i++)
            {
                var actionData = edgeData.Actions[i];
                var action = ConvertToNativeAction(actionData);
                IntPtr actionPtr = IntPtr.Add(edge.actions, i * actionSize);
                Marshal.StructureToPtr(action, actionPtr, false);
            }
        }
        else
        {
            edge.actions = IntPtr.Zero;
        }

        return edge;
    }

    private static void FreeNativeEdge(ref Edge edge)
    {
        if (edge.edgeId != IntPtr.Zero) Marshal.FreeHGlobal(edge.edgeId);
        if (edge.edgeDescription != IntPtr.Zero) Marshal.FreeHGlobal(edge.edgeDescription);
        if (edge.startNodeId != IntPtr.Zero) Marshal.FreeHGlobal(edge.startNodeId);
        if (edge.endNodeId != IntPtr.Zero) Marshal.FreeHGlobal(edge.endNodeId);
        if (edge.orientationType != IntPtr.Zero) Marshal.FreeHGlobal(edge.orientationType);
        if (edge.direction != IntPtr.Zero) Marshal.FreeHGlobal(edge.direction);

        FreeNativeTrajectory(ref edge.trajectory);

        if (edge.actions != IntPtr.Zero)
        {
            int actionSize = Marshal.SizeOf<Action>();
            for (int i = 0; i < (int)edge.actions_count; i++)
            {
                IntPtr actionPtr = IntPtr.Add(edge.actions, i * actionSize);
                Action action = Marshal.PtrToStructure<Action>(actionPtr);
                FreeNativeAction(ref action);
            }
            Marshal.FreeHGlobal(edge.actions);
        }
    }

    private static Trajectory ConvertToNativeTrajectory(OrderTrajectoryData trajectoryData)
    {
        var trajectory = new Trajectory
        {
            degree = trajectoryData.Degree,
            knotVector_count = (nuint)trajectoryData.KnotVector.Count,
            controlPoints_count = (nuint)trajectoryData.ControlPoints.Count
        };

        // Allocate knot vector
        if (trajectoryData.KnotVector.Count > 0)
        {
            trajectory.knotVector = Marshal.AllocHGlobal(sizeof(double) * trajectoryData.KnotVector.Count);
            Marshal.Copy(trajectoryData.KnotVector.ToArray(), 0, trajectory.knotVector, trajectoryData.KnotVector.Count);
        }
        else
        {
            trajectory.knotVector = IntPtr.Zero;
        }

        // Allocate control points
        if (trajectoryData.ControlPoints.Count > 0)
        {
            int cpSize = Marshal.SizeOf<ControlPoint>();
            trajectory.controlPoints = Marshal.AllocHGlobal(cpSize * trajectoryData.ControlPoints.Count);
            
            for (int i = 0; i < trajectoryData.ControlPoints.Count; i++)
            {
                var cpData = trajectoryData.ControlPoints[i];
                var cp = new ControlPoint
                {
                    x = cpData.X,
                    y = cpData.Y,
                    weight = cpData.Weight
                };
                IntPtr cpPtr = IntPtr.Add(trajectory.controlPoints, i * cpSize);
                Marshal.StructureToPtr(cp, cpPtr, false);
            }
        }
        else
        {
            trajectory.controlPoints = IntPtr.Zero;
        }

        return trajectory;
    }

    private static void FreeNativeTrajectory(ref Trajectory trajectory)
    {
        if (trajectory.knotVector != IntPtr.Zero) Marshal.FreeHGlobal(trajectory.knotVector);
        if (trajectory.controlPoints != IntPtr.Zero) Marshal.FreeHGlobal(trajectory.controlPoints);
    }

    private static Action ConvertToNativeAction(OrderActionData actionData)
    {
        var action = new Action
        {
            actionType = Marshal.StringToHGlobalAnsi(actionData.ActionType),
            actionId = Marshal.StringToHGlobalAnsi(actionData.ActionId),
            actionDescription = Marshal.StringToHGlobalAnsi(actionData.ActionDescription),
            blockingType = Marshal.StringToHGlobalAnsi(actionData.BlockingType),
            actionParameters_count = (nuint)actionData.ActionParameters.Count
        };

        // Convert parameters
        if (actionData.ActionParameters.Count > 0)
        {
            int paramSize = Marshal.SizeOf<ActionParameter>();
            action.actionParameters = Marshal.AllocHGlobal(paramSize * actionData.ActionParameters.Count);
            
            for (int i = 0; i < actionData.ActionParameters.Count; i++)
            {
                var paramData = actionData.ActionParameters[i];
                var param = new ActionParameter
                {
                    key = Marshal.StringToHGlobalAnsi(paramData.Key),
                    value = Marshal.StringToHGlobalAnsi(paramData.Value)
                };
                IntPtr paramPtr = IntPtr.Add(action.actionParameters, i * paramSize);
                Marshal.StructureToPtr(param, paramPtr, false);
            }
        }
        else
        {
            action.actionParameters = IntPtr.Zero;
        }

        return action;
    }

    private static void FreeNativeAction(ref Action action)
    {
        if (action.actionType != IntPtr.Zero) Marshal.FreeHGlobal(action.actionType);
        if (action.actionId != IntPtr.Zero) Marshal.FreeHGlobal(action.actionId);
        if (action.actionDescription != IntPtr.Zero) Marshal.FreeHGlobal(action.actionDescription);
        if (action.blockingType != IntPtr.Zero) Marshal.FreeHGlobal(action.blockingType);

        if (action.actionParameters != IntPtr.Zero)
        {
            int paramSize = Marshal.SizeOf<ActionParameter>();
            for (int i = 0; i < (int)action.actionParameters_count; i++)
            {
                IntPtr paramPtr = IntPtr.Add(action.actionParameters, i * paramSize);
                ActionParameter param = Marshal.PtrToStructure<ActionParameter>(paramPtr);
                if (param.key != IntPtr.Zero) Marshal.FreeHGlobal(param.key);
                if (param.value != IntPtr.Zero) Marshal.FreeHGlobal(param.value);
            }
            Marshal.FreeHGlobal(action.actionParameters);
        }
    }
}