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

package rds

import (
	"math"
)

const (
	defaultSubcarrierHz = 57000.0
	defaultBitRateHz    = 1187.5

	// Each RDS group has 4 blocks of 26 bits each (16 data + 10 check).
	bitsPerBlock = 26
	bitsPerGroup = 4 * bitsPerBlock // 104
)

// -----------------------------------------------------------------------
// CRC / offset words per IEC 62106 / EN 50067
// -----------------------------------------------------------------------

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 {
	check := crc10(data) ^ offsetWords[offset]
	return (uint32(data) << 10) | uint32(check)
}

// -----------------------------------------------------------------------
// Group building
// -----------------------------------------------------------------------

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

func buildGroup2A(pi uint16, pty uint8, tp bool, abFlag bool, segIdx int, rt string) [4]uint16 {
	rt = normalizeRT(rt)
	blockA := pi
	var blockB uint16
	blockB = 2 << 12
	if tp {
		blockB |= 1 << 10
	}
	blockB |= uint16(pty&0x1F) << 5
	if abFlag {
		blockB |= 1 << 4
	}
	blockB |= uint16(segIdx & 0x0F)
	ci := segIdx * 4
	ch0, ch1, ch2, ch3 := padRT(rt, ci)
	blockC := (uint16(ch0) << 8) | uint16(ch1)
	blockD := (uint16(ch2) << 8) | uint16(ch3)
	return [4]uint16{blockA, blockB, blockC, blockD}
}

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

// -----------------------------------------------------------------------
// Group scheduler
// -----------------------------------------------------------------------

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

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
}

// -----------------------------------------------------------------------
// Encoder
// -----------------------------------------------------------------------

// Encoder produces a standards-grade RDS BPSK subcarrier at 57 kHz.
// Output is unity-normalized (peak ±1.0). The caller (combiner) controls
// the actual injection level.
type Encoder struct {
	config     RDSConfig
	sampleRate float64

	scheduler *GroupScheduler
	diff      diffEncoder

	bitBuf   [bitsPerGroup]uint8
	bitLen   int
	bitPos   int
	bitPhase float64
	subPhase float64
}

// NewEncoder builds a new encoder for the provided configuration and sample rate.
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
}

// Reset restarts the encoder.
func (e *Encoder) Reset() {
	e.bitPhase = 0
	e.subPhase = 0
	e.diff = diffEncoder{}
	e.scheduler = newGroupScheduler(e.config)
	e.bitLen = 0
	e.bitPos = 0
	e.loadNextGroup()
}

// NextSample returns the next RDS subcarrier sample.
// Zero-allocation hot path for real-time use.
func (e *Encoder) NextSample() float64 {
	var symbol float64
	if e.bitLen == 0 || e.bitBuf[e.bitPos] == 0 {
		symbol = -1
	} else {
		symbol = 1
	}

	value := symbol * math.Sin(2*math.Pi*e.subPhase)

	e.subPhase += defaultSubcarrierHz / e.sampleRate
	if e.subPhase >= 1 {
		e.subPhase -= math.Floor(e.subPhase)
	}

	e.bitPhase += defaultBitRateHz / e.sampleRate
	if e.bitPhase >= 1 {
		steps := int(e.bitPhase)
		e.bitPhase -= float64(steps)
		e.bitPos += steps
		if e.bitPos >= e.bitLen {
			e.loadNextGroup()
		}
	}

	return value
}

// Generate produces n RDS samples. Convenience wrapper; prefer NextSample()
// in real-time paths.
func (e *Encoder) Generate(samples int) []float64 {
	out := make([]float64, samples)
	for i := range out {
		out[i] = e.NextSample()
	}
	return out
}

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