using RobotNet10.RobotApp.Shared.NavigationMonitor;

namespace RobotNet10.RobotApp.Services.NavigationMonitor;

/// <summary>
/// Stateless safety checker: evaluates telemetry against configurable thresholds.
/// </summary>
public static class NavigationSafetyChecker
{
    public static List<NavigationSafetyViolationDto> Check(
        NavigationTelemetryDto telemetry,
        NavigationSafetyConfigDto config)
    {
        var violations = new List<NavigationSafetyViolationDto>();
        var now = telemetry.TimestampMs;

        // 1. Linear velocity
        if (Math.Abs(telemetry.LinearVelocity) > config.MaxLinearVelocity)
        {
            violations.Add(new NavigationSafetyViolationDto
            {
                TimestampMs = now,
                Type = SafetyViolationType.LinearVelocity,
                Severity = SafetyViolationSeverity.Warning,
                Value = Math.Abs(telemetry.LinearVelocity),
                Threshold = config.MaxLinearVelocity,
                Message = $"Linear velocity {Math.Abs(telemetry.LinearVelocity):F2} m/s exceeds limit {config.MaxLinearVelocity:F2} m/s"
            });
        }

        // 2. Angular velocity
        if (Math.Abs(telemetry.AngularVelocity) > config.MaxAngularVelocity)
        {
            violations.Add(new NavigationSafetyViolationDto
            {
                TimestampMs = now,
                Type = SafetyViolationType.AngularVelocity,
                Severity = SafetyViolationSeverity.Warning,
                Value = Math.Abs(telemetry.AngularVelocity),
                Threshold = config.MaxAngularVelocity,
                Message = $"Angular velocity {Math.Abs(telemetry.AngularVelocity):F2} rad/s exceeds limit {config.MaxAngularVelocity:F2} rad/s"
            });
        }

        // 3. Linear acceleration
        if (Math.Abs(telemetry.LinearAcceleration) > config.MaxLinearAcceleration)
        {
            violations.Add(new NavigationSafetyViolationDto
            {
                TimestampMs = now,
                Type = SafetyViolationType.LinearAcceleration,
                Severity = SafetyViolationSeverity.Warning,
                Value = Math.Abs(telemetry.LinearAcceleration),
                Threshold = config.MaxLinearAcceleration,
                Message = $"Linear acceleration {Math.Abs(telemetry.LinearAcceleration):F2} m/s² exceeds limit {config.MaxLinearAcceleration:F2} m/s²"
            });
        }

        // 4. Cross-track error (only when navigating and path is available)
        double maxCteRadians = config.MaxCrossTrackError;
        if (telemetry.Driving && telemetry.CrossTrackError > maxCteRadians)
        {
            violations.Add(new NavigationSafetyViolationDto
            {
                TimestampMs = now,
                Type = SafetyViolationType.CrossTrackError,
                Severity = SafetyViolationSeverity.Critical,
                Value = telemetry.CrossTrackError,
                Threshold = config.MaxCrossTrackError,
                Message = $"CTE {telemetry.CrossTrackError:F3} m exceeds limit {config.MaxCrossTrackError:F3} m"
            });
        }

        // 5. Heading error (only when navigating and path is available)
        double maxHeadingRadians = config.MaxHeadingError * Math.PI / 180.0;
        if (telemetry.Driving && Math.Abs(telemetry.HeadingError) > maxHeadingRadians)
        {
            violations.Add(new NavigationSafetyViolationDto
            {
                TimestampMs = now,
                Type = SafetyViolationType.HeadingError,
                Severity = SafetyViolationSeverity.Critical,
                Value = Math.Abs(telemetry.HeadingError) * 180.0 / Math.PI,
                Threshold = config.MaxHeadingError,
                Message = $"Heading error {Math.Abs(telemetry.HeadingError) * 180.0 / Math.PI:F1}° exceeds limit {config.MaxHeadingError:F1}°"
            });
        }

        return violations;
    }
}