using RobotNet.VDA5050.Type;
using RobotNet10.Common;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Events;
using RobotNet10.RobotApp.Events.Events;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Modules;
using RobotNet10.RobotApp.Motion;
using RobotNet10.RobotApp.Services.ConfigManager;
using RobotNet10.RobotApp.Services.Exceptions;
using RobotNet10.RobotApp.Services.Robot.Connection;
using RobotNet10.RobotApp.Services.State;

namespace RobotNet10.RobotApp.Services.Robot;

public partial class RobotController(IOrder OrderManager,
                             INavigation NavigationManager,
                             IAction ActionManager,
                             IPlcController PlcController,
                             IDeviceProvider DeviceProvider,
                             IConfiguration Configuration,
                             IError ErrorManager,
                             Logger<RobotController> Logger,
                             IRobotConnectionsService RobotConnectionsService,
                             IRobotEventBus RobotEventBus,
                             RobotStateMachine StateManager,
                             ManualControlService RFControl,
                             PS5ControllerService Ps5Controller,
                             IRobotConfiguration RobotConfiguration,
                             RobotStates StateService,
                             RobotVisualization VisualizationService,
                             ILiftModule LiftModule,
                             ILocalization Localization,
                             IRotationModule RotateModule,
                             IInverseKinematics? InverseKinematics = null) : BackgroundService, IRobotController
{
    private readonly Mutex NewOrderMutex = new();
    private readonly Mutex NewInstanceMutex = new();
    private readonly Lock _stateTransitionLock = new();
    private WatchThread<RobotController>? _watchTimer;
    private bool _rfHandleHasPriority = false;
    private IBattery? Battery;
    private OperatingMode _previousPlcMode = OperatingMode.SERVICE;
    private double _batteryLowThresholdPercent = 20.0;

    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
    {
        await Task.Yield();
        await StateManager.InitializeAsync();
        while (!stoppingToken.IsCancellationRequested)
        {
            if (StateManager.CurrentState == RobotStateType.Standby) break;
            await Task.Delay(1000, stoppingToken);
        }

        // Subscribe to PLC events
        PlcController.OnPeripheralModeChanged += OnPlcModeChanged;
        PlcController.OnStop += OnStop;
        PlcController.OnButtonPressed += OnButtonPressed;

        // Subscribe to RF Handle mode changes (via RobotController for PLC sync)
        RFControl.OnRfModeChanged += OnRfModeChanged;

        // Subscribe to fatal errors
        ErrorManager.OnNewFatalError += OnNewFatalError;

        // Start WatchThread at 5Hz (200ms)
        _watchTimer = new WatchThread<RobotController>(200, WatchThreadCallback, null);
        _watchTimer.Start();

        var deviceBattery = DeviceProvider.GetDeviceByType(Client.Shared.Devices.DeviceType.Battery);
        if(deviceBattery is IBattery battery) Battery = battery;
        _batteryLowThresholdPercent = ResolveBatteryLowThresholdPercent();

        // Initial mode switch based on current PLC mode
        _previousPlcMode = PlcController.PeripheralMode;
        SwitchModeChanged(PlcController.PeripheralMode);
        PlcController.SetRFMode(RFMode.None);
        while (!stoppingToken.IsCancellationRequested)
        {
            if(RFControl.IsRunning)
            {
                RFControl.Start();
                break;
            }
            await Task.Delay(2000);
        }
    }

    public override Task StopAsync(CancellationToken cancellationToken)
    {
        StopHandler();
        return base.StopAsync(cancellationToken);
    }

    public async Task ModuleInitializeAsync()
    {
        while (true)
        {
            if (StateManager.IsInitialized) break;
            await Task.Delay(500);
        }

        // Start MQTT independently so connection topics are available even if hardware init is slow.
        _ = RobotConnectionsService.StartAsync(CancellationToken.None);
        // Start VDA5050 publishers early so state/visualization topics keep updating.
        StateService.Start();
        VisualizationService.Start();

        while (true)
        {
            try
            {
                if (!RobotConfiguration.GetSimulationConfig().IsEnable)
                {
                    Logger.Info("Checking hardware...");

                    await PlcController.Start(CancellationToken.None);

                    while (!PlcController.IsReady || !DeviceProvider.AreDevicesConnected)
                    {
                        // (!DeviceProvider.AreDevicesConnected) Logger.Info(" - Devices service not ready");
                        //if (!PlcController.IsReady) Logger.Info(" - Peripheral service not ready");
                        if (PlcController.IsReady) PlcController.SetSystemState(SystemState.INIT);
                        // if (!LiftModule.IsReady) Logger.Info(" - LiftModule service not ready");
                        // if (!RotateModule.IsReady) Logger.Info(" - RotateModule service not ready");
                        await Task.Delay(3000);
                    }
                }
                Logger.Info("Hardware modules ready");

                // Start software modules independently
                NavigationManager.Start();

                StateManager.Fire(RobotEventType.InitializeCompleted);
                Logger.Info("Initialization completed");
                break;
            }
            catch (Exception ex)
            {
                Logger.Warning($"Robot initialize failed: {ex.Message}");
                await Task.Delay(2000);
            }
        }
    }

    public void StopHandler()
    {
        _watchTimer?.Dispose();
        _watchTimer = null;

        if (RobotConnectionsService.IsConnected)
        {
            var pubOffline = RobotConnectionsService.PublishConnectionStateAsync(ConnectionState.OFFLINE);
            pubOffline.Wait();
        }

        var stopConnection = RobotConnectionsService.StopAsync();
        stopConnection.Wait();
        RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
        RobotEventBus.InstantActionReceived -= NewInstantActionUpdated;
        NavigationManager.Stop();
        PlcController.Stop();
        PlcController.OnPeripheralModeChanged -= OnPlcModeChanged;
        PlcController.OnStop -= OnStop;
        PlcController.OnButtonPressed -= OnButtonPressed;
        RFControl.OnRfModeChanged -= OnRfModeChanged;
        ErrorManager.OnNewFatalError -= OnNewFatalError;
    }

    public void NewOrderUpdated(object? sender, OrderChangedEvent e)
    {
        if (NewOrderMutex.WaitOne(2000))
        {
            try
            {
                var orderMsg = e.OrderMessage;
                if (!StateManager.IsInState(RobotStateType.Auto)) throw new OrderException(RobotErrors.Error1006(StateManager.CurrentState.ToString()));
                if (!Localization.IsReady) throw new OrderException(RobotErrors.Error3001());
                OrderManager.UpdateOrder(orderMsg);
            }
            catch (RobotException orEx)
            {
                if (orEx.Error is not null)
                {
                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
                    Logger.Warning($"New order error: {orEx.Error.ErrorDescription}");
                }
                else Logger.Warning($"New order error: {orEx.Message}");
            }
            catch (Exception ex)
            {
                Logger.Warning($"Order processing error: {ex.Message}");
            }
            finally
            {
                NewOrderMutex.ReleaseMutex();
            }
        }
    }

    public void NewInstantActionUpdated(object? sender, InstantActionChangedEvent e)
    {
        if (NewInstanceMutex.WaitOne(2000))
        {
            try
            {
                var instantAction = e.InstantActionMessage;

                // VDA5050: Filter instant actions based on current robot state
                var filteredActions = FilterInstantActionsByState(instantAction.Actions);

                if (filteredActions.Length > 0)
                {
                    ActionManager.AddInstantAction(filteredActions);
                }
            }
            catch (RobotException acEx)
            {
                if (acEx.Error is not null)
                {
                    ErrorManager.AddError(acEx.Error, TimeSpan.FromSeconds(10));
                    Logger.Warning($"InstantAction error: {acEx.Error.ErrorDescription}");
                }
                else Logger.Warning($"InstantAction error: {acEx.Message}");
            }
            catch (Exception ex)
            {
                Logger.Warning($"InstantAction processing error: {ex.Message}");
            }
            finally
            {
                NewInstanceMutex.ReleaseMutex();
            }
        }
    }

    /// <summary>
    /// Filter instant actions based on current robot state for security and safety
    /// </summary>
    private RobotNet.VDA5050.InstantAction.Action[] FilterInstantActionsByState(RobotNet.VDA5050.InstantAction.Action[] actions)
    {
        var currentState = StateManager.CurrentState;
        var allowedActions = new List<RobotNet.VDA5050.InstantAction.Action>();

        foreach (var action in actions)
        {
            bool isAllowed = IsActionAllowedInState(action.ActionType, currentState);

            if (isAllowed)
            {
                allowedActions.Add(action);
            }
            else
            {
                // VDA5050: Report rejected instant action as error
                var error = new RobotError
                {
                    ErrorType = "instantActionRejected",
                    ErrorLevel = RobotNet.VDA5050.Type.ErrorLevel.WARNING,
                    ErrorDescription = $"Instant action '{action.ActionType}' rejected - not allowed in state '{currentState}'",
                    ErrorReferences = [
                        new() { ReferenceKey = "actionId", ReferenceValue = action.ActionId },
                        new() { ReferenceKey = "actionType", ReferenceValue = action.ActionType },
                        new() { ReferenceKey = "robotState", ReferenceValue = currentState.ToString() }
                    ]
                };
                ErrorManager.AddError(error, TimeSpan.FromSeconds(10));
                Logger.Warning($"Instant action {action.ActionId} (type: {action.ActionType}) rejected - not allowed in state {currentState}");
            }
        }

        return [.. allowedActions];
    }

    /// <summary>
    /// Define which instant actions are allowed in each robot state
    /// </summary>
    private static bool IsActionAllowedInState(string actionType, RobotStateType state)
    {
        // Actions allowed in ALL states (read-only or critical control)
        var alwaysAllowedActions = new HashSet<string>
        {
            "cancelOrder",       // VDA5050: Must work in all states
            "stateRequest",      // Read-only
            "factsheetRequest",  // Read-only
        };

        if (alwaysAllowedActions.Contains(actionType)) return true;

        // Actions allowed only in Auto state
        if (state == RobotStateType.Auto ||
            state == RobotStateType.Idle ||
            state == RobotStateType.Executing ||
            state == RobotStateType.Paused ||
            state == RobotStateType.Canceling)
        {
            return true;  // All actions allowed in Auto mode
        }

        // Shared set of maintenance/setup actions (used in Service, Manual, System, Standby)
        var maintenanceAllowedActions = new HashSet<string>
        {
            "initPosition",
            "pick",
            "drop",
            "rotate",
            "liftRotate",
            "homingCamera",
            "liftCameraByHeight",
            "controlLight",
            "cameraLightOn",
            "cameraLightOff",
            "mutedBaseOn",
            "mutedBaseOff",
            "mutedLoadOn",
            "mutedLoadOff",
            "dockTo",
            "moveStraightToCoor",
            "moveStraightWithDistance"
        };

        // Actions allowed in Service/Override/Manual states (maintenance/manual control)
        if (state == RobotStateType.Service ||
            state == RobotStateType.Remote_Override ||
            state == RobotStateType.Manual)
        {
            return maintenanceAllowedActions.Contains(actionType);
        }

        // After ReleaseStop robot goes to System/Standby - allow maintenance actions so operator can e.g. lift camera before switching mode
        if (state == RobotStateType.System || state == RobotStateType.Standby)
        {
            return maintenanceAllowedActions.Contains(actionType);
        }

        // Stop and Fault states: only critical control actions
        if (state == RobotStateType.Stop || state == RobotStateType.Fault)
        {
            // Already handled by alwaysAllowedActions above
            return false;
        }

        // Default: reject
        return false;
    }

    public void Pause()
    {
        OrderManager.PauseOrder();
        ActionManager.PauseActions();
    }

    public void Resume()
    {
        OrderManager.ResumeOrder();
        ActionManager.ResumeActions();
    }

    public bool TryClearFault()
    {
        lock (_stateTransitionLock)
        {
            if (!StateManager.IsInState(RobotStateType.Fault)) return false;

            if (PlcController.IsReady && !PlcController.IsDisconected)
                ErrorManager.DeleteErrorId(2003);

            ErrorManager.ClearFatalErrors();

            if (!ErrorManager.HasFatalError)
            {
                Logger.Info("TryClearFault: Exiting Fault");
                StateManager.Fire(RobotEventType.ExitFault);
                return true;
            }
            return false;
        }
    }

    private void OnPlcModeChanged(OperatingMode mode)
    {
        lock (_stateTransitionLock)
        {
            if (_rfHandleHasPriority)
            {
                Logger.Info($"PLC mode change to {mode} ignored - RF Handle has priority");
                return;
            }
            // Khi chuyển từ Lock (SERVICE) sang Auto hoặc Manual: ghi M815 xuống PLC, reset fault, enable động cơ
            if (_previousPlcMode == OperatingMode.SERVICE && (mode == OperatingMode.AUTOMATIC || mode == OperatingMode.MANUAL))
            {
                Logger.Info($"PLC Lock -> {mode}: áp dụng ApplyResetFromPlc (M815 + fault reset + enable)");
                ApplyResetFromPlc();
            }
            _previousPlcMode = mode;
            SwitchModeChanged(mode);
        }
    }

    /// <summary>Reset theo PLC (M815): ghi M815 xuống PLC (pulse), clear fault robot, reset fault động cơ, enable lại động cơ (retry đến khi OperationEnabled).</summary>
    private void ApplyResetFromPlc()
    {
        Logger.Info("ApplyResetFromPlc: bắt đầu (M815 pulse, clear fault, fault reset + enable drive)");
        try { PlcController.WriteAlarmResetM815(); } catch (Exception ex) { Logger.Warning($"WriteAlarmResetM815: {ex.Message}"); }
        TryClearFault();
        try
        {
            InverseKinematics?.FaultReset();
            // Đợi servo thoát Fault (CiA402 có thể cần >1s để cập nhật statusword)
            Thread.Sleep(1500);
            InverseKinematics?.FaultReset();
            Thread.Sleep(800);
            // Enable 2 động cơ giống enable bằng tay trên device: gửi lệnh trực tiếp, await từng bước
            if (InverseKinematics != null)
            {
                InverseKinematics.EnableAsync(CancellationToken.None).GetAwaiter().GetResult();
                if (InverseKinematics.IsOperationEnabled)
                    Logger.Info("ApplyResetFromPlc: 2 động cơ đã enable (OperationEnabled)");
                else
                    Logger.Warning("ApplyResetFromPlc: động cơ chưa lên OperationEnabled sau EnableAsync");
            }
        }
        catch (Exception ex) { Logger.Warning($"FaultReset/Enable drive: {ex.Message}"); }
    }

    private void SwitchModeChanged(OperatingMode mode)
    {
        // Pause order when leaving Auto mode
        if (StateManager.IsInState(RobotStateType.Auto) && mode != OperatingMode.AUTOMATIC)
        {
            Pause();
            RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
            // Keep InstantActionReceived subscription - instant actions (e.g. cancelOrder) must work in all states
        }

        switch (mode)
        {
            case OperatingMode.AUTOMATIC:
                Ps5Controller.Disable();
                StateManager.Fire(RobotEventType.EnterAuto);
                // Prevent duplicate subscriptions
                RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
                RobotEventBus.OrderMessageReceived += NewOrderUpdated;
                RobotEventBus.InstantActionReceived -= NewInstantActionUpdated;
                RobotEventBus.InstantActionReceived += NewInstantActionUpdated;
                Resume();
                break;
            case OperatingMode.MANUAL:
                Ps5Controller.Enable();
                StateManager.Fire(RobotEventType.EnterManual);
                break;
            case OperatingMode.SERVICE:
                Ps5Controller.Disable();
                StateManager.Fire(RobotEventType.EnterService);
                break;
            default:
                break;
        }
    }

    private void OnStop(StopStateType state)
    {
        lock (_stateTransitionLock)
        {
            if (state != StopStateType.None)
            {
                _rfHandleHasPriority = false; // Safety overrides RF Handle
                if (!StateManager.IsInState(RobotStateType.Stop))
                {
                    Pause();
                    StateManager.Fire(RobotEventType.EnterStop);
                }
            }
            else
            {
                // No physical Start button: leave Stop as soon as PLC reports all safety inputs clear.
                TryReleaseStopAfterSafetyClear();
            }
        }
    }

    /// <summary>
    /// Exit Stop when EMC/bumper are released. Previously required a Start button; this robot has none.
    /// </summary>
    private void TryReleaseStopAfterSafetyClear()
    {
        if (!StateManager.IsInState(RobotStateType.Stop))
            return;
        if (PlcController.Emergency || PlcController.Bumper)
            return;

        Logger.Info("Robot Controller: Safety cleared; releasing Stop (auto, no Start button)");
        StateManager.Fire(RobotEventType.ReleaseStop);
    }

    private void OnButtonPressed(PeripheralButton button)
    {
        lock (_stateTransitionLock)
        {
            if (button == PeripheralButton.Reset)
            {
                // M815 Reset: clear robot fault + reset fault động cơ
                ApplyResetFromPlc();
            }
        }
    }

    private void OnNewFatalError()
    {
        lock (_stateTransitionLock)
        {
            if (!StateManager.IsInState(RobotStateType.Fault))
            {
                _rfHandleHasPriority = false; // Fault overrides RF Handle
                Pause();
                StateManager.Fire(RobotEventType.EnterFault);
            }
        }
    }

    private void OnRfModeChanged(RFMode rfMode)
    {
        lock (_stateTransitionLock)
        {
            // Ignore RF mode changes while in Stop or Fault — safety overrides everything
            // RF mode will be re-evaluated when returning to Standby via WatchThread
            if (StateManager.IsInState(RobotStateType.Stop) || StateManager.IsInState(RobotStateType.Fault))
            {
                Logger.Info($"RF mode change to {rfMode} ignored - robot in {StateManager.CurrentState}");
                return;
            }

            switch (rfMode)
            {
                case RFMode.Maintenance:
                    // RF Handle requests Service mode
                    _rfHandleHasPriority = true;
                    if (StateManager.IsInState(RobotStateType.Auto))
                    {
                        Pause();
                        RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
                        // Keep InstantActionReceived subscription - instant actions must work in Service mode
                    }
                    StateManager.Fire(RobotEventType.EnterService);
                    break;

                case RFMode.Override:
                    // RF Handle requests Remote Override
                    _rfHandleHasPriority = true;
                    if (StateManager.IsInState(RobotStateType.Auto))
                    {
                        Pause();
                        RobotEventBus.OrderMessageReceived -= NewOrderUpdated;
                        // Keep InstantActionReceived subscription - instant actions must work in Override mode
                    }
                    StateManager.Fire(RobotEventType.RemoteOverride);
                    break;

                case RFMode.Default:
                case RFMode.None:
                    // RF Handle released control or disconnected - return to PLC-determined mode
                    _rfHandleHasPriority = false;
                    PlcController.SetRFEStop(false);
                    if (StateManager.IsInState(RobotStateType.Service) || StateManager.IsInState(RobotStateType.Remote_Override))
                    {
                        SwitchModeChanged(PlcController.PeripheralMode);
                    }
                    break;
            }
        }
    }

    private void WatchThreadCallback()
    {
        lock (_stateTransitionLock)
        {
            // 1. Fatal error detection
            if (ErrorManager.HasFatalError && !StateManager.IsInState(RobotStateType.Fault))
            {
                Logger.Warning("Robot Controller: Fatal error detected, transitioning to Fault state");
                Pause();
                StateManager.Fire(RobotEventType.EnterFault);
                return;
            }

            // 2. In Stop: release automatically when PLC shows safety clear (backup if OnStop edge was missed)
            if (StateManager.IsInState(RobotStateType.Stop))
            {
                TryReleaseStopAfterSafetyClear();
                return;
            }

            // 2b. Fault auto-recovery
            if (StateManager.IsInState(RobotStateType.Fault))
            {
                if (PlcController.IsReady && !PlcController.IsDisconected)
                    ErrorManager.DeleteErrorId(2003);

                if (!ErrorManager.HasFatalError)
                {
                    Logger.Info("Robot Controller: Fatal errors resolved, auto-recovering from Fault");
                    StateManager.Fire(RobotEventType.ExitFault);
                }
                return;
            }

            var plcMode = PlcController.PeripheralMode;

            // 3. If in Standby, trigger mode switch (e.g., after ReleaseStop or initialization)
            if (StateManager.CurrentState == RobotStateType.Standby)
            {
                // Check if RF Handle has an active mode that should take priority
                // (RF mode preserved on PLC during Stop/Fault, re-evaluated here after release)
                var rfMode = PlcController.CurrentRFMode;
                if (rfMode == RFMode.Maintenance)
                {
                    Logger.Info("Robot Controller: Standby → RF Handle Maintenance detected, entering Service");
                    _rfHandleHasPriority = true;
                    StateManager.Fire(RobotEventType.EnterService);
                }
                else if (rfMode == RFMode.Override)
                {
                    Logger.Info("Robot Controller: Standby → RF Handle Override detected, entering Remote_Override");
                    _rfHandleHasPriority = true;
                    StateManager.Fire(RobotEventType.RemoteOverride);
                }
                else
                {
                    SwitchModeChanged(plcMode);
                }
                return;
            }

            // 4. PLC mode mismatch check — ONLY when RF Handle does NOT have priority
            if (!_rfHandleHasPriority)
            {
                var currentModeMatch = plcMode switch
                {
                    OperatingMode.AUTOMATIC => StateManager.IsInState(RobotStateType.Auto),
                    OperatingMode.MANUAL => StateManager.IsInState(RobotStateType.Manual),
                    OperatingMode.SERVICE => StateManager.IsInState(RobotStateType.Service),
                    _ => true
                };

                if (!currentModeMatch)
                {
                    Logger.Warning($"Robot Controller: PLC mode mismatch. PLC: {plcMode}, State: {StateManager.CurrentState}");
                    SwitchModeChanged(plcMode);
                }
            }

            // 5. Backup stop detection (ALWAYS runs, even when RF Handle has priority)
            bool hasSafetyStop = PlcController.Emergency || PlcController.Bumper;

            if (hasSafetyStop && !StateManager.IsInState(RobotStateType.Stop))
            {
                Logger.Warning("Robot Controller: Safety stop detected from PLC properties");
                _rfHandleHasPriority = false; // Safety overrides RF Handle
                Pause();
                StateManager.Fire(RobotEventType.EnterStop);
            }

            // 6. Check has load
            if (LiftModule.IsReady && LiftModule.Position == LiftPosition.Top) PlcController.SetHasLoad(true);
            else PlcController.SetHasLoad(false);

            // 7. Check Pin: set M929 when battery percentage is below configured threshold.
            if(Battery != null
               && Battery.CurrentBatteryState.HasValue
               && !double.IsNaN(Battery.CurrentBatteryState.Value.Percentage)
               && Battery.CurrentBatteryState.Value.Percentage < _batteryLowThresholdPercent)
            {
                PlcController.SetBatteryLow(true);
            }
            else PlcController.SetBatteryLow(false);

            // 8. Check PLC connection
            if (PlcController.IsDisconected)
            {
                ErrorManager.AddError(RobotErrors.Error2003());
            }
            else
            {
                ErrorManager.DeleteErrorId(2003);
            }
        }
    }

    private double ResolveBatteryLowThresholdPercent()
    {
        const double defaultThreshold = 20.0;
        try
        {
            if (Battery is not DeviceBase batteryDevice)
            {
                return defaultThreshold;
            }

            var devicesSection = Configuration.GetSection("Devices");
            foreach (var section in devicesSection.GetChildren())
            {
                var deviceId = section.GetValue<string>("DeviceId");
                if (!string.Equals(deviceId, batteryDevice.DeviceId, StringComparison.OrdinalIgnoreCase))
                {
                    continue;
                }

                var threshold = section.GetSection("Connection").GetValue<double?>("LowBatteryThresholdPercent");
                if (!threshold.HasValue)
                {
                    return defaultThreshold;
                }

                var clamped = Math.Clamp(threshold.Value, 0.0, 100.0);
                if (Math.Abs(clamped - threshold.Value) > double.Epsilon)
                {
                    Logger.Warning($"Battery low threshold {threshold.Value} out of range [0..100], clamped to {clamped}");
                }
                Logger.Info($"Battery low threshold loaded from config: {clamped}%");
                return clamped;
            }

            return defaultThreshold;
        }
        catch (Exception ex)
        {
            Logger.Warning($"Failed to resolve battery low threshold from config, fallback {defaultThreshold}%: {ex.Message}");
            return defaultThreshold;
        }
    }
}