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

package rds

import (
	"math"
	"os"
)

// RDS2 subcarrier frequencies (IEC 62106-1:2018, Figure 3).
// All are integer multiples of 19 kHz / 2.
const (
	RDS2Stream1Freq = 66500.0  // 3.5 × 19 kHz
	RDS2Stream2Freq = 71250.0  // 3.75 × 19 kHz
	RDS2Stream3Freq = 76000.0  // 4 × 19 kHz
)

// RDS2Encoder generates the three additional RDS2 subcarrier signals.
// Each stream carries Group Type C data at 1187.5 bps.
// The output is added to the composite MPX signal.
type RDS2Encoder struct {
	streams    [3]*streamEncoder
	sampleRate float64
	enabled    bool

	// RFT state: file segments to transmit
	rftFile   *RFTFile
	rftSegIdx int
}

// streamEncoder handles one RDS2 subcarrier stream.
type streamEncoder struct {
	sampleRate   float64
	carrierFreq  float64
	carrierPhase float64
	carrierStep  float64

	// Biphase encoding (same as Stream 0)
	spb          int       // samples per bit
	waveform     []float64 // resampled PiFmRds biphase waveform
	wfLen        int
	ring         []float64
	ringSize     int
	bitBuffer    [bitsPerGroup]int
	bitPos       int
	prevOutput   int
	curOutput    int
	sampleCount  int
	inSampleIdx  int
	outSampleIdx int

	// Group C queue
	groups []GroupC
	groupIdx int
}

// NewRDS2Encoder creates an RDS2 encoder for the given sample rate.
// Call LoadLogo() to set up the station logo for RFT transmission.
func NewRDS2Encoder(sampleRate float64) *RDS2Encoder {
	freqs := [3]float64{RDS2Stream1Freq, RDS2Stream2Freq, RDS2Stream3Freq}
	e := &RDS2Encoder{sampleRate: sampleRate}
	for i := 0; i < 3; i++ {
		e.streams[i] = newStreamEncoder(sampleRate, freqs[i])
	}
	return e
}

func newStreamEncoder(sampleRate, carrierFreq float64) *streamEncoder {
	spb := int(math.Round(sampleRate / rdsBitRate))
	ratio := sampleRate / refRate

	// Resample PiFmRds waveform (same as main RDS encoder)
	wfLen := int(math.Round(float64(refFilterSize) * ratio))
	waveform := make([]float64, wfLen)
	for i := range waveform {
		srcPos := float64(i) / ratio
		idx := int(srcPos)
		frac := srcPos - float64(idx)
		if idx+1 < refFilterSize {
			waveform[i] = refWaveform[idx]*(1-frac) + refWaveform[idx+1]*frac
		} else if idx < refFilterSize {
			waveform[i] = refWaveform[idx]
		}
	}
	// Normalize to peak=1.0
	var peak float64
	for _, v := range waveform {
		if a := math.Abs(v); a > peak {
			peak = a
		}
	}
	if peak > 0 {
		for i := range waveform {
			waveform[i] /= peak
		}
	}

	ringSize := spb + wfLen
	return &streamEncoder{
		sampleRate:   sampleRate,
		carrierFreq:  carrierFreq,
		carrierPhase: 0,
		carrierStep:  carrierFreq / sampleRate,
		spb:          spb,
		waveform:     waveform,
		wfLen:        wfLen,
		ring:         make([]float64, ringSize),
		ringSize:     ringSize,
		bitPos:       bitsPerGroup,
		sampleCount:  spb,
		outSampleIdx: ringSize - 1,
	}
}

// Enable activates or deactivates RDS2 output.
func (e *RDS2Encoder) Enable(on bool) {
	e.enabled = on
}

// Enabled returns whether RDS2 is active.
func (e *RDS2Encoder) Enabled() bool {
	return e.enabled
}

// LoadLogo reads a logo file and segments it for RFT transmission on Stream 1.
func (e *RDS2Encoder) LoadLogo(path string) error {
	data, err := os.ReadFile(path)
	if err != nil {
		return err
	}
	// Detect file type
	ft := uint8(RFTFileTypePNG)
	if len(data) >= 3 && data[0] == 0xFF && data[1] == 0xD8 && data[2] == 0xFF {
		ft = RFTFileTypeJPEG
	}
	e.rftFile = SegmentFile(data, 0, ft) // fileID=0 = station logo
	e.rftSegIdx = 0
	return nil
}

