using System.Threading;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Motion;
using RobotNet10.RobotApp.Services.Robot;
using RobotNet10.Shared.Geometry;

namespace RobotNet10.RobotApp.Services.State;

/// <summary>
/// Class chứa các implementation methods cho Entry/Exit actions của State Machine
/// Sử dụng IServiceProvider để có thể inject các services khác khi cần
/// </summary>
public class RobotStateMachineExecute(IServiceScopeFactory ServiceScopeFactory, IPlcController PlcController, Logger<RobotStateMachineExecute> Logger)
{
    // ===========================
    // SYSTEM STATE HANDLERS
    // ===========================

    /// <summary>
    /// Entry: System -> Initializing
    /// Thực hiện khởi tạo các component của robot
    /// </summary>
    public async Task EntryInitializingAsync()
    {
        Logger.Info("==> Entry Initializing State");

        try
        {
            // TODO: Khởi tạo các services
            // - Kiểm tra kết nối phần cứng
            // - Khởi tạo Driver
            // - Khởi tạo Sensors
            // - Load cấu hình

            using var scope = ServiceScopeFactory.CreateAsyncScope();
            var robotController = scope.ServiceProvider.GetRequiredService<RobotController>();
            await robotController.ModuleInitializeAsync();
        }
        catch (Exception ex)
        {
            Logger.Error($"Initialization error: {ex.Message}");
            throw;
        }
    }

    public void EntryInitializing() => EntryInitializingAsync().GetAwaiter().GetResult();

    /// <summary>
    /// Entry: System -> Standby
    /// Robot đã sẵn sàng hoạt động
    /// </summary>
    public void EntryStandby()
    {
        Logger.Info("==> Entry Standby State");
        PlcController.SetSystemState(SystemState.IDLE);

        using var scope = ServiceScopeFactory.CreateAsyncScope();
        var stateService = scope.ServiceProvider.GetRequiredService<RobotStates>();
        var visualizationService = scope.ServiceProvider.GetRequiredService<RobotVisualization>();
        stateService.Start();
        visualizationService.Start();
    }

    /// <summary>
    /// Entry: System -> Shutting_Down
    /// Bắt đầu quá trình tắt máy
    /// </summary>
    public async Task EntryShuttingDownAsync()
    {
        Logger.Info("==> Entry Shutting Down State");

        try
        {
            // TODO:
            // - Lưu trạng thái hiện tại
            // - Dừng tất cả các task đang chạy
            // - Ngắt kết nối an toàn

            using var scope = ServiceScopeFactory.CreateAsyncScope();
            var robotController = scope.ServiceProvider.GetRequiredService<RobotController>();
            robotController.StopHandler();

            Logger.Info("Shutdown completed");
        }
        catch (Exception ex)
        {
            Logger.Error($"Shutdown error: {ex.Message}");
        }
    }

    public void EntryShuttingDown() => EntryShuttingDownAsync().GetAwaiter().GetResult();

    /// <summary>
    /// Exit: System -> Shutting_Down
    /// </summary>
    public void ExitShuttingDown()
    {
        Logger.Info("<== Exit Shutting Down State");
    }

    // ===========================
    // AUTO MODE STATE HANDLERS
    // ===========================

    /// <summary>
    /// Entry: Auto -> Idle
    /// Robot sẵn sàng nhận nhiệm vụ mới
    /// </summary>
    public void EntryIdle()
    {
        Logger.Info("==> Entry Idle State");
        PlcController.SetSystemState(SystemState.IDLE);
    }

    /// <summary>
    /// Entry: Auto -> Executing
    /// Bắt đầu thực hiện nhiệm vụ
    /// </summary>
    public void EntryExecuting()
    {
        Logger.Info("==> Entry Executing State");
        using var scope = ServiceScopeFactory.CreateAsyncScope();
        var PlcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
        PlcController.SetOperationState(OperationState.Move);
        // TODO:
        // - Lấy nhiệm vụ từ queue
        // - Khởi tạo execution context
        // - Bắt đầu timer theo dõi
        // - Cập nhật VDA5050 state = "EXECUTING"
    }

