using RobotNet.VDA5050;
using RobotNet.VDA5050.Order;
using RobotNet10.GlobalPathPlanner.Model;
using System.Text.Json;

namespace RobotNet10.FleetManager.Services.TrafficControl.Helpers;

/// <summary>
/// Helper class to convert MapManager entities to GlobalPathPlanner models
/// </summary>
public static class MapDataConverter
{
    /// <summary>
    /// Convert Node (MapManager) to GlobalNode (PathPlanner)
    /// </summary>
    public static GlobalNode ToGlobalNode(MapManager.Data.Node node, Guid mapId)
    {
        return new GlobalNode
        {
            Id = node.Id,
            MapId = mapId,
            Name = node.NodeName ?? node.NodeId,
            X = node.X,
            Y = node.Y,
            Orientation = Orientation.NONE // Default, can be enhanced later
        };
    }

    /// <summary>
    /// Convert Edge (MapManager) to GlobalEdge (PathPlanner)
    /// </summary>
    public static GlobalEdge ToGlobalEdge(MapManager.Data.Edge edge, Guid mapId, Guid vehicleId)
    {
        var globalEdge = new GlobalEdge
        {
            Id = edge.Id,
            MapId = mapId,
            StartNodeId = edge.StartNodeId,
            EndNodeId = edge.EndNodeId,
            Degree = 1,
            ControlPoint1X = 0.0,
            ControlPoint1Y = 0.0,
            ControlPoint2X = 0.0,
            ControlPoint2Y = 0.0
        };
        // Get edge vehicle properties for trajectory (if available)
        var vehicleProperty = edge.VehicleProperties?.FirstOrDefault(prop => prop.VehicleTypeId == vehicleId);
        if(vehicleProperty is not null)
        {
            // Use trajectory fields directly from Entity
            if (vehicleProperty.TrajectoryDegree.HasValue)
            {
                globalEdge.Degree = vehicleProperty.TrajectoryDegree.Value;
            }
            
            globalEdge.ControlPoint1X = vehicleProperty.TrajectoryControlPoint1X ?? 0.0;
            globalEdge.ControlPoint1Y = vehicleProperty.TrajectoryControlPoint1Y ?? 0.0;
            globalEdge.ControlPoint2X = vehicleProperty.TrajectoryControlPoint2X ?? 0.0;
            globalEdge.ControlPoint2Y = vehicleProperty.TrajectoryControlPoint2Y ?? 0.0;
        }
        return globalEdge;
    }

    /// <summary>
    /// Convert list of Nodes to GlobalNodes
    /// </summary>
    public static GlobalNode[] ToGlobalNodes(IEnumerable<MapManager.Data.Node> nodes, Guid mapId)
    {
        return [.. nodes.Select(n => ToGlobalNode(n, mapId))];
    }

    /// <summary>
    /// Convert list of Edges to GlobalEdges
    /// </summary>
    public static GlobalEdge[] ToGlobalEdges(IEnumerable<MapManager.Data.Edge> edges, Guid mapId, Guid vehicleId)
    {
        return [.. edges.Select(e => ToGlobalEdge(e, mapId, vehicleId))];
    }
}