using RobotNet.MapShares;
using RobotNet.MapShares.Dtos;
using RobotNet.RobotManager.Services.Planner.Space;
using RobotNet.RobotShares.Dtos;
using RobotNet.RobotShares.Enums;

namespace RobotNet.RobotManager.Services.Traffic;


public class Agent
{
    public IRobotController Robot { get; set; } = null!;
    public AgentModel AgentModel { get; set; } = new();
    public Guid MapId { get; set; }
    public int InNodeIndex { get; set; }
    public TrafficNodeDto InNode { get; set; } = new();
    public int ReleaseNodeIndex => Nodes.IndexOf(ReleaseNode);
    public TrafficNodeDto ReleaseNode { get; set; } = new();
    public TrafficNodeDto GoalNode { get; set; } = new();
    public List<TrafficNodeDto> Nodes { get; set; } = [];
    public List<TrafficEdgeDto> Edges { get; set; } = [];
    public List<TrafficNodeDto> SubNodes { get; set; } = [];
    public List<TrafficEdgeDto> SubEdges { get; set; } = [];
    public List<TrafficNodeDto> GivewayNodes { get; set; } = [];
    public List<TrafficEdgeDto> GivewayEdges { get; set; } = [];
    public TrafficSolutionState State { get; set; }
    public RefreshPathState RefreshPathState { get; set; } = RefreshPathState.Compeleted;
    public string Message { get; set; } = "";

    private static double GetDistance(List<TrafficNodeDto> nodes)
    {
        double distance = 0;
        for (int i = 0; i < nodes.Count - 1; i++)
        {
            distance += Math.Sqrt(Math.Pow(nodes[i].X - nodes[i + 1].X, 2) + Math.Pow(nodes[i].Y - nodes[i + 1].Y, 2));
        }
        return distance;
    }

    public bool Checking(double trafficAvoidableNodeMax, double trafficDistanceMax)
    {
        if (State != TrafficSolutionState.GiveWay && State != TrafficSolutionState.LoopResolve &&
            RefreshPathState != RefreshPathState.Created && RefreshPathState != RefreshPathState.Refreshing)
        {
            if (ReleaseNodeIndex != -1 && InNodeIndex <= ReleaseNodeIndex && InNodeIndex < Nodes.Count)
            {
                var releaseNodes = Nodes.GetRange(InNodeIndex + 1, ReleaseNodeIndex - InNodeIndex);
                if (releaseNodes.Where(n => n.IsAvoidableNode).Count() < trafficAvoidableNodeMax) return true;
                var distance = GetDistance([.. Nodes.GetRange(InNodeIndex, ReleaseNodeIndex - InNodeIndex + 1)]);
                distance -= Math.Sqrt(Math.Pow(Robot.VisualizationMsg.AgvPosition.X - Nodes[InNodeIndex].X, 2) + Math.Pow(Robot.VisualizationMsg.AgvPosition.Y - Nodes[InNodeIndex].Y, 2));
                if (distance < trafficDistanceMax) return true;
            }
        }
        return false;
    }

    public TrafficNodeDto[] GetChekingNodes(double trafficAvoidableNodeMax, double trafficDistanceMin)
    {
        List<TrafficNodeDto> releaseNodes = [];
        double distance = 0;
        distance -= Math.Sqrt(Math.Pow(Nodes[InNodeIndex].X - Robot.VisualizationMsg.AgvPosition.X, 2) + Math.Pow(Nodes[InNodeIndex].Y - Robot.VisualizationMsg.AgvPosition.Y, 2));
        int index = InNodeIndex < ReleaseNodeIndex ? InNodeIndex + 1 : InNodeIndex;
        for (; index < Nodes.Count; index++)
        {
            releaseNodes.Add(Nodes[index]);
            if (index < Nodes.Count - 1)
            {
                var remainingNodes = Nodes.GetRange(index + 1, Nodes.Count - (index + 1));
                if (!remainingNodes.Any(n => n.IsAvoidableNode))
                {
                    index = Nodes.Count;
                    break;
                }
            }
            if (index > 0) distance += Math.Sqrt(Math.Pow(Nodes[index].X - Nodes[index - 1].X, 2) + Math.Pow(Nodes[index].Y - Nodes[index - 1].Y, 2));
            if (distance < trafficDistanceMin || !Nodes[index].IsAvoidableNode) continue;
            if (releaseNodes.Where(n => n.IsAvoidableNode).Count() >= trafficAvoidableNodeMax) break;
        }
        return index > ReleaseNodeIndex ? [.. Nodes.GetRange(ReleaseNodeIndex, index - ReleaseNodeIndex + (index < Nodes.Count ? 1 : 0))] : [];
    }

