using Microsoft.Extensions.Options;
using RobotNet10.RobotApp.SLAM.Cartographer;
using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;
using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Mcl;

/// <summary>
/// MCL (Monte Carlo Localization) service. Port of xloc MCL for SetInitialPoseAsync flow.
/// Uses likelihood field measurement model (type 0), differential drive motion model, resampling.
/// </summary>
public class MclService(IOptions<CartographerConfiguration> config)
{
    #region Fields and Properties

    private readonly MclConfiguration _options = config.Value.Mcl;
    private readonly Random _rnd = new();

    // Map
    private double[,]? _distMap; // distance to nearest occupied (meters)
    private double _mapResolution;
    private double _mapOriginX, _mapOriginY, _mapOriginYaw;
    private int _mapWidth, _mapHeight;
    private bool _gotMap;

    // Scan
    private double _angleMin, _angleMax, _angleIncrement, _rangeMin, _rangeMax;
    private double[]? _ranges;
    private bool _gotScan;

    // Pose and particles
    private MclPose2d _mclPose;
    private MclPose2d _odomPose;
    private List<MclParticle> _particles = [];
    private double _deltaX, _deltaY, _deltaDist, _deltaYaw;
    private double _deltaXSum, _deltaYSum, _deltaDistSum, _deltaYawSum, _deltaTimeSum;

    // Measurement model constants (set when map is set)
    private double _normConstHit, _denomHit, _pRand, _measurementModelRandom, _measurementModelInvalidScan;

    // Base link to laser (for sensor pose). Simplified: use (0,0,0) if not set.
    private double _baseLink2LaserX, _baseLink2LaserY, _baseLink2LaserYaw;

    private bool _isInitialized = true;
    /// <summary>When true, ResetParticlesDistribution uses InitialNoiseWhenPoseGiven* so particles stay near user-provided initial pose.</summary>
    private bool _useTightInitialNoise;
    /// <summary>When true, force resample even when motion is zero (for global localization when robot is stationary).</summary>
    private bool _forceResampleForGlobalLocalization;

    // Likelihood and augmented MCL (xloc: totalLikelihood_, averageLikelihood_, omegaSlow_, omegaFast_, amclRandomParticlesRate_)
    private double _totalLikelihood, _averageLikelihood;
    private int _maxLikelihoodParticleIdx;
    private double _omegaSlow, _omegaFast, _amclRandomParticlesRate;

    // Reliability / decision model (xloc: estimateReliability_, reliabilities_, maes_, reliability_)
    private double[]? _reliabilities;
    private double[]? _maes;
    private double _reliability = 0.5;
    private const double MaxResidualError = 1.0;

    // GL pose sampler (xloc: glParticles_, glSampledPoses_, canUseGLSampledPoses_)
    private List<MclParticle> _glParticles = [];
    private List<MclPose2d> _glSampledPoses = [];
    private double _glSampledPosesStamp;
    private bool _canUseGLSampledPoses;
    private bool _isGLSampledPosesUpdated;
    private readonly List<bool> _likelihoodShiftedSteps = [];

    public bool IsReady => _gotMap && _gotScan;

    /// <summary>Current reliability [0,1] when EstimateReliability is true (xloc: reliability_).</summary>
    public double Reliability => _reliability;

    /// <summary>Total likelihood after measurement update (xloc: totalLikelihood_). Higher = better scan-map match.</summary>
    public double TotalLikelihood => _totalLikelihood;

    /// <summary>MAE in meters for the best particle when EstimateReliability is true (xloc: maes_[maxLikelihoodParticleIdx_]). Lower = better fit. Null if not available.</summary>
    public double? MaeForBestParticle
    {
        get
        {
            if (!_options.EstimateReliability || _maes == null || _maxLikelihoodParticleIdx < 0 || _maxLikelihoodParticleIdx >= _maes.Length)
                return null;
            return _maes[_maxLikelihoodParticleIdx];
        }
    }

    #endregion

    #region Map Setup

    public void SetMap(OccupancyGrid grid)
    {
        _mapWidth = grid.Width;
        _mapHeight = grid.Height;
        _mapResolution = grid.Resolution;
        _mapOriginX = grid.Origin.Position.X;
        _mapOriginY = grid.Origin.Position.Y;
        _mapOriginYaw = grid.Origin.Orientation.ToYawRadian();

        // Binary map: 0 = occupied (100), 1 = free
        var binMap = new byte[_mapHeight, _mapWidth];
        int occupiedCount = 0;
        for (int v = 0; v < _mapHeight; v++)
        {
            for (int u = 0; u < _mapWidth; u++)
            {
                int node = v * _mapWidth + u;
                int val = grid.Data[node];
                binMap[v, u] = (byte)(val == 100 ? 0 : 1);
                if (val == 100) occupiedCount++;
            }
        }

        // Euclidean distance transform (Felzenszwalb-Huttenlocher)
        _distMap = DistanceTransformHelper.ComputeEuclidean(binMap, _mapWidth, _mapHeight, _mapResolution);

        _gotMap = true;

        // Fixed parameters for measurement model (xloc MCL.cc). _rangeMax set when scan arrives.
        _normConstHit = 1.0 / Math.Sqrt(2.0 * _options.VarHit * Math.PI);
        _denomHit = 1.0 / (2.0 * _options.VarHit);
        double rangeMaxForRand = _rangeMax > 0 ? _rangeMax : 20.0;
        _pRand = 1.0 / (rangeMaxForRand / _mapResolution);
        _measurementModelRandom = _options.ZRand * _pRand;
        _measurementModelInvalidScan = _options.ZMax + _options.ZRand * _pRand;
    }

