using RobotNet.MapShares;
using RobotNet.MapShares.Dtos;
using RobotNet.MapShares.Enums;
using RobotNet.RobotManager.Services.Planner.AStar;
using RobotNet.RobotManager.Services.Planner.Fokrlift;
using RobotNet.RobotManager.Services.Planner.Space;
using RobotNet.RobotShares.Enums;
using RobotNet.RobotShares.Models;
using RobotNet.Shares;

namespace RobotNet.RobotManager.Services.Planner.ForkliftV2;

public class ForkLiftPathPlannerV2 : IPathPlanner
{
    private List<NodeDto> Nodes = [];
    private List<EdgeDto> Edges = [];
    private const double ReverseDirectionAngleDegree = 80;
    private const double RobotDirectionWithAngle = 90;
    private const double Ratio = 0.1;

    private readonly PathPlanningOptions Options = new()
    {
        LimitDistanceToEdge = 1,
        LimitDistanceToNode = 0.3,
        ResolutionSplit = 0.1,
    };

    public void SetData(NodeDto[] nodes, EdgeDto[] edges)
    {
        Nodes = [.. nodes];
        Edges = [.. edges];
    }

    public void SetOptions(PathPlanningOptions options)
    {
        Options.LimitDistanceToNode = options.LimitDistanceToNode;
        Options.LimitDistanceToEdge = options.LimitDistanceToEdge;
        Options.ResolutionSplit = options.ResolutionSplit;
    }

