using Appccelerate.StateMachine;
using Appccelerate.StateMachine.Machine;
using Microsoft.Extensions.Configuration;
using Microsoft.Extensions.Hosting;
using Microsoft.Extensions.Logging;
using RobotNet10.RobotApp.Motion;
using RobotNet10.Shared.Geometry;
// using RobotNet10.Shared.Numbers;
using SDL2;
using System;
using System.Threading;
using System.Threading.Tasks;

namespace RobotNet10.RobotApp.Motion;

/// <summary>
/// States cho PS5ControllerService state machine
/// </summary>
public enum PS5ControllerState
{
    Disabled,
    Active
}

/// <summary>
/// Triggers cho PS5ControllerService state machine
/// </summary>
public enum PS5ControllerTrigger
{
    Enable,
    Disable,
    SafetyStop
}

/// <summary>
/// Service điều khiển robot từ tay cầm PS5
/// - R1, R2, L1, L2: Chọn mức vận tốc (0.3, 0.6, 0.9, 1.2 m/s)
/// - Left stick (núm trái): Điều khiển lên/xuống (linear velocity)
/// - Right stick (núm phải): Điều khiển trái/phải (angular velocity)
/// </summary>
public class PS5ControllerService : IHostedService, IDisposable
{
    private readonly PassiveStateMachine<PS5ControllerState, PS5ControllerTrigger> _stateMachine;
    private readonly PS5ControllerConfiguration _config;
    private readonly IInverseKinematics? _inverseKinematics;
    private readonly ILogger<PS5ControllerService> _logger;
    private readonly object _lock = new();

    private IntPtr _gamepad = IntPtr.Zero;
    private int _gamepadIndex = -1;
    private CancellationTokenSource? _updateCts;
    private Task? _updateTask;
    private PS5ControllerState _currentState = PS5ControllerState.Disabled;
    private bool _disposed = false;
    private bool _sdlInitialized = false;
    private bool _wasGamepadConnected = false;
    // Current velocity being sent to robot
    private Twist _currentTwist = new();

    // Current selected velocity level (determined by trigger buttons)
    private double _currentMaxVelocity = 0.0;

    /// <summary>
    /// Gets the current state of the PS5ControllerService
    /// </summary>
    public PS5ControllerState State => _currentState;

    /// <summary>
    /// Gets the current twist being sent to robot
    /// </summary>
    public Twist CurrentTwist
    {
        get
        {
            lock (_lock)
            {
                return _currentTwist;
            }
        }
    }

    /// <summary>
    /// Gets the current selected velocity level (m/s)
    /// </summary>
    public double CurrentMaxVelocity
    {
        get
        {
            lock (_lock)
            {
                return _currentMaxVelocity;
            }
        }
    }

