fm-rds-tx/internal/dsp/bs412.go

package dsp

import "math"

// BS412Limiter implements ITU-R BS.412 MPX power limiting.
// Measures the rolling 60-second average power of the composite signal
// and reduces audio gain when the power exceeds the threshold.
//
// The threshold is specified in dBr where 0 dBr is the reference power
// of a fully modulated mono signal (composite peak = 1.0, power = 0.5).
//
// Pilot and RDS power are accounted for: the audio power budget is
// reduced by their constant contribution so the total stays within limits.
type BS412Limiter struct {
	enabled       bool
	thresholdPow  float64 // linear power threshold for total MPX
	audioBudget   float64 // = thresholdPow - pilotPow - rdsPow

	// Rolling 60-second power integrator
	powerBuf  []float64 // per-chunk average power values
	bufIdx    int
	bufFull   bool   // true once the buffer has wrapped at least once
	powerSum  float64

	// Slow gain controller
	gain          float64 // current output gain (0..1)
	attackCoeff   float64 // gain reduction speed
	releaseCoeff  float64 // gain recovery speed
}

// NewBS412Limiter creates a BS.412 MPX power limiter.
//
// Parameters:
//   - thresholdDBr: power limit in dBr (0 = standard, +3 = relaxed)
//   - pilotLevel: pilot amplitude in composite (e.g. 0.09)
//   - rdsInjection: RDS amplitude in composite (e.g. 0.04)
//   - chunkDurationSec: duration of each processing chunk (e.g. 0.05 for 50ms)
func NewBS412Limiter(thresholdDBr, pilotLevel, rdsInjection, chunkDurationSec float64) *BS412Limiter {
	// Reference power: 0 dBr = power of mono sine at peak=1.0 = 0.5
	refPower := 0.5
	thresholdPow := refPower * math.Pow(10, thresholdDBr/10)

	// Constant power contributions from pilot and RDS
	pilotPow := pilotLevel * pilotLevel / 2   // sine wave RMS²
	rdsPow := rdsInjection * rdsInjection / 4 // BPSK has ~half the power of a sine

	audioBudget := thresholdPow - pilotPow - rdsPow
	if audioBudget < 0.01 {
		audioBudget = 0.01
	}

	// 60-second window in chunks
	windowSec := 60.0
	bufLen := int(math.Ceil(windowSec / chunkDurationSec))
	if bufLen < 10 {
		bufLen = 10
	}

	// Attack: ~2 seconds (slow, avoids pumping)
	// Release: ~5 seconds (very slow, smooth recovery)
	attackTC := 2.0 / chunkDurationSec   // time constant in chunks
	releaseTC := 5.0 / chunkDurationSec

	return &BS412Limiter{
		enabled:      true,
		thresholdPow: thresholdPow,
		audioBudget:  audioBudget,
		powerBuf:     make([]float64, bufLen),
		gain:         1.0,
		attackCoeff:  1.0 - math.Exp(-1.0/attackTC),
		releaseCoeff: 1.0 - math.Exp(-1.0/releaseTC),
	}
}

// UpdateChunkDuration reconfigures the limiter for a new chunk size.
// Call this from GenerateFrame when the actual chunk duration is known
// (computed as samples/sampleRate) to avoid calibration errors if the
// engine's chunk duration differs from the value passed to NewBS412Limiter.
// Safe to call on every chunk; no-ops when duration has not changed.
func (l *BS412Limiter) UpdateChunkDuration(chunkSec float64) {
	if chunkSec <= 0 {
		return
	}
	windowSec := 60.0
	newBufLen := int(math.Ceil(windowSec / chunkSec))
	if newBufLen < 10 {
		newBufLen = 10
	}
	if newBufLen == len(l.powerBuf) {
		return // no change
	}
	// Resize buffer — drop history to avoid stale power readings from the
	// old window size distorting the rolling average.
	l.powerBuf = make([]float64, newBufLen)
	l.bufIdx = 0
	l.bufFull = false
	l.powerSum = 0
	attackTC := 2.0 / chunkSec
	releaseTC := 5.0 / chunkSec
	l.attackCoeff = 1.0 - math.Exp(-1.0/attackTC)
	l.releaseCoeff = 1.0 - math.Exp(-1.0/releaseTC)
}

// ProcessChunk measures the audio power of a chunk and returns the
// gain factor to apply to the audio composite for BS.412 compliance.
// Call once per chunk with the average audio power of that chunk.
//
// audioPower = (1/N) × Σ sample²  over the chunk's audio composite samples.
func (l *BS412Limiter) ProcessChunk(audioPower float64) float64 {
	if !l.enabled {
		return 1.0
	}

	// Update rolling 60-second power average
	old := l.powerBuf[l.bufIdx]
	l.powerBuf[l.bufIdx] = audioPower
	l.powerSum += audioPower - old
	// BUG-G fix: float64 accumulation over 1200+ chunks can drift slightly
	// negative due to rounding. A negative powerSum → negative avgPower →
	// math.Sqrt of negative → NaN → gain becomes NaN, silently disabling
	// the limiter. Clamp to zero to keep the invariant powerSum >= 0.
	if l.powerSum < 0 {
		l.powerSum = 0
	}
	l.bufIdx++
	if l.bufIdx >= len(l.powerBuf) {
		l.bufIdx = 0
		l.bufFull = true
	}

	// Calculate average power over the window
	var count int
	if l.bufFull {
		count = len(l.powerBuf)
	} else {
		count = l.bufIdx
	}
	if count < 1 {
		return 1.0
	}
	avgPower := l.powerSum / float64(count)

	// Target gain: bring average audio power to budget
	targetGain := 1.0
	if avgPower > l.audioBudget && avgPower > 0 {
		targetGain = math.Sqrt(l.audioBudget / avgPower)
	}

	// Smooth gain changes (slow attack, slower release)
	if targetGain < l.gain {
		l.gain += l.attackCoeff * (targetGain - l.gain)
	} else {
		l.gain += l.releaseCoeff * (targetGain - l.gain)
	}

	// Clamp
	if l.gain < 0.01 {
		l.gain = 0.01
	}
	if l.gain > 1.0 {
		l.gain = 1.0
	}

	return l.gain
}

// CurrentGain returns the current gain factor (0..1).
// Called at the start of each chunk to get the gain to apply.
func (l *BS412Limiter) CurrentGain() float64 {
	return l.gain
}

// CurrentGainDB returns the current gain reduction in dB (negative = reducing).
func (l *BS412Limiter) CurrentGainDB() float64 {
	if l.gain <= 0 {
		return -100
	}
	return 20 * math.Log10(l.gain)
}

// Reset clears the power history and restores unity gain.
func (l *BS412Limiter) Reset() {
	for i := range l.powerBuf {
		l.powerBuf[i] = 0
	}
	l.bufIdx = 0
	l.bufFull = false
	l.powerSum = 0
	l.gain = 1.0
}