    #endregion

    #region Sensor Input

    /// <summary>Set base_link to laser transform (xloc: baseLink2Laser_). Call before RunOneIteration when sensor is offset.</summary>
    public void SetBaseLinkToLaser(double x, double y, double yawRad)
    {
        _baseLink2LaserX = x;
        _baseLink2LaserY = y;
        _baseLink2LaserYaw = yawRad;
    }

    /// <summary>Set global-localization sampled poses for this scan (xloc: glSampledPosesCB). Call when UseGLPoseSampler and poses available.</summary>
    public void SetGLSampledPoses(IReadOnlyList<MclPose2d> poses, double stampSec)
    {
        _glSampledPoses = [.. poses];
        _glSampledPosesStamp = stampSec;
        _isGLSampledPosesUpdated = true;
    }

    #endregion

    #region Public API

    public void SetInitialPose(Pose pose, bool useTightNoise = false)
    {
        double yaw = pose.Orientation.ToYawRadian();
        _mclPose = new MclPose2d(pose.Position.X, pose.Position.Y, yaw);
        _useTightInitialNoise = useTightNoise;
        // Enable forced resampling for global localization (when robot is stationary and searching for initial pose)
        // This ensures particles converge even without motion
        _forceResampleForGlobalLocalization = !useTightNoise;
        ResetParticlesDistribution();
        _odomPose = new MclPose2d(0, 0, 0);
        _deltaX = _deltaY = _deltaDist = _deltaYaw = 0;
        _isInitialized = true;
        if (_options.EstimateReliability)
            ResetReliabilities();
    }

    public void OnScan(double angleMin, double angleMax, double angleIncrement, double rangeMin, double rangeMax, IReadOnlyList<double> ranges)
    {
        _angleMin = angleMin;
        _angleMax = angleMax;
        _angleIncrement = angleIncrement;
        _rangeMin = rangeMin;
        _rangeMax = rangeMax;
        if (_ranges == null || _ranges.Length != ranges.Count)
            _ranges = new double[ranges.Count];
        for (int i = 0; i < ranges.Count; i++)
            _ranges[i] = ranges[i];
        _gotScan = true;
    }

    public void OnOdom(double deltaTimeSec, double linearX, double linearY, double angularZ)
    {
        if (_isInitialized) { _isInitialized = false; return; }
        if (deltaTimeSec <= 0) return;
        _deltaX += linearX * deltaTimeSec;
        _deltaY += linearY * deltaTimeSec;
        _deltaYaw += angularZ * deltaTimeSec;
        double dx = linearX * deltaTimeSec;
        double dy = linearY * deltaTimeSec;
        _deltaDist += Math.Sqrt(dx * dx + dy * dy);
        _deltaTimeSum += deltaTimeSec;
    }

    public Pose RunOneIteration()
    {
        if (!IsReady || _distMap == null || _ranges == null || _particles.Count == 0)
            return GetPose();

        UpdateParticlesByMotionModel();
        CalculateLikelihoodsByMeasurementModel();
        if (_options.EstimateReliability)
            CalculateLikelihoodsByDecisionModel();
        if (_options.UseGLPoseSampler  && _glSampledPoses.Count > 0)
            CalculateGLSampledPosesLikelihood();
        if (_options.UseAugmentedMcl)
            CalculateAMCLRandomParticlesRate();
        EstimatePose();
        ResampleParticles();
        return GetPose();
    }

    public Pose GetPose()
    {
        return new Pose
        {
            Position = new Vector3(_mclPose.X, _mclPose.Y, 0),
            Orientation = Quaternion.FromYawRadian(_mclPose.Yaw)
        };
    }

    public void ResetRunning() { }

    /// <summary>
    /// Disable forced resampling for global localization mode.
    /// Call this after MCL converges to switch to normal tracking mode.
    /// </summary>
    public void DisableGlobalLocalizationMode()
    {
        _forceResampleForGlobalLocalization = false;
    }

    #endregion

    #region Coordinate Helpers

    private void Xy2Uv(double x, double y, out int u, out int v)
    {
        double dx = x - _mapOriginX;
        double dy = y - _mapOriginY;
        double yaw = -_mapOriginYaw;
        double xx = dx * Math.Cos(yaw) - dy * Math.Sin(yaw);
        double yy = dx * Math.Sin(yaw) + dy * Math.Cos(yaw);
        u = (int)(xx / _mapResolution);
        v = (int)(yy / _mapResolution);
    }

    private bool OnMap(int u, int v) => u >= 0 && u < _mapWidth && v >= 0 && v < _mapHeight;

    #endregion

    #region Particle Distribution

    private double NRand(double sigma)
    {
        double u1 = _rnd.NextDouble();
        if (u1 < 1e-10) u1 = 1e-10;
        double u2 = _rnd.NextDouble();
        return sigma * Math.Sqrt(-2.0 * Math.Log(u1)) * Math.Cos(2.0 * Math.PI * u2);
    }

    private void ResetParticlesDistribution()
    {
        int n = _options.ParticlesNum;
        _particles = new List<MclParticle>(n);
        double xo = _mclPose.X, yo = _mclPose.Y, yawo = _mclPose.Yaw;
        double sigmaX = _useTightInitialNoise && _options.InitialNoiseWhenPoseGivenX > 0 ? _options.InitialNoiseWhenPoseGivenX : _options.InitialNoiseX;
        double sigmaY = _useTightInitialNoise && _options.InitialNoiseWhenPoseGivenY > 0 ? _options.InitialNoiseWhenPoseGivenY : _options.InitialNoiseY;
        double sigmaYaw = _useTightInitialNoise && _options.InitialNoiseWhenPoseGivenYaw > 0 ? _options.InitialNoiseWhenPoseGivenYaw : _options.InitialNoiseYaw;
        double wo = 1.0 / n;
        for (int i = 0; i < n; i++)
        {
            double x = xo + NRand(sigmaX);
            double y = yo + NRand(sigmaY);
            double yaw = yawo + NRand(sigmaYaw);
            _particles.Add(new MclParticle(x, y, yaw, wo));
        }
        if (_options.EstimateReliability)
            ResetReliabilities();
    }

