riojoy/tests/RioJoy.Core.Tests/Calibration/AxisCalibratorTests.cs

using RioJoy.Core.Calibration;
using RioJoy.Core.Protocol;
using Xunit;

namespace RioJoy.Core.Tests.Calibration;

public class AxisCalibratorTests
{
    // AnalogReport(throttle, leftPedal, rightPedal, joystickY, joystickX)
    private static AnalogReport Report(
        short throttle = 0, short left = 0, short right = 0, short y = 0, short x = 0) =>
        new(throttle, left, right, y, x);

    // --- Throttle (Z) --------------------------------------------------------

    [Fact]
    public void Throttle_BelowStart_IsProportional()
    {
        var cal = new AxisCalibrator();
        cal.ResetThrottle();

        // At-rest after reset: throttle equals its tracked start ⇒ centered at 0.
        Assert.Equal(0, cal.Update(Report(throttle: 0)).Z);

        // Pulled back 400 of 800 → half range × 32 = 16000.
        Assert.Equal(16000, cal.Update(Report(throttle: -400)).Z);

        // Full back (800) → 32000.
        Assert.Equal(32000, cal.Update(Report(throttle: -800)).Z);
    }

    [Fact]
    public void Throttle_OutOfRange_Saturates()
    {
        Assert.Equal(0, new AxisCalibrator().Update(Report(throttle: 900)).Z);       // > +range
        Assert.Equal(32000, new AxisCalibrator().Update(Report(throttle: -900)).Z);  // < -range
    }

    [Fact]
    public void Throttle_Invert_Flips()
    {
        var cal = new AxisCalibrator(new AxisCalibrationConfig { InvertZ = true });
        // throttle=900 → raw 0 → inverted to Max.
        Assert.Equal(AxisOutputs.Max, cal.Update(Report(throttle: 900)).Z);
    }

    // --- Pedals (Rx / Ry / Rz) ----------------------------------------------

    [Fact]
    public void Pedals_EnableZR_CentersRxRy_AndMixesRudder()
    {
        var cal = new AxisCalibrator(); // EnableZR default true
        AxisOutputs o = cal.Update(Report(left: 0, right: 0));

        Assert.Equal(AxisOutputs.Center, o.Rx);
        Assert.Equal(AxisOutputs.Center, o.Ry);
        Assert.Equal(AxisOutputs.Center, o.Rz); // rudder with both pedals at rest
    }

    [Fact]
    public void Pedals_DisableZR_DriveRxDirectly_AndCenterRz()
    {
        var cal = new AxisCalibrator(new AxisCalibrationConfig { EnableZR = false });

        // First sample sets the auto-range start; second (less depressed) produces a value.
        cal.Update(Report(left: -100));
        AxisOutputs o = cal.Update(Report(left: 0));

        // lP = (-100) - 0 = -100 → ×1000/500 = -200 → |·|×32 = 6400.
        Assert.Equal(6400, o.Rx);
        Assert.Equal(AxisOutputs.Center, o.Rz); // Rz centered when ZR disabled

        // The pedal readouts mirror Rx/Ry when there is no mix.
        Assert.Equal(o.Rx, cal.LeftPedalOutput);
        Assert.Equal(o.Ry, cal.RightPedalOutput);
    }

    [Fact]
    public void PedalOutputs_TrackPedals_EvenWithZrMix()
    {
        var cal = new AxisCalibrator(); // EnableZR default true

        cal.Update(Report(left: -100, right: -100));
        AxisOutputs o = cal.Update(Report(left: 0, right: 0));

        // The mix centers Rx/Ry, but the pedal readouts still track the pedals
        // (same math as the ZR-off Rx/Ry: 6400 for this deflection).
        Assert.Equal(AxisOutputs.Center, o.Rx);
        Assert.Equal(AxisOutputs.Center, o.Ry);
        Assert.Equal(6400, cal.LeftPedalOutput);
        Assert.Equal(6400, cal.RightPedalOutput);
    }

    // --- Joystick (X / Y) ----------------------------------------------------

    [Fact]
    public void Joystick_AtZero_IsCentered()
    {
        AxisOutputs o = new AxisCalibrator().Update(Report(x: 0, y: 0));
        Assert.Equal(AxisOutputs.Center, o.X);
        Assert.Equal(AxisOutputs.Center, o.Y);
    }

