Denso/srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Services/SafetyMonitor.cs

using RobotNet10.NavigationTune.Shared.Models;

namespace RobotNet10.NavigationTune.Services;

/// <summary>
/// Safety monitor for test execution
/// </summary>
public class SafetyMonitor(SafetyConfig config)
{
    private readonly List<SafetyViolation> _violations = new();
    private DateTime? _trackingErrorStart;

    /// <summary>
    /// Check safety conditions
    /// </summary>
    public bool CheckSafety(TelemetryData telemetry, ReferencePath referencePath)
    {
        bool isSafe = true;

        // 1. Check cross-track error
        if (telemetry.CrossTrackError > config.MaxCrossTrackError)
        {
            LogViolation(new SafetyViolation
            {
                Type = ViolationType.CrossTrackError,
                Severity = ViolationSeverity.Critical,
                Value = telemetry.CrossTrackError,
                Threshold = config.MaxCrossTrackError,
                Message = $"CTE {telemetry.CrossTrackError:F3}m exceeds limit {config.MaxCrossTrackError:F3}m",
                Timestamp = DateTime.UtcNow
            });
            isSafe = false;
        }

        // 2. Check heading error
        if (Math.Abs(telemetry.HeadingError) > config.MaxHeadingError)
        {
            LogViolation(new SafetyViolation
            {
                Type = ViolationType.HeadingError,
                Severity = ViolationSeverity.Critical,
                Value = Math.Abs(telemetry.HeadingError),
                Threshold = config.MaxHeadingError,
                Message = $"Heading error {telemetry.HeadingError * 180 / Math.PI:F1}° exceeds limit",
                Timestamp = DateTime.UtcNow
            });
            isSafe = false;
        }

        // 3. Check velocity limits
        if (Math.Abs(telemetry.RobotTwist.Linear) > config.MaxLinearVelocity * 1.1)
        {
            LogViolation(new SafetyViolation
            {
                Type = ViolationType.VelocityLimit,
                Severity = ViolationSeverity.Warning,
                Value = Math.Abs(telemetry.RobotTwist.Linear),
                Threshold = config.MaxLinearVelocity,
                Message = $"Linear velocity {telemetry.RobotTwist.Linear:F2} m/s exceeds limit",
                Timestamp = DateTime.UtcNow
            });
        }

        // 4. Check sustained tracking error
        if (telemetry.CrossTrackError > config.MaxCrossTrackError * 0.5)
        {
            _trackingErrorStart ??= DateTime.UtcNow;

            var duration = (DateTime.UtcNow - _trackingErrorStart.Value).TotalMilliseconds;
            if (duration > config.MaxTrackingErrorDuration)
            {
                LogViolation(new SafetyViolation
                {
                    Type = ViolationType.SustainedTrackingError,
                    Severity = ViolationSeverity.Critical,
                    Value = duration,
                    Threshold = config.MaxTrackingErrorDuration,
                    Message = $"Tracking error sustained for {duration:F0}ms",
                    Timestamp = DateTime.UtcNow
                });
                isSafe = false;
            }
        }
        else
        {
            _trackingErrorStart = null;
        }

        return isSafe;
    }

    public List<SafetyViolation> GetViolations() => _violations;

    public void Reset()
    {
        _violations.Clear();
        _trackingErrorStart = null;
    }

    private void LogViolation(SafetyViolation violation)
    {
        _violations.Add(violation);
    }
}

/// <summary>
/// Safety configuration
/// </summary>
public class SafetyConfig
{
    public double MaxCrossTrackError { get; set; } = 0.5; // meters
    public double MaxHeadingError { get; set; } = 45f * Math.PI / 180f; // radians (45 degrees)
    public double MaxLinearVelocity { get; set; } = 1.5; // m/s
    public double MaxAngularVelocity { get; set; } = 6.0; // rad/s
    public int MaxTrackingErrorDuration { get; set; } = 3000; // milliseconds
}