    /// <summary>
    /// Exit: Auto -> Executing
    /// </summary>
    public void ExitExecuting()
    {
        Logger.Info("<== Exit Executing State");

        using var scope = ServiceScopeFactory.CreateAsyncScope();
        var PlcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
        PlcController.SetOperationState(OperationState.None);

        // TODO:
        // - Cleanup execution context
        // - Dừng timer
    }

    /// <summary>
    /// Entry: Auto -> Paused
    /// Tạm dừng thực hiện nhiệm vụ
    /// </summary>
    public void EntryPaused()
    {
        Logger.Info("==> Entry Paused State");

        // TODO:
        // - Lưu trạng thái hiện tại
        // - Dừng robot
        // - Cập nhật VDA5050 state = "PAUSED"

        // Dừng robot
        StopRobot();
    }

    /// <summary>
    /// Exit: Auto -> Paused
    /// Resume từ trạng thái pause
    /// </summary>
    public void ExitPaused()
    {
        Logger.Info("<== Exit Paused State");

        // TODO:
        // - Khôi phục trạng thái
        // - Chuẩn bị tiếp tục thực hiện
    }

    /// <summary>
    /// Entry: Auto -> Canceling
    /// Đang hủy nhiệm vụ
    /// </summary>
    public void EntryCanceling()
    {
        Logger.Info("==> Entry Canceling State");

        // TODO:
        // - Dừng robot ngay lập tức
        // - Hủy tất cả các task con
        // - Cleanup resources

        StopRobot();
    }

    /// <summary>
    /// Exit: Auto -> Canceling
    /// Hoàn tất việc hủy
    /// </summary>
    public void ExitCanceling()
    {
        Logger.Info("<== Exit Canceling State");

        // TODO:
        // - Gửi thông báo nhiệm vụ đã hủy
        // - Reset execution context
        // - Cập nhật VDA5050 với error/cancelled
    }

    /// <summary>
    /// Entry: Auto -> Recovering
    /// Đang khôi phục từ lỗi
    /// </summary>
    public async Task EntryRecoveringAsync()
    {
        Logger.Info("==> Entry Recovering State");

        try
        {
            // TODO:
            // - Phân tích lỗi
            // - Thực hiện recovery procedure
            // - Kiểm tra trạng thái hệ thống

            Logger.Info("Analyzing error...");
            await Task.Delay(100);

            Logger.Info("Recovering system...");
            await Task.Delay(500);

            Logger.Info("Recovery succeeded");
        }
        catch (Exception ex)
        {
            Logger.Error($"Recovery error: {ex.Message}");
        }
    }

    public void EntryRecovering() => EntryRecoveringAsync().GetAwaiter().GetResult();

    /// <summary>
    /// Entry: Auto -> Remote_Override (OVERRIDE)
    /// Chuyển sang chế độ điều khiển từ xa - cho phép override tất cả safety
    /// </summary>
    public void EntryRemoteOverride()
    {
        Logger.Info("==> Entry Remote Override State (OVERRIDE)");
        PlcController.SetSystemState(SystemState.OVERRIDE);
        StopRobot();

        // Set ManualControlService state to Override - cho phép điều khiển với full override
        try
        {
            using var scope = ServiceScopeFactory.CreateScope();
            var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
            manualControlService.SetState(ManualControlState.Override);
            Logger.Info("ManualControlService set to Override state");
        }
        catch (Exception ex)
        {
            Logger.Error($"Error setting ManualControlService to Override: {ex.Message}");
        }
    }

    /// <summary>
    /// Exit: Auto -> Remote_Override (OVERRIDE)
    /// </summary>
    public void ExitRemoteOverride()
    {
        Logger.Info("<== Exit Remote Override State (OVERRIDE)");

        // Clear ManualControlService external state
        try
        {
            using var scope = ServiceScopeFactory.CreateScope();
            var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
            manualControlService.ClearExternalState();
            Logger.Info("ManualControlService external state cleared");
        }
        catch (Exception ex)
        {
            Logger.Error($"Error clearing ManualControlService external state: {ex.Message}");
        }
    }

    // ===========================
    // EXECUTING - MOVING STATE HANDLERS
    // ===========================

