using RobotNet.VDA5050;
using RobotNet.VDA5050.State;
using RobotNet.VDA5050.Type;
using RobotNet10.Common;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Motion;
using RobotNet10.RobotApp.Services.ConfigManager;
using RobotNet10.RobotApp.Services.Navigation;
using RobotNet10.RobotApp.Services.Robot.Connection;
using RobotNet10.RobotApp.Services.State;

namespace RobotNet10.RobotApp.Services.Robot;

public class RobotStates(IConnectionConfig ConnectionConfig,
                         IRobotConnectionsService RobotConnectionsService,
                         RobotStateMachine StateManager,
                         ILogger<RobotStates> Logger,
                         IOrder OrderManager,
                         IAction ActionManager,
                         IPlcController PeripheralManager,
                         IInfomation InfoManager,
                         IError ErrorManager, 
                         ILocalization LocalizationManager,
                         IDeviceProvider DeviceProvider,
                         ILoad LoadManager,
                         INavigation NavigationManager,
                         IVelocityController VelocityController) : IState
{
    private uint HeaderId = 0;

    private WatchTimerAsync<RobotStates>? UpdateStateTimer;
    private const int UpdateStateInterval = 1000;

    public async Task PubState()
    {
        try
        {
            if (!RobotConnectionsService.IsConnected) return;
            await RobotConnectionsService.PublishStateAsync(GetStateMsg());
        }
        catch { }
    }

    private StateMsg GetStateMsg()
    {
        var vdaConfig = ConnectionConfig.GetVDA5050Config();
        var batteryDevice = DeviceProvider.GetDevice("battery-varta-001");
        RobotNet10.Shared.Sensor.BatteryState batteryState = new();
        if (batteryDevice is IBattery battery && battery.CurrentBatteryState.HasValue && battery.CurrentBatteryState is RobotNet10.Shared.Sensor.BatteryState state)
        {
            batteryState = state;
        }
        return new StateMsg
        {
            HeaderId = HeaderId++,
            Manufacturer = vdaConfig.Manufacturer,
            Version = vdaConfig.Version,
            SerialNumber = vdaConfig.SerialNumber,
            Maps = [],
            OrderId = OrderManager.OrderId,
            OrderUpdateId = OrderManager.OrderUpdateId,
            ZoneSetId = LocalizationManager.CurrentActiveMap,
            LastNodeId = OrderManager.LastNodeId,
            LastNodeSequenceId = OrderManager.LastNodeSequenceId,
            Driving = NavigationManager.Driving,
            Paused = OrderManager.IsPaused,
            NewBaseRequest = OrderManager.NewBaseRequest,
            DistanceSinceLastNode = OrderManager.DistanceSinceLastNode,
            OperatingMode = PeripheralManager.PeripheralMode.ToString(),
            NodeStates = OrderManager.NodeStates,
            EdgeStates = OrderManager.EdgeStates,
            ActionStates = ActionManager.ActionStates,
            Information = [General, .. InfoManager.InformationState],
            Errors = ErrorManager.ErrorsState,
            AgvPosition = new()
            {
                X = LocalizationManager.X,
                Y = LocalizationManager.Y,
                Theta = LocalizationManager.Theta,
                LocalizationScore = LocalizationManager.LocalizationScore,
                MapId = LocalizationManager.CurrentActiveMap,
                DeviationRange = LocalizationManager.DeviationRange,
                PositionInitialized = LocalizationManager.PositionInitialized,
            },
            BatteryState = new()
            {
                Charging = batteryState.Current > 0,
                BatteryHealth = 100,
                Reach = 0,
                BatteryVoltage = batteryState.Voltage is double.NaN ? 0 : batteryState.Voltage,
                BatteryCharge = batteryState.Percentage is double.NaN ? 0 : batteryState.Percentage,
            },
            Loads = LoadManager.Load,
            Velocity = new()
            {
                Vx = VelocityController.ActualVelocity.Linear,
                Vy = 0,
                Omega = VelocityController.ActualVelocity.Angular,
            },
            SafetyState = new()
            {
                FieldViolation = !PeripheralManager.LidarBackProtectField || !PeripheralManager.LidarFrontProtectField || PeripheralManager.LidarFrontTimProtectField,
                EStop = PeripheralManager.Emergency || PeripheralManager.Bumper ? EStop.AUTOACK : EStop.NONE,
            }
        };
    }

    private Information General => new()
    {
        InfoType = InformationType.GENERAL.ToJsonString(),
        InfoDescription = "Thông tin chung của robot",
        InfoLevel = InfoLevel.INFO,
        InfoReferences =
        [
            new InfomationReference
            {
                ReferenceKey = InformationReferencesKey.STATE.ToJsonString(),
                ReferenceValue = StateManager.CurrentState.ToString(),
            },
        ],
    };

    private async Task UpdateStateHandler()
    {
        await PubState();
    }

    public void Start()
    {
        if (UpdateStateTimer is not null) Stop();

        UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
        UpdateStateTimer.Start();
    }

    public void Stop()
    {
        UpdateStateTimer?.Dispose();
        UpdateStateTimer = null;
    }   
}