using Microsoft.AspNetCore.SignalR;
using Microsoft.Extensions.Logging;
using RobotNet10.Common;
using RobotNet10.RobotApp.Hubs;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Services.Navigation;
using RobotNet10.RobotApp.Services.Robot.Modules;
using RobotNet10.RobotApp.Services.Simulation;
using RobotNet10.RobotApp.Shared.Enums;
using RobotNet10.RobotApp.Shared.NavigationMonitor;

namespace RobotNet10.RobotApp.Services.NavigationMonitor;

/// <summary>
/// Independent monitoring service for CSharpNavigation.
/// Periodically reads robot state and broadcasts telemetry via SignalR.
/// Can be enabled/disabled at runtime without affecting navigation.
/// </summary>
public class NavigationMonitorService : IHostedService, IDisposable
{
    private const int DefaultIntervalMs = 100; // 10Hz internal tick (safety checks)
    private const int BroadcastEveryNTicks = 5; // Broadcast telemetry every 5 ticks = 2Hz

    private readonly ILocalization _localization;
    private readonly IVelocityController _velocityController;
    private readonly RobotNavigation _navigation;
    private readonly IHubContext<NavigationMonitorHub> _hubContext;
    private readonly ILogger<NavigationMonitorService> _logger;

    private WatchTimer<NavigationMonitorService>? _timer;
    private volatile bool _telemetryEnabled;
    private volatile bool _safetyStopEnabled;
    private NavigationSafetyConfigDto _safetyConfig = new();
    private readonly Lock _configLock = new();

    // Safety stop latch state
    private volatile bool _safetyStopLatched;
    private string _safetyStopReason = "";
    private int _safetyStopGraceTicks;
    private const int SafetyStopGraceCount = 20; // 2s at 10Hz

    // State for acceleration calculation
    private double _lastLinearVel;
    private double _lastAngularVel;
    private long _lastTimestampMs;

    // Broadcast throttle counter
    private int _broadcastTickCounter;

    public NavigationMonitorService(
        ILocalization localization,
        IVelocityController velocityController,
        RobotNavigation navigation,
        IHubContext<NavigationMonitorHub> hubContext,
        ILogger<NavigationMonitorService> logger)
    {
        _localization = localization;
        _velocityController = velocityController;
        _navigation = navigation;
        _hubContext = hubContext;
        _logger = logger;
    }

    public Task StartAsync(CancellationToken cancellationToken)
    {
        _logger.LogInformation("NavigationMonitorService started (telemetry disabled by default)");
        return Task.CompletedTask;
    }

    public Task StopAsync(CancellationToken cancellationToken)
    {
        StopTimer();
        _logger.LogInformation("NavigationMonitorService stopped");
        return Task.CompletedTask;
    }

    public void SetTelemetryEnabled(bool enabled)
    {
        _telemetryEnabled = enabled;
        if (enabled)
            StartTimer();
        else
            StopTimer();

        _logger.LogInformation("Navigation telemetry {State}", enabled ? "enabled" : "disabled");
        BroadcastStateAsync();
    }

    public void SetSafetyStopEnabled(bool enabled)
    {
        _safetyStopEnabled = enabled;
        _logger.LogInformation("Navigation safety stop {State}", enabled ? "enabled" : "disabled");
        BroadcastStateAsync();
    }

    public void UpdateSafetyConfig(NavigationSafetyConfigDto config)
    {
        lock (_configLock)
        {
            _safetyConfig = config;
        }
        _logger.LogInformation("Navigation safety config updated");
        BroadcastStateAsync();
    }

