using System;
using System.Collections;
using UnityEngine;

public class ActionExecutor
{
    private readonly MonoBehaviour monoBehaviour;
    private readonly AnimationControllerAPM animationController;
    private readonly ActionStateSender actionStateSender;
    private readonly Transform transform;
    private volatile bool isCancelled;
    private readonly float rotationSpeed = 180f; // Tốc độ xoay (độ/giây)
    private readonly float moveSpeed; // Tốc độ di chuyển từ AMR.defaultSpeed
    private readonly float checkPalletRadius; // Bán kính kiểm tra pallet
    private readonly float checkPalletMaxDistance; // Khoảng cách kiểm tra pallet
    private readonly LayerMask checkPalletTargetMask; // LayerMask kiểm tra pallet
    private readonly bool isDebuggingPalletCheck; // Bật/tắt debug vùng kiểm tra pallet

    public ActionExecutor(
        MonoBehaviour monoBehaviour,
        AnimationControllerAPM animationController,
        ActionStateSender actionStateSender,
        Transform transform,
        float moveSpeed,
        float checkPalletRadius,
        float checkPalletMaxDistance,
        LayerMask checkPalletTargetMask,
        bool isDebuggingPalletCheck)
    {
        this.monoBehaviour = monoBehaviour;
        this.animationController = animationController;
        this.actionStateSender = actionStateSender;
        this.transform = transform;
        this.moveSpeed = moveSpeed;
        this.checkPalletRadius = checkPalletRadius;
        this.checkPalletMaxDistance = checkPalletMaxDistance;
        this.checkPalletTargetMask = checkPalletTargetMask;
        this.isDebuggingPalletCheck = isDebuggingPalletCheck;
        this.isCancelled = false;

        if (animationController != null)
        {
            animationController.OnAnimationStateChanged += OnAnimationStateChanged;
        }
        else
        {
            Debug.LogError("AnimationControllerAPM is null in ActionExecutor constructor!");
        }
    }

    public void CancelActions()
    {
        isCancelled = true;
        Debug.Log("Đã hủy thực thi actions.");
    }

    public IEnumerator ExecuteNodeActions(Node node)
    {
        if (node == null || node.Actions == null || node.Actions.Length == 0)
        {
            yield break;
        }

        foreach (var action in node.Actions)
        {
            if (isCancelled)
            {
                Debug.Log($"Hủy thực thi actions tại node {node.NodeDescription} do isCancelled=true.");
                yield break;
            }

            // Gửi trạng thái WAITING
            yield return actionStateSender.SendActionStateAsync(
                action,
                node.NodeId,
                "NODE",
                "WAITING",
                "Action is waiting to start");

            // Thực thi hành động
            ExecutionResult execResult = null;
            yield return monoBehaviour.StartCoroutine(ExecuteAction(action, node, result => execResult = result));

            bool isActionSuccessful = execResult != null && execResult.IsActionSuccessful;
            string actionStatus = execResult != null ? execResult.ActionStatus : "FAILED";
            string resultDescription = execResult != null ? execResult.ResultDescription : "Unknown error";

            // Gửi trạng thái cuối cùng
            yield return actionStateSender.SendActionStateAsync(
                action,
                node.NodeId,
                "NODE",
                actionStatus,
                resultDescription);

            // Nếu hành động HARD thất bại, dừng lại
            if (action.BlockingType == "HARD" && actionStatus != "FINISHED")
            {
                Debug.LogError($"Action {action.ActionId} tại node {node.NodeDescription} có blockingType HARD nhưng không hoàn tất (status: {actionStatus}). Bỏ qua các hành động tiếp theo trong node này.");
                if (action.ActionType == "checkPallet")
                {
                    isCancelled = true;
                    yield return actionStateSender.SendCancelOrder(node, action);
                    yield break;
                }
                yield break;
            }

            // Đợi hành động HARD hoàn tất (pick, drop, rotation, moveBackward, startInPallet)
            if (isActionSuccessful && action.BlockingType == "HARD")
            {
                if (action.ActionType == "rotation")
                {
                    float rotationAngle = GetRotationAngle(action);
                    if (rotationAngle != 0f)
                    {
                        yield return monoBehaviour.StartCoroutine(RotateRobot(rotationAngle));
                    }
                    else
                    {
                        isActionSuccessful = false;
                        actionStatus = "FAILED";
                        resultDescription = "Invalid rotation angle";
                    }
                }
                else if (action.ActionType == "moveBackward")
                {
                    float distance = GetBackwardDistance(action);
                    if (distance > 0f)
                    {
                        yield return monoBehaviour.StartCoroutine(MoveBackward(distance));
                    }
                    else
                    {
                        isActionSuccessful = false;
                        actionStatus = "FAILED";
                        resultDescription = "Invalid or non-positive backward distance";
                    }
                }
                else if (action.ActionType == "startInPallet")
                {
                    float x = 0f, y = 0f, theta = 0f, speed = moveSpeed;
                    bool hasValidParams = ParseStartInPalletParams(action, out x, out y, out theta, out speed);
                    if (hasValidParams)
                    {
                        yield return monoBehaviour.StartCoroutine(MoveToPalletPosition(x, y, theta, speed));
                    }
                    else
                    {
                        isActionSuccessful = false;
                        actionStatus = "FAILED";
                        resultDescription = "Invalid or missing parameters for startInPallet";
                    }
                }

                // Gửi lại trạng thái nếu có lỗi
                if (!isActionSuccessful)
                {
                    yield return actionStateSender.SendActionStateAsync(
                        action,
                        node.NodeId,
                        "NODE",
                        actionStatus,
                        resultDescription);
                }
            }

            // Kiểm tra trạng thái sau khi thực thi
            if (action.BlockingType == "HARD" && actionStatus != "FINISHED")
            {
                Debug.LogError($"Action {action.ActionType} (ActionId: {action.ActionId}) failed with status {actionStatus}. Stopping further actions in node {node.NodeDescription}.");
                yield break;
            }
            else
            {
                Debug.Log($"Action {action.ActionType} (ActionId: {action.ActionId}) completed with status {actionStatus}. Proceeding to next action.");
            }
        }
    }

