static void half_band_filter_init(rate_shared_t p, int which,
int num_taps, double[] h, double Fp, double att, int multiplier,
double phase, bool allow_aliasing)
{
dft_filter_t f = p.half_band[which];
int dft_length, i;
if (f.num_taps != 0)
{
return;
}
if (h != null)
{
dft_length = lsx_set_dft_length(num_taps);
f.coefs = new double[dft_length];
for (i = 0; i < num_taps; ++i)
{
f.coefs[(i + dft_length - num_taps + 1) & (dft_length - 1)]
= h[Math.Abs(num_taps / 2 - i)] / dft_length * 2 * multiplier;
}
f.post_peak = num_taps / 2;
}
else
{
h = lsx_design_lpf(Fp, 1.0, 2.0, allow_aliasing, att, ref num_taps, 0);
if (phase != 50)
{
lsx_fir_to_phase(ref h, ref num_taps, ref f.post_peak, phase, p.info);
}
else
{
f.post_peak = num_taps / 2;
}
dft_length = lsx_set_dft_length(num_taps);
f.coefs = new double[dft_length];
for (i = 0; i < num_taps; ++i)
{
f.coefs[(i + dft_length - num_taps + 1) & (dft_length - 1)]
= h[i] / dft_length * 2 * multiplier;
}
}
//assert(num_taps & 1);
f.num_taps = num_taps;
f.dft_length = dft_length;
//lsx_debug("fir_len=%i dft_length=%i Fp=%g att=%g mult=%i",
//num_taps, dft_length, Fp, att, multiplier);
SOXFft.safe_rdft(dft_length, 1, f.coefs, p.info);
}
static unsafe void double_sample(stage_t p, fifo_t output_fifo)
{
int num_in = Math.Max(0, p.fifo.occupancy);
dft_filter_t f = p.shared.half_band[1];
int overlap = f.num_taps - 1;
while (num_in > f.dft_length >> 1)
{
int input_offs = p.fifo.offset;
p.fifo.read((f.dft_length - overlap) >> 1, null);
num_in -= (f.dft_length - overlap) >> 1;
int output_offs = output_fifo.reserve(f.dft_length);
output_fifo.trim_by(overlap);
fixed(byte *pinput = &p.fifo.data[input_offs])
fixed(byte *poutput = &output_fifo.data[output_offs])
fixed(double *lsx_fft_sc = p.shared.info.lsx_fft_sc)
fixed(int *lsx_fft_br = p.shared.info.lsx_fft_br)
{
double *input = (double *)pinput;
double *output = (double *)poutput;
for (int j = 0, i = 0; i < f.dft_length; ++j, i += 2)
{
output[i] = input[j];
output[i + 1] = 0;
}
SOXFft.rdft(f.dft_length, 1, output, lsx_fft_br, lsx_fft_sc);
output[0] *= f.coefs[0];
output[1] *= f.coefs[1];
for (int i = 2; i < f.dft_length; i += 2)
{
double tmp = output[i];
output[i] = f.coefs[i] * tmp - f.coefs[i + 1] * output[i + 1];
output[i + 1] = f.coefs[i + 1] * tmp + f.coefs[i] * output[i + 1];
}
SOXFft.rdft(f.dft_length, -1, output, lsx_fft_br, lsx_fft_sc);
}
}
}
static unsafe void half_sample(stage_t p, fifo_t output_fifo)
{
int num_in = Math.Max(0, p.fifo.occupancy);
dft_filter_t f = p.shared.half_band[p.which];
int overlap = f.num_taps - 1;
while (num_in >= f.dft_length)
{
int input_offs = p.fifo.offset;
p.fifo.read(f.dft_length - overlap, null);
num_in -= f.dft_length - overlap;
int output_offs = output_fifo.reserve(f.dft_length);
output_fifo.trim_by((f.dft_length + overlap) >> 1);
Buffer.BlockCopy(p.fifo.data, input_offs, output_fifo.data, output_offs, f.dft_length * sizeof(double));
fixed(byte *poutput = &output_fifo.data[output_offs])
fixed(double *lsx_fft_sc = p.shared.info.lsx_fft_sc)
fixed(int *lsx_fft_br = p.shared.info.lsx_fft_br)
{
double *output = (double *)poutput;
SOXFft.rdft(f.dft_length, 1, output, lsx_fft_br, lsx_fft_sc);
output[0] *= f.coefs[0];
output[1] *= f.coefs[1];
for (int i = 2; i < f.dft_length; i += 2)
{
double tmp = output[i];
output[i] = f.coefs[i] * tmp - f.coefs[i + 1] * output[i + 1];
output[i + 1] = f.coefs[i + 1] * tmp + f.coefs[i] * output[i + 1];
}
SOXFft.rdft(f.dft_length, -1, output, lsx_fft_br, lsx_fft_sc);
for (int j = 1, i = 2; i < f.dft_length - overlap; ++j, i += 2)
{
output[j] = output[i];
}
}
}
}
static void lsx_fir_to_phase(ref double[] h, ref int len, ref int post_len, double phase, thread_fft_cache info)
{
double phase1 = (phase > 50 ? 100 - phase : phase) / 50;
int i, work_len, begin, end, imp_peak = 0, peak = 0;
double imp_sum = 0, peak_imp_sum = 0;
double prev_angle2 = 0, cum_2pi = 0, prev_angle1 = 0, cum_1pi = 0;
for (i = len, work_len = 2 * 2 * 8; i > 1; work_len <<= 1, i >>= 1)
{
;
}
double[] pi_wraps = new double[work_len + 2]; /* +2: (UN)PACK */
double[] work = new double[(work_len + 2) / 2];
Buffer.BlockCopy(h, 0, work, 0, len * sizeof(double));
SOXFft.safe_rdft(work_len, 1, work, info); /* Cepstral: */
work[work_len] = work[1]; work[work_len + 1] = work[0]; //LSX_UNPACK(work, work_len);
for (i = 0; i <= work_len; i += 2)
{
double angle = Math.Atan2(work[i + 1], work[i]);
double detect = 2 * Math.PI;
double delta = angle - prev_angle2;
double adjust = detect * ((delta < -detect * 0.7 ? 1 : 0) - (delta > detect * 0.7 ? 1 : 0));
prev_angle2 = angle;
cum_2pi += adjust;
angle += cum_2pi;
detect = Math.PI;
delta = angle - prev_angle1;
adjust = detect * ((delta < -detect * .7 ? 1 : 0) - (delta > detect * .7 ? 1 : 0));
prev_angle1 = angle;
cum_1pi += Math.Abs(adjust); /* fabs for when 2pi and 1pi have combined */
pi_wraps[i >> 1] = cum_1pi;
double tt = Math.Sqrt(work[i] * work[i] + work[i + 1] * work[i + 1]);
// assert(tt >= 0)
work[i] = tt > 0 ? Math.Log(tt) : -26;
work[i + 1] = 0;
}
work[1] = work[work_len]; // LSX_PACK(work, work_len);
SOXFft.safe_rdft(work_len, -1, work, info);
for (i = 0; i < work_len; ++i)
{
work[i] *= 2.0 / work_len;
}
for (i = 1; i < work_len / 2; ++i)
{
/* Window to reject acausal components */
work[i] *= 2;
work[i + work_len / 2] = 0;
}
SOXFft.safe_rdft(work_len, 1, work, info);
for (i = 2; i < work_len; i += 2) /* Interpolate between linear & min phase */
{
work[i + 1] = phase1 * i / work_len * pi_wraps[work_len >> 1] +
(1 - phase1) * (work[i + 1] + pi_wraps[i >> 1]) - pi_wraps[i >> 1];
}
work[0] = Math.Exp(work[0]);
work[1] = Math.Exp(work[1]);
for (i = 2; i < work_len; i += 2)
{
double x = Math.Exp(work[i]);
work[i] = x * Math.Cos(work[i + 1]);
work[i + 1] = x * Math.Sin(work[i + 1]);
}
SOXFft.safe_rdft(work_len, -1, work, info);
for (i = 0; i < work_len; ++i)
{
work[i] *= 2.0 / work_len;
}
/* Find peak pos. */
for (i = 0; i <= (int)(pi_wraps[work_len >> 1] / Math.PI + .5); ++i)
{
imp_sum += work[i];
if (Math.Abs(imp_sum) > Math.Abs(peak_imp_sum))
{
peak_imp_sum = imp_sum;
peak = i;
}
//if (work[i] > work[imp_peak]) /* For debug check only */
//imp_peak = i;
}
while (peak > 0 && Math.Abs(work[peak - 1]) > Math.Abs(work[peak]) && work[peak - 1] * work[peak] > 0)
{
--peak;
}
if (phase1 == 0)
{
begin = 0;
}
else if (phase1 == 1)
{
begin = peak - len / 2;
}
else
{
begin = (int)((.997 - (2 - phase1) * .22) * len + .5);
end = (int)((.997 + (0 - phase1) * .22) * len + .5);
begin = peak - begin - (begin & 1);
end = peak + 1 + end + (end & 1);
len = end - begin;
double[] h1 = new double[len];
Buffer.BlockCopy(h, 0, h1, 0, Math.Min(h.Length, h1.Length) * sizeof(double));
h = h1;
}
for (i = 0; i < len; ++i)
{
h[i] =
work[(begin + (phase > 50 ? len - 1 - i : i) + work_len) & (work_len - 1)];
}
post_len = phase > 50 ? peak - begin : begin + len - (peak + 1);
//lsx_debug("nPI=%g peak-sum@%i=%g (val@%i=%g); len=%i post=%i (%g%%)",
// pi_wraps[work_len >> 1] / M_PI, peak, peak_imp_sum, imp_peak,
// work[imp_peak], *len, *post_len, 100 - 100. * *post_len / (*len - 1));
//free(pi_wraps), free(work);
}