public static unsafe void safe_rdft(int len, int type, double[] d, thread_fft_cache info)
{
//assert(is_power_of_2(len));
update_fft_cache(len, info);
fixed(int *lsx_fft_br = info.lsx_fft_br)
fixed(double *lsx_fft_sc = info.lsx_fft_sc)
fixed(double *dd = d)
rdft(len, type, dd, lsx_fft_br, lsx_fft_sc);
}
static void update_fft_cache(int len, thread_fft_cache info)
{
if (len > info.fft_len)
{
int old_n = info.fft_len;
int[] old_br = info.lsx_fft_br;
double[] old_sc = info.lsx_fft_sc;
info.fft_len = len;
info.lsx_fft_br = new int[dft_br_len(info.fft_len)];
info.lsx_fft_sc = new double[dft_sc_len(info.fft_len)];
if (old_n == 0)
{
info.lsx_fft_br[0] = 0;
}
else
{
Buffer.BlockCopy(old_br, 0, info.lsx_fft_br, 0, old_br.Length * sizeof(int));
Buffer.BlockCopy(old_sc, 0, info.lsx_fft_sc, 0, old_sc.Length * sizeof(double));
}
}
}
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);
}
public static unsafe void safe_rdft(int len, int type, double[] d, thread_fft_cache info)
{
//assert(is_power_of_2(len));
update_fft_cache(len, info);
fixed (int* lsx_fft_br = info.lsx_fft_br)
fixed (double* lsx_fft_sc = info.lsx_fft_sc)
fixed (double* dd = d)
rdft(len, type, dd, lsx_fft_br, lsx_fft_sc);
}
static void update_fft_cache(int len, thread_fft_cache info)
{
if (len > info.fft_len)
{
int old_n = info.fft_len;
int[] old_br = info.lsx_fft_br;
double[] old_sc = info.lsx_fft_sc;
info.fft_len = len;
info.lsx_fft_br = new int[dft_br_len(info.fft_len)];
info.lsx_fft_sc = new double[dft_sc_len(info.fft_len)];
if (old_n == 0)
info.lsx_fft_br[0] = 0;
else
{
Buffer.BlockCopy(old_br, 0, info.lsx_fft_br, 0, old_br.Length * sizeof(int));
Buffer.BlockCopy(old_sc, 0, info.lsx_fft_sc, 0, old_sc.Length * sizeof(double));
}
}
}
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);
}