fm-rds-tx/internal/stereo/encoder.go

package stereo

import (
	"math"

	"github.com/jan/fm-rds-tx/internal/audio"
	"github.com/jan/fm-rds-tx/internal/dsp"
)

const ssbHilbertTaps = 127

// Mode selects the stereo subcarrier modulation method.
type Mode int

const (
	ModeDSB Mode = iota // Standard DSB-SC (FCC §73.322 compliant)
	ModeSSB             // SSB-SC LSB only (Foti/Tarsio, experimental)
	ModeVSB             // Vestigial SB (0-200Hz DSB, above SSB; experimental)
)

// ParseMode converts a string to Mode. Returns ModeDSB for unknown values.
func ParseMode(s string) Mode {
	switch s {
	case "SSB", "ssb":
		return ModeSSB
	case "VSB", "vsb":
		return ModeVSB
	default:
		return ModeDSB
	}
}

// String returns the mode name.
func (m Mode) String() string {
	switch m {
	case ModeSSB:
		return "SSB"
	case ModeVSB:
		return "VSB"
	default:
		return "DSB"
	}
}

// Components holds the individual MPX components produced by the stereo encoder.
// All outputs are unity-normalized. The combiner controls actual injection levels.
type Components struct {
	Mono   float64 // (L+R)/2 baseband
	Stereo float64 // (L-R)/2 modulated onto 38 kHz subcarrier
	Pilot  float64 // sin(pilotPhase), unity amplitude
}

// sampleDelay is a simple fixed-sample delay line.
type sampleDelay struct {
	buf []float64
	pos int
}

func newSampleDelay(samples int) *sampleDelay {
	if samples <= 0 {
		return nil
	}
	return &sampleDelay{buf: make([]float64, samples)}
}

func (d *sampleDelay) Process(in float64) float64 {
	if d == nil || len(d.buf) == 0 {
		return in
	}
	out := d.buf[d.pos]
	d.buf[d.pos] = in
	d.pos++
	if d.pos >= len(d.buf) {
		d.pos = 0
	}
	return out
}

func (d *sampleDelay) Reset() {
	if d == nil {
		return
	}
	for i := range d.buf {
		d.buf[i] = 0
	}
	d.pos = 0
}

// StereoEncoder generates stereo MPX primitives from stereo audio frames.
// Supports DSB-SC (standard), SSB-SC (lower sideband only), and VSB modes.
type StereoEncoder struct {
	pilot     dsp.PilotGenerator
	lastPhase float64
	mode      Mode

	// SSB/VSB paths use a Hilbert transformer. The Hilbert FIR introduces a
	// fixed group delay, so the mono path must be delayed by the same amount.
	hilbert   *dsp.HilbertFilter
	monoDelay *sampleDelay

	// VSB remains experimental. The low band is kept as DSB, while the high band
	// is encoded as SSB. Mono delay compensation still applies so the decoded
	// stereo matrix does not produce comb/reverb artefacts.
	vsbLPF    *dsp.FilterChain
	hilbertHi *dsp.HilbertFilter
}

// NewStereoEncoder creates a StereoEncoder configured for the provided sample rate.
func NewStereoEncoder(sampleRate float64) StereoEncoder {
	return StereoEncoder{
		pilot: dsp.NewPilotGenerator(sampleRate),
		mode:  ModeDSB,
	}
}

// SetMode changes the stereo encoding mode and (re)initializes internal state.
func (s *StereoEncoder) SetMode(mode Mode, sampleRate float64) {
	s.mode = mode
	s.hilbert = nil
	s.hilbertHi = nil
	s.vsbLPF = nil
	s.monoDelay = nil

	switch mode {
	case ModeSSB:
		s.hilbert = dsp.NewHilbertFilter(ssbHilbertTaps)
		s.monoDelay = newSampleDelay((ssbHilbertTaps - 1) / 2)
	case ModeVSB:
		s.hilbert = dsp.NewHilbertFilter(ssbHilbertTaps)
		s.hilbertHi = dsp.NewHilbertFilter(ssbHilbertTaps)
		s.vsbLPF = dsp.NewLPF4(200, sampleRate)
		s.monoDelay = newSampleDelay((ssbHilbertTaps - 1) / 2)
	}
}

// Encode converts a stereo frame into MPX components.
func (s *StereoEncoder) Encode(frame audio.Frame) Components {
	pilotPhase := s.pilot.Phase()
	s.lastPhase = pilotPhase
	pilot := s.pilot.Sample()

	sub38sin := math.Sin(2 * math.Pi * 2 * pilotPhase)
	sub38cos := math.Cos(2 * math.Pi * 2 * pilotPhase)

	diff := float64(frame.Difference())
	mono := float64(frame.Mono())
	monoOut := mono

	var stereoOut float64

	switch s.mode {
	case ModeSSB:
		if s.hilbert == nil {
			stereoOut = diff * sub38sin
			break
		}
		monoOut = s.monoDelay.Process(mono)
		// SSB-LSB: s(t) = m_delayed(t)·sin(ωt) - m̂(t)·cos(ωt)
		// The - sign selects LSB (below 38 kHz).
		// ×2 compensates for the removed upper sideband (+6 dB).
		delayedDiff, hilb := s.hilbert.Process(diff)
		stereoOut = 2 * (delayedDiff*sub38sin - hilb*sub38cos)

	case ModeVSB:
		if s.hilbertHi == nil || s.vsbLPF == nil {
			stereoOut = diff * sub38sin
			break
		}
		monoOut = s.monoDelay.Process(mono)

		// Experimental VSB split:
		// - 0..200 Hz remains DSB to avoid aggressive low-frequency image shift.
		// - Above 200 Hz the residual is encoded as SSB-LSB.
		// The critical reverb bug was the mono-vs-diff timing mismatch; that is
		// fixed here by delaying mono by the Hilbert group delay.
		lo := s.vsbLPF.Process(diff)
		hi := diff - lo

		dsbPart := lo * sub38sin
		delayedHi, hilbHi := s.hilbertHi.Process(hi)
		ssbPart := 2 * (delayedHi*sub38sin - hilbHi*sub38cos)

		stereoOut = dsbPart + ssbPart

	default: // ModeDSB
		stereoOut = diff * sub38sin
	}

	return Components{
		Mono:   monoOut,
		Stereo: stereoOut,
		Pilot:  pilot,
	}
}

// Reset restarts the pilot generator and clears filter state.
func (s *StereoEncoder) Reset() {
	s.pilot.Reset()
	if s.hilbert != nil {
		s.hilbert.Reset()
	}
	if s.hilbertHi != nil {
		s.hilbertHi.Reset()
	}
	if s.vsbLPF != nil {
		s.vsbLPF.Reset()
	}
	if s.monoDelay != nil {
		s.monoDelay.Reset()
	}
}

// PilotPhase returns the pilot phase used in the most recent Encode() call.
func (s *StereoEncoder) PilotPhase() float64 {
	return s.lastPhase
}

// RDSCarrier returns sin(3 * pilotPhase * 2π) — the 57 kHz carrier
// phase-locked to the pilot, as required by the RDS standard.
func (s *StereoEncoder) RDSCarrier() float64 {
	return math.Sin(2 * math.Pi * 3 * s.lastPhase)
}