BQP/srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/Configs.cs

namespace RobotNet10.NavigationTune.Shared.Models;

/// <summary>
/// PID Controller configuration
/// </summary>
public class PIDConfig
{
    public double Kp { get; set; }
    public double Ki { get; set; }
    public double Kd { get; set; }
    /// <summary>
    /// Integral chỉ tích lũy khi |error| &lt;= IntegralZone.
    /// Giá trị 0 = không giới hạn (integral luôn tích lũy).
    /// </summary>
    public double IntegralZone { get; set; }
}

/// <summary>
/// Motor Dynamics configuration
/// </summary>
public class MotorDynamicsConfig
{
    /// <summary>
    /// Time constant (τ) - thời gian để motor đạt 63.2% của target velocity
    /// Đơn vị: giây (s)
    /// Typical: 0.1 - 0.5s cho DC motor với driver PID
    /// </summary>
    public double Tau { get; set; }

    /// <summary>
    /// Pure delay (δ) - độ trễ trước khi motor bắt đầu phản ứng
    /// Đơn vị: giây (s)
    /// Bao gồm: communication delay + driver processing
    /// Typical: 0.02 - 0.1s
    /// </summary>
    public double Delta { get; set; }
}

/// <summary>
/// Stanley Controller configuration
/// Path tracking using cross-track error and heading error
/// </summary>
public class StanleyConfig
{
    #region Core Stanley Parameters

    /// <summary>
    /// Cross-track error gain (K)
    /// Default: 2.5
    ///
    /// Meaning: How aggressively to correct lateral position error
    /// Formula: δ = ψ + arctan(K × e / (v + Ks))
    ///
    /// ↑ Increase (3.0-5.0):
    ///   ✓ Faster correction of cross-track error
    ///   ✓ Tighter path following
    ///   ✗ May cause oscillation
    ///   ✗ Less smooth on noisy paths
    ///
    /// ↓ Decrease (1.5-2.0):
    ///   ✓ Smoother motion
    ///   ✓ Less oscillation
    ///   ✗ Slower error correction
    ///   ✗ Larger cross-track error
    ///
    /// Tuning Tips:
    /// - Start: 2.5 for general use
    /// - High precision: 3.0-4.0
    /// - Smooth priority: 1.5-2.0
    /// - Check stability by observing steering oscillation
    /// </summary>
    public double K { get; set; } = 2.5;

    /// <summary>
    /// Softening constant (Ks) - meters/second
    /// Default: 0.1 m/s
    ///
    /// Meaning: Added to velocity denominator to prevent division by zero at low speeds
    /// Formula: δ = ψ + arctan(K × e / (v + Ks))
    ///
    /// ↑ Increase (0.15-0.2):
    ///   ✓ Less aggressive correction at low speed
    ///   ✓ Smoother motion when starting
    ///   ✗ Slower error correction at low speed
    ///
    /// ↓ Decrease (0.05-0.08):
    ///   ✓ More responsive at low speed
    ///   ✗ May cause oscillation when slow
    ///   ✗ Risk of instability near zero velocity
    ///
    /// Tuning Tips:
    /// - Should be ~10% of typical operating velocity
    /// - If robot oscillates when slow: increase to 0.15-0.2
    /// - If too sluggish at startup: decrease to 0.05-0.08
    /// </summary>
    public double Ks { get; set; } = 0.1;

    #endregion

    #region Vehicle Parameters

    /// <summary>
    /// Wheelbase (L) - distance between front and rear axles (meters)
    /// Default: 0.5m
    ///
    /// Meaning: Distance from rear axle (robot center) to virtual front axle
    /// Used to calculate front axle position and convert steering angle to angular velocity
    ///
    /// IMPORTANT: Must match actual robot geometry
    ///
    /// Formula: ω = (v × tan(δ)) / L
    /// </summary>
    public double WheelBase { get; set; } = 0.6;

