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

package rds

// RDS encoder following the PiFmRds reference implementation.
// Generates shaped biphase BPSK at 228 kHz (4× 57 kHz).
//
// Architecture:
//   raw bits → CRC+offset → differential encoding → shaped biphase waveform
//   → 57 kHz modulation via {0,+1,0,-1} at 228 kHz
//
// The shaped biphase waveform is a pre-computed RRC-filtered Manchester pulse.
// Successive symbols overlap-add in a ring buffer, eliminating ISI.
// 57 kHz carrier is exact at 228 kHz (period = 4 samples), no sin() needed.
//
// Call NextSample228k() at exactly 228 kHz to get the modulated RDS subcarrier.

const (
	rdsRate       = 228000.0 // internal sample rate, must be 4×57000
	rdsBitRate    = 1187.5   // bits per second
	samplesPerBit = 192      // rdsRate / rdsBitRate = 228000/1187.5 = 192
	bitsPerBlock  = 26
	bitsPerGroup  = 4 * bitsPerBlock // 104
)

const crcPoly = 0x1B9

var offsetWords = map[byte]uint16{
	'A': 0x0FC, 'B': 0x198, 'C': 0x168, 'c': 0x350, 'D': 0x1B4,
}

func crc10(data uint16) uint16 {
	var reg uint32 = uint32(data) << 10
	for i := 15; i >= 0; i-- {
		if reg&(1<<(uint(i)+10)) != 0 {
			reg ^= uint32(crcPoly) << uint(i)
		}
	}
	return uint16(reg & 0x3FF)
}

func encodeBlock(data uint16, offset byte) uint32 {
	return (uint32(data) << 10) | uint32(crc10(data)^offsetWords[offset])
}

// --- Group building (unchanged) ---

func buildGroup0A(pi uint16, pty uint8, tp, ta bool, segIdx int, ps string) [4]uint16 {
	ps = normalizePS(ps)
	var bB uint16
	if tp {
		bB |= 1 << 10
	}
	bB |= uint16(pty&0x1F) << 5
	if ta {
		bB |= 1 << 4
	}
	bB |= 1 << 3
	bB |= uint16(segIdx & 0x03)
	ci := segIdx * 2
	return [4]uint16{pi, bB, pi, (uint16(ps[ci]) << 8) | uint16(ps[ci+1])}
}

func buildGroup2A(pi uint16, pty uint8, tp bool, abFlag bool, segIdx int, rt string) [4]uint16 {
	rt = normalizeRT(rt)
	var bB uint16 = 2 << 12
	if tp {
		bB |= 1 << 10
	}
	bB |= uint16(pty&0x1F) << 5
	if abFlag {
		bB |= 1 << 4
	}
	bB |= uint16(segIdx & 0x0F)
	ci := segIdx * 4
	c0, c1, c2, c3 := padRT(rt, ci)
	return [4]uint16{pi, bB, (uint16(c0) << 8) | uint16(c1), (uint16(c2) << 8) | uint16(c3)}
}

func padRT(rt string, off int) (byte, byte, byte, byte) {
	g := func(i int) byte {
		if i < len(rt) {
			return rt[i]
		}
		return ' '
	}
	return g(off), g(off + 1), g(off + 2), g(off + 3)
}

// --- Group scheduler (unchanged) ---

type GroupScheduler struct {
	cfg      RDSConfig
	psIdx    int
	rtIdx    int
	phase    int
	rtABFlag bool
}

func newGroupScheduler(cfg RDSConfig) *GroupScheduler {
	return &GroupScheduler{cfg: cfg}
}

func (gs *GroupScheduler) NextGroup() [4]uint16 {
	if gs.phase < 4 {
		g := buildGroup0A(gs.cfg.PI, gs.cfg.PTY, gs.cfg.TP, gs.cfg.TA, gs.psIdx, gs.cfg.PS)
		gs.psIdx = (gs.psIdx + 1) % 4
		gs.phase++
		return g
	}
	g := buildGroup2A(gs.cfg.PI, gs.cfg.PTY, gs.cfg.TP, gs.rtABFlag, gs.rtIdx, gs.cfg.RT)
	gs.rtIdx++
	rtSegs := rtSegmentCount(gs.cfg.RT)
	if gs.rtIdx >= rtSegs {
		gs.rtIdx = 0
		gs.rtABFlag = !gs.rtABFlag
	}
	gs.phase++
	if gs.phase >= 4+rtSegs {
		gs.phase = 0
	}
	return g
}

