using RobotNet.MapShares;
using RobotNet.MapShares.Dtos;
using RobotNet.MapShares.Enums;
using RobotNet.RobotManager.Services.Planner;
using RobotNet.RobotManager.Services.Planner.Space;
using RobotNet.RobotShares.Enums;
using RobotNet.Shares;
using System.Collections.Generic;

namespace RobotNet.RobotManager.Services;

public enum PathPlanningType
{
    None,
    Angle,
    StartDirection,
    EndDirection,
}

public class PathPlanner(IConfiguration Configuration, MapManager MapManager)
{
    private readonly double ResolutionSplit = Configuration.GetValue("PathPlanning:ResolutionSplit", 0.1);
    private readonly PathPlanningType PathPlanningType = Configuration.GetValue("PathPlanning:Type", PathPlanningType.None);
    private readonly RobotDirection StartDirection = Configuration.GetValue("PathPlanning:StartDirection", RobotDirection.NONE);
    private readonly RobotDirection EndDirection = Configuration.GetValue("PathPlanning:EndDirection", RobotDirection.NONE);

    public async Task<MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)>> Planning(double xRef, double yRef, double thetaRef, NavigationType type, Guid mapId, string nodeName)
    {
        var map = await MapManager.GetMapData(mapId);
        if (!map.IsSuccess) return new(false, map.Message);
        if (map.Data is null) return new(false, "Dữ liệu bản đồ có lỗi");

        var goalNode = map.Data.Nodes.FirstOrDefault(n => n.Name == nodeName);
        if (goalNode is null) return new(false, "Đích đến không tồn tại");

        PathPlannerManager PathPlannerManager = new();
        var PathPlanner = PathPlannerManager.GetPathPlanningService(type);
        PathPlanner.SetData(map.Data.Nodes, map.Data.Edges);

        if (type == NavigationType.Forklift) return PathPlanner.PathPlanningWithAngle(xRef, yRef, thetaRef, goalNode.Id);
        else
        {
            return PathPlanningType switch
            {
                PathPlanningType.None => PathPlanner.PathPlanning(xRef, yRef, thetaRef, goalNode.Id),
                PathPlanningType.Angle => PathPlanner.PathPlanningWithAngle(xRef, yRef, thetaRef, goalNode.Id),
                PathPlanningType.StartDirection => PathPlanner.PathPlanningWithStartDirection(xRef, yRef, thetaRef, goalNode.Id, StartDirection),
                PathPlanningType.EndDirection => PathPlanner.PathPlanningWithFinalDirection(xRef, yRef, thetaRef, goalNode.Id, EndDirection),
                _ => PathPlanner.PathPlanning(xRef, yRef, thetaRef, goalNode.Id),
            };
        }
    }

    public async Task<MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)>> Planning(Guid startNodeId, double thetaRef, NavigationType type, Guid mapId, Guid goalId)
    {
        var map = await MapManager.GetMapData(mapId);
        if (!map.IsSuccess) return new(false, map.Message);
        if (map.Data is null) return new(false, "Dữ liệu bản đồ có lỗi");

        PathPlannerManager PathPlannerManager = new();
        var PathPlanner = PathPlannerManager.GetPathPlanningService(type);
        PathPlanner.SetData(map.Data.Nodes, map.Data.Edges);

        if (type == NavigationType.Forklift) return PathPlanner.PathPlanningWithAngle(startNodeId, thetaRef, goalId);
        else
        {
            return PathPlanningType switch
            {
                PathPlanningType.None => PathPlanner.PathPlanning(startNodeId, thetaRef, goalId),
                PathPlanningType.Angle => PathPlanner.PathPlanningWithAngle(startNodeId, thetaRef, goalId),
                PathPlanningType.StartDirection => PathPlanner.PathPlanningWithStartDirection(startNodeId, thetaRef, goalId, StartDirection),
                PathPlanningType.EndDirection => PathPlanner.PathPlanningWithFinalDirection(startNodeId, thetaRef, goalId, EndDirection),
                _ => PathPlanner.PathPlanning(startNodeId, thetaRef, goalId),
            };
        }
    }

    public static MessageResult<NodeDto[]> PathSplit(NodeDto[] nodes, EdgeDto[] edges, double resolutionSplit)
    {
        if (nodes.Length < 1 || edges.Length == 0) return new(false, "Dữ liệu không hợp lệ");
        List<NodeDto> pathSplit = [];
        pathSplit.Add(new()
        {
            Id = nodes[0].Id,
            X = nodes[0].X,
            Y = nodes[0].Y,
            Theta = nodes[0].Theta,
            Direction = nodes[0].Direction,
            Name = nodes[0].Name,
            Actions = "CheckingNode"
        });
        foreach (var edge in edges)
        {
            var startNode = nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
            var endNode = nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
            if (startNode is null || endNode is null) return new(false, "Dữ liệu lỗi: Điểm đầu cuối của edge không có trong danh sách nodes");
            NodeDto controlNode = new();

            var EdgeCaculatorModel = new EdgeCaculatorModel()
            {
                X1 = startNode.X,
                Y1 = startNode.Y,
                X2 = endNode.X,
                Y2 = endNode.Y,
                ControlPoint1X = edge.ControlPoint1X,
                ControlPoint1Y = edge.ControlPoint1Y,
                ControlPoint2X = edge.ControlPoint2X,
                ControlPoint2Y = edge.ControlPoint2Y,
                TrajectoryDegree = edge.TrajectoryDegree,
            };
            var length = MapEditorHelper.GetEdgeLength(EdgeCaculatorModel);

            if (length <= 0) continue;
            double step = resolutionSplit / length;

            for (double t = step; t <= 1 - step; t += step)
            {
                (double x, double y) = MapEditorHelper.Curve(t, EdgeCaculatorModel);
                pathSplit.Add(new()
                {
                    Id = Guid.NewGuid(),
                    X = x,
                    Y = y,
                    Theta = startNode.Theta,
                    Direction = startNode.Direction,
                });
            }

            pathSplit.Add(new()
            {
                Id = endNode.Id,
                X = endNode.X,
                Y = endNode.Y,
                Theta = endNode.Theta,
                Direction = endNode.Direction,
                Name = endNode.Name,
                Actions = "CheckingNode"
            });
        }
        return new(true) { Data = [.. pathSplit] };
    }

    public MessageResult<NodeDto[]> PathSplit(NodeDto[] nodes, EdgeDto[] edges)
    {
        if (nodes.Length < 1 || edges.Length == 0) return new(false, "Dữ liệu không hợp lệ");
        List<NodeDto> pathSplit = [];
        pathSplit.Add(new()
        {
            Id = nodes[0].Id,
            X = nodes[0].X,
            Y = nodes[0].Y,
            Theta = nodes[0].Theta,
            Direction = nodes[0].Direction,
            Name = nodes[0].Name,
            Actions = "CheckingNode"
        });
        foreach (var edge in edges)
        {
            var startNode = nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
            var endNode = nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
            if (startNode is null || endNode is null) return new(false, "Dữ liệu lỗi: Điểm đầu cuối của edge không có trong danh sách nodes");
            NodeDto controlNode = new();

            var EdgeCaculatorModel = new EdgeCaculatorModel()
            {
                X1 = startNode.X,
                Y1 = startNode.Y,
                X2 = endNode.X,
                Y2 = endNode.Y,
                ControlPoint1X = edge.ControlPoint1X,
                ControlPoint1Y = edge.ControlPoint1Y,
                ControlPoint2X = edge.ControlPoint2X,
                ControlPoint2Y = edge.ControlPoint2Y,
                TrajectoryDegree = edge.TrajectoryDegree,
            };
            var length = MapEditorHelper.GetEdgeLength(EdgeCaculatorModel);

            if (length <= 0) continue;
            double step = ResolutionSplit / length;

            for (double t = step; t <= 1 - step; t += step)
            {
                (double x, double y) = MapEditorHelper.Curve(t, EdgeCaculatorModel);
                pathSplit.Add(new()
                {
                    Id = Guid.NewGuid(),
                    X = x,
                    Y = y,
                    Theta = startNode.Theta,
                    Direction = startNode.Direction,
                });
            }

            pathSplit.Add(new()
            {
                Id = endNode.Id,
                X = endNode.X,
                Y = endNode.Y,
                Theta = endNode.Theta,
                Direction = endNode.Direction,
                Name = endNode.Name,
                Actions = "CheckingNode"
            });
        }
        return new(true) { Data = [.. pathSplit] };
    }

    public static RobotDirection GetRobotStartDirection(NodeDto currentNode, NodeDto nearNode, EdgeDto edge, double robotInNodeAngle)
    {
        NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, nearNode, edge, 0.1);

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

    public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, NodeDto[] nodes, EdgeDto[] edges)
    {
        RobotDirection[] RobotDirectionInNode = new RobotDirection[nodes.Length];
        if (nodes.Length > 0) RobotDirectionInNode[0] = currentDirection;
        if (nodes.Length > 2)
        {
            for (int i = 1; i < nodes.Length - 1; i++)
            {
                NodeDto nearLastNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i - 1], edges[i - 1], 0.1);
                NodeDto nearFutureNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i + 1], edges[i], 0.1); ;
                var angle = MapEditorHelper.GetAngle(nodes[i], nearLastNode, nearFutureNode);
                if (angle < 89) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
                else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
            }
        }
        if (nodes.Length > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
        return RobotDirectionInNode;
    }

    public static double[] GetRobotThetaInPath(NodeDto[] nodes, EdgeDto[] edges)
    {
        if (nodes.Length < 2 || edges.Length < 1 || nodes.Length - 1 != edges.Length) return [];
        double[] RobotThetaInNode = new double[nodes.Length];
        if (nodes.Length > 1)
        {
            for (int i = 0; i < nodes.Length - 1; i++)
            {
                NodeDto nearFutureNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i + 1], edges[i], 0.1);
                var angleForward = Math.Atan2(nearFutureNode.Y - nodes[i].Y, nearFutureNode.X - nodes[i].X) * 180 / Math.PI;
                var angleBackward = Math.Atan2(nodes[i].Y - nearFutureNode.Y, nodes[i].X - nearFutureNode.X) * 180 / Math.PI;

                RobotThetaInNode[i] = nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward;
            }
            RobotThetaInNode[^1] = RobotThetaInNode[^2];
        }
        return RobotThetaInNode;
    }

    public static NodeDto[] CalculatorDirection(NodeDto[] nodes, EdgeDto[] edges) => CalculatorDirection(RobotDirection.FORWARD, nodes, edges);

    public static NodeDto[] CalculatorDirection(RobotDirection startDirection, NodeDto[] nodes, EdgeDto[] edges)
    {
        var directions = GetRobotDirectionInPath(startDirection, nodes, edges);
        NodeDto[] returnNodes = [.. nodes];
        for (int i = 0; i < returnNodes.Length; i++)
        {
            returnNodes[i].Direction = MapCompute.GetNodeDirection(directions[i]);
        }
        var thetas = GetRobotThetaInPath(returnNodes, edges);
        for (int i = 0; i < returnNodes.Length; i++)
        {
            returnNodes[i].Theta = thetas[i];
        }
        return returnNodes;
    }

    public static NodeDto[] CalculatorDirection(double theta, NodeDto[] nodes, EdgeDto[] edges)
    {
        if (nodes.Length < 2) return nodes;
        var RobotNearNode = new NodeDto()
        {
            X = nodes[0].X + Math.Cos(theta * Math.PI / 180),
            Y = nodes[0].Y + Math.Sin(theta * Math.PI / 180),
        };
        nodes[0].Direction = MapEditorHelper.GetAngle(nodes[0], RobotNearNode, nodes[1]) > 89 ? MapShares.Enums.Direction.BACKWARD : MapShares.Enums.Direction.FORWARD;
        var directions = GetRobotDirectionInPath(MapCompute.GetRobotDirection(nodes[0].Direction), nodes, edges);
        NodeDto[] returnNodes = [.. nodes];
        for (int i = 0; i < returnNodes.Length; i++)
        {
            returnNodes[i].Direction = MapCompute.GetNodeDirection(directions[i]);
        }
        var thetas = GetRobotThetaInPath(returnNodes, edges);
        for (int i = 0; i < returnNodes.Length; i++)
        {
            returnNodes[i].Theta = thetas[i];
        }
        return returnNodes;
    }

    public static NodeDto[] CalculatorDirection(double theta, NodeDto[] nodes)
    {
        if (nodes.Length < 2) return nodes;
        var RobotNearNode = new NodeDto()
        {
            X = nodes[0].X + Math.Cos(theta * Math.PI / 180),
            Y = nodes[0].Y + Math.Sin(theta * Math.PI / 180),
        };
        nodes[0].Direction = MapEditorHelper.GetAngle(nodes[0], RobotNearNode, nodes[1]) > 89 ? MapShares.Enums.Direction.BACKWARD : MapShares.Enums.Direction.FORWARD;
        for (int i = 1; i < nodes.Length - 1; i++)
        {
            if (nodes[i].X == nodes[i - 1].X && nodes[i].Y == nodes[i - 1].Y) nodes[i].Direction = nodes[i - 1].Direction;
            else if (nodes[i].X == nodes[i + 1].X && nodes[i].Y == nodes[i + 1].Y) nodes[i].Direction = nodes[i - 1].Direction;
            else
            {
                var angle = MapEditorHelper.GetAngle(nodes[i], nodes[i - 1], nodes[i + 1]);
                nodes[i].Direction = angle > 89 ? nodes[i - 1].Direction : (nodes[i - 1].Direction == MapShares.Enums.Direction.FORWARD ? MapShares.Enums.Direction.BACKWARD : MapShares.Enums.Direction.FORWARD);
            }
        }
        nodes[^1].Direction = nodes[^2].Direction;
        return nodes;
    }

    public async Task<MessageResult<Dictionary<Guid, ZoneDto[]>>> GetZones(Guid mapId, NodeDto[] nodes)
    {
        var map = await MapManager.GetMapData(mapId);
        if (!map.IsSuccess) return new(false, map.Message);
        if (map.Data is null) return new(false, "Dữ liệu bản đồ có lỗi");

        Dictionary<Guid, ZoneDto[]> NodeInZones = [];
        foreach (var node in nodes)
        {
            List<ZoneDto> zones = [];
            foreach(var zone in map.Data.Zones)
            {
                if (MapEditorHelper.IsPointInside(node.X, node.Y, zone)) zones.Add(zone);
            }
            if (zones.Count > 0) NodeInZones.Add(node.Id, [..zones]);
        }
        return new(true, "") { Data = NodeInZones };
    }

    public static ZoneDto[] GetZones(NodeDto[] nodes, Dictionary<Guid, ZoneDto[]> zones)
    {
        List<ZoneDto> returnZones = [];
        foreach (var node in nodes)
        {
            if (zones.TryGetValue(node.Id, out ZoneDto[]? zone) && zone is not null && zone.Length > 0)
                returnZones.AddRange(zone.Where(z => !returnZones.Any(zo => zo.Id == z.Id)));
        }
        return [.. returnZones];
    }
}