    /// <summary>
    /// Entry: Executing -> Moving
    /// Bắt đầu di chuyển
    /// </summary>
    public void EntryMoving()
    {
        Logger.Info("==> Entry Moving State");
        PlcController.SetSystemState(SystemState.PROCCESSING);
    }

    /// <summary>
    /// Exit: Executing -> Moving
    /// </summary>
    public void ExitMoving()
    {
        Logger.Info("<== Exit Moving State");

        // TODO:
        // - Dừng robot
        // - Lưu vị trí cuối cùng

        StopRobot();
    }

    /// <summary>
    /// Entry: Moving -> Navigation
    /// Chế độ navigation bình thường
    /// </summary>
    public void EntryNavigation()
    {
        Logger.Info("==> Entry Navigation State");

        // TODO:
        // - Load path từ planner
        // - Bắt đầu path following
        // - Monitor obstacles
    }

    /// <summary>
    /// Entry: Moving -> Avoidance
    /// Đang tránh chướng ngại vật
    /// </summary>
    public void EntryAvoidance()
    {
        Logger.Info("==> Entry Avoidance State");

        // TODO:
        // - Giảm tốc độ
        // - Tính toán đường tránh
        // - Theo dõi obstacle
    }

    /// <summary>
    /// Exit: Moving -> Avoidance
    /// </summary>
    public void ExitAvoidance()
    {
        Logger.Info("<== Exit Avoidance State");

        // TODO:
        // - Quay lại tốc độ bình thường
        // - Resume path chính
    }

    /// <summary>
    /// Entry: Moving -> Approach
    /// Tiếp cận mục tiêu (precision mode)
    /// </summary>
    public void EntryApproach()
    {
        Logger.Info("==> Entry Approach State");

        // TODO:
        // - Chuyển sang precision mode
        // - Giảm tốc độ tối đa
        // - Sử dụng sensors chính xác cao
    }

    /// <summary>
    /// Entry: Moving -> Tracking
    /// Theo dõi mục tiêu động
    /// </summary>
    public void EntryTracking()
    {
        Logger.Info("==> Entry Tracking State");

        // TODO:
        // - Bật target tracking
        // - Theo dõi vị trí mục tiêu
        // - Điều chỉnh trajectory theo realtime
    }

    /// <summary>
    /// Entry: Moving -> Repositioning
    /// Điều chỉnh lại vị trí
    /// </summary>
    public void EntryRepositioning()
    {
        Logger.Info("==> Entry Repositioning State");

        // TODO:
        // - Tính toán vị trí mong muốn
        // - Di chuyển điều chỉnh nhỏ
        // - Kiểm tra orientation
    }

    // ===========================
    // EXECUTING - ACT STATE HANDLERS
    // ===========================

    /// <summary>
    /// Entry: Executing -> ACT
    /// Bắt đầu thực hiện action
    /// </summary>
    public void EntryACT()
    {
        Logger.Info("==> Entry ACT State");

        // TODO:
        // - Dừng robot hoàn toàn
        // - Chuẩn bị thực hiện action
        // - Kiểm tra vị trí chính xác

        StopRobot();
    }

    /// <summary>
    /// Exit: Executing -> ACT
    /// </summary>
    public void ExitACT()
    {
        Logger.Info("<== Exit ACT State");

        // TODO:
        // - Cleanup action resources
        // - Kiểm tra kết quả action
    }

    /// <summary>
    /// Entry: ACT -> Docking
    /// Đang thực hiện docking
    /// </summary>
    public async Task EntryDockingAsync()
    {
        Logger.Info("==> Entry Docking State");
        PlcController.SetSystemState(SystemState.DOCKING);

        try
        {
            // TODO:
            // - Tìm vị trí dock station
            // - Align với dock
            // - Di chuyển vào dock từ từ

            Logger.Info("Searching for dock station...");
            await Task.Delay(200);

            Logger.Info("Aligning with dock...");
            await Task.Delay(300);

            Logger.Info("Moving into dock...");
            await Task.Delay(500);

            Logger.Info("Docking succeeded");
        }
        catch (Exception ex)
        {
            Logger.Error($"Docking error: {ex.Message}");
            throw;
        }
    }

    public void EntryDocking() => EntryDockingAsync().GetAwaiter().GetResult();

