BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Helper/ActionConflictDetector.cs

using RobotNet.VDA5050.Type;
using RobotNet10.RobotApp.Services.Robot.Actions;

namespace RobotNet10.RobotApp.Services.Robot.Helper;

/// <summary>
/// Detects conflicts between actions according to VDA5050
/// </summary>
public class ActionConflictDetector
{
    // VDA5050: Counter-action pairs that conflict
    private static readonly Dictionary<ActionType, ActionType> CounterActions = new()
    {
        { ActionType.START_CHARGING, ActionType.STOP_CHARGING },
        { ActionType.STOP_CHARGING, ActionType.START_CHARGING },
        { ActionType.START_PAUSE, ActionType.STOP_PAUSE },
        { ActionType.STOP_PAUSE, ActionType.START_PAUSE },
    };

    // Actions that target the same resource and cannot run simultaneously
    private static readonly HashSet<ActionType> LoadHandlingActions =
    [
        ActionType.PICK,
        ActionType.DROP,
        ActionType.LIFT_ROTATE,
        ActionType.ROTATE,
        ActionType.ROTATE_KEEP_LIFT
    ];

    private static readonly HashSet<ActionType> ChargingActions =
    [
        ActionType.START_CHARGING,
        ActionType.STOP_CHARGING
    ];

    // Actions that use the Navigation module - cannot run simultaneously
    private static readonly HashSet<ActionType> NavigationActions =
    [
        ActionType.DOCK_TO,
        ActionType.MOVE_STRAIGHT_TO_COOR,
        ActionType.MOVE_STRAIGHT_WITH_DISTANCE,
        ActionType.FINE_POSITIONING,
        ActionType.INIT_POSITION,
        ActionType.START_CHARGING,
        ActionType.STOP_CHARGING
    ];

    // Functional module actions - cannot run while robot is moving (navigation active)
    private static readonly HashSet<ActionType> FunctionalModuleActions =
    [
        ActionType.PICK,
        ActionType.DROP,
        ActionType.LIFT_ROTATE,
        ActionType.ROTATE,
        ActionType.ROTATE_KEEP_LIFT,
        ActionType.DOCK_TO,
        ActionType.DETECT_OBJECT,
        ActionType.START_CHARGING,
        ActionType.STOP_CHARGING,
        ActionType.FINE_POSITIONING
    ];

    /// <summary>
    /// Check if instant action conflicts with any running actions (ORDER or INSTANT)
    /// </summary>
    public ConflictResult CheckConflict(
        RobotNet.VDA5050.InstantAction.Action instantAction,
        IEnumerable<RobotAction> runningActions,
        bool isOrderActive = false,
        bool isDriving = false)
    {
        if (!RobotNet.VDA5050.EnumHelper.TryParse(instantAction.ActionType, out ActionType instantType))
        {
            return ConflictResult.Invalid("Invalid action type");
        }

        // VDA5050: cancelOrder and read-only actions must NEVER be blocked
        if (instantType == ActionType.CANCEL_ORDER ||
            instantType == ActionType.STATE_REQUEST ||
            instantType == ActionType.FACTSHEET_REQUEST)
        {
            return ConflictResult.NoConflict();
        }

        // Check: Navigation instant action while Order is active
        if (isOrderActive && NavigationActions.Contains(instantType))
        {
            return ConflictResult.Conflict(
                ConflictType.NavigationOrderConflict,
                $"Navigation action {instantType} rejected - Order is active, cannot execute navigation instant actions",
                null
            );
        }

        // Check: Functional module or navigation action while robot is driving
        if (isDriving && (FunctionalModuleActions.Contains(instantType) || NavigationActions.Contains(instantType)))
        {
            return ConflictResult.Conflict(
                ConflictType.DrivingConflict,
                $"Action {instantType} rejected - robot is currently moving",
                null
            );
        }

        foreach (var runningAction in runningActions)
        {
            // 1. Check counter-action conflict
            var counterConflict = CheckCounterActionConflict(instantType, runningAction);
            if (counterConflict.HasConflict)
            {
                return counterConflict;
            }

            // 2. Check resource conflict
            var resourceConflict = CheckResourceConflict(instantAction, instantType, runningAction);
            if (resourceConflict.HasConflict)
            {
                return resourceConflict;
            }

            // 3. Check navigation conflict (two navigation actions cannot run simultaneously)
            var navConflict = CheckNavigationConflict(instantType, runningAction);
            if (navConflict.HasConflict)
            {
                return navConflict;
            }

            // 4. Check BlockingType conflict
            var blockingConflict = CheckBlockingTypeConflict(instantAction, runningAction);
            if (blockingConflict.HasConflict)
            {
                return blockingConflict;
            }
        }

        return ConflictResult.NoConflict();
    }

