using RobotApp.Common.Shares.Enums;
using RobotApp.Interfaces;

namespace RobotApp.Services.Robot;

public partial class RobotController
{
    private readonly AutoResetEvent StartButtonPressedEvent = new(false);
    private readonly AutoResetEvent StopButtonPressedEvent = new(false);
    private readonly AutoResetEvent ResetButtonPressedEvent = new(false);

    private async Task InitializationingHandler(CancellationToken cancellationToken)
    {
        try
        {
            StateManager.InitializeHierarchyStates();
            PeripheralManager.SetSytemState(SystemState.INIT);
            PeripheralManager.OnPeripheralModeChanged += SwichModeChanged;
            PeripheralManager.OnButtonPressed += OnButtonPressed;
            PeripheralManager.OnStop += OnStop;

            while (!cancellationToken.IsCancellationRequested)
            {
                if (StateManager.CurrentStateName == Enum.GetName(SystemStateType.Initializing))
                {
                    if (PeripheralManager.IsReady &&
                        PeripheralManager.LiftMotorReady &&
                        PeripheralManager.LeftMotorReady &&
                        PeripheralManager.RightMotorReady &&
                        BatteryManager.IsReady && 
                        Localization.IsReady && 
                        NavigationManager.IsReady &&
                        RobotConfiguration.IsReady
                        ) break;
                }
                await Task.Delay(1000, cancellationToken);
            }
            Logger.Info("Robot đã khởi tạo xong. Đang kết nối tới Fleet Manager.");
            
            ConnectionManager.OrderUpdated += NewOrderUpdated;
            ConnectionManager.ActionUpdated += NewInstantActionUpdated;
            await ConnectionManager.StartConnection(cancellationToken);
            Logger.Info("Robot đã kết nối tới Fleet Manager.");
            StateManager.TransitionTo(SystemStateType.Standby);

            if (!RobotConfiguration.IsSimulation)
            {
                StartButtonPressedEvent.Reset();
                Logger.Info("Chờ nút Start được nhấn để vào chế độ hoạt động...");
                if (StartButtonPressedEvent.WaitOne())
                {
                    StateManager.TransitionTo(RootStateType.Auto);
                    PeripheralManager.SetSytemState(SystemState.IDLE);
                }
            }
            else StateManager.TransitionTo(RootStateType.Auto);

            ErrorManager.AddError(RobotErrors.Error2001());
        }
        catch (Exception ex)
        {
            Logger.Error($"Khởi tạo RobotController xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
        }
    }

    private void StopHandler()
    {
        var stopConnection = ConnectionManager.StopConnection();
        stopConnection.Wait();
        PeripheralManager.OnPeripheralModeChanged -= SwichModeChanged;
        PeripheralManager.OnButtonPressed -= OnButtonPressed;
        PeripheralManager.OnStop -= OnStop;
    }

    private void SwichModeChanged(PeripheralMode mode)
    {
        switch (mode)
        {
            case PeripheralMode.AUTOMATIC:
                if (StartButtonPressedEvent.WaitOne())
                {
                    StateManager.TransitionTo(RootStateType.Auto);
                    PeripheralManager.SetSytemState(SystemState.IDLE);
                }
                break;
            case PeripheralMode.MANUAL:
                StateManager.TransitionTo(RootStateType.Manual);
                PeripheralManager.SetSytemState(SystemState.NONE);
                break;
            case PeripheralMode.SERVICE:
                StateManager.TransitionTo(RootStateType.Service);
                PeripheralManager.SetSytemState(SystemState.NONE);
                break;
            default:
                break;
        }
    }

    private void OnButtonPressed(PeripheralButton buttonPressed)
    {
        switch(buttonPressed)
        {
            case PeripheralButton.Start:
                StartButtonPressedEvent.Set();
                break;
            case PeripheralButton.Reset:
                ResetButtonPressedEvent.Set();
                break;
            case PeripheralButton.Stop:
                StopButtonPressedEvent.Set();
                break;
            default:
                break;
        }
    }

    private void OnStop(StopStateType state)
    {
        StateManager.TransitionToStop(state);
        PeripheralManager.SetSytemState(SystemState.PAUSED);
    }
}