BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotDockToAction.cs

using Microsoft.EntityFrameworkCore;
using RobotNet.VDA5050.Factsheet;
using RobotNet.VDA5050.Type;
using RobotNet10.RobotApp.Data;
using RobotNet10.RobotApp.Detection;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Services.Exceptions;
using RobotNet10.RobotApp.Shared.Enums;
using RobotNet10.Shared.Detection;
using RobotNet10.Shared.Enum;
using RobotNet10.Shared.Numbers;
using System.Collections.ObjectModel;
using System.Text.Json;

namespace RobotNet10.RobotApp.Services.Robot.Actions;

[RobotAction(ActionType.DOCK_TO,
            [ActionScope.INSTANT, ActionScope.NODE],
            [BlockingType.HARD],
            "Robot di chuyển vào vị trí đặc biệt",
            "Robot đã dock đến vị trí sạc hoặc bến đỗ.")]
public class RobotDockToAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
    {
        new()
        {
            Key = "stationId",
            Description = "ID của vị trí dock.",
            ValueDataType = ValueDataType.STRING,
            IsOptional = false,
        },
        new()
        {
            Key = "direction",
            Description = "Hướng di chuyển: FORWARD, BACKWARD",
            ValueDataType = ValueDataType.STRING,
            IsOptional = true,
        },
    }.AsReadOnly();

    private IMarkerDetector? MarkerDetector;
    private INavigation? Navigation;
    private IDetectSession? DetectSession;
    private string? StationId;
    private RobotDirection? Direction = null;

    private const int MAX_TIMEOUT = 60000 * 5;
    private int CountTimeout = 0;
    private bool IsFindedGoal = false;
    private bool IsStartDockTo = false;
    private bool IsHasLoad = false;

    protected override async Task StartAction()
    {
        MarkerDetector = ServiceProvider.GetRequiredService<IMarkerDetector>();
        Navigation = ServiceProvider.GetRequiredService<INavigation>();

        if (!string.IsNullOrEmpty(StationId))
        {
            using var scope = ServiceProvider.CreateScope();
            var dbContext = scope.ServiceProvider.GetRequiredService<ApplicationDbContext>();
            var config = await dbContext.DockStationConfigs
                .Include(d => d.MarkerEntries)
                .FirstOrDefaultAsync(d => d.StationId == StationId && d.IsActive);

            if (config is not null)
            {
                var MarkersSearchRequest = new MarkersSearchRequest
                {
                    X = config.X,
                    Y = config.Y,
                    Yaw = config.Yaw,
                    Width = config.Width,
                    Length = config.Length,
                    MarkerSearchRequests = [.. config.MarkerEntries
                        .OrderBy(m => m.Priority)
                        .Select(m => new MarkerEntry
                        {
                            MarkerId = m.MarkerId ?? string.Empty,
                            Type = (MarkerType)m.Type,
                            Priority = m.Priority,
                            DeviceId = m.DeviceId ?? string.Empty,
                            Code = m.Code ?? string.Empty,
                            ReferencePoints = DeserializeReferencePoints(m.ReferencePointsJson)
                        })]
                };
                DetectSession = await MarkerDetector.CreateSessionAsync(MarkersSearchRequest);
                var plcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
                IsHasLoad = plcController.SetHasLoadValue;
                //var StateMachine = scope.ServiceProvider.GetRequiredService<RobotStateMachine>();
                //StateMachine.Fire(RobotEventType.StartDocking);
                await base.StartAction();
            }
        }
        else
        {
            SetStatus(ActionEvent.FAILED);
            ResultDescription = "Cannot get Marker Detection";
        }
    }

    private static Vector2[] DeserializeReferencePoints(string? json)
    {
        if (string.IsNullOrWhiteSpace(json)) return [];
        try { return JsonSerializer.Deserialize<Vector2[]>(json) ?? []; }
        catch { return []; }
    }

    protected override Task StopAction()
    {
        Navigation?.CancelMovement();
        return base.StopAction();
    }

    protected override Task CleanupAction()
    {
        DetectSession?.Dispose();
        DetectSession = null;
        MarkerDetector?.Dispose();
        MarkerDetector = null;
        //using var scope = ServiceProvider.CreateAsyncScope();
        //var StateMachine = scope.ServiceProvider.GetRequiredService<RobotStateMachine>();
        //StateMachine.Fire(RobotEventType.CompleteDocking);
        return base.CleanupAction();
    }

    protected override Task ExecuteAction()
    {
        if (DetectSession is null)
        {
            SetStatus(ActionEvent.FAILED);
            ResultDescription = "Module Detect Marker is not existed";
        }
        else if (Navigation is null)
        {
            SetStatus(ActionEvent.FAILED);
            ResultDescription = "Module Navigation is not existed";
        }
        else
        {
            if (!IsFindedGoal) IsFindedGoal = DetectSession.Goal is not null;

            if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
            {
                Navigation?.CancelMovement();
                SetStatus(ActionEvent.FAILED);
                ResultDescription = "Timeout action";
            }
            else if (!IsFindedGoal) base.ExecuteAction();
            else
            {
                if (!IsStartDockTo)
                {
                    Navigation.DockTo(DetectSession, IsHasLoad, Direction);
                    IsStartDockTo = true;
                }
                if (Navigation.State == NavigationState.Completed)
                {
                    SetStatus(ActionEvent.FINISHED);
                    ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
                }
                else if (Navigation.State == NavigationState.Error)
                {
                    SetStatus(ActionEvent.FAILED);
                    ResultDescription = $"Action Handle [{Type} - {Id}] is failed: Navigation Failed";
                }
            }
        }
        return base.ExecuteAction();
    }

    protected override void Initialize()
    {
        base.Initialize();

        if (ActionParameters.Count > 0)
        {
            foreach (var parameterStore in ActionParameters)
            {
                if (!parameterStore.IsOptional)
                {
                    if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
                }
            }
            Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
        }

        var stationPara = Parameters.FirstOrDefault(p => p.Key == "stationId") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'stationId'");
        StationId = stationPara.Value;
        var directionPara = Parameters.FirstOrDefault(p => p.Key == "direction");
        Direction = directionPara is not null && Enum.TryParse<RobotDirection>(directionPara.Value, out var direction) ? direction : null;
    }
}