Substation/State.cs

using System.Collections.Concurrent;

namespace Substation;

public class State
{
    public readonly object Lock = new();

    public bool DirtyA, DirtyB;
    public int DesiredA, DesiredB;

    public byte[]? LoopFreqA, LoopIntA;
    public byte[]? LoopFreqB, LoopIntB;

    public readonly ConcurrentQueue<WaveFrame> StreamA = new();
    public readonly ConcurrentQueue<WaveFrame> StreamB = new();

    public double LimitScaleA, LimitScaleB;
    public byte[]? LastFreqA, LastIntA, LastFreqB, LastIntB;
    public bool LastActiveA, LastActiveB;

    public int MaxA = 200;
    public int MaxB = 200;
    public LimitMode LimitModeA = LimitMode.Clamp;
    public LimitMode LimitModeB = LimitMode.Clamp;
    public bool SwapChannels;
    public double AmpA = 1.0;
    public double AmpB = 1.0;

    public void SetLimitA(int max, LimitMode mode)
    {
        lock (Lock)
        {
            MaxA = Math.Clamp(max, 0, 200);
            LimitModeA = mode;
            DirtyA = true;
        }
    }

    public void SetLimitB(int max, LimitMode mode)
    {
        lock (Lock)
        {
            MaxB = Math.Clamp(max, 0, 200);
            LimitModeB = mode;
            DirtyB = true;
        }
    }

    public void SetAmpA(double amp)
    {
        lock (Lock)
        {
            AmpA = Math.Clamp(amp, 0.1, 10.0);
            DirtyA = true;
        }
    }

    public void SetAmpB(double amp)
    {
        lock (Lock)
        {
            AmpB = Math.Clamp(amp, 0.1, 10.0);
            DirtyB = true;
        }
    }

    public bool IsSignaling
    {
        get
        {
            lock (Lock)
            {
                bool hasPattern = LoopFreqA != null || LoopFreqB != null
                                  || !StreamA.IsEmpty || !StreamB.IsEmpty;
                bool hasStrength = DesiredA > 0 || DesiredB > 0;
                return hasPattern && hasStrength;
            }
        }
    }

    public void SetStrength(char ch, int value)
    {
        lock (Lock)
        {
            var clamped = Math.Clamp(value, 0, 200);
            if (ch == 'A') { DesiredA = clamped; DirtyA = true; }
            else           { DesiredB = clamped; DirtyB = true; }
        }
    }

    public void SetLoop(char ch, byte[] freq, byte[] intensity)
    {
        lock (Lock)
        {
            if (ch == 'A') { LoopFreqA = freq; LoopIntA = intensity; }
            else           { LoopFreqB = freq; LoopIntB = intensity; }
        }
    }

    public void EnqueueStream(char ch, IEnumerable<WaveFrame> frames)
    {
        var queue = ch == 'A' ? StreamA : StreamB;
        foreach (var f in frames)
            queue.Enqueue(f);
    }

    public void Stop(char ch)
    {
        lock (Lock)
        {
            if (ch == 'A') { LoopFreqA = null; LoopIntA = null; }
            else           { LoopFreqB = null; LoopIntB = null; }
        }
        var queue = ch == 'A' ? StreamA : StreamB;
        while (queue.TryDequeue(out _)) { }
    }

    public (StrengthMode modeA, byte valA, StrengthMode modeB, byte valB,
            byte[] freqA, byte[] intA, byte[] freqB, byte[] intB)
        ConsumeTick()
    {
        lock (Lock)
        {
            var modeA = DirtyA ? StrengthMode.Abs : StrengthMode.None;
            var modeB = DirtyB ? StrengthMode.Abs : StrengthMode.None;

            byte ApplyLimit(int desired, int max, LimitMode mode) =>
                mode == LimitMode.Scale
                    ? (byte)(desired * max / 200)
                    : (byte)Math.Clamp(desired, 0, max);
            var valA  = ApplyLimit(DesiredA, MaxA, LimitModeA);
            var valB  = ApplyLimit(DesiredB, MaxB, LimitModeB);
            LimitScaleA = DesiredA > 0 ? (double)valA / DesiredA : 0;
            LimitScaleB = DesiredB > 0 ? (double)valB / DesiredB : 0;
            DirtyA = false;
            DirtyB = false;

            bool hasA = !StreamA.IsEmpty || (LoopFreqA != null && LoopIntA != null);
            bool hasB = !StreamB.IsEmpty || (LoopFreqB != null && LoopIntB != null);
            var (freqA, intARaw) = PopWave(StreamA, LoopFreqA, LoopIntA);
            var (freqB, intBRaw) = PopWave(StreamB, LoopFreqB, LoopIntB);

            // Apply amplifier to intensity, clamp to 0-100
            var ampA = AmpA;
            var ampB = AmpB;
            var intA = new byte[4];
            var intB = new byte[4];
            for (int i = 0; i < 4; i++)
            {
                intA[i] = (byte)Math.Clamp((int)Math.Round(intARaw[i] * ampA), 0, 100);
                intB[i] = (byte)Math.Clamp((int)Math.Round(intBRaw[i] * ampB), 0, 100);
            }

            LastActiveA = hasA;
            LastActiveB = hasB;
            LastFreqA = freqA; LastIntA = intA;
            LastFreqB = freqB; LastIntB = intB;

            if (SwapChannels)
            {
                (valA, valB) = (valB, valA);
                (modeA, modeB) = (modeB, modeA);
                (freqA, freqB) = (freqB, freqA);
                (intA, intB) = (intB, intA);
                (LastFreqA, LastFreqB) = (LastFreqB, LastFreqA);
                (LastIntA, LastIntB) = (LastIntB, LastIntA);
                (LastActiveA, LastActiveB) = (LastActiveB, LastActiveA);
                (LimitScaleA, LimitScaleB) = (LimitScaleB, LimitScaleA);
            }

            return (modeA, valA, modeB, valB, freqA, intA, freqB, intB);
        }
    }

    static (byte[] freq, byte[] intensity) PopWave(
        ConcurrentQueue<WaveFrame> stream,
        byte[]? loopFreq, byte[]? loopInt)
    {
        if (stream.TryDequeue(out var frame))
            return (frame.Freq, frame.Intensity);

        if (loopFreq != null && loopInt != null)
            return (loopFreq, loopInt);

        return (B0.ChannelOff, B0.IntensityOff);
    }
}