BQP/srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/OpenACS/OpenACSPublisher.cs

using RobotNet.VDA5050.State;
using RobotNet.VDA5050.Type;
using RobotNet10.Common;
using RobotNet10.FleetManager.Services.ConfigManager;
using RobotNet10.FleetManager.Services.RobotManager;

namespace RobotNet10.FleetManager.Services.OpenACS;

public class OpenACSPublisher(IConfiguration configuration,
    Logger<OpenACSPublisher> Logger,
    ILogger<OpenACSPublisher> ILogger,
    IRobotManagerService RobotManager,
    IACSTrafficConfig ACSTrafficConfig) : BackgroundService
{
    public int PublishCount { get; private set; }
    private WatchTimerAsync<OpenACSPublisher>? Timer;
    private readonly string ACSSiteCode = configuration["ACSStatusConfig:SiteCode"] ?? "VN03";
    private readonly string ACSAreaCode = configuration["ACSStatusConfig:AreaCode"] ?? "DA3_FL1";
    private readonly string ACSAreaName = configuration["ACSStatusConfig:AreaName"] ?? "DA3_WM";
    private readonly double ACSExtendX = configuration.GetValue<double>("ACSStatusConfig:ExtendX");
    private readonly double ACSExtendY = configuration.GetValue<double>("ACSStatusConfig:ExtendY");
    private readonly double ACSExtendTheta = configuration.GetValue<double>("ACSStatusConfig:ExtendTheta");

    private readonly SemaphoreSlim _timerLock = new(1, 1);
    private async Task TimerHandler()
    {
        if (ACSTrafficConfig.PublishEnable && !string.IsNullOrEmpty(ACSTrafficConfig.PublishURL))
        {
            try
            {
                using var HttpClient = new HttpClient() { Timeout = TimeSpan.FromSeconds(15) };
                var robotControllers = RobotManager.GetAllRobotControllers();
                foreach (var robot in robotControllers)
                {
                    var startTime = DateTime.Now;
                    if (robot.Value.Data.State == null || robot.Value.Data.State.AgvPosition == null) continue;
                    if ((startTime - robot.Value.Data.State.Timestamp).TotalMilliseconds > ACSTrafficConfig.PublishInterval) continue;

                    int batLevel = (int)robot.Value.Data.State.BatteryState.BatteryHealth;
                    if (batLevel <= 0) batLevel = 85;

                    int batVol = (int)(robot.Value.Data.State.BatteryState.BatteryVoltage ?? 0);
                    if (batVol <= 0) batVol = 24;
                    var status = new ACSPublishModel()
                    {
                        Header = new("AGV_STATUS", robot.Value.Data.State.Timestamp.ToString("yyyy-MM-dd HH:mm:ss.fff")),
                        Body = new()
                        {
                            Id = robot.Key,
                            Location = new()
                            {
                                X = (robot.Value.Data.State.AgvPosition.X + ACSExtendX).ToString(),
                                Y = (robot.Value.Data.State.AgvPosition.Y + ACSExtendY).ToString(),
                                Z = "0",
                                Direction = (robot.Value.Data.State.AgvPosition.Theta + ACSExtendTheta).ToString(),
                            },
                            SiteCode = ACSSiteCode,
                            AreaCode = ACSAreaCode,
                            AreaName = ACSAreaName,
                            MarkerId = string.IsNullOrEmpty(robot.Value.Data.State.LastNodeId) ? null : robot.Value.Data.State.LastNodeId,
                            BatteryId = null,
                            BatteryLevel = batLevel.ToString(),
                            BatteryVoltage = batVol.ToString(),
                            BatterySOH = null,
                            BatteryCurrent = "1.0",
                            BatteryTemprature = "30",
                            StationId = null,
                            Loading = robot.Value.Data.State.Loads.Length != 0 ? "1" : "0",
                            ErrorCode = GetErrorCode(robot.Value.Data.State.Errors ?? [])?.ToString() ?? null,
                            State = GetStatus(robot.Value.Data.State).ToString(),
                        }
                    };

                    var response = await HttpClient.PostAsJsonAsync(ACSTrafficConfig.PublishURL, status);
                    if (response.IsSuccessStatusCode)
                    {
                        var result = await response.Content.ReadFromJsonAsync<ACSStatusResponse>();
                        if (result == null)
                        {
                            Logger.Error("Failed to convert response.Content to ACSStatusResponse");
                        }
                        else if (result.Header?.MessageName == "AGV_STATUS_ACK" && result.Body?.Result == "OK")
                        {
                            PublishCount++;
                        }
                        else
                        {
                            Logger.Warning($"ACS response is not OK: {System.Text.Json.JsonSerializer.Serialize(result)}");
                        }
                    }
                    else
                    {
                        Logger.Warning($"ACS publish to {ACSTrafficConfig.PublishURL} failed: {response.StatusCode}");
                    }
                }
            }
            catch (Exception ex)
            {
                Logger.Warning($"ACS publish to {ACSTrafficConfig.PublishURL} error: {ex.Message}");
            }
        }
    }

    private static int GetStatus(StateMsg state)
    {
        if (GetError(state) == ErrorLevel.FATAL || GetError(state) == ErrorLevel.WARNING) return (int)AGVState.Error;
        else if (state.BatteryState.Charging) return (int)AGVState.Charging;
        else if (state.Paused) return (int)AGVState.Pause;
        else if (IsIdle(state)) return (int)AGVState.Idle;
        else if (IsWorking(state)) return (int)AGVState.Run;
        else return (int)AGVState.Stop;
    }

    private static string? GetErrorCode(Error[] errors)
    {
        var error = errors.FirstOrDefault();
        if (error is not null && int.TryParse(error.ErrorType, out int errorCode)) return errorCode.ToString();
        return null;
    }

    private static bool IsIdle(StateMsg state)
    {
        if (state.NodeStates.Length != 0 || state.EdgeStates.Length != 0) return false;
        return true;
    }

    private static bool IsWorking(StateMsg state)
    {
        if (state.NodeStates.Length != 0 || state.EdgeStates.Length != 0) return true;
        return false;
    }

    private static ErrorLevel GetError(StateMsg state)
    {
        if (state.Errors is not null)
        {
            if (state.Errors.Any(error => error.ErrorLevel == ErrorLevel.FATAL)) return ErrorLevel.FATAL;
            if (state.Errors.Any(error => error.ErrorLevel == ErrorLevel.WARNING)) return ErrorLevel.WARNING;
        }
        return ErrorLevel.NONE;
    }

    private async Task InitializeTimerAsync()
    {
        if (ACSTrafficConfig.PublishInterval == Timer?.Interval) return;
        if (_timerLock.Wait(1000))
        {
            try
            {
                Timer?.Dispose();
                Timer = new(ACSTrafficConfig.PublishInterval, TimerHandler, ILogger);
                Timer.Start();
            }
            finally
            {
                _timerLock.Release();
            }
        }
    }

    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
    {
        await Task.Yield();
        while (!stoppingToken.IsCancellationRequested)
        {
            try
            {
                ACSTrafficConfig.ConfigChanged += ConfigChanged;
                await InitializeTimerAsync();
                break;
            }
            catch (Exception ex)
            {
                Logger.Warning($"ACS Publisher: Initialization error: {ex.Message}");
                await Task.Delay(2000, stoppingToken);
            }
        }
    }

    public async void ConfigChanged(object? sender, EventArgs e)
    {
        await InitializeTimerAsync();
    }

    public override Task StopAsync(CancellationToken cancellationToken)
    {
        ACSTrafficConfig.ConfigChanged -= ConfigChanged;
        Timer?.Dispose();
        _timerLock?.Dispose();
        return Task.CompletedTask;
    }
}