Add SILK float pitch/resampler primitives

internal/silk/pitch_util.go

@@ -0,0 +1,130 @@
+// SPDX-FileCopyrightText: 2026 The Pion community <https://pion.ly>
+// SPDX-License-Identifier: MIT
+
+package silk
+
+import "math"
+
+// Helpers used by the pitch analysis core: 2:1 downsampler, float/short
+// conversions, energy, cross-correlation, and index-tracking insertion sorts.
+
+const (
+	resamplerDown2Coef0 = 9872
+	resamplerDown2Coef1 = 39809 - 65536 // -25727
+)
+
+// resamplerDown2 halves the sample rate with a second-order all-pass filter
+// (silk_resampler_down2). out must have len(in)/2 samples; state persists
+// across calls.
+func resamplerDown2(state *[2]int32, out, in []int16) {
+	for k := range len(in) >> 1 { //nolint:varnamelen // k indexes the sample pair.
+		in32 := int32(in[2*k]) << 10
+		y := in32 - state[0]
+		x := smlawb(y, y, resamplerDown2Coef1)
+		out32 := state[0] + x
+		state[0] = in32 + x
+
+		in32 = int32(in[2*k+1]) << 10
+		y = in32 - state[1]
+		x = smulwb(y, resamplerDown2Coef0)
+		out32 = out32 + state[1] + x
+		state[1] = in32 + x
+
+		out[k] = int16(sat16(rshiftRound32(out32, 11))) //nolint:gosec // G115
+	}
+}
+
+// float2ShortArray rounds and saturates float samples to int16.
+func float2ShortArray(out []int16, in []float32) {
+	for i := range out {
+		out[i] = int16(sat16(int32(math.RoundToEven(float64(in[i]))))) //nolint:gosec // G115
+	}
+}
+
+// short2FloatArray widens int16 samples to float.
+func short2FloatArray(out []float32, in []int16) {
+	for i := range out {
+		out[i] = float32(in[i])
+	}
+}
+
+// energyFLP returns the signal energy in double precision.
+func energyFLP(data []float32, n int) float64 {
+	var result float64
+	for i := range n {
+		result += float64(data[i]) * float64(data[i])
+	}
+
+	return result
+}
+
+// pitchXcorr computes xcorr[j] = <x, y[j:]> for j in [0, maxPitch).
+func pitchXcorr(x, y, xcorr []float32, length, maxPitch int) {
+	for j := range maxPitch {
+		xcorr[j] = float32(innerProductFLP(x, y[j:], length))
+	}
+}
+
+// insertionSortDecreasingFLP sorts the first K of L values into decreasing
+// order, tracking their original indices (silk_insertion_sort_decreasing_FLP).
+//
+//nolint:dupl,varnamelen // twin of the increasing-order sort below; l/k are lengths, as in the C reference.
+func insertionSortDecreasingFLP(a []float32, idx []int, l, k int) {
+	for i := range k {
+		idx[i] = i
+	}
+	for i := 1; i < k; i++ {
+		value := a[i]
+		j := i - 1
+		for ; j >= 0 && value > a[j]; j-- {
+			a[j+1] = a[j]
+			idx[j+1] = idx[j]
+		}
+		a[j+1] = value
+		idx[j+1] = i
+	}
+	for i := k; i < l; i++ {
+		value := a[i]
+		if value > a[k-1] { //nolint:gosec // G602: k-1 >= 0 for k >= 1.
+			j := k - 2
+			for ; j >= 0 && value > a[j]; j-- {
+				a[j+1] = a[j]
+				idx[j+1] = idx[j]
+			}
+			a[j+1] = value
+			idx[j+1] = i
+		}
+	}
+}
+
+// insertionSortIncreasing sorts the first K of L values into increasing order,
+// tracking their original indices (silk_insertion_sort_increasing).
+//
+//nolint:dupl,varnamelen // twin of the decreasing-order sort above; l/k are lengths, as in the C reference.
+func insertionSortIncreasing(a []int32, idx []int, l, k int) {
+	for i := range k {
+		idx[i] = i
+	}
+	for i := 1; i < k; i++ {
+		value := a[i]
+		j := i - 1
+		for ; j >= 0 && value < a[j]; j-- {
+			a[j+1] = a[j]
+			idx[j+1] = idx[j]
+		}
+		a[j+1] = value
+		idx[j+1] = i
+	}
+	for i := k; i < l; i++ {
+		value := a[i]
+		if value < a[k-1] { //nolint:gosec // G602: k-1 >= 0 for k >= 1.
+			j := k - 2
+			for ; j >= 0 && value < a[j]; j-- {
+				a[j+1] = a[j]
+				idx[j+1] = idx[j]
+			}
+			a[j+1] = value
+			idx[j+1] = i
+		}
+	}
+}

