using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

public class PathMover
{
    private readonly Transform transform;
    private readonly MonoBehaviour monoBehaviour;
    private readonly float defaultSpeed;
    private readonly float rotationSpeed;
    private readonly int trajectoryResolution;
    private readonly ActionExecutor actionExecutor;
    private bool isMoving;
    private volatile bool isCancelled;
    private Coroutine moveCoroutine;
    private float currentTheta;
    private float currentVdaTheta;
    private readonly float rotationThreshold = 0.05f; // Ngưỡng xoay 0.1 rad (~5.7°)
    private bool isBackward = false; // Trạng thái đi lùi

    public event Action OnMoveCompleted;

    public PathMover(
        Transform transform,
        MonoBehaviour monoBehaviour,
        float defaultSpeed,
        int trajectoryResolution,
        ActionExecutor actionExecutor,
        float rotationSpeed = 180f)
    {
        this.transform = transform;
        this.monoBehaviour = monoBehaviour;
        this.defaultSpeed = defaultSpeed;
        this.rotationSpeed = rotationSpeed;
        this.trajectoryResolution = trajectoryResolution;
        this.actionExecutor = actionExecutor;
        isMoving = false;
        isCancelled = false;
        currentTheta = 0f;
        currentVdaTheta = 0f;
        isBackward = false;
    }

    public bool IsMoving => isMoving;
    public float CurrentTheta => currentTheta;
    public float CurrentVdaTheta => currentVdaTheta;

    public void SetTheta(float vdaTheta)
    {
        currentVdaTheta = vdaTheta;
        currentTheta = NormalizeTheta(-vdaTheta * Mathf.Rad2Deg);
        transform.eulerAngles = new Vector3(0, -vdaTheta * Mathf.Rad2Deg, 0);
    }

    public void UpdateVdaThetaFromUnity(float eulerYDegrees)
    {
        currentVdaTheta = NormalizeAngle(-eulerYDegrees * Mathf.Deg2Rad);
        currentTheta = NormalizeTheta(eulerYDegrees);
    }

    public void StartMove(Node[] nodes, Edge[] edges)
    {
        if (!isMoving)
        {
            isCancelled = false;
            isBackward = false; // Reset trạng thái đi lùi
            moveCoroutine = monoBehaviour.StartCoroutine(MoveAlongPath(nodes, edges));
            Debug.Log("Bắt đầu di chuyển mới.");
        }
        else
        {
            Debug.LogWarning("AMR đang di chuyển, bỏ qua lệnh mới.");
        }
    }

