using Appccelerate.StateMachine;
using Appccelerate.StateMachine.Machine;
using RobotNet10.CANOpen.CiA402.Enums;
using RobotNet10.RobotApp.Devices;
using System.Threading;

namespace RobotNet10.RobotApp.Modules;

/// <summary>
/// Triggers cho LiftModuleService state machine
/// </summary>
public enum LiftModuleTrigger
{
    StartInitialize,
    InitializationCompleted,
    StartHoming,
    HomingCompleted,
    HomingFailed,
    StartMoving,
    StopMoving,
    MovingCompleted,
    MovingFailed,
    ErrorOccurred,
    Reset
}

/// <summary>
/// Configuration cho LiftModuleService
/// </summary>
internal class LiftModuleConfiguration
{
    public bool Enable { get; set; }
    public string DeviceId { get; set; } = "lift-motor";
    public byte HomingMethod { get; set; } = 35; // Default homing method
    public int HomingSpeed { get; set; } = 1000; // encoder counts/s
    public int HomingOffset { get; set; } = 0; // encoder counts
    public uint ProfileVelocity { get; set; } = 2000; // encoder counts/s
    public uint ProfileAcceleration { get; set; } = 5000; // encoder counts/s²
    public uint ProfileDeceleration { get; set; } = 5000; // encoder counts/s²
    public int MinPosition { get; set; } // Giới hạn dưới (null = không giới hạn)
    public int MaxPosition { get; set; } // Giới hạn trên (null = không giới hạn)
    /// <summary>Chiều cao min (m) tương ứng MinPosition. Dùng cho liftCameraByHeight.</summary>
    public double MinHeightM { get; set; } = 0;
    /// <summary>Chiều cao max (m) tương ứng MaxPosition. Dùng cho liftCameraByHeight.</summary>
    public double MaxHeightM { get; set; } = 1;
    public int StatusWordCheckIntervalMs { get; set; } = 50; // Interval để check status word khi moving
}

/// <summary>
/// Service điều khiển module nâng hạ
/// Sử dụng CiA402Servo để điều khiển động cơ nâng
/// </summary>
public class LiftModuleService : IHostedService, ILiftModule, IDisposable
{
    private readonly PassiveStateMachine<LiftModuleState, LiftModuleTrigger> _stateMachine;
    private readonly LiftModuleConfiguration _config;
    private readonly IDeviceProvider _deviceProvider;
    private readonly ILogger<LiftModuleService> _logger;
    private readonly Lock _lock = new();

    private readonly IRotationModule _rotationModule;
    private ICiA402Servo? _servo;
    private LiftModuleState _currentState = LiftModuleState.Uninitialized;
    private LiftPosition _currentPosition = LiftPosition.Bottom;
    private bool _isHomed = false;
    private bool _disposed = false;
    private int _targetPosition = 0;
    private CancellationTokenSource? _movementCts;

    public bool Enable => _config.Enable;

    // Nới điều kiện IsReady: chỉ cần module được bật và servo đã lấy được từ DeviceProvider
    public bool IsReady => _config.Enable && _servo != null;

    public LiftModuleState State => _currentState;
    public LiftPosition Position => _currentPosition;

