void apf(short *in, short *coeff, short *out, long delayi, short alpha,
short beta, short u, short agc, short ltgain, short order, short length, short br)
{
static int FirstTime = 1;
static short FIRmem[ORDER]; /* FIR filter memory */
static short IIRmem[ORDER]; /* IIR filter memory */
static short last;
static short Residual[ACBMemSize + SubFrameSize]; /* local residual */
short wcoef1[ORDER];
short wcoef2[ORDER];
short scratch[SubFrameSize];
short temp[SubFrameSize];
short mem[ORDER];
long sum1, sum2;
long gamma, APFgain;
short i, j, n, best;
short Stemp, shift1, shift2;
long Ltemp;
/* initialization -- should be done in init routine for implementation */
if (FirstTime)
{
FirstTime = 0;
for (i = 0; i < ORDER; i++)
FIRmem[i] = 0;
for (i = 0; i < ORDER; i++)
IIRmem[i] = 0;
for (i = 0; i < ACBMemSize; i++)
Residual[i] = 0;
last = 0;
}
/* Compute weighted LPC coefficients */
weight(wcoef1, coeff, alpha, order);
weight(wcoef2, coeff, beta, order);
/* Tilt speech */
/*...no tilt in non-voiced regions...*/
for (i = 0, sum2 = 0; i < length - 1; i++)
sum2 = L_mac(sum2, in[i], in[i + 1]);
if (sum2 < 0)
u = 0; /*...no tilt...*/
for (i = 0; i < length; i++)
{
scratch[i] = msu_r(L_deposit_h(in[i]), u, last);
last = in[i];
}
/* Compute residual */
fir(scratch, scratch, wcoef1, FIRmem, order, length);
for (i = 0; i < SubFrameSize ; i++)
Residual[ACBMemSize+i] = scratch[i];
/* long term filtering */
/* Find best integer delay around delayi */
j = extract_h(L_add(delayi, 32768));
sum1 = 0;
shift1 = 1;
best = j;
for (i = Max(DMIN, j - 3); i <= Min(DMAX, j + 3); i++)
{
shift2 = 1;
for (n = ACBMemSize, sum2 = 0; n < ACBMemSize + length; n++)
{
Ltemp = L_mult(Residual[n], Residual[n - i]);
Ltemp = L_shr(Ltemp, shift2);
sum2 = L_add(sum2, Ltemp);
if (sum2 >= 0x40000000)
{
sum2 = L_shr(sum2, 1);
shift2++;
}
}
if( ((shift1 >= shift2) && (L_shr(sum2,sub(shift1,shift2)) > sum1))
|| ((shift1 < shift2) && (sum2 > L_shr(sum1,sub(shift2,shift1)))))
{
sum1 = sum2;
shift1 = shift2;
best = i;
}
}
/* Get beta for delayi */
shift1 = 1;
for (i = ACBMemSize, sum1 = 0; i < ACBMemSize + length; i++)
{
Ltemp = L_mult(Residual[i - best], Residual[i - best]);
Ltemp = L_shr(Ltemp, shift1);
sum1 = L_add(sum1, Ltemp);
if (sum1 >= 0x40000000)
{
sum1 = L_shr(sum1, 1);
shift1++;
}
}
shift2 = 1;
for (i = ACBMemSize, sum2 = 0; i < ACBMemSize + length; i++)
{
Ltemp = L_mult(Residual[i], Residual[i - best]);
Ltemp = L_shr(Ltemp, shift2);
sum2 = L_add(sum2, Ltemp);
if (sum2 >= 0x40000000)
{
sum2 = L_shr(sum2, 1);
shift2++;
}
}
if (shift1 > shift2)
{
shift1 = sub(shift1, shift2);
sum2 = L_shr(sum2, shift1);
}
else if (shift1 < shift2)
{
shift2 = sub(shift2, shift1);
sum1 = L_shr(sum1, shift2);
}
if ((sum2 == 0) || (sum1 == 0) || (br == 1))
for (i = 0; i < length; i++)
temp[i] = Residual[i + ACBMemSize];
else
{
if (sum2 >= sum1)
gamma = 0x7fffffff; /* Clip gamma at 1.0 */
else if (sum2 < 0)
gamma = 0;
else
{
shift1 = norm_l(sum1);
sum1 = L_shl(sum1, shift1);
sum2 = L_shl(sum2, shift1);
gamma = L_divide(sum2, sum1);
}
if (gamma < 0x40000000)
for (i = 0; i < length; i++)
temp[i] = Residual[i + ACBMemSize];
else
{
/* Do actual filtering */
for (i = 0; i < length; i++)
{
Ltemp = L_mpy_ls(gamma, ltgain);
Ltemp = L_mpy_ls(Ltemp, Residual[ACBMemSize + i - best]);
temp[i] = add(Residual[ACBMemSize + i], round(Ltemp));
}
}
}
/* iir short term filter - first run */
for (i = 0; i < length; i++)
scratch[i] = temp[i];
for (i = 0; i < order; i++)
mem[i] = IIRmem[i];
iir(scratch, scratch, wcoef2, mem, order, length);
/* Get filter gain */
shift1 = 1;
for (i = 0, sum1 = 0; i < length; i++)
{
Ltemp = L_mult(in[i], in[i]);
Ltemp = L_shr(Ltemp, shift1);
sum1 = L_add(sum1, Ltemp);
if (sum1 >= 0x40000000)
{
sum1 = L_shr(sum1, 1);
shift1++;
}
}
shift2 = 1;
for (i = 0, sum2 = 0; i < length; i++)
{
Ltemp = L_mult(scratch[i], scratch[i]);
Ltemp = L_shr(Ltemp, shift2);
sum2 = L_add(sum2, Ltemp);
if (sum2 >= 0x40000000)
{
sum2 = L_shr(sum2, 1);
shift2++;
}
}
if (shift1 > shift2)
{
shift1 = sub(shift1, shift2);
sum2 = L_shr(sum2, shift1);
}
else if (shift1 < shift2)
{
shift2 = sub(shift2, shift1);
sum1 = L_shr(sum1, shift2);
}
if (sum2 != 0)
{
shift1 = norm_l(sum2);
sum2 = L_shl(sum2, shift1);
shift1 = sub(shift1, 2); /* For (1. < APFgain < 2.) */
sum1 = L_shl(sum1, shift1);
Ltemp = L_divide(sum1, sum2);
shift1 = norm_l(Ltemp);
Ltemp = L_shl(Ltemp, shift1);
Stemp = sqroot(Ltemp);
if (shift1 & 1)
APFgain = L_mult(0x5a82, Stemp);
else
APFgain = L_deposit_h(Stemp);
shift1 = shr(shift1, 1);
APFgain = L_shr(APFgain, shift1);
/* Re-normalize the speech signal */
for (i = 0; i < length; i++)
{
Ltemp = L_mpy_ls(APFgain, temp[i]);
Ltemp = L_shl(Ltemp, 1); /* For (1. < APFgain < 2.) */
temp[i] = round(Ltemp);
}
}
else
APFgain = 0x40000000;
/* iir short term filter - second run */
iir(out, temp, wcoef2, IIRmem, order, length);
/* Update residual buffer */
for (i = 0; i < ACBMemSize; i++)
Residual[i] = Residual[i + length];
}
void oblicz_pierwiastki(struct rownanie * e) {
if (e->wsp[0]!=0) {
float d=oblicz_d(e);
if (d>0) {
e->x1r=-e->wsp[1]-sqroot(d)/(2*e->wsp[0]);
e->x2r=-e->wsp[1]+sqroot(d)/(2*e->wsp[0]);
printf("\n2 pierwiastki x1=%f, x2=%f", e->x1r, e->x2r);
} else if (d==0) {
e->x1r=-e->wsp[1]/(2*e->wsp[0]);
printf("\n1 pierwiastek xo=%f",e->x1r);
} else {
e->x1r=(-e->wsp[1]/(float)(2*e->wsp[0]));
e->x2r=e->x1r;
float absd=d<0?-d:d;
e->u.x1u=-sqroot(absd)/(2*e->wsp[0]);
e->u.x2u=-e->u.x1u;
printf("\nPierwiastki zespolone:\n");
formatuj_zesp(e->x1r, e->u.x1u);
formatuj_zesp(e->x2r, e->u.x2u);
}
} else {
if (e->wsp[1]!=0) {
e->x1r=-e->wsp[2]/e->wsp[1];
printf("\nx1=%f",e->x1r);
} else if(e->wsp[1]==0 && e->wsp[2]!=0) printf("\nr.sprzeczne");
else if (e->wsp[1]==0 && e->wsp[2]==0) printf("\nrnieoznaczone");
}
}
int checkprime(int x)
{
int i;
for(i=2;i<=sqroot(x); i++)
{
if((x/i)*i==x){return 0;}
}
return 1;
}
void fndppf(short *delay, short *beta, short *buf, short dmin, short dmax, short length)
{
static short b = -10224; /* rom storage */
static short a[3] =
{-18739, 16024, -4882}; /* a[] scaled down by 4 */
short dnew = 0;
short sum;
long Lsum;
register short m, i, n;
static short DECbuf[FrameSize / 4];
long Lcorrmax, Lcmax, Ltmp;
short tap1;
short M1, M2, dnewtmp = 0;
static short lastgoodpitch = 0;
static short lastbeta = 0;
static short memory[3];
static int FirstTime = 1;
short Lsum_scale;
short shift, Lcorr_scale, Lcmax_scale;
short n1, n2, nq, nq1;
long Ltempf;
/* init static variables (should be in init routine for implementation) */
if (FirstTime)
{
FirstTime = 0;
n1 = (shr(FrameSize, 2));
for (i = 0; i < n1; i++)
DECbuf[i] = 0;
memory[0] = memory[1] = memory[2] = 0;
}
/* Shift memory of DECbuf */
for (i = 0; i < shr(length, 3); i++)
{
DECbuf[i] = DECbuf[i + shr(length, 3)];
}
/* filter signal and decimate */
for (i = 0, n = shr(length, 3); i < shr(length, 1); i++)
{
Ltempf = L_shr(L_deposit_h(buf[i + shr(length, 1)]), 4);
Ltempf = L_msu(Ltempf, memory[0], a[0]);
Ltempf = L_msu(Ltempf, memory[1], a[1]);
Ltempf = L_msu(Ltempf, memory[2], a[2]);
Ltempf = L_shl(Ltempf, 2);
shift = 0;
if ((i + 1) % 4 == 0)
{
Lsum = L_add(Ltempf, L_deposit_h(memory[2]));
Lsum = L_mac(Lsum, memory[0], b);
Lsum = L_mac(Lsum, memory[1], b);
DECbuf[n++] = round(L_shl(Lsum, 1));
}
memory[2] = memory[1];
memory[1] = memory[0];
memory[0] = round(Ltempf);
}
/* perform first search for best delay value in decimated domain */
Lcorrmax = (LW_MIN);
Lcorr_scale = 1;
for (m = shr(dmin, 2); m <= shr(dmax, 2); m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++)
{
Ltempf = L_mult(DECbuf[i], DECbuf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax))
|| ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale))))
)
{
Lcorrmax = Lsum;
Lcorr_scale = n1;
dnew = m;
}
}
/* Compare against lastgoodpitch */
if (lastgoodpitch != 0 && (abs_s(sub(lastgoodpitch, shl(dnew, 2))) > 2))
{
M1 = sub(shr(lastgoodpitch, 2), 2);
if (M1 < shr(dmin, 2))
M1 = shr(dmin, 2);
M2 = add(M1, 4);
if (M2 > shr(dmax, 2))
M2 = shr(dmax, 2);
Lcmax = LW_MIN;
Lcmax_scale = 1;
for (m = M1; m <= M2; m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++)
{
Ltempf = L_mult(DECbuf[i], DECbuf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax))
|| ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale))))
)
{ /* Gives some bias to low delays */
Lcmax = Lsum;
Lcmax_scale = n1;
dnewtmp = m;
}
}
Lsum = L_mpy_ls(Lcorrmax, 27361);
if (
((Lcmax_scale >= Lcorr_scale) && (L_shr(Lsum, sub(Lcmax_scale, Lcorr_scale)) < Lcmax))
|| ((Lcmax_scale < Lcorr_scale) && (Lsum < L_shr(Lcmax, sub(Lcorr_scale, Lcmax_scale))))
)
{
dnew = dnewtmp;
}
}
/* perform first search for best delay value in non-decimated buffer */
M1 = Max(sub(shl(dnew, 2), 3), dmin);
if (M1 < dmin)
M1 = dmin;
M2 = Min(add(shl(dnew, 2), 3), dmax);
if (M2 > dmax)
M2 = dmax;
Lcorrmax = LW_MIN;
Lcorr_scale = 1;
for (m = M1; m <= M2; m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < sub(length, m); i++)
{
Ltempf = L_mult(buf[i], buf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax))
|| ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale))))
)
{
Lcorrmax = Lsum;
Lcorr_scale = n1;
dnew = m;
}
}
Lsum_scale = 1;
for (i = 0, Lsum = 0; i < sub(length, dnew); i++)
{
Ltempf = L_mult(buf[i + dnew], buf[i + dnew]);
Ltempf = L_shr(Ltempf, Lsum_scale);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
Lsum_scale++;
}
}
Lcmax_scale = 1;
for (i = 0, Lcmax = 0; i < length - dnew; i++)
{
Ltempf = L_mult(buf[i], buf[i]);
Ltempf = L_shr(Ltempf, Lcmax_scale);
Lcmax = L_add(Lcmax, Ltempf);
if (L_abs(Lcmax) >= 0x40000000)
{
Lcmax = L_shr(Lcmax, 1);
Lcmax_scale++;
}
}
nq = norm_l(Lsum);
Lsum = L_shl(Lsum, nq);
nq1 = norm_l(Lcmax);
Lcmax = L_shl(Lcmax, nq1);
Lsum = L_mpy_ll(Lsum, Lcmax);
n1 = norm_l(Lsum);
Lsum = L_shl(Lsum, n1);
sum = sqroot(Lsum);
n1 = add(add(n1, nq), nq1);
n1 = sub(sub(n1, Lcmax_scale), Lsum_scale);
n2 = shr(n1, 1);
if (n1 & 1)
Lsum = L_mult(sum, 23170);
else
Lsum = L_deposit_h(sum);
n2 = add(n2, Lcorr_scale);
Lcorrmax = L_shl(Lcorrmax, n2);
if ((Lsum == 0) || (Lcorrmax <= 0))
*beta = 0;
else if (Lcorrmax > Lsum)
*beta = 0x7fff;
else
*beta = round(L_divide(Lcorrmax, Lsum));
/* perform search for best delay value in around old pitch delay */
if (lastgoodpitch != 0)
{
M1 = lastgoodpitch - 6;
M2 = lastgoodpitch + 6;
if (M1 < dmin)
M1 = dmin;
if (M2 > dmax)
M2 = dmax;
if (dnew > M2 || dnew < M1)
{
Lcmax = LW_MIN;
Lcmax_scale = 1;
for (m = M1; m <= M2; m++)
{
n1 = 1;
for (i = 0, Lsum = 0; i < length - m; i++)
{
Ltempf = L_mult(buf[i], buf[i + m]);
Ltempf = L_shr(Ltempf, n1);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
n1++;
}
}
if (
((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax))
|| ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale))))
)
{
Lcmax = Lsum;
dnewtmp = m;
Lcmax_scale = n1;
}
}
Lcorr_scale = 1;
for (i = 0, Ltmp = 0; i < length - dnewtmp; i++)
{
Ltempf = L_mult(buf[i + dnewtmp], buf[i + dnewtmp]);
Ltempf = L_shr(Ltempf, Lcorr_scale);
Ltmp = L_add(Ltmp, Ltempf);
if (L_abs(Ltmp) >= 0x40000000)
{
Ltmp = L_shr(Ltmp, 1);
Lcorr_scale++;
}
}
Lsum_scale = 1;
for (i = 0, Lsum = 0; i < length - dnewtmp; i++)
{
Ltempf = L_mult(buf[i], buf[i]);
Ltempf = L_shr(Ltempf, Lsum_scale);
Lsum = L_add(Lsum, Ltempf);
if (L_abs(Lsum) >= 0x40000000)
{
Lsum = L_shr(Lsum, 1);
Lsum_scale++;
}
}
nq = norm_l(Ltmp);
Ltmp = L_shl(Ltmp, nq);
nq1 = norm_l(Lsum);
Lsum = L_shl(Lsum, nq1);
Ltmp = L_mpy_ll(Ltmp, Lsum);
n1 = norm_l(Ltmp);
Ltmp = L_shl(Ltmp, n1);
sum = sqroot(Ltmp);
n1 = add(add(n1, nq), nq1);
n1 = sub(sub(n1, Lsum_scale), Lcorr_scale);
n2 = shr(n1, 1);
if (n1 & 1)
Ltmp = L_mult(sum, 23170);
else
Ltmp = L_deposit_h(sum);
n2 = add(n2, Lcmax_scale);
Lcmax = L_shl(Lcmax, n2);
if ((Ltmp == 0) || (Lcmax <= 0))
tap1 = 0;
else if (Lcmax >= Ltmp)
tap1 = 0x7fff;
else
tap1 = round(L_divide(Lcmax, Ltmp));
/* Replace dnew with dnewtmp if tap1 is large enough */
if ((dnew > M2 && (shr(tap1, 1) > mult_r(9830, *beta))) ||
(dnew < M1 && (shr(tap1, 1) > mult_r(19661, *beta))))
{
dnew = dnewtmp;
*beta = (tap1);
}
}
}
*delay = dnew;
if (*beta > 13107)
{
lastgoodpitch = dnew;
lastbeta = *beta;
}
else
{
lastbeta = mult_r(24576, lastbeta);
if (lastbeta < 9830)
lastgoodpitch = 0;
}
}
short e_ran_g(short *seed0)
{
static int iset = 0;
static long gset;
long rsq, ltemp1, ltemp2;
short sv1, sv2, rsq_s;
short shft_fctr, stemp1;
short shft_fctr1;
/* ======================================================================== */
long ltmp1, ltmp2;
short ans = 0;
short stmp2;
/* ======================================================================== */
if (iset == 0)
{
sv1 = shl(sub(ran0(seed0), 16384), 1);
sv2 = shl(sub(ran0(seed0), 16384), 1);
/* rsq = sv1 * sv1 + sv2 * sv2; */
ltemp1 = L_mult(sv1, sv1);
ltemp2 = L_mult(sv2, sv2);
rsq = L_add(L_shr(ltemp1, 1), L_shr(ltemp2, 1));
if (rsq >= 1073741824 || rsq == 0){
/* If condition not met, don't iterate; use */
/* rough approximation. */
ans = shr(sv1,3);
ans = add(ans, shr(sv2,3));
ans = add(ans, shr(sub(ran0(seed0), 16384),2));
return (ans);
}
/*
* error in rsq doesn't seem to contribute to the final error in e_ran_g
*/
/*
* rsq scale down by two: input to fnLog must be scaled up by 2.
*/
rsq = L_shl(rsq, 1);
/* stemp1 = round(L_negate(fnLog(rsq))); */
ltmp1 = L_negate(fnLog(rsq));
/*
* rsq must be greater than the log of lsq for the fractional
* divide to work. therfore normalize rsq.
*/
shft_fctr = norm_l(rsq);
rsq_s = round(L_shl(rsq, shft_fctr));
stmp2 = (divide_s(round(ltmp1), rsq_s));
/*
* stemp2 must be normalized before taking its square root.
* (increases precision).
*/
shft_fctr1 = norm_s(stmp2);
ltmp2 = L_deposit_h(shl(stmp2, shft_fctr1));
stemp1 = sqroot(ltmp2);
/*
* shifting involved before taking the square root:
* LEFT << shft_fctr. (LEFT because rsq is in the denominator
* of ltemp2 quotion).
* LEFT << 6. (multiply by 2 in original code and multiply by 32
* because output of fnLog scaled down by 32).
* RIGHT >> shft_fctr1. (normalization taken before sqroot).
*/
shft_fctr = shft_fctr + 6 - shft_fctr1;
/*
* PROPERTY: sqrt(2^n) = 2^(n/2)
* if shft_fctr is odd; multiply stemp1 by sqrt(2)/2 and
* increment number of shifts by 1. Can now use shft_fctr / 2.
*/
if (shft_fctr & 0x0001)
{
stemp1 = mult(stemp1, 23170);
shft_fctr++;
}
shft_fctr = shr(shft_fctr, 1);
/*
* normalize stemp1 for the following multiplication.
* adjust shft_fctr accordingly.
*/
shft_fctr1 = norm_s(stemp1);
stemp1 = shl(stemp1, shft_fctr1);
shft_fctr = shft_fctr - shft_fctr1;
gset = L_mult(sv1, stemp1);
/*
* final output is scaled down by 4, therefore shift up by
* shft_fctr - 2.
*/
gset = L_shl(gset, shft_fctr - 2);
iset = 1;
return round(L_shl(L_mult(sv2, stemp1), shft_fctr - 2));
}
else
{
iset = 0;
return round(gset);
}
}
float distance(float x, float y, float z)
{
float d1 = sqroot((x * x) + (y * y));
float d2 = sqroot((d1 * d1) + (z * z));
return d2;
}
