// SafeC Standard Library — resampling core, written once for both sample types.
//
// Internal: included by resample.sc (double samples) and resample_f32.sc
// (float samples), each its own translation unit. Positions, the kernel
// and the sums are double either way; only loads and stores use RS_T. The
// includer defines RS_T and the public names RS_NEAREST, RS_LINEAR, RS_SINC.
#pragma once
void RS_NEAREST(const &RS_T in, unsigned long inLen, &RS_T out, unsigned long outLen) {
if (inLen == 0UL || outLen == 0UL) { return; }
double scale = (double)inLen / (double)outLen;
unsigned long i = 0UL;
while (i < outLen) {
double srcPos = (double)i * scale;
unsigned long idx = (unsigned long)(srcPos + 0.5);
if (idx >= inLen) { idx = inLen - 1UL; }
RS_T v;
unsafe {
v = in[idx];
out[i] = v;
}
i = i + 1UL;
}
}
void RS_LINEAR(const &RS_T in, unsigned long inLen, &RS_T out, unsigned long outLen) {
if (inLen == 0UL || outLen == 0UL) { return; }
if (inLen == 1UL) {
RS_T v;
unsafe { v = in[0]; }
unsigned long i = 0UL;
while (i < outLen) {
unsafe { out[i] = v; }
i = i + 1UL;
}
return;
}
double scale = 0.0;
if (outLen > 1UL) {
scale = (double)(inLen - 1UL) / (double)(outLen - 1UL);
}
unsigned long i = 0UL;
while (i < outLen) {
double srcPos = (double)i * scale;
unsigned long idx0 = (unsigned long)srcPos;
unsigned long idx1 = idx0 + 1UL;
if (idx1 >= inLen) { idx1 = inLen - 1UL; }
double frac = srcPos - (double)idx0;
double v0;
double v1;
unsafe {
v0 = (double)in[idx0];
v1 = (double)in[idx1];
out[i] = (RS_T)(v0 + frac * (v1 - v0));
}
i = i + 1UL;
}
}
static double resample_sinc_(double x) {
if (x == 0.0) { return 1.0; }
double px = SAFEC_DSP_PI_RESAMPLE * x;
return sin_d(px) / px;
}
// Each output is a windowed-sinc sum over the taps k = kStart + t around
// its source position, with x = srcPos - kStart:
// weight_t = sin(pi c (x - t)) / (pi (x - t)) * (1 + cos(pi (x - t) / hw)) / 2
// sin(pi c (x - t)) and cos(pi (x - t) / hw) expand by the angle-difference
// formulas into four trig values per output (of x) times tables of the tap
// angles (of t), made once per call (and the four values themselves are
// carried from output to output by rotation). The taps are then independent, so they
// run four to a vector. Outputs whose x is a whole number (a tap at x - t = 0)
// take the per-tap form below, as do kernels wider than the tables. Outputs
// are independent and run on the thread pool for longer signals.
#define RS_DMAX 8UL // largest kStart step advanced by rotation
#define RS_ANCHOR 64UL // exact trig at least this often
#ifdef __SAFEC_FREESTANDING__
#define RS_TAB 72UL
#else
#define RS_TAB 264UL
#endif
struct ResampleJob {
const RS_T* in;
RS_T* out;
unsigned long inLen;
unsigned long hw;
double scale;
double cutoff;
};
// One output by direct per-tap evaluation (any x, any hw).
static double resample_one_(const &ResampleJob j, double x, unsigned long kStart, unsigned long kEnd)
__attribute__((always_inline)) {
double c = j.cutoff;
double wb = SAFEC_DSP_PI_RESAMPLE / (double)j.hw;
double sum = 0.0;
unsigned long k = kStart;
while (k <= kEnd) {
double d = x - (double)(k - kStart);
double sincVal = d == 0.0 ? c : sin_d(SAFEC_DSP_PI_RESAMPLE * c * d) / (SAFEC_DSP_PI_RESAMPLE * d);
double v;
unsafe { v = (double)j.in[k]; }
sum = __builtin_fmuladd(v, sincVal * 0.5 * (1.0 + cos_d(wb * d)), sum);
k = k + 1UL;
}
return sum;
}
static void resample_outs_body_(const &ResampleJob j, unsigned long lo, unsigned long hi) __attribute__((always_inline)) {
double c = j.cutoff;
double wb = SAFEC_DSP_PI_RESAMPLE / (double)j.hw;
double pic = SAFEC_DSP_PI_RESAMPLE * c;
int tabbed = 2UL * j.hw + 1UL <= RS_TAB - 3UL;
// c1/s1 = cos/sin(pi c t), c2/s2 = cos/sin(wb t), tt = t (doubles)
double c1[RS_TAB]; double s1[RS_TAB]; double c2[RS_TAB]; double s2[RS_TAB]; double tt[RS_TAB];
if (tabbed) {
unsigned long t = 0UL;
while (t < RS_TAB) {
double td = (double)t;
c1[t] = cos_d(pic * td); s1[t] = sin_d(pic * td);
c2[t] = cos_d(wb * td); s2[t] = sin_d(wb * td);
tt[t] = td;
t = t + 1UL;
}
}
// From one output to the next x moves by scale - d, d = the step of
// kStart (a small integer), so sin/cos(pic x) and sin/cos(wb x) advance
// by rotations from these tables; every RS_ANCHOR outputs, or after a
// larger step, they are recomputed exactly so rounding can't build up.
double rcp[RS_DMAX + 1UL]; double rsp[RS_DMAX + 1UL]; double rcw[RS_DMAX + 1UL]; double rsw[RS_DMAX + 1UL];
unsigned long dd = 0UL;
while (dd <= RS_DMAX) {
double step = j.scale - (double)dd;
rcp[dd] = cos_d(pic * step); rsp[dd] = sin_d(pic * step);
rcw[dd] = cos_d(wb * step); rsw[dd] = sin_d(wb * step);
dd = dd + 1UL;
}
double ps = 0.0; double pc = 1.0; double ws = 0.0; double wc = 1.0;
unsigned long prevK = 0UL;
unsigned long i = lo;
while (i < hi) {
double srcPos = (double)i * j.scale;
unsigned long center = (unsigned long)srcPos;
unsigned long kStart = center >= j.hw ? center - j.hw : 0UL;
unsigned long kEnd = center + j.hw;
if (kEnd >= j.inLen) { kEnd = j.inLen - 1UL; }
double x = srcPos - (double)kStart;
unsigned long d = kStart - prevK;
if (i == lo || (i - lo) % RS_ANCHOR == 0UL || kStart < prevK || d > RS_DMAX) {
ps = sin_d(pic * x); pc = cos_d(pic * x);
ws = sin_d(wb * x); wc = cos_d(wb * x);
} else {
double t = __builtin_fmuladd(ps, rcp[d], pc * rsp[d]);
pc = __builtin_fmuladd(pc, rcp[d], -ps * rsp[d]);
ps = t;
t = __builtin_fmuladd(ws, rcw[d], wc * rsw[d]);
wc = __builtin_fmuladd(wc, rcw[d], -ws * rsw[d]);
ws = t;
}
prevK = kStart;
double sum = 0.0;
if (!tabbed || x == (double)(unsigned long)x) {
sum = resample_one_(j, x, kStart, kEnd);
} else {
unsigned long m = kEnd - kStart + 1UL;
f64x4 vps; vps[0] = ps; vps[1] = ps; vps[2] = ps; vps[3] = ps;
f64x4 vpc; vpc[0] = pc; vpc[1] = pc; vpc[2] = pc; vpc[3] = pc;
f64x4 vws; vws[0] = ws; vws[1] = ws; vws[2] = ws; vws[3] = ws;
f64x4 vwc; vwc[0] = wc; vwc[1] = wc; vwc[2] = wc; vwc[3] = wc;
f64x4 vx; vx[0] = x; vx[1] = x; vx[2] = x; vx[3] = x;
f64x4 half; half[0] = 0.5; half[1] = 0.5; half[2] = 0.5; half[3] = 0.5;
f64x4 acc = half - half;
unsigned long t = 0UL;
unsafe {
const RS_T* src = &j.in[kStart];
// The table rows run 3 past m, so the last group may read
// them; its extra lanes are dropped by the zero inputs.
while (t < m) {
f64x4 v;
v[0] = (double)src[t];
v[1] = t + 1UL < m ? (double)src[t + 1UL] : 0.0;
v[2] = t + 2UL < m ? (double)src[t + 2UL] : 0.0;
v[3] = t + 3UL < m ? (double)src[t + 3UL] : 0.0;
f64x4 num = vps * *(const f64x4*)&c1[t] - vpc * *(const f64x4*)&s1[t]; // sin(pi c (x - t))
f64x4 win = __builtin_fmuladd(vwc, *(const f64x4*)&c2[t], vws * *(const f64x4*)&s2[t]); // cos(wb (x - t))
f64x4 d = vx - *(const f64x4*)&tt[t];
f64x4 w = num * (half + half * win) / d;
acc = __builtin_fmuladd(v, w, acc);
t = t + 4UL;
}
}
sum = ((acc[0] + acc[1]) + (acc[2] + acc[3])) / SAFEC_DSP_PI_RESAMPLE; // the 1/pi of each weight
}
unsafe { j.out[i] = (RS_T)sum; }
i = i + 1UL;
}
}
static void resample_outs_base_(const &ResampleJob j, unsigned long lo, unsigned long hi) { resample_outs_body_(j, lo, hi); }
#if defined(__x86_64__)
static void resample_outs_fma_(const &ResampleJob j, unsigned long lo, unsigned long hi)
__attribute__((target("avx2,fma"))) { resample_outs_body_(j, lo, hi); }
#endif
static void resample_outs_(&ResampleJob j, unsigned long lo, unsigned long hi) {
#if defined(__x86_64__)
if (simd_level() >= SIMD_AVX2) { resample_outs_fma_(j, lo, hi); return; }
#endif
resample_outs_base_(j, lo, hi);
}
void RS_SINC(const &RS_T in, unsigned long inLen, &RS_T out, unsigned long outLen,
unsigned long kernelHalfWidth) {
if (inLen == 0UL || outLen == 0UL || kernelHalfWidth == 0UL) { return; }
double ratio = (double)outLen / (double)inLen;
struct ResampleJob j;
unsafe {
j.in = (const RS_T*)in;
j.out = (RS_T*)out;
}
j.inLen = inLen;
j.hw = kernelHalfWidth;
j.scale = (double)inLen / (double)outLen;
j.cutoff = ratio > 1.0 ? 1.0 : ratio;
if (outLen * kernelHalfWidth >= 131072UL) parallel_for(outLen, 2048UL, resample_outs_, &j);
else resample_outs_(&j, 0UL, outLen);
}