using RobotNet.MapShares.Dtos;
using RobotNet.RobotManager.Services.OpenACS;
using RobotNet.RobotManager.Services.Planner.Space;
using RobotNet.RobotManager.Services.Simulation.Algorithm;
using RobotNet.RobotManager.Services.Simulation.Models;
using RobotNet.RobotManager.Services.Traffic;
using RobotNet.RobotShares.Dtos;
using RobotNet.RobotShares.Enums;
using RobotNet.Shares;

namespace RobotNet.RobotManager.Services.Simulation;

public abstract class NavigationService : INavigationService, IDisposable
{
    protected readonly VisualizationService Visualization;
    protected readonly VelocityController VelocityController;
    protected readonly RobotSimulationModel RobotModel;
    protected readonly TrafficManager TrafficManager;
    protected readonly TrafficACS TrafficACS;
    protected readonly PathPlanner PathPlanner;
    protected readonly MapManager MapManager;
    protected readonly LoggerController<NavigationService> Logger;

    private WatchTimer<NavigationService>? navTimer;
    protected const int SampleTimeMilliseconds = 50;

    private WatchTimerAsync<NavigationService>? checkingTimer;
    private const int CheckingTimeMilliseconds = 1000;

    protected double TargetAngle = 0;
    protected PID? RotatePID;
    protected readonly double AngularVelocity;

    protected PID? MovePID;
    protected FuzzyLogic? MoveFuzzy;
    protected PurePursuit? MovePurePursuit;

    private Guid TrafficManagerGoalId = Guid.Empty;
    private Guid TrafficACSGoalId = Guid.Empty;
    protected NavigationNode? NavigationGoal;
    protected NavigationNode? FinalGoal;
    protected List<NavigationNode>? NavigationPath;
    protected List<NavigationNode> CheckingNodes = [];
    private List<NavigationNode> RefreshPath = [];
    private List<NavigationNode> BaseNodes = [];
    private bool IsWaittingStop;
    private double RotateAngle;
    protected Dictionary<Guid, ZoneDto[]> Zones = [];

    protected NavigationAction Action = NavigationAction.Stop;

    public NavigationNode? InNode { get; set; } = null;
    public NavigationStateType NavigationState { get; set; } = NavigationStateType.Ready;
    public TrafficSolutionState TrafficSolutionState { get; private set; } = TrafficSolutionState.None;
    public bool IsError { get; private set; }
    public bool IsCompleted { get; private set; } = true;
    public bool IsProcessing { get; private set; }
    public bool IsCanceled { get; private set; }
    public string[] Errors => [];
    public NavigationPathEdge[] FullPath => GetFullPath();
    public NavigationPathEdge[] BasePath => GetBasePath();
    public Guid MapId { get; set; } = Guid.Empty;
    public List<ZoneDto> CurrentZones { get; set; } = [];

    public NavigationService(VisualizationService visualization, RobotSimulationModel model, IServiceProvider ServiceProvider)
    {
        Visualization = visualization;
        VelocityController = new VelocityController(Visualization).WithDeceleration(model.Deceleration).WithAcceleration(model.Acceleration).WithSampleTime(SampleTimeMilliseconds / 1000.0);
        RobotModel = model;
        AngularVelocity = MathExtension.CheckLimit(RobotModel.MaxAngularVelocity * RobotModel.Width / RobotModel.RadiusWheel, 2 * Math.PI, 0);
        TrafficManager = ServiceProvider.GetRequiredService<TrafficManager>();
        TrafficACS = ServiceProvider.GetRequiredService<TrafficACS>();
        PathPlanner = ServiceProvider.GetRequiredService<PathPlanner>();
        MapManager = ServiceProvider.GetRequiredService<MapManager>();
        Logger = ServiceProvider.GetRequiredService<LoggerController<NavigationService>>();
    }

    public virtual MessageResult Rotate(double angle)
    {
        RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
        TargetAngle = MathExtension.CheckLimit(angle, 180, -180);
        Action = NavigationAction.Rotate;
        NavStart();
        CheckingStart();
        return new(true);
    }

