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"
)

// 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; Omnia-style)
)

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

// 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: Hilbert transform for quadrature modulation
	hilbert *dsp.HilbertFilter

	// VSB: crossover filter splits L-R into low (<200Hz, DSB) and high (>200Hz, SSB)
	vsbLPF    *dsp.FilterChain // 200 Hz LPF for VSB low band
	vsbHPF    *dsp.FilterChain // 200 Hz HPF for VSB high band (derived from allpass - LPF)
	hilbertHi *dsp.HilbertFilter // separate Hilbert for high band
}

// 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. Creates Hilbert filter if needed.
func (s *StereoEncoder) SetMode(mode Mode, sampleRate float64) {
	s.mode = mode
	if mode == ModeSSB || mode == ModeVSB {
		// 127-tap Hilbert FIR at 228kHz: group delay = 63 samples = 0.276ms
		s.hilbert = dsp.NewHilbertFilter(127)
	}
	if mode == ModeVSB {
		// Crossover at 200 Hz for vestigial sideband
		s.vsbLPF = dsp.NewLPF4(200, sampleRate)
		s.vsbHPF = dsp.NewLPF4(200, sampleRate) // we'll subtract to get HPF
		s.hilbertHi = dsp.NewHilbertFilter(127)
	}
}

// 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())

	var stereoOut float64

	switch s.mode {
	case ModeSSB:
		if s.hilbert == nil {
			// Fallback to DSB if not initialized
			stereoOut = diff * sub38sin
			break
		}
		// SSB-LSB: s(t) = m_delayed(t)·sin(ωt) - m̂(t)·cos(ωt)
		// The - sign selects LSB (below 38 kHz).
		// ×2 compensates for removed USB (+6 dB).
		delayed, hilb := s.hilbert.Process(diff)
		stereoOut = 2 * (delayed*sub38sin - hilb*sub38cos)

	case ModeVSB:
		if s.hilbert == nil || s.vsbLPF == nil {
			stereoOut = diff * sub38sin
			break
		}
		// VSB: 0-200Hz → DSB, 200Hz+ → SSB-LSB
		lo := s.vsbLPF.Process(diff)
		hiRef := s.vsbHPF.Process(diff) // same LPF for HPF derivation
		hi := diff - hiRef              // highpass = original - lowpass (requires matching delay, approximate)

		// Low band: standard DSB
		dsbPart := lo * sub38sin

		// High band: SSB-LSB with Hilbert
		delayed, hilb := s.hilbertHi.Process(hi)
		ssbPart := 2 * (delayed*sub38sin - hilb*sub38cos)

		stereoOut = dsbPart + ssbPart

	default: // ModeDSB
		stereoOut = diff * sub38sin
	}

	return Components{
		Mono:   mono,
		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()
	}
}

// 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)
}