using RobotNet.VDA5050.Type;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Services.ConfigManager;

namespace RobotNet10.RobotApp.Services.Robot;

public partial class RobotPlcController(IRobotConfiguration RobotConfiguration, IDeviceProvider DeviceProvider, Logger<RobotPlcController> Logger) : IPlcController
{
    public bool IsReady { get; private set; } = false;
    public bool IsDisconected => !IsSimulation && (ModbusTcpDevice is null || !ModbusTcpDevice.IsConnected);
    public event Action<SafetySpeed>? OnSafetySpeedChanged;
    public event Action<OperatingMode>? OnPeripheralModeChanged;
    public event Action<PeripheralButton>? OnButtonPressed;
    public event Action<StopStateType>? OnStop;

    private bool IsSimulation => RobotConfiguration.GetSimulationConfig().IsEnable;

    private IModbusTcpDevice? ModbusTcpDevice;

    // Edge detection tracking fields
    private StopStateType _lastStopState = StopStateType.None;
    private bool _lastButtonStart, _lastButtonReset, _lastButtonStop;

    public async Task Start(CancellationToken cancellationToken)
    {
        LidarBackProtectField = true;
        LidarFrontProtectField = true;
        if (IsSimulation)
        {
            PeripheralMode = OperatingMode.AUTOMATIC;
        }
        else if (ModbusTcpDevice is null)
        {
            while (!cancellationToken.IsCancellationRequested)
            {
                if (DeviceProvider.AreDevicesLoaded) break;
                await Task.Delay(500);
            }

            var device = DeviceProvider.GetDevice("plc-001");
            if (device is IModbusTcpDevice modbusDevice)
            {
                ModbusTcpDevice = modbusDevice;
                ModbusTcpDevice.DataRegisterChanged += ModbusDataChanged;
                while (!cancellationToken.IsCancellationRequested)
                {
                    if (modbusDevice.IsConnected) break;
                    await Task.Delay(500);
                }
            }
            else return;
        }
        IsReady = true;
    }

    public void Stop()
    {

        ModbusTcpDevice?.DataRegisterChanged -= ModbusDataChanged;
        ModbusTcpDevice = null;

        // Reset edge detection state
        _lastStopState = StopStateType.None;
        _lastButtonStart = false;
        _lastButtonReset = false;
        _lastButtonStop = false;

        IsReady = false;
    }

    private void ModbusDataChanged(ModbusRegisterType type)
    {
        if (type == ModbusRegisterType.Coil)
        {
            // Read-only data from PLC
            ReadSafetyProtect();
            ReadSafetySpeed();
            ReadButton();
            ReadSwitch();
            ReadLiftState();
            ReadMotorState();
            ReadOtherState();
        }
    }