    public virtual MessageResult MoveStraight(NavigationNode[] path)
    {
        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
        NavigationPath = [.. path];
        CheckingNodes = [NavigationPath[0], NavigationPath[^1]];
        InNode = CheckingNodes[0];

        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath(path);
        double Angle = Math.Atan2(path[1].Y - path[0].Y, path[1].X - path[0].X);
        Rotate(Angle * 180 / Math.PI);
        return new(true);
    }

    public virtual MessageResult Move(NavigationNode[] path)
    {
        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
        NavigationPath = [.. path];
        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
        if (CheckingNodes.Count < 2) return new(false, "Đường dẫn traffic không hợp lệ");
        InNode = CheckingNodes[0];

        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
        double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
        Rotate(Angle * 180 / Math.PI);
        return new(true);
    }

    public virtual MessageResult Move(NodeDto[] nodes, EdgeDto[] edges)
    {
        if (nodes.Length < 2) return new(false, "Đường dẫn không hợp lệ");
        var pathNodes = PathPlanner.CalculatorDirection(MapCompute.GetRobotDirection(nodes[0].Direction), nodes, edges);
        var pathSplit = PathPlanner.PathSplit(pathNodes, edges);
        if (!pathSplit.IsSuccess) return new(false, pathSplit.Message);
        if (pathSplit.Data is null) return new(false, "Đường dẫn không hợp lệ");

        var getZones = PathPlanner.GetZones(MapId, nodes);
        getZones.Wait();
        Zones = getZones.Result.Data ?? [];

        NavigationPath = [..pathSplit.Data.Select(n => new NavigationNode()
        {
            Id = n.Id,
            X = n.X,
            Y = n.Y,
            Theta = n.Theta,
            Direction = MapCompute.GetRobotDirection(n.Direction),
            Actions = n.Actions,
        })];
        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
        InNode = CheckingNodes[0];
        FinalGoal = CheckingNodes[^1];

        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);

        double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
        double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
        Rotate(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
        return new(true);
    }

    public MessageResult Cancel()
    {
        Dispose();
        VelocityController.Stop();
        NavigationState = NavigationStateType.Ready;
        IsCanceled = true;
        return new(true);
    }

    protected virtual void NavigationHandler()
    {
    }

    private void HandleRefreshPath(TrafficSolution trafficSolution)
    {
        var edgesDto = trafficSolution.Edges.Select(e => new EdgeDto
        {
            Id = e.Id,
            StartNodeId = e.StartNodeId,
            EndNodeId = e.EndNodeId,
            TrajectoryDegree = e.TrajectoryDegree,
            ControlPoint1X = e.ControlPoint1X,
            ControlPoint1Y = e.ControlPoint1Y,
            ControlPoint2X = e.ControlPoint2X,
            ControlPoint2Y = e.ControlPoint2Y,
        });
        List<NodeDto> nodesDto = [..trafficSolution.Nodes.Select(n => new NodeDto
                    {
                        Id = n.Id,
                        X = n.X,
                        Y = n.Y,
                        Name = n.Name,
                        Direction = MapCompute.GetNodeDirection(n.Direction),
                    })];
        var splitPath = PathPlanner.PathSplit([.. nodesDto], [.. edgesDto]);

        if (splitPath.IsSuccess)
        {
            if (splitPath.Data != null)
            {
                RefreshPath = [..splitPath.Data.Select(n => new NavigationNode()
                            {
                                Id = n.Id,
                                X = n.X,
                                Y = n.Y,
                                Theta = n.Theta,
                                Direction = MapCompute.GetRobotDirection(n.Direction),
                                Actions = n.Actions,
                            })];
            }
        }
    }

    private void HandleCompleteState(TrafficSolution trafficSolution)
    {
        if (trafficSolution.State == TrafficSolutionState.Complete && RefreshPath.Count > 0)
        {
            NavigationPath = RefreshPath;
            CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
            if (MovePurePursuit is not null)
            {
                MovePurePursuit.UpdatePath([.. NavigationPath]);
                MovePurePursuit.OnNodeIndex = MovePurePursuit.GetOnNode(Visualization.X, Visualization.Y).index;
            }
            RefreshPath = [];

            double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
            double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
            TargetAngle = MathExtension.CheckLimit(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward, 180, -180);
            Action = NavigationAction.Rotate;
        }
    }