    public void CancelMove()
    {
        if (moveCoroutine != null)
        {
            monoBehaviour.StopCoroutine(moveCoroutine);
            moveCoroutine = null;
            Debug.Log("Đã dừng coroutine MoveAlongPath.");
        }
        isMoving = false;
        isCancelled = true;
        actionExecutor.CancelActions();
        isBackward = false; // Reset trạng thái đi lùi
        Debug.Log("Đã hủy di chuyển.");
    }
    private float RoundThetaToStandardAngle(float thetaRad)
    {
        float thetaDeg = thetaRad * Mathf.Rad2Deg;
        float[] standardAngles = { 0f, 90f, 180f, 270f };
        float roundingThreshold = 10f; // Ngưỡng làm tròn ±5°
        float closestAngle = standardAngles.OrderBy(a => Mathf.Abs(Mathf.DeltaAngle(thetaDeg, a))).First();

        if (Mathf.Abs(Mathf.DeltaAngle(thetaDeg, closestAngle)) <= roundingThreshold)
        {
            return closestAngle * Mathf.Deg2Rad;
        }

        return thetaRad;
    }
    private IEnumerator MoveAlongPath(Node[] nodes, Edge[] edges)
    {
        isMoving = true;
        var sortedEdges = edges.OrderBy(e => e.SequenceId).ToList();
        var sortedNodes = nodes.OrderBy(n => n.SequenceId).ToList();
        bool isLastEdge = false;

        // Xác định edge cong gần node cuối
        var curveEdges = sortedEdges.Where(e => e.Trajectory.Degree > 1).ToList();
        var lastCurveEdge = curveEdges.OrderByDescending(e => e.SequenceId).FirstOrDefault();
        int lastCurveSequenceId = lastCurveEdge != null ? lastCurveEdge.SequenceId : -1;

        // Xác định các node giao giữa đường thẳng và đường cong
        HashSet<string> intersectionNodeIds = new();
        for (int i = 0; i < sortedEdges.Count - 1; i++)
        {
            var currentEdge = sortedEdges[i];
            var nextEdge = sortedEdges[i + 1];
            bool isCurrentStraight = currentEdge.Trajectory.Degree == 1;
            bool isNextCurve = nextEdge.Trajectory.Degree > 1;
            bool isCurrentCurve = currentEdge.Trajectory.Degree > 1;
            bool isNextStraight = nextEdge.Trajectory.Degree == 1;

            if ((isCurrentStraight && isNextCurve) || (isCurrentCurve && isNextStraight))
            {
                intersectionNodeIds.Add(currentEdge.EndNodeId);
            }
        }

        // Xử lý node đầu tiên
        var firstNode = sortedNodes.FirstOrDefault(n => n.SequenceId == 0);
        if (firstNode != null)
        {
            // Xoay theo theta của node đầu tiên
            float vdaTheta = firstNode.NodePosition.Theta;
            float thetaDiff = Mathf.Abs(NormalizeAngle(vdaTheta - currentVdaTheta));
            if (thetaDiff > rotationThreshold)
            {
                currentVdaTheta = vdaTheta;
                float unityEulerY = -vdaTheta * Mathf.Rad2Deg;
                Quaternion targetRotation = Quaternion.Euler(0, unityEulerY, 0);
                while (Quaternion.Angle(transform.rotation, targetRotation) > 1f)
                {
                    transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, rotationSpeed * Time.deltaTime / 180f);
                    currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
                    yield return null;
                }
                transform.rotation = targetRotation;
            }

            // Thực thi hành động tại node đầu tiên
            if (firstNode.Actions != null && firstNode.Actions.Length > 0)
            {
                yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(firstNode));
                if (isCancelled) { isMoving = false; yield break; }
            }
        }

        for (int edgeIndex = 0; edgeIndex < sortedEdges.Count; edgeIndex++)
        {
            var edge = sortedEdges[edgeIndex];
            isLastEdge = (edgeIndex == sortedEdges.Count - 1);

            if (isCancelled) { isMoving = false; yield break; }

            if (edge.Trajectory?.ControlPoints == null || edge.Trajectory.ControlPoints.Length == 0)
                continue;

            var startNode = sortedNodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
            var endNode = sortedNodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);

            // Thực thi hành động tại startNode
            if (startNode != null && startNode.Actions != null && startNode.Actions.Length > 0)
            {
                yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(startNode));
                if (isCancelled) { isMoving = false; yield break; }
            }

            // Kích hoạt đi lùi tại điểm bắt đầu của edge cong cuối
            if (!isBackward && edge.SequenceId == lastCurveSequenceId)
            {
                isBackward = true;
            }

            // Thực thi các hành động của edge
            foreach (var action in edge.Actions)
            {
                yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteEdgeAction(edge, action));
                if (isCancelled) { isMoving = false; yield break; }
            }

            float speed = edge.MaxSpeed > 0 ? edge.MaxSpeed : defaultSpeed;
            List<Vector3> points;
            bool isCurve = edge.Trajectory.Degree > 1;

            if (!isCurve)
            {
                points = new List<Vector3>
            {
                new Vector3(edge.Trajectory.ControlPoints[0].X, transform.position.y, edge.Trajectory.ControlPoints[0].Y),
                new Vector3(edge.Trajectory.ControlPoints[1].X, transform.position.y, edge.Trajectory.ControlPoints[1].Y)
            };
            }
            else
            {
                points = GenerateBSplinePoints(edge.Trajectory, trajectoryResolution);
                if (points.Count == 0) continue;

                var lastControlPoint = edge.Trajectory.ControlPoints.Last();
                points[points.Count - 1] = new Vector3(lastControlPoint.X, transform.position.y, lastControlPoint.Y);
            }

            // Tính tổng quãng đường của edge
            float totalDistance = 0f;
            for (int i = 0; i < points.Count - 1; i++)
            {
                totalDistance += Vector3.Distance(points[i], points[i + 1]);
            }

            // Tính góc mục tiêu tại endNode, làm tròn ngay từ đầu cho edge cong
            float endTheta = endNode != null ? endNode.NodePosition.Theta : currentVdaTheta;
            if (isCurve && endNode != null)
            {
                endTheta = RoundThetaToStandardAngle(endTheta);
                Debug.Log($"Bắt đầu edge cong {edge.EdgeId}: Tổng quãng đường={totalDistance:F2}m, " +
                          $"Góc chuẩn mục tiêu tại endNode={endTheta:F2} rad (eulerY={-endTheta * Mathf.Rad2Deg:F2}°)");
            }
            Quaternion endRotation = Quaternion.Euler(0, -endTheta * Mathf.Rad2Deg, 0);

            for (int i = 0; i < points.Count - 1; i++)
            {
                if (isCancelled) { isMoving = false; yield break; }

                Vector3 currentPoint = points[i];
                Vector3 nextPoint = points[i + 1];
                Vector3 forward = (nextPoint - currentPoint).normalized;

                if (isBackward)
                {
                    forward = -forward;
                }

                float distanceToNext = Vector3.Distance(transform.position, nextPoint);
                float step = speed * Time.deltaTime;

                while (distanceToNext > 0.01f)
                {
                    if (isCancelled) { isMoving = false; yield break; }

                    transform.position = Vector3.MoveTowards(transform.position, nextPoint, step);

                    if (isCurve && forward != Vector3.zero)
                    {
                        // Tính tiến độ di chuyển trên toàn bộ edge
                        float remainingDistance = 0f;
                        for (int j = i; j < points.Count - 1; j++)
                        {
                            remainingDistance += Vector3.Distance(points[j], points[j + 1]);
                        }
                        float progress = 1f - (remainingDistance / totalDistance);
                        if (progress < 0) progress = 0;
                        if (progress > 1) progress = 1;

                        // Tính góc hiện tại từ vector forward
                        Quaternion tangentRotation = Quaternion.LookRotation(forward, Vector3.up);
                        tangentRotation *= Quaternion.Euler(0, -90, 0);

                        // Tính góc cần xoay và thời gian còn lại
                        float angleToTarget = Quaternion.Angle(tangentRotation, endRotation);
                        float timeRemaining = remainingDistance / speed;
                        float adjustedRotationSpeed = timeRemaining > 0 ? (angleToTarget / timeRemaining) : rotationSpeed;
                        adjustedRotationSpeed = Mathf.Max(adjustedRotationSpeed, rotationSpeed);

                        // Xoay dần về góc mục tiêu tại endNode (đã làm tròn)
                        Quaternion targetRotation = Quaternion.Slerp(tangentRotation, endRotation, progress);
                        float currentAngleToTarget = Quaternion.Angle(transform.rotation, targetRotation);

                        if (currentAngleToTarget > 0.1f)
                        {
   
                            transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, adjustedRotationSpeed * Time.deltaTime / 180f);
                            currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
                            currentVdaTheta = NormalizeAngle(-transform.eulerAngles.y * Mathf.Deg2Rad);
                        }
                    }

                    distanceToNext = Vector3.Distance(transform.position, nextPoint);
                    yield return null;
                }

                transform.position = nextPoint;
            }

            // Kiểm tra khớp góc tại endNode của edge cong
            if (isCurve && endNode != null)
            {
                float roundedTheta = endTheta; // Đã làm tròn từ đầu
                float thetaDiff = Mathf.Abs(NormalizeAngle(roundedTheta - currentVdaTheta));
            }

            // Xử lý endNode
            if (endNode != null)
            {
                bool shouldRotate = false;
                string rotateReason = "";

                // Kiểm tra xem endNode có cần xoay không
                if (isLastEdge)
                {
                    shouldRotate = true;
                    rotateReason = $"node cuối của edge cuối {edge.EdgeId}";
                }
                else if (intersectionNodeIds.Contains(endNode.NodeId))
                {
                    shouldRotate = true;
                    rotateReason = $"node giao giữa đường thẳng và đường cong, NodeId={endNode.NodeId}";
                }

                if (shouldRotate)
                {
                    float vdaTheta = endNode.NodePosition.Theta;
                    float roundedTheta = RoundThetaToStandardAngle(vdaTheta); // Làm tròn theta cho node giao hoặc node cuối
                    float thetaDiffEnd = Mathf.Abs(NormalizeAngle(roundedTheta - currentVdaTheta));
                    if (thetaDiffEnd > rotationThreshold)
                    {
                        currentVdaTheta = roundedTheta;
                        float unityEulerY = -roundedTheta * Mathf.Rad2Deg;
                        Quaternion targetRotation = Quaternion.Euler(0, unityEulerY, 0);
                        while (Quaternion.Angle(transform.rotation, targetRotation) > 0.1f)
                        {
                            transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, 0.07f);
                            currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
                            yield return null;
                        }
                        transform.rotation = targetRotation;
                    }
                }

                // Thực thi hành động tại endNode
                if (endNode.Actions != null && endNode.Actions.Length > 0)
                {
                    yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(endNode));
                    if (isCancelled) { isMoving = false; yield break; }
                }
            }

            // Tắt chế độ đi lùi
            if (isBackward)
            {
                isBackward = false;
                Debug.Log("Tắt chế độ đi lùi khi kết thúc order: isBackward=false");
            }
        }

        isMoving = false;
        OnMoveCompleted?.Invoke();
    }

    private float NormalizeTheta(float eulerYDegrees)
    {
        float theta = eulerYDegrees * Mathf.Deg2Rad;
        while (theta > Mathf.PI) theta -= 2 * Mathf.PI;
        while (theta < -Mathf.PI) theta += 2 * Mathf.PI;
        return theta;
    }

    private float NormalizeAngle(float angleRad)
    {
        while (angleRad > Mathf.PI) angleRad -= 2 * Mathf.PI;
        while (angleRad < -Mathf.PI) angleRad += 2 * Mathf.PI;
        return angleRad;
    }

    private List<Vector3> GenerateBSplinePoints(Trajectory trajectory, int resolution)
    {
        List<Vector3> points = new();
        int degree = trajectory.Degree;
        float[] knots = trajectory.KnotVector;
        ControlPoint[] controlPoints = trajectory.ControlPoints;

        if (controlPoints == null || controlPoints.Length < degree + 1 || knots == null || knots.Length < controlPoints.Length + degree + 1)
            return points;

        float minU = knots[degree];
        float maxU = knots[controlPoints.Length];
        if (minU >= maxU) return points;

        float minDistance = 0.1f;
        Vector3? lastPoint = null;

        for (int i = 0; i <= resolution; i++)
        {
            float u = minU + (maxU - minU) * i / (float)resolution;
            Vector2 point = CalculateBSplinePoint(u, degree, knots, controlPoints);
            if (point != Vector2.zero)
            {
                Vector3 pos = new(point.x, transform.position.y, point.y);
                if (lastPoint == null || Vector3.Distance(pos, lastPoint.Value) >= minDistance)
                {
                    points.Add(pos);
                    lastPoint = pos;
                }
            }
        }

        return points;
    }

    private Vector2 CalculateBSplinePoint(float u, int degree, float[] knots, ControlPoint[] controlPoints)
    {
        Vector2 point = Vector2.zero;
        int n = controlPoints.Length - 1;

        for (int i = 0; i <= n; i++)
        {
            float basis = CalculateBasisFunction(i, degree, u, knots);
            point += new Vector2(controlPoints[i].X, controlPoints[i].Y) * basis;
        }

        return point;
    }

    private float CalculateBasisFunction(int i, int degree, float u, float[] knots)
    {
        if (degree == 0)
            return (u >= knots[i] && u < knots[i + 1]) ? 1f : 0f;

        float left = 0f;
        float right = 0f;

        float denom1 = knots[i + degree] - knots[i];
        if (denom1 > 0)
            left = ((u - knots[i]) / denom1) * CalculateBasisFunction(i, degree - 1, u, knots);

        float denom2 = knots[i + degree + 1] - knots[i + 1];
        if (denom2 > 0)
            right = ((knots[i + degree + 1] - u) / denom2) * CalculateBasisFunction(i + 1, degree - 1, u, knots);

        return left + right;
    }
}