    /// <summary>
    /// Entry: ACT -> Docked
    /// Đã docked thành công
    /// </summary>
    public void EntryDocked()
    {
        Logger.Info("==> Entry Docked State");
        PlcController.SetSystemState(SystemState.DOCKING);
    }

    /// <summary>
    /// Entry: ACT -> Charging
    /// Đang sạc pin
    /// </summary>
    public async Task EntryChargingAsync()
    {
        Logger.Info("==> Entry Charging State");
        PlcController.SetSystemState(SystemState.CHARGING);

        try
        {
            // TODO:
            // - Bắt đầu charging
            // - Monitor battery level
            // - Monitor charging current/voltage

            Logger.Info("Connecting charging power...");
            await Task.Delay(100);

            Logger.Info("Starting battery charging...");
            // Charging loop sẽ được xử lý bởi battery service
        }
        catch (Exception ex)
        {
            Logger.Error($"Charging error: {ex.Message}");
            throw;
        }
    }

    public void EntryCharging() => EntryChargingAsync().GetAwaiter().GetResult();

    /// <summary>
    /// Exit: ACT -> Charging
    /// </summary>
    public void ExitCharging()
    {
        Logger.Info("<== Exit Charging State");

        // TODO:
        // - Dừng charging
        // - Ngắt kết nối nguồn an toàn
        // - Log battery level
    }

    /// <summary>
    /// Entry: ACT -> Undocking
    /// Đang rời khỏi dock
    /// </summary>
    public async Task EntryUndockingAsync()
    {
        Logger.Info("==> Entry Undocking State");
        PlcController.SetSystemState(SystemState.DOCKING);

        try
        {
            // TODO:
            // - Kiểm tra an toàn
            // - Ngắt kết nối với dock
            // - Di chuyển ra khỏi dock

            Logger.Info("Checking safety...");
            await Task.Delay(100);

            Logger.Info("Disconnecting dock...");
            await Task.Delay(200);

            Logger.Info("Moving out of dock...");
            await Task.Delay(500);

            Logger.Info("Undocking succeeded");
        }
        catch (Exception ex)
        {
            Logger.Error($"Undocking error: {ex.Message}");
            throw;
        }
    }

    public void EntryUndocking() => EntryUndockingAsync().GetAwaiter().GetResult();

    /// <summary>
    /// Entry: ACT -> Loading
    /// Đang tải hàng
    /// </summary>
    public async Task EntryLoadingAsync()
    {
        Logger.Info("==> Entry Loading State");
        PlcController.SetSystemState(SystemState.PROCCESSING);

        try
        {
            // TODO:
            // - Kiểm tra payload capacity
            // - Điều khiển loading mechanism
            // - Xác nhận hàng đã được tải

            Logger.Info("Preparing loading...");
            await Task.Delay(200);

            Logger.Info("Loading cargo...");
            await Task.Delay(1000);

            Logger.Info("Loading completed");
        }
        catch (Exception ex)
        {
            Logger.Error($"Loading error: {ex.Message}");
            throw;
        }
    }

    public void EntryLoading() => EntryLoadingAsync().GetAwaiter().GetResult();

    /// <summary>
    /// Entry: ACT -> Unloading
    /// Đang dỡ hàng
    /// </summary>
    public async Task EntryUnloadingAsync()
    {
        Logger.Info("==> Entry Unloading State");
        PlcController.SetSystemState(SystemState.PROCCESSING);

        try
        {
            // TODO:
            // - Kiểm tra vị trí unload
            // - Điều khiển unloading mechanism
            // - Xác nhận hàng đã được dỡ

            Logger.Info("Preparing unloading...");
            await Task.Delay(200);

            Logger.Info("Unloading cargo...");
            await Task.Delay(1000);

            Logger.Info("Unloading completed");
        }
        catch (Exception ex)
        {
            Logger.Error($"Unloading error: {ex.Message}");
            throw;
        }
    }

    public void EntryUnloading() => EntryUnloadingAsync().GetAwaiter().GetResult();