    private void HandleGivewayState(TrafficSolution trafficSolution)
    {
        if (trafficSolution.GivewayNodes[^1].Id == trafficSolution.ReleaseNode.Id && NavigationGoal?.Id != trafficSolution.ReleaseNode.Id)
        {
            var splitPath = PathPlanner.PathSplit([..trafficSolution.GivewayNodes.Select(n => new NodeDto
                        {
                            Id = n.Id,
                            X = n.X,
                            Y = n.Y,
                            Name = n.Name,
                            Direction = MapCompute.GetNodeDirection(n.Direction),
                        })], [.. trafficSolution.GivewayEdges.Select(e => new EdgeDto
                        {
                            Id = e.Id,
                            StartNodeId = e.StartNodeId,
                            EndNodeId = e.EndNodeId,
                            TrajectoryDegree = e.TrajectoryDegree,
                            ControlPoint1X = e.ControlPoint1X,
                            ControlPoint1Y = e.ControlPoint1Y,
                            ControlPoint2X = e.ControlPoint2X,
                            ControlPoint2Y = e.ControlPoint2Y,
                        })]);
            if (splitPath.IsSuccess && splitPath.Data != null && MovePurePursuit != null)
            {
                NavigationPath = [..splitPath.Data.Select(n => new NavigationNode()
                                {
                                    Id = n.Id,
                                    X = n.X,
                                    Y = n.Y,
                                    Theta = n.Theta,
                                    Direction = MapCompute.GetRobotDirection(n.Direction),
                                    Actions = n.Actions,
                                })];
                CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
                MovePurePursuit.UpdatePath([.. NavigationPath]);
                MovePurePursuit.OnNodeIndex = 0;

                double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
                double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
                TargetAngle = MathExtension.CheckLimit(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward, 180, -180);
                Action = NavigationAction.Rotate;
            }
        }
    }

    private async Task<Guid> RequestInACS(Dictionary<Guid, ZoneDto[]> newZones)
    {
        Guid trafficACSrelaseNodeId = Guid.Empty;
        foreach (var zone in newZones)
        {
            if (zone.Value.Length == 0) trafficACSrelaseNodeId = zone.Key;
            else
            {
                bool requestSuccess = true;
                foreach (var zoneACS in zone.Value)
                {
                    if (string.IsNullOrEmpty(zoneACS.Name)) continue;
                    if (CurrentZones.Any(z => z.Id == zoneACS.Id)) continue;

                    var getInTrafficACS = await TrafficACS.RequestIn(RobotModel.RobotId, zoneACS.Name);
                    if (getInTrafficACS.IsSuccess && getInTrafficACS.Data) CurrentZones.Add(zoneACS);
                    else
                    {
                        requestSuccess = false;
                        break;
                    }
                }
                if (requestSuccess) trafficACSrelaseNodeId = zone.Key;
                else break;
            }
        }
        return trafficACSrelaseNodeId;
    }