    public NavigationMonitorStateDto GetState()
    {
        NavigationSafetyConfigDto configCopy;
        lock (_configLock)
        {
            configCopy = new NavigationSafetyConfigDto
            {
                MaxLinearVelocity = _safetyConfig.MaxLinearVelocity,
                MaxAngularVelocity = _safetyConfig.MaxAngularVelocity,
                MaxLinearAcceleration = _safetyConfig.MaxLinearAcceleration,
                MaxCrossTrackError = _safetyConfig.MaxCrossTrackError,
                MaxHeadingError = _safetyConfig.MaxHeadingError
            };
        }

        return new NavigationMonitorStateDto
        {
            TelemetryEnabled = _telemetryEnabled,
            SafetyStopEnabled = _safetyStopEnabled,
            SafetyStopLatched = _safetyStopLatched,
            SafetyStopReason = _safetyStopReason,
            SafetyConfig = configCopy,
            UpdateFrequencyHz = 1000.0 / (DefaultIntervalMs * BroadcastEveryNTicks)
        };
    }

    public void ReleaseSafetyStop()
    {
        if (!_safetyStopLatched) return;
        _safetyStopLatched = false;
        _safetyStopReason = "";
        _safetyStopGraceTicks = SafetyStopGraceCount;
        _navigation.Refresh();
        _logger.LogInformation("Safety stop released by user");
        BroadcastStateAsync();
    }

    private void StartTimer()
    {
        StopTimer();
        _lastTimestampMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
        _lastLinearVel = 0;
        _lastAngularVel = 0;
        _broadcastTickCounter = 0;
        _timer = new WatchTimer<NavigationMonitorService>(DefaultIntervalMs, OnTick, _logger);
        _timer.Start();
    }

    private void StopTimer()
    {
        _timer?.Dispose();
        _timer = null;
    }

