namespace RobotNet10.RobotApp.Services.Navigation.CSharp;

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>
/// Mô hình động học của motor driver
/// First-order system: v(t) = v_cmd × (1 - e^(-(t-δ)/τ))
/// </summary>
public class MotorDynamicsModel
{
    /// <summary>
    /// Time constant (τ) - thời gian để motor đạt 63.2% của target velocity
    /// Đơn vị: giây (s)3
    /// 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>
    /// Constructor với giá trị mặc định
    /// </summary>
    public MotorDynamicsModel()
    {
        Tau = 0.3;      // 300ms time constant
        Delta = 0.05f;   // 50ms delay
    }

    public MotorDynamicsModel(MotorDynamicsConfig cog)
    {
        Tau = cog.Tau;
        Delta = cog.Delta;
    }

    /// <summary>
    /// Predict vận tốc tại thời điểm tương lai
    /// </summary>
    /// <param name="vCmd">Velocity command đã gửi</param>
    /// <param name="vActual">Velocity thực tế hiện tại</param>
    /// <param name="timeAhead">Thời gian dự đoán về tương lai (s)</param>
    /// <returns>Vận tốc dự đoán</returns>
    public double PredictVelocity(double vCmd, double vActual, double timeAhead)
    {
        // Nếu thời gian dự đoán < delay
        // → Motor chưa bắt đầu phản ứng
        if (timeAhead < Delta)
        {
            return vActual;
        }

        // Thời gian hiệu dụng (sau khi trừ delay)
        double effectiveTime = timeAhead - Delta;

        // First-order system response
        // response = 1 - e^(-t/τ)
        double response = 1.0 - Math.Exp(-effectiveTime / Tau);

        // Velocity prediction
        // v_future = v_actual + (v_cmd - v_actual) × response
        double vPredicted = vActual + (vCmd - vActual) * response;

        return vPredicted;
    }

    /// <summary>
    /// Tính settling time (thời gian để đạt 95% target)
    /// </summary>
    public double GetSettlingTime()
    {
        // 95% response: t = -τ × ln(0.05) ≈ 3τ
        return Delta + 3.0 * Tau;
    }

    /// <summary>
    /// Tính rise time (thời gian để đạt từ 10% đến 90%)
    /// </summary>
    public double GetRiseTime()
    {
        // Rise time ≈ 2.2τ
        return 2.2 * Tau;
    }

    public override string ToString()
    {
        return $"MotorModel(τ={Tau:F3}s, δ={Delta:F3}s, settling={GetSettlingTime():F3}s)";
    }
}