    private void ResetReliabilities()
    {
        int n = _particles.Count;
        _reliabilities = new double[n];
        for (int i = 0; i < n; i++)
            _reliabilities[i] = 0.5;
        _maes = new double[n];
    }

    #endregion

    #region Motion Model

    private void UpdateParticlesByMotionModel()
    {
        double deltaX = _deltaX, deltaY = _deltaY, deltaDist = _deltaDist, deltaYaw = _deltaYaw;
        _deltaX = _deltaY = _deltaDist = _deltaYaw = 0;
        _deltaXSum += Math.Abs(deltaX);
        _deltaYSum += Math.Abs(deltaY);
        _deltaDistSum += Math.Abs(deltaDist);
        _deltaYawSum += Math.Abs(deltaYaw);

        if (!_options.UseOmniDirectionalModel)
        {
            // Differential drive model (xloc MCL.cc:341-368)
            double yaw = _mclPose.Yaw;
            double t = yaw + deltaYaw / 2.0;
            double x = _mclPose.X + deltaDist * Math.Cos(t);
            double y = _mclPose.Y + deltaDist * Math.Sin(t);
            yaw += deltaYaw;
            _mclPose = new MclPose2d(x, y, yaw);

            double dist2 = deltaDist * deltaDist;
            double yaw2 = deltaYaw * deltaYaw;
            double distRandVal = dist2 * _options.OdomNoiseDdm[0] + yaw2 * _options.OdomNoiseDdm[1];
            double yawRandVal = dist2 * _options.OdomNoiseDdm[2] + yaw2 * _options.OdomNoiseDdm[3];

            double[]? relTransDdm = _options.RelTransDdm;
            bool estimateReliability = _options.EstimateReliability && _reliabilities != null;

            for (int i = 0; i < _particles.Count; i++)
            {
                var p = _particles[i];
                double ddist = deltaDist + NRand(distRandVal);
                double dyaw = deltaYaw + NRand(yawRandVal);
                yaw = p.Pose.Yaw;
                t = yaw + dyaw / 2.0;
                x = p.Pose.X + ddist * Math.Cos(t);
                y = p.Pose.Y + ddist * Math.Sin(t);
                yaw += dyaw;
                p.Pose = new MclPose2d(x, y, yaw);
                p.W = 1.0 / _particles.Count;

                if (estimateReliability && relTransDdm != null && relTransDdm.Length >= 2)
                {
                    double decayRate = 1.0 - (relTransDdm[0] * ddist * ddist + relTransDdm[1] * dyaw * dyaw);
                    if (decayRate <= 0.0) decayRate = 1e-6;
                    _reliabilities![i] *= decayRate;
                }
            }
        }
        else
        {
            // Omni-directional model (xloc MCL.cc:369-400)
            double yaw = _mclPose.Yaw;
            double t = yaw + deltaYaw / 2.0;
            double x = _mclPose.X + deltaX * Math.Cos(t) + deltaY * Math.Cos(t + Math.PI / 2.0);
            double y = _mclPose.Y + deltaX * Math.Sin(t) + deltaY * Math.Sin(t + Math.PI / 2.0);
            yaw += deltaYaw;
            _mclPose = new MclPose2d(x, y, yaw);

            double x2 = deltaX * deltaX;
            double y2 = deltaY * deltaY;
            double yaw2 = deltaYaw * deltaYaw;
            double xRandVal = x2 * _options.OdomNoiseOdm[0] + y2 * _options.OdomNoiseOdm[1] + yaw2 * _options.OdomNoiseOdm[2];
            double yRandVal = x2 * _options.OdomNoiseOdm[3] + y2 * _options.OdomNoiseOdm[4] + yaw2 * _options.OdomNoiseOdm[5];
            double yawRandVal = x2 * _options.OdomNoiseOdm[6] + y2 * _options.OdomNoiseOdm[7] + yaw2 * _options.OdomNoiseOdm[8];

            double[]? relTransOdm = _options.RelTransOdm;
            bool estimateReliability = _options.EstimateReliability && _reliabilities != null;

            for (int i = 0; i < _particles.Count; i++)
            {
                var p = _particles[i];
                double dx = deltaX + NRand(xRandVal);
                double dy = deltaY + NRand(yRandVal);
                double dyaw = deltaYaw + NRand(yawRandVal);
                yaw = p.Pose.Yaw;
                t = yaw + dyaw / 2.0;
                x = p.Pose.X + dx * Math.Cos(t) + dy * Math.Cos(t + Math.PI / 2.0);
                y = p.Pose.Y + dx * Math.Sin(t) + dy * Math.Sin(t + Math.PI / 2.0);
                yaw += dyaw;
                p.Pose = new MclPose2d(x, y, yaw);
                p.W = 1.0 / _particles.Count;

                if (estimateReliability && relTransOdm != null && relTransOdm.Length >= 3)
                {
                    double decayRate = 1.0 - (relTransOdm[0] * dx * dx + relTransOdm[1] * dy * dy + relTransOdm[2] * dyaw * dyaw);
                    if (decayRate <= 0.0) decayRate = 1e-6;
                    _reliabilities![i] *= decayRate;
                }
            }
        }
    }

