using Appccelerate.StateMachine;
using Appccelerate.StateMachine.Machine;
using Microsoft.AspNetCore.SignalR;
using RobotNet10.RobotApp.Shared.Enums;
using RobotNet10.RobotApp.SLAM.Cartographer.Enums;

namespace RobotNet10.RobotApp.SLAM.Cartographer;

public partial class CartographerService
{
    #region State Machine Definition

    private PassiveStateMachine<SLAMState, CartographerTrigger> BuildStateMachine()
    {
        var builder = new StateMachineDefinitionBuilder<SLAMState, CartographerTrigger>();

        // Idle state
        builder.In(SLAMState.Idle)
            .ExecuteOnEntry(OnIdleStateEntry)
            .On(CartographerTrigger.Start)
            .Goto(SLAMState.Initializing);

        // Initializing state
        builder.In(SLAMState.Initializing)
            .ExecuteOnEntry(OnInitializingStateEntry)
            .On(CartographerTrigger.InitializationComplete)
            .Goto(SLAMState.Ready)
            .On(CartographerTrigger.InitializationFailed)
            .Goto(SLAMState.Error);

        // Ready state
        builder.In(SLAMState.Ready)
            .ExecuteOnEntry(OnReadyStateEntry)
            .On(CartographerTrigger.StartLocalization)
            .Goto(SLAMState.Relocalizing)
            .On(CartographerTrigger.StartScanMapping)
            .Goto(SLAMState.ScanMapping)
            .On(CartographerTrigger.Shutdown)
            .Goto(SLAMState.Idle);

        // InitializingLocalizing state - with background task for async localization setup
        // Background task will load map, start MCL (if enabled), or add trajectory (if MCL disabled), then fire MclConverged/MclSkipped trigger
        builder.In(SLAMState.Relocalizing)
            .ExecuteOnEntry(OnInitializingLocalizingStateEntry)
            .ExecuteOnExit(OnInitializingLocalizingStateExit)
            .On(CartographerTrigger.MclConverged)
            .Goto(SLAMState.Localizing)
            .On(CartographerTrigger.MclSkipped)
            .Goto(SLAMState.Localizing)
            .On(CartographerTrigger.StopLocalization)
            .Goto(SLAMState.Ready)
            .On(CartographerTrigger.ErrorOccurred)
            .Goto(SLAMState.Error);

        // Localizing state
        builder.In(SLAMState.Localizing)
            .ExecuteOnEntry(OnLocalizingStateEntry)
            .ExecuteOnExit(OnLocalizingStateExit)
            .On(CartographerTrigger.MclConverged)
            .Goto(SLAMState.Localizing)  // Self-transition: handle MCL convergence after it converges
            .On(CartographerTrigger.StartLocalization)
            .Goto(SLAMState.Relocalizing)  // Allow transition back to InitializingLocalizing for SetInitialPose
            .On(CartographerTrigger.StopLocalization)
            .Goto(SLAMState.Ready)
            .On(CartographerTrigger.ErrorOccurred)
            .Goto(SLAMState.Error);

        // ScanMapping state
        builder.In(SLAMState.ScanMapping)
            .ExecuteOnEntry(OnScanMappingStateEntry)
            .ExecuteOnExit(OnScanMappingStateExit)
            .On(CartographerTrigger.SaveMap)
            .Goto(SLAMState.SavingMap)
            .On(CartographerTrigger.ErrorOccurred)
            .Goto(SLAMState.Error);

        // SavingMap state - with background task for async save workflow
        builder.In(SLAMState.SavingMap)
            .ExecuteOnEntry(OnSavingMapStateEntry)
            .On(CartographerTrigger.MapSaved)
            .Goto(SLAMState.Ready)
            .On(CartographerTrigger.ErrorOccurred)
            .Goto(SLAMState.Error);

        // Error state
        builder.In(SLAMState.Error)
            .ExecuteOnEntry(OnErrorStateEntry)
            .On(CartographerTrigger.Reset)
            .Goto(SLAMState.Idle);

        return builder
            .WithInitialState(SLAMState.Idle)
            .Build()
            .CreatePassiveStateMachine();
    }

    #endregion

    #region State Machine Helpers

    /// <summary>
    /// Thread-safe wrapper for state machine Fire() (Fix 7).
    /// PassiveStateMachine is not thread-safe; all Fire/Start/Stop calls must be synchronized.
    /// </summary>
    private void FireStateMachine(CartographerTrigger trigger)
    {
        lock (_stateMachineLock)
        {
            _stateMachine.Fire(trigger);
        }
    }

    private async Task NotifyStateChangedAsync(SLAMState state)
    {
        try
        {
            await _hubContext.Clients.All.SendAsync("OnStateChanged", state);
            _logger.LogDebug("CartographerService: Notified state change to {State}", state);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerService: Failed to notify state change to {State}", state);
        }
    }

    #endregion
}