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

using RobotNet.VDA5050.Factsheet;
using RobotNet.VDA5050.Type;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Services.Exceptions;
using RobotNet10.RobotApp.Shared.Enums;
using System.Collections.ObjectModel;

namespace RobotNet10.RobotApp.Services.Robot.Actions;

[RobotAction(ActionType.MOVE_STRAIGHT_WITH_DISTANCE,
            [ActionScope.INSTANT],
            [BlockingType.HARD],
            "Di chuyển thẳng với khoảng cách xác định.",
            "Robot đã di chuyển thẳng với khoảng cách xác định.")]
public class RobotMoveStraightWithDistanceAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
    {
        new()
        {
            Key = "distance",
            Description = "Khoảng cách di chuyển. (m)",
            ValueDataType = ValueDataType.FLOAT,
            IsOptional = false,
        },
        new()
        {
            Key = "direction",
            Description = "Hướng di chuyển: FORWARD, BACKWARD",
            ValueDataType = ValueDataType.STRING,
            IsOptional = true,
        },
        new()
        {
            Key = "angle",
            Description = "Góc di chuyển so với hướng hiện tại của robot. (rad)",
            ValueDataType = ValueDataType.FLOAT,
            IsOptional = false,
        }
    }.AsReadOnly();

    private INavigation? Navigation;
    private ILocalization? Localization;
    private double Distance;
    private double Angle;
    private RobotDirection? Direction = null;

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

    protected override Task StartAction()
    {
        Navigation = ServiceProvider.GetRequiredService<INavigation>();
        Localization = ServiceProvider.GetRequiredService<ILocalization>();
        using var scope = ServiceProvider.CreateScope();
        var plcController = scope.ServiceProvider.GetRequiredService<IPlcController>();
        IsHasLoad = plcController.SetHasLoadValue;
        return base.StartAction();
    }

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

    protected override Task ExecuteAction()
    {
        if (Navigation is null || Localization is null)
        {
            SetStatus(ActionEvent.FAILED);
            ResultDescription = "Module Navigation or Localization is not existed";
        }
        else
        {
            if (CountTimeout++ > MAX_TIMEOUT / ActionInterval)
            {
                Navigation?.CancelMovement();
                SetStatus(ActionEvent.FAILED);
                ResultDescription = "Timeout action";
            }
            else
            {
                if (!IsStartMoveStraight)
                {
                    double targetX = Localization.X + Distance * Math.Cos(Angle);
                    double targetY = Localization.Y + Distance * Math.Sin(Angle);
                    Navigation.MoveStraight(targetX, targetY, IsHasLoad, Direction);
                    IsStartMoveStraight = 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 distancePara = Parameters.FirstOrDefault(p => p.Key == "distance") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'distance'");
        Distance = double.Parse(distancePara.Value);
        var anglePara = Parameters.FirstOrDefault(p => p.Key == "angle") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'angle'");
        Angle = double.Parse(anglePara.Value);
        var directionPara = Parameters.FirstOrDefault(p => p.Key == "direction");
        Direction = directionPara is not null && Enum.TryParse<RobotDirection>(directionPara.Value, out var direction) ? direction : null;
    }
}