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

package dsp

import "math"

// Biquad is a generic second-order IIR filter (direct form II transposed).
type Biquad struct {
	b0, b1, b2 float64
	a1, a2     float64
	z1, z2     float64
}

// Process filters one sample.
func (f *Biquad) Process(in float64) float64 {
	out := f.b0*in + f.z1
	f.z1 = f.b1*in - f.a1*out + f.z2
	f.z2 = f.b2*in - f.a2*out
	return out
}

// Reset clears state.
func (f *Biquad) Reset() { f.z1 = 0; f.z2 = 0 }

// FilterChain cascades multiple biquad sections in series.
// Used for higher-order filters (e.g. 4th-order = 2 biquads).
type FilterChain struct {
	Stages []Biquad
}

// Process runs input through all stages in series.
func (c *FilterChain) Process(in float64) float64 {
	x := in
	for i := range c.Stages {
		x = c.Stages[i].Process(x)
	}
	return x
}

// Reset clears all filter state.
func (c *FilterChain) Reset() {
	for i := range c.Stages {
		c.Stages[i].Reset()
	}
}

// --- Factory functions ---

// NewBiquadLPF creates a 2nd-order Butterworth lowpass (Q = 1/√2).
func NewBiquadLPF(cutoffHz, sampleRate float64) *Biquad {
	return newBiquadLPFWithQ(cutoffHz, sampleRate, math.Sqrt2/2)
}

func newBiquadLPFWithQ(cutoffHz, sampleRate, q float64) *Biquad {
	if cutoffHz <= 0 || sampleRate <= 0 || cutoffHz >= sampleRate/2 {
		return &Biquad{b0: 1} // passthrough
	}
	omega := 2 * math.Pi * cutoffHz / sampleRate
	cosW := math.Cos(omega)
	sinW := math.Sin(omega)
	alpha := sinW / (2 * q)
	a0 := 1 + alpha
	return &Biquad{
		b0: (1 - cosW) / 2 / a0,
		b1: (1 - cosW) / a0,
		b2: (1 - cosW) / 2 / a0,
		a1: (-2 * cosW) / a0,
		a2: (1 - alpha) / a0,
	}
}

// NewLPF4 creates a 4th-order Butterworth lowpass (two cascaded biquads).
func NewLPF4(cutoffHz, sampleRate float64) *FilterChain {
	q1 := 1.0 / (2 * math.Cos(math.Pi/8))   // ≈ 0.5412
	q2 := 1.0 / (2 * math.Cos(3*math.Pi/8))  // ≈ 1.3066
	return &FilterChain{
		Stages: []Biquad{
			*newBiquadLPFWithQ(cutoffHz, sampleRate, q1),
			*newBiquadLPFWithQ(cutoffHz, sampleRate, q2),
		},
	}
}

// NewLPF8 creates an 8th-order Butterworth lowpass (four cascaded biquads).
// Provides -48dB/octave rolloff. At 228kHz with fc=15kHz:
//
//	15kHz: -6dB, 19kHz: -28dB, 38kHz: -72dB, 57kHz: -108dB
func NewLPF8(cutoffHz, sampleRate float64) *FilterChain {
	// 8th-order Butterworth pole angles: π/16, 3π/16, 5π/16, 7π/16
	q1 := 1.0 / (2 * math.Cos(math.Pi/16))    // ≈ 0.5098
	q2 := 1.0 / (2 * math.Cos(3*math.Pi/16))   // ≈ 0.6013
	q3 := 1.0 / (2 * math.Cos(5*math.Pi/16))   // ≈ 0.8999
	q4 := 1.0 / (2 * math.Cos(7*math.Pi/16))   // ≈ 2.5629
	return &FilterChain{
		Stages: []Biquad{
			*newBiquadLPFWithQ(cutoffHz, sampleRate, q1),
			*newBiquadLPFWithQ(cutoffHz, sampleRate, q2),
			*newBiquadLPFWithQ(cutoffHz, sampleRate, q3),
			*newBiquadLPFWithQ(cutoffHz, sampleRate, q4),
		},
	}
}

// NewNotch creates a 2nd-order IIR notch (bandstop) filter.
// Q controls width: higher Q = narrower notch.
// Typical: Q=5 → ~4kHz wide at -3dB, Q=10 → ~2kHz wide.
func NewNotch(centerHz, sampleRate, q float64) *Biquad {
	if centerHz <= 0 || sampleRate <= 0 || centerHz >= sampleRate/2 {
		return &Biquad{b0: 1}
	}
	omega := 2 * math.Pi * centerHz / sampleRate
	cosW := math.Cos(omega)
	alpha := math.Sin(omega) / (2 * q)
	a0 := 1 + alpha
	return &Biquad{
		b0: 1 / a0,
		b1: -2 * cosW / a0,
		b2: 1 / a0,
		a1: -2 * cosW / a0,
		a2: (1 - alpha) / a0,
	}
}

// --- Broadcast-specific filter factories ---

// NewAudioLPF creates the broadcast-standard audio lowpass at 14kHz.
// 8th-order Butterworth: -21dB@19kHz per pass. Two passes through the
// clip-filter-clip loop give -42dB broadband floor at 19kHz.
func NewAudioLPF(sampleRate float64) *FilterChain {
	return NewLPF8(14000, sampleRate)
}

// NewPilotNotch creates a double-cascade 19kHz notch for maximum
// rejection at the pilot frequency. Two stages give >60dB rejection.
// Applied BEFORE stereo encoding to kill audio energy at 19kHz.
func NewPilotNotch(sampleRate float64) *FilterChain {
	return &FilterChain{
		Stages: []Biquad{
			*NewNotch(19000, sampleRate, 5),
			*NewNotch(19000, sampleRate, 5),
		},
	}
}

// NewCompositeProtection creates double-cascade notch filters for the
// composite clipper. Each band gets two notch stages for >60dB rejection.
// Applied to clipped audio composite to remove clip harmonics from the
// pilot (19kHz) and RDS (57kHz) bands.
func NewCompositeProtection(sampleRate float64) (notch19, notch57 *FilterChain) {
	notch19 = &FilterChain{
		Stages: []Biquad{
			*NewNotch(19000, sampleRate, 3),
			*NewNotch(19000, sampleRate, 3),
		},
	}
	notch57 = &FilterChain{
		Stages: []Biquad{
			*NewNotch(57000, sampleRate, 3),
			*NewNotch(57000, sampleRate, 3),
		},
	}
	return
}