internal/silk/pitch_util_test.go

@@ -0,0 +1,82 @@
+// SPDX-FileCopyrightText: 2026 The Pion community <https://pion.ly>
+// SPDX-License-Identifier: MIT
+
+package silk
+
+import (
+	"math"
+	"testing"
+
+	"github.com/stretchr/testify/assert"
+	"github.com/stretchr/testify/require"
+)
+
+func TestResamplerDown2(t *testing.T) {
+	in := make([]int16, 64)
+	for i := range in {
+		in[i] = 1000
+	}
+	out := make([]int16, len(in)/2)
+	var state [2]int32
+	resamplerDown2(&state, out, in)
+
+	// A DC input should settle near the same DC level after the transient.
+	assert.InDelta(t, 1000, out[len(out)-1], 30)
+
+	// Deterministic for identical state and input.
+	out2 := make([]int16, len(in)/2)
+	var state2 [2]int32
+	resamplerDown2(&state2, out2, in)
+	assert.Equal(t, out, out2)
+}
+
+func TestFloatShortConversion(t *testing.T) {
+	out16 := make([]int16, 4)
+	float2ShortArray(out16, []float32{0.4, 1.6, -2.5, 100000})
+	assert.Equal(t, int16(0), out16[0])
+	assert.Equal(t, int16(2), out16[1])
+	assert.Equal(t, int16(-2), out16[2]) // round half to even
+	assert.Equal(t, int16(math.MaxInt16), out16[3])
+
+	outF := make([]float32, 2)
+	short2FloatArray(outF, []int16{-5, 7})
+	assert.Equal(t, []float32{-5, 7}, outF)
+}
+
+func TestEnergyFLP(t *testing.T) {
+	assert.InDelta(t, 1+4+9, energyFLP([]float32{1, 2, 3}, 3), 1e-9)
+}
+
+func TestPitchXcorr(t *testing.T) {
+	// A period-4 signal correlates most strongly at lag 4.
+	length := 32
+	sig := make([]float32, length+8)
+	for i := range sig {
+		sig[i] = float32(math.Sin(2 * math.Pi * float64(i) / 4))
+	}
+	xcorr := make([]float32, 8)
+	pitchXcorr(sig, sig, xcorr, length, 8)
+	assert.Greater(t, xcorr[4], xcorr[1])
+	assert.Greater(t, xcorr[4], xcorr[2])
+	assert.Greater(t, xcorr[4], xcorr[3])
+}
+
+func TestInsertionSortDecreasingFLP(t *testing.T) {
+	a := []float32{3, 1, 4, 1, 5, 9, 2, 6}
+	idx := make([]int, 4)
+	insertionSortDecreasingFLP(a, idx, len(a), 4)
+	assert.Equal(t, []float32{9, 6, 5, 4}, a[:4])
+	assert.Equal(t, 5, idx[0]) // 9 was at index 5
+	assert.Equal(t, 7, idx[1]) // 6 was at index 7
+	assert.Equal(t, 4, idx[2]) // 5 was at index 4
+	assert.Equal(t, 2, idx[3]) // 4 was at index 2
+}
+
+func TestInsertionSortIncreasing(t *testing.T) {
+	a := []int32{3, 1, 4, 1, 5, 9, 2, 6}
+	idx := make([]int, 4)
+	insertionSortIncreasing(a, idx, len(a), 4)
+	require.Equal(t, []int32{1, 1, 2, 3}, a[:4])
+	assert.Equal(t, 6, idx[2]) // value 2 was at index 6
+	assert.Equal(t, 0, idx[3]) // value 3 was at index 0
+}