    public (TrafficSolutionState state, string message) UpdateGiveWay(TrafficNodeDto[] giveWayNodes, PathPlanner pathPlanner, MapManager map)
    {
        try
        {
            if (giveWayNodes.Length < 2) return (TrafficSolutionState.UnableResolve, "Lộ trình tránh đường không hợp lệ");
            if (!giveWayNodes.Any(n => !Nodes.Contains(n)))
            {
                var giveNodeIndex = Nodes.IndexOf(giveWayNodes[^1]);
                if (giveNodeIndex >= ReleaseNodeIndex)
                {
                    ReleaseNode = giveWayNodes[^1];
                    State = TrafficSolutionState.Complete;
                    return (TrafficSolutionState.Complete, "");
                }
            }
            var giveWayIndex = Nodes.IndexOf(giveWayNodes[0]);
            var releaseNodeInGivewaysIndex = Array.FindIndex(giveWayNodes, n=> n.Id == ReleaseNode.Id);
            if (giveWayIndex != -1)
            {
                List<TrafficNodeDto> subGiveWayNodes = [];
                List<EdgeDto> subGiveWayEdges = [];
                if (releaseNodeInGivewaysIndex != -1) subGiveWayNodes = giveWayNodes.ToList().GetRange(releaseNodeInGivewaysIndex, giveWayNodes.Length - releaseNodeInGivewaysIndex);
                else if (giveWayIndex == ReleaseNodeIndex) subGiveWayNodes = [.. giveWayNodes];
                else if (giveWayIndex < ReleaseNodeIndex)
                {
                    subGiveWayNodes = Nodes.GetRange(giveWayIndex + 1, ReleaseNodeIndex - giveWayIndex);
                    subGiveWayNodes.Reverse();
                    subGiveWayNodes.AddRange(giveWayNodes);
                }
                else
                {
                    subGiveWayNodes = Nodes.GetRange(ReleaseNodeIndex, giveWayIndex - ReleaseNodeIndex);
                    subGiveWayNodes.AddRange(giveWayNodes);
                }
                List<EdgeDto> edges = [..map.GetEdges(MapId, [..subGiveWayNodes.Select(n => new NodeDto
                    {
                        Id = n.Id,
                        Name = n.Name,
                        X = n.X,
                        Y = n.Y,
                        MapId = MapId,
                    })])];
                if (edges.Count > 0)
                {
                    double releaseTheta;
                    if (ReleaseNodeIndex > 0)
                    {
                        var (nearNodeX, nearNodeY) = MapEditorHelper.Curve(0.9, new EdgeCaculatorModel
                        {
                            TrajectoryDegree = Edges[ReleaseNodeIndex - 1].TrajectoryDegree,
                            ControlPoint1X = Edges[ReleaseNodeIndex - 1].ControlPoint1X,
                            ControlPoint1Y = Edges[ReleaseNodeIndex - 1].ControlPoint1Y,
                            ControlPoint2X = Edges[ReleaseNodeIndex - 1].ControlPoint2X,
                            ControlPoint2Y = Edges[ReleaseNodeIndex - 1].ControlPoint2Y,
                            X1 = Nodes[ReleaseNodeIndex - 1].X,
                            Y1 = Nodes[ReleaseNodeIndex - 1].Y,
                            X2 = Nodes[ReleaseNodeIndex].X,
                            Y2 = Nodes[ReleaseNodeIndex].Y,
                        });
                        var relaseForward = Math.Atan2(Nodes[ReleaseNodeIndex].Y - nearNodeY, Nodes[ReleaseNodeIndex].X - nearNodeX) * 180 / Math.PI;
                        var releaseBackward = Math.Atan2(nearNodeY - Nodes[ReleaseNodeIndex].Y, nearNodeX - Nodes[ReleaseNodeIndex].X) * 180 / Math.PI;
                        releaseTheta = Nodes[ReleaseNodeIndex - 1].Direction == RobotDirection.FORWARD ? relaseForward : releaseBackward;
                    }
                    else
                    {
                        var (nearNodeX, nearNodeY) = MapEditorHelper.Curve(0.1, new EdgeCaculatorModel
                        {
                            TrajectoryDegree = Edges[ReleaseNodeIndex].TrajectoryDegree,
                            ControlPoint1X = Edges[ReleaseNodeIndex].ControlPoint1X,
                            ControlPoint1Y = Edges[ReleaseNodeIndex].ControlPoint1Y,
                            ControlPoint2X = Edges[ReleaseNodeIndex].ControlPoint2X,
                            ControlPoint2Y = Edges[ReleaseNodeIndex].ControlPoint2Y,
                            X1 = Nodes[ReleaseNodeIndex].X,
                            Y1 = Nodes[ReleaseNodeIndex].Y,
                            X2 = Nodes[ReleaseNodeIndex + 1].X,
                            Y2 = Nodes[ReleaseNodeIndex + 1].Y,
                        });
                        var releaseBackward = Math.Atan2(Nodes[ReleaseNodeIndex].Y - nearNodeY, Nodes[ReleaseNodeIndex].X - nearNodeX) * 180 / Math.PI;
                        var relaseForward = Math.Atan2(nearNodeY - Nodes[ReleaseNodeIndex].Y, nearNodeX - Nodes[ReleaseNodeIndex].X) * 180 / Math.PI;
                        releaseTheta = Nodes[0].Direction == RobotDirection.FORWARD ? relaseForward : releaseBackward;
                    }
                    List<NodeDto> nodes = [..PathPlanner.CalculatorDirection(releaseTheta, [..subGiveWayNodes.Select(n => new NodeDto
                    {
                        Id = n.Id,
                        X = n.X,
                        Y = n.Y,
                        Name = n.Name,
                        MapId = MapId,
                    })], [..edges])];
                    GivewayNodes = [.. nodes.Select(n => new TrafficNodeDto
                    {
                        Id = n.Id,
                        X = n.X,
                        Y = n.Y,
                        Name = n.Name,
                        Direction = MapCompute.GetRobotDirection(n.Direction),
                    })];
                    //Console.WriteLine($"{Robot.SerialNumber} giveway: [{string.Join(",", GivewayNodes.Select(n => $"({n.Name} - {n.Direction})"))}]");
                    foreach (var node in GivewayNodes)
                    {
                        node.IsAvoidableNode = map.GetNegativeNodes(MapId, node.Id).Length > 2;
                        if (node.IsAvoidableNode)
                        {
                            node.AvoidablePaths = [.. map.GetNegativePaths(MapId, new() { Id = node.Id, X = node.X, Y = node.Y, Name = node.Name }, 2)
                                                .Where(path => path.Length > 0)
                                                .Select(path => path.Select(n => new TrafficNodeDto { Id = n.Id, X = n.X, Y = n.Y, Name = n.Name }).ToArray())];
                        }
                    }
                    GivewayEdges = [.. edges.Select(n => new TrafficEdgeDto
                    {
                        Id = n.Id,
                        ControlPoint1X = n.ControlPoint1X,
                        ControlPoint1Y = n.ControlPoint1Y,
                        ControlPoint2X = n.ControlPoint2X,
                        ControlPoint2Y = n.ControlPoint2Y,
                        StartNodeId = n.StartNodeId,
                        EndNodeId = n.EndNodeId,
                        TrajectoryDegree = n.TrajectoryDegree,
                    })];
                    State = TrafficSolutionState.GiveWay;
                    var angleForward = Math.Atan2(GivewayNodes[^1].Y - GivewayNodes[^2].Y, GivewayNodes[^1].X - GivewayNodes[^2].X) * 180 / Math.PI;
                    var angleBackward = Math.Atan2(GivewayNodes[^2].Y - GivewayNodes[^1].Y, GivewayNodes[^2].X - GivewayNodes[^1].X) * 180 / Math.PI;
                    RefreshPath(GivewayNodes[^1], GivewayNodes[^1].Direction == RobotDirection.FORWARD ? angleForward : angleBackward, pathPlanner, map);
                    return (TrafficSolutionState.GiveWay, "");
                }
                return (TrafficSolutionState.UnableResolve, $"Không tìm thấy edge yêu cầu phải tránh [{GivewayNodes[0].Name} - {GivewayNodes[1].Name}]");
            }
            return (TrafficSolutionState.UnableResolve, $"Không tìm thấy điểm xung đột {GivewayNodes[0].Name}");
        }
        catch (Exception ex)
        {
            return (TrafficSolutionState.UnableResolve, $"Cập nhật lộ trình tránh đường xảy ra lỗi: {ex.Message}");
        }
    }