    /// <summary>
    /// Maximum steering angle (radians)
    /// Default: 0.5 rad (≈28.6°)
    ///
    /// Meaning: Physical limit of equivalent steering angle
    ///
    /// ↑ Increase (0.6-0.8 rad ≈ 34-46°):
    ///   ✓ Sharper turns possible
    ///   ✗ May exceed robot's turning capability
    ///
    /// ↓ Decrease (0.3-0.4 rad ≈ 17-23°):
    ///   ✓ Safer, gentler turns
    ///   ✗ Cannot track sharp curves
    ///
    /// Tuning Tips:
    /// - Test robot's max practical turn rate
    /// - Calculate: δ_max = arctan(L × ω_max / v_typical)
    /// - Example: L=0.5m, ω_max=2rad/s, v=1m/s → δ_max = 0.785 rad (45°)
    /// - Conservative: 0.4-0.5 rad
    /// </summary>
    public double MaxSteeringAngle { get; set; } = 0.5;

    #endregion

    #region Curvature Feedforward Parameters

    /// <summary>
    /// Enable curvature feedforward term
    /// Default: true
    ///
    /// Meaning: Add path curvature prediction to steering command
    /// Formula: δ = ψ + arctan(K×e/(v+Ks)) + arctan(κ×L)
    ///
    /// ✓ Enabled:
    ///   ✓ Better tracking on curved paths
    ///   ✓ Anticipates turns, less lag
    ///   ✗ Requires accurate path curvature
    ///
    /// ✗ Disabled:
    ///   ✓ Simpler, more predictable
    ///   ✓ Works with rough path data
    ///   ✗ May lag on curves
    ///
    /// Tuning Tips:
    /// - Enable for smooth, well-defined paths
    /// - Disable if path is noisy or has discontinuities
    /// </summary>
    public bool EnableCurvatureFeedforward { get; set; } = true;

    /// <summary>
    /// Curvature feedforward gain
    /// Default: 1.0
    ///
    /// Meaning: Scaling factor for curvature term
    /// Full formula: δ = ψ + arctan(K×e/(v+Ks)) + KCurvatureFF × arctan(κ×L)
    ///
    /// ↑ Increase (1.2-1.5):
    ///   ✓ More aggressive curve anticipation
    ///   ✓ Less lag on sharp turns
    ///   ✗ May overshoot on curves
    ///
    /// ↓ Decrease (0.7-0.9):
    ///   ✓ Gentler curve following
    ///   ✗ More lag on curves
    ///
    /// Tuning Tips:
    /// - Start at 1.0
    /// - If cutting corners: increase to 1.1-1.3
    /// - If overshooting curves: decrease to 0.8-0.9
    /// </summary>
    public double KCurvatureFF { get; set; } = 1.0;

    #endregion

    #region Goal Approach Parameters

    /// <summary>
    /// Distance to goal to consider "reached" (meters)
    /// Default: 0.05m (5cm)
    ///
    /// Meaning: Stop criterion - when within this distance, goal is reached
    ///
    /// ↑ Increase (0.08-0.1m):
    ///   ✓ Faster completion
    ///   ✗ Lower precision
    ///
    /// ↓ Decrease (0.02-0.03m):
    ///   ✓ Higher precision
    ///   ✗ May never reach due to localization error
    ///
    /// Tuning Tips:
    /// - Must be ≥ 2× localization RMS error
    /// - Typical: 0.03-0.05m
    /// </summary>
    public double GoalTolerance { get; set; } = 0.05;

    /// <summary>
    /// Heading tolerance at goal (degrees)
    /// Default: 5.0°
    ///
    /// Meaning: Acceptable heading error when reaching goal
    ///
    /// Tuning Tips:
    /// - Strict docking: 2-3°
    /// - Normal navigation: 5-10°
    /// </summary>
    public double HeadingTolerance { get; set; } = 5.0;

    /// <summary>
    /// Distance to start increasing K gain near goal (meters)
    /// Default: 1.0m
    ///
    /// Meaning: When within this distance, K gain increases linearly
    /// to improve tracking accuracy during final approach
    ///
    /// ↑ Increase (1.5-2.0m):
    ///   ✓ Earlier tightening, smoother transition
    ///   ✗ May be too aggressive on long approach
    ///
    /// ↓ Decrease (0.5-0.8m):
    ///   ✓ Only tighten very close to goal
    ///   ✗ Less time to correct errors
    ///
    /// Tuning Tips:
    /// - Should be larger than GoalTolerance × 10
    /// - Typical: 0.8-1.5m
    /// </summary>
    public double GoalApproachDistance { get; set; } = 1.0;

