using RobotNet.VDA5050.Order;
using RobotNet.VDA5050.Type;
using RobotNet10.Common;
using RobotNet10.RobotApp.Services.Exceptions;
using RobotNet10.RobotApp.Services.Robot.Models;
using RobotNet10.RobotApp.Shared.Enums;

namespace RobotNet10.RobotApp.Services.Robot.Helper;

public class OrderConverter
{
    public static (OrderNode[] Nodes, OrderEdge[] Edges) Validate(Node[] nodes, Edge[] edges, double currentTheta)
    {
        if (nodes.Length < 2) throw new PathPlannerException(RobotErrors.Error1002(nodes.Length));
        if (edges.Length != nodes.Length - 1) throw new PathPlannerException(RobotErrors.Error1004(nodes.Length, edges.Length));

        OrderNode[] orderNodes = [..nodes.Select(n => new OrderNode
        {
            NodeId = n.NodeId,
            SequenceId = n.SequenceId,
            X = n.NodePosition?.X ?? 0,
            Y = n.NodePosition?.Y ?? 0,
            Theta = n.NodePosition?.Theta,
            AllowedDeviationXY = n.NodePosition?.AllowedDeviationXY,
            AllowedDeviationTheta = n.NodePosition?.AllowedDeviationTheta,
        })];

        List<OrderEdge> orderEdges = [];
        foreach (var edge in edges)
        {
            var trajectory = edge.Trajectory;
            var controlPoints = trajectory?.ControlPoints;
            orderEdges.Add(new()
            {
                EdgeId = edge.EdgeId,
                SequenceId = edge.SequenceId,
                StartNodeId = edge.StartNodeId,
                EndNodeId = edge.EndNodeId,
                Orientation = edge.Orientation,
                OrientationType = edge.OrientationType,
                RotationAllowed = edge.RotationAllowed,
                Speed = edge.MaxSpeed,
                Degree = edge.Trajectory?.Degree ?? 1,
                ControlPoint1X = controlPoints is { Length: > 2 } ? controlPoints[1].X : 0,
                ControlPoint1Y = controlPoints is { Length: > 2 } ? controlPoints[1].Y : 0,
                ControlPoint2X = controlPoints is { Length: > 3 } ? controlPoints[2].X : 0,
                ControlPoint2Y = controlPoints is { Length: > 3 } ? controlPoints[2].Y : 0,
            });
        }
        // cần xử lí để lấy direction
        var currentDirection = GetDirectionInNode(nodes[0].NodePosition?.Theta ?? currentTheta, orderNodes[0], orderNodes[1], orderEdges[0]);
        for(int i = 0; i < orderEdges.Count; i++)
        {
            currentDirection = OrientationToDirection(currentDirection, orderNodes[i], orderNodes[i + 1], orderEdges[i]);
            orderEdges[i].Direction = currentDirection;
            orderNodes[i].ContinueTheta = GetAngleInNodeStart(orderNodes[i], orderNodes[i + 1], orderEdges[i]);
            if (i > 0)
            {
                var inNodeAngle = GetAngleInNodeEnd(orderNodes[i], orderNodes[i - 1], orderEdges[i - 1]);
                if (orderNodes[i].Theta is { } theta && Math.Abs(SpaceCompute.NormalizeRadianAngle(inNodeAngle) - SpaceCompute.NormalizeRadianAngle(theta)) > 0.04)
                {
                    orderNodes[i].IsWaitRotating = true;
                }
                if (!orderNodes[i].IsWaitRotating && orderNodes[i].ContinueTheta is { } continueTheta)
                {
                    if (Math.Abs(SpaceCompute.NormalizeRadianAngle(inNodeAngle) - SpaceCompute.NormalizeRadianAngle(continueTheta)) > 0.785)
                    {
                        orderNodes[i].IsWaitRotating = true;
                    }
                }
            }
        }
        return (orderNodes , [..orderEdges]);
    }

    private static RobotDirection ConvertTangentialOrientation(double orientation)
    {
        // Normalize về [0, 2*PI] để dễ xử lý
        double normalizedAngle = SpaceCompute.NormalizeRadianAngle(orientation);
        if (normalizedAngle < 0) normalizedAngle += 2 * Math.PI;

        // Forward: orientation gần 0 (hoặc 2*PI)
        // Backward: orientation gần PI

        // Kiểm tra gần 0 hoặc 2*PI (Forward)
        if (normalizedAngle <= Math.PI / 2 || normalizedAngle >= 3 * Math.PI / 2)
        {
            return RobotDirection.FORWARD;
        }
        // Kiểm tra gần PI (Backward)
        else
        {
            return RobotDirection.BACKWARD;
        }
    }

