using Appccelerate.StateMachine;
using Appccelerate.StateMachine.Machine;
using CartographerSharp.Mapping;
using CartographerSharp.Models.Mapping;
using CartographerSharp.Sensor;
using Microsoft.AspNetCore.SignalR;
using Microsoft.Extensions.Options;
using System.Collections.Concurrent;
using System.Diagnostics;
using System.Globalization;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Hubs;
using RobotNet10.RobotApp.Motion;
using RobotNet10.RobotApp.Shared;
using RobotNet10.RobotApp.Shared.Enums;
using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
using RobotNet10.RobotApp.SLAM.Cartographer.Mcl;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;
using RobotNet10.Shared.Numbers;
using SkiaSharp;

namespace RobotNet10.RobotApp.SLAM.Cartographer;

/// <summary>
/// Main service for Cartographer SLAM integration.
/// Manages state machine, sensor subscriptions, and handles both localization and scan mapping.
/// Implements IHostedService to start automatically when application starts.
///
/// Split into partial classes:
///   CartographerService.cs              - Fields, properties, constructor
///   CartographerService.StateMachine.cs - State machine definition and helpers
///   CartographerService.StateHandlers.cs- State entry/exit handlers
///   CartographerService.SensorProcessing.cs - Sensor data callbacks and MCL processing
///   CartographerService.ApiMethods.cs   - ISLAMService method implementations
///   CartographerService.MapManagement.cs- Map CRUD, loading, wall alignment
///   CartographerService.Lifecycle.cs    - IHostedService, state persistence, IDisposable
/// </summary>
public partial class CartographerService : ISLAMService, IHostedService, IDisposable
{
    #region Fields

    private readonly PassiveStateMachine<SLAMState, CartographerTrigger> _stateMachine;
    private readonly CartographerConfiguration _config;
    private readonly IDeviceProvider _deviceProvider;
    private readonly SensorPipeline? _sensorPipeline;
    private readonly IHubContext<SLAMHub> _hubContext;
    private readonly ILogger<CartographerService> _logger;
    private readonly Lock _lock = new();
    private readonly Lock _stateMachineLock = new();

    // State tracking (synchronized with state machine in ExecuteOnEntry handlers)
    private SLAMState _currentState = SLAMState.Idle;

    // Error tracking
    private string? _lastError;
    private Exception? _lastException;

    // Unified MapBuilder and TrajectoryBuilder for both ScanMapping and Localization
    private IMapBuilder? _mapBuilder;
    private ITrajectoryBuilder? _trajectoryBuilder;
    private int _trajectoryId = -1;
    private string? _currentMapName; // Store current map name for both ScanMapping and Localization
    private Pose? _wallAlignedPose; // Store wall-aligned pose calculated during ScanMapping entry for use when saving
    private readonly Lock _wallAlignedPoseLock = new(); // Synchronization for _wallAlignedPose
    private CancellationTokenSource? _savingMapCts; // CancellationTokenSource for save map operation

    // Covariance computation throttle (Fix 3)
    private int _covarianceComputing; // 0 = idle, 1 = computing

    // Pose sync: Stopwatch for periodic pose saving during Localizing/ScanMapping
    private readonly Stopwatch _poseSyncStopwatch = new();

    // Pose snapshot: lock-free cached pose + confidence (updated by background thread or MCL)
    // Access via Volatile.Read/Write for memory ordering guarantees
    private PoseSnapshot _poseSnapshot = PoseSnapshot.Empty;

    // Background pose extrapolation thread (runs at 20Hz for ScanMapping, 100Hz for Localizing)
    private Thread? _poseThread;
    private volatile bool _poseThreadRunning;

    // Background trigger thread: handles deferred state machine triggers from sensor processing
    private Thread? _triggerThread;
    private volatile bool _triggerThreadRunning;

    // Confidence metrics: written by sensor callback, read by pose thread
    private Matrix3x3? _poseCovariance;
    private int _constraintCount = 0;
    private double _averageConstraintQuality = 0.0;
    private double _lastMatchingScore = -1.0; // ScanMapping: PoseConfidence from scan matcher (use Volatile.Read/Write)

    // Localization: Constraint caching for performance
    private readonly TrajectoryConstraintCache _constraintCache = new(TimeSpan.FromMilliseconds(100));

    // MCL (Monte Carlo Localization): when Enabled, SetInitialPoseAsync runs MCL until convergence then adds trajectory (xloc flow). Created internally, only used here.
    private readonly MclService? _mcl;
    private readonly MclProcessor? _mclProcessor;

