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

package rds

import (
	"math"
)

const (
	defaultSubcarrierHz = 57000.0
	defaultBitRateHz    = 1187.5
	defaultAmplitude    = 0.05

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

// Generator polynomial: x^10 + x^8 + x^7 + x^5 + x^4 + x^3 + 1 = 0x1B9
const crcPoly = 0x1B9

// Offset words for blocks A, B, C, C', D.
var offsetWords = map[byte]uint16{
	'A': 0x0FC,
	'B': 0x198,
	'C': 0x168,
	'c': 0x350, // C' for type B groups
	'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
// -----------------------------------------------------------------------

// buildGroup0A creates a type 0A group (basic tuning and PS name).
// segIdx selects which 2-char segment of the 8-char PS name (0..3).
func buildGroup0A(pi uint16, pty uint8, tp, ta bool, segIdx int, ps string) [4]uint16 {
	ps = normalizePS(ps)

	blockA := pi

	// Block B: group type 0A (0000 0), TP, PTY, TA, MS=1, DI=0, segment address
	var blockB uint16
	if tp {
		blockB |= 1 << 10
	}
	blockB |= uint16(pty&0x1F) << 5
	if ta {
		blockB |= 1 << 4
	}
	blockB |= 1 << 3 // MS = music
	blockB |= uint16(segIdx & 0x03)

	// Block C: AF (not implemented) – send PI as filler (common practice)
	blockC := pi

	// Block D: 2 PS characters
	ci := segIdx * 2
	ch0 := uint16(ps[ci])
	ch1 := uint16(ps[ci+1])
	blockD := (ch0 << 8) | ch1

	return [4]uint16{blockA, blockB, blockC, blockD}
}

// buildGroup2A creates a type 2A group (RadioText).
// segIdx selects which 4-char segment (0..15) of the 64-char RT.
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 // group type 2
	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
// -----------------------------------------------------------------------

// GroupScheduler cycles through 0A and 2A groups.
type GroupScheduler struct {
	cfg      RDSConfig
	psIdx    int
	rtIdx    int
	rtABFlag bool
	phase    int
}

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

// NextGroup returns the next RDS group as 4 raw 16-bit words.
func (gs *GroupScheduler) NextGroup() [4]uint16 {
	// Pattern: 4x 0A (full PS cycle), then N x 2A (full RT cycle), repeat.
	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.
type Encoder struct {
	config     RDSConfig
	sampleRate float64
	amplitude  float64

	scheduler *GroupScheduler
	diff      diffEncoder

	bitBuf   []uint8
	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,
		amplitude:  defaultAmplitude,
		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.bitBuf = nil
	e.bitPos = 0
	e.loadNextGroup()
}

// Generate produces the requested number of RDS samples.
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.bitBuf = make([]uint8, 0, bitsPerGroup)
	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 = append(e.bitBuf, diffBit)
		}
	}
	e.bitPos = 0
}

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

func (e *Encoder) nextSample() float64 {
	symbol := e.currentSymbol()
	value := e.amplitude * 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 >= len(e.bitBuf) {
			e.loadNextGroup()
		}
	}

	return value
}