    private static ConflictResult CheckCounterActionConflict(ActionType instantType, RobotAction runningAction)
    {
        if (CounterActions.TryGetValue(instantType, out var counterType) &&
            counterType == runningAction.Type)
        {
            return ConflictResult.Conflict(
                ConflictType.CounterAction,
                $"InstantAction {instantType} conflicts with running action {runningAction.Type}",
                runningAction.Id
            );
        }

        return ConflictResult.NoConflict();
    }

    private static ConflictResult CheckResourceConflict(
        RobotNet.VDA5050.InstantAction.Action instantAction,
        ActionType instantType,
        RobotAction runningAction)
    {
        // Check if both actions target load handling
        if (LoadHandlingActions.Contains(instantType) &&
            LoadHandlingActions.Contains(runningAction.Type))
        {
            // Check if same LHD (Load Handling Device)
            var instantLhd = GetParameterValue(instantAction.ActionParameters, "lhd");
            var orderLhd = GetParameterValue(runningAction.Parameters, "lhd");

            // If both specify LHD and they're the same, or if neither specifies (default LHD)
            if (string.IsNullOrEmpty(instantLhd) || string.IsNullOrEmpty(orderLhd) ||
                instantLhd == orderLhd)
            {
                return ConflictResult.Conflict(
                    ConflictType.ResourceConflict,
                    $"InstantAction {instantType} conflicts with {runningAction.Type} - same Load Handling Device",
                    runningAction.Id
                );
            }
        }

        // Check if both actions target charging
        if (ChargingActions.Contains(instantType) &&
            ChargingActions.Contains(runningAction.Type))
        {
            return ConflictResult.Conflict(
                ConflictType.ResourceConflict,
                $"InstantAction {instantType} conflicts with {runningAction.Type} - same charging system",
                runningAction.Id
            );
        }

        return ConflictResult.NoConflict();
    }

    private static ConflictResult CheckNavigationConflict(ActionType instantType, RobotAction runningAction)
    {
        // Two navigation actions cannot run simultaneously
        if (NavigationActions.Contains(instantType) &&
            NavigationActions.Contains(runningAction.Type) &&
            !runningAction.IsCompleted)
        {
            return ConflictResult.Conflict(
                ConflictType.NavigationConflict,
                $"Navigation action {instantType} conflicts with running navigation action {runningAction.Type}",
                runningAction.Id
            );
        }

        return ConflictResult.NoConflict();
    }

    private static ConflictResult CheckBlockingTypeConflict(
        RobotNet.VDA5050.InstantAction.Action instantAction,
        RobotAction runningAction)
    {
        // HARD instant action cannot run when HARD action is running
        if (instantAction.BlockingType == BlockingType.HARD &&
            runningAction.BlockingType == BlockingType.HARD &&
            !runningAction.IsCompleted)
        {
            return ConflictResult.Conflict(
                ConflictType.BlockingTypeConflict,
                $"InstantAction (HARD) cannot run while action {runningAction.Type} (HARD) is running",
                runningAction.Id
            );
        }

        return ConflictResult.NoConflict();
    }

    private static string? GetParameterValue(
        RobotNet.VDA5050.InstantAction.ActionParameter[]? parameters,
        string key)
    {
        return parameters?.FirstOrDefault(p => p.Key == key)?.Value;
    }
}

/// <summary>
/// Result of conflict detection
/// </summary>
public class ConflictResult
{
    public bool HasConflict { get; init; }
    public ConflictType Type { get; init; }
    public string Description { get; init; } = "";
    public string? ConflictingActionId { get; init; }

    public static ConflictResult NoConflict() => new() { HasConflict = false };

    public static ConflictResult Conflict(ConflictType type, string description, string? conflictingActionId = null)
        => new()
        {
            HasConflict = true,
            Type = type,
            Description = description,
            ConflictingActionId = conflictingActionId
        };

    public static ConflictResult Invalid(string description)
        => new()
        {
            HasConflict = true,
            Type = ConflictType.Invalid,
            Description = description
        };
}

/// <summary>
/// Types of conflicts
/// </summary>
public enum ConflictType
{
    None,
    CounterAction,          // e.g., startCharging vs stopCharging
    ResourceConflict,       // e.g., two pick actions on same LHD
    NavigationConflict,     // e.g., two navigation actions (dockTo vs moveStraight)
    NavigationOrderConflict,// Navigation instant action while Order is active
    DrivingConflict,        // Functional module action while robot is moving
    BlockingTypeConflict,   // e.g., HARD vs HARD
    Invalid                 // Invalid action type or parameters
}