using RioJoy.Core.Protocol;

namespace RioJoy.Core.Calibration;

/// <summary>
/// Converts raw RIO analog samples into calibrated virtual-joystick axis values.
/// Stateful port of <c>UpdateThrottle</c>/<c>UpdatePadal</c>/<c>UpdateJoystick</c>
/// (riovjoy2.cpp#L1504+); see docs/PROTOCOL.md §4. State (auto-ranging start
/// positions, observed min/max for X/Y, last outputs, throttle ratchet) carries
/// across <see cref="Update"/> calls exactly as the legacy globals did.
///
/// <para>Final outputs are clamped to <c>0..<see cref="AxisOutputs.Max"/></c> — the
/// documented axis range — which also guards a legacy quirk where a value pinned
/// at its observed extreme could compound across polls (see ⚠️ below).</para>
/// </summary>
public sealed class AxisCalibrator
{
    private const int RangeThrottle = 800;
    private const int DeadzoneThrottle = 50;
    private const int RangePadal = 500;
    private const int DeadzonePadal = 10;
    private const int DeadzoneJoystick = 5;

    private readonly AxisCalibrationConfig _config;

    // Auto-ranging start positions.
    private int _throttleStart = int.MinValue;
    private int _leftPedalStart = int.MaxValue;
    private int _rightPedalStart = int.MaxValue;

    // Last computed (pre-clamp) outputs — persist across calls like the legacy globals.
    private int _throttleLast = AxisOutputs.Center;
    private int _leftPedalLast = AxisOutputs.Center;
    private int _rightPedalLast = AxisOutputs.Center;
    private int _joystickXLast = AxisOutputs.Center;
    private int _joystickYLast = AxisOutputs.Center;

    // Throttle ratchet direction. Initialized -1; the legacy never sets it
    // positive, so the "back" branches are effectively dead but ported faithfully.
    private int _throttleResult = -1;

    // Observed min/max for the joystick axes (auto-ranging rate).
    private int _minX, _maxX, _minY, _maxY;

    public AxisCalibrator(AxisCalibrationConfig? config = null)
    {
        _config = config ?? new AxisCalibrationConfig();
    }

    /// <summary>
    /// Calibrate one raw analog sample and return the six axis values. Updates the
    /// observed X/Y min/max first (as the legacy <c>AnalogEvent</c> did), then runs
    /// the per-axis math.
    /// </summary>
    public AxisOutputs Update(AnalogReport raw)
    {
        _maxX = Math.Max(raw.JoystickX, _maxX);
        _minX = Math.Min(raw.JoystickX, _minX);
        _maxY = Math.Max(raw.JoystickY, _maxY);
        _minY = Math.Min(raw.JoystickY, _minY);

        int z = Throttle(raw.Throttle);
        (int rx, int ry, int rz) = Pedals(raw.LeftPedal, raw.RightPedal);
        int x = JoystickX(raw.JoystickX);
        int y = JoystickY(raw.JoystickY);

        return new AxisOutputs(
            Clamp(x), Clamp(y), Clamp(z), Clamp(rx), Clamp(ry), Clamp(rz));
    }

    private static int Clamp(int v) => RioJoy.Core.Compat.Net48Math.Clamp(v, 0, AxisOutputs.Max);

    /// <summary>
    /// The calibrated left-pedal position from the last <see cref="Update"/>
    /// (<c>0..32766</c>) — always tracks the pedal, even when
    /// <see cref="AxisCalibrationConfig.EnableZR"/> centers <see cref="AxisOutputs.Rx"/>.
    /// </summary>
    public int LeftPedalOutput => Clamp(_leftPedalLast);

    /// <summary>Right-pedal counterpart of <see cref="LeftPedalOutput"/>.</summary>
    public int RightPedalOutput => Clamp(_rightPedalLast);

    private int Throttle(int throttle)
    {
        if (throttle < -RangeThrottle || throttle > RangeThrottle)
        {
            if (throttle < RangeThrottle)
                _throttleLast = _throttleResult > 0 ? 1000 : -1000;
            else
                _throttleLast = 0;
        }
        else
        {
            if (_throttleStart < throttle)
                _throttleStart = throttle;

            int lT = _throttleStart - throttle;
            if (lT > 800)
                lT = 800;

            if (lT != 0)
            {
                lT = lT * 1000 / 800;
                if (lT is > -DeadzoneThrottle and < DeadzoneThrottle)
                    _throttleLast = 0;
                else if (_throttleResult > 0) // back
                    _throttleLast = 900 + (lT * _throttleResult / 10);
                else // front
                    _throttleLast = lT * _throttleResult;
            }
            // lT == 0 leaves _throttleLast unchanged (legacy behavior).
        }

        _throttleLast = Math.Abs(_throttleLast * 32);
        return _config.InvertZ ? AxisOutputs.Max - _throttleLast : _throttleLast;
    }