    // Pending state machine trigger: used to defer FireStateMachine calls from lidar processing thread
    // This prevents deadlock when MCL converges and tries to stop the lidar thread from within itself
    // Uses int because volatile cannot be used with nullable enums; -1 = no pending, >= 0 = trigger value
    private int _pendingTrigger = -1;
    private readonly ManualResetEventSlim _pendingTriggerEvent = new(false);

    // Sample point cloud in map/global frame from AddSensorData SamplePointCloudGlobal; returned by GetAggregatedSamplePointCloud
    private List<Vector3> _completedAccumulatedSamplePointClouds = [];
    private readonly Lock _pointCloudLock = new();
    private readonly Stopwatch _pointCloudUpdateStopwatch = Stopwatch.StartNew();

    // Reusable buffer for MCL scan conversion (Fix 6a)
    private List<Vector2>? _mclScanBuffer;

    // Pending localization data (set by StartLocalization, consumed by state handlers)
    private readonly Lock _localizationLock = new();
    private string? _pendingLocalizationMapName;
    private Pose? _pendingLocalizationInitialPose;
    private bool _isSetInitialPoseFlow; // Flag to differentiate SetInitialPose from StartLocalization

    // Helper processors
    private readonly SlamResultProcessor? _slamResultProcessor;
    private readonly OccupancyGridManager _occupancyGridManager;
    private readonly MapSaveProcessor? _mapSaveProcessor;

    // Drift detection for Localizing state
    private readonly DriftDetector _driftDetector;

    // Map processing status tracking
    private readonly ConcurrentDictionary<string, bool> _mapProcessingStatus = new();

    private readonly IOdometryEstimator? _odometryEstimator;

    // Dispose tracking
    private volatile bool _disposed = false;

    #endregion

    #region Properties

    /// <summary>
    /// Gets the MapBuilder instance (internal - for OccupancyGridProvider).
    /// Works for both ScanMapping and Localization modes.
    /// </summary>
    internal IMapBuilder? MapBuilder => _mapBuilder;

    /// <summary>
    /// Gets the current trajectory ID (internal).
    /// Works for both ScanMapping and Localization modes.
    /// </summary>
    internal int TrajectoryId => _trajectoryId;

    /// <summary>
    /// Gets the current pose in global (map) frame.
    /// Lock-free: returns cached snapshot updated by background pose thread (20Hz ScanMapping, 100Hz Localizing)
    /// or directly by MCL during Relocalizing.
    /// </summary>
    public Pose CurrentPose => Volatile.Read(ref _poseSnapshot).Pose;

    /// <summary>
    /// Gets the covariance of pose estimate (only available during localization).
    /// Lock-free: reads from cached snapshot.
    /// </summary>
    public Matrix3x3? PoseCovariance => Volatile.Read(ref _poseSnapshot).Covariance;

    /// <summary>
    /// Gets the current map name (available during Localization or ScanMapping)
    /// </summary>
    public string? CurrentMap
    {
        get
        {
            lock (_lock)
            {
                return _currentMapName;
            }
        }
    }

    /// <summary>
    /// Gets the confidence score. Meaning varies by state:
    /// ScanMapping: PoseConfidence from scan matcher.
    /// Localizing: LocalizationScore from covariance/constraints/scanMatchScore.
    /// Relocalizing: MCL reliability.
    /// Lock-free: reads from cached snapshot.
    /// </summary>
    public double? LocalizationScore => Volatile.Read(ref _poseSnapshot).Score;

    /// <summary>
    /// Gets the current drift detection metrics (only meaningful during Localizing state).
    /// Provides detailed breakdown of localization quality factors including scan match score,
    /// odometry residual, and drift status.
    /// </summary>
    public DriftDetector.DriftMetrics DriftMetrics => _driftDetector.GetLatestMetrics();

    /// <summary>
    /// Gets the current drift status (only meaningful during Localizing state).
    /// Returns Stable, Warning, Critical, or Lost based on combined metrics analysis.
    /// </summary>
    public DriftDetector.DriftStatus DriftStatus => _driftDetector.GetLatestMetrics().Status;

    /// <summary>
    /// Gets the current state (synchronized with state machine)
    /// </summary>
    public SLAMState State
    {
        get
        {
            lock (_lock)
            {
                return _currentState;
            }
        }
    }

    #endregion

    #region Pose Thread

