using Microsoft.EntityFrameworkCore;
using RobotNet.MapShares;
using RobotNet.MapShares.Dtos;
using RobotNet.MapShares.Enums;
using RobotNet.RobotManager.Data;
using RobotNet.RobotManager.Services.Planner.Space;
using RobotNet.RobotManager.Services.Robot;
using RobotNet.RobotManager.Services.Simulation.Models;
using RobotNet.RobotManager.Services.Traffic;
using RobotNet.RobotShares.Dtos;
using RobotNet.RobotShares.VDA5050.Factsheet;
using RobotNet.RobotShares.VDA5050.FactsheetExtend;
using RobotNet.RobotShares.VDA5050.State;
using RobotNet.RobotShares.VDA5050.Visualization;
using RobotNet.Shares;
using Action = RobotNet.RobotShares.VDA5050.InstantAction.Action;

namespace RobotNet.RobotManager.Services.Simulation;

public class RobotSimulation : IRobotController, IDisposable
{
    public string SerialNumber { get; }
    public bool IsOnline { get; set; } = true;
    public bool IsWorking => NavigationService is not null && NavigationService.IsProcessing;
    public string State { get; } = "Simulation";
    public AutoResetEvent RobotUpdated { get; set; } = new(false);
    public string[] CurrentZones => NavigationService is null ? [] : [.. NavigationService.CurrentZones.Select(z => z.Name)];
    public StateMsg StateMsg { get => GetState(); set { } }
    public VisualizationMsg VisualizationMsg { get => GetVisualization(); set { } }
    public FactSheetMsg FactSheetMsg { get; set; } = new();
    public FactsheetExtendMsg FactsheetExtendMsg { get; set; } = new();
    public VisualizationService Visualization { get; set; }
    public NavigationPathEdge[] BasePath => NavigationService is null ? [] : NavigationService.BasePath;
    public NavigationPathEdge[] FullPath => NavigationService is null ? [] : NavigationService.FullPath;
    public RobotOrderDto OrderState => GetOrderState();
    public RobotActionDto[] ActionStates => [];

    private INavigationService? NavigationService;
    private readonly IServiceProvider ServiceProvider;
    private readonly LoggerController<RobotSimulation> Logger;
    private readonly RobotSimulationModel RobotSimulationModel;
    private CancellationTokenSource? CancelRandom;

    public RobotSimulation(string robotid, RobotModel model, IServiceProvider serviceProvider)
    {
        SerialNumber = robotid;
        RobotSimulationModel = new RobotSimulationModel()
        {
            RobotId = robotid,
            Acceleration = 2,
            Deceleration = 1,
            Length = model.Length / 2,
            Width = model.Width,
            MaxAngularVelocity = 0.3,
            MaxVelocity = 1.5,
            RadiusWheel = 0.1,
            NavigationType = model.NavigationType,
        };
        Visualization = new VisualizationService().WithRadiusWheel(RobotSimulationModel.RadiusWheel).WithRadiusRobot(RobotSimulationModel.Width);
        ServiceProvider = serviceProvider;
        Logger = ServiceProvider.GetRequiredService<LoggerController<RobotSimulation>>();
    }

    public void Initialize(double x, double y, double theta) => Visualization.LocalizationInitialize(x, y, theta);

    public MessageResult Rotate(double angle)
    {
        if (NavigationService is not null && NavigationService.NavigationState != NavigationStateType.Ready) return new(false, "Robot chưa sẵn sàng thực hiện nhiệm vụ");
        var olderNavigationService = NavigationService;
        NavigationService = NavigationManager.GetNavigation(Visualization, RobotSimulationModel, ServiceProvider);
        if (NavigationService is not null)
        {
            NavigationService.CurrentZones = olderNavigationService is null ? [] : olderNavigationService.CurrentZones;
            return NavigationService.Rotate(angle);
        }
        return new(false, "Model chưa được thiết lập");
    }