    #endregion

    #region Unknown Scan Rejection

    /// <summary>Reject beams likely "unknown" (xloc rejectUnknownScan). Minimal port: ray-cast from MCL pose, zero ranges beyond expected.</summary>
    private void RejectUnknownScan()
    {
        if (_ranges == null || _distMap == null) return;
        // _ranges is already binned (numBins elements from ConvertToScan); use every beam (step 1).
        double xo = _baseLink2LaserX, yo = _baseLink2LaserY, yawo = _baseLink2LaserYaw;
        double yaw = _mclPose.Yaw;
        double sensorX = xo * Math.Cos(yaw) - yo * Math.Sin(yaw) + _mclPose.X;
        double sensorY = xo * Math.Sin(yaw) + yo * Math.Cos(yaw) + _mclPose.Y;
        double sensorYaw = yawo + yaw;
        double hitThreshold = 0.5 * _mapResolution;
        for (int i = 0; i < _ranges.Length; i++)
        {
            double r = _ranges[i];
            if (r <= _rangeMin || r >= _rangeMax) continue;
            double angle = i * _angleIncrement + _angleMin + sensorYaw;
            double dx = _mapResolution * Math.Cos(angle);
            double dy = _mapResolution * Math.Sin(angle);
            double x = sensorX, y = sensorY;
            double expectedRange = -1;
            for (double rangeStep = 0; rangeStep <= _rangeMax; rangeStep += _mapResolution)
            {
                Xy2Uv(x, y, out int u, out int v);
                if (!OnMap(u, v)) break;
                double dist = _distMap[v, u];
                if (dist < hitThreshold) { expectedRange = rangeStep; break; }
                x += dx; y += dy;
            }
            if (expectedRange >= 0 && r > expectedRange + hitThreshold)
            {
                double pShort = _options.LambdaShort * Math.Exp(-_options.LambdaShort * r) / (1.0 - Math.Exp(-_options.LambdaShort * _rangeMax)) * _mapResolution;
                double pBeam = _measurementModelRandom;
                if (pShort / (pShort + pBeam) >= _options.UnknownScanProbThreshold)
                    _ranges[i] = 0;
            }
        }
    }

    /// <summary>Estimate unknown scan using class-conditional measurement model (xloc MCL.cc:1599-1630). For measurement model type 2.</summary>
    private double[] EstimateUnknownScanWithClassConditionalMeasurementModel(MclPose2d pose)
    {
        if (_ranges == null || _distMap == null) return _ranges ?? [];

        double[] unknownScanRanges = new double[_ranges.Length];
        Array.Copy(_ranges, unknownScanRanges, _ranges.Length);

        double xo = _baseLink2LaserX, yo = _baseLink2LaserY, yawo = _baseLink2LaserYaw;
        double yaw = pose.Yaw;
        double sensorX = xo * Math.Cos(yaw) - yo * Math.Sin(yaw) + pose.X;
        double sensorY = xo * Math.Sin(yaw) + yo * Math.Cos(yaw) + pose.Y;
        double sensorYaw = yawo + yaw;

        for (int i = 0; i < unknownScanRanges.Length; i++)
        {
            double r = unknownScanRanges[i];
            if (r <= _rangeMin || r >= _rangeMax)
            {
                unknownScanRanges[i] = 0;
                continue;
            }

            double t = sensorYaw + i * _angleIncrement + _angleMin;
            double x = r * Math.Cos(t) + sensorX;
            double y = r * Math.Sin(t) + sensorY;

            Xy2Uv(x, y, out int u, out int v);
            double pKnown;
            double pUnknown = _options.LambdaUnknown * Math.Exp(-_options.LambdaUnknown * r) / (1.0 - Math.Exp(-_options.LambdaUnknown * _rangeMax)) * _mapResolution * _options.PUnknownPrior;

            if (OnMap(u, v))
            {
                double dist = _distMap[v, u];
                double pHit = _normConstHit * Math.Exp(-(dist * dist) * _denomHit) * _mapResolution;
                pKnown = (_options.ZHit * pHit + _measurementModelRandom) * _options.PKnownPrior;
            }
            else
            {
                pKnown = _measurementModelRandom * _options.PKnownPrior;
            }

            double sum = pKnown + pUnknown;
            if (sum > 0)
                pUnknown /= sum;

            if (pUnknown < _options.UnknownScanProbThreshold)
                unknownScanRanges[i] = 0;
        }

        return unknownScanRanges;
    }

    #endregion

    #region Measurement Models

    private double CalculateLikelihoodFieldModel(MclPose2d pose, double range, double rangeAngle)
    {
        if (range <= _rangeMin || range >= _rangeMax) return _measurementModelInvalidScan;
        double t = pose.Yaw + rangeAngle;
        double x = range * Math.Cos(t) + pose.X;
        double y = range * Math.Sin(t) + pose.Y;
        Xy2Uv(x, y, out int u, out int v);
        if (!OnMap(u, v)) return _measurementModelRandom;
        double dist = _distMap![v, u];
        double pHit = _normConstHit * Math.Exp(-(dist * dist) * _denomHit) * _mapResolution;
        double p = _options.ZHit * pHit + _measurementModelRandom;
        return Math.Min(p, 1.0);
    }