    private void OnTick()
    {
        try
        {
            var now = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
            var dtSeconds = (now - _lastTimestampMs) / 1000.0;
            if (dtSeconds <= 0) dtSeconds = DefaultIntervalMs / 1000.0;

            // Read robot state
            var x = _localization.X;
            var y = _localization.Y;
            var theta = _localization.Theta;
            var (linearVel, angularVel) = _velocityController.ActualVelocity;
            var modelConfidence = _velocityController.GetModelConfidence();
            var navState = _navigation.State;
            var driving = _navigation.Driving;

            // Calculate acceleration
            var linearAccel = (linearVel - _lastLinearVel) / dtSeconds;
            var angularAccel = (angularVel - _lastAngularVel) / dtSeconds;
            _lastLinearVel = linearVel;
            _lastAngularVel = angularVel;
            _lastTimestampMs = now;

            // Calculate CTE and heading error if navigating with path
            double cte = 0;
            double headingError = 0;
            double distanceToGoal = 0;

            var path = _navigation.CurrentPath; // capture once (volatile)
            if (path is { Count: >= 2 } && driving)
            {
                // Skip heading error check during Rotating/FinePositioning:
                // - Rotating: robot intentionally turning in place, heading diverges from path tangent
                // - FinePositioning: robot re-aligning to dock goal, heading changes rapidly
                bool skipHeadingCheck = navState is NavigationState.Rotating or NavigationState.FinePositioning;
                (cte, headingError) = CalculatePathErrors(x, y, theta, path, skipHeadingCheck);
                var goal = path[^1];
                distanceToGoal = Math.Sqrt((x - goal.X) * (x - goal.X) + (y - goal.Y) * (y - goal.Y));
            }

            // DockTo-specific telemetry
            DockToTelemetryDto? dockToTelemetry = null;
            if (_navigation.IsDockingActive)
            {
                var dockGoal = _navigation.DockGoal;
                var dockStart = _navigation.DockStartNode;
                dockToTelemetry = new DockToTelemetryDto
                {
                    Phase = _navigation.DockPhase,
                    Direction = _navigation.DockDirection,
                    RetryCount = _navigation.DockRetryCount,
                    MaxRetries = _navigation.DockMaxRetries,
                    GoalX = dockGoal?.X ?? 0,
                    GoalY = dockGoal?.Y ?? 0,
                    GoalTheta = dockGoal?.Theta ?? 0,
                    TotalWaypoints = _navigation.DockWaypointCount,
                    StartX = dockStart?.X ?? 0,
                    StartY = dockStart?.Y ?? 0,
                    StartTheta = dockStart?.Theta ?? 0,
                    Waypoints = DownsampleWaypoints(_navigation.DockWaypoints, 30)
                };
            }

            // Downsample current path waypoints for client visualization
            var waypoints = path is { Count: >= 2 } && driving
                ? DownsampleNavigationWaypoints(path, 50)
                : [];

            var telemetry = new NavigationTelemetryDto
            {
                TimestampMs = now,
                X = x,
                Y = y,
                Theta = theta,
                LinearVelocity = linearVel,
                AngularVelocity = angularVel,
                LinearAcceleration = double.IsFinite(linearAccel) ? linearAccel : 0,
                AngularAcceleration = double.IsFinite(angularAccel) ? angularAccel : 0,
                NavigationState = navState.ToString(),
                Driving = driving,
                CrossTrackError = cte,
                HeadingError = headingError,
                DistanceToGoal = distanceToGoal,
                ModelConfidence = modelConfidence,
                DockTo = dockToTelemetry,
                Waypoints = waypoints
            };

            // Broadcast telemetry at reduced rate (2Hz) to save client resources
            _broadcastTickCounter++;
            if (_broadcastTickCounter >= BroadcastEveryNTicks)
            {
                _broadcastTickCounter = 0;
                _ = _hubContext.Clients.Group("monitor").SendAsync("ReceiveTelemetry", telemetry);
            }

            // Auto-release latch if navigation ended while latched
            if (_safetyStopLatched && !driving)
            {
                _safetyStopLatched = false;
                _safetyStopReason = "";
                _navigation.Refresh();
                _logger.LogInformation("Safety stop auto-released: navigation ended");
                BroadcastStateAsync();
            }

            // Grace period countdown after user release
            if (_safetyStopGraceTicks > 0) _safetyStopGraceTicks--;

            // Safety checks
            NavigationSafetyConfigDto configSnapshot;
            lock (_configLock)
            {
                configSnapshot = new NavigationSafetyConfigDto
                {
                    MaxLinearVelocity = _safetyConfig.MaxLinearVelocity,
                    MaxAngularVelocity = _safetyConfig.MaxAngularVelocity,
                    MaxLinearAcceleration = _safetyConfig.MaxLinearAcceleration,
                    MaxCrossTrackError = _safetyConfig.MaxCrossTrackError,
                    MaxHeadingError = _safetyConfig.MaxHeadingError
                };
            }

            var violations = NavigationSafetyChecker.Check(telemetry, configSnapshot);
            if (violations.Count > 0)
            {
                foreach (var v in violations)
                {
                    _ = _hubContext.Clients.Group("monitor").SendAsync("ReceiveSafetyViolation", v);
                }

                // Latch safety stop on critical violations: Pause navigation + hold
                if (_safetyStopEnabled && !_safetyStopLatched
                    && _safetyStopGraceTicks == 0
                    && violations.Exists(v => v.Severity == SafetyViolationSeverity.Critical))
                {
                    _safetyStopLatched = true;
                    _safetyStopReason = violations.First(v => v.Severity == SafetyViolationSeverity.Critical).Message;
                    _navigation.SafetyStop();
                    _logger.LogWarning("Safety stop latched: {Reason}", _safetyStopReason);
                    BroadcastStateAsync();
                }
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error in NavigationMonitor tick");
        }
    }

    /// <summary>
    /// Calculate cross-track error and heading error from current position to closest path point.
    /// </summary>
    private static (double cte, double headingError) CalculatePathErrors(
        double x, double y, double theta,
        IReadOnlyList<NavigationNode> path,
        bool skipHeadingCheck = false)
    {
        // Find closest point on path
        int closestIndex = 0;
        double minDist = double.MaxValue;

        for (int i = 0; i < path.Count; i++)
        {
            var dx = x - path[i].X;
            var dy = y - path[i].Y;
            var dist = dx * dx + dy * dy;
            if (dist < minDist)
            {
                minDist = dist;
                closestIndex = i;
            }
        }

        double cte = Math.Sqrt(minDist);

        if (skipHeadingCheck)
            return (cte, 0);

        // Calculate heading error using path tangent, accounting for Direction
        double headingError = 0;
        double refTheta;
        if (closestIndex < path.Count - 1)
        {
            var dx = path[closestIndex + 1].X - path[closestIndex].X;
            var dy = path[closestIndex + 1].Y - path[closestIndex].Y;
            refTheta = Math.Atan2(dy, dx);
        }
        else if (closestIndex > 0)
        {
            var dx = path[closestIndex].X - path[closestIndex - 1].X;
            var dy = path[closestIndex].Y - path[closestIndex - 1].Y;
            refTheta = Math.Atan2(dy, dx);
        }
        else
        {
            return (cte, 0);
        }

        // When path segment is BACKWARD, robot faces opposite to path tangent
        if (path[closestIndex].Direction == RobotDirection.BACKWARD)
            refTheta = NormalizeAngle(refTheta + Math.PI);

        headingError = NormalizeAngle(theta - refTheta);

        return (cte, headingError);
    }

    private static double NormalizeAngle(double angle)
    {
        while (angle > Math.PI) angle -= 2 * Math.PI;
        while (angle < -Math.PI) angle += 2 * Math.PI;
        return angle;
    }

    private static List<DockWaypointDto> DownsampleWaypoints(IReadOnlyList<NavigationNode>? waypoints, int maxPoints)
    {
        if (waypoints is null or { Count: 0 }) return [];
        if (waypoints.Count <= maxPoints)
            return waypoints.Select(w => new DockWaypointDto { X = w.X, Y = w.Y }).ToList();

        var result = new List<DockWaypointDto>(maxPoints);
        result.Add(new DockWaypointDto { X = waypoints[0].X, Y = waypoints[0].Y });

        double step = (double)(waypoints.Count - 1) / (maxPoints - 1);
        for (int i = 1; i < maxPoints - 1; i++)
        {
            int idx = (int)Math.Round(i * step);
            result.Add(new DockWaypointDto { X = waypoints[idx].X, Y = waypoints[idx].Y });
        }

        var last = waypoints[^1];
        result.Add(new DockWaypointDto { X = last.X, Y = last.Y });
        return result;
    }

    private static List<WaypointDto> DownsampleNavigationWaypoints(IReadOnlyList<NavigationNode> waypoints, int maxPoints)
    {
        if (waypoints.Count <= maxPoints)
            return waypoints.Select(w => new WaypointDto { X = w.X, Y = w.Y, Direction = w.Direction.ToString() }).ToList();

        var result = new List<WaypointDto>(maxPoints);
        result.Add(new WaypointDto { X = waypoints[0].X, Y = waypoints[0].Y, Direction = waypoints[0].Direction.ToString() });

        double step = (double)(waypoints.Count - 1) / (maxPoints - 1);
        for (int i = 1; i < maxPoints - 1; i++)
        {
            int idx = (int)Math.Round(i * step);
            result.Add(new WaypointDto { X = waypoints[idx].X, Y = waypoints[idx].Y, Direction = waypoints[idx].Direction.ToString() });
        }

        var last = waypoints[^1];
        result.Add(new WaypointDto { X = last.X, Y = last.Y, Direction = last.Direction.ToString() });
        return result;
    }

    private void BroadcastStateAsync()
    {
        var state = GetState();
        _ = _hubContext.Clients.Group("monitor").SendAsync("ReceiveMonitorState", state);
    }

    public void Dispose()
    {
        StopTimer();
        GC.SuppressFinalize(this);
    }
}