namespace RobotNet10.RobotApp.SLAM.Cartographer;

/// <summary>
/// MCL configuration (from mcl.yaml). Used when SetInitialPoseAsync runs MCL before adding localization trajectory.
/// </summary>
public class MclConfiguration
{
    public bool Enabled { get; set; }
    public double InitialNoiseX { get; set; } = 1.0;
    public double InitialNoiseY { get; set; } = 1.0;
    public double InitialNoiseYaw { get; set; } = 0.3;
    /// <summary>When user provides initial pose (SetInitialPoseAsync), use these smaller noise values so particles stay near the pose and iteration 1 does not jump far. If zero, fall back to InitialNoiseX/Y/Yaw.</summary>
    public double InitialNoiseWhenPoseGivenX { get; set; } = 0.15;
    public double InitialNoiseWhenPoseGivenY { get; set; } = 0.15;
    public double InitialNoiseWhenPoseGivenYaw { get; set; } = 0.05;
    public int ParticlesNum { get; set; } = 1000;
    public bool UseAugmentedMcl { get; set; }
    public bool AddRandomParticlesInResampling { get; set; } = true;
    public double RandomParticlesRate { get; set; } = 0.1;
    public double[] RandomParticlesNoise { get; set; } = [0.05f, 0.05f, 0.1];
    public double[] OdomNoiseDdm { get; set; } = [1.0, 0.5, 0.5, 1.5];
    /// <summary>Odometry noise for omni-directional model (9 params: xx, xy, xyaw, yx, yy, yyaw, yawx, yawy, yawyaw). Used when UseOmniDirectionalModel=true.</summary>
    public double[] OdomNoiseOdm { get; set; } = [4.0, 1.0, 1.0, 1.0, 4.0, 1.0, 1.0, 1.0, 8.0];
    public bool UseOmniDirectionalModel { get; set; }
    public int MeasurementModelType { get; set; } = 0;
    /// <summary>Scan step for likelihood calculation (xloc: scanStep_ = 10). Use every Nth beam to avoid underflow with many beams. 1 = use all beams (slow, underflow risk), 10 = every 10th beam (recommended).</summary>
    public int ScanStep { get; set; } = 10;
    public double ZHit { get; set; } = 0.9;
    public double ZShort { get; set; } = 0.2;
    public double ZMax { get; set; } = 0.05;
    public double ZRand { get; set; } = 0.05;
    public double VarHit { get; set; } = 0.08;
    public double LambdaShort { get; set; } = 1.0;
    public double LambdaUnknown { get; set; } = 0.01;
    /// <summary>Prior probability for known obstacles (class-conditional model). Default: 0.5. xloc: pKnownPrior_.</summary>
    public double KnownClassPrior { get; set; } = 0.5;
    /// <summary>Computed: Prior probability for known obstacles (same as KnownClassPrior, for C# code compatibility).</summary>
    public double PKnownPrior => KnownClassPrior;
    /// <summary>Computed: Prior probability for unknown obstacles (1 - KnownClassPrior).</summary>
    public double PUnknownPrior => 1.0 - KnownClassPrior;
    public double UnknownScanProbThreshold { get; set; } = 0.9;
    public double AlphaSlow { get; set; } = 0.001;
    public double AlphaFast { get; set; } = 0.99;
    public bool RejectUnknownScan { get; set; } = true;
    public double ResampleThresholdEss { get; set; } = 0.5;
    public double[] ResampleThresholds { get; set; } = [0.2, 0.2, 0.2, 0.02f, -99999.0];
    /// <summary>Reliability transition decay for differential drive (2 params: dist_coeff, yaw_coeff). Used when UseOmniDirectionalModel=false.</summary>
    public double[] RelTransDdm { get; set; } = [0.0, 0.0];
    /// <summary>Reliability transition decay for omni-directional (3 params: x_coeff, y_coeff, yaw_coeff). Used when UseOmniDirectionalModel=true.</summary>
    public double[] RelTransOdm { get; set; } = [0.0, 0.0, 0.0];
    public int ClassifierType { get; set; }
    /// <summary>Estimate reliability per particle (MAE-based); used by decision model (xloc: estimateReliability_).</summary>
    public bool EstimateReliability { get; set; }
    /// <summary>MAE failure threshold in meters for simple decision model when classifier files not used (xloc: ~0.12).</summary>
    public double FailureThreshold { get; set; } = 0.12;
    /// <summary>Use global-localization pose sampler (merge external poses as extra particles).</summary>
    public bool UseGLPoseSampler { get; set; }
    /// <summary>Max time diff (sec) between scan and GL poses to fuse (xloc: glSampledPoseTimeTH_).</summary>
    public double GLSampledPoseTimeTH { get; set; } = 0.5;
    /// <summary>GMM positional variance for GL predictive likelihood (xloc: gmmPositionalVariance_).</summary>
    public double GmmPositionalVariance { get; set; } = 0.1;
    /// <summary>GMM angular variance for GL predictive likelihood (xloc: gmmAngularVariance_).</summary>
    public double GmmAngularVariance { get; set; } = 0.1;
    /// <summary>Uniform rate in predictive distribution for GL (xloc: predDistUnifRate_).</summary>
    public double PredDistUnifRate { get; set; } = 0.05;
    /// <summary>Pose change below this (meters) for convergence (xloc: pose_change_threshold 0.03).</summary>
    public double ConvergencePoseChangeThresholdMeters { get; set; } = 0.03;
    /// <summary>Yaw change below this (radians) for convergence (xloc: yaw_change_threshold 0.05).</summary>
    public double ConvergenceYawChangeThresholdRad { get; set; } = 0.05;
    /// <summary>Pose must stay stable for this duration (seconds) before convergence (xloc: stable_duration 0.3).</summary>
    public double ConvergenceStableDurationSeconds { get; set; } = 0.3;
    /// <summary>Min reliability [0,1] to allow pose-stable convergence (xloc: reliability_ >= 0.9).</summary>
    public double ConvergenceReliabilityMin { get; set; } = 0.9;
    /// <summary>Max MAE (meters) for best particle to allow pose-stable convergence (xloc: mae <= 0.12).</summary>
    public double ConvergenceMaeMaxMeters { get; set; } = 0.12;
    /// <summary>Timeout in seconds; after this, force convergence with initial pose (for Relocalizing state auto-resume fallback).</summary>
    public double ConvergenceTimeoutSeconds { get; set; } = 30.0;
    /// <summary>
    /// Max MCL iterations before forcing convergence.
    /// Note: C# does 5 iterations per scan. At 10Hz lidar = 50 iter/sec.
    /// 1500 iterations = 30 seconds at 10Hz, 60 seconds at 5Hz (matches ConvergenceTimeoutSeconds).
    /// </summary>
    public int ConvergenceMaxIterations { get; set; } = 1500;
    /// <summary>
    /// Min MCL iterations before allowing convergence (at least 2 scans at 10Hz).
    /// </summary>
    public int ConvergenceMinIterations { get; set; } = 10;
    /// <summary>
    /// DeviceId of the lidar used for MCL scan (xloc uses a single "scan" topic).
    /// If null or empty, the first lidar in Sensors.Lidars is used.
    /// When multiple lidars exist, only this lidar's range data is passed to MCL; others are ignored during MCL phase.
    /// </summary>
    public string? PrimaryLidarId { get; set; }

    /// <summary>
    /// MCL reliability monitoring configuration for Localizing state
    /// </summary>
    public MclReliabilityMonitoringConfiguration ReliabilityMonitoring { get; set; } = new();
}

/// <summary>
/// MCL reliability monitoring configuration for Localizing state
/// Runs MCL periodically (every 2 seconds by default) to provide reliability and MAE metrics
/// </summary>
public class MclReliabilityMonitoringConfiguration
{
    /// <summary>
    /// Enable MCL reliability monitoring during Localizing state
    /// </summary>
    public bool EnableReliabilityMonitoring { get; set; } = true;

    /// <summary>
    /// Interval between MCL updates (seconds)
    /// </summary>
    public double MonitoringIntervalSeconds { get; set; } = 2.0;

    /// <summary>
    /// Number of particles for monitoring (less than initialization for performance)
    /// Recommended: 500 (vs 1000 for initialization)
    /// </summary>
    public int MonitoringParticlesNum { get; set; } = 500;

    /// <summary>
    /// Number of MCL iterations per monitoring cycle
    /// </summary>
    public int MonitoringIterationsPerCycle { get; set; } = 5;
}