    /// <summary>Beam model: ray-cast to find expected range, compare with observed (xloc MCL.cc:1535-1574).</summary>
    private double CalculateBeamModel(MclPose2d pose, double range, double rangeAngle)
    {
        if (range <= _rangeMin || range >= _rangeMax) return _measurementModelInvalidScan;

        double t = pose.Yaw + rangeAngle;
        double x = pose.X;
        double y = pose.Y;
        double dx = _mapResolution * Math.Cos(t);
        double dy = _mapResolution * Math.Sin(t);
        double expectedRange = -1.0;
        double hitThreshold = 0.5 * _mapResolution;

        // Ray-cast to find expected range (distance to first occupied cell)
        for (double r = 0.0; r < _rangeMax; r += _mapResolution)
        {
            Xy2Uv(x, y, out int u, out int v);
            if (!OnMap(u, v)) break;
            double dist = _distMap![v, u];
            if (dist < hitThreshold)
            {
                expectedRange = r;
                break;
            }
            x += dx;
            y += dy;
        }

        double p;
        if (range <= expectedRange)
        {
            // Observed range is shorter than expected: hit or short
            double error = expectedRange - range;
            double pHit = _normConstHit * Math.Exp(-(error * error) * _denomHit) * _mapResolution;
            double pShort = _options.LambdaShort * Math.Exp(-_options.LambdaShort * range) / (1.0 - Math.Exp(-_options.LambdaShort * _rangeMax)) * _mapResolution;
            p = _options.ZHit * pHit + _options.ZShort * pShort + _measurementModelRandom;
        }
        else
        {
            // Observed range is longer than expected: random
            p = _measurementModelRandom;
        }
        return Math.Min(p, 1.0);
    }

    /// <summary>Class-conditional measurement model: probabilistic model for known vs unknown obstacles (xloc MCL.cc:1576-1597).</summary>
    private double CalculateClassConditionalMeasurementModel(MclPose2d pose, double range, double rangeAngle)
    {
        if (range <= _rangeMin || range >= _rangeMax) return _measurementModelInvalidScan;

        double t = pose.Yaw + rangeAngle;
        double x = range * Math.Cos(t) + pose.X;
        double y = range * Math.Sin(t) + pose.Y;

        // Unknown class: exponential decay with unknown prior
        double pUnknown = _options.LambdaUnknown * Math.Exp(-_options.LambdaUnknown * range) / (1.0 - Math.Exp(-_options.LambdaUnknown * _rangeMax)) * _mapResolution * _options.PUnknownPrior;

        Xy2Uv(x, y, out int u, out int v);
        double p = pUnknown;
        if (OnMap(u, v))
        {
            // Known class: likelihood field + random
            double dist = _distMap![v, u];
            double pHit = _normConstHit * Math.Exp(-(dist * dist) * _denomHit) * _mapResolution;
            p += (_options.ZHit * pHit + _measurementModelRandom) * _options.PKnownPrior;
        }
        else
        {
            // Off map: known random
            p += _measurementModelRandom * _options.PKnownPrior;
        }
        return Math.Min(p, 1.0);
    }

    private void CalculateLikelihoodsByMeasurementModel()
    {
        if (_distMap == null || _ranges == null)
        {
            return;
        }

        int scanStep = _options.ScanStep > 0 ? _options.ScanStep : 10;

        // Count valid ranges
        int validRanges = 0;
        for (int i = 0; i < _ranges.Length; i++)
        {
            if (_ranges[i] > _rangeMin && _ranges[i] < _rangeMax)
                validRanges++;
        }
        
        if (_options.RejectUnknownScan && (_options.MeasurementModelType == 0 || _options.MeasurementModelType == 1))
            RejectUnknownScan();

        // Use scanStep like xloc (xloc uses scanStep=10 by default to avoid underflow with many beams)
        double xo = _baseLink2LaserX, yo = _baseLink2LaserY, yawo = _baseLink2LaserYaw;

        _likelihoodShiftedSteps.Clear();
        for (int i = 0; i < _particles.Count; i++)
            _particles[i].W = 0;

        for (int i = 0; i < _ranges.Length; i += scanStep)
        {
            double r = _ranges[i];
            double rangeAngle = i * _angleIncrement + _angleMin; // beam angle in scan frame (relative to sensor)
            double stepMax = double.MinValue;
            for (int j = 0; j < _particles.Count; j++)
            {
                double yaw = _particles[j].Pose.Yaw;
                double sensorX = xo * Math.Cos(yaw) - yo * Math.Sin(yaw) + _particles[j].Pose.X;
                double sensorY = xo * Math.Sin(yaw) + yo * Math.Cos(yaw) + _particles[j].Pose.Y;
                double sensorYaw = yawo + yaw;
                var sensorPose = new MclPose2d(sensorX, sensorY, sensorYaw);
                double p = _options.MeasurementModelType switch
                {
                    0 => CalculateLikelihoodFieldModel(sensorPose, r, rangeAngle),
                    1 => CalculateBeamModel(sensorPose, r, rangeAngle),
                    2 => CalculateClassConditionalMeasurementModel(sensorPose, r, rangeAngle),
                    _ => CalculateLikelihoodFieldModel(sensorPose, r, rangeAngle)
                };

                double w = _particles[j].W + Math.Log(p);
                _particles[j].W = w;
                if (j == 0 || w > stepMax) stepMax = w;
            }
            // xloc: shift log weights if max < -300 to avoid underflow when converting to exp
            bool shifted = stepMax < -300.0;
            _likelihoodShiftedSteps.Add(shifted);
            if (shifted)
            {
                for (int j = 0; j < _particles.Count; j++)
                    _particles[j].W += 300.0;
            }
        }

        double sum = 0, max = double.MinValue;
        int maxIdx = 0;
        for (int i = 0; i < _particles.Count; i++)
        {
            double w = Math.Exp(_particles[i].W);
            _particles[i].W = w;
            sum += w;
            if (w > max) { max = w; maxIdx = i; }
        }
        _totalLikelihood = sum;
        _averageLikelihood = _particles.Count > 0 ? sum / _particles.Count : 0;
        _maxLikelihoodParticleIdx = maxIdx;
    }

