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

namespace RobotNet.RobotManager.Services.Planner.Differential;

public class DifferentialPathPlanner : IPathPlanner
{
    private List<NodeDto> Nodes = [];
    private List<EdgeDto> Edges = [];
    private const double ReverseDirectionAngleDegree = 89;
    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 EdgeDto[] GetEdgesPlanning(NodeDto[] nodes)
    {
        var EdgesPlanning = new List<EdgeDto>();
        for (int i = 0; i < nodes.Length - 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}]");
        if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([goal], []) };

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

        RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
        var FinalDirection = GetRobotDirectionInPath(CurrenDirection, Path.Data, [..edgeplannings]);
        foreach (var item in Path.Data)
        {
            item.Direction = MapCompute.GetNodeDirection(FinalDirection[Path.Data.IndexOf(item)]);
        }

        return new(true)
        {
            Data = ([.. Path.Data], [.. 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 basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
        if (!basicPath.IsSuccess) return basicPath;
        if(basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([goal], []) };

        RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
        foreach (var node in basicPath.Data.Nodes)
        {
            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
        }

        return basicPath;
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
    {
        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
        if (!basicPath.IsSuccess) return basicPath;
        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };

        if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
        foreach (var node in basicPath.Data.Nodes)
        {
            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
        }

        return basicPath;
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
    {
        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
        if (!basicPath.IsSuccess) return basicPath;
        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };

        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
        foreach (var node in basicPath.Data.Nodes)
        {
            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
        }

        return basicPath;
    }

    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}]");
        if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };

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

        RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
        var FinalDirection = GetRobotDirectionInPath(CurrenDirection, [.. Path.Data], [.. edgeplannings]);
        for (int i = 0; i < Path.Data.Length; i++)
        {
            Path.Data[i].Direction = MapCompute.GetNodeDirection(FinalDirection[i]);
        }

        return new(true)
        {
            Data = ([.. Path.Data], [.. 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 basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
        if (!basicPath.IsSuccess) return basicPath;
        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };

        RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
        foreach (var node in basicPath.Data.Nodes)
        {
            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
        }

        return basicPath;
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
    {
        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
        if (!basicPath.IsSuccess) return basicPath;
        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };

        if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
        foreach (var node in basicPath.Data.Nodes)
        {
            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
        }

        return basicPath;
    }

    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
    {
        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
        if (!basicPath.IsSuccess) return basicPath;
        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };

        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
        foreach (var node in basicPath.Data.Nodes)
        {
            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
        }

        return basicPath;
    }
}