    public PS5ControllerService(
        IConfiguration configuration,
        IInverseKinematics inverseKinematics,
        ILogger<PS5ControllerService> logger)
    {
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));

        // Try to get IInverseKinematics (optional - may not be available)
        _inverseKinematics = inverseKinematics;

        // Load configuration
        var configSection = configuration.GetSection("Motion:PS5Controller");
        if (!configSection.Exists())
        {
            throw new InvalidOperationException("Configuration section 'Motion:PS5Controller' not found in appsettings.json");
        }

        _config = new PS5ControllerConfiguration();
        configSection.Bind(_config);

        // Validate configuration
        ValidateConfiguration();

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

    private void ValidateConfiguration()
    {
        if (_config.R1Velocity <= 0)
            throw new InvalidOperationException("R1Velocity must be > 0");

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

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

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

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

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

    private PassiveStateMachine<PS5ControllerState, PS5ControllerTrigger> BuildStateMachine()
    {
        var builder = new StateMachineDefinitionBuilder<PS5ControllerState, PS5ControllerTrigger>();

        // Disabled state
        builder.In(PS5ControllerState.Disabled)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = PS5ControllerState.Disabled;
                }
                StopUpdateLoop();
                StopRobot();
                _logger.LogInformation("PS5ControllerService state: Disabled");
            })
            .On(PS5ControllerTrigger.Enable)
            .Goto(PS5ControllerState.Active);

        // Active state
        builder.In(PS5ControllerState.Active)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = PS5ControllerState.Active;
                }
                _logger.LogInformation("PS5ControllerService state: Active");
                StartUpdateLoop();
            })
            .On(PS5ControllerTrigger.Disable)
                .Goto(PS5ControllerState.Disabled)
            .On(PS5ControllerTrigger.SafetyStop)
                .Goto(PS5ControllerState.Disabled)
                .Execute(() =>
                {
                    _logger.LogWarning("PS5ControllerService: Safety stop triggered");
                });

        return builder
            .WithInitialState(PS5ControllerState.Disabled)
            .Build()
            .CreatePassiveStateMachine();
    }

    private void StartUpdateLoop()
    {
        StopUpdateLoop(); // Stop existing loop if any

        if (_gamepad == IntPtr.Zero || !IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
        {
            if (!TryReconnectGamepad(false))
            {
                _logger.LogWarning("PS5ControllerService: No gamepad connected, cannot start update loop");
                return;
            }
        }

        lock (_lock)
        {
            _updateCts = new CancellationTokenSource();
            var token = _updateCts.Token;
            _updateTask = Task.Run(async () => await UpdateLoopAsync(token), token);
        }
    }

    private void StopUpdateLoop()
    {
        Task? taskToWait = null;
        CancellationTokenSource? ctsToCancel = null;

        lock (_lock)
        {
            ctsToCancel = _updateCts;
            taskToWait = _updateTask;
            _updateCts = null;
            _updateTask = null;
        }

        if (ctsToCancel != null)
        {
            try
            {
                ctsToCancel.Cancel();
            }
            catch (ObjectDisposedException)
            {
                // CTS already disposed, ignore
            }
        }

        if (taskToWait != null)
        {
            try
            {
                taskToWait.Wait(TimeSpan.FromSeconds(1));
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error stopping update loop");
            }
        }

        ctsToCancel?.Dispose();
    }

    private async Task UpdateLoopAsync(CancellationToken cancellationToken)
    {
        var updateInterval = TimeSpan.FromMilliseconds(1000.0 / _config.UpdateRate);

        while (!cancellationToken.IsCancellationRequested)
        {
            try
            {
                // Check if gamepad is still connected
                if (_gamepad == IntPtr.Zero || !IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
                {
                    StopRobot();

                    if (!TryReconnectGamepad(false))
                    {
                        await Task.Delay(updateInterval, cancellationToken);
                        continue;
                    }
                }

                // Update velocity from gamepad
                await UpdateVelocityFromGamepadAsync(cancellationToken);

                await Task.Delay(updateInterval, cancellationToken);
            }
            catch (OperationCanceledException)
            {
                break;
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error in update loop");
                await Task.Delay(updateInterval, cancellationToken);
            }
        }
    }

    private async Task UpdateVelocityFromGamepadAsync(CancellationToken cancellationToken)
    {
        if (_gamepad == IntPtr.Zero)
            return;

        // Update SDL events (required for gamepad state)
        SDL.SDL_PumpEvents();

        // Get button states
        byte r1Pressed = SDL.SDL_GameControllerGetButton(_gamepad, SDL.SDL_GameControllerButton.SDL_CONTROLLER_BUTTON_RIGHTSHOULDER);
        byte r2Pressed = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_TRIGGERRIGHT) > 0 ? (byte)1 : (byte)0;
        byte l1Pressed = SDL.SDL_GameControllerGetButton(_gamepad, SDL.SDL_GameControllerButton.SDL_CONTROLLER_BUTTON_LEFTSHOULDER);
        byte l2Pressed = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_TRIGGERLEFT) > 0 ? (byte)1 : (byte)0;

        // Determine velocity level based on trigger buttons (priority: L2 > L1 > R2 > R1)
        double maxVelocity = 0.0;
        //Velocity
        double linearVelocity = 0.0;
        double angularVelocity = 0.0;
        if (l2Pressed != 0)
            maxVelocity = _config.L2Velocity;
        else if (l1Pressed != 0)
            maxVelocity = _config.L1Velocity;
        else if (r2Pressed != 0)
            maxVelocity = _config.R2Velocity;
        else if (r1Pressed != 0)
            maxVelocity = _config.R1Velocity;
        else if( l2Pressed == 0 && l1Pressed == 0 && r2Pressed == 0 && r1Pressed == 0)
            StopRobot();
        // Console.WriteLine($"l2: {l2Pressed}, l1: {l1Pressed}, r2: {r2Pressed}, r1: {r1Pressed}, maxVelocity: {maxVelocity}, linearVelocity: {linearVelocity}");
        lock (_lock)
        {
            _currentMaxVelocity = maxVelocity;
        }

        // Get stick values (left stick for linear, right stick for angular)
        short leftStickY = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_LEFTY);
        short rightStickX = SDL.SDL_GameControllerGetAxis(_gamepad, SDL.SDL_GameControllerAxis.SDL_CONTROLLER_AXIS_RIGHTX);

        // Normalize stick values from [-32768, 32767] to [-1.0, 1.0]
        // Left stick Y: up is negative, down is positive (invert for forward/backward)
        // Right stick X: left is negative, right is positive
        double leftStickYNormalized = -leftStickY / 32768.0; // Invert so up = positive (forward)
        double rightStickXNormalized = -rightStickX / 32768.0;

        // Apply deadzone (5% deadzone)
        const double deadzone = 0.1;
        if (Math.Abs(leftStickYNormalized) < deadzone)
            leftStickYNormalized = 0.0;
        else
        {
            // Rescale after deadzone
            var sign = Math.Sign(leftStickYNormalized);
            leftStickYNormalized = sign * ((Math.Abs(leftStickYNormalized) - deadzone) / (1.0 - deadzone));
        }

        if (Math.Abs(rightStickXNormalized) < deadzone)
            rightStickXNormalized = 0.0;
        else
        {
            // Rescale after deadzone
            var sign = Math.Sign(rightStickXNormalized);
            rightStickXNormalized = sign * ((Math.Abs(rightStickXNormalized) - deadzone) / (1.0 - deadzone));
        }

        // Calculate velocities
        linearVelocity = leftStickYNormalized * maxVelocity;
        angularVelocity = rightStickXNormalized * _config.MaxAngularVelocity;
        // Console.WriteLine($"linearVelocity: {linearVelocity}, angularVelocity: {angularVelocity}");
        // Console.WriteLine($"leftStickYNormalized: {leftStickYNormalized}, maxVelocity: {maxVelocity}, linearVelocity: {linearVelocity}");
        // if(leftStickYNormalized == 0 || rightStickXNormalized == 0)
        // {
        //     linearVelocity = 0;
        //     angularVelocity = 0;
        // }
        // Build twist
        var twist = new Twist
        {
            Linear = new Vector3(linearVelocity, 0, 0),
            Angular = new Vector3(0, 0, angularVelocity)
        };

        // Update current twist
        lock (_lock)
        {
            _currentTwist = twist;
        }

        // Send to IInverseKinematics if available
        if (_inverseKinematics != null && maxVelocity > 0.0)
        {
            try
            {
                await _inverseKinematics.SetVelocityAsync(twist, cancellationToken);
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error sending velocity to IInverseKinematics");
            }
        }

        // Log occasionally for debugging
        if (DateTime.Now.Millisecond % 500 < 50) // Log roughly every 500ms
        {
            _logger.LogDebug("PS5Controller: MaxVel={MaxVel:F2}, Linear={Linear:F2}, Angular={Angular:F2}",
                maxVelocity, linearVelocity, angularVelocity);
        }
    }

    private void StopRobot()
    {
        var zeroTwist = new Twist();
        lock (_lock)
        {
            _currentTwist = zeroTwist;
            _currentMaxVelocity = 0.0;
        }

        // Send zero velocity to IInverseKinematics if available
        if (_inverseKinematics != null)
        {
            try
            {
                _ = Task.Run(async () => await _inverseKinematics.SetVelocityAsync(zeroTwist));
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error stopping robot");
            }
        }
    }

    private bool InitializeSDL()
    {
        if (_sdlInitialized)
            return true;

        try
        {
            if (SDL.SDL_Init(SDL.SDL_INIT_GAMECONTROLLER) < 0)
            {
                _logger.LogError("Failed to initialize SDL: {Error}", SDL.SDL_GetError());
                return false;
            }

            SDL.SDL_GameControllerEventState(SDL.SDL_ENABLE);
            _sdlInitialized = true;
            _logger.LogInformation("SDL initialized successfully for gamepad support");
            return true;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error initializing SDL");
            return false;
        }
    }

    private bool OpenGamepad()
    {
        if (_gamepad != IntPtr.Zero)
        {
            if (IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
                return true;

            SDL.SDL_GameControllerClose(_gamepad);
            _gamepad = IntPtr.Zero;
            _gamepadIndex = -1;
        }

        SDL.SDL_PumpEvents();

        int numJoysticks = SDL.SDL_NumJoysticks();
        _logger.LogDebug("Found {Count} joystick(s)", numJoysticks);

        if (numJoysticks <= 0)
            return false;

        // If GamepadIndex is configured, always try that exact index.
        if (_config.GamepadIndex.HasValue)
        {
            return TryOpenGamepadAtIndex(_config.GamepadIndex.Value);
        }

        // Otherwise auto-detect first available game controller.
        for (int i = 0; i < numJoysticks; i++)
        {
            if (TryOpenGamepadAtIndex(i))
                return true;
        }

        return false;
    }

    private bool TryOpenGamepadAtIndex(int index)
    {
        if (!IsTrue(SDL.SDL_IsGameController(index)))
            return false;

        _gamepad = SDL.SDL_GameControllerOpen(index);
        if (_gamepad == IntPtr.Zero)
        {
            _logger.LogWarning("Failed to open game controller {Index}: {Error}", index, SDL.SDL_GetError());
            return false;
        }

        _gamepadIndex = index;
        var name = SDL.SDL_GameControllerName(_gamepad);
        _logger.LogInformation("Opened game controller: {Name} (index {Index})", name, index);
        return true;
    }

    private bool TryReconnectGamepad(bool logWhenUnavailable)
    {
        var reconnected = OpenGamepad();

        if (reconnected)
        {
            if (!_wasGamepadConnected)
                _logger.LogInformation("PS5ControllerService: Gamepad connected/reconnected");

            _wasGamepadConnected = true;
            return true;
        }

        if (_wasGamepadConnected)
        {
            _logger.LogWarning("PS5ControllerService: Gamepad disconnected, waiting for reconnect");
        }
        else if (logWhenUnavailable)
        {
            _logger.LogWarning("PS5ControllerService: No gamepad connected");
        }

        _wasGamepadConnected = false;
        return false;
    }

    private static bool IsTrue(SDL.SDL_bool value) => value == SDL.SDL_bool.SDL_TRUE;

    #region IHostedService

    public async Task StartAsync(CancellationToken cancellationToken)
    {
        _logger.LogInformation("Starting PS5ControllerService...");

        try
        {
            if (!InitializeSDL())
            {
                _logger.LogWarning("Failed to initialize SDL. PS5ControllerService will not be available.");
                return;
            }

            if (!TryReconnectGamepad(true))
            {
                _logger.LogWarning("No gamepad found at startup. PS5ControllerService will auto-reconnect when gamepad is plugged in.");
            }

            _logger.LogInformation("PS5ControllerService initialized successfully");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error starting PS5ControllerService");
        }

        await Task.CompletedTask;
    }

    public async Task StopAsync(CancellationToken cancellationToken)
    {
        _logger.LogInformation("Stopping PS5ControllerService...");

        try
        {
            Disable();
            StopUpdateLoop();

            if (_gamepad != IntPtr.Zero)
            {
                SDL.SDL_GameControllerClose(_gamepad);
                _gamepad = IntPtr.Zero;
                _gamepadIndex = -1;
            }

            if (_sdlInitialized)
            {
                SDL.SDL_QuitSubSystem(SDL.SDL_INIT_GAMECONTROLLER);
                _sdlInitialized = false;
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error stopping PS5ControllerService");
        }

        await Task.CompletedTask;
    }

    #endregion

    #region Public Control Methods

    /// <summary>
    /// Enable PS5 controller control
    /// </summary>
    public void Enable()
    {
        // Try to open gamepad if not already open
        if (_gamepad == IntPtr.Zero)
        {
            if (!TryReconnectGamepad(true))
            {
                _logger.LogWarning("Cannot enable PS5 controller: No gamepad connected");
                return;
            }
        }

        // Check if gamepad is still connected
        if (!IsTrue(SDL.SDL_GameControllerGetAttached(_gamepad)))
        {
            _logger.LogWarning("Cannot enable PS5 controller: Gamepad not connected");
            return;
        }

        // Check if IInverseKinematics (DifferentialDrive) is ready
        if (_inverseKinematics is DifferentialDrive differentialDrive)
        {
            if (differentialDrive.State != DifferentialDriveState.OperationEnabled)
            {
                _logger.LogInformation("DifferentialDrive is not in OperationEnabled state (current: {State}). Enabling it...", differentialDrive.State);

                int maxAttempts = 10;
                int attemptDelay = 300; // ms

                for (int i = 0; i < maxAttempts && differentialDrive.State != DifferentialDriveState.OperationEnabled; i++)
                {
                    var currentState = differentialDrive.State;
                    differentialDrive.Enable();

                    System.Threading.Thread.Sleep(attemptDelay);

                    if (differentialDrive.State == currentState && i > 0)
                    {
                        _logger.LogWarning("DifferentialDrive state did not change after Enable() call. State: {State}", currentState);
                        break;
                    }
                }

                if (differentialDrive.State != DifferentialDriveState.OperationEnabled)
                {
                    _logger.LogWarning("Cannot enable PS5 controller: DifferentialDrive is not ready. Current state: {State}", differentialDrive.State);
                    return;
                }

                _logger.LogInformation("DifferentialDrive is now in OperationEnabled state");
            }
        }
        else if (_inverseKinematics == null)
        {
            _logger.LogWarning("Cannot enable PS5 controller: IInverseKinematics is not available");
            return;
        }

        _stateMachine.Fire(PS5ControllerTrigger.Enable);
    }

    /// <summary>
    /// Disable PS5 controller control
    /// </summary>
    public void Disable()
    {
        _stateMachine.Fire(PS5ControllerTrigger.Disable);
    }

    #endregion

    #region IDisposable

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

        _disposed = true;

        try
        {
            Disable();
            StopUpdateLoop();
            _stateMachine.Stop();

            if (_gamepad != IntPtr.Zero)
            {
                SDL.SDL_GameControllerClose(_gamepad);
                _gamepad = IntPtr.Zero;
            }

            if (_sdlInitialized)
            {
                SDL.SDL_QuitSubSystem(SDL.SDL_INIT_GAMECONTROLLER);
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error disposing PS5ControllerService");
        }
    }

    #endregion
}