    public void SetHorizontalLoad(bool value)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
        var write = ModbusTcpDevice.WriteCoilAsync(SetHorizontalLoadAddress, value, CancellationToken.None);
        write.Wait();
    }

    public void SetMutedBase(bool muted)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
        var write = ModbusTcpDevice.WriteCoilAsync(SetMutedBaseAddress, muted, CancellationToken.None);
        write.Wait();
    }

    public void SetMutedLoad(bool muted)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
        var write = ModbusTcpDevice.WriteCoilAsync(SetMutedLoadAddress, muted, CancellationToken.None);
        write.Wait();
    }

    /// <summary>Bật/tắt đèn — ghi coil M918 (TCP address 2966).</summary>
    public void SetLightOn(bool value)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
        var write = ModbusTcpDevice.WriteCoilAsync(SetLightOnAddress, value, CancellationToken.None);
        write.Wait();
    }

    public void SetOperationState(OperationState state)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
        var write = Task.Run(async () =>
        {
            switch (state)
            {
                case OperationState.Move:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteMoveState);
                    break;
                case OperationState.Lifting:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteLiftingState);
                    break;
                case OperationState.LiftRotating:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteLiftRotatingState);
                    break;
                case OperationState.None:
                default:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteClearState);
                    break;
            }
        });
        write.Wait();
    }

    public void SetSystemState(SystemState state)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
        var write = Task.Run(async () =>
        {
            switch (state)
            {
                case SystemState.INIT:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotInitState);
                    break;
                case SystemState.PAUSED:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotPauseState);
                    break;
                case SystemState.IDLE:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotIdleState);
                    break;
                case SystemState.PROCCESSING:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotProccessingState);
                    break;
                case SystemState.DOCKING:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotDockingState);
                    break;
                case SystemState.MAINTENANCE:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotMaintenanceState);
                    break;
                case SystemState.MANUAL:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotManualState);
                    break;
                case SystemState.OVERRIDE:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotOverrideState);
                    break;
                case SystemState.CHARGING:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotCharingState);
                    break;
                case SystemState.ERROR:
                    await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotErrorState);
                    break;
                default:
                    break;
            }
        });
        write.Wait();
    }

    public void SetEnableCharger(bool value)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
        var write = ModbusTcpDevice.WriteCoilAsync(EnableChargerAddress, value, CancellationToken.None);
        write.Wait();
    }

    public void SetRFMode(RFMode mode)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
        var write = Task.Run(async () =>
        {
            switch (mode)
            {
                case RFMode.Default:
                    await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeDefault);
                    break;
                case RFMode.Maintenance:
                    await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeMaintenance);
                    break;
                case RFMode.Override:
                    await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeOverride);
                    break;
                case RFMode.None:
                default:
                    await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeNone);
                    break;
            }
        });
        write.Wait();
    }

    public void SetHasLoad(bool hasLoad)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
        var write = ModbusTcpDevice.WriteCoilAsync(SetHasLoadAddress, hasLoad, CancellationToken.None);
        write.Wait();
    }

    public void SetRFEStop(bool stop)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
        var write = ModbusTcpDevice.WriteCoilAsync(SetRFEStopAddress, stop, CancellationToken.None);
        write.Wait();
    }

    public void SetBatteryLow(bool value)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
        var write = ModbusTcpDevice.WriteCoilAsync(SetBatteryLowAddress, value, CancellationToken.None);
        write.Wait();
    }

    /// <summary>Ghi hướng di chuyển xuống PLC: tiến M931, lùi M932; không đi thì cả hai off.</summary>
    public void SetDirectionForwardBackward(bool forward, bool backward)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) return;
        // Tiến: M931 on, M932 off. Lùi: M931 off, M932 on. Không đi: cả hai off (không bao giờ cả hai on)
        var w1 = ModbusTcpDevice.WriteCoilAsync(DirectionForwardAddress, forward, CancellationToken.None);
        var w2 = ModbusTcpDevice.WriteCoilAsync(DirectionBackwardAddress, backward, CancellationToken.None);
        Task.WaitAll(w1, w2);
    }

    /// <summary>Ghi M815 Alarm Reset xuống PLC (pulse) — gửi ngay ON rồi OFF, cùng coil như khi bấm M815 trên device.</summary>
    public void WriteAlarmResetM815()
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) return;
        ushort addr = AlarmResetM815WriteAddress;
        ModbusTcpDevice.WriteCoilImmediateAsync(addr, true, CancellationToken.None).GetAwaiter().GetResult();
        Thread.Sleep(150);
        ModbusTcpDevice.WriteCoilImmediateAsync(addr, false, CancellationToken.None).GetAwaiter().GetResult();
        Logger.Info($"WriteAlarmResetM815: pulsed coil {addr} (M815) ON -> OFF");
    }

    /// <summary>Lift: Homing — pulse coil M933.</summary>
    public void LiftHoming()
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected.");
        ushort addr = LiftHomingAddress;
        ModbusTcpDevice.WriteCoilImmediateAsync(addr, true, CancellationToken.None).GetAwaiter().GetResult();
        Thread.Sleep(150);
        ModbusTcpDevice.WriteCoilImmediateAsync(addr, false, CancellationToken.None).GetAwaiter().GetResult();
        Logger.Info($"LiftHoming: pulsed coil {addr} (M933) ON -> OFF");
    }

    /// <summary>Lift: Điều khiển velocity — lên (M934), xuống (M935); cả hai off = dừng.</summary>
    public void SetLiftVelocity(bool up, bool down)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) return;
        var w1 = ModbusTcpDevice.WriteCoilAsync(LiftVelocityUpAddress, up, CancellationToken.None);
        var w2 = ModbusTcpDevice.WriteCoilAsync(LiftVelocityDownAddress, down, CancellationToken.None);
        Task.WaitAll(w1, w2);
    }

    /// <summary>Lift: Ghi vị trí đích (10000 = 0.01m) vào 2 holding registers rồi pulse M936.</summary>
    public void SetLiftPositionAndGo(int position)
    {
        if (IsSimulation) return;
        if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected.");
        var high = (ushort)((position >> 16) & 0xFFFF);
        var low = (ushort)(position & 0xFFFF);
        ModbusTcpDevice.WriteHoldingRegistersAsync(LiftTargetPositionRegister, [high, low], CancellationToken.None).GetAwaiter().GetResult();
        ModbusTcpDevice.WriteCoilImmediateAsync(LiftGoToPositionAddress, true, CancellationToken.None).GetAwaiter().GetResult();
        Thread.Sleep(150);
        ModbusTcpDevice.WriteCoilImmediateAsync(LiftGoToPositionAddress, false, CancellationToken.None).GetAwaiter().GetResult();
        Logger.Info($"SetLiftPositionAndGo: position={position} (10000=0.01m), pulsed M936");
    }

    private static SafetySpeed ReadSpeed(bool[] flag)
    {
        if (flag.Length < 7) return SafetySpeed.Very_Slow;
        if (flag[0]) return SafetySpeed.Very_Slow;   // giới hạn chặt nhất
        if (flag[1]) return SafetySpeed.Slow;
        if (flag[2]) return SafetySpeed.Normal;
        if (flag[3]) return SafetySpeed.Medium;
        if (flag[4]) return SafetySpeed.Optimal;
        if (flag[5]) return SafetySpeed.Fast;
        if (flag[6]) return SafetySpeed.Very_Fast;

        return SafetySpeed.Very_Fast; // không có giới hạn nào
    }

    private void ReadSafetySpeed()
    {
        var device = ModbusTcpDevice;
        if (device is null) return;

        bool[] speed = device.ReadCoils(SpeedLimitReadAddress, SpeedRange);
        if (speed.Length == SpeedRange)
        {
            var activeSpeed = ReadSpeed(speed);
            if (activeSpeed != SafetySpeed)
            {
                SafetySpeed = activeSpeed;
                OnSafetySpeedChanged?.Invoke(activeSpeed);
            } 
        }
        else Logger.Warning($"Read Safety Speed is failed: data length {speed.Length} is wrong.");
    }

    private void ReadButton()
    {
        var device = ModbusTcpDevice;
        if (device is null) return;

        bool[] buttons = device.ReadCoils((ushort)(ReadOnlyStartAddress + StartButtonOffsetAddress), 3);
        if (buttons.Length == 3)
        {
            var newStart = buttons[0];
            var newReset = buttons[1];
            var newStop = buttons[2];

            // Rising edge detection - only fire when button state changes from false to true
            if (newStart && !_lastButtonStart) OnButtonPressed?.Invoke(PeripheralButton.Start);
            if (newReset && !_lastButtonReset) OnButtonPressed?.Invoke(PeripheralButton.Reset);
            if (newStop && !_lastButtonStop) OnButtonPressed?.Invoke(PeripheralButton.Stop);

            // Update tracking state
            _lastButtonStart = newStart;
            _lastButtonReset = newReset;
            _lastButtonStop = newStop;

            // Update public properties
            ButtonStart = newStart;
            ButtonReset = newReset;
            ButtonStop = newStop;
        }
        else Logger.Warning($"Read button is failed: data length {buttons.Length} is wrong.");
    }

    private void ReadSwitch()
    {
        var device = ModbusTcpDevice;
        if (device is null) return;

        bool[] switchs = device.ReadCoils((ushort)(ReadOnlyStartAddress + SwitchLockOffsetAddress), 3);
        if (switchs.Length == 3)
        {
            var oldMode = PeripheralMode;
            if (switchs[0])
            {
                PeripheralMode = OperatingMode.SERVICE;
            }
            else if (switchs[1])
            {
                PeripheralMode = OperatingMode.AUTOMATIC;
            }
            else if (switchs[2])
            {
                PeripheralMode = OperatingMode.MANUAL;
            }
            if (oldMode != PeripheralMode) OnPeripheralModeChanged?.Invoke(PeripheralMode);
        }
        else Logger.Warning($"Read switch mode is failed: data length {switchs.Length} is wrong.");
    }

    private void ReadSafetyProtect()
    {
        var device = ModbusTcpDevice;
        if (device is null) return;

        bool[] sensors = device.ReadCoils((ushort)(ReadOnlyStartAddress + EmergencyOffsetAddress), 5);
        if (sensors.Length == 5)
        {
            Emergency = sensors[0];
            Bumper = sensors[1];
            LidarFrontProtectField = sensors[2];
            LidarBackProtectField = sensors[3];
            LidarFrontTimProtectField = sensors[4];

            // Determine current stop state
            StopStateType currentState;
            if (Emergency) currentState = StopStateType.EMC;
            else if (Bumper) currentState = StopStateType.Bumper;
            else currentState = StopStateType.None;

            // Only fire event when state actually changes
            if (currentState != _lastStopState)
            {
                _lastStopState = currentState;
                OnStop?.Invoke(currentState);
            }
        }
        else Logger.Warning($"Read safety protect is failed: data length {sensors.Length} is wrong.");
    }

    /// <summary>
    /// Update lift state from Modbus cache
    /// </summary>
    private void ReadLiftState()
    {
        var device = ModbusTcpDevice;
        if (device is null) return;

        LiftedUp = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftedUpOffsetAddress));
        LiftedDown = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftedDownOffsetAddress));
        LiftHome = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftHomeOffsetAddress));
    }

    /// <summary>
    /// Update motor ready state from Modbus cache
    /// </summary>
    private void ReadMotorState()
    {
        var device = ModbusTcpDevice;
        if (device is null) return;

        LeftMotorReady = device.ReadCoil((ushort)(ReadOnlyStartAddress + LeftMotorReadyOffsetAddress));
        RightMotorReady = device.ReadCoil((ushort)(ReadOnlyStartAddress + RightMotorReadyOffsetAddress));
        LiftMotorReady = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftMotorReadyOffsetAddress));
    }

    /// <summary>
    /// Update other state from Modbus cache
    /// </summary>
    private void ReadOtherState()
    {
        var device = ModbusTcpDevice;
        if (device is null) return;

        HasLoad = device.ReadCoil((ushort)(ReadOnlyStartAddress + HasLoadOffsetAddress));
        EnabledCharger = device.ReadCoil((ushort)(ReadOnlyStartAddress + EnabledChargerOffsetAddress));
        Charging = device.ReadCoil((ushort)(ReadOnlyStartAddress + ResponseChargingOffsetAddress));
        MutedBase = device.ReadCoil((ushort)(ReadOnlyStartAddress + MutedBaseOffsetAddress));
        MutedLoad = device.ReadCoil((ushort)(ReadOnlyStartAddress + MutedLoadOffsetAddress));
    }
}