    private (int rx, int ry, int rz) Pedals(int leftPedal, int rightPedal)
    {
        // LEFT pedal
        if (leftPedal < -RangePadal || leftPedal > RangePadal)
        {
            _leftPedalLast = leftPedal > RangePadal ? -1000 : 0;
        }
        else
        {
            if (_leftPedalStart > leftPedal)
                _leftPedalStart = leftPedal;

            int lP = _leftPedalStart - leftPedal;
            if (lP != 0)
                lP = lP * 1000 / RangePadal;
            if (Math.Abs(lP) < DeadzonePadal)
                lP = 0;
            _leftPedalLast = lP;
        }
        _leftPedalLast = Math.Abs(_leftPedalLast * 32);
        if (_config.InvertXR)
            _leftPedalLast = AxisOutputs.Max - _leftPedalLast;

        // RIGHT pedal
        if (rightPedal < -RangePadal || rightPedal > RangePadal)
        {
            _rightPedalLast = rightPedal > RangePadal ? 1000 : 0;
        }
        else
        {
            if (_rightPedalStart > rightPedal)
                _rightPedalStart = rightPedal;

            int lP = _rightPedalStart - rightPedal;
            if (lP != 0)
                lP = lP * 1000 / RangePadal;
            if (Math.Abs(lP) < DeadzonePadal)
                lP = 0;
            _rightPedalLast = -lP;
        }
        _rightPedalLast = Math.Abs(_rightPedalLast * 32);
        if (_config.InvertYR)
            _rightPedalLast = AxisOutputs.Max - _rightPedalLast;

        int rudder = AxisOutputs.Center - (_leftPedalLast / 2) + (_rightPedalLast / 2);
        if (_config.InvertZR)
            rudder = AxisOutputs.Max - rudder;

        // EnableZR mixes the pedals into the rudder (Rz) and centers Rx/Ry;
        // otherwise the pedals drive Rx/Ry directly and Rz is centered.
        int rx = _config.EnableZR ? AxisOutputs.Center : _leftPedalLast;
        int ry = _config.EnableZR ? AxisOutputs.Center : _rightPedalLast;
        int rz = _config.EnableZR ? rudder : AxisOutputs.Center;
        return (rx, ry, rz);
    }

    private int JoystickX(int joystickX)
    {
        int startingLeftRate = Math.Min(_minX, -80);
        int startingRightRate = Math.Max(_maxX, 80);
        int sLeftRate = AxisOutputs.Center / (Math.Abs(startingLeftRate) - DeadzoneJoystick);
        int sRightRate = AxisOutputs.Center / (startingRightRate - DeadzoneJoystick);

        int lJx = joystickX;
        if (lJx < 0) // LEFT
        {
            lJx += DeadzoneJoystick;
            _joystickXLast = lJx < 0 ? AxisOutputs.Center - (lJx * sLeftRate) : AxisOutputs.Center;
        }
        else if (lJx > 0) // RIGHT
        {
            lJx -= DeadzoneJoystick;
            // 16838 (not 16383) and a +2 nudge are deliberate legacy anti-snap tweaks.
            _joystickXLast = lJx > 0 ? 16838 - ((lJx + 2) * sRightRate) : AxisOutputs.Center;
        }
        // lJx == 0 leaves _joystickXLast unchanged (legacy behavior).

        return _config.InvertX ? AxisOutputs.Max - _joystickXLast : _joystickXLast;
    }

    private int JoystickY(int joystickY)
    {
        int startingUpRate = Math.Min(_minY, -80);
        int startingDownRate = Math.Max(_maxY, 80);
        int sUpRate = AxisOutputs.Center / (Math.Abs(startingUpRate) - DeadzoneJoystick);
        int sDownRate = AxisOutputs.Center / (startingDownRate - DeadzoneJoystick);

        int lJy = joystickY;
        if (lJy < 0) // UP
        {
            lJy += DeadzoneJoystick;
            _joystickYLast = lJy < 0 ? AxisOutputs.Center - (lJy * sUpRate) : AxisOutputs.Center;
        }
        else if (lJy > 0) // DOWN
        {
            lJy -= DeadzoneJoystick;
            _joystickYLast = lJy > 0 ? AxisOutputs.Center - (lJy * sDownRate) : AxisOutputs.Center;
        }
        // lJy == 0 leaves _joystickYLast unchanged (legacy behavior).

        return _config.InvertY ? AxisOutputs.Max - _joystickYLast : _joystickYLast;
    }

    // --- Resets (port of the RIOcmd axis-reset cases, riovjoy2.cpp#L1852) -------

    /// <summary>Dispatch a calibration reset for a RIO-command reset code.</summary>
    public void Reset(Mapping.RioCommandCode command)
    {
        switch (command)
        {
            case Mapping.RioCommandCode.ResetAllAxes: ResetAll(); break;
            case Mapping.RioCommandCode.ResetThrottle: ResetThrottle(); break;
            case Mapping.RioCommandCode.ResetLeftPedal: ResetLeftPedal(); break;
            case Mapping.RioCommandCode.ResetRightPedal: ResetRightPedal(); break;
            case Mapping.RioCommandCode.ResetVerticalJoystick: ResetVerticalJoystick(); break;
            case Mapping.RioCommandCode.ResetHorizontalJoystick: ResetHorizontalJoystick(); break;
        }
    }

    public void ResetAll()
    {
        _throttleResult = -1;
        _throttleStart = int.MinValue;
        _leftPedalStart = int.MaxValue;
        _rightPedalStart = int.MaxValue;
        _joystickXLast = AxisOutputs.Center;
        _joystickYLast = AxisOutputs.Center;
        _throttleLast = 0;
        _leftPedalLast = 0;
        _rightPedalLast = 0;
        _minX = _maxX = _minY = _maxY = 0;
    }

    public void ResetThrottle()
    {
        _throttleResult = -1;
        _throttleStart = int.MinValue;
        _throttleLast = 0;
    }

    public void ResetLeftPedal()
    {
        _leftPedalStart = int.MaxValue;
        _leftPedalLast = 0;
    }

    public void ResetRightPedal()
    {
        _rightPedalStart = int.MaxValue;
        _rightPedalLast = 0;
    }

    public void ResetVerticalJoystick()
    {
        _joystickYLast = AxisOutputs.Center;
        _maxY = _minY = 0;
    }

    public void ResetHorizontalJoystick()
    {
        _joystickXLast = AxisOutputs.Center;
        _maxX = _minX = 0;
    }
}