    public LiftModuleService(
        IConfiguration configuration,
        IDeviceProvider deviceProvider,
        IRotationModule rotationModule,
        ILogger<LiftModuleService> logger)
    {
        _deviceProvider = deviceProvider;
        _rotationModule = rotationModule;
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));

        _stateMachine = BuildStateMachine();

        // Load configuration
        _config = new LiftModuleConfiguration();
        var configSection = configuration.GetSection("Modules:LiftModule");
        if (configSection.Exists())
        {
            configSection.Bind(_config);

            // Validate configuration
            ValidateConfiguration();

            // Build state machine
            _stateMachine.Start();
        }
    }

    private void ValidateConfiguration()
    {
        // Nếu Enable = false, không cần validate configuration
        if (!_config.Enable)
        {
            _logger.LogInformation("LiftModule is disabled (Enable = false). Skipping configuration validation.");
            return;
        }

        if (string.IsNullOrWhiteSpace(_config.DeviceId))
            throw new InvalidOperationException("DeviceId is required in LiftModule configuration");

        if (_config.HomingSpeed <= 0)
            throw new InvalidOperationException("HomingSpeed must be > 0");

        if (_config.ProfileVelocity <= 0)
            throw new InvalidOperationException("ProfileVelocity must be > 0");

        if (_config.ProfileAcceleration <= 0)
            throw new InvalidOperationException("ProfileAcceleration must be > 0");

        if (_config.ProfileDeceleration <= 0)
            throw new InvalidOperationException("ProfileDeceleration must be > 0");

        if (_config.MinPosition >= _config.MaxPosition)
            throw new InvalidOperationException("MinPosition must be less than MaxPosition");
    }

    private PassiveStateMachine<LiftModuleState, LiftModuleTrigger> BuildStateMachine()
    {
        var builder = new StateMachineDefinitionBuilder<LiftModuleState, LiftModuleTrigger>();

        // Uninitialized state
        builder.In(LiftModuleState.Uninitialized)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = LiftModuleState.Uninitialized;
                    _isHomed = false;
                }
            })
            .On(LiftModuleTrigger.StartInitialize)
            .Goto(LiftModuleState.Initializing);

        // Initializing state
        builder.In(LiftModuleState.Initializing)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = LiftModuleState.Ready;
                }
                _ = Task.Run(async () =>
                {
                    try
                    {
                        if (_servo == null)
                            throw new InvalidOperationException("Servo is not available");

                        // Set profile and homing parameters using async API
                        await _servo.SetProfileVelocityAsync(_config.ProfileVelocity);
                        await _servo.SetProfileAccelerationAsync(_config.ProfileAcceleration);
                        await _servo.SetProfileDecelerationAsync(_config.ProfileDeceleration);
                        await _servo.SetHomingMethodAsync(_config.HomingMethod);
                        await _servo.SetHomingSpeedAsync(_config.HomingSpeed);
                        await _servo.SetHomingOffsetAsync(_config.HomingOffset);
                        _stateMachine.Fire(LiftModuleTrigger.InitializationCompleted);
                    }
                    catch (Exception ex)
                    {
                        _logger.LogError(ex, "Error during initialization");
                        _stateMachine.Fire(LiftModuleTrigger.ErrorOccurred);
                    }
                });
            })
            .On(LiftModuleTrigger.InitializationCompleted)
            .Goto(LiftModuleState.Homing)
            .On(LiftModuleTrigger.ErrorOccurred)
            .Goto(LiftModuleState.Error);

        // Homing state
        builder.In(LiftModuleState.Homing)
            .ExecuteOnEntry(async () =>
            {
                lock (_lock)
                {
                    _currentState = LiftModuleState.Homing;
                }
                try
                {
                    await PerformHomingAsync();
                    UpdatePosition();
                    _stateMachine.Fire(LiftModuleTrigger.HomingCompleted);
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "Error during homing");
                    _stateMachine.Fire(LiftModuleTrigger.HomingFailed);
                }
            })
            .On(LiftModuleTrigger.HomingCompleted)
            .Goto(LiftModuleState.Ready)
            .On(LiftModuleTrigger.HomingFailed)
            .Goto(LiftModuleState.Error)
            .On(LiftModuleTrigger.ErrorOccurred)
            .Goto(LiftModuleState.Error);

        // Ready state
        builder.In(LiftModuleState.Ready)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = LiftModuleState.Ready;
                }
            })
            .On(LiftModuleTrigger.StartMoving)
            .Goto(LiftModuleState.Moving)
            .On(LiftModuleTrigger.ErrorOccurred)
            .Goto(LiftModuleState.Error);

        // Moving state
        builder.In(LiftModuleState.Moving)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = LiftModuleState.Moving;
                    _movementCts = new CancellationTokenSource();
                }
                var cts = _movementCts;
                _ = Task.Run(async () =>
                {
                    try
                    {
                        if (_servo is null)
                        {
                            _logger.LogError("LiftModule Moving: servo is null, cannot call motor.");
                            _stateMachine.Fire(LiftModuleTrigger.MovingFailed);
                            return;
                        }

                        _logger.LogInformation("LiftModule: Moving state entered, calling servo.MoveToPositionAsync(position={Position}).", _targetPosition);
                        // MoveToPositionAsync handles SetOperationMode, Enable and profile parameters
                        await _servo.MoveToPositionAsync(
                            _targetPosition,
                            _config.ProfileVelocity,
                            _config.ProfileAcceleration,
                            _config.ProfileDeceleration);

                        // Wait for movement to complete (cancellable when user presses Stop)
                        await WaitForMovementCompletedAsync(cts?.Token ?? CancellationToken.None);

                        UpdatePosition();
                        // Return to Ready state
                        _stateMachine.Fire(LiftModuleTrigger.MovingCompleted);
                    }
                    catch (OperationCanceledException)
                    {
                        // User pressed Stop; state already changed to Ready via StopMoving
                        UpdatePosition();
                    }
                    catch (Exception ex)
                    {
                        _logger.LogError(ex, "Error during movement to position {Position}", _targetPosition);
                        _stateMachine.Fire(LiftModuleTrigger.MovingFailed);
                        throw;
                    }
                });
            })
            .On(LiftModuleTrigger.StopMoving)
            .Goto(LiftModuleState.Ready)
            .On(LiftModuleTrigger.MovingCompleted)
            .Goto(LiftModuleState.Ready)
            .On(LiftModuleTrigger.MovingFailed)
            .Goto(LiftModuleState.Error)
            .On(LiftModuleTrigger.ErrorOccurred)
            .Goto(LiftModuleState.Error);

        // Error state
        builder.In(LiftModuleState.Error)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = LiftModuleState.Error;
                }
                _logger.LogError("LiftModule state: Error");
            })
            .On(LiftModuleTrigger.Reset)
            .Goto(LiftModuleState.Uninitialized);

        return builder
            .WithInitialState(LiftModuleState.Uninitialized)
            .Build()
            .CreatePassiveStateMachine();
    }

    #region IHostedService

    public async Task StartAsync(CancellationToken cancellationToken)
    {
        try
        {
            // Nếu Enable = false, không khởi tạo
            if (!_config.Enable)
            {
                _logger.LogInformation("LiftModule is disabled (Enable = false). Service will not be initialized.");
                return;
            }

            // Đợi DeviceProvider kết nối xong tất cả devices
            var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);

            if (!connected)
            {
                _logger.LogWarning("Timeout waiting for devices to connect. LiftModuleService will not be initialized.");
                return;
            }

            // Get CiA402Servo device
            var device = _deviceProvider.GetDevice(_config.DeviceId);
            if (device is not ICiA402Servo servo)
            {
                _logger.LogError("Device '{DeviceId}' is not an ICiA402Servo", _config.DeviceId);
                return;
            }

            _servo = servo;

            // Chỉ chờ rotation module khi rotation được bật (có hardware)
            if (_rotationModule.Enable)
            {
                while (!_rotationModule.IsReady && !cancellationToken.IsCancellationRequested)
                {
                    await Task.Delay(500, CancellationToken.None);
                }
            }

            // Start initialization (will trigger homing automatically)
            _stateMachine.Fire(LiftModuleTrigger.StartInitialize);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error starting LiftModuleService");
        }
    }

    public async Task StopAsync(CancellationToken cancellationToken)
    {
        try
        {
            // Disable servo if moving
            if (_servo != null && _currentState == LiftModuleState.Moving)
            {
                try
                {
                    await _servo.DisableAsync(cancellationToken);
                }
                catch (Exception ex)
                {
                    _logger.LogWarning(ex, "Error disabling servo during stop");
                }
            }

            _stateMachine.Stop();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "LiftModuleService: Error during StopAsync()");
            throw;
        }
    }

    #endregion

    #region Private Methods

    private void UpdatePosition()
    {
        if (_servo != null && Math.Abs(_servo.CachedPosition - _config.MinPosition) < 1000) _currentPosition = LiftPosition.Bottom;
        else if (_servo != null && Math.Abs(_servo.CachedPosition - _config.MaxPosition) < 1000) _currentPosition = LiftPosition.Top;
        else _currentPosition = LiftPosition.Middle;
    }

    private async Task PerformHomingAsync()
    {
        if (_servo == null)
            throw new InvalidOperationException("Servo is not available");

        // Check if servo is in fault state
        var isFault = await _servo.IsInFaultStateAsync();
        if (isFault)
        {
            _logger.LogWarning("Servo is in fault state, attempting fault reset...");
            await _servo.FaultResetAsync();
            Thread.Sleep(200);
        }

        // Enable servo
        await _servo.EnableAsync();
        Thread.Sleep(100);

        // StartHomingAsync chờ HomingAttained/HomingError/timeout rồi clear controlword và chuyển mode về Profile Position.
        // Sau khi chuyển mode, bit HomingAttained có thể bị xóa nên không poll lại statusword ở đây — tin kết quả từ StartHomingAsync (return = thành công, throw = lỗi/timeout).
        await _servo.StartHomingAsync(_config.HomingMethod, _config.HomingSpeed, CancellationToken.None);

        lock (_lock)
        {
            _isHomed = true;
        }
    }

    private async Task WaitForMovementCompletedAsync(CancellationToken cancellationToken = default)
    {
        if (_servo == null)
            throw new InvalidOperationException("Servo is not available");

        var maxWaitTime = TimeSpan.FromMinutes(5); // Long timeout for safety
        var startTime = DateTime.UtcNow;
        var checkInterval = TimeSpan.FromMilliseconds(_config.StatusWordCheckIntervalMs);

        while (DateTime.UtcNow - startTime < maxWaitTime)
        {
            cancellationToken.ThrowIfCancellationRequested();

            var statusword = await _servo.GetStatuswordAsync(cancellationToken);
            var state = statusword.GetState();

            // Check for fault
            if (state == DriveState.Fault)
            {
                throw new InvalidOperationException("Movement failed: Servo entered fault state");
            }

            // Check if target reached
            var currentPosition = await _servo.GetActualPositionAsync(cancellationToken);
            if (statusword.TargetReached && Math.Abs(currentPosition - _targetPosition) < 900)
            {
                return;
            }

            await Task.Delay(checkInterval, cancellationToken);
        }

        throw new TimeoutException($"Movement timeout after {maxWaitTime.TotalMinutes} minutes");
    }

    #endregion

    #region ILiftModule Implementation

    public async Task LiftUpAsync(CancellationToken cancellationToken = default)
    {
        if (!_config.Enable)
        {
            return;
        }

        if (_servo == null)
            throw new InvalidOperationException("Servo is not available");


        await LiftToPositionAsync(_config.MaxPosition, cancellationToken);
    }

    public async Task LiftDownAsync(CancellationToken cancellationToken = default)
    {
        if (!_config.Enable)
        {
            return;
        }

        if (_servo == null)
            throw new InvalidOperationException("Servo is not available");

        await LiftToPositionAsync(_config.MinPosition, cancellationToken);
    }

    public async Task LiftStopAsync(CancellationToken cancellationToken = default)
    {
        if (!_config.Enable)
        {
            return;
        }

        if (_servo == null)
            return;

        lock (_lock)
        {
            if (_currentState != LiftModuleState.Moving)
            {
                _logger.LogDebug("LiftStopAsync ignored: module not in Moving state. Current state: {State}", _currentState);
                return;
            }
        }

        try
        {
            _movementCts?.Cancel();
            await _servo.QuickStopAsync(cancellationToken);
            _stateMachine.Fire(LiftModuleTrigger.StopMoving);
            _logger.LogInformation("LiftModule: Stop requested, QuickStop sent, state -> Ready.");
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex, "LiftStopAsync: error during stop");
        }
    }

    public async Task LiftToPositionAsync(int position, CancellationToken cancellationToken = default)
    {
        if (!_config.Enable)
        {
            return;
        }

        if (_servo == null)
            throw new InvalidOperationException("Servo is not available");

        // Check limits
        if (position < _config.MinPosition)
        {
            throw new ArgumentOutOfRangeException(nameof(position),
                $"Position {position} is below MinPosition {_config.MinPosition}");
        }

        if (position > _config.MaxPosition)
        {
            throw new ArgumentOutOfRangeException(nameof(position),
                $"Position {position} is above MaxPosition {_config.MaxPosition}");
        }

        // Chỉ chấp nhận lệnh move khi đang ở Ready (state machine chỉ chuyển Ready -> Moving)
        lock (_lock)
        {
            if (_currentState != LiftModuleState.Ready)
            {
                _logger.LogWarning("LiftToPositionAsync ignored: module not in Ready state. Current state: {State}", _currentState);
                return;
            }
        }

        // Fire trigger to enter Moving state (must be done synchronously before async operations)
        try
        {
            _targetPosition = position;
            _stateMachine.Fire(LiftModuleTrigger.StartMoving);
            _logger.LogInformation("LiftModule: StartMoving fired, target position {Position} (encoder). Servo will be called in Moving state.", position);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Failed to fire StartMoving trigger");
            throw new InvalidOperationException("Failed to start movement", ex);
        }

    }

    public Task<int> GetCurrentPositionAsync(CancellationToken cancellationToken = default)
    {
        if (!_config.Enable)
        {
            return Task.FromResult(0);
        }

        if (_servo == null)
            throw new InvalidOperationException("Servo is not available");

        // Use async API; callers can await this Task
        return _servo.GetActualPositionAsync(cancellationToken);
    }

    public async Task<bool> IsHomedAsync(CancellationToken cancellationToken = default)
    {
        if (!_config.Enable)
        {
            return false;
        }

        lock (_lock)
        {
            return _isHomed;
        }
    }

    public async Task HomeAsync(CancellationToken cancellationToken = default)
    {
        if (!_config.Enable)
        {
            return;
        }

        if (_servo == null)
            throw new InvalidOperationException("Servo is not available");

        // Làm homing giống hệt UI Devices (CiA402ServoCard)
        // 1. Reset fault nếu có
        await _servo.TryFaultResetAsync(cancellationToken);

        // 2. Đảm bảo đã enable
        await _servo.EnableAsync(cancellationToken);

        // 3. Ghi lại homing params từ config
        await _servo.SetHomingMethodAsync(_config.HomingMethod, cancellationToken);
        await _servo.SetHomingSpeedAsync(_config.HomingSpeed, cancellationToken);
        await _servo.SetHomingOffsetAsync(_config.HomingOffset, cancellationToken);

        // 4. Start homing (non‑blocking đối với caller)
        _logger.LogInformation("LiftModule: calling servo.StartHomingAsync(method={Method}, speed={Speed}).", _config.HomingMethod, _config.HomingSpeed);
        await _servo.StartHomingAsync(_config.HomingMethod, _config.HomingSpeed, cancellationToken);

        // Không chờ kết thúc để tránh block UI; chỉ cập nhật trạng thái cục bộ sơ bộ
        UpdatePosition();
    }

    public int GetPositionFromHeightM(double heightM)
    {
        if (_config.MinHeightM >= _config.MaxHeightM)
            return _config.MinPosition;
        var t = (heightM - _config.MinHeightM) / (_config.MaxHeightM - _config.MinHeightM);
        t = Math.Clamp(t, 0, 1);
        return _config.MinPosition + (int)(t * (_config.MaxPosition - _config.MinPosition));
    }

    #endregion

    #region IDisposable

    public void Dispose()
    {
        if (_disposed)
            return;

        _disposed = true;

        try
        {
            _stateMachine.Stop();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error disposing LiftModuleService");
        }
        GC.SuppressFinalize(this);
    }

    #endregion
}