    // Helper class for execution result
    private class ExecutionResult
    {
        public bool IsActionSuccessful { get; set; }
        public string ActionStatus { get; set; }
        public string ResultDescription { get; set; }
    }

    public IEnumerator ExecuteEdgeAction(Edge edge, ActionData action)
    {
        if (isCancelled)
        {
            Debug.Log($"Hủy thực thi action {action.ActionType} tại edge {edge.EdgeDescription} do isCancelled=true.");
            yield break;
        }

        // Gửi trạng thái WAITING
        yield return actionStateSender.SendActionStateAsync(
            action,
            edge.EdgeId,
            "EDGE",
            "WAITING",
            "Action is waiting to start");

        // Gửi trạng thái RUNNING
        yield return actionStateSender.SendActionStateAsync(
            action,
            edge.EdgeId,
            "EDGE",
            "RUNNING",
            "Action is running");

        string actionStatus = "FINISHED";
        string resultDescription = "Action executed successfully";
        bool needYield = false;
        float x = 0f, y = 0f, theta = 0f, speed = moveSpeed;

        // Xử lý hành động trong một khối try-catch riêng
        try
        {
            switch (action.ActionType)
            {
                case "backwardNavigation":
                    break;

                case "pick":
                case "drop":
                    if (animationController != null)
                    {
                        if (action.ActionType == "pick")
                            animationController.OnKey2(); // Gắn pallet vào fork trong OnKey2
                        else
                            animationController.OnKey1(); // Tách pallet khỏi fork trong OnKey1
                    }
                    else
                    {
                        actionStatus = "FAILED";
                        resultDescription = "AnimationControllerAPM not found";
                    }
                    break;

                case "startInPallet":
                    bool hasValidParams = ParseStartInPalletParams(action, out x, out y, out theta, out speed);
                    if (hasValidParams)
                    {
                        needYield = true; // Đánh dấu cần yield coroutine
                    }
                    else
                    {
                        actionStatus = "FAILED";
                        resultDescription = "Invalid or missing parameters for startInPallet";
                    }
                    break;

                default:
                    actionStatus = "FAILED";
                    resultDescription = $"Unsupported ActionType: {action.ActionType}";
                    break;
            }
        }
        catch (Exception ex)
        {
            actionStatus = "FAILED";
            resultDescription = $"Action failed: {ex.Message}";
        }

        // Xử lý yield ngoài khối try-catch
        if (needYield && actionStatus == "FINISHED")
        {
            yield return monoBehaviour.StartCoroutine(MoveToPalletPosition(x, y, theta, speed));
        }

        // Gửi trạng thái cuối cùng
        yield return actionStateSender.SendActionStateAsync(
            action,
            edge.EdgeId,
            "EDGE",
            actionStatus,
            resultDescription);

        // Nếu hành động HARD thất bại, hủy di chuyển
        if (action.BlockingType == "HARD" && actionStatus != "FINISHED")
        {
            isCancelled = true;
        }
    }
    private IEnumerator WaitForAnimation(string actionType, float timeout, Action<bool> onComplete)
    {
        bool animationCompleted = false;
        float elapsed = 0f;

        void OnAnimationState(string type, string state)
        {
            if (type == actionType && state == "FINISHED")
            {
                animationCompleted = true;
                Debug.Log($"Animation {actionType} hoàn tất qua callback.");
            }
        }

        animationController.OnAnimationStateChanged += OnAnimationState;

        // Fallback: Kiểm tra trạng thái Animator nếu Animation Events không hoạt động
        Animator animator = animationController?.GetComponent<Animator>();
        string expectedState = actionType == "pick" ? "Up" : "Down";

        while (!animationCompleted && elapsed < timeout && !isCancelled)
        {
            if (animator != null)
            {
                AnimatorStateInfo stateInfo = animator.GetCurrentAnimatorStateInfo(0);
                if (stateInfo.IsName(expectedState) && stateInfo.normalizedTime >= 1f)
                {
                    animationCompleted = true;
                    Debug.Log($"Animation {actionType} hoàn tất qua polling.");
                }
            }
            elapsed += Time.deltaTime;
            yield return null;
        }

        animationController.OnAnimationStateChanged -= OnAnimationState;

        bool success = animationCompleted && !isCancelled;
        onComplete?.Invoke(success);

        if (!success)
        {
            Debug.LogWarning($"Animation {actionType} không hoàn tất: completed={animationCompleted}, cancelled={isCancelled}, timeout={timeout}s");
        }
    }
    private IEnumerator ExecuteAction(ActionData action, Node node, Action<ExecutionResult> callback)
    {
        var result = new ExecutionResult
        {
            IsActionSuccessful = true,
            ActionStatus = "FINISHED",
            ResultDescription = "Action executed successfully"
        };

        bool needToYieldCancelOrder = false;
        bool needToYieldMovement = false;
        bool needToYieldAnimation = false;
        float x = 0f, y = 0f, theta = 0f, speed = moveSpeed;

        try
        {
            switch (action.ActionType)
            {
                case "pick":
                case "drop":
                    if (animationController != null)
                    {
                        if (action.ActionType == "pick")
                            animationController.OnKey2(); // Gắn pallet vào fork
                        else
                            animationController.OnKey1(); // Tách pallet khỏi fork
                        needToYieldAnimation = true; // Đánh dấu cần đợi animation
                    }
                    else
                    {
                        result.IsActionSuccessful = false;
                        result.ActionStatus = "FAILED";
                        result.ResultDescription = "AnimationControllerAPM not found";
                    }
                    break;

                case "rotation":
                    float rotationAngle = GetRotationAngle(action);
                    if (rotationAngle != 0f)
                    {
                        if (action.BlockingType != "HARD")
                        {
                            monoBehaviour.StartCoroutine(RotateRobot(rotationAngle));
                        }
                    }
                    else
                    {
                        result.IsActionSuccessful = false;
                        result.ActionStatus = "FAILED";
                        result.ResultDescription = "Invalid or missing rotation angle in parameters";
                    }
                    break;

                case "moveBackward":
                    float distance = GetBackwardDistance(action);
                    if (distance > 0f)
                    {
                        if (action.BlockingType != "HARD")
                        {
                            monoBehaviour.StartCoroutine(MoveBackward(distance));
                        }
                    }
                    else
                    {
                        result.IsActionSuccessful = false;
                        result.ActionStatus = "FAILED";
                        result.ResultDescription = "Invalid or non-positive backward distance in parameters";
                    }
                    break;

                case "checkPallet":
                    bool palletDetected = CheckPallet(action);
                    if (palletDetected)
                    {
                        result.ActionStatus = "FINISHED";
                        result.ResultDescription = "Pallet detected successfully";
                    }
                    else
                    {
                        result.IsActionSuccessful = false;
                        result.ActionStatus = "FAILED";
                        result.ResultDescription = "Không có pallet";
                        needToYieldCancelOrder = true;
                    }
                    break;

                case "startInPallet":
                    bool hasValidParams = ParseStartInPalletParams(action, out x, out y, out theta, out speed);
                    if (hasValidParams)
                    {
                        needToYieldMovement = true; // Đánh dấu cần di chuyển
                    }
                    else
                    {
                        result.IsActionSuccessful = false;
                        result.ActionStatus = "FAILED";
                        result.ResultDescription = "Invalid or missing parameters for startInPallet";
                    }
                    break;

                default:
                    result.IsActionSuccessful = false;
                    result.ActionStatus = "FAILED";
                    result.ResultDescription = $"Unsupported ActionType: {action.ActionType}";
                    break;
            }
        }
        catch (Exception ex)
        {
            result.IsActionSuccessful = false;
            result.ActionStatus = "FAILED";
            result.ResultDescription = $"Action failed: {ex.Message}";
            Debug.LogError($"Lỗi khi thực thi action {action.ActionType} tại node {node.NodeDescription}: {ex.Message}");
        }

        // Xử lý các yield ngoài khối try-catch
        if (needToYieldAnimation && result.ActionStatus == "FINISHED")
        {
            float animationDuration = animationController.GetAnimationDuration(action.ActionType);
            if (animationDuration <= 0f)
            {
                result.IsActionSuccessful = false;
                result.ActionStatus = "FAILED";
                result.ResultDescription = "Invalid animation duration";
                Debug.LogError($"Invalid animation duration for {action.ActionType}. Check AnimationControllerAPM clip names.");
            }
            else
            {
                bool animationSuccess = false;
                yield return monoBehaviour.StartCoroutine(WaitForAnimation(action.ActionType, animationDuration + 1f, success => animationSuccess = success));
                if (!animationSuccess)
                {
                    result.IsActionSuccessful = false;
                    result.ActionStatus = "FAILED";
                    result.ResultDescription = "Animation did not complete within timeout or was cancelled";
                }
            }
        }

        if (needToYieldMovement && result.ActionStatus == "FINISHED")
        {
            yield return monoBehaviour.StartCoroutine(MoveToPalletPosition(x, y, theta, speed));
        }

        // Gọi callback với kết quả
        callback?.Invoke(result);

        // Xử lý hủy order nếu cần
        if (needToYieldCancelOrder)
        {
            yield return actionStateSender.SendCancelOrder(node, action);
        }
    }