    private void UpdateTraffic()
    {
        var trafficManagerGoalIndex = CheckingNodes.FindIndex(n => n.Id == TrafficManagerGoalId);
        var trafficACSGoalIndex = CheckingNodes.FindIndex(n => n.Id == TrafficACSGoalId);
        if (trafficManagerGoalIndex != -1 && trafficACSGoalIndex != -1)
        {
            var goalIndex = Math.Min(trafficManagerGoalIndex, trafficACSGoalIndex);
            NavigationNode goal = CheckingNodes[goalIndex];
            var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == (InNode is null ? Guid.Empty : InNode.Id));
            inNodeIndex = inNodeIndex != -1 ? inNodeIndex : 0;
            if (BaseNodes.Count == 0 || BaseNodes[^1].Id != goal.Id) BaseNodes = CheckingNodes.GetRange(inNodeIndex, goalIndex + 1 - inNodeIndex);

            if (IsWaittingStop)
            {
                if (NavigationState == NavigationStateType.Waitting)
                {
                    TargetAngle = MathExtension.CheckLimit(RotateAngle, 180, -180);
                    Action = NavigationAction.Rotate;
                    IsWaittingStop = false;
                }
            }
            else
            {
                for (int i = inNodeIndex + 1; i <= goalIndex; i++)
                {
                    if (Math.Abs(CheckingNodes[i].Theta - CheckingNodes[i - 1].Theta) > 30)
                    {
                        goal = CheckingNodes[i];
                        IsWaittingStop = true;
                        RotateAngle = CheckingNodes[i].Theta;
                        break;
                    }
                }
                UpdateGoal(goal);
            }
        }
    }

    private async Task GoOutTrafficACS()
    {
        if (BaseNodes is null || BaseNodes.Count == 0 || InNode is null) return;
        var goalIndex = CheckingNodes.FindIndex(n => n.Id == BaseNodes[^1].Id);
        if (goalIndex == -1) return;
        var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == InNode.Id);
        inNodeIndex = inNodeIndex != -1 ? inNodeIndex : 0;
        if (goalIndex <= inNodeIndex) return;
        var baseNodes = CheckingNodes.GetRange(inNodeIndex, goalIndex + 1 - inNodeIndex);
        var baseZones = PathPlanner.GetZones([..baseNodes.Select(n => n.ToNodeDto())], Zones);
        var outZones = CurrentZones.Where(z => !baseZones.Any(bz => bz.Id == z.Id)).ToList();
        foreach (var zoneACS in outZones)
        {
            if (string.IsNullOrEmpty(zoneACS.Name)) continue;
            var outTrafficACS = await TrafficACS.RequestOut(RobotModel.RobotId, zoneACS.Name);
            if (outTrafficACS.IsSuccess && outTrafficACS.Data) CurrentZones.RemoveAll(z => z.Id == zoneACS.Id);
        }
    }

    protected virtual async Task CheckingHandler()
    {
        try
        {
            if (NavigationPath is null || InNode is null) throw new Exception("Đường đi không tồn tại");

            await GoOutTrafficACS();

            if (!TrafficManager.Enable)
            {
                if (TrafficManagerGoalId != CheckingNodes[^1].Id) TrafficManagerGoalId = CheckingNodes[^1].Id;
            }
            else
            {
                TrafficManager.UpdateInNode(RobotModel.RobotId, InNode.Id);
                var trafficSolution = TrafficManager.GetTrafficNode(RobotModel.RobotId);
                if (trafficSolution.State != TrafficSolutionState.RefreshPath && TrafficSolutionState == TrafficSolutionState.RefreshPath && trafficSolution.Edges.Length > 0 && trafficSolution.Nodes.Length > 1)
                {
                    HandleRefreshPath(trafficSolution);
                }
                if (trafficSolution.State == TrafficSolutionState.Complete || trafficSolution.State == TrafficSolutionState.Waitting)
                {
                    HandleCompleteState(trafficSolution);
                }
                else if (trafficSolution.State == TrafficSolutionState.GiveWay && trafficSolution.GivewayNodes.Count > 1 && trafficSolution.GivewayEdges.Count > 0)
                {
                    HandleGivewayState(trafficSolution);
                }
                var goal = CheckingNodes.FirstOrDefault(n => n.Id == trafficSolution.ReleaseNode.Id);
                if (goal is not null && goal.Id != TrafficManagerGoalId) TrafficManagerGoalId = goal.Id;
                TrafficSolutionState = trafficSolution.State;
            }

            if (!TrafficACS.Enable)
            {
                if (TrafficACSGoalId != CheckingNodes[^1].Id) TrafficACSGoalId = CheckingNodes[^1].Id;
            }
            else
            {
                var newZones = TrafficACS.GetZones(InNode?.Id ?? Guid.Empty, [.. CheckingNodes.Select(n => n.ToNodeDto())], Zones);
                var trafficACSrelaseNodeId = await RequestInACS(newZones);
                if (trafficACSrelaseNodeId != Guid.Empty && trafficACSrelaseNodeId != TrafficACSGoalId) TrafficACSGoalId = trafficACSrelaseNodeId;
            }
            UpdateTraffic();
        }
        catch (Exception ex)
        {
            Logger.Warning($"{RobotModel.RobotId} Checking ex: {ex.Message}");
        }
    }

    private NavigationPathEdge[] GetBasePath()
    {
        if (InNode is not null && BaseNodes is not null && BaseNodes.Count > 1)
        {
            var inNodeIndex = BaseNodes.FindIndex(n => n.Id == InNode.Id);
            if (inNodeIndex != -1 && inNodeIndex < BaseNodes.Count - 1)
            {
                var nodes = BaseNodes.GetRange(inNodeIndex, BaseNodes.Count - inNodeIndex);
                var edges = MapManager.GetEdges(MapId, [..nodes.Select(n => new NodeDto
                {
                    Id = n.Id,
                })]);
                if (edges.Length == nodes.Count - 1 && nodes.Count > 1)
                {
                    List<NavigationPathEdge> pathEdges = [];
                    var edge = MapManager.GetEdge(nodes[0].Id, nodes[1].Id, MapId);
                    if (edge is null)
                    {
                        pathEdges.Add(new()
                        {
                            StartX = Visualization.X,
                            StartY = Visualization.Y,
                            EndX = nodes[1].X,
                            EndY = nodes[1].Y,
                            Degree = 1,
                        });
                    }
                    else
                    {
                        var splitStartPath = PathPlanner.PathSplit([new NodeDto
                        {
                            Id = nodes[0].Id,
                            X = nodes[0].X,
                            Y = nodes[0].Y,
                        }, new NodeDto
                        {
                            Id = nodes[1].Id,
                            X = nodes[1].X,
                            Y = nodes[1].Y,
                            Direction = MapCompute.GetNodeDirection(nodes[0].Direction),
                        }], [edge]);
                        if (splitStartPath.IsSuccess && splitStartPath.Data is not null)
                        {
                            int index = 0;
                            double minDistance = double.MaxValue;
                            for (int i = 0; i < splitStartPath.Data.Length; i++)
                            {
                                var distance = Math.Sqrt(Math.Pow(Visualization.X - splitStartPath.Data[i].X, 2) + Math.Pow(Visualization.Y - splitStartPath.Data[i].Y, 2));
                                if (distance < minDistance)
                                {
                                    minDistance = distance;
                                    index = i;
                                }
                            }
                            for (int i = index; i < splitStartPath.Data.Length - 1; i++)
                            {
                                pathEdges.Add(new()
                                {
                                    StartX = splitStartPath.Data[i].X,
                                    StartY = splitStartPath.Data[i].Y,
                                    EndX = splitStartPath.Data[i + 1].X,
                                    EndY = splitStartPath.Data[i + 1].Y,
                                    Degree = 1,
                                });
                            }
                        }
                    }
                    for (int i = 1; i < nodes.Count - 1; i++)
                    {
                        pathEdges.Add(new()
                        {
                            StartX = nodes[i].X,
                            StartY = nodes[i].Y,
                            EndX = nodes[i + 1].X,
                            EndY = nodes[i + 1].Y,
                            ControlPoint1X = edges[i].ControlPoint1X,
                            ControlPoint1Y = edges[i].ControlPoint1Y,
                            ControlPoint2X = edges[i].ControlPoint2X,
                            ControlPoint2Y = edges[i].ControlPoint2Y,
                            Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3,
                        });
                    }
                    return [.. pathEdges];
                }
            }
        }
        return [];
    }

    private NavigationPathEdge[] GetFullPath()
    {
        if (InNode is not null && CheckingNodes is not null && CheckingNodes.Count > 1)
        {
            var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == InNode.Id);
            if (inNodeIndex != -1 && inNodeIndex < CheckingNodes.Count - 1)
            {
                var nodes = CheckingNodes.GetRange(inNodeIndex, CheckingNodes.Count - inNodeIndex);
                var edges = MapManager.GetEdges(MapId, [..nodes.Select(n => new NodeDto
                {
                    Id = n.Id,
                })]);
                if (edges.Length == nodes.Count - 1 && nodes.Count > 1)
                {
                    List<NavigationPathEdge> pathEdges = [];
                    var edge = MapManager.GetEdge(nodes[0].Id, nodes[1].Id, MapId);
                    if (edge is null)
                    {
                        pathEdges.Add(new()
                        {
                            StartX = Visualization.X,
                            StartY = Visualization.Y,
                            EndX = nodes[1].X,
                            EndY = nodes[1].Y,
                            Degree = 1,
                        });
                    }
                    else
                    {
                        var splitStartPath = PathPlanner.PathSplit([new NodeDto
                        {
                            Id = nodes[0].Id,
                            X = nodes[0].X,
                            Y = nodes[0].Y,
                        }, new NodeDto
                        {
                            Id = nodes[1].Id,
                            X = nodes[1].X,
                            Y = nodes[1].Y,
                            Direction = MapCompute.GetNodeDirection(nodes[0].Direction),
                        }], [edge]);
                        if (splitStartPath.IsSuccess && splitStartPath.Data is not null)
                        {
                            int index = 0;
                            double minDistance = double.MaxValue;
                            for (int i = 0; i < splitStartPath.Data.Length; i++)
                            {
                                var distance = Math.Sqrt(Math.Pow(Visualization.X - splitStartPath.Data[i].X, 2) + Math.Pow(Visualization.Y - splitStartPath.Data[i].Y, 2));
                                if (distance < minDistance)
                                {
                                    minDistance = distance;
                                    index = i;
                                }
                            }
                            for (int i = index; i < splitStartPath.Data.Length - 1; i++)
                            {
                                pathEdges.Add(new()
                                {
                                    StartX = splitStartPath.Data[i].X,
                                    StartY = splitStartPath.Data[i].Y,
                                    EndX = splitStartPath.Data[i + 1].X,
                                    EndY = splitStartPath.Data[i + 1].Y,
                                    Degree = 1,
                                });
                            }
                        }
                    }
                    for (int i = 1; i < nodes.Count - 1; i++)
                    {
                        pathEdges.Add(new()
                        {
                            StartX = nodes[i].X,
                            StartY = nodes[i].Y,
                            EndX = nodes[i + 1].X,
                            EndY = nodes[i + 1].Y,
                            ControlPoint1X = edges[i].ControlPoint1X,
                            ControlPoint1Y = edges[i].ControlPoint1Y,
                            ControlPoint2X = edges[i].ControlPoint2X,
                            ControlPoint2Y = edges[i].ControlPoint2Y,
                            Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3,
                        });
                    }
                    return [.. pathEdges];
                }
            }
        }
        return [];
    }

    public bool UpdateGoal(NavigationNode goal)
    {
        if (NavigationGoal is null || NavigationGoal.Id != goal.Id)
        {
            if (NavigationPath is not null && NavigationPath.Any(node => node.Id == goal.Id))
            {
                NavigationGoal = goal;
                MovePurePursuit?.UpdateGoal(goal);
                return true;
            }
            return false;
        }
        return true;
    }

    private void ClearPath()
    {
        NavigationPath = null;
        NavigationGoal = null;
        InNode = null;
        CheckingNodes.Clear();
        TrafficManager.DeleteAgent(RobotModel.RobotId);
    }

    public void Dispose()
    {
        CheckingStop();
        NavStop();
        ClearPath();
        NavigationState = NavigationStateType.Ready;
        VelocityController.Stop();
        IsCompleted = true;
        IsProcessing = false;
        GC.SuppressFinalize(this);
    }

    protected void NavStart()
    {
        navTimer = new(SampleTimeMilliseconds, NavigationHandler, Logger);
        navTimer.Start();
        IsCompleted = false;
        IsCanceled = false;
        IsError = false;
        IsProcessing = true;
    }

    protected void NavStop()
    {
        navTimer?.Dispose();
    }

    protected void CheckingStart()
    {
        checkingTimer = new(CheckingTimeMilliseconds, CheckingHandler, Logger);
        checkingTimer.Start();
    }

    public void CreateComledted()
    {
        IsCompleted = true;
        IsCanceled = false;
        IsError = false;
    }

    protected void CheckingStop()
    {
        checkingTimer?.Dispose();
    }
}