    public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, List<NodeDto> nodeplanning, List<EdgeDto> edgePlanning)
    {
        RobotDirection[] RobotDirectionInNode = new RobotDirection[nodeplanning.Count];
        if (nodeplanning.Count > 0) RobotDirectionInNode[0] = currentDirection;
        if (nodeplanning.Count > 2)
        {
            for (int i = 1; i < nodeplanning.Count - 1; i++)
            {
                NodeDto startNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i - 1], edgePlanning[i - 1], Ratio);
                NodeDto endNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i + 1], edgePlanning[i], Ratio); ;
                var angle = MapEditorHelper.GetAngle(nodeplanning[i], startNode, endNode);
                if (angle < ReverseDirectionAngleDegree) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
                else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
            }
        }
        if (nodeplanning.Count > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
        return RobotDirectionInNode;
    }

    public static RobotDirection GetRobotDirection(NodeDto currentNode, NodeDto nearNode, EdgeDto edge, double robotInNodeAngle, bool isReverse)
    {
        NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, nearNode, edge, Ratio);

        var RobotNearNode = new NodeDto()
        {
            X = currentNode.X + Math.Cos(robotInNodeAngle * Math.PI / 180),
            Y = currentNode.Y + Math.Sin(robotInNodeAngle * Math.PI / 180),
        };
        var angle = MapEditorHelper.GetAngle(currentNode, NearNode, RobotNearNode);
        if (angle > ReverseDirectionAngleDegree) return isReverse ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
        else return isReverse ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
    }

    private static NodeDto[] CalculatorDirection(List<NodeDto> nodes, List<EdgeDto> edges, RobotDirection currentDirection)
    {
        var FinalDirection = GetRobotDirectionInPath(currentDirection, nodes, edges);
        NodeDto[] returnNodes = [.. nodes];
        foreach (var item in returnNodes)
        {
            item.Direction = MapCompute.GetNodeDirection(FinalDirection[nodes.IndexOf(item)]);
        }
        return returnNodes;
    }

    private List<EdgeDto> GetEdgesPlanning(List<NodeDto> nodes)
    {
        var EdgesPlanning = new List<EdgeDto>();
        for (int i = 0; i < nodes.Count - 1; i++)
        {
            var edge = Edges.FirstOrDefault(e => e.StartNodeId == nodes[i].Id && e.EndNodeId == nodes[i + 1].Id ||
                                                  e.EndNodeId == nodes[i].Id && e.StartNodeId == nodes[i + 1].Id);
            if (edge is null)
            {
                if (i != 0) return [];
                EdgesPlanning.Add(new EdgeDto()
                {
                    Id = Guid.NewGuid(),
                    StartNodeId = nodes[i].Id,
                    EndNodeId = nodes[i + 1].Id,
                    DirectionAllowed = DirectionAllowed.Both,
                    TrajectoryDegree = TrajectoryDegree.One,
                });
                continue;
            }
            bool isReverse = nodes[i].Id != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three; ;
            EdgesPlanning.Add(new()
            {
                Id = edge.Id,
                StartNodeId = nodes[i].Id,
                EndNodeId = nodes[i + 1].Id,
                DirectionAllowed = edge.DirectionAllowed,
                TrajectoryDegree = edge.TrajectoryDegree,
                ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
                ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
                ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
                ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
            });
        }
        return EdgesPlanning;
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
    {
        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");

        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
        var Path = AStarPathPlanner.Planning(x,
                                              y,
                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
                                              Options.LimitDistanceToEdge,
                                              Options.LimitDistanceToNode,
                                              cancellationToken);
        if (!Path.IsSuccess) return new(false, Path.Message);
        if (Path.Data is null || Path.Data.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}, {theta}]") { Data = ([goal], []) };
        if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };

        var edgeplannings = GetEdgesPlanning([.. Path.Data]);
        return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
    {
        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");

        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
        var Path = SSEAStarPathPlanner.PlanningWithGoalAngle(x,
                                              y,
                                              theta,
                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y, Theta = goal.Theta },
                                              Options.LimitDistanceToEdge,
                                              Options.LimitDistanceToNode,
                                              cancellationToken);
        if (!Path.IsSuccess|| Path.Data is null || Path.Data.Count < 1)
        {
            var ForkliftV1 = new ForkliftPathPlanner();
            ForkliftV1.SetData([.. Nodes], [.. Edges]);
            return ForkliftV1.PathPlanningWithAngle(x,
                                                    y,
                                                    theta,
                                                    goal.Id,
                                                    cancellationToken);
        }
        else
        {
            if (Path.Data.Count == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{x}, {y}, {theta}]") { Data = ([goal], [])};

            var edgeplannings = GetEdgesPlanning(Path.Data);
            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");

            return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
        }
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
    {
        throw new NotImplementedException();
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
    {
        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");

        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
        var Path = SSEAStarPathPlanner.PlanningWithFinalDirection(x,
                                              y,
                                              theta,
                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
                                              goalDirection,
                                              Options.LimitDistanceToEdge,
                                              Options.LimitDistanceToNode,
                                              cancellationToken);
        if (!Path.IsSuccess || Path.Data is null || Path.Data.Count < 1)
        {
            var ForkliftV1 = new ForkliftPathPlanner();
            ForkliftV1.SetData([.. Nodes], [.. Edges]);
            return ForkliftV1.PathPlanningWithFinalDirection(x,
                                                                  y,
                                                                  theta,
                                                                  goal.Id,
                                                                  goalDirection,
                                                                  cancellationToken);
        }
        else
        {
            if (Path.Data.Count == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{x}, {y}, {theta}]") { Data = ([goal], []) };

            var edgeplannings = GetEdgesPlanning(Path.Data);
            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");

            return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
        }
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
    {
        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");

        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");

        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
        var Path = AStarPathPlanner.Planning(startNode,
                                            goal,
                                            cancellationToken);
        if (!Path.IsSuccess) return new(false, Path.Message);
        if (Path.Data is null || Path.Data.Length < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };
        if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };

        var edgeplannings = GetEdgesPlanning([.. Path.Data]);

        return new(true) { Data = ([.. CalculatorDirection([..Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
    {
        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");

        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");

        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
        var Path = SSEAStarPathPlanner.PlanningWithGoalAngle(startNode,
                                                            theta,
                                                            goal,
                                                            cancellationToken);
        if (!Path.IsSuccess || Path.Data is null || Path.Data.Length < 1)
        {
            var ForkliftV1 = new ForkliftPathPlanner();
            ForkliftV1.SetData([.. Nodes], [.. Edges]);
            return ForkliftV1.PathPlanningWithAngle(startNodeId,
                                                    theta,
                                                    goal.Id,
                                                    cancellationToken);
        }
        else
        {
            if (Path.Data.Length == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };

            var edgeplannings = GetEdgesPlanning([.. Path.Data]);
            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");

            return new(true) { Data = ([.. CalculatorDirection([.. Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
        }
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
    {
        throw new NotImplementedException();
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
    {
        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");

        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");

        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
        var Path = SSEAStarPathPlanner.PlanningWithFinalDirection(startNode,
                                                                  theta,
                                                                  goal,
                                                                  goalDirection,
                                                                  cancellationToken);
        if (!Path.IsSuccess || Path.Data is null || Path.Data.Length < 1)
        {
            var ForkliftV1 = new ForkliftPathPlanner();
            ForkliftV1.SetData([.. Nodes], [.. Edges]);
            return ForkliftV1.PathPlanningWithFinalDirection(startNodeId,
                                                                theta,
                                                                goal.Id,
                                                                goalDirection,
                                                                cancellationToken);
        }
        else
        {
            if (Path.Data.Length == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };

            var edgeplannings = GetEdgesPlanning([.. Path.Data]);
            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");

            return new(true) { Data = ([.. CalculatorDirection([.. Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
        }
    }
}