    private bool ParseStartInPalletParams(ActionData action, out float x, out float y, out float theta, out float speed)
    {
        x = 0f;
        y = 0f;
        theta = 0f;
        speed = moveSpeed; // Tốc độ mặc định nếu không được cung cấp

        if (action.Parameters == null || action.Parameters.Length == 0)
        {
            Debug.LogWarning($"Action parameters are null or empty for startInPallet action (ActionId: {action.ActionId}).");
            return false;
        }

        bool hasX = false, hasY = false, hasTheta = false;

        foreach (var param in action.Parameters)
        {
            if (param.Key == "X" && float.TryParse(param.Value.ToString(), out float parsedX))
            {
                x = parsedX;
                hasX = true;
            }
            else if (param.Key == "Y" && float.TryParse(param.Value.ToString(), out float parsedY))
            {
                y = parsedY;
                hasY = true;
            }
            else if (param.Key == "Theta" && float.TryParse(param.Value.ToString(), out float parsedTheta))
            {
                theta = parsedTheta;
                hasTheta = true;
            }
            else if (param.Key == "speed" && float.TryParse(param.Value.ToString(), out float parsedSpeed) && parsedSpeed > 0f)
            {
                speed = parsedSpeed;
                Debug.Log($"Sử dụng tốc độ từ actionParameters: {speed} m/s");
            }
        }

        if (!hasX || !hasY || !hasTheta)
        {
            Debug.LogWarning($"Missing parameters for startInPallet action (ActionId: {action.ActionId}). Required: X, Y, Theta. Found: X={hasX}, Y={hasY}, Theta={hasTheta}");
            return false;
        }

        return true;
    }