    private void RefreshPath(TrafficNodeDto currentNode, double theta, PathPlanner planner, MapManager map)
    {
        RefreshPathState = RefreshPathState.Created;
        var plannerTask = Task.Run(async () =>
        {
            RefreshPathState = RefreshPathState.Refreshing;
            var path = await planner.Planning(currentNode.Id, theta, AgentModel.NavigationType, MapId, GoalNode.Id);
            if (!path.IsSuccess)
            {
                RefreshPathState = RefreshPathState.Error;
                Message = path.Message;
                return;
            }
            if (path.Data.Nodes is null || path.Data.Edges is null || path.Data.Edges.Length == 0 || path.Data.Nodes.Length < 2)
            {
                RefreshPathState = RefreshPathState.Error;
                Message = $"Đường dẫn tới đích [{GoalNode.Name} - {GoalNode.Id}] từ [{currentNode.Name} - {currentNode.Id}] không tồn tại";
                return;
            }
            SubEdges = [..path.Data.Edges.Select(n => new TrafficEdgeDto()
                {
                    Id = n.Id,
                    StartNodeId = n.StartNodeId,
                    EndNodeId = n.EndNodeId,
                    TrajectoryDegree = n.TrajectoryDegree,
                    ControlPoint1X = n.ControlPoint1X,
                    ControlPoint1Y = n.ControlPoint1Y,
                    ControlPoint2X = n.ControlPoint2X,
                    ControlPoint2Y = n.ControlPoint2Y,
                })];
            SubNodes = [..path.Data.Nodes.Select(n => new TrafficNodeDto()
                {
                    Id = n.Id,
                    Name = n.Name,
                    X = n.X,
                    Y = n.Y,
                    Direction = MapCompute.GetRobotDirection(n.Direction)
                })];
            foreach (var node in SubNodes)
            {
                node.IsAvoidableNode = map.GetNegativeNodes(MapId, node.Id).Length > 2;
                if (node.IsAvoidableNode)
                {
                    node.AvoidablePaths = [.. map.GetNegativePaths(MapId, new() { Id = node.Id, X = node.X, Y = node.Y, Name = node.Name }, 2)
                                        .Where(path => path.Length > 0)
                                        .Select(path => path.Select(n => new TrafficNodeDto { Id = n.Id, X = n.X, Y = n.Y, Name = n.Name }).ToArray())];
                }
            }
            //Console.WriteLine($"{Robot.SerialNumber} refresh path: [{string.Join(",", SubNodes.Select(n => $"({n.Name} - {n.Direction})"))}]");
            RefreshPathState = RefreshPathState.Compeleted;
        });
    }
}