func rtSegmentCount(rt string) int {
	rt = normalizeRT(rt)
	n := (len(rt) + 3) / 4
	if n == 0 {
		n = 1
	}
	if n > 16 {
		n = 16
	}
	return n
}

// --- Differential encoder ---

type diffEncoder struct{ prev uint8 }

func (d *diffEncoder) encode(bit uint8) uint8 {
	out := d.prev ^ bit
	d.prev = out
	return out
}

// --- Biphase waveform ---
// A biphase symbol at 228 kHz / 1187.5 bps = 192 samples per bit.
// First half (96 samples): +1, second half (96 samples): -1.
// This is the "rectangular biphase pulse". PiFmRds uses an RRC-shaped
// version, but even rectangular works for most receivers. We apply a
// simple raised-cosine taper at the transitions to reduce spectral splatter.
//
// The waveform is 192 samples long. It gets overlap-added into the ring
// buffer, so adjacent symbols blend at boundaries.

const waveformLen = samplesPerBit // 192
const taperLen = 8                // smooth transitions over 8 samples

var biphaseWaveform [waveformLen]float32

func init() {
	// Generate biphase pulse: +1 for first half, -1 for second half,
	// with raised-cosine tapered transitions.
	half := waveformLen / 2
	for i := 0; i < waveformLen; i++ {
		if i < half {
			biphaseWaveform[i] = 1.0
		} else {
			biphaseWaveform[i] = -1.0
		}
	}
	// Taper at start (0 → +1)
	for i := 0; i < taperLen; i++ {
		t := float32(i) / float32(taperLen)
		biphaseWaveform[i] *= t
	}
	// Taper at midpoint (+1 → -1): ramp over taperLen samples centered at half
	for i := 0; i < taperLen; i++ {
		t := float32(i) / float32(taperLen)
		// Crossfade from +1 to -1
		idx := half - taperLen/2 + i
		if idx >= 0 && idx < waveformLen {
			biphaseWaveform[idx] = 1.0 - 2.0*t
		}
	}
	// Taper at end (-1 → 0)
	for i := 0; i < taperLen; i++ {
		t := float32(i) / float32(taperLen)
		biphaseWaveform[waveformLen-1-i] *= t
	}
}

// --- Encoder ---

const ringSize = samplesPerBit + waveformLen // overlap region

// Encoder generates RDS subcarrier samples at 228 kHz.
// Call NextSample228k() at 228 kHz rate to get modulated output.
type Encoder struct {
	config    RDSConfig
	scheduler *GroupScheduler
	diff      diffEncoder

	// Bit stream
	bitBuf [bitsPerGroup]int
	bitLen int
	bitPos int

	// Ring buffer for overlap-add shaped biphase
	ring      [ringSize]float32
	ringWrite int // next write position for new symbol
	ringRead  int // current read position

	// Sample counter within current bit
	sampleCount int

	// 57 kHz carrier phase: cycles through 0,1,2,3 at 228 kHz
	carrierPhase int

	// For backward-compat Generate() used in tests
	SampleRate float64
}

func NewEncoder(cfg RDSConfig) (*Encoder, error) {
	if cfg.SampleRate <= 0 {
		cfg.SampleRate = rdsRate
	}
	cfg.PS = normalizePS(cfg.PS)
	cfg.RT = normalizeRT(cfg.RT)

	enc := &Encoder{
		config:     cfg,
		SampleRate: cfg.SampleRate,
		scheduler:  newGroupScheduler(cfg),
	}
	enc.loadNextGroup()
	// Pre-load first symbol into ring buffer
	enc.emitSymbol()
	return enc, nil
}