    public MessageResult MoveStraight(double x, double y)
    {
        try
        {
            var scope = ServiceProvider.CreateAsyncScope();
            var PathPlanningService = scope.ServiceProvider.GetRequiredService<PathPlanner>();
            var headRobotNode = new NodeDto()
            {
                Id = Guid.NewGuid(),
                X = Visualization.X * Math.Acos(Visualization.Theta * Math.PI / 180),
                Y = Visualization.Y * Math.Asin(Visualization.Theta * Math.PI / 180),
            };
            var currentRobotNode = new NodeDto()
            {
                Id = Guid.NewGuid(),
                X = Visualization.X,
                Y = Visualization.Y,
            };
            var goalNode = new NodeDto()
            {
                Id = Guid.NewGuid(),
                X = x,
                Y = y,
            };
            goalNode.Theta = MapEditorHelper.GetAngle(currentRobotNode, headRobotNode, goalNode) > 90 ? Math.Atan2(currentRobotNode.Y - goalNode.Y, currentRobotNode.X - goalNode.X) : Math.Atan2(goalNode.Y - currentRobotNode.Y, goalNode.X - currentRobotNode.X);
            currentRobotNode.Theta = goalNode.Theta;
            NodeDto[] nodesplanning = [currentRobotNode, goalNode];
            EdgeDto[] edgesplanning = [new()
            {
                Id= Guid.NewGuid(),
                DirectionAllowed = DirectionAllowed.Both,
                TrajectoryDegree = TrajectoryDegree.One,
                StartNodeId = currentRobotNode.Id,
                EndNodeId = goalNode.Id
            }];
            var resultSplit = PathPlanningService.PathSplit(nodesplanning, edgesplanning);
            if (!resultSplit.IsSuccess) return new(false, resultSplit.Message);
            if (resultSplit.Data is null || resultSplit.Data.Length < 1) return new(false, "Không tìm thấy đường dẫn tới đích");
            if (resultSplit.Data.Length < 1) return new(false, "Dữ liệu truyền vào không đúng");
            var navigationPath = resultSplit.Data.Select(n => new NavigationNode()
            {
                Id = Guid.NewGuid(),
                X = n.X,
                Y = n.Y,
                Theta = n.Theta,
                Direction = n.Direction == Direction.FORWARD ? RobotShares.Enums.RobotDirection.FORWARD : n.Direction == Direction.BACKWARD ? RobotShares.Enums.RobotDirection.BACKWARD : RobotShares.Enums.RobotDirection.NONE,
                Actions = n.Actions,
            }).ToArray();

            if (NavigationService is not null && NavigationService.NavigationState != NavigationStateType.Ready) return new(false, "Robot chưa sẵn sàng thực hiện nhiệm vụ");
            var olderNavigationService = NavigationService;
            NavigationService = NavigationManager.GetNavigation(Visualization, RobotSimulationModel, ServiceProvider);
            if (NavigationService is not null)
            {
                NavigationService.CurrentZones = olderNavigationService is null ? [] : olderNavigationService.CurrentZones;
                var moveTask = NavigationService.MoveStraight(navigationPath);
                return moveTask;
            }
            return new(false, "Model chưa được thiết lập");
        }
        catch (Exception ex)
        {
            return new(false, ex.Message);
        }
    }

    public Task<MessageResult> CancelOrder()
    {
        NavigationService?.Cancel();
        CancelRandom?.Cancel();
        return Task.FromResult<MessageResult>(new(true));
    }