    /// <summary>
    /// Entry: ACT -> TechAction
    /// Thực hiện các action kỹ thuật đặc biệt
    /// </summary>
    public async Task EntryTechActionAsync()
    {
        Logger.Info("==> Entry Tech Action State");
        PlcController.SetSystemState(SystemState.PROCCESSING);

        try
        {
            // TODO:
            // - Xác định loại tech action
            // - Thực hiện action tương ứng
            // - Log kết quả

            Logger.Info("Executing tech action...");
            await Task.Delay(500);

            Logger.Info("Tech action completed");
        }
        catch (Exception ex)
        {
            Logger.Error($"Tech action error: {ex.Message}");
            throw;
        }
    }

    public void EntryTechAction() => EntryTechActionAsync().GetAwaiter().GetResult();

    // ===========================
    // MODE TRANSITION HANDLERS
    // ===========================

    /// <summary>
    /// Entry: Manual Mode
    /// Chuyển sang chế độ thủ công
    /// </summary>
    public void EntryManual()
    {
        Logger.Info("==> Entry Manual Mode");
        PlcController.SetSystemState(SystemState.MANUAL);
        StopRobot();
    }

    /// <summary>
    /// Exit: Manual Mode
    /// </summary>
    public void ExitManual()
    {
        Logger.Info("<== Exit Manual Mode");

        // TODO:
        // - Kiểm tra an toàn
        // - Tắt manual control
        // - Bật lại autonomous control
    }

    /// <summary>
    /// Entry: Service Mode (MAINTENANCE)
    /// Chuyển sang chế độ bảo trì - cho phép điều khiển robot từ RF Handle
    /// </summary>
    public void EntryService()
    {
        Logger.Info("==> Entry Service Mode (MAINTENANCE)");
        PlcController.SetSystemState(SystemState.MAINTENANCE);
        StopRobot();

        // Set ManualControlService state to Maintenance - cho phép điều khiển từ RF Handle
        try
        {
            using var scope = ServiceScopeFactory.CreateScope();
            var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
            manualControlService.SetState(ManualControlState.Maintenance);
            Logger.Info("ManualControlService set to Maintenance state");
        }
        catch (Exception ex)
        {
            Logger.Error($"Error setting ManualControlService to Maintenance: {ex.Message}");
        }
    }

    /// <summary>
    /// Exit: Service Mode (MAINTENANCE)
    /// </summary>
    public void ExitService()
    {
        Logger.Info("<== Exit Service Mode (MAINTENANCE)");

        // Clear ManualControlService external state
        try
        {
            using var scope = ServiceScopeFactory.CreateScope();
            var manualControlService = scope.ServiceProvider.GetRequiredService<ManualControlService>();
            manualControlService.ClearExternalState();
            Logger.Info("ManualControlService external state cleared");
        }
        catch (Exception ex)
        {
            Logger.Error($"Error clearing ManualControlService external state: {ex.Message}");
        }
    }

    /// <summary>
    /// Entry: Stop State
    /// Robot bị dừng (Emergency, Bumper, etc.)
    /// </summary>
    public void EntryStop()
    {
        Logger.Warning("==> Entry STOP State");
        PlcController.SetSystemState(SystemState.PAUSED);
        EmergencyStop();
    }

    /// <summary>
    /// Exit: Stop State
    /// Giải phóng stop
    /// </summary>
    public void ExitStop()
    {
        Logger.Info("<== Exit Stop State");

        // Giống Lock (SERVICE) → Auto/Manual: M815 pulse + fault reset + EnableAsync (RobotController.ApplyResetFromPlc).
        ApplyAlarmResetAndEnableMotorsLikeLock();
    }

    /// <summary>
    /// Entry: Fault State
    /// Robot gặp lỗi nghiêm trọng
    /// </summary>
    public void EntryFault()
    {
        Logger.Error("==> Entry FAULT State");
        PlcController.SetSystemState(SystemState.ERROR);
        EmergencyStop();
    }

    /// <summary>
    /// Exit: Fault State
    /// Đã khắc phục lỗi
    /// </summary>
    public void ExitFault()
    {
        Logger.Info("<== Exit Fault State");

        // 1. Clear remaining fatal errors
        try
        {
            using var scope = ServiceScopeFactory.CreateScope();
            var errorManager = scope.ServiceProvider.GetRequiredService<IError>();
            errorManager.ClearFatalErrors();
        }
        catch (Exception ex) { Logger.Error($"ExitFault: Error clearing errors: {ex.Message}"); }

        // 2. PLC alarm reset + servo: cùng chuỗi như thoát Lock / ApplyResetFromPlc
        ApplyAlarmResetAndEnableMotorsLikeLock();
    }

