namespace RobotNet10.NavigationTune.Execution;

/// <summary>
/// Adapter to wrap IVelocityController from RobotApp
/// This will be implemented in the application layer that has access to RobotApp
/// </summary>
public class VelocityControllerAdapter(
    Func<(double Linear, double Angular)> getActualVelocity,
    Action<double, double> setVelocity,
    Func<double> getModelConfidence,
    Action<double>? setAcceleration = null,
    Action<double>? setDeceleration = null) : IVelocityProvider
{
    private readonly Func<(double Linear, double Angular)> _getActualVelocity = getActualVelocity;
    private readonly Action<double, double> _setVelocity = setVelocity;
    private readonly Func<double> _getModelConfidence = getModelConfidence;
    private readonly Action<double>? _setAcceleration = setAcceleration;
    private readonly Action<double>? _setDeceleration = setDeceleration;

    public (double Linear, double Angular) GetActualVelocity()
    {
        return _getActualVelocity();
    }

    public void SetVelocity(double linearVel, double angularVel)
    {
        _setVelocity(linearVel, angularVel);
    }

    public double GetModelConfidence()
    {
        return _getModelConfidence();
    }

    public void SetAcceleration(double acc)
    {
        _setAcceleration?.Invoke(acc);
    }

    public void SetDeceleration(double dec)
    {
        _setDeceleration?.Invoke(dec);
    }
}