    /// <summary>Residual errors: distance from each beam endpoint to nearest occupied (xloc getResidualErrors). -1 if invalid.</summary>
    private List<double> GetResidualErrors(MclPose2d pose, int scanStep = 1)
    {
        if (_distMap == null || _ranges == null) return [];
        double xo = _baseLink2LaserX, yo = _baseLink2LaserY, yawo = _baseLink2LaserYaw;
        double yaw = pose.Yaw;
        double sensorX = xo * Math.Cos(yaw) - yo * Math.Sin(yaw) + pose.X;
        double sensorY = xo * Math.Sin(yaw) + yo * Math.Cos(yaw) + pose.Y;
        double sensorYaw = yawo + yaw;
        int estimatedCount = (_ranges.Length + scanStep - 1) / scanStep;
        var errors = new List<double>(estimatedCount);
        for (int i = 0; i < _ranges.Length; i += scanStep)
        {
            double r = _ranges[i];
            if (r <= _rangeMin || r >= _rangeMax) { errors.Add(-1.0); continue; }
            double t = i * _angleIncrement + _angleMin + sensorYaw;
            double x = r * Math.Cos(t) + sensorX;
            double y = r * Math.Sin(t) + sensorY;
            Xy2Uv(x, y, out int u, out int v);
            if (!OnMap(u, v)) { errors.Add(-1.0); continue; }
            errors.Add(_distMap[v, u]);
        }
        return errors;
    }

    /// <summary>MAE over valid residual errors in [0, MaxResidualError] (xloc MAEClassifier::getMAE).</summary>
    private static double GetMAE(List<double> residualErrors, double maxResidualError = MaxResidualError)
    {
        double sum = 0;
        int num = 0;
        foreach (double e in residualErrors)
        {
            if (e >= 0 && e <= maxResidualError) { sum += e; num++; }
        }
        return num == 0 ? 0.0 : sum / num;
    }

    #endregion

    #region Decision Model and Reliability

    /// <summary>Simple decision model: decay by MAE vs failure threshold; update reliability (xloc calculateDecisionModel when no histogram files).</summary>
    private double CalculateDecisionModelSimple(double mae, ref double reliability)
    {
        double th = _options.FailureThreshold;
        if (th <= 0) th = 0.12;
        double decisionLikelihood = Math.Exp(-mae / th);
        double rel = decisionLikelihood * reliability;
        double relInv = (1.0 - decisionLikelihood) * (1.0 - reliability);
        double p = rel + relInv;
        if (p < 1e-9) p = 1e-9;
        reliability = Math.Clamp(rel / p, 0.0001, 0.9999);
        return Math.Min(p, 1.0);
    }

    private void CalculateLikelihoodsByDecisionModel()
    {
        if (_reliabilities == null || _maes == null || _totalLikelihood <= 0) return;

        // Estimate unknown scan if using class-conditional model (xloc MCL.cc:491-494)
        if (_options.MeasurementModelType == 2 && _maxLikelihoodParticleIdx >= 0 && _maxLikelihoodParticleIdx < _particles.Count)
        {
            var mlPose = _particles[_maxLikelihoodParticleIdx].Pose;
            _ = EstimateUnknownScanWithClassConditionalMeasurementModel(mlPose);
        }

        int scanStep = _options.ScanStep > 0 ? _options.ScanStep : 10;
        int n = _particles.Count;
        double sum = 0;
        double max = 0;
        int maxIdx = 0;
        for (int i = 0; i < n; i++)
        {
            var residualErrors = GetResidualErrors(_particles[i].Pose, scanStep);
            double mae = GetMAE(residualErrors);
            _maes[i] = mae;
            double rel = _reliabilities[i];
            double decisionLikelihood = CalculateDecisionModelSimple(mae, ref rel);
            _reliabilities[i] = rel;
            double w = _particles[i].W * decisionLikelihood;
            _particles[i].W = w;
            sum += w;
            if (w > max) { max = w; maxIdx = i; }
        }
        _totalLikelihood = sum;
        _averageLikelihood = n > 0 ? sum / n : 0;
        _maxLikelihoodParticleIdx = maxIdx;
        _reliability = _reliabilities[maxIdx];
    }

    private void CalculateAMCLRandomParticlesRate()
    {
        _omegaSlow += _options.AlphaSlow * (_averageLikelihood - _omegaSlow);
        _omegaFast += _options.AlphaFast * (_averageLikelihood - _omegaFast);
        _amclRandomParticlesRate = _omegaSlow > 1e-10 ? Math.Max(0, 1.0 - _omegaFast / _omegaSlow) : 0;
    }

    #endregion

    #region GL Sampled Poses