    /// <summary>
    /// Pulse M815 (alarm reset trên PLC), sau đó FaultReset + chờ + EnableAsync — khớp RobotController.ApplyResetFromPlc (không gọi TryClearFault).
    /// </summary>
    private void ApplyAlarmResetAndEnableMotorsLikeLock()
    {
        Logger.Info("ApplyAlarmResetAndEnableMotorsLikeLock: M815 + fault reset + enable (như Lock → Auto/Manual)");
        try { PlcController.WriteAlarmResetM815(); }
        catch (Exception ex) { Logger.Warning($"WriteAlarmResetM815: {ex.Message}"); }

        try
        {
            using var scope = ServiceScopeFactory.CreateScope();
            var ik = scope.ServiceProvider.GetService<IInverseKinematics>();
            if (ik == null)
            {
                Logger.Warning("ApplyAlarmResetAndEnableMotorsLikeLock: IInverseKinematics không có");
                return;
            }

            ik.FaultReset();
            Thread.Sleep(1500);
            ik.FaultReset();
            Thread.Sleep(800);
            ik.EnableAsync(CancellationToken.None).GetAwaiter().GetResult();
            if (ik.IsOperationEnabled)
                Logger.Info("ApplyAlarmResetAndEnableMotorsLikeLock: động cơ OperationEnabled");
            else
                Logger.Warning("ApplyAlarmResetAndEnableMotorsLikeLock: chưa OperationEnabled sau EnableAsync");
        }
        catch (Exception ex)
        {
            Logger.Warning($"ApplyAlarmResetAndEnableMotorsLikeLock: FaultReset/Enable — {ex.Message}");
        }
    }

    // ===========================
    // HELPER METHODS
    // ===========================

    /// <summary>
    /// Dừng robot bình thường - gửi zero velocity đến IInverseKinematics
    /// </summary>
    private void StopRobot()
    {
        try
        {
            using var scope = ServiceScopeFactory.CreateScope();
            var inverseKinematics = scope.ServiceProvider.GetService<IInverseKinematics>();

            if (inverseKinematics == null)
            {
                Logger.Warning("IInverseKinematics not available, cannot stop robot");
                return;
            }

            // Send zero velocity to stop the robot
            // SetVelocity not available - commented out
            // var zeroTwist = new Twist();
            // inverseKinematics.SetVelocity(zeroTwist);

            Logger.Info("Robot stopped (zero velocity intended)");
        }
        catch (AggregateException ex)
        {
            Logger.Error($"Stop robot error: {ex.InnerException?.Message ?? ex.Message}");
        }
        catch (Exception ex)
        {
            Logger.Error($"Stop robot error: {ex.Message}");
        }
    }

    /// <summary>
    /// Dừng khẩn cấp robot - gửi zero velocity và disable IInverseKinematics
    /// </summary>
    private void EmergencyStop()
    {
        try
        {
            using var scope = ServiceScopeFactory.CreateScope();
            var inverseKinematics = scope.ServiceProvider.GetService<IInverseKinematics>();

            if (inverseKinematics == null)
            {
                Logger.Warning("IInverseKinematics not available, cannot emergency stop robot");
                return;
            }

            // Send zero velocity first
            // SetVelocity not available - commented out
            // var zeroTwist = new Twist();
            // inverseKinematics.SetVelocity(zeroTwist);

            // Disable the drive (if supported)
            try
            {
                inverseKinematics.Disable();
            }
            catch (Exception disableEx)
            {
                Logger.Warning($"Could not disable IInverseKinematics: {disableEx.Message}");
            }

            Logger.Warning("EMERGENCY STOP! Robot stopped and disabled");
        }
        catch (AggregateException ex)
        {
            Logger.Error($"Emergency stop error: {ex.InnerException?.Message ?? ex.Message}");
        }
        catch (Exception ex)
        {
            Logger.Error($"Emergency stop error: {ex.Message}");
        }
    }
}