// NextSample returns the combined RDS2 subcarrier sample for all 3 streams.
// Add this to the composite MPX signal with appropriate injection level.
func (e *RDS2Encoder) NextSample() float64 {
	if !e.enabled {
		return 0
	}
	var sum float64
	for i := 0; i < 3; i++ {
		sum += e.streams[i].nextSample()
	}
	return sum
}

// NextSampleWithPilot returns the RDS2 sample using pilot-locked carriers.
// pilotPhase is the current pilot oscillator phase (0-1 range).
func (e *RDS2Encoder) NextSampleWithPilot(pilotPhase float64) float64 {
	if !e.enabled {
		return 0
	}
	// RDS2 carriers are at 3.5×, 3.75×, 4× pilot frequency
	// sin(2π × N × pilotPhase) where N = 3.5, 3.75, 4.0
	multipliers := [3]float64{3.5, 3.75, 4.0}
	var sum float64
	for i := 0; i < 3; i++ {
		carrier := math.Sin(2 * math.Pi * multipliers[i] * pilotPhase)
		sum += e.streams[i].nextSampleWithCarrier(carrier)
	}
	return sum
}

// FeedGroups distributes Group C data to the stream encoders.
// Call periodically (e.g. once per frame) to keep streams fed.
func (e *RDS2Encoder) FeedGroups() {
	if !e.enabled {
		return
	}

	// Stream 1: Station logo via RFT (if loaded)
	if e.rftFile != nil && len(e.rftFile.Segments) > 0 {
		seg := e.rftFile.Segments[e.rftSegIdx]
		gc := GroupC{FH: seg.FH, Data: seg.Payload}
		e.streams[0].pushGroup(gc)
		e.rftSegIdx = (e.rftSegIdx + 1) % len(e.rftFile.Segments)
	}

	// Streams 2 & 3: available for future ODAs
	// For now, send idle groups (FH=0, all zeros)
}

// --- streamEncoder methods ---

func (se *streamEncoder) pushGroup(gc GroupC) {
	se.groups = append(se.groups, gc)
}

func (se *streamEncoder) nextSample() float64 {
	carrier := math.Sin(2 * math.Pi * se.carrierPhase)
	se.carrierPhase += se.carrierStep
	if se.carrierPhase >= 1.0 {
		se.carrierPhase -= 1.0
	}
	return se.nextSampleWithCarrier(carrier)
}

func (se *streamEncoder) nextSampleWithCarrier(carrier float64) float64 {
	if se.sampleCount >= se.spb {
		if se.bitPos >= bitsPerGroup {
			se.getNextGroup()
			se.bitPos = 0
		}
		curBit := se.bitBuffer[se.bitPos]
		se.prevOutput = se.curOutput
		se.curOutput = se.prevOutput ^ curBit
		inverting := (se.curOutput == 1)

		idx := se.inSampleIdx
		for j := 0; j < se.wfLen; j++ {
			val := se.waveform[j]
			if inverting {
				val = -val
			}
			se.ring[idx] += val
			idx++
			if idx >= se.ringSize {
				idx = 0
			}
		}
		se.inSampleIdx += se.spb
		if se.inSampleIdx >= se.ringSize {
			se.inSampleIdx -= se.ringSize
		}
		se.bitPos++
		se.sampleCount = 0
	}

	envelope := se.ring[se.outSampleIdx]
	se.ring[se.outSampleIdx] = 0
	se.outSampleIdx++
	if se.outSampleIdx >= se.ringSize {
		se.outSampleIdx = 0
	}
	se.sampleCount++
	return envelope * carrier
}

func (se *streamEncoder) getNextGroup() {
	var group [4]uint16

	if se.groupIdx < len(se.groups) {
		gc := se.groups[se.groupIdx]
		group = buildGroupC(gc)
		se.groupIdx++
		if se.groupIdx >= len(se.groups) {
			se.groupIdx = 0 // loop
		}
	}
	// else: idle group (all zeros)

	// Encode group into bit buffer using same CRC/offset as Stream 0
	pos := 0
	for blk, off := range [4]byte{'A', 'B', 'C', 'D'} {
		encoded := encodeBlock(group[blk], off)
		for bit := bitsPerBlock - 1; bit >= 0; bit-- {
			se.bitBuffer[pos] = int((encoded >> uint(bit)) & 1)
			pos++
		}
	}
}