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

package dsp

import "math"

// PreEmphasis implements a first-order high-shelf filter for FM broadcast
// pre-emphasis. Standard time constants are 50 µs (Europe/world) and
// 75 µs (North America/South Korea).
//
// Transfer function: H(s) = 1 + s*τ
// Bilinear transform to discrete: H(z) = (b0 + b1*z^-1) / (1 + a1*z^-1)
type PreEmphasis struct {
	b0, b1, a1 float64
	x1, y1     float64 // state
	enabled    bool
}

// NewPreEmphasis creates a pre-emphasis filter for the given time constant
// and sample rate. tau is in microseconds (50 or 75).
func NewPreEmphasis(tauMicroseconds, sampleRate float64) *PreEmphasis {
	if tauMicroseconds <= 0 || sampleRate <= 0 {
		return &PreEmphasis{enabled: false}
	}
	tau := tauMicroseconds * 1e-6

	// Bilinear transform of H(s) = 1 + s*tau
	// With pre-warping: let c = 2*fs
	// Numerator: (1 + tau*c) + (-1 + tau*c)*z^-1  =>  b0 = 1+tau*c, b1 = -1+tau*c
	// Denominator: (1 + tau*c) + (1 - tau*c)*z^-1  (but we normalize so a0=1)
	// Wait - cleaner approach: standard first-order shelf.

	// H(s) = (1 + s*tau) mapped with bilinear: s = c*(1 - z^-1)/(1 + z^-1), c = 2*fs
	// De-emphasis: H_de(z) = (1-alpha)/(1 - alpha*z^-1), alpha = exp(-1/(tau*fs))
	// Pre-emphasis: H_pre(z) = 1/H_de(z) = (1 - alpha*z^-1)/(1-alpha)
	// y[n] = (x[n] - alpha*x[n-1]) / (1 - alpha)
	alpha := math.Exp(-1.0 / (tau * sampleRate))
	gain := 1.0 / (1.0 - alpha)

	return &PreEmphasis{
		b0:      gain,
		b1:      -alpha * gain,
		a1:      0, // FIR, no feedback
		enabled: true,
	}
}

// Process applies the pre-emphasis filter to a single sample.
func (p *PreEmphasis) Process(in float64) float64 {
	if !p.enabled {
		return in
	}
	out := p.b0*in + p.b1*p.x1
	p.x1 = in
	return out
}

// Reset clears the filter state.
func (p *PreEmphasis) Reset() {
	p.x1 = 0
	p.y1 = 0
}

// DeEmphasis implements the complementary de-emphasis filter.
// H(z) = (1-alpha)/(1 - alpha*z^-1)
type DeEmphasis struct {
	alpha   float64
	gain    float64
	prevOut float64
	enabled bool
}

// NewDeEmphasis creates a de-emphasis filter.
func NewDeEmphasis(tauMicroseconds, sampleRate float64) *DeEmphasis {
	if tauMicroseconds <= 0 || sampleRate <= 0 {
		return &DeEmphasis{enabled: false}
	}
	tau := tauMicroseconds * 1e-6
	alpha := math.Exp(-1.0 / (tau * sampleRate))
	return &DeEmphasis{alpha: alpha, gain: 1.0 - alpha, enabled: true}
}

// Process applies the de-emphasis filter.
func (d *DeEmphasis) Process(in float64) float64 {
	if !d.enabled {
		return in
	}
	out := d.gain*in + d.alpha*d.prevOut
	d.prevOut = out
	return out
}

// Reset clears the filter state.
func (d *DeEmphasis) Reset() {
	d.prevOut = 0
}