    [Fact]
    public void JoystickX_RightAndLeft_MoveOppositeSidesOfCenter()
    {
        // Right (positive raw) → below center; with auto-range seeded by this sample.
        Assert.Equal(52, new AxisCalibrator().Update(Report(x: 80)).X);
        // Left (negative raw) → above center.
        Assert.Equal(32733, new AxisCalibrator().Update(Report(x: -80)).X);
    }

    [Fact]
    public void JoystickX_Invert_Flips()
    {
        var cal = new AxisCalibrator(new AxisCalibrationConfig { InvertX = true });
        Assert.Equal(AxisOutputs.Max - 52, cal.Update(Report(x: 80)).X);
    }

    [Fact]
    public void ResetHorizontalJoystick_RecentersX()
    {
        var cal = new AxisCalibrator();
        Assert.Equal(52, cal.Update(Report(x: 80)).X);

        cal.ResetHorizontalJoystick();
        // After reset, X holds center until the stick next moves off zero.
        Assert.Equal(AxisOutputs.Center, cal.Update(Report(x: 0)).X);
    }

    // --- Detent regression (legacy quirk deliberately fixed) -----------------

    [Fact]
    public void Throttle_FirstPollAtRest_IsZero_NotPinned()
    {
        // Regression: the legacy Center init + detent-hold compounded through the
        // ×32 rescale and pinned Z at Max on the very first poll at rest — full
        // reverse on a Centered thumb route, full thrust on a UnipolarPositive one
        // (Descent). Rest must read 0 with no reset and stay there.
        var cal = new AxisCalibrator();
        Assert.Equal(0, cal.Update(Report(throttle: 0)).Z);
        Assert.Equal(0, cal.Update(Report(throttle: 0)).Z);
    }

    [Fact]
    public void Throttle_ReleaseExactlyOntoStart_ReturnsToZero_NoSpike()
    {
        // Regression: a release landing exactly on the tracked start (lT == 0)
        // used to hold the last moving value, which the ×32 rescale turned into a
        // one-poll full-throttle spike until jitter broke it.
        var cal = new AxisCalibrator();
        Assert.Equal(0, cal.Update(Report(throttle: 0)).Z);        // seed start at rest
        Assert.Equal(16000, cal.Update(Report(throttle: -400)).Z); // pushed halfway
        Assert.Equal(0, cal.Update(Report(throttle: 0)).Z);        // release onto start
    }

    [Fact]
    public void Throttle_SampleAboveRunningMax_ReadsAsRest()
    {
        // Samples above the tracked start re-base the auto-range (lT == 0 again)
        // and must read as rest, not re-enter the legacy hold path.
        var cal = new AxisCalibrator();
        cal.Update(Report(throttle: -400)); // power-on with lever pushed: becomes the start
        Assert.Equal(0, cal.Update(Report(throttle: -100)).Z); // above max → re-based, rest
    }

    [Fact]
    public void Joystick_ReleaseToExactZero_Centers_NotHeld()
    {
        // Regression: the legacy left the output unchanged on a raw of exactly 0,
        // which a jittering pot never produces - but vRIO's pad deadzone does,
        // sustained, on every release. The held output latched the last in-motion
        // value and the ship kept turning with the stick centered (bench
        // 2026-08-01). A fast release goes straight from deflected to exact 0
        // with no intermediate sample; both axes must land on center.
        var cal = new AxisCalibrator();
        cal.Update(Report(x: -3000, y: 2000)); // hard over
        AxisOutputs o = cal.Update(Report(x: 0, y: 0)); // released, deadzoned to exact 0
        Assert.Equal(AxisOutputs.Center, o.X);
        Assert.Equal(AxisOutputs.Center, o.Y);
    }

    [Fact]
    public void Joystick_ExactZero_StaysCentered_AcrossRepeatedPolls()
    {
        // The latch fed forward: once held off-center, every subsequent exact-0
        // poll (one per 55 ms, forever) kept it there. Center must be stable.
        var cal = new AxisCalibrator();
        cal.Update(Report(x: 5000));
        for (int i = 0; i < 5; i++)
            Assert.Equal(AxisOutputs.Center, cal.Update(Report(x: 0)).X);
    }
}