using RobotNet10.CANOpen.CiA402.Enums;
using RobotNet10.RobotApp.Motion;
using RobotNet10.RobotApp.Services.ConfigManager;

namespace RobotNet10.RobotApp.Services.Navigation.CSharp;



/// <summary>
/// Configuration cho signal processing
/// </summary>
public class VelocitySignalProcessingConfig
{
    /// <summary>
    /// Hệ số lọc cho encoder velocity
    /// Giá trị nhỏ (0.1-0.2): Smooth nhưng lag
    /// Giá trị lớn (0.3-0.4): Responsive nhưng nhiễu
    /// </summary>
    public double AlphaFilter { get; set; } = 0.3;

    /// <summary>
    /// Ngưỡng phát hiện encoder nhiễu (m/s)
    /// Nếu thay đổi vận tốc > threshold trong 1 cycle → có thể nhiễu
    /// </summary>
    public double NoiseThreshold { get; set; } = 0.5;
}

/// <summary>
/// Configuration cho velocity estimator
/// </summary>
public class VelocityEstimatorConfig
{
    // Blend ratio limits
    public double MinBlendRatio { get; set; } = 0.15f;
    public double MaxBlendRatio { get; set; } = 0.8f;
    public double DefaultBlendRatio { get; set; } = 0.6;

    // Adaptive blending thresholds
    public double GoodTrackingThreshold { get; set; } = 0.12f;   // < 10% error
    public double ModerateTrackingThreshold { get; set; } = 0.3; // < 30% error

    // Blend ratios for different tracking qualities
    public double GoodTrackingBlend { get; set; } = 0.7;
    public double ModerateTrackingBlend { get; set; } = 0.5;
    public double PoorTrackingBlend { get; set; } = 0.25f;

    // Model confidence decay
    public double ConfidenceDecayRate { get; set; } = 0.95f;
    public double MinConfidence { get; set; } = 0.3;
}