func (e *Encoder) Reset() {
	e.diff = diffEncoder{}
	e.scheduler = newGroupScheduler(e.config)
	e.bitLen = 0
	e.bitPos = 0
	e.sampleCount = 0
	e.carrierPhase = 0
	e.ringWrite = 0
	e.ringRead = 0
	for i := range e.ring {
		e.ring[i] = 0
	}
	e.loadNextGroup()
	e.emitSymbol()
}

// NextSample228k returns the next RDS subcarrier sample.
// MUST be called at exactly 228000 Hz.
// Output is the shaped biphase envelope modulated onto 57 kHz.
func (e *Encoder) NextSample228k() float64 {
	// Read shaped envelope from ring buffer
	envelope := float64(e.ring[e.ringRead])
	e.ring[e.ringRead] = 0 // clear after reading
	e.ringRead++
	if e.ringRead >= ringSize {
		e.ringRead = 0
	}

	// Advance sample counter; emit next symbol when bit period ends
	e.sampleCount++
	if e.sampleCount >= samplesPerBit {
		e.sampleCount = 0
		e.bitPos++
		if e.bitPos >= e.bitLen {
			e.loadNextGroup()
		}
		e.emitSymbol()
	}

	// 57 kHz modulation: at 228 kHz, period = 4 samples.
	// Phase pattern: {0, +1, 0, -1} → multiply envelope by carrier
	var carrier float64
	switch e.carrierPhase {
	case 0:
		carrier = 0
	case 1:
		carrier = 1
	case 2:
		carrier = 0
	case 3:
		carrier = -1
	}
	e.carrierPhase++
	if e.carrierPhase >= 4 {
		e.carrierPhase = 0
	}

	return envelope * carrier
}

// emitSymbol writes the shaped biphase waveform for the current bit
// into the ring buffer using overlap-add.
func (e *Encoder) emitSymbol() {
	// Differential encoding output determines polarity
	diffBit := e.bitBuf[e.bitPos]
	sign := float32(1.0)
	if diffBit == 1 {
		sign = -1.0
	}

	// Overlap-add the biphase waveform into the ring buffer
	idx := e.ringWrite
	for i := 0; i < waveformLen; i++ {
		e.ring[idx] += biphaseWaveform[i] * sign
		idx++
		if idx >= ringSize {
			idx = 0
		}
	}
	e.ringWrite += samplesPerBit
	if e.ringWrite >= ringSize {
		e.ringWrite -= ringSize
	}
}

func (e *Encoder) loadNextGroup() {
	group := e.scheduler.NextGroup()
	e.bitLen = 0
	for blk, off := range [4]byte{'A', 'B', 'C', 'D'} {
		enc := encodeBlock(group[blk], off)
		for bit := bitsPerBlock - 1; bit >= 0; bit-- {
			raw := uint8((enc >> uint(bit)) & 1)
			e.bitBuf[e.bitLen] = int(e.diff.encode(raw))
			e.bitLen++
		}
	}
	e.bitPos = 0
}

// --- Backward-compatible API for tests ---

// NextSample returns a single RDS sample at 228 kHz. Same as NextSample228k.
func (e *Encoder) NextSample() float64 {
	return e.NextSample228k()
}

// Generate produces n RDS samples at 228 kHz.
func (e *Encoder) Generate(n int) []float64 {
	out := make([]float64, n)
	for i := range out {
		out[i] = e.NextSample228k()
	}
	return out
}

// Symbol returns +1/-1 for the current differential bit (NRZ, no biphase).
// Only for the software decoder test path.
func (e *Encoder) Symbol() float64 {
	if e.bitLen == 0 {
		return -1
	}
	sym := float64(1)
	if e.bitBuf[e.bitPos] == 0 {
		sym = -1
	}
	// Advance bit clock at 228 kHz rate
	e.sampleCount++
	if e.sampleCount >= samplesPerBit {
		e.sampleCount = 0
		e.bitPos++
		if e.bitPos >= e.bitLen {
			e.loadNextGroup()
		}
	}
	return sym
}