using CartActivator;

namespace DiffWheelM;

public class Remote : ManualController<CartDefinition>
{
    [AsControlItem(name = "速度摇杆", keyboard = "Up,Down,Left,Right", joystick = "Axis0,Axis1")]
    public Stick Direction;

    [AsControlItem(name = "速度比例", keyboard = "Q,W")]
    public Throttle SpeedRatio;

    public override void Operation()
    {
        var maxSpeed = Math.Max(0, cart.MaxManualSpeed);
        var speedRatio = Math.Clamp(SpeedRatio.val, 0, 1);

        var left = Direction.y - Direction.x;
        var right = Direction.y + Direction.x;
        var wheelScale = Math.Max(1, Math.Max(Math.Abs(left), Math.Abs(right)));

        left = left / wheelScale * maxSpeed * speedRatio;
        right = right / wheelScale * maxSpeed * speedRatio;

        cart.SpeedLeft = Math.Clamp(left, -maxSpeed, maxSpeed);
        cart.SpeedRight = Math.Clamp(right, -maxSpeed, maxSpeed);

        statusText = $"L={cart.SpeedLeft:0.000}m/s, R={cart.SpeedRight:0.000}m/s";
    }
}