fm-rds-tx/internal/dsp/upsample.go

package dsp

import (
	"math"

	"github.com/jan/fm-rds-tx/internal/output"
)

// FMPhaseUpsampler performs FM modulation + upsampling via phase-domain
// interpolation. Maintains continuous phase across successive calls.
type FMPhaseUpsampler struct {
	srcRate      float64
	dstRate      float64
	maxDeviation float64
	ratio        float64
	phase        float64 // persistent across calls
}

func NewFMPhaseUpsampler(srcRate, dstRate, maxDeviation float64) *FMPhaseUpsampler {
	return &FMPhaseUpsampler{
		srcRate:      srcRate,
		dstRate:      dstRate,
		maxDeviation: maxDeviation,
		ratio:        dstRate / srcRate,
	}
}

func (u *FMPhaseUpsampler) Process(frame *output.CompositeFrame) *output.CompositeFrame {
	if frame == nil || len(frame.Samples) == 0 {
		return frame
	}

	srcLen := len(frame.Samples)

	// Accumulate phase at source rate, continuing from previous chunk
	phases := make([]float64, srcLen)
	for i, s := range frame.Samples {
		u.phase += 2 * math.Pi * float64(s.I) * u.maxDeviation / u.srcRate
		phases[i] = u.phase
	}

	// Keep phase bounded
	if u.phase > 1e9 {
		offset := math.Floor(u.phase/(2*math.Pi)) * 2 * math.Pi
		u.phase -= offset
		for i := range phases {
			phases[i] -= offset
		}
	}

	// Interpolate phase to target rate
	dstLen := int(float64(srcLen) * u.ratio)
	dst := make([]output.IQSample, dstLen)
	step := 1.0 / u.ratio

	pos := 0.0
	for i := 0; i < dstLen; i++ {
		idx := int(pos)
		frac := pos - float64(idx)

		var p float64
		if idx+1 < srcLen {
			p = phases[idx]*(1-frac) + phases[idx+1]*frac
		} else if idx < srcLen {
			p = phases[idx]
		}

		dst[i] = output.IQSample{
			I: float32(math.Cos(p)),
			Q: float32(math.Sin(p)),
		}
		pos += step
	}

	return &output.CompositeFrame{
		Samples:      dst,
		SampleRateHz: u.dstRate,
		Timestamp:    frame.Timestamp,
		Sequence:     frame.Sequence,
	}
}

// UpsampleFMPhase is the stateless version for offline/test use.
func UpsampleFMPhase(frame *output.CompositeFrame, targetRate, maxDeviation float64) *output.CompositeFrame {
	u := NewFMPhaseUpsampler(frame.SampleRateHz, targetRate, maxDeviation)
	return u.Process(frame)
}