using System.Text.Json;
using RobotNet.VDA5050.Type;

namespace RobotNet10.RobotApp.Navigation;

// Use aliases so Node/Edge resolve to VDA5050 (this file is in Navigation namespace which has native Node/Edge structs).
using VdaNode = RobotNet.VDA5050.Order.Node;
using VdaEdge = RobotNet.VDA5050.Order.Edge;
using VdaTrajectory = RobotNet.VDA5050.Order.Trajectory;
using VdaOrderMsg = RobotNet.VDA5050.Order.OrderMsg;

/// <summary>
/// Converts VDA5050 order (Node[]/Edge[] from MQTT) to OrderData for Navigation C API (MoveToOrder).
/// </summary>
public static class VDA5050ToOrderDataConverter
{
    /// <summary>
    /// Convert VDA5050 nodes and edges (e.g. from OrderMsg received via MQTT) to OrderData for navigation.
    /// </summary>
    public static OrderData ToOrderData(VdaNode[] nodes, VdaEdge[] edges, VdaOrderMsg? orderMsg = null)
    {
        var data = new OrderData
        {
            HeaderId = (int)(orderMsg?.HeaderId ?? 0),
            Timestamp = orderMsg?.Timestamp.ToString("o") ?? DateTime.UtcNow.ToString("o"),
            Version = orderMsg?.Version ?? "2.1.0",
            Manufacturer = orderMsg?.Manufacturer ?? string.Empty,
            SerialNumber = orderMsg?.SerialNumber ?? string.Empty,
            OrderId = orderMsg?.OrderId ?? string.Empty,
            OrderUpdateId = (uint)(orderMsg?.OrderUpdateId ?? 0),
            ZoneSetId = orderMsg?.ZoneSetId,
            Nodes = new List<OrderNodeData>(nodes.Length),
            Edges = new List<OrderEdgeData>(edges.Length)
        };

        foreach (var n in nodes)
            data.Nodes.Add(ToOrderNodeData(n));
        foreach (var e in edges)
            data.Edges.Add(ToOrderEdgeData(e));

        return data;
    }

    private static OrderNodeData ToOrderNodeData(VdaNode n)
    {
        var node = new OrderNodeData
        {
            NodeId = n.NodeId,
            SequenceId = n.SequenceId,
            NodeDescription = n.NodeDescription ?? string.Empty,
            Released = n.Released,
            NodePosition = n.NodePosition != null ? new OrderNodePositionData
            {
                X = n.NodePosition.X,
                Y = n.NodePosition.Y,
                Theta = n.NodePosition.Theta,
                AllowedDeviationXY = n.NodePosition.AllowedDeviationXY,
                AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
                MapId = n.NodePosition.MapId,
                MapDescription = n.NodePosition.MapDescription
            } : null,
            Actions = new List<OrderActionData>()
        };
        return node;
    }

    private static OrderEdgeData ToOrderEdgeData(VdaEdge e)
    {
        var edge = new OrderEdgeData
        {
            EdgeId = e.EdgeId,
            SequenceId = e.SequenceId,
            EdgeDescription = e.EdgeDescription,
            Released = e.Released,
            StartNodeId = e.StartNodeId,
            EndNodeId = e.EndNodeId,
            MaxSpeed = e.MaxSpeed,
            MaxHeight = e.MaxHeight,
            MinHeight = e.MinHeight,
            Orientation = e.Orientation ?? 0,
            OrientationType = e.OrientationType.HasValue ? JsonSerializer.SerializeToElement((int)e.OrientationType.Value) : null,
            Direction = e.Direction ?? string.Empty,
            RotationAllowed = e.RotationAllowed,
            MaxRotationSpeed = e.MaxRotationSpeed,
            Length = e.Length ?? 0,
            Trajectory = e.Trajectory != null ? ToOrderTrajectoryData(e.Trajectory) : null,
            Actions = new List<OrderActionData>()
        };
        return edge;
    }

    private static OrderTrajectoryData? ToOrderTrajectoryData(VdaTrajectory? t)
    {
        if (t == null) return null;
        return new OrderTrajectoryData
        {
            Degree = (uint)t.Degree,
            KnotVector = t.KnotVector?.ToList() ?? new List<double>(),
            ControlPoints = t.ControlPoints?.Select(cp => new OrderControlPointData
            {
                X = cp.X,
                Y = cp.Y,
                Weight = cp.Weight ?? 1.0
            }).ToList() ?? new List<OrderControlPointData>()
        };
    }
}