    public async Task<MessageResult> MoveToNode(string goalName, IDictionary<string, IEnumerable<Action>>? actions = null, double? lastAngle = null)
    {
        try
        {
            var scope = ServiceProvider.CreateAsyncScope();
            var PathPlanner = scope.ServiceProvider.GetRequiredService<PathPlanner>();
            var AppDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
            var TrafficManager = ServiceProvider.GetRequiredService<TrafficManager>();
            var robotDb = await AppDb.Robots.FirstOrDefaultAsync(r => r.RobotId == SerialNumber);
            if (robotDb is null) return new(false, "RobotId không tồn tại");
            var robotModelDb = await AppDb.RobotModels.FirstOrDefaultAsync(r => r.Id == robotDb.ModelId);
            if (robotModelDb is null) return new(false, "Robot Model không tồn tại");

            var path = await PathPlanner.Planning(Visualization.X, Visualization.Y, Visualization.Theta, RobotSimulationModel.NavigationType, robotDb.MapId, goalName);
            if (!path.IsSuccess) return new(false, path.Message);
            if (path.IsSuccess && path.Data.Nodes.Length < 2)
            {
                NavigationService?.CreateComledted();
                return new(true, "");
            }
            if (path.Data.Nodes is null || path.Data.Edges is null) return new(false, $"Đường dẫn tới đích {goalName} từ [{Visualization.X} - {Visualization.Y} - {Visualization.Theta}] không tồn tại");

            if (NavigationService is not null && NavigationService.NavigationState != NavigationStateType.Ready) return new(false, "Robot chưa sẵn sàng thực hiện nhiệm vụ");
            var olderNavigationService = NavigationService;
            NavigationService = NavigationManager.GetNavigation(Visualization, RobotSimulationModel, ServiceProvider);
            if (NavigationService is not null)
            {
                var nodes = path.Data.Nodes;
                var createAgent = TrafficManager.CreateAgent(robotDb.MapId, this, new()
                {
                    NavigationType = robotModelDb.NavigationType,
                    Length = robotModelDb.Length,
                    Width = robotModelDb.Width,
                    NavigationPointX = robotModelDb.OriginX,
                    NavigationPointY = robotModelDb.OriginY,
                },
                [..nodes.Select(n => new TrafficNodeDto()
                {
                    Id = n.Id,
                    Name = n.Name,
                    X = n.X,
                    Y = n.Y,
                    Theta = n.Theta,
                    Direction = MapCompute.GetRobotDirection(n.Direction),
                })], [..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,
                })]);
                if (!createAgent.IsSuccess)
                {
                    Logger.Warning($"{SerialNumber} - Không thể tạo traffic agent: {createAgent.Message}");
                    return new(false, $"Không thể tạo traffic agent: {createAgent.Message}");
                }
                NavigationService.MapId = robotDb.MapId;
                NavigationService.CurrentZones = olderNavigationService is null ? [] : olderNavigationService.CurrentZones;
                var moveTask = NavigationService.Move(nodes, path.Data.Edges);
                return moveTask;
            }
            return new(false, "Model chưa được thiết lập");
        }
        catch (Exception ex)
        {
            return new(false, ex.Message);
        }
    }

    private VisualizationMsg GetVisualization()
    {
        var scope = ServiceProvider.CreateAsyncScope();
        var AppDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
        var robotDb = AppDb.Robots.FirstOrDefault(r => r.RobotId == SerialNumber);
        return new()
        {
            SerialNumber = SerialNumber,
            MapId = robotDb is null ? string.Empty : robotDb.MapId.ToString(),
            AgvPosition = new()
            {
                X = Visualization.X,
                Y = Visualization.Y,
                Theta = Visualization.Theta,
                PositionInitialized = true,
                LocalizationScore = 100,
                DeviationRange = 100,
            },
            Velocity = new()
            {
                Vx = Visualization.Vx,
                Vy = Visualization.Vy,
                Omega = Visualization.Omega,
            }
        };
    }

    private StateMsg GetState()
    {
        return new()
        {
            SerialNumber = SerialNumber,
            Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
            NewBaseRequest = true,
            BatteryState = new()
            {
                BatteryHealth = 100,
                BatteryCharge = 0,
                BatteryVoltage = 24,
                Charging = false,
            },
            ActionStates = [],
            Errors = [],
            Information = [],
            NodeStates = [],
            Loads = [],
        };
    }

    public Task<MessageResult<string>> InstantAction(Action action, bool waittingFeedback)
    {
        return Task.FromResult<MessageResult<string>>(new(true));
    }

    public void Log(string message, LogLevel level = LogLevel.Information)
    {
        switch (level)
        {
            case LogLevel.Trace: Logger.Trace($"{SerialNumber} - {message}"); break;
            case LogLevel.Error: Logger.Error($"{SerialNumber} - {message}"); break;
            case LogLevel.Warning: Logger.Warning($"{SerialNumber} - {message}"); break;
            case LogLevel.Critical: Logger.Critical($"{SerialNumber} - {message}"); break;
            case LogLevel.Information: Logger.Info($"{SerialNumber} - {message}"); break;
            case LogLevel.Debug: Logger.Debug($"{SerialNumber} - {message}"); break;
            default: Logger.Debug($"{SerialNumber} - {message}"); break;
        }
    }

    public Task<MessageResult> MoveRandom(List<string> nodes)
    {
        try
        {
            CancelRandom = new CancellationTokenSource();
            var random = new Random();
            var randomTask = Task.Run(async () =>
            {
                while (!CancelRandom.IsCancellationRequested)
                {
                    var index = random.Next(nodes.Count);
                    await MoveToNode(nodes[index]);
                    while (!CancelRandom.IsCancellationRequested)
                    {
                        if (!IsWorking) break;
                        await Task.Delay(1000);
                    }
                    await Task.Delay(2000);
                }
            }, cancellationToken: CancelRandom.Token);
        }
        catch { }
        return Task.FromResult<MessageResult>(new(true));
    }

    public Task<MessageResult> CancelAction()
    {
        return Task.FromResult<MessageResult>(new(true, ""));
    }

    public RobotOrderDto GetOrderState()
    {
        return new()
        {
            OrderId = StateMsg.OrderId,
            IsCompleted = NavigationService is not null && NavigationService.IsCompleted,
            IsError = NavigationService is not null && NavigationService.IsError,
            IsProcessing = NavigationService is not null && NavigationService.IsProcessing,
            IsCanceled = NavigationService is not null && NavigationService.IsCanceled,
            Errors = NavigationService is not null ? NavigationService.Errors : [],
        };
    }

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