    private static RobotDirection ConvertGlobalOrientation(double orientation, OrderNode inNode, OrderNode futureNode, OrderEdge edge)
    {
        (double futurex, double futurey) = SpaceCompute.BezierPoint(0.1, new()
        {
            StartX = inNode.X,
            StartY = inNode.Y,
            EndX = futureNode.X,
            EndY = futureNode.Y,
            ControlPoint1X = edge.ControlPoint1X ?? 0,
            ControlPoint1Y = edge.ControlPoint1Y ?? 0,
            ControlPoint2X = edge.ControlPoint2X ?? 0,
            ControlPoint2Y = edge.ControlPoint2Y ?? 0,
            Degree = edge.Degree,
        });

        var edgeAngle = Math.Atan2(futurey - inNode.Y, futurex - inNode.X);

        // Tính góc chênh lệch giữa orientation và edge angle
        double angleDiff = SpaceCompute.NormalizeRadianAngle(orientation - edgeAngle);

        // Nếu góc chênh lệch gần 0 -> Forward
        // Nếu góc chênh lệch gần PI -> Backward
        double absAngleDiff = Math.Abs(angleDiff);

        if (absAngleDiff <= Math.PI / 2)
        {
            return RobotDirection.FORWARD;
        }
        else
        {
            return RobotDirection.BACKWARD;
        }
    }

    private static RobotDirection GetDirectionInNode(double currentTheta, OrderNode inNode, OrderNode futureNode, OrderEdge edge)
    {
        (double futurex, double futurey) = SpaceCompute.BezierPoint(0.1, new()
        {
            StartX = inNode.X,
            StartY = inNode.Y,
            EndX = futureNode.X,
            EndY = futureNode.Y,
            ControlPoint1X = edge.ControlPoint1X ?? 0,
            ControlPoint1Y = edge.ControlPoint1Y ?? 0,
            ControlPoint2X = edge.ControlPoint2X ?? 0,
            ControlPoint2Y = edge.ControlPoint2Y ?? 0,
            Degree = edge.Degree,
        });
        (double robotx, double roboty) =
        (
            inNode.X + Math.Cos(currentTheta),
            inNode.Y + Math.Sin(currentTheta)
        );

        var angle = SpaceCompute.GetVectorAngle(
                inNode.X,
                inNode.Y,
                robotx,
                roboty,
                futurex,
                futurey);
        return angle > 90 ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
    }

    private static double GetAngleInNodeEnd(OrderNode inNode, OrderNode oldNode, OrderEdge edge)
    {
        (double oldX, double oldY) = SpaceCompute.BezierPoint(0.9, new()
        {
            StartX = oldNode.X,
            StartY = oldNode.Y,
            EndX = inNode.X,
            EndY = inNode.Y,
            ControlPoint1X = edge.ControlPoint1X ?? 0,
            ControlPoint1Y = edge.ControlPoint1Y ?? 0,
            ControlPoint2X = edge.ControlPoint2X ?? 0,
            ControlPoint2Y = edge.ControlPoint2Y ?? 0,
            Degree = edge.Degree,
        });
        var dy = inNode.Y - oldY;
        var dx = inNode.X - oldX;
        return edge.Direction == RobotDirection.FORWARD ? Math.Atan2(dy, dx) : Math.Atan2(-dy, -dx);
    }

    private static double GetAngleInNodeStart(OrderNode inNode, OrderNode futureNode, OrderEdge edge)
    {
        (double futureX, double futureY) = SpaceCompute.BezierPoint(0.1, new()
        {
            StartX = inNode.X,
            StartY = inNode.Y,
            EndX = futureNode.X,
            EndY = futureNode.Y,
            ControlPoint1X = edge.ControlPoint1X ?? 0,
            ControlPoint1Y = edge.ControlPoint1Y ?? 0,
            ControlPoint2X = edge.ControlPoint2X ?? 0,
            ControlPoint2Y = edge.ControlPoint2Y ?? 0,
            Degree = edge.Degree,
        });
        var dy = futureY - inNode.Y;
        var dx = futureX - inNode.X;
        return edge.Direction == RobotDirection.FORWARD ? Math.Atan2(dy, dx) : Math.Atan2(-dy, -dx);
    }

    public static RobotDirection OrientationToDirection(RobotDirection currentDirection, OrderNode inNode, OrderNode futureNode, OrderEdge edge)
    {
        if(edge.Orientation.HasValue && edge.OrientationType is not null)
        {
            switch (edge.OrientationType)
            {
                case OrientationType.TANGENTIAL:
                    return ConvertTangentialOrientation(edge.Orientation.Value);

                case OrientationType.GLOBAL:
                    return ConvertGlobalOrientation(edge.Orientation.Value, inNode, futureNode, edge);
            }
        }
        if (inNode.Theta.HasValue) return GetDirectionInNode(inNode.Theta.Value, inNode, futureNode, edge);
        return currentDirection;
    }
}