    /// <summary>
    /// Starts the background pose extrapolation thread.
    /// </summary>
    /// <param name="intervalMs">Polling interval: 50ms for ScanMapping (20Hz), 10ms for Localizing (100Hz)</param>
    private void StartPoseThread(int intervalMs)
    {
        if (_poseThreadRunning) return;
        _poseThreadRunning = true;
        _poseThread = new Thread(() => PoseThreadProc(intervalMs))
        {
            Name = "PoseExtrapolator",
            IsBackground = true,
            Priority = ThreadPriority.AboveNormal
        };
        _poseThread.Start();
    }

    /// <summary>
    /// Stops the background pose extrapolation thread and waits for it to finish.
    /// </summary>
    private void StopPoseThread()
    {
        if (!_poseThreadRunning) return;
        _poseThreadRunning = false;
        _poseThread?.Join(500);
        if (_poseThread?.IsAlive == true)
            _logger.LogWarning("CartographerService: Pose thread did not stop within timeout");
        _poseThread = null;
    }

    /// <summary>
    /// Background thread loop: polls TryGetExtrapolatedPoseFilter, applies localToGlobal transform,
    /// and publishes global-frame PoseSnapshot via volatile write.
    /// </summary>
    private void PoseThreadProc(int intervalMs)
    {
        Thread.BeginThreadAffinity();
        try
        {
            while (_poseThreadRunning && !_disposed)
            {
                try
                {
                    // Read references without lock (tolerate stale by 1 iteration)
                    var tb = _trajectoryBuilder;
                    var mb = _mapBuilder;
                    var trajId = _trajectoryId;

                    if (tb != null)
                    {
                        var extrapolated = tb.TryGetExtrapolatedPose(DateTime.UtcNow.Ticks);
                        if (extrapolated.HasValue)
                        {
                            Pose globalPose;
                            if (mb != null && trajId >= 0)
                            {
                                var localToGlobal = mb.PoseGraph.GetLocalToGlobalTransform(trajId);
                                globalPose = PoseConverter.ToPose(localToGlobal * extrapolated.Value);
                            }
                            else
                            {
                                globalPose = PoseConverter.ToPose(extrapolated.Value);
                            }

                            // Build confidence-aware snapshot based on current state
                            var state = _currentState;
                            PoseSnapshot snapshot;

                            if (state == SLAMState.ScanMapping)
                            {
                                var matchScore = Volatile.Read(ref _lastMatchingScore);
                                double? score = null;
                                if (matchScore >= 0)
                                {
                                    // Normalize to [0, 1] range if input is in [0, 100] range
                                    // PoseConfidence from Cartographer returns 0-100 (percentage)
                                    score = matchScore > 1.0 ? matchScore / 100.0 : matchScore;
                                    score = Math.Clamp(score.Value, 0.0, 1.0);
                                }
                                snapshot = new PoseSnapshot(globalPose, score: score);
                            }
                            else
                            {
                                // Localizing: read covariance fields and compute score with scan match
                                Matrix3x3? cov;
                                int cc;
                                double cq;
                                lock (_lock)
                                {
                                    cov = _poseCovariance;
                                    cc = _constraintCount;
                                    cq = _averageConstraintQuality;
                                }

                                // Include scan match score for drift detection
                                var matchScore = Volatile.Read(ref _lastMatchingScore);
                                double? scanMatchScore = matchScore >= 0 ? matchScore : null;

                                // Get odometry pose for cross-validation (helps distinguish drift vs dynamic obstacles)
                                Pose? odometryPose = _odometryEstimator?.CurrentPose;

                                // Update drift detector with all available metrics
                                var driftMetrics = _driftDetector.Update(
                                    cartographerPose: globalPose,
                                    odometryPose: odometryPose,
                                    scanMatchScore: scanMatchScore ?? 0.5,
                                    covariance: cov,
                                    constraintCount: cc,
                                    constraintQuality: cq,
                                    mclPose: null, // MCL not running during Localizing
                                    mclReliability: null);

                                // Use drift detector's combined score as the localization score
                                // This includes scan match score with proper weighting
                                var locScore = driftMetrics.CombinedScore;

                                snapshot = new PoseSnapshot(
                                    globalPose,
                                    cov,
                                    locScore,
                                    scanMatchScore,
                                    driftMetrics.Status);
                            }

                            Volatile.Write(ref _poseSnapshot, snapshot);
                        }
                    }
                }
                catch (Exception ex)
                {
                    _logger.LogDebug(ex, "CartographerService: PoseThread iteration failed");
                }

                Thread.Sleep(intervalMs);
            }
        }
        finally
        {
            Thread.EndThreadAffinity();
        }
    }

    #endregion

    #region Trigger Thread

    /// <summary>
    /// Queues a state machine trigger to be fired from a dedicated thread.
    /// This prevents deadlock when MCL converges inside the lidar processing thread
    /// and tries to stop the same thread via state transition.
    /// </summary>
    private void QueueStateMachineTrigger(CartographerTrigger trigger)
    {
        Interlocked.Exchange(ref _pendingTrigger, (int)trigger);
        _pendingTriggerEvent.Set();
    }

    /// <summary>
    /// Starts the background trigger thread that handles deferred state machine transitions.
    /// </summary>
    private void StartTriggerThread()
    {
        if (_triggerThreadRunning) return;
        _triggerThreadRunning = true;
        _triggerThread = new Thread(TriggerThreadProc)
        {
            Name = "StateMachineTrigger",
            IsBackground = true,
            Priority = ThreadPriority.AboveNormal
        };
        _triggerThread.Start();
    }

    /// <summary>
    /// Stops the background trigger thread.
    /// </summary>
    private void StopTriggerThread()
    {
        if (!_triggerThreadRunning) return;
        _triggerThreadRunning = false;
        _pendingTriggerEvent.Set(); // Wake thread to exit
        _triggerThread?.Join(1000);
        if (_triggerThread?.IsAlive == true)
            _logger.LogWarning("CartographerService: Trigger thread did not stop within timeout");
        _triggerThread = null;
    }

    /// <summary>
    /// Background thread that processes deferred state machine triggers.
    /// </summary>
    private void TriggerThreadProc()
    {
        while (_triggerThreadRunning && !_disposed)
        {
            try
            {
                _pendingTriggerEvent.Wait(100); // Wait with timeout to check running flag
                _pendingTriggerEvent.Reset();

                var triggerValue = Interlocked.Exchange(ref _pendingTrigger, -1);
                if (triggerValue >= 0)
                {
                    var trigger = (CartographerTrigger)triggerValue;
                    _logger.LogDebug("CartographerService: Firing deferred trigger {Trigger}", trigger);
                    FireStateMachine(trigger);
                }
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "CartographerService: Error in TriggerThreadProc");
            }
        }
    }

    #endregion

    #region Constructor

    public CartographerService(
        IOptions<CartographerConfiguration> configuration,
        IDeviceProvider deviceProvider,
        IHubContext<SLAMHub> hubContext,
        ILogger<CartographerService> logger,
        IOdometryEstimator odometryEstimator,
        ILoggerFactory loggerFactory)
    {
        _config = configuration?.Value ?? throw new ArgumentNullException(nameof(configuration));
        _deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
        _hubContext = hubContext ?? throw new ArgumentNullException(nameof(hubContext));
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
        _odometryEstimator = odometryEstimator;

        // Initialize components based on Enable flag
        if (_config.Enable)
        {
            // Full SLAM mode: create all components
            _mcl = _config.Mcl.Enabled ? new MclService(configuration) : null;
            _sensorPipeline = new SensorPipeline(
                deviceProvider,
                odometryEstimator,
                configuration,
                logger,
                OnAddRangeData,
                OnAddImuData,
                OnAddOdometryData);
            _mclProcessor = _config.Mcl.Enabled ? new MclProcessor(_config, loggerFactory.CreateLogger<MclProcessor>()) : null;
            _slamResultProcessor = new SlamResultProcessor(loggerFactory.CreateLogger<SlamResultProcessor>());

            // Wire up MclProcessor odometry event if enabled
            if (_mclProcessor != null && _mcl != null)
            {
                _mclProcessor.OnOdometryProcessed += (dt, vx, vy, vtheta) => _mcl.OnOdom(dt, vx, vy, vtheta);
            }
        }
        else
        {
            // Localization-only mode: minimal components
            _mcl = null;
            _sensorPipeline = null;
            _mclProcessor = null;
            _slamResultProcessor = null;

            _logger.LogInformation("CartographerService: Running in localization-only mode (Enable = false). Only StartLocalization, SetInitialPose, and StopLocalization are available.");
        }

        // Always create OccupancyGridManager (needed for both modes to load occupancy grid from file)
        _occupancyGridManager = new OccupancyGridManager(_config, loggerFactory.CreateLogger<OccupancyGridManager>());

        // Always create MapSaveProcessor (needed for both modes to transform maps and save metadata)
        _mapSaveProcessor = new MapSaveProcessor(_config, loggerFactory.CreateLogger<MapSaveProcessor>());

        // Always create DriftDetector for monitoring localization quality
        _driftDetector = new DriftDetector();

        // Build state machine
        _stateMachine = BuildStateMachine();
        // Note: State machine will be started in StartAsync (IHostedService)
    }

    #endregion
}