    private void CalculateGLSampledPosesLikelihood()
    {
        if (_glSampledPoses.Count == 0 || !_isGLSampledPosesUpdated) return;
        double stampSec = 0; // we don't have scan stamp in C#; use 0 so time check is relaxed if needed
        if (Math.Abs(stampSec - _glSampledPosesStamp) > _options.GLSampledPoseTimeTH) return;

        _isGLSampledPosesUpdated = false;
        int glNum = _glSampledPoses.Count;
        _glParticles = new List<MclParticle>(glNum);
        double xo = _baseLink2LaserX, yo = _baseLink2LaserY, yawo = _baseLink2LaserYaw;
        int scanStep = _options.ScanStep > 0 ? _options.ScanStep : 10;

        for (int i = 0; i < glNum; i++)
        {
            var pose = _glSampledPoses[i];
            _glParticles.Add(new MclParticle(pose.X, pose.Y, pose.Yaw, 0));
        }

        for (int i = 0; i < _ranges!.Length; i += scanStep)
        {
            double r = _ranges[i];
            double rangeAngle = i * _angleIncrement + _angleMin;
            for (int j = 0; j < glNum; j++)
            {
                var p = _glParticles[j];
                double yaw = p.Pose.Yaw;
                double sensorX = xo * Math.Cos(yaw) - yo * Math.Sin(yaw) + p.Pose.X;
                double sensorY = xo * Math.Sin(yaw) + yo * Math.Cos(yaw) + p.Pose.Y;
                double sensorYaw = yawo + yaw;
                var sensorPose = new MclPose2d(sensorX, sensorY, sensorYaw);
                double prob = CalculateLikelihoodFieldModel(sensorPose, r, rangeAngle);
                _glParticles[j] = new MclParticle(p.Pose, p.W + Math.Log(prob));
            }
            if (i < _likelihoodShiftedSteps.Count && _likelihoodShiftedSteps[i])
            {
                for (int j = 0; j < glNum; j++)
                {
                    var p = _glParticles[j];
                    _glParticles[j] = new MclParticle(p.Pose, p.W + 300.0);
                }
            }
        }

        double gmmPosVar = _options.GmmPositionalVariance;
        double gmmAngVar = _options.GmmAngularVariance;
        double normConst = 1.0 / Math.Sqrt(2.0 * Math.PI * (gmmPosVar + gmmPosVar + gmmAngVar));
        double angleRes = Math.PI / 180.0;
        double gmmRate = 1.0 - _options.PredDistUnifRate;
        int mainN = _particles.Count;
        double sum = _totalLikelihood;
        double max = 0;
        int maxIdx = -1;

        for (int i = 0; i < glNum; i++)
        {
            double w = Math.Exp(_glParticles[i].W);
            double gmmVal = 0;
            var glPose = _glParticles[i].Pose;
            for (int j = 0; j < mainN; j++)
            {
                double dx = glPose.X - _particles[j].Pose.X;
                double dy = glPose.Y - _particles[j].Pose.Y;
                double dyaw = MclPose2d.NormalizeYaw(glPose.Yaw - _particles[j].Pose.Yaw);
                gmmVal += normConst * Math.Exp(-((dx * dx) / (2 * gmmPosVar) + (dy * dy) / (2 * gmmPosVar) + (dyaw * dyaw) / (2 * gmmAngVar)));
            }
            double pGmm = glNum * gmmVal * _mapResolution * _mapResolution * angleRes / mainN;
            double predLikelihood = gmmRate * pGmm + _options.PredDistUnifRate * 1e-8;
            w *= predLikelihood;
            w = Math.Min(w, 1.0);
            _glParticles[i] = new MclParticle(_glParticles[i].Pose, w);
            sum += w;
            if (w > max) { max = w; maxIdx = i; }
        }

        if (double.IsNaN(sum))
        {
            _canUseGLSampledPoses = false;
            return;
        }
        _totalLikelihood = sum;
        _averageLikelihood = (mainN + glNum) > 0 ? sum / (mainN + glNum) : 0;
        if (maxIdx >= 0)
        {
            _maxLikelihoodParticleIdx = mainN + maxIdx;
        }
        _canUseGLSampledPoses = true;
    }

    #endregion

    #region Pose Estimation

    /// <summary>
    /// Weighted mean pose (xloc estimatePose). Weights must be used with sum for correct average when not normalized.
    /// When sum is too small (underflow) or result is non-finite, keep previous pose or use max-likelihood particle to avoid (0,0) collapse.
    /// </summary>
    private void EstimatePose()
    {
        double tmpYaw = _mclPose.Yaw;
        double x = 0, y = 0, yaw = 0, sum = 0;
        for (int i = 0; i < _particles.Count; i++)
        {
            double w = _particles[i].W;
            x += _particles[i].Pose.X * w;
            y += _particles[i].Pose.Y * w;
            double dyaw = tmpYaw - _particles[i].Pose.Yaw;
            dyaw = MclPose2d.NormalizeYaw(dyaw);
            yaw += dyaw * w;
            sum += w;
        }
        double x2 = x, y2 = y, yaw2 = yaw;
        if (_canUseGLSampledPoses && _glParticles.Count > 0)
        {
            for (int i = 0; i < _glParticles.Count; i++)
            {
                double w = _glParticles[i].W;
                x += _glParticles[i].Pose.X * w;
                y += _glParticles[i].Pose.Y * w;
                double dyaw = tmpYaw - _glParticles[i].Pose.Yaw;
                dyaw = MclPose2d.NormalizeYaw(dyaw);
                yaw += dyaw * w;
                sum += w;
            }
            if (sum > 1.0)
            {
                x = x2; y = y2; yaw = yaw2;
            }
        }
        if (sum > 1e-10 && double.IsFinite(sum))
        {
            x /= sum;
            y /= sum;
            yaw = tmpYaw - (yaw / sum);
            if (double.IsFinite(x) && double.IsFinite(y) && double.IsFinite(yaw))
            {
                // Reject estimate if it jumps too far (avoids garbage from degenerate likelihood / wrong frame)
                const double maxPoseJumpMeters = 50.0;
                double dx = x - _mclPose.X;
                double dy = y - _mclPose.Y;
                double dist = Math.Sqrt(dx * dx + dy * dy);
                if (dx * dx + dy * dy <= maxPoseJumpMeters * maxPoseJumpMeters)
                {
                    _mclPose = new MclPose2d(x, y, yaw);
                }
                return;
            }
        }
        // Underflow or non-finite: use max-likelihood particle to avoid (0,0) or garbage
        if (_maxLikelihoodParticleIdx >= 0)
        {
            if (_maxLikelihoodParticleIdx < _particles.Count)
            {
                var best = _particles[_maxLikelihoodParticleIdx].Pose;
                _mclPose = new MclPose2d(best.X, best.Y, best.Yaw);
            }
            else if (_canUseGLSampledPoses && _glParticles.Count > 0)
            {
                int glIdx = _maxLikelihoodParticleIdx - _particles.Count;
                if (glIdx >= 0 && glIdx < _glParticles.Count)
                {
                    var best = _glParticles[glIdx].Pose;
                    _mclPose = new MclPose2d(best.X, best.Y, best.Yaw);
                }
            }
        }
    }