    private IEnumerator MoveToPalletPosition(float x, float y, float theta, float speed)
    {
        if (isCancelled)
        {
            Debug.Log("Hủy di chuyển đến vị trí pallet do isCancelled=true.");
            yield break;
        }

        // Kiểm tra pallet trước khi di chuyển
        if (!CheckPallet(null)) // Gọi CheckPallet với action null để sử dụng tham số mặc định
        {
            Debug.LogWarning("Không phát hiện pallet trước khi thực hiện startInPallet, nhưng vẫn tiếp tục di chuyển.");
        }

        Vector3 targetPosition = new Vector3(x, transform.position.y, y);
        float distance = Vector3.Distance(transform.position, targetPosition);

        if (distance <= 0.01f)
        {
            Debug.LogWarning("Distance to pallet position is too small, skipping movement.");
            yield break;
        }

        // Di chuyển đến vị trí (x, y)
        Vector3 startPosition = transform.position;
        float elapsed = 0f;
        float duration = distance / speed;

        while (elapsed < duration && !isCancelled)
        {
            float t = elapsed / duration;
            transform.position = Vector3.Lerp(startPosition, targetPosition, t);
            elapsed += Time.deltaTime;
            yield return null;
        }

        if (!isCancelled)
        {
            transform.position = targetPosition;
            Debug.Log($"Hoàn thành di chuyển đến vị trí pallet ({x}, {y}) với tốc độ {speed} m/s.");
        }
        else
        {
            Debug.Log("Hủy di chuyển đến vị trí pallet do isCancelled=true.");
            yield break;
        }

        // Không xoay, giữ nguyên góc hiện tại
        Debug.Log($"Không xoay sau khi di chuyển đến vị trí pallet: giữ góc hiện tại, Unity eulerAngles.y={transform.eulerAngles.y:F2}°");
    }

