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,
	}
}

// NewChebyshevI creates an Nth-order Chebyshev Type I lowpass filter.
// Passband ripple in dB (typ. 0.5), then steep rolloff into stopband.
// Much steeper transition band than Butterworth at the same order.
// At 228kHz, 8th-order, 0.5dB ripple, fc=15kHz: -40dB@19kHz (vs -17dB Butterworth).
func NewChebyshevI(order int, rippleDB, cutoffHz, sampleRate float64) *FilterChain {
	if order < 2 || order%2 != 0 {
		return &FilterChain{Stages: []Biquad{{b0: 1}}}
	}
	if cutoffHz <= 0 || sampleRate <= 0 || cutoffHz >= sampleRate/2 {
		return &FilterChain{Stages: []Biquad{{b0: 1}}}
	}

	N := order
	nSections := N / 2

	// Chebyshev parameters
	epsilon := math.Sqrt(math.Pow(10, rippleDB/10) - 1)
	v := math.Asinh(1/epsilon) / float64(N)

	// Bilinear transform constant and frequency pre-warp
	c := 2.0 * sampleRate
	warp := c * math.Tan(math.Pi*cutoffHz/sampleRate)

	stages := make([]Biquad, nSections)

	for i := 0; i < nSections; i++ {
		// Analog prototype pole (normalized Ωc=1)
		angle := float64(2*i+1) * math.Pi / float64(2*N)
		sigmaN := -math.Sinh(v) * math.Sin(angle)
		omegaN := math.Cosh(v) * math.Cos(angle)

		// Scale to actual cutoff frequency
		sigma := sigmaN * warp
		omega := omegaN * warp

		// Analog section: H(s) = A / (s² + Bs + A)
		A := sigma*sigma + omega*omega
		B := -2 * sigma // positive (sigma is negative)

		// Bilinear transform to digital biquad
		c2 := c * c
		a0 := c2 + B*c + A

		stages[i] = Biquad{
			b0: A / a0,
			b1: 2 * A / a0,
			b2: A / a0,
			a1: (-2*c2 + 2*A) / a0,
			a2: (c2 - B*c + A) / a0,
		}
	}

	// Normalize DC gain to unity (Chebyshev even-order has -ripple at DC)
	dcGain := 1.0
	for _, s := range stages {
		dcGain *= (s.b0 + s.b1 + s.b2) / (1 + s.a1 + s.a2)
	}
	if dcGain > 0 {
		corr := 1.0 / dcGain
		stages[0].b0 *= corr
		stages[0].b1 *= corr
		stages[0].b2 *= corr
	}

	return &FilterChain{Stages: stages}
}

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

// NewAudioLPF creates the broadcast-standard audio lowpass at 15kHz.
// 8th-order Chebyshev Type I with 0.5dB passband ripple.
// Flat to 15kHz, then steep wall: -40dB@19kHz (vs -17dB Butterworth).
// Two passes through clip-filter-clip: -80dB broadband at 19kHz.
func NewAudioLPF(sampleRate float64) *FilterChain {
	return NewChebyshevI(8, 0.5, 15000, sampleRate)
}

// NewPilotNotch creates a double-cascade 19kHz notch for maximum
// rejection at the pilot frequency. Q=15: only 1.3kHz wide (18.4–19.6kHz).
// The 8th-order LPF handles broadband; this kills the exact 19kHz peak.
func NewPilotNotch(sampleRate float64) *FilterChain {
	return &FilterChain{
		Stages: []Biquad{
			*NewNotch(19000, sampleRate, 15),
			*NewNotch(19000, sampleRate, 15),
		},
	}
}

// NewCompositeProtection creates double-cascade notch filters for the
// composite clipper. Q=10: ~1.9kHz wide at 19kHz, ~5.7kHz wide at 57kHz.
// Narrow enough to preserve audio/stereo, deep enough to protect pilot/RDS.
func NewCompositeProtection(sampleRate float64) (notch19, notch57 *FilterChain) {
	notch19 = &FilterChain{
		Stages: []Biquad{
			*NewNotch(19000, sampleRate, 10),
			*NewNotch(19000, sampleRate, 10),
		},
	}
	notch57 = &FilterChain{
		Stages: []Biquad{
			*NewNotch(57000, sampleRate, 10),
			*NewNotch(57000, sampleRate, 10),
		},
	}
	return
}