    #endregion

    #region Resampling

    /// <summary>
    /// Resample particles (xloc resampleParticles). Skip if ESS above threshold or motion below thresholds.
    /// Supports GL particles and random particles (addRandomParticlesInResampling / augmented MCL).
    /// </summary>
    private void ResampleParticles()
    {
        int mainN = _particles.Count;
        int totalN = mainN;
        if (_canUseGLSampledPoses && _glParticles.Count > 0)
            totalN = mainN + _glParticles.Count;

        double threshold = totalN * _options.ResampleThresholdEss;
        double sumW = _totalLikelihood;
        if (sumW > 1e-10)
        {
            double sumW2 = 0;
            for (int i = 0; i < mainN; i++) sumW2 += _particles[i].W * _particles[i].W;
            if (_canUseGLSampledPoses && _glParticles.Count > 0)
                for (int i = 0; i < _glParticles.Count; i++) sumW2 += _glParticles[i].W * _glParticles[i].W;
            double effectiveSampleSize = (sumW * sumW) / sumW2;
            if (effectiveSampleSize > threshold)
                return;
        }
        // Skip motion threshold check when in global localization mode (robot stationary, searching for pose)
        // This allows particles to converge even without motion
        if (!_forceResampleForGlobalLocalization)
        {
            if (_deltaXSum < _options.ResampleThresholds[0] && _deltaYSum < _options.ResampleThresholds[1] &&
                _deltaDistSum < _options.ResampleThresholds[2] && _deltaYawSum < _options.ResampleThresholds[3] &&
                _deltaTimeSum < _options.ResampleThresholds[4])
            {
                return;
            }
        }

        _deltaXSum = _deltaYSum = _deltaDistSum = _deltaYawSum = _deltaTimeSum = 0;

        if (sumW <= 0) return;

        var wBuffer = new double[totalN];
        wBuffer[0] = _particles[0].W / sumW;
        for (int i = 1; i < mainN; i++)
            wBuffer[i] = wBuffer[i - 1] + _particles[i].W / sumW;
        if (_canUseGLSampledPoses && _glParticles.Count > 0)
        {
            for (int i = 0; i < _glParticles.Count; i++)
                wBuffer[mainN + i] = (mainN + i == 0 ? 0 : wBuffer[mainN + i - 1]) + _glParticles[i].W / sumW;
        }

        var tmpMain = _particles.Select(p => new MclParticle(p.Pose, p.W)).ToList();
        double[]? tmpRel = _reliabilities != null ? (double[])_reliabilities.Clone() : null;
        double wo = 1.0 / mainN;

        bool useRandomParticles = _options.AddRandomParticlesInResampling || _options.UseAugmentedMcl;
        double randomRate = _options.AddRandomParticlesInResampling ? _options.RandomParticlesRate : 0;
        if (_options.UseAugmentedMcl && _amclRandomParticlesRate > 0)
        {
            randomRate = _amclRandomParticlesRate;
            _omegaSlow = _omegaFast = 0;
        }
        else if (!_options.AddRandomParticlesInResampling)
            randomRate = 0;

        int resampledNum = (int)(useRandomParticles && randomRate > 0
            ? Math.Max(0, (int)((1.0 - randomRate) * mainN))
            : mainN);
        int randomNum = mainN - resampledNum;

        double xo = _mclPose.X, yo = _mclPose.Y, yawo = _mclPose.Yaw;
        var noise = _options.RandomParticlesNoise ?? [0.05, 0.05, 0.1];
        if (noise.Length < 3) noise = [0.05, 0.05, 0.1];
        
        for (int i = 0; i < resampledNum; i++)
        {
            double darts = _rnd.NextSingle();
            int j = Array.BinarySearch(wBuffer, 0, totalN, darts);
            if (j < 0) j = ~j; // BinarySearch returns bitwise complement of next larger index
            if (j >= totalN) j = totalN - 1;

            if (j < mainN)
            {
                _particles[i].Pose = tmpMain[j].Pose;
                if (tmpRel != null && i < _reliabilities!.Length && j < tmpRel.Length)
                    _reliabilities[i] = tmpRel[j];
            }
            else if (_canUseGLSampledPoses && j - mainN < _glParticles.Count)
            {
                _particles[i].Pose = _glParticles[j - mainN].Pose;
            }
            _particles[i].W = wo;
        }
        for (int i = resampledNum; i < resampledNum + randomNum; i++)
        {
            double x = xo + NRand(noise[0]);
            double y = yo + NRand(noise[1]);
            double yaw = yawo + NRand(noise[2]);
            _particles[i].Pose = new MclPose2d(x, y, yaw);
            _particles[i].W = wo;
            if (_options.EstimateReliability && _reliabilities != null && i < _reliabilities.Length)
                _reliabilities[i] = _reliability;
        }

        _canUseGLSampledPoses = false;
    }

    #endregion
}