    private bool CheckPallet(ActionData action)
    {
        float radius = checkPalletRadius;
        float maxDistance = checkPalletMaxDistance;
        LayerMask targetMask = checkPalletTargetMask;

        if (action?.Parameters != null)
        {
            foreach (var param in action.Parameters)
            {
                if (param.Key == "radius" && float.TryParse(param.Value.ToString(), out float parsedRadius))
                {
                    radius = parsedRadius;
                    Debug.Log($"Sử dụng radius từ actionParameters: {radius}");
                }
                else if (param.Key == "distance" && float.TryParse(param.Value.ToString(), out float parsedDistance))
                {
                    maxDistance = parsedDistance;
                    Debug.Log($"Sử dụng maxDistance từ actionParameters: {maxDistance}");
                }
            }
        }

        Vector3 origin = transform.position;
        Vector3 direction = transform.right; // Thay đổi từ transform.forward sang transform.right để kiểm tra theo trục X
        Vector3 endPoint = origin + direction * maxDistance;

        // Hiển thị vùng kiểm tra pallet khi bật debug
        if (isDebuggingPalletCheck)
        {
            // Vẽ đường từ vị trí robot đến điểm kiểm tra (sử dụng Debug.DrawRay để tương thích với runtime)
            Debug.DrawRay(origin, direction * maxDistance, Color.green, 0.1f);
        }

        Collider[] hits = Physics.OverlapSphere(endPoint, radius, targetMask);
        foreach (var hit in hits)
        {
            if (hit.CompareTag("Pallet") || hit.name.Contains("Pallet"))
            {
                Debug.Log($"Phát hiện pallet tại node: {hit.name} (vị trí: {endPoint})");

                if (animationController != null)
                {
                    animationController.pallet = hit.transform;
                    Debug.Log($"Đã gán pallet {hit.name} vào AnimationControllerAPM.pallet.");
                }
                else
                {
                    Debug.LogWarning("AnimationControllerAPM là null, không thể gán pallet.");
                }

                return true; // Phát hiện pallet
            }
        }

        Debug.Log($"Không phát hiện pallet tại điểm kiểm tra (vị trí: {endPoint})");

        if (animationController != null)
        {
            animationController.pallet = null;
            Debug.Log("Không tìm thấy pallet, đặt AnimationControllerAPM.pallet thành null.");
        }

        return false; // Không phát hiện pallet
    }
    private float GetRotationAngle(ActionData action)
    {
        if (action.Parameters == null || action.Parameters.Length == 0)
        {
            Debug.LogWarning($"Action parameters are null or empty for rotation action (ActionId: {action.ActionId}).");
            return 0f;
        }

        foreach (var param in action.Parameters)
        {
            if (param.Key == "ro")
            {
                Debug.Log($"Found rotation parameter 'ro' with value: {param.Value} for ActionId: {action.ActionId}");
                if (float.TryParse(param.Value.ToString(), out float angle))
                {
                    return angle;
                }
                else
                {
                    Debug.LogWarning($"Invalid rotation parameter value: {param.Value} for ActionId: {action.ActionId}");
                    return 0f;
                }
            }
        }

        Debug.LogWarning($"Rotation parameter 'ro' not found in action parameters for ActionId: {action.ActionId}.");
        return 0f;
    }

