using RobotNet.VDA5050;
using RobotNet.VDA5050.Order;
using RobotNet10.FleetManager.Services.TrafficControl.Models;
using RobotNet10.GlobalPathPlanner.Model;
using RobotNet10.MapManager.Data;
using Edge = RobotNet10.MapManager.Data.Edge;
using Node = RobotNet10.MapManager.Data.Node;

namespace RobotNet10.FleetManager.Services.TrafficControl.Helpers;

/// <summary>
/// Helper class to convert path planner results to RobotRoute
/// </summary>
public static class RouteConverter
{
    /// <summary>
    /// Convert path planner result (GlobalNode[], GlobalEdge[]) to RobotRoute
    /// </summary>
    public static RobotRoute ConvertToRobotRoute(
        string robotId,
        GlobalNode[] pathNodes,
        GlobalEdge[] pathEdges,
        List<Node> allNodes,
        List<Edge> allEdges,
        double? lastAngle,
        object? logger = null, // Accept any logger type for flexibility
        Guid? vehicleTypeId = null, // VehicleTypeId to get VehicleProperties
        string? mapId = null) // MapId (LevelId as string) for NodePosition
    {
        // Log virtual node creation
        if (logger != null)
        {
            try
            {
                var loggerType = logger.GetType();
                var infoMethod = loggerType.GetMethod("Info", [typeof(string)]);
                infoMethod?.Invoke(logger, [$"ConvertToRobotRoute: {pathNodes.Length} node, {pathEdges.Length}"]);
            }
            catch { }
        }
        var route = new RobotRoute
        {
            RobotId = robotId,
            OrderId = Guid.NewGuid().ToString(), // Generate new order ID
            OrderUpdateId = 0,
            CreatedAt = DateTime.UtcNow,
            LastUpdated = DateTime.UtcNow
        };

        var segments = new List<RouteSegment>();
        var nodeMap = allNodes.ToDictionary(n => n.Id, n => n);
        var edgeMap = allEdges.ToDictionary(e => e.Id, e => e);

        // Get LevelId from first available node (for creating virtual nodes/edges)
        var levelId = allNodes.FirstOrDefault()?.LevelId ?? Guid.Empty;

        // Create segments following VDA5050 pattern:
        // Node (seq 0), Edge (seq 1), Node (seq 2), Edge (seq 3), ..., Node (seq N)
        // Route: n nodes, n-1 edges

        int sequenceId = 0;

        for (int i = 0; i < pathNodes.Length; i++)
        {
            var globalNode = pathNodes[i];

            // Check if node exists in map, if not (first node when robot is on edge), create virtual node
            if (!nodeMap.TryGetValue(globalNode.Id, out Node? node))
            {
                // This is a virtual node created by A* when robot is on an edge
                // Only the first node can be virtual
                if (i == 0)
                {
                    // Get LevelId from next node if available, otherwise use from allNodes
                    if (pathNodes.Length > 1 && nodeMap.TryGetValue(pathNodes[1].Id, out var nextNode))
                    {
                        levelId = nextNode.LevelId;
                    }

                    // Create virtual node from GlobalNode
                    node = new Node
                    {
                        Id = globalNode.Id,
                        LevelId = levelId,
                        NodeId = globalNode.Id.ToString(),
                        NodeName = "Virtual Start Node",
                        X = globalNode.X,
                        Y = globalNode.Y
                    };

                    // Log virtual node creation
                    if (logger != null)
                    {
                        try
                        {
                            var loggerType = logger.GetType();
                            var infoMethod = loggerType.GetMethod("Info", [typeof(string)]);
                            infoMethod?.Invoke(logger, [$"Created virtual start node {globalNode.Id} at ({globalNode.X:F2}, {globalNode.Y:F2}) - robot is on edge"]);
                        }
                        catch { }
                    }
                }
                else
                {
                    // Node not found and not first node - this is an error
                    if (logger != null)
                    {
                        try
                        {
                            var loggerType = logger.GetType();
                            var errorMethod = loggerType.GetMethod("Error", [typeof(string)]);
                            errorMethod?.Invoke(logger, [$"Node {globalNode.Id} not found in map at index {i}"]);
                        }
                        catch
                        {
                            // Ignore if Error method doesn't exist
                        }
                    }
                    throw new InvalidOperationException($"Node {globalNode.Id} not found in map at index {i}");
                }
            }

            // Get NodeVehicleProperty for this vehicle type if available
            NodeVehicleProperty? nodeVehicleProperty = null;
            if (vehicleTypeId.HasValue && node.VehicleProperties != null)
            {
                nodeVehicleProperty = node.VehicleProperties.FirstOrDefault(vp => vp.VehicleTypeId == vehicleTypeId.Value);
            }

            // Parse actions from NodeVehicleProperty
            RobotNet.VDA5050.InstantAction.Action[] nodeActions = [];
            if (!string.IsNullOrEmpty(nodeVehicleProperty?.Actions))
            {
                try
                {
                    var parsedActions = System.Text.Json.JsonSerializer.Deserialize<RobotNet.VDA5050.InstantAction.ActionLIF[]>(nodeVehicleProperty.Actions, JsonOptionExtends.Read);
                    if (parsedActions != null)
                    {
                        nodeActions = [..parsedActions.Where(a => a.RequirementType == RobotNet.VDA5050.Type.RequirementType.REQUIRED).Select(a => new RobotNet.VDA5050.InstantAction.Action()
                        {
                            ActionId = Guid.NewGuid().ToString(),
                            ActionDescription = a.ActionDescription,
                            ActionParameters = [..a.ActionParameters],
                            BlockingType = a.BlockingType,
                            ActionType = a.ActionType
                        })];
                    }
                }
                catch { }
            }

            // Create VDA5050 Node with full information
            var vdaNode = new RobotNet.VDA5050.Order.Node
            {
                NodeId = node.Id.ToString(),
                SequenceId = sequenceId,
                Released = false,
                NodeDescription = node.NodeDescription ?? string.Empty,
                NodePosition = new NodePosition
                {
                    X = node.X,
                    Y = node.Y,
                    Theta = i == pathNodes.Length - 1 && lastAngle.HasValue ? lastAngle.Value : nodeVehicleProperty?.Theta, 
                    AllowedDeviationXY = nodeVehicleProperty?.AllowedDeviationXY,
                    AllowedDeviationTheta = nodeVehicleProperty?.AllowedDeviationTheta,
                    MapId = node.MapId ?? mapId ?? string.Empty
                },
                Actions = nodeActions
            };

            // Add node segment (even sequence IDs) with VDA5050 Node
            var nodeSegment = new RouteSegment
            {
                NodeId = node.Id,
                EdgeId = null,
                StartNodeId = node.Id,
                EndNodeId = null,
                Released = false,
                VdaNode = vdaNode,
                VdaEdge = null
            };

            segments.Add(nodeSegment);
            sequenceId++;

            // Add edge segment if not the last node
            if (i < pathNodes.Length - 1 && i < pathEdges.Length)
            {
                var globalEdge = pathEdges[i];

                // Find edge by Id or by StartNodeId and EndNodeId
                Edge? edge = allEdges.FirstOrDefault(e => e.StartNodeId == globalEdge.StartNodeId &&
                                                          e.EndNodeId == globalEdge.EndNodeId);
                if (edge is null && edgeMap.TryGetValue(globalEdge.Id, out Edge? value))
                {
                    edge = value;
                    if(i == 0)
                    {
                        edge.StartNodeId = globalEdge.StartNodeId;
                        edge.EndNodeId = globalEdge.EndNodeId;
                        edge.VehicleProperties = [];
                    }
                }

                // If edge not found and this is the first edge (i == 0), create virtual edge
                if (edge == null && i == 0)
                {
                    // Create virtual edge from GlobalEdge
                    edge = new Edge
                    {
                        Id = globalEdge.Id,
                        LevelId = levelId,
                        EdgeId = globalEdge.Id.ToString(), // Virtual edge identifier
                        StartNodeId = globalEdge.StartNodeId, // Current node (may be virtual)
                        EndNodeId = globalEdge.EndNodeId,
                        EdgeDescription = "Virtual Start Edge",
                    };

                    // Log virtual edge creation
                    if (logger != null)
                    {
                        try
                        {
                            var loggerType = logger.GetType();
                            var infoMethod = loggerType.GetMethod("Info", [typeof(string)]);
                            infoMethod?.Invoke(logger, [$"Created virtual start edge from node {globalEdge.StartNodeId} to node {globalEdge.EndNodeId} - robot is on edge"]);
                        }
                        catch { }
                    }
                }

                if (edge != null)
                {
                    // Get EdgeVehicleProperty for this vehicle type if available
                    EdgeVehicleProperty? edgeVehicleProperty = null;
                    if (vehicleTypeId.HasValue && edge.VehicleProperties != null)
                    {
                        edgeVehicleProperty = edge.VehicleProperties.FirstOrDefault(vp => vp.VehicleTypeId == vehicleTypeId.Value);
                    }

                    // Calculate edge length (Euclidean distance between start and end nodes)
                    var dx = pathNodes[i].X - pathNodes[i + 1].X;
                    var dy = pathNodes[i].Y - pathNodes[i + 1].Y;
                    double edgeLength = Math.Sqrt(dx * dx + dy * dy);

                    // Build trajectory from EdgeVehicleProperty fields
                    var startNode = pathNodes[i];
                    var endNode = pathNodes[i + 1];
                    Trajectory? trajectory = null;
                    if (edgeVehicleProperty?.TrajectoryDegree.HasValue == true)
                    {
                        var degree = edgeVehicleProperty.TrajectoryDegree.Value;

                        // Build control points array based on degree
                        List<ControlPoint> controlPoints =
                        [
                            // Always add start node as first control point
                            new() {
                                X = startNode.X,
                                Y = startNode.Y,
                                Weight = 1.0
                            }
                        ];
                        
                        // Add control point 1 for degree 2 and 3
                        if (degree >= 2 && edgeVehicleProperty.TrajectoryControlPoint1X.HasValue && edgeVehicleProperty.TrajectoryControlPoint1Y.HasValue)
                        {
                            controlPoints.Add(new ControlPoint
                            {
                                X = edgeVehicleProperty.TrajectoryControlPoint1X.Value,
                                Y = edgeVehicleProperty.TrajectoryControlPoint1Y.Value,
                                Weight = 1.0
                            });
                        }
                        else if (degree >= 2)
                        {
                            // Default: midpoint between start and end
                            controlPoints.Add(new ControlPoint
                            {
                                X = (startNode.X + endNode.X) / 2.0,
                                Y = (startNode.Y + endNode.Y) / 2.0,
                                Weight = 1.0
                            });
                        }
                        
                        // Add control point 2 for degree 3
                        if (degree >= 3 && edgeVehicleProperty.TrajectoryControlPoint2X.HasValue && edgeVehicleProperty.TrajectoryControlPoint2Y.HasValue)
                        {
                            controlPoints.Add(new ControlPoint
                            {
                                X = edgeVehicleProperty.TrajectoryControlPoint2X.Value,
                                Y = edgeVehicleProperty.TrajectoryControlPoint2Y.Value,
                                Weight = 1.0
                            });
                        }
                        else if (degree >= 3)
                        {
                            // Default: one-third point from start
                            controlPoints.Add(new ControlPoint
                            {
                                X = startNode.X + (endNode.X - startNode.X) / 3.0,
                                Y = startNode.Y + (endNode.Y - startNode.Y) / 3.0,
                                Weight = 1.0
                            });
                        }
                        
                        // Always add end node as last control point
                        controlPoints.Add(new ControlPoint
                        {
                            X = endNode.X,
                            Y = endNode.Y,
                            Weight = 1.0
                        });
                        
                        // Build knot vector based on degree
                        double[] knotVector = degree switch
                        {
                            1 => [0, 0, 1, 1],
                            2 => [0, 0, 0, 1, 1, 1],
                            3 => [0, 0, 0, 0, 1, 1, 1, 1],
                            _ => [0, 0, 1, 1] // Default to degree 1
                        };
                        
                        trajectory = new Trajectory
                        {
                            Degree = degree,
                            KnotVector = knotVector,
                            ControlPoints = [.. controlPoints]
                        };
                    }

                    // Build corridor from EdgeVehicleProperty fields
                    Corridor? corridor = null;
                    if (edgeVehicleProperty != null && 
                        (edgeVehicleProperty.CorridorLeftWidth.HasValue || 
                         edgeVehicleProperty.CorridorRightWidth.HasValue || 
                         edgeVehicleProperty.CorridorRefPoint.HasValue))
                    {
                        corridor = new Corridor
                        {
                            LeftWidth = edgeVehicleProperty.CorridorLeftWidth ?? 0.0,
                            RightWidth = edgeVehicleProperty.CorridorRightWidth ?? 0.0,
                            CorridorRefPoint = edgeVehicleProperty.CorridorRefPoint ?? RobotNet.VDA5050.Type.CorridorRefPoint.KINEMATICCENTER
                        };
                    }

                    // Parse actions from EdgeVehicleProperty
                    RobotNet.VDA5050.InstantAction.Action[] edgeActions = [];
                    if (!string.IsNullOrEmpty(edgeVehicleProperty?.Actions))
                    {
                        try
                        {
                            var parsedActions = System.Text.Json.JsonSerializer.Deserialize<RobotNet.VDA5050.InstantAction.ActionLIF[]?>(edgeVehicleProperty.Actions, JsonOptionExtends.Read);
                            if (parsedActions != null)
                            {
                                edgeActions = [..parsedActions.Where(a => a.RequirementType == RobotNet.VDA5050.Type.RequirementType.REQUIRED).Select(a => new RobotNet.VDA5050.InstantAction.Action()
                                {
                                    ActionId = Guid.NewGuid().ToString(),
                                    ActionDescription = a.ActionDescription,
                                    ActionParameters = [..a.ActionParameters],
                                    BlockingType = a.BlockingType,
                                    ActionType = a.ActionType
                                })];
                            }
                        }
                        catch
                        {
                            // Ignore parse errors, use empty array
                        }
                    }

                    // tính toán orientation cho robot

                    // Create VDA5050 Edge with full information
                    var vdaEdge = new RobotNet.VDA5050.Order.Edge
                    {
                        EdgeId = edge.Id.ToString(),
                        SequenceId = sequenceId,
                        Released = false,
                        EdgeDescription = edge.EdgeDescription,
                        StartNodeId = edge.StartNodeId.ToString(),
                        EndNodeId = edge.EndNodeId.ToString(),
                        MaxSpeed = edgeVehicleProperty?.MaxSpeed,
                        MaxHeight = edgeVehicleProperty?.MaxHeight,
                        MinHeight = edgeVehicleProperty?.MinHeight ,
                        Orientation = startNode.Orientation == Orientation.FORWARD ? 0 : startNode.Orientation == Orientation.BACKWARD ? Math.PI : null,
                        OrientationType = RobotNet.VDA5050.Type.OrientationType.TANGENTIAL,
                        Direction = string.Empty, // Not in EdgeVehicleProperty
                        RotationAllowed = edgeVehicleProperty?.RotationAllowed ,
                        MaxRotationSpeed = edgeVehicleProperty?.MaxRotationSpeed,
                        Length = edgeLength,
                        Trajectory = trajectory,
                        Corridor = corridor,
                        Actions = edgeActions
                    };

                    // Add edge segment (odd sequence IDs) with VDA5050 Edge
                    var edgeSegment = new RouteSegment
                    {
                        NodeId = edge.EndNodeId, // Target node of this edge
                        EdgeId = edge.Id,
                        StartNodeId = edge.StartNodeId,
                        EndNodeId = edge.EndNodeId,
                        Released = false,
                        VdaNode = null, // Edge segment doesn't have node
                        VdaEdge = vdaEdge
                    };

                    segments.Add(edgeSegment);
                    sequenceId++;
                }
                else
                {
                    // Edge not found and not first edge - this is an error
                    if (logger != null)
                    {
                        try
                        {
                            var loggerType = logger.GetType();
                            var warningMethod = loggerType.GetMethod("Warning", [typeof(string)]);
                            warningMethod?.Invoke(logger, [$"Edge not found for path segment {i}: StartNodeId={globalEdge.StartNodeId}, EndNodeId={globalEdge.EndNodeId}"]);
                        }
                        catch
                        {
                            // Ignore if Warning method doesn't exist
                        }
                    }
                }
            }
        }

        route.FullRoute = segments;
        route.CurrentSegmentIndex = 0;

        return route;
    }

    /// <summary>
    /// Split route into Base and Horizon
    /// </summary>
    public static void SplitRouteIntoBaseAndHorizon(RobotRoute route, int baseSegmentCount)
    {
        if (route.FullRoute.Count == 0)
            return;

        // Ensure baseSegmentCount doesn't exceed available segments
        var actualBaseCount = Math.Min(baseSegmentCount, route.FullRoute.Count - 1);
        if (actualBaseCount < 1)
            actualBaseCount = 1; // At least 1 segment in base

        // Split: Base gets first N segments, Horizon gets the rest
        route.Base = [.. route.FullRoute.Take(actualBaseCount)];
        route.Horizon = [.. route.FullRoute.Skip(actualBaseCount)];

        // Mark base segments as released
        foreach (var segment in route.Base)
        {
            segment.Released = true;
        }
    }
}