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

package rds

import "math"

const (
	defaultSubcarrierHz = 57000.0
	defaultBitRateHz    = 1187.5
	bitsPerBlock        = 26
	bitsPerGroup        = 4 * bitsPerBlock
)

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

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

type GroupScheduler struct {
	cfg RDSConfig; psIdx, rtIdx, 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
}

type diffEncoder struct{ prev uint8 }

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

// Encoder produces RDS biphase-coded BPSK data locked to the pilot tone.
//
// Per IEC 62106, RDS uses biphase coding: the differentially-encoded data
// is modulo-2 added with a clock signal at the data rate. This means every
// bit period has a guaranteed polarity transition at the midpoint.
//
// The bit clock runs at 1187.5 bps = pilot_freq / 16.
// Each bit period spans 16 pilot cycles. At the 8th cycle (midpoint),
// the biphase symbol inverts polarity.
type Encoder struct {
	config    RDSConfig
	scheduler *GroupScheduler
	diff      diffEncoder
	bitBuf    [bitsPerGroup]uint8
	bitLen    int
	bitPos    int

	// Pilot-derived timing
	pilotCycles    int
	lastPilotPhase float64
	inSecondHalf   bool // true when in 2nd half of biphase bit period

	// Internal oscillator for Generate() backward compat
	sampleRate float64
	subPhase   float64
	bitPhase   float64
}

func NewEncoder(cfg RDSConfig) (*Encoder, error) {
	if cfg.SampleRate <= 0 { cfg.SampleRate = 228000 }
	cfg.PS = normalizePS(cfg.PS); cfg.RT = normalizeRT(cfg.RT)
	enc := &Encoder{config: cfg, sampleRate: cfg.SampleRate, scheduler: newGroupScheduler(cfg)}
	enc.loadNextGroup()
	return enc, nil
}

func (e *Encoder) Reset() {
	e.bitPhase = 0; e.subPhase = 0; e.pilotCycles = 0; e.lastPilotPhase = 0
	e.inSecondHalf = false
	e.diff = diffEncoder{}; e.scheduler = newGroupScheduler(e.config)
	e.bitLen = 0; e.bitPos = 0; e.loadNextGroup()
}

// BiphaseSymbolForPilotPhase returns the biphase-coded BPSK symbol (+1/-1)
// for the current sample, with timing locked to the pilot phase.
//
// Biphase coding: for each bit period (16 pilot cycles):
//   - First 8 cycles: symbol = data_symbol
//   - Last 8 cycles:  symbol = -data_symbol (inverted)
//
// This guarantees a transition at every bit midpoint, which is how
// hardware RDS decoders synchronize their clock.
func (e *Encoder) BiphaseSymbolForPilotPhase(pilotPhase float64) float64 {
	// Detect pilot cycle completion: phase wraps from >0.5 to <0.5
	if e.lastPilotPhase > 0.5 && pilotPhase < 0.5 {
		e.pilotCycles++
		if e.pilotCycles == 8 {
			// Midpoint of bit period: enter second half (biphase inversion)
			e.inSecondHalf = true
		}
		if e.pilotCycles >= 16 {
			// End of bit period: advance to next bit
			e.pilotCycles = 0
			e.inSecondHalf = false
			e.bitPos++
			if e.bitPos >= e.bitLen { e.loadNextGroup() }
		}
	}
	e.lastPilotPhase = pilotPhase

	sym := e.currentSymbol()
	if e.inSecondHalf {
		sym = -sym // biphase: invert in second half
	}
	return sym
}

// Symbol returns current symbol with internal bit clock (for tests).
func (e *Encoder) Symbol() float64 {
	sym := e.currentSymbol(); e.advanceInternal(); return sym
}

// NextSample returns complete RDS sample with internal 57 kHz (for tests).
// Note: uses NRZ (not biphase) for backward compat with software decoder.
func (e *Encoder) NextSample() float64 {
	sym := e.currentSymbol()
	v := sym * math.Sin(2*math.Pi*e.subPhase)
	e.subPhase += defaultSubcarrierHz / e.sampleRate
	if e.subPhase >= 1 { e.subPhase -= math.Floor(e.subPhase) }
	e.advanceInternal()
	return v
}

// Generate produces n samples with internal oscillator (for tests).
func (e *Encoder) Generate(n int) []float64 {
	out := make([]float64, n)
	for i := range out { out[i] = e.NextSample() }
	return out
}

func (e *Encoder) advanceInternal() {
	e.bitPhase += defaultBitRateHz / e.sampleRate
	if e.bitPhase >= 1 {
		s := int(e.bitPhase); e.bitPhase -= float64(s); e.bitPos += s
		if e.bitPos >= e.bitLen { e.loadNextGroup() }
	}
}

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-- {
			e.bitBuf[e.bitLen] = e.diff.encode(uint8((enc >> uint(bit)) & 1))
			e.bitLen++
		}
	}
	e.bitPos = 0
}

func (e *Encoder) currentSymbol() float64 {
	if e.bitLen == 0 || e.bitBuf[e.bitPos] == 0 { return -1 }
	return 1
}