    private float GetBackwardDistance(ActionData action)
    {
        if (action.Parameters == null || action.Parameters.Length == 0)
        {
            Debug.LogWarning($"Action parameters are null or empty for moveBackward action (ActionId: {action.ActionId}).");
            return 0f;
        }

        foreach (var param in action.Parameters)
        {
            if (param.Key == "distance")
            {
                if (float.TryParse(param.Value.ToString(), out float distance) && distance > 0f)
                {
                    Debug.Log($"Found backward distance parameter 'distance' with value: {distance} for ActionId: {action.ActionId}");
                    return distance;
                }
                else
                {
                    Debug.LogWarning($"Invalid backward distance value: {param.Value} (must be positive) for ActionId: {action.ActionId}");
                    return 0f;
                }
            }
        }

        Debug.LogWarning($"Backward distance parameter 'distance' not found in action parameters for ActionId: {action.ActionId}.");
        return 0f;
    }

    private IEnumerator RotateRobot(float angleRad)
    {
        if (Mathf.Approximately(angleRad, 0f))
        {
            Debug.LogWarning("Rotation angle is zero, skipping rotation.");
            yield break;
        }

        float angleDeg = angleRad * Mathf.Rad2Deg;
        Quaternion startRotation = transform.rotation;
        Quaternion targetRotation = startRotation * Quaternion.Euler(0f, angleDeg, 0f);
        float elapsed = 0f;
        float duration = Mathf.Abs(angleDeg) / rotationSpeed;

        while (elapsed < duration && !isCancelled)
        {
            float t = elapsed / duration;
            transform.rotation = Quaternion.Slerp(startRotation, targetRotation, t);
            elapsed += Time.deltaTime;
            yield return null;
        }

        if (!isCancelled)
        {
            transform.rotation = targetRotation;
            Debug.Log($"Hoàn thành xoay {angleRad} rad ({angleDeg} độ).");
        }
        else
        {
            Debug.Log("Hủy xoay do isCancelled=true.");
        }
    }

    private IEnumerator MoveBackward(float distance)
    {
        if (distance <= 0f)
        {
            Debug.LogWarning("Backward distance is zero or negative, skipping moveBackward.");
            yield break;
        }

        Vector3 startPosition = transform.position;
        Vector3 direction = -transform.forward; // Hướng ngược lại
        Vector3 targetPosition = startPosition + direction * distance;
        float elapsed = 0f;
        float duration = distance / moveSpeed;

        while (elapsed < duration && !isCancelled)
        {
            float t = elapsed / duration;
            transform.position = Vector3.Lerp(startPosition, targetPosition, t);
            elapsed += Time.deltaTime;
            yield return null;
        }

        if (!isCancelled)
        {
            transform.position = targetPosition;
            Debug.Log($"Hoàn thành đi lùi {distance} mét.");
        } 
        else
        {
            Debug.Log("Hủy đi lùi do isCancelled=true.");
        }
    }

    private void OnAnimationStateChanged(string actionType, string state)
    {
        Debug.Log($"Animation state changed: {actionType} -> {state}");
    }
}