public class VelocityController(IInverseKinematics InverseKinematic,
                                OdometryService odometryService,
                                IRobotConfiguration RobtoConfiguration,
                                INavigationConfig NavigationConfig,
                                ILogger<VelocityController> _logger) : IVelocityController
{
    public (double Linear, double Angular) ActualVelocity => GetCurrentVel();
    public (double Linear, double Angular) RawVelocity => GetRawCurrentVel();

    // vận tốc tính toán m/s và rad/s đối với vận tốc góc
    private double _rightVelCmd = 0;
    private double _leftVelCmd = 0;
    private double _oldRightVel = 0;
    private double _oldLeftVel = 0;

    private MotorDynamicsConfig _motorDynamicsConifg = new();
    private MotorDynamicsModel _motorDynamicsModel = new();
    private VelocityEstimatorConfig _estimatorConfig = new();
    public PurePursuitConfig _purePursuitConfig = new();
    private VelocitySignalProcessingConfig _signalConfig = new();
    private readonly CircularBuffer<double> _predictionErrors = new(20);

    private double _currentConfidence = 1.0;
    private readonly double wheelBase = RobtoConfiguration.GetRobotPhysicalConfig().WheelBase;
    private int _ensureIKReadyCounter = 0; // Counter for logging throttling

    public void SetVelocity(double linearVel, double angularVel)
    {
        // InverseKinematic.SetVelocity not available - commented out
        // InverseKinematic.SetVelocity(new()
        // {
        //     Linear = new(){
        //         X = linearVel,
        //         Y = 0,
        //     },
        //     Angular = new(){
        //         Z = angularVel,
        //     },
        // });
        _leftVelCmd = linearVel - (wheelBase / 2) * angularVel;
        _rightVelCmd = linearVel + (wheelBase / 2) * angularVel;
    }
    public (double linearVel, double angularVel) GetRawCurrentVel()
    {
        try
        {
            var odom = odometryService.CurrentOdometry;
            double vActual = odom.Twist.Twist.Linear.X;
            double omegaActual = odom.Twist.Twist.Angular.Z;
            return (vActual, omegaActual);
        }
        catch { return (0, 0); }
    }

    public (double linearVel, double angularVel) GetCurrentVel()
    {
        try
        {
            var odom = odometryService.CurrentOdometry;
            var vActual = odom.Twist.Twist.Linear.X;
            var omegaActual = odom.Twist.Twist.Angular.Z;

            // Convert linear/angular back to left/right wheel velocities for the estimator
            _oldLeftVel = vActual - (wheelBase / 2) * omegaActual;
            _oldRightVel = vActual + (wheelBase / 2) * omegaActual;

            return Estimate(_oldLeftVel, _oldRightVel, _leftVelCmd, _rightVelCmd, wheelBase);
        }
        catch { return (0, 0); }
    }

    /// <summary>
    /// Exponential Moving Average (EMA) Low-Pass Filter
    /// </summary>
    /// <param name="newValue">Giá trị mới từ sensor</param>
    /// <param name="oldValue">Giá trị đã lọc trước đó</param>
    /// <param name="alpha">Hệ số lọc (0-1). Càng nhỏ càng smooth, càng lớn càng responsive</param>
    /// <returns>Giá trị sau khi lọc</returns>
    private static double LowPassFilter(double newValue, double oldValue, double alpha)
    {
        // Validate alpha
        if (alpha < 0 || alpha > 1)
        {
            throw new ArgumentException("Alpha must be between 0 and 1", nameof(alpha));
        }

        return alpha * newValue + (1.0 - alpha) * oldValue;
    }

    /// <summary>
    /// MAIN FUNCTION: Estimate velocity
    /// </summary>
    private (double linearVel, double angularVel) Estimate(
        double vLeftActual,      // Từ encoder (filtered)
        double vRightActual,     // Từ encoder (filtered)
        double vLeftCmdPrev,     // Command từ cycle trước
        double vRightCmdPrev,    // Command từ cycle trước
        double wheelbase)
    {
        // 1. Tính vận tốc actual (linear & angular)
        double vActual = (vLeftActual + vRightActual) / 2.0;
        double omegaActual = (vRightActual - vLeftActual) / wheelbase;

        // 2. Tính vận tốc command từ cycle trước
        double vCmdPrev = (vLeftCmdPrev + vRightCmdPrev) / 2.0;
        double omegaCmdPrev = (vRightCmdPrev - vLeftCmdPrev) / wheelbase;

        // 3. Tính prediction horizon
        double predictionHorizon = CalculatePredictionHorizon(vActual);

        // 4. Predict velocity cho từng bánh
        double vLeftPredicted = _motorDynamicsModel.PredictVelocity(
            vLeftCmdPrev,
            vLeftActual,
            predictionHorizon
        );

        double vRightPredicted = _motorDynamicsModel.PredictVelocity(
            vRightCmdPrev,
            vRightActual,
            predictionHorizon
        );

        // 5. Tính linear & angular predicted
        double vPredicted = (vLeftPredicted + vRightPredicted) / 2.0;
        double omegaPredicted = (vRightPredicted - vLeftPredicted) / wheelbase;

        // 6. Update model confidence
        UpdateModelConfidence(vPredicted, vActual);

        // 7. Tính tracking error
        double linearErr = CalculateLinearTrackingError(vCmdPrev, vActual);
        double angularErr = CalculateAngularTrackingError(omegaCmdPrev, omegaActual);
        double combinedTrackingError = 0.65f * linearErr + 0.35f * angularErr;

        // 8. Calculate adaptive blend ratio
        double blendRatio = CalculateAdaptiveBlendRatio(
            combinedTrackingError,
            _currentConfidence
        );

        // 9. Blend predicted và actual
        var vHybrid = (blendRatio * vPredicted) + ((1.0 - blendRatio) * vActual);
        double omegaHybrid = blendRatio * omegaPredicted + (1.0 - blendRatio) * omegaActual;

        // 10. Return result
        return (vHybrid, omegaHybrid);
    }

    /// <summary>
    /// Tính prediction horizon dựa vào lookahead distance
    /// </summary>
    private double CalculatePredictionHorizon(double vActual)
    {
        // Lookahead distance
        double lookahead = _purePursuitConfig.LookaheadMin + _purePursuitConfig.Kdd * Math.Abs(vActual);

        lookahead = Math.Clamp(lookahead, _purePursuitConfig.LookaheadMin, _purePursuitConfig.LookaheadMax);

        // Prediction time = lookahead / velocity
        // Nếu vận tốc quá nhỏ, dùng một giá trị minimum
        double predictionTime = lookahead / Math.Max(Math.Abs(vActual), 0.1);

        // Giới hạn prediction time (không nên quá xa)
        predictionTime = Math.Clamp(predictionTime, 0.1, 2.0);

        return predictionTime;
    }

    /// <summary>
    /// Tính linear velocity tracking error (normalized)
    /// </summary>
    private static double CalculateLinearTrackingError(double vCmd, double vActual)
    {
        double error = Math.Abs(vCmd - vActual);
        double normalizedError = error / Math.Max(Math.Abs(vCmd), 0.1);
        return normalizedError;
    }

    /// <summary>
    /// Tính angular velocity tracking error (normalized)
    /// </summary>
    private static double CalculateAngularTrackingError(double oCmd, double oActual)
    {
        double error = Math.Abs(oCmd - oActual);
        double normalizedError = error / Math.Max(Math.Abs(oCmd), 0.05f);
        return normalizedError;
    }

    /// <summary>
    /// Update model confidence dựa trên prediction accuracy
    /// </summary>
    private void UpdateModelConfidence(double vPredictedPrev, double vActualNow)
    {
        // Prediction error từ cycle trước
        double predError = Math.Abs(vPredictedPrev - vActualNow) / Math.Max(Math.Abs(vActualNow), 0.1);

        _predictionErrors.Add(predError);

        // Tính confidence dựa trên average error
        if (_predictionErrors.Count > 0)
        {
            double avgError = _predictionErrors.Average();

            // Confidence = 1 - avgError (capped)
            double newConfidence = Math.Clamp(1.0 - avgError, 0.0, 1.0);

            // Smooth update với decay
            _currentConfidence = _estimatorConfig.ConfidenceDecayRate * _currentConfidence + (1.0 - _estimatorConfig.ConfidenceDecayRate) * newConfidence;

            _currentConfidence = Math.Max(_currentConfidence, _estimatorConfig.MinConfidence);
        }
    }

    /// <summary>
    /// Calculate adaptive blend ratio
    /// </summary>
    private double CalculateAdaptiveBlendRatio(
        double trackingError,
        double modelConfidence)
    {
        double alpha;

        // Factor 1: Tracking error
        if (trackingError < _estimatorConfig.GoodTrackingThreshold)
        {
            // Motor tracking tốt → tin prediction nhiều
            alpha = _estimatorConfig.GoodTrackingBlend;
        }
        else if (trackingError < _estimatorConfig.ModerateTrackingThreshold)
        {
            // Moderate error → balanced
            alpha = _estimatorConfig.ModerateTrackingBlend;
        }
        else
        {
            // Poor tracking (slip/overload) → tin actual nhiều
            alpha = _estimatorConfig.PoorTrackingBlend;
        }

        // Factor 2: Model confidence
        // Nếu model không chính xác, giảm blend ratio
        alpha *= modelConfidence;

        // Clamp trong khoảng cho phép
        alpha = Math.Clamp(alpha, _estimatorConfig.MinBlendRatio, _estimatorConfig.MaxBlendRatio);

        return alpha;
    }

    /// <summary>
    /// Reset estimator state
    /// </summary>
    public void Reset()
    {
        _predictionErrors.Clear();
        _currentConfidence = 1.0;
    }

    /// <summary>
    /// Get current model confidence
    /// </summary>
    public double GetModelConfidence()
    {
        return _currentConfidence;
    }

    public void LoadConfig()
    {
        _motorDynamicsConifg = NavigationConfig.GetMotorDynamicsConfig();
        _motorDynamicsModel = new(_motorDynamicsConifg);
        _estimatorConfig = NavigationConfig.GetVelocityEstimatorConfig();
        _purePursuitConfig = NavigationConfig.GetPurepursuitConfig();
        _signalConfig = NavigationConfig.GetVelocitySignalProcessingConfig();
    }

    public void SetAcceleration(double acc)
    {
        // SetAcceleration not available - commented out
        // InverseKinematic.SetAcceleration(acc);
    }

    public void SetDeceleration(double dec)
    {
        // SetDeceleration not available - commented out
        // InverseKinematic.SetDeceleration(dec);
    }

    public bool EnsureInverseKinematicsReady(CancellationToken cancellationToken)
    {
        try
        {
            // Increment counter for logging throttling
            _ensureIKReadyCounter++;

            // Check if need to reset fault first
            // Note: DifferentialDrive doesn't expose IsFaulted, so we try FaultReset if not enabled
            if (!InverseKinematic.IsOperationEnabled)
            {
                // Try fault reset first (in case it's in fault state)
                InverseKinematic.FaultReset();
                PreciseDelay(200, cancellationToken);
            }

            // Check if IInverseKinematics is in OperationEnabled state
            if (!InverseKinematic.IsOperationEnabled)
            {
                // Auto-enable IInverseKinematics through state transitions
                // Enable() is a convenience method that automatically transitions through all states
                int maxAttempts = 3;
                int attemptDelay = 300; // ms

                for (int i = 0; i < maxAttempts && !InverseKinematic.IsOperationEnabled; i++)
                {
                    InverseKinematic.Enable();
                    PreciseDelay(attemptDelay, cancellationToken);
                }

                // Check if enabled successfully
                if (!InverseKinematic.IsOperationEnabled)
                {
                    // Log only once every 10 times to avoid spam
                    if (_ensureIKReadyCounter % 10 == 0)
                    {
                        _logger.LogWarning("IInverseKinematics is not in OperationEnabled state. Cannot send velocity.");
                    }
                    return false;
                }
            }

            // Check and set operation mode to ProfileVelocity (synchronous)
            try
            {
                // GetOperationMode/SetOperationMode not available - commented out
                // OperationMode currentMode = InverseKinematic.GetOperationMode();
                // if (currentMode != OperationMode.ProfileVelocity)
                // {
                //     InverseKinematic.SetOperationMode(OperationMode.ProfileVelocity);
                // }
            }
            catch (Exception ex)
            {
                // Log only once every 10 times to avoid spam
                if (_ensureIKReadyCounter % 10 == 0)
                {
                    _logger.LogWarning(ex, "Error checking/setting operation mode");
                }
                // Continue anyway to prevent blocking
            }

            return true;
        }
        catch (OperationCanceledException)
        {
            throw;
        }
        catch (Exception ex)
        {
            // Log only once every 10 times to avoid spam
            if (_ensureIKReadyCounter % 10 == 0)
            {
                _logger.LogError(ex, "Error ensuring IInverseKinematics ready");
            }
            return false;
        }
    }

    /// <summary>
    /// Precise synchronous delay using Thread.Sleep for longer delays and SpinWait for short delays
    /// This ensures accurate timing for the update loop without async overhead
    /// </summary>
    private static void PreciseDelay(int milliseconds, CancellationToken cancellationToken)
    {
        if (milliseconds <= 0)
            return;

        if (milliseconds > 1)
        {
            // Use Thread.Sleep for longer delays (synchronous, more precise in dedicated thread)
            // Check cancellation periodically during sleep
            var sleepStart = DateTime.UtcNow;
            while ((DateTime.UtcNow - sleepStart).TotalMilliseconds < milliseconds)
            {
                if (cancellationToken.IsCancellationRequested)
                    return;

                var remaining = milliseconds - (int)(DateTime.UtcNow - sleepStart).TotalMilliseconds;
                if (remaining > 0)
                {
                    Thread.Sleep(Math.Min(remaining, 10)); // Sleep in 10ms chunks to check cancellation
                }
            }
        }
        else
        {
            // Use SpinWait for very short delays to maintain precise timing
            var spinWait = new SpinWait();
            for (int i = 0; i < 10; i++)
            {
                if (cancellationToken.IsCancellationRequested)
                    break;
                spinWait.SpinOnce();
            }
        }
    }
}