Substation/MusicAnalyzer.cs

using System.Numerics;
using FftSharp;

namespace Substation;

public static class MusicAnalyzer
{
    public const int WindowSize = 2048;
    public const int HopSize = 1024;
    public const double TickDuration = 0.1;

    const double RhythmLow = 20, RhythmHigh = 250;
    const double MelodyLow = 300, MelodyHigh = 4000;

    public static (double rhythmEnergy, double rhythmFlux, double melodyEnergy, double melodyFreq)
        ExtractFeatures(double[] magnitude, double[]? prevRhythmMag, int sampleRate)
    {
        double binWidth = (double)sampleRate / WindowSize;
        int rhythmLoBin = (int)(RhythmLow / binWidth);
        int rhythmHiBin = (int)(RhythmHigh / binWidth);
        int melodyLoBin = (int)(MelodyLow / binWidth);
        int melodyHiBin = (int)(MelodyHigh / binWidth);

        // Rhythm band energy
        double rhythmEnergy = 0;
        for (int i = rhythmLoBin; i <= rhythmHiBin && i < magnitude.Length; i++)
            rhythmEnergy += magnitude[i] * magnitude[i];
        rhythmEnergy = Math.Sqrt(rhythmEnergy / (rhythmHiBin - rhythmLoBin + 1));

        // Spectral flux (positive change in rhythm band)
        double rhythmFlux = 0;
        if (prevRhythmMag != null)
        {
            for (int i = rhythmLoBin; i <= rhythmHiBin && i < magnitude.Length; i++)
            {
                double diff = magnitude[i] - prevRhythmMag[i];
                if (diff > 0) rhythmFlux += diff;
            }
        }

        // Melody band: energy + dominant frequency (spectral peak)
        double melodyEnergy = 0;
        double peakMag = 0;
        int peakBin = melodyLoBin;
        for (int i = melodyLoBin; i <= melodyHiBin && i < magnitude.Length; i++)
        {
            double m = magnitude[i];
            melodyEnergy += m * m;
            if (m > peakMag)
            {
                peakMag = m;
                peakBin = i;
            }
        }
        melodyEnergy = Math.Sqrt(melodyEnergy / (melodyHiBin - melodyLoBin + 1));
        double melodyFreq = peakBin * binWidth;

        return (rhythmEnergy, rhythmFlux, melodyEnergy, melodyFreq);
    }

    public static int MapPitchToPeriod(double hz)
    {
        // Map melody frequency (300-4000Hz) to e-stim period (10-1000ms)
        // Logarithmic mapping: low notes → deep, high notes → buzzy
        double logFreq = Math.Log(Math.Clamp(hz, MelodyLow, MelodyHigh));
        double logMin = Math.Log(MelodyLow);
        double logMax = Math.Log(MelodyHigh);
        double t = (logFreq - logMin) / (logMax - logMin); // 0..1
        // Invert: high freq → short period (buzzy), low freq → long period (deep)
        int ms = (int)Math.Round(1000 - t * 990); // 1000ms..10ms
        return Math.Clamp(ms, 10, 1000);
    }

    public class TickFeature
    {
        public double RhythmFlux;
        public double MelodyEnergy;
        public double MelodyCount;
        public readonly List<double> MelodyFreqSamples = new();
    }

    public static (WaveFrame chA, WaveFrame chB) BuildWaveFrame(TickFeature tf, double maxFlux, double maxMelodyEnergy)
    {
        const int rhythmFreqMs = 150;

        double normalizedFlux = maxFlux > 0 ? tf.RhythmFlux / maxFlux : 0;
        int onsetIntensity = (int)Math.Round(normalizedFlux * 100);
        var intA = new[]
        {
            (byte)Math.Clamp(onsetIntensity, 0, 100),
            (byte)Math.Clamp(onsetIntensity * 6 / 10, 0, 100),
            (byte)Math.Clamp(onsetIntensity * 3 / 10, 0, 100),
            (byte)0
        };
        var freqA = Freq.Compress4(new[] { rhythmFreqMs, rhythmFreqMs, rhythmFreqMs, rhythmFreqMs });

        double avgEnergy = tf.MelodyCount > 0 ? tf.MelodyEnergy / tf.MelodyCount : 0;
        double normalizedEnergy = maxMelodyEnergy > 0 ? avgEnergy / maxMelodyEnergy : 0;
        int melodyIntensity = (int)Math.Round(normalizedEnergy * 80);
        melodyIntensity = Math.Clamp(melodyIntensity, 0, 100);

        double weightedFreq = 0;
        double totalWeight = 0;
        foreach (var f in tf.MelodyFreqSamples)
        {
            weightedFreq += f * f;
            totalWeight += f;
        }
        double avgMelodyHz = totalWeight > 0 ? weightedFreq / totalWeight : 500;
        int estimsMs = MapPitchToPeriod(avgMelodyHz);

        var intB = new[]
        {
            (byte)melodyIntensity, (byte)melodyIntensity,
            (byte)melodyIntensity, (byte)melodyIntensity
        };
        var freqB = Freq.Compress4(new[] { estimsMs, estimsMs, estimsMs, estimsMs });

        return (new WaveFrame(freqA, intA), new WaveFrame(freqB, intB));
    }
}