// SafeC Standard Library — General bilinear transform + Z-domain response
// implementation (see ztransform.h)
#pragma once
#include <std/dsp/ztransform.h>
#include <std/dsp/complex_dsp.h>
#include <std/math.h>
#include <std/mem.h>
#define SAFEC_DSP_TWO_PI_ZT 6.28318530717958647692
namespace std {
// Multiply the ascending-power-of-z polynomial 'poly' (length polyLen) by
// the linear factor (z + factor), writing the length-(polyLen+1) result
// into 'out'.
static void poly_mul_linear_(const double* poly, unsigned long polyLen, double factor, double* out) {
unsigned long j = 0UL;
while (j <= polyLen) {
unsafe { out[j] = 0.0; }
j = j + 1UL;
}
j = 0UL;
while (j < polyLen) {
double c;
unsafe { c = poly[j]; }
unsafe {
out[j] = out[j] + c * factor;
out[j + 1UL] = out[j + 1UL] + c;
}
j = j + 1UL;
}
}
void bilinear_nth_order_n_(const &double B, const &double A, unsigned long order,
double fs, &double bOut, &double aOut) {
unsigned long m = order;
double c = 2.0 * fs;
double* numPoly;
double* denPoly;
double* bufA;
double* bufB;
unsafe {
numPoly = (double*)alloc_raw_((m + 1UL) * (unsigned long)sizeof(double));
denPoly = (double*)alloc_raw_((m + 1UL) * (unsigned long)sizeof(double));
bufA = (double*)alloc_raw_((m + 1UL) * (unsigned long)sizeof(double));
bufB = (double*)alloc_raw_((m + 1UL) * (unsigned long)sizeof(double));
}
unsigned long j = 0UL;
while (j <= m) {
unsafe { numPoly[j] = 0.0; denPoly[j] = 0.0; }
j = j + 1UL;
}
double ck = 1.0; // c^k, built up incrementally
unsigned long k = 0UL;
while (k <= m) {
// Build (z-1)^k * (z+1)^(m-k), ascending powers, into bufA.
unsigned long curLen = 1UL;
unsafe { bufA[0] = 1.0; }
unsigned long t = 0UL;
while (t < k) {
unsafe { poly_mul_linear_(bufA, curLen, -1.0, bufB); }
curLen = curLen + 1UL;
unsigned long cpy = 0UL;
while (cpy < curLen) {
unsafe { bufA[cpy] = bufB[cpy]; }
cpy = cpy + 1UL;
}
t = t + 1UL;
}
unsigned long remain = m - k;
t = 0UL;
while (t < remain) {
unsafe { poly_mul_linear_(bufA, curLen, 1.0, bufB); }
curLen = curLen + 1UL;
unsigned long cpy = 0UL;
while (cpy < curLen) {
unsafe { bufA[cpy] = bufB[cpy]; }
cpy = cpy + 1UL;
}
t = t + 1UL;
}
double bk;
double ak;
unsafe { bk = B[k]; ak = A[k]; }
j = 0UL;
while (j <= m) {
double coeff;
unsafe { coeff = bufA[j]; }
unsafe {
numPoly[j] = numPoly[j] + bk * ck * coeff;
denPoly[j] = denPoly[j] + ak * ck * coeff;
}
j = j + 1UL;
}
ck = ck * c;
k = k + 1UL;
}
// numPoly/denPoly are ascending powers of z; reversing index j -> m-j
// converts z^j into the negative-power-of-z term z^-(m-j), and
// dividing by denPoly[m] normalizes aOut[0] to 1.
double a0;
unsafe { a0 = denPoly[m]; }
j = 0UL;
while (j <= m) {
double nb;
double na;
unsafe { nb = numPoly[m - j]; na = denPoly[m - j]; }
unsafe {
bOut[j] = nb / a0;
aOut[j] = na / a0;
}
j = j + 1UL;
}
unsafe {
dealloc(numPoly);
dealloc(denPoly);
dealloc(bufA);
dealloc(bufB);
}
}
static struct Complex ztransform_response_(const double* b, const double* a, unsigned long order,
double fs, double freqHz) {
double omega = SAFEC_DSP_TWO_PI_ZT * freqHz / fs;
struct Complex zInv = Complex::new(cos_d(-omega), sin_d(-omega));
struct Complex numAcc = Complex::new(0.0, 0.0);
struct Complex denAcc = Complex::new(0.0, 0.0);
struct Complex zPow = Complex::new(1.0, 0.0);
unsigned long i = 0UL;
while (i <= order) {
double bi;
double ai;
unsafe { bi = b[i]; ai = a[i]; }
struct Complex bCoef = Complex::new(bi, 0.0);
struct Complex aCoef = Complex::new(ai, 0.0);
struct Complex bTerm = bCoef * zPow;
struct Complex aTerm = aCoef * zPow;
struct Complex newNum = numAcc + bTerm;
struct Complex newDen = denAcc + aTerm;
numAcc = newNum;
denAcc = newDen;
struct Complex nextZPow = zPow * zInv;
zPow = nextZPow;
i = i + 1UL;
}
return numAcc / denAcc;
}
double ztransform_response_mag_n_(const &double b, const &double a, unsigned long order,
double fs, double freqHz) {
struct Complex h = ztransform_response_(b, a, order, fs, freqHz);
return h.abs();
}
double ztransform_response_phase_n_(const &double b, const &double a, unsigned long order,
double fs, double freqHz) {
struct Complex h = ztransform_response_(b, a, order, fs, freqHz);
return h.arg();
}
// ── Slice API (the public signatures; the (reference, length) cores above
// are the *_n_ functions) ─────────────────────────────────────────────────────
int bilinear_nth_order([]const double B, []const double A, double fs, []double bOut, []double aOut) {
if (B.len == 0UL) return 0;
unsigned long order = B.len - 1UL;
if (A.len < order + 1UL || bOut.len < order + 1UL || aOut.len < order + 1UL) return 0;
unsafe { bilinear_nth_order_n_((const &double)B.ptr, (const &double)A.ptr, order, fs, (&double)bOut.ptr, (&double)aOut.ptr); }
return 1;
}
double ztransform_response_mag([]const double b, []const double a, double fs, double freqHz) {
unsigned long m = b.len < a.len ? b.len : a.len;
if (m == 0UL) return 0.0;
unsafe { return ztransform_response_mag_n_((const &double)b.ptr, (const &double)a.ptr, m - 1UL, fs, freqHz); }
}
double ztransform_response_phase([]const double b, []const double a, double fs, double freqHz) {
unsigned long m = b.len < a.len ? b.len : a.len;
if (m == 0UL) return 0.0;
unsafe { return ztransform_response_phase_n_((const &double)b.ptr, (const &double)a.ptr, m - 1UL, fs, freqHz); }
}
} // namespace std