    /// <summary>
    /// K gain multiplier at goal position
    /// Default: 2.0 (K doubles when at goal)
    ///
    /// Meaning: At goal, effective K = K × GoalGainMultiplier
    /// Linearly interpolated from 1.0 at GoalApproachDistance to this value at goal
    ///
    /// ↑ Increase (2.5-3.0):
    ///   ✓ Much tighter tracking near goal
    ///   ✗ Risk of oscillation
    ///
    /// ↓ Decrease (1.3-1.5):
    ///   ✓ Gentler increase
    ///   ✗ Less improvement near goal
    ///
    /// Tuning Tips:
    /// - Start at 2.0
    /// - If oscillating near goal: decrease to 1.5
    /// - If still drifting: increase to 2.5
    /// </summary>
    public double GoalGainMultiplier { get; set; } = 2.0;

    #endregion

    #region Low Speed Control Parameters

    /// <summary>
    /// Velocity threshold below which direct angular control activates (m/s)
    /// Default: 0.3 m/s
    ///
    /// Meaning: Below this speed, bicycle model is blended with direct proportional control.
    /// This prevents the angular velocity from collapsing to zero when the robot
    /// decelerates near the goal.
    ///
    /// Problem it solves:
    ///   Bicycle model: ω = v × tan(δ) / L
    ///   When v → 0, ω → 0, even if δ is large → robot cannot correct
    ///
    /// ↑ Increase (0.4-0.5):
    ///   ✓ Direct control kicks in earlier
    ///   ✗ May feel less smooth at moderate speeds
    ///
    /// ↓ Decrease (0.15-0.2):
    ///   ✓ Only activates at very low speed
    ///   ✗ May still drift at medium-low speeds
    ///
    /// Tuning Tips:
    /// - Should be close to NavigationConfig.MinLinearVelocity × 2-3
    /// - Typical: 0.2-0.4 m/s
    /// </summary>
    public double LowSpeedThreshold { get; set; } = 0.3;

    /// <summary>
    /// Angular velocity gain for direct control at low speeds
    /// Default: 1.5
    ///
    /// Meaning: At zero speed, ω = LowSpeedAngularGain × steeringAngle
    /// Ensures the robot can still correct heading/cross-track errors
    /// when the bicycle model would produce near-zero angular velocity.
    ///
    /// ↑ Increase (2.0-3.0):
    ///   ✓ Stronger correction at low speed
    ///   ✗ May oscillate near goal
    ///
    /// ↓ Decrease (0.8-1.0):
    ///   ✓ Gentler low-speed correction
    ///   ✗ Slower error correction
    ///
    /// Tuning Tips:
    /// - Start at 1.5
    /// - If oscillating at low speed: decrease to 1.0
    /// - If not correcting fast enough: increase to 2.0
    /// </summary>
    public double LowSpeedAngularGain { get; set; } = 1.5;

    #endregion

    #region Angular Velocity Limit

    /// <summary>
    /// Maximum angular velocity during final approach (rad/s)
    /// Default: 1.0 rad/s
    ///
    /// Meaning: Clamps the angular velocity output of Stanley controller
    /// to prevent excessive rotation near the goal.
    ///
    /// ↑ Increase (1.5-2.0):
    ///   ✓ Faster heading correction
    ///   ✗ May overshoot or oscillate
    ///
    /// ↓ Decrease (0.5-0.8):
    ///   ✓ Smoother, gentler rotation near goal
    ///   ✗ Slower heading correction
    ///
    /// Tuning Tips:
    /// - Should be ≤ robot's physical max angular velocity
    /// - Typically lower than PurePursuit MaxAngularVelocity for smoother final approach
    /// - Start at 1.0, decrease if robot oscillates near goal
    /// </summary>
    public double MaxAngularVelocity { get; set; } = 1.0;

    #endregion

    #region Path Resolution

    /// <summary>
    /// Waypoint spacing for path sampling (meters)
    /// Default: 0.05m (5cm)
    ///
    /// Meaning: Distance between interpolated path points
    /// Same as PurePursuit.ResolutionSplit for consistency
    /// </summary>
    public double ResolutionSplit { get; set; } = 0.05;

    #endregion
}