using System.Diagnostics;
using CartographerSharp.Mapping;
using CartographerSharp.Models.Mapping;
using CartographerSharp.Sensor;
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;

namespace RobotNet10.RobotApp.SLAM.Cartographer;

public partial class CartographerService
{
    #region Sensor Data Callbacks

    private void OnAddRangeData(string deviceId, RangeDataPayload payload)
    {
        // Get current state and relevant objects
        var currentState = State;
        var tb = _trajectoryBuilder;
        bool mclRunning = _mclProcessor?.IsRunning ?? false;
        MclService? mcl = _mcl;

        // Handle data based on current state
        switch (currentState)
        {
            case SLAMState.Relocalizing:
                // InitializingLocalizing: MCL is computing initial pose
                // - If MCL enabled and running: Process with MCL (no trajectory yet)
                // - If MCL disabled: Trajectory already added, process with CartographerSharp
                if (mclRunning && mcl != null && _mclProcessor != null)
                {
                    // MCL path: Feed scan to MCL from primary lidar only
                    string? primaryLidarId = _mclProcessor.PrimaryLidarId;
                    if (string.IsNullOrEmpty(primaryLidarId) || deviceId == primaryLidarId)
                    {
                        try
                        {
                            RunMclOnScan(payload, mcl);
                        }
                        catch (Exception ex)
                        {
                            _logger.LogError(ex, "CartographerService: OnAddRangeData MCL failed for {DeviceId}", deviceId);
                        }
                    }
                }
                else if (tb != null)
                {
                    // Non-MCL path: Trajectory already added in StartLocalizationAsync, process with CartographerSharp
                    ProcessWithTrajectoryBuilder(deviceId, payload, tb);
                }
                break;

            case SLAMState.Localizing:
                // Localizing: Use CartographerSharp for localization
                if (tb != null)
                {
                    ProcessWithTrajectoryBuilder(deviceId, payload, tb);
                }
                else
                {
                    _logger.LogWarning("CartographerService.OnAddRangeData: Localizing but _trajectoryBuilder is null!");
                }
                break;

            case SLAMState.ScanMapping:
                // ScanMapping: Use CartographerSharp for SLAM
                if (tb != null)
                {
                    ProcessWithTrajectoryBuilder(deviceId, payload, tb);
                }
                break;

            default:
                // Other states (Idle, Ready, SavingMap, Error): Ignore sensor data
                break;
        }
    }

    private void OnAddImuData(string deviceId, ImuData data) => _trajectoryBuilder?.AddSensorData(deviceId, data);

    private void OnAddOdometryData(string sensorId, OdometryData data)
    {
        // Feed to trajectory builder (existing)
        _trajectoryBuilder?.AddSensorData(sensorId, data);

        // Feed to MCL when running (xloc flow: mcl_->CallOdomCallback continuously)
        if (_mclProcessor?.IsRunning ?? false)
        {
            _mclProcessor.ProcessOdometry(data);
        }
    }

    #endregion

    #region Trajectory Builder Processing

    /// <summary>
    /// Process range data with trajectory builder (CartographerSharp)
    /// Used for both Localizing and ScanMapping states
    /// </summary>
    private void ProcessWithTrajectoryBuilder(string deviceId, RangeDataPayload payload, ITrajectoryBuilder tb)
    {
        try
        {
            var result = tb.AddSensorData(deviceId, payload.BaseLink);
            
            // Skip if disposed to prevent issues during shutdown
            if (_disposed)
            {
                return;
            }

            // Get current state and pose for operations that don't depend on result.HasValue
            var currentState = State;
            var currentPose = Volatile.Read(ref _poseSnapshot).Pose;

            // === SAMPLE POINT CLOUD (does NOT depend on hasValue) ===
            // Process sample point cloud even when result.HasValue is false
            // Use SamplePointCloudGlobal from result if available, otherwise transform raw payload to global frame
            if (_pointCloudUpdateStopwatch.ElapsedMilliseconds >= 1000)
            {
                // Capture variables for Task.Run closure
                var capturedPayload = payload;
                var capturedPose = currentPose;
                var capturedResult = result;
                var logger = _logger;

                // Move ALL calculation into Task.Run to avoid blocking sensor thread
                _ = Task.Run(() =>
                {
                    List<Vector3>? pointsToStore = null;

                    // Check if result has SamplePointCloudGlobal (preferred)
                    if (capturedResult.HasValue && capturedResult.Value.SamplePointCloudGlobal is { } pcFromResult)
                    {
                        // Use CartographerSharp's sample point cloud (already in global frame)
                        pointsToStore = new List<Vector3>(pcFromResult.Count);
                        foreach (var p in pcFromResult.Points)
                            pointsToStore.Add(new Vector3(p.Position.X, p.Position.Y, p.Position.Z));

                        if (pointsToStore != null && pointsToStore.Count > 0)
                        {
                            lock (_pointCloudLock)
                            {
                                _completedAccumulatedSamplePointClouds = pointsToStore;
                            }
                        }

                        _pointCloudUpdateStopwatch.Restart();
                    }
                });
            }

            // === POSE SYNC (does NOT depend on hasValue) ===
            // Periodic pose sync to file (for auto-resume on next startup)
            var syncInterval = _config.MapStorage.PoseSyncIntervalSeconds;
            if (syncInterval > 0 && _poseSyncStopwatch.Elapsed.TotalSeconds >= syncInterval)
            {
                var mapName = _currentMapName;
                if (!string.IsNullOrEmpty(mapName) && (currentState == SLAMState.Localizing || currentState == SLAMState.ScanMapping))
                {
                    // Use cached global-frame pose from snapshot
                    SavePoseToFile(mapName, currentPose);
                    _poseSyncStopwatch.Restart();
                }
            }

            // === CARTOGRAPHER RESULT PROCESSING (DEPENDS on hasValue) ===
            // Skip result processing if no valid result
            if (!result.HasValue)
            {
                return;
            }

            var r = result.Value;

            // Process based on mode
            if (currentState == SLAMState.ScanMapping)
            {
                // Store matching score for pose thread (volatile write, no lock needed)
                Volatile.Write(ref _lastMatchingScore, r.PoseConfidence);

                // Process scan mapping result (only if processor is available)
                _slamResultProcessor?.ProcessScanMappingResult(r, currentState, out _);

                // Signal insertion to OccupancyGridManager when a scan was inserted into submap.
                // This sets a flag that ShouldUpdateGrid() will check.
                if (r.InsertionResult.HasValue)
                {
                    _occupancyGridManager.SignalInsertion();
                }

                // Update occupancy grid when BOTH conditions are met:
                // 1. At least 3 seconds have elapsed since last update
                // 2. At least one scan was inserted since last update (SignalInsertion was called)
                if (_mapBuilder != null && _occupancyGridManager.ShouldUpdateGrid())
                {
                    var mapBuilder = _mapBuilder;
                    _ = Task.Run(() => _occupancyGridManager.UpdateFromMapBuilder(mapBuilder));
                }
            }
            else if (currentState == SLAMState.Localizing)
            {
                // Store scan match score for drift detection (same as ScanMapping but also used in LocalizationScore)
                // This is critical for detecting map drift during localization
                Volatile.Write(ref _lastMatchingScore, r.PoseConfidence);

                // Get cached covariance data
                var cachedCovariance = _slamResultProcessor?.PoseCovariance;
                var cachedConstraintCount = _slamResultProcessor?.ConstraintCount ?? 0;
                var cachedAverageQuality = _slamResultProcessor?.AverageConstraintQuality ?? 0.0;

                // Update confidence with scan match score (pose is handled by background thread at 100Hz)
                UpdateConfidenceMetrics(cachedCovariance, cachedConstraintCount, cachedAverageQuality, r.PoseConfidence);

                // Calculate covariance asynchronously (throttled: skip if already computing)
                IMapBuilder? mbSnapshot;
                int trajIdSnapshot;
                lock (_lock)
                {
                    mbSnapshot = _mapBuilder;
                    trajIdSnapshot = _trajectoryId;
                }
                if (mbSnapshot != null && Interlocked.CompareExchange(ref _covarianceComputing, 1, 0) == 0)
                {
                    _ = Task.Run(() =>
                    {
                        try
                        {
                            var constraints = _constraintCache.GetOrUpdate(mbSnapshot, trajIdSnapshot, _logger);

                            CovarianceCalculator.Result covResult;
                            try
                            {
                                covResult = constraints != null
                                    ? CovarianceCalculator.Calculate(constraints)
                                    : new CovarianceCalculator.Result(null, 0, 0.0);
                            }
                            catch (Exception ex)
                            {
                                _logger.LogWarning(ex, "CartographerService: Failed to calculate covariance from constraints");
                                covResult = new CovarianceCalculator.Result(null, 0, 0.0);
                            }

                            _slamResultProcessor?.UpdateCovariance(covResult.Covariance, covResult.ConstraintCount, covResult.AverageConstraintQuality);

                            lock (_lock)
                            {
                                _poseCovariance = covResult.Covariance;
                            }
                        }
                        catch (Exception ex)
                        {
                            _logger.LogWarning(ex, "CartographerService: Error calculating covariance asynchronously");
                        }
                        finally
                        {
                            Interlocked.Exchange(ref _covarianceComputing, 0);
                        }
                    });
                }
            }
        }
        catch (Exception ex)
        {
            _logger.LogDebug(ex, "CartographerService: ProcessWithTrajectoryBuilder failed for {DeviceId}", deviceId);
        }
    }

    #endregion

    #region MCL Scan Processing

    /// <summary>Runs MCL with one scan from the primary lidar. Convergence logic matches xloc MCLThread: pose stable (dist+yaw) + stable_duration + reliability/MAE, or timeout.</summary>
    private void RunMclOnScan(RangeDataPayload payload, MclService mcl)
    {
        // Base_link to laser was set at MCL start (SetMclBaseLinkToLaser). SensorPipeline always provides SensorFrame; use it for MCL (beam angles in sensor frame).
        var ranges = payload.SensorFrame.Ranges;
        _mclScanBuffer ??= new List<Vector2>(ranges.Count);
        _mclScanBuffer.Clear();
        foreach (var r in ranges)
            _mclScanBuffer.Add(new Vector2(r.Position.X, r.Position.Y));
        var pointsForScan = _mclScanBuffer;

        // Use actual angle information from filtered point cloud (SensorPipeline provides accurate angles after filtering)
        // Convert from radians to degrees for MclScanHelper.ConvertToScan
        double angleMinDeg = payload.ActualAngleMin * (180f / Math.PI);
        double angleMaxDeg = payload.ActualAngleMax * (180f / Math.PI);

        // Use actual number of points from scan data
        int numBins = pointsForScan.Count;

        double rangeMin = _config.TrajectoryBuilder.MinRange;
        double rangeMax = _config.TrajectoryBuilder.MaxRange;
        var (angleMin, angleMax, angleIncrement, _, _, mclRanges) = MclScanHelper.ConvertToScan(pointsForScan, angleMinDeg, angleMaxDeg, numBins, rangeMin, rangeMax);

        mcl.OnScan(angleMin, angleMax, angleIncrement, rangeMin, rangeMax, mclRanges);

        const int iterationsPerScan = 5;
        for (int i = 0; i < iterationsPerScan; i++)
            mcl.RunOneIteration();

        Pose currentPose = mcl.GetPose();
        double reliability = mcl.Reliability;
        double? mae = mcl.MaeForBestParticle;
        double totalLikelihood = mcl.TotalLikelihood;

        // Use MclProcessor to check convergence with reason
        var convergenceReason = _mclProcessor?.CheckConvergenceWithReason(currentPose, reliability, mae, out var _)
            ?? MclConvergenceReason.NotConverged;
        
        // Note: OnPoseUpdated is called in all paths below (converged/not-converged),
        // which publishes pose to _poseSnapshot with MCL reliability and MAE for proper score calculation.

        if (convergenceReason != MclConvergenceReason.NotConverged)
        {
            _mclProcessor?.Stop();

            // Disable global localization mode after convergence to switch to normal tracking mode
            mcl.DisableGlobalLocalizationMode();

            if (_mapBuilder == null)
            {
                _logger.LogWarning("CartographerService: MCL converged but MapBuilder is null; trajectory not added.");
                return;
            }

            // Verify no active trajectory before adding new one (defensive check)
            int currentTrajId;
            lock (_lock)
            {
                currentTrajId = _trajectoryId;
            }

            if (currentTrajId != -1)
            {
                _logger.LogWarning("CartographerService: MCL converged but active trajectory {TrajectoryId} exists. " +
                    "Waiting for trajectory to finish before adding new one.", currentTrajId);

                // Don't add trajectory yet, MCL will check again on next scan
                // This should never happen if first defensive check is working, but being defensive
                OnPoseUpdated(currentPose, null, mclReliability: reliability, mclMae: mae);
                return;
            }

            // Determine which pose to use for trajectory:
            // - PoseStable: MCL converged successfully, use MCL's current pose
            // - Timeout/MaxIterations: MCL failed to converge, fall back to initial pose
            Pose poseForTrajectory;
            if (convergenceReason == MclConvergenceReason.PoseStable)
            {
                poseForTrajectory = currentPose;
                _logger.LogInformation("CartographerService: MCL converged successfully, using MCL pose ({X:F3}, {Y:F3})",
                    currentPose.Position.X, currentPose.Position.Y);
            }
            else
            {
                // Timeout or MaxIterations - use initial pose (saved from auto-resume)
                poseForTrajectory = _mclProcessor?.InitialPose ?? currentPose;
                _logger.LogWarning("CartographerService: MCL did not converge (reason={Reason}), falling back to initial pose ({X:F3}, {Y:F3})",
                    convergenceReason, poseForTrajectory.Position.X, poseForTrajectory.Position.Y);
            }

            var (trajId, trajBuilder, poseInMapFrame) = _mapBuilder.AddLocalizationTrajectoryBuilder(_config, poseForTrajectory);
            lock (_lock)
            {
                _trajectoryId = trajId;
                _trajectoryBuilder = trajBuilder;
            }

            if (poseInMapFrame.HasValue)
            {
                var p = PoseConverter.ToPose(poseInMapFrame.Value);
                _logger.LogDebug("CartographerService: AddLocalizationTrajectoryBuilder trajId: {TrajId}, poseInMapFrame: [{X}, {Y}, {Z}] yaw: {Yaw:F4} ({YawDeg:F2})",
                    trajId, poseInMapFrame.Value.Translation.X, poseInMapFrame.Value.Translation.Y, poseInMapFrame.Value.Translation.Z, GetYawFromPose(p), GetYawFromPose(p) * 180.0 / Math.PI);
                // NOTE: Don't call StartRelocalization here - MCL already found the pose.
                // StartRelocalization triggers constraint-based relocalization which is redundant
                // and causes excessive optimization runs (sets _isRelocalized=false).
            }

            // Pass pose and MCL reliability/MAE for proper score calculation
            // Use poseForTrajectory to be consistent with what we're using for the trajectory
            OnPoseUpdated(poseForTrajectory, null, mclReliability: reliability, mclMae: mae);

            // Use deferred trigger to avoid deadlock: FireStateMachine would call PauseSubscriptions
            // which tries to stop the lidar thread, but we're currently running inside that thread.
            // QueueStateMachineTrigger fires the trigger from a separate thread.
            QueueStateMachineTrigger(CartographerTrigger.MclConverged);
        }
        else
        {
            // Pass MCL reliability and MAE for proper score calculation during convergence
            OnPoseUpdated(currentPose, null, mclReliability: reliability, mclMae: mae);
        }
    }

    /// <summary>Sets MCL base_link to laser transform from primary lidar config. Call once when starting MCL (after SetMap and SetInitialPose).</summary>
    /// <remarks>Config Transform is "lidar frame to base_link frame": P_base = R*P_lidar + Translation, so Translation = lidar origin in base_link (sensor pose in base_link). MCL expects baseLink2Laser = sensor pose in base_link (same meaning), so we pass (tx, ty, yaw) as-is; no inverse needed.</remarks>
    private void SetMclBaseLinkToLaser(string primaryLidarId, MclService mcl)
    {
        var lidarConfig = string.IsNullOrEmpty(primaryLidarId) ? null : _config.Sensors.Lidars.FirstOrDefault(l => l.DeviceId == primaryLidarId);
        if (lidarConfig?.Transform == null)
            return;
        var lidarTransform = lidarConfig.Transform;
        double tx = lidarTransform.Translation.X;
        double ty = lidarTransform.Translation.Y;
        double yawLidarInBase = lidarTransform.Rotation.ToYawRadian();
        mcl.SetBaseLinkToLaser(tx, ty, yawLidarInBase);
    }

    /// <summary>Resolves effective primary lidar for MCL (PrimaryLidarId if present in config, else first lidar). Logs and validates; returns empty if no lidars.</summary>
    private string GetEffectiveMclPrimaryLidarIdAndLog()
    {
        var lidars = _config.Sensors.Lidars;
        string? configured = string.IsNullOrEmpty(_config.Mcl.PrimaryLidarId) ? null : _config.Mcl.PrimaryLidarId;
        if (lidars.Count == 0)
        {
            if (!string.IsNullOrEmpty(configured))
                _logger.LogWarning("CartographerService: MCL PrimaryLidarId is set to {PrimaryLidarId} but Sensors.Lidars is empty; MCL will accept any device.", configured);
            return string.Empty;
        }
        if (!string.IsNullOrEmpty(configured))
        {
            var found = lidars.Any(l => l.DeviceId == configured);
            if (found)
            {
                _logger.LogInformation("CartographerService: MCL using primary lidar: {PrimaryLidarId}", configured);
                return configured;
            }
            _logger.LogWarning(
                "CartographerService: MCL PrimaryLidarId '{PrimaryLidarId}' not found in Sensors.Lidars; using first lidar: {FirstId}",
                configured, lidars[0].DeviceId);
        }
        var firstId = lidars[0].DeviceId ?? string.Empty;
        if (string.IsNullOrEmpty(configured))
            _logger.LogInformation("CartographerService: MCL using first lidar: {FirstId}", firstId);
        return firstId;
    }

    #endregion

    #region Pose Update

    /// <summary>
    /// Updates pose snapshot and confidence metrics.
    /// Used by MCL (Relocalizing) and state entry handlers when no background pose thread is running.
    /// When background pose thread IS running (ScanMapping/Localizing), use UpdateConfidenceMetrics instead.
    /// </summary>
    /// <param name="pose">Current pose in global frame</param>
    /// <param name="covariance">Pose covariance (optional)</param>
    /// <param name="constraintCount">Number of constraints (default 0)</param>
    /// <param name="constraintQuality">Average constraint quality 0-1 (default 0)</param>
    /// <param name="mclReliability">MCL reliability 0-1 (optional, for Relocalizing state)</param>
    /// <param name="mclMae">MCL mean absolute error in meters (optional, for Relocalizing state)</param>
    private void OnPoseUpdated(
        Pose pose,
        Matrix3x3? covariance,
        int constraintCount = 0,
        double constraintQuality = 0.0,
        double? mclReliability = null,
        double? mclMae = null)
    {
        // Publish pose snapshot (atomic volatile write)
        // For Relocalizing: use MCL reliability and MAE for score calculation
        var score = LocalizationScoreCalculator.Calculate(
            covariance,
            constraintCount,
            constraintQuality,
            mclReliability: mclReliability,
            mclMae: mclMae);

        // Determine drift status based on MCL reliability
        var driftStatus = DriftDetector.DriftStatus.Stable;
        if (mclReliability.HasValue)
        {
            if (mclReliability.Value < 0.3)
                driftStatus = DriftDetector.DriftStatus.Critical;
            else if (mclReliability.Value < 0.5)
                driftStatus = DriftDetector.DriftStatus.Warning;
        }

        Volatile.Write(ref _poseSnapshot, new PoseSnapshot(
            pose,
            covariance,
            score,
            scanMatchScore: null, // Not available during Relocalizing
            driftStatus));

        // Also update covariance fields for when pose thread takes over
        lock (_lock)
        {
            _poseCovariance = covariance;
            _constraintCount = constraintCount;
            _averageConstraintQuality = constraintQuality;
        }
    }

    /// <summary>
    /// Updates only confidence metrics (covariance, constraints) without touching pose.
    /// Used by sensor callback during ScanMapping/Localizing when background pose thread handles pose.
    /// </summary>
    private void UpdateConfidenceMetrics(Matrix3x3? covariance, int constraintCount, double constraintQuality)
    {
        lock (_lock)
        {
            _poseCovariance = covariance;
            _constraintCount = constraintCount;
            _averageConstraintQuality = constraintQuality;
        }
    }

    /// <summary>
    /// Updates confidence metrics including scan match score for Localizing state.
    /// Scan match score is critical for detecting map drift.
    /// </summary>
    private void UpdateConfidenceMetrics(Matrix3x3? covariance, int constraintCount, double constraintQuality, double scanMatchScore)
    {
        lock (_lock)
        {
            _poseCovariance = covariance;
            _constraintCount = constraintCount;
            _averageConstraintQuality = constraintQuality;
        }
        // Scan match score is stored via volatile write (already done before calling this method)
        // It will be read by background pose thread for LocalizationScore calculation
    }

    /// <summary>
    /// Transform raw payload points to global frame using the given pose.
    /// Used as fallback when CartographerSharp doesn't return SamplePointCloudGlobal.
    /// </summary>
    private static List<Vector3> TransformPayloadToGlobalFrame(RangeDataPayload payload, Pose pose)
    {
        var yaw = GetYawFromPose(pose);
        var cosYaw = Math.Cos(yaw);
        var sinYaw = Math.Sin(yaw);
        var poseX = pose.Position.X;
        var poseY = pose.Position.Y;

        var result = new List<Vector3>(payload.BaseLink.Ranges.Count);
        foreach (var range in payload.BaseLink.Ranges)
        {
            // Transform from base_link frame to global frame
            var localX = range.Position.X;
            var localY = range.Position.Y;
            var globalX = poseX + localX * cosYaw - localY * sinYaw;
            var globalY = poseY + localX * sinYaw + localY * cosYaw;
            result.Add(new Vector3(globalX, globalY, 0));
        }
        return result;
    }

    private static double GetYawFromPose(Pose p)
    {
        var q = p.Orientation;
        return Math.Atan2(2.0 * (q.W * q.Z + q.X * q.Y), 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z));
    }

    #endregion
}