1 /*
2   lpred.c:
3   Copyright 2020 Victor Lazzarini
4   streaming linear prediction
5   This file is part of Csound.
6   The Csound Library is free software; you can redistribute it
7   and/or modify it under the terms of the GNU Lesser General Public
8   License as published by the Free Software Foundation; either
9   version 2.1 of the License, or (at your option) any later version.
10   Csound is distributed in the hope that it will be useful,
11   but WITHOUT ANY WARRANTY; without even the implied warranty of
12   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13   GNU Lesser General Public License for more details.
14   You should have received a copy of the GNU Lesser General Public
15   License along with Csound; if not, write to the Free Software
16   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
17   02110-1301 USA
18 */
19 
20 #include <stdlib.h>
21 #include <math.h>
22 #include "csoundCore.h"
23 #include "csound.h"
24 #include "fftlib.h"
25 #include "lpred.h"
26 
magc(MYCMPLX c)27   static inline MYFLT magc(MYCMPLX c) {
28       return HYPOT(c.re, c.im);
29   }
30 
phsc(MYCMPLX c)31   static inline MYFLT phsc(MYCMPLX c) {
32     return ATAN2(c.im, c.re);
33   }
34 
35 /** autocorrelation
36     r - output
37     s - input
38     size - input size
39     returns r
40 */
csoundAutoCorrelation(CSOUND * csound,MYFLT * r,MYFLT * s,int size)41 MYFLT *csoundAutoCorrelation(CSOUND *csound, MYFLT *r, MYFLT *s, int size){
42   MYFLT sum;
43   int n,m,o;
44   for(n=0; n < size; n++) {
45     sum = FL(0.0);
46     for(m=n,o=0; m < size; m++,o++)
47       sum += s[o]*s[m];
48     r[n] = sum;
49   }
50   return r;
51 }
52 
53 typedef struct LPCparam_ {
54   MYFLT *r, *E, *b, *k, *pk, *am, *tmpmem, *cf, cps, rms;
55   MYCMPLX *pl;
56   int32_t N, M;
57 } LPCparam;
58 
59 
60 /** Set up linear prediction memory for
61     autocorrelation size N and predictor order M
62 */
csoundLPsetup(CSOUND * csound,int N,int M)63 void *csoundLPsetup(CSOUND *csound, int N, int M) {
64   LPCparam *p = csound->Calloc(csound, sizeof(LPCparam));
65 
66   if(N) {
67     // allocate LP analysis memory if needed
68     N = N < M+1 ? M+1 : N;
69     p->r = csound->Calloc(csound, sizeof(MYFLT)*N);
70     p->pk = csound->Calloc(csound, sizeof(MYFLT)*N);
71     p->am = csound->Calloc(csound, sizeof(MYFLT)*N);
72     p->E = csound->Calloc(csound, sizeof(MYFLT)*(M+1));
73     p->k = csound->Calloc(csound, sizeof(MYFLT)*(M+1));
74     p->b = csound->Calloc(csound, sizeof(MYFLT)*(M+1)*(M+1));
75   }
76 
77   // otherwise just allocate coefficient/pole memory
78   p->pl = csound->Calloc(csound, sizeof(MYCMPLX)*(M+1));
79   p->cf = csound->Calloc(csound, sizeof(MYFLT)*(M+1));
80   p->tmpmem = csound->Calloc(csound, sizeof(MYFLT)*(M+1));
81 
82   p->N = N;
83   p->M = M;
84   p->cps = 0;
85   p->rms  = 0;
86   return p;
87 }
88 
89 /** Free linear prediction memory
90  */
csoundLPfree(CSOUND * csound,void * parm)91 void csoundLPfree(CSOUND *csound, void *parm) {
92   LPCparam *p = (LPCparam *) parm;
93   csound->Free(csound, p->r);
94   csound->Free(csound, p->b);
95   csound->Free(csound, p->k);
96   csound->Free(csound, p->E);
97   csound->Free(csound, p);
98 }
99 
100 /** Linear Prediction function
101     output format: M+1 MYFLT array [E,c1,c2,...,cm]
102     NB: c0 is always 1
103 */
csoundLPred(CSOUND * csound,void * parm,MYFLT * x)104 MYFLT *csoundLPred(CSOUND *csound, void *parm, MYFLT *x){
105 
106   LPCparam *p = (LPCparam *) parm;
107   MYFLT *r = p->r;
108   MYFLT *E = p->E;
109   MYFLT *b = p->b;
110   MYFLT *k = p->k;
111   MYFLT s;
112   int N = p->N;
113   int M = p->M;
114   int L = M+1;
115   int m,i;
116 
117   r = csoundAutoCorrelation(csound,r,x,N);
118   MYFLT ro = r[0];
119   p->rms = SQRT(ro/N);
120   if (ro > FL(0.0)) {
121     /* if signal power > 0 , do linear prediction */
122     for(i=0;i<L;i++) r[i] /= ro;
123     E[0] = r[0];
124     b[M*L] = 1.;
125     for(m=1;m<L;m++) {
126       s = 0.;
127       b[(m-1)*L] = 1.;
128       for(i=0;i<m;i++)
129         s += b[(m-1)*L+i]*r[m-i];
130       k[m] = -(s)/E[m-1];
131       b[m*L+m] = k[m];
132       for(i=1;i<m;i++)
133         b[m*L+i] = b[(m-1)*L+i] + k[m]*b[(m-1)*L+(m-i)];
134       E[m] = (1 - k[m]*k[m])*E[m-1];
135     }
136     /* replace first coeff with squared error E*/
137     b[M*L] = E[M];
138   }
139   /* return E + coeffs */
140   return &b[M*L];
141 }
142 
143 /** LP coeffs to Cepstrum
144     takes an array c of N size
145     and an array b of M+1 size with M all-pole coefficients
146     and squared error E in place of coefficient 0 [E,c1,...,cM]
147     returns N cepstrum coefficients
148 */
csoundLPCeps(CSOUND * csound,MYFLT * c,MYFLT * b,int N,int M)149 MYFLT *csoundLPCeps(CSOUND *csound, MYFLT *c, MYFLT *b,
150                     int N, int M){
151   int n,m;
152   MYFLT s;
153   c[0] = -LOG(b[0]);
154   c[1] = b[1];
155   for(n=2;n<N;n++){
156     if(n > M)
157       c[n] = 0;
158     else {
159       s = 0.;
160       for(m=1;m<n;m++)
161         s += (m/n)*c[m]*b[n-m];
162       c[n] = b[n] - s;
163     }
164   }
165   for(n=0;n<N;n++) c[n] *= -1;
166   return c;
167 }
168 
169 /** Cepstrum to LP coeffs
170     takes an array c of N size
171     and an array b of M+1 size
172     returns M lp coefficients and squared error E in place of
173     of coefficient 0 [E,c1,...,cM]
174 */
175 
csoundCepsLP(CSOUND * csound,MYFLT * b,MYFLT * c,int M,int N)176 MYFLT *csoundCepsLP(CSOUND *csound, MYFLT *b, MYFLT *c,
177                     int M, int N){
178   int n,m;
179   MYFLT s;
180   b[0]  = 1;
181   b[1] = -c[1];
182   for(m=2;m<M+1;m++) {
183     s = 0.;
184     for(n=1;n<m;n++)
185       s -= (m-n)*b[n]*c[m-n];
186     b[m] = -c[m] + s/m;
187   }
188   b[0] = EXP(c[0]);
189   return b;
190 }
191 
192 
193 /** Computes real cepstrum in place from a PVS frame
194     buf: non-negative spectrum in PVS_AMP_* format
195     size: size of buf (N + 2)
196     returns: real-valued cepstrum
197 */
csoundPvs2RealCepstrum(CSOUND * csound,MYFLT * buf,int size)198 MYFLT *csoundPvs2RealCepstrum(CSOUND *csound, MYFLT *buf, int size){
199   int i;
200   for(i = 0; i < size - 2; i+=2) {
201     buf[i] = LOG(buf[i]);
202     buf[i+1] = 0;
203   }
204   csoundInverseRealFFT(csound, buf, size - 2);
205   buf[size-2] = buf[size-1] = FL(0.0);
206   return buf;
207 }
208 
209 /** Computes magnitude spectrum of a PVS frame from
210     a real-valued cepstrum (in-place)
211     buf: real-valued cepstrum
212     size: size of buf (N)
213     returns: PVS_AMP_* frame
214 */
csoundRealCepstrum2Pvs(CSOUND * csound,MYFLT * buf,int size)215 MYFLT *csoundRealCepstrum2Pvs(CSOUND *csound, MYFLT *buf, int size){
216   int i;
217   csoundRealFFT(csound, buf, size);
218   for(i = 2; i < size; i+=2) {
219     buf[i] = EXP(buf[i]);
220     buf[i+1] = FL(0.0);
221   }
222   return buf;
223 }
224 
225 
pkpick(LPCparam * p)226 static void pkpick(LPCparam *p){
227   int n = 0, i, t1 = 0, t2 = 0;
228   MYFLT *r = p->r, *pk = p->pk;
229   for(int i = 1; i < p->N; i++) {
230     if (r[i] > r[i-1]) t1 = 1;
231     else t1 = 0;
232     if (r[i] >= r[i+1]) t2 = 1;
233     else t2 = 0;
234     if (t1 && t2) {
235       pk[n] = i;
236       n += 1;
237     }
238   }
239   for(i=n; i < p->N; i++) pk[i] = FL(-1.0);
240 }
241 
242 
243 
pkinterp(LPCparam * p)244 static void pkinterp(LPCparam *p){
245   int i, pn, N = p->N;
246   MYFLT tmp,y1,y2,a,b;
247   MYFLT *r = p->r, *pk = p->pk, *am = p->am;
248   for(i=0; i < N; i++) {
249     pn = (int) pk[i];
250     if(pn > 0) {
251       if(pn != 0) tmp = r[pn-1];
252       else tmp = r[pn+1];
253       y1 = r[pn] - tmp;
254       if(pn < N-1)
255         y2 = r[pn+1] - tmp;
256       else
257         y2 = r[pn] - tmp;
258       a = (y2-2*y1)/2;
259       b = 1-y1/a;
260       pk[i] = pn-1+b/2;
261       am[i] = tmp-a*b*b/4;
262     }
263     else break;
264   }
265 }
266 
267 /* autocorrelation CPS
268  */
csoundLPcps(CSOUND * csound,void * parm)269 MYFLT csoundLPcps(CSOUND *csound, void *parm){
270   LPCparam *p = (LPCparam *) parm;
271   int i;
272   MYFLT mx = FL(0.0), pmx, sr = csound->GetSr(csound);
273   MYFLT *pk = p->pk, *am = p->am;
274   pkpick(p);
275   pkinterp(p);
276   pmx = p->pk[0];
277   for(i=0;i<p->N;i++){
278     if(pk[i] < 0) break;
279     if(am[i] > mx) {
280       mx = am[i];
281       pmx = pk[i];
282     }
283   }
284   return (p->cps = sr/pmx);
285 }
286 
csoundLPrms(CSOUND * csound,void * parm)287 MYFLT csoundLPrms(CSOUND *csound, void *parm){
288   LPCparam *p = (LPCparam *) parm;
289   return p->rms;
290 }
291 
292 
csoundLPcoefs(CSOUND * csound,void * parm)293 MYFLT *csoundLPcoefs(CSOUND *csound, void *parm) {
294   LPCparam *p = (LPCparam *) parm;
295   return &(p->b[p->M*(p->M+1)]);
296 }
297 
298 
findzeros(int32_t M,MYFLT * a,MYCMPLX * zero,MYFLT * tmpbuf,int32_t itmax)299 static int32_t findzeros(int32_t M, MYFLT *a, MYCMPLX *zero,
300                          MYFLT *tmpbuf, int32_t itmax)
301 {
302   MYFLT        u, v, w, k, m, f, fm, fc, xm, ym, xr, yr, xc, yc;
303   MYFLT        dx, dy, term, factor, tmp;
304   int32_t      n1, i, j, p, iter, pt = 0;
305   unsigned char conv;;
306   factor = 1.0;
307   if (!a[0]) {
308     return 0;
309   }
310   memcpy(&tmpbuf[1], a, sizeof(M+1));
311   n1 = M;
312   while (n1 > 0) {
313     if (a[n1] == 0) {
314       zero[pt].re = 0, zero[pt].im = 0;
315       pt += 1;
316       n1  -= 1;
317     }
318     else {
319       p = n1-1;
320       xc = 0, yc = 0;
321       fc = a[n1]*a[n1];
322       fm = fc;
323       xm = 0.0, ym = 0.0;
324       dx = pow(fabs(a[M]/a[0]),1./n1);
325       dy = 0;
326       iter = 0;
327       conv = 0;
328       while (!conv) {
329         iter += 1;
330         if (iter>itmax) {
331           for (i=0; i<=M; i++)
332             a[i] = tmpbuf[i+1];
333           return pt;
334         }
335         for (i=1; i<=4; i++) {
336           u = -dy;
337           dy = dx;
338           dx = u;
339           xr = xc+dx, yr = yc+dy;
340           u = 0, v = 0;
341           k = 2*xr;
342           m = xr*xr+yr*yr;
343           for (j=0; j<=p; j++) {
344             w = a[j] + k*u - m*v;
345             v = u;
346             u = w;
347           }
348 
349           tmp = a[n1] + u*xr - m*v;
350           f = tmp*tmp + (u*u*yr*yr);
351           if (f<fm) {
352             xm = xr, ym = yr;
353             fm = f;
354           }
355         }
356         if (fm<fc) {
357           dx = 1.5*dx,dy = 1.5*dy;
358           xc = xm, yc = ym;
359           fc = fm;
360         }
361         else {
362           u = .4*dx - .3*dy;
363           dy = .4*dy + .3*dx;
364           dx = u;
365         }
366         u = fabs(xc) + fabs(yc);
367         term = u + (fabs(dx) + fabs(dy))*factor;
368         if ((u==term)||(fc==0))
369           conv = 1;
370       }
371       u = 0.0, v = 0.0;
372       k = 2.0*xc;
373       m = xc*xc;
374       for (j=0; j<=p; j++) {
375         w = a[j] + k*u - m*v;
376         v = u;
377         u = w;
378       }
379       tmp = a[n1] + u*xc - m*v;
380       if (tmp*tmp<=fc) {
381         u = 0.0;
382         for (j=0; j<=p; j++) {
383           a[j] = u*xc + a[j];
384           u = a[j];
385         }
386         zero[pt].re = xc, zero[pt].im = 0;
387         pt += 1;
388       }
389       else {
390         u = 0, v = 0;
391         k = 2*xc;
392         m = xc*xc + yc*yc;
393         p = n1-2;
394         for (j=0; j<=p; j++) {
395           a[j] += k*u-m*v;
396           w = a[j];
397           v = u;
398           u = w;
399         }
400         zero[pt].re = xc, zero[pt].im = yc;
401         pt += 1;
402         zero[pt].re = xc, zero[pt].im = -yc;
403         pt += 1;
404       }
405       n1 = p;
406     }
407   }
408   for (i=0; i<=M; i++)
409     a[i] = tmpbuf[i+1];
410 
411   return pt;
412 }
413 
414 
invertfilter(int32_t M,MYCMPLX * zr)415 static MYCMPLX *invertfilter(int32_t M, MYCMPLX *zr)
416 {
417   int32_t    i;
418   MYFLT pr,pi,pow;
419 
420   for (i=0; i < M; i++) {
421     pr = zr[i].re;
422     pi = zr[i].im;
423     pow = pr*pr+pi*pi;
424     zr[i].re = pr/pow;
425     zr[i].im = -pi/pow;
426   }
427   return zr;
428 }
429 
zero2coef(int32_t M,MYCMPLX * zr,MYFLT * c,MYFLT * tmp)430 static MYFLT *zero2coef(int32_t M, MYCMPLX *zr, MYFLT *c, MYFLT *tmp)
431 {
432   int32_t  j, k;
433   MYFLT  pr, pi, cr, ci;
434   c[0] = 1;
435   tmp[0] = 0;
436   for (j=0; j < M; j++) {
437     c[j+1] = 1;
438     tmp[j+1] = 0;
439     pr = zr[j].re;
440     pi = zr[j].im;
441     for (k=j; k>=0; k--) {
442       cr = c[k];
443       ci = tmp[k];
444       c[k] = -(cr*pr-ci*pi);
445       tmp[k] = -(ci*pr+cr*pi);
446       if (k>0) {
447         c[k] += c[k-1];
448         tmp[k] += tmp[k-1];
449       }
450     }
451   }
452   pr = c[0];
453   for (j=0; j <= M; j++) c[j] = c[j]/pr;
454   return c;
455 }
456 
457 #define MAX_ITER 2000
csoundCoef2Pole(CSOUND * csound,void * parm,MYFLT * c)458 MYCMPLX *csoundCoef2Pole(CSOUND *csound, void *parm, MYFLT *c){
459   LPCparam *p = (LPCparam *) parm;
460   MYCMPLX *pl = p->pl;
461   MYFLT *buf = p->tmpmem, *cf = p->cf;
462   int32_t i, j, M = p->M;
463   cf[M] = 1.0;
464   for (i=0; i< (M+1)/2; i++) {
465     j = M-1-i;
466     cf[i] = c[j];
467     cf[j] = c[i];
468   }
469   findzeros(M, cf, pl, buf, MAX_ITER);
470   invertfilter(M, pl);
471   return pl;
472 }
473 
csoundPole2Coef(CSOUND * csound,void * parm,MYCMPLX * pl)474 MYFLT *csoundPole2Coef(CSOUND *csound, void *parm, MYCMPLX *pl) {
475   LPCparam *p = (LPCparam *) parm;
476   pl = invertfilter(p->M, pl);
477   return zero2coef(p->M, pl, p->cf, p->tmpmem);
478 }
479 
csoundStabiliseAllpole(CSOUND * csound,void * parm,MYFLT * c,int mode)480 MYFLT *csoundStabiliseAllpole(CSOUND *csound, void *parm, MYFLT *c, int mode){
481   if (mode) {
482     LPCparam *p = (LPCparam *) parm;
483     MYCMPLX *pl;
484     MYFLT pm, pf;
485     int32_t i, M = p->M;
486 
487     pl = csoundCoef2Pole(csound,parm,c);
488     for(i=0; i < M; i++) {
489       pm = magc(pl[i]);
490       if(pm >= 1.){
491         if (mode == 1) {
492           pf = phsc(pl[i]);
493           pm = 1/pm;
494           pl[i].re  = pm*COS(pf);
495           pl[i].im  = pm*SIN(pf);
496         } else {
497           pl[i].re /= pm;
498           pl[i].im /= pm;
499         }
500       }
501     }
502     return csoundPole2Coef(csound,parm,pl);
503   }
504   else return c;
505 }
506 
507 /* opcodes */
508 /* lpcfilter - take lpred input from table */
lpfil_init(CSOUND * csound,LPCFIL * p)509 int32_t lpfil_init(CSOUND *csound, LPCFIL *p) {
510 
511   FUNC *ft = csound->FTnp2Find(csound, p->ifn);
512 
513   if (ft != NULL) {
514     MYFLT *c;
515     int N = *p->isiz < ft->flen ? *p->isiz : ft->flen;
516     uint32_t Nbytes = N*sizeof(MYFLT);
517     uint32_t Mbytes = *p->iord*sizeof(MYFLT);
518     p->M = *p->iord;
519     p->N = N;
520 
521     p->setup = csound->LPsetup(csound,N,p->M);
522     if(*p->iwin != 0) {
523       FUNC *ftw = csound->FTnp2Find(csound, p->iwin);
524       MYFLT *buf, incr, k;
525       int i;
526       p->wlen = ftw->flen;
527       p->win = ftw->ftable;
528       if(p->buf.auxp == NULL || Nbytes > p->buf.size)
529         csound->AuxAlloc(csound, Nbytes, &p->buf);
530       buf = (MYFLT*) p->buf.auxp;
531       incr = p->wlen/N;
532       for(i=0, k=0; i < N; i++, k+=incr)
533         buf[i] = ft->ftable[i]*p->win[(int)k];
534       c = csound->LPred(csound,p->setup,buf);
535     }
536     else {
537       p->win = NULL;
538       c = csound->LPred(csound,p->setup,ft->ftable);
539     }
540 
541     if(p->coefs.auxp == NULL || Mbytes > p->coefs.size)
542       csound->AuxAlloc(csound, Mbytes, &p->coefs);
543     memcpy(p->coefs.auxp, &c[1], Mbytes);
544 
545     /* keep filter data as doubles */
546     Mbytes *= sizeof(double)/sizeof(MYFLT);
547     if(p->del.auxp == NULL || Mbytes > p->del.size)
548       csound->AuxAlloc(csound, Mbytes, &p->del);
549     memset(p->del.auxp, 0, Mbytes);
550 
551     p->rp = 0;
552     p->ft = ft;
553     p->g = csoundLPrms(csound,p->setup)*SQRT(c[0]);
554     return OK;
555   }
556   csound->InitError(csound, Str("function table %d not found\n"), (int) *p->ifn);
557   return NOTOK;
558 }
559 
560 
lpfil_perf(CSOUND * csound,LPCFIL * p)561 int32_t lpfil_perf(CSOUND *csound, LPCFIL *p) {
562   MYFLT *cfs = (MYFLT *) p->coefs.auxp;
563   double *yn = (double *) p->del.auxp, y;
564   MYFLT *out = p->out;
565   MYFLT *in = p->in;
566   MYFLT g = p->g;
567   int32_t M = p->M, m;
568   int32_t pp, rp = p->rp;
569   uint32_t offset = p->h.insdshead->ksmps_offset;
570   uint32_t early  = p->h.insdshead->ksmps_no_end;
571   uint32_t n, nsmps = CS_KSMPS;
572 
573   if (UNLIKELY(offset)) {
574     memset(out, '\0', offset*sizeof(MYFLT));
575   }
576   if (UNLIKELY(early)) {
577     nsmps -= early;
578     memset(&out[nsmps], '\0', early*sizeof(MYFLT));
579   }
580 
581   if(*p->kflag) {
582     MYFLT *c;
583     int32_t off = *p->koff;
584     int32_t len = p->ft->flen;
585     if (off + p->N > len)
586       off = len - p->N;
587     if(p->win) {
588       MYFLT *buf, incr, k;
589       int i, N = p->N;
590       buf = (MYFLT*) p->buf.auxp;
591       incr = p->wlen/N;
592       for(i=0, k=0; i < N; i++, k+=incr)
593         buf[i] = p->ft->ftable[i+off]*p->win[(int)k];
594       c = csound->LPred(csound,p->setup,buf);
595     } else
596       c = csound->LPred(csound,p->setup,
597                         p->ft->ftable+off);
598     memcpy(p->coefs.auxp, &c[1], M*sizeof(MYFLT));
599     g = p->g = csoundLPrms(csound,p->setup)*SQRT(c[0]);
600   }
601 
602   for(n=offset; n < nsmps; n++) {
603     pp = rp;
604     y =  (double) in[n]*g; /* need to scale input */
605     for(m = 0; m < M; m++) {
606       // filter convolution
607       y -= cfs[M - m - 1]*yn[pp];
608       pp = pp != M - 1 ? pp + 1: 0;
609     }
610     out[n] = (MYFLT) (yn[rp] = y);
611     rp = rp != M - 1 ? rp + 1: 0;
612   }
613   p->rp = rp;
614   return OK;
615 }
616 
617 
618 /* lpcfilter - take lpred input from sig */
lpfil2_init(CSOUND * csound,LPCFIL2 * p)619 int32_t lpfil2_init(CSOUND *csound, LPCFIL2 *p) {
620   uint32_t Nbytes = *p->isiz*sizeof(MYFLT);
621   uint32_t Mbytes = *p->iord*sizeof(MYFLT);
622   p->M = *p->iord;
623   p->N = *p->isiz;
624 
625   if(*p->iwin != 0) {
626     FUNC *ftw = csound->FTnp2Find(csound, p->iwin);
627     p->wlen = ftw->flen;
628     p->win = ftw->ftable;
629   } else p->win = NULL;
630 
631   p->setup = csound->LPsetup(csound,p->N,p->M);
632 
633   if(p->cbuf.auxp == NULL || Nbytes > p->cbuf.size)
634     csound->AuxAlloc(csound, Nbytes, &p->cbuf);
635   if(p->buf.auxp == NULL || Nbytes > p->buf.size)
636     csound->AuxAlloc(csound, Nbytes, &p->buf);
637 
638   /* keep filter data as doubles */
639   Mbytes *= sizeof(double)/sizeof(MYFLT);
640   if(p->coefs.auxp == NULL || Mbytes > p->coefs.size)
641     csound->AuxAlloc(csound, Mbytes, &p->coefs);
642 
643 
644   if(p->del.auxp == NULL || Mbytes > p->del.size)
645     csound->AuxAlloc(csound, Mbytes, &p->del);
646   memset(p->del.auxp, 0, Mbytes);
647   p->cp = 1;
648   p->rp = p->bp = 0;
649   return OK;
650 }
651 
lpfil2_perf(CSOUND * csound,LPCFIL2 * p)652 int32_t lpfil2_perf(CSOUND *csound, LPCFIL2 *p) {
653   MYFLT *cfs = (MYFLT *) p->coefs.auxp;
654   MYFLT *buf = (MYFLT *) p->buf.auxp;
655   MYFLT *cbuf = (MYFLT *) p->cbuf.auxp;
656   double *yn = (double *) p->del.auxp, y;
657   MYFLT *out = p->out;
658   MYFLT *in = p->in;
659   MYFLT *sig = p->sig;
660   MYFLT g = p->g;
661   int32_t M = p->M, m, flag = (int32_t) *p->flag;
662   int32_t N = p->N;
663   int32_t pp, rp = p->rp, bp = p->bp, cp = p->cp;
664   uint32_t offset = p->h.insdshead->ksmps_offset;
665   uint32_t early  = p->h.insdshead->ksmps_no_end;
666   uint32_t n, nsmps = CS_KSMPS;
667 
668   if (UNLIKELY(offset)) {
669     memset(out, '\0', offset*sizeof(MYFLT));
670   }
671   if (UNLIKELY(early)) {
672     nsmps -= early;
673     memset(&out[nsmps], '\0', early*sizeof(MYFLT));
674   }
675 
676   for(n=offset; n < nsmps; n++) {
677     cbuf[bp] = sig[n];
678     bp = bp != N - 1 ? bp + 1 : 0;
679     if(--cp == 0 && flag) {
680       MYFLT *c, k, incr = p->wlen/N;
681       int32_t j,i;
682       for (j=bp,i=0, k=0; i < N; j++,i++,k+=incr) {
683         buf[i] = p->win == NULL ? cbuf[j%N] : p->win[(int)k]*cbuf[j%N];
684       }
685       c = csound->LPred(csound,p->setup,buf);
686       memcpy(p->coefs.auxp, &c[1], M*sizeof(MYFLT));
687       g = p->g = csoundLPrms(csound,p->setup)*SQRT(c[0]);
688       cp = (int32_t) (*p->prd > 1 ? *p->prd : 1);
689     }
690     pp = rp;
691     y =  (double) in[n]*g; /* need to scale input */
692     for(m = 0; m < M; m++) {
693       // filter convolution
694       y -= cfs[M - m - 1]*yn[pp];
695       pp = pp != M - 1 ? pp + 1: 0;
696     }
697     out[n] = (MYFLT) (yn[rp] = y);
698     rp = rp != M - 1 ? rp + 1: 0;
699   }
700   p->rp = rp;
701   p->bp = bp;
702   p->cp = cp;
703   return OK;
704 }
705 
706 /* linear prediction analysis
707  */
708 #include "arrays.h"
709 
710 /* function table input */
lpred_alloc(CSOUND * csound,LPREDA * p)711 int32_t lpred_alloc(CSOUND *csound, LPREDA *p) {
712   FUNC *ft = csound->FTnp2Find(csound, p->ifn);
713   if (ft != NULL) {
714     int N = *p->isiz < ft->flen ? *p->isiz : ft->flen;
715     uint32_t Nbytes = N*sizeof(MYFLT);
716     if(*p->iwin){
717       FUNC *win = csound->FTnp2Find(csound, p->iwin);
718       p->win = win->ftable;
719       p->wlen = win->flen;
720     } else p->win = NULL;
721     p->M = *p->iord;
722     p->N = N;
723     p->setup = csound->LPsetup(csound,N,p->M);
724     if(p->buf.auxp == NULL || Nbytes > p->buf.size)
725       csound->AuxAlloc(csound, Nbytes, &p->buf);
726     tabinit(csound,p->out,p->M);
727     p->ft = ft;
728     return OK;
729   }
730   else
731     csound->InitError(csound, Str("function table %d not found\n"), (int) *p->ifn);
732   return NOTOK;
733 }
734 
lpred_run(CSOUND * csound,LPREDA * p)735 int32_t lpred_run(CSOUND *csound, LPREDA *p) {
736   MYFLT *c;
737   if (*p->flag) {
738     int N = p->N;
739     MYFLT k, incr = p->wlen/N, *ft = p->ft->ftable;
740     MYFLT *buf = (MYFLT *) p->buf.auxp;
741     int32_t off = *p->off;
742     int32_t len = p->ft->flen;
743     int32_t i;
744     if (off + p->N > len)
745       off = len - p->N;
746     p->M = *p->iord;
747     p->N = N;
748     for (i=0, k=0; i < N; i++,k+=incr) {
749       buf[i] = p->win == NULL ? ft[i+off] : p->win[(int)k]*ft[i+off];
750     }
751     c = csound->LPred(csound,p->setup, buf);
752   }
753   c = csoundLPcoefs(csound,p->setup);
754   memcpy(p->out->data, &c[1], sizeof(MYFLT)*p->M);
755   *p->err = SQRT(c[0]);
756   *p->rms = csoundLPrms(csound,p->setup);
757   *p->cps = csoundLPcps(csound,p->setup);
758   return OK;
759 }
760 
761 /* i-time version */
lpred_i(CSOUND * csound,LPREDA * p)762 int32_t lpred_i(CSOUND *csound, LPREDA *p) {
763   if(lpred_alloc(csound,p) == OK)
764     return lpred_run(csound,p);
765   else return NOTOK;
766 }
767 
768 /* audio signal input */
lpred_alloc2(CSOUND * csound,LPREDA2 * p)769 int32_t lpred_alloc2(CSOUND *csound, LPREDA2 *p) {
770   int N = *p->isiz;
771   uint32_t Nbytes = N*sizeof(MYFLT);
772   if(*p->iwin){
773     FUNC *win = csound->FTnp2Find(csound, p->iwin);
774     p->win = win->ftable;
775     p->wlen = win->flen;
776   } else p->win = NULL;
777   p->M = *p->iord;
778   p->N = N;
779   p->setup = csound->LPsetup(csound,N,p->M);
780   if(p->buf.auxp == NULL || Nbytes > p->buf.size)
781     csound->AuxAlloc(csound, Nbytes, &p->buf);
782   if(p->cbuf.auxp == NULL || Nbytes > p->cbuf.size)
783     csound->AuxAlloc(csound, Nbytes, &p->cbuf);
784   tabinit(csound,p->out,p->M);
785   p->cp = 1;
786   p->bp = 0;
787   return OK;
788 }
789 
790 
791 
lpred_run2(CSOUND * csound,LPREDA2 * p)792 int32_t lpred_run2(CSOUND *csound, LPREDA2 *p) {
793   MYFLT *buf = (MYFLT *) p->buf.auxp;
794   MYFLT *cbuf = (MYFLT *) p->cbuf.auxp;
795   MYFLT *in = p->in;
796   int32_t M = p->M, flag = (int32_t) *p->flag;
797   int32_t N = p->N;
798   int32_t bp = p->bp, cp = p->cp;
799   uint32_t offset = p->h.insdshead->ksmps_offset;
800   uint32_t early  = p->h.insdshead->ksmps_no_end;
801   uint32_t n, nsmps = CS_KSMPS;
802   MYFLT *c;
803 
804   if (UNLIKELY(early))
805     nsmps -= early;
806 
807   for(n=offset; n < nsmps; n++) {
808     cbuf[bp] = in[n];
809     bp = bp != N - 1 ? bp + 1 : 0;
810     if(--cp == 0 && flag) {
811       MYFLT k, incr = p->wlen/N;
812       int32_t j,i;
813       for (j=bp,i=0, k=0; i < N; j++,i++,k+=incr) {
814         buf[i] = p->win == NULL ? cbuf[j%N] : p->win[(int)k]*cbuf[j%N];
815       }
816       c = csound->LPred(csound,p->setup,buf);
817       cp = (int32_t) (*p->prd > 1 ? *p->prd : 1);
818     }
819   }
820   c = csoundLPcoefs(csound,p->setup);
821   memcpy(p->out->data, &c[1], sizeof(MYFLT)*M);
822   *p->err = SQRT(c[0]);
823   *p->rms = csoundLPrms(csound,p->setup);
824   *p->cps = csoundLPcps(csound,p->setup);
825   p->bp = bp;
826   p->cp = cp;
827   return OK;
828 }
829 
830 /* allpole - take lpred input from array */
lpfil3_init(CSOUND * csound,LPCFIL3 * p)831 int32_t lpfil3_init(CSOUND *csound, LPCFIL3 *p) {
832   p->M = p->coefs->sizes[0];
833   uint32_t  Mbytes = p->M*sizeof(double);
834   if(p->del.auxp == NULL || Mbytes > p->del.size)
835     csound->AuxAlloc(csound, Mbytes, &p->del);
836   memset(p->del.auxp, 0, Mbytes);
837   p->rp = 0;
838   return OK;
839 }
840 
841 
lpfil3_perf(CSOUND * csound,LPCFIL3 * p)842 int32_t lpfil3_perf(CSOUND *csound, LPCFIL3 *p) {
843   MYFLT *cfs = (MYFLT *) p->coefs->data;
844   double *yn = (double *) p->del.auxp, y;
845   MYFLT *out = p->out;
846   MYFLT *in = p->in;
847   int32_t M = p->M, m;
848   int32_t pp, rp = p->rp;
849   uint32_t offset = p->h.insdshead->ksmps_offset;
850   uint32_t early  = p->h.insdshead->ksmps_no_end;
851   uint32_t n, nsmps = CS_KSMPS;
852 
853   if (UNLIKELY(offset)) {
854     memset(out, '\0', offset*sizeof(MYFLT));
855   }
856   if (UNLIKELY(early)) {
857     nsmps -= early;
858     memset(&out[nsmps], '\0', early*sizeof(MYFLT));
859   }
860 
861   for(n=offset; n < nsmps; n++) {
862     pp = rp;
863     y = (double) in[n];
864     for(m = 0; m < M; m++) {
865       // filter convolution
866       y -= cfs[M - m - 1]*yn[pp];
867       pp = pp != M - 1 ? pp + 1: 0;
868     }
869     out[n] = (MYFLT) (yn[rp] = y);
870     rp = rp != M - 1 ? rp + 1: 0;
871   }
872   p->rp = rp;
873   return OK;
874 }
875 
876 /* pvs <-> lpc */
877 
lpcpvs_init(CSOUND * csound,LPCPVS * p)878 int32_t lpcpvs_init(CSOUND *csound, LPCPVS *p) {
879   int N = *p->isiz;
880   uint32_t Nbytes = N*sizeof(MYFLT);
881   if(*p->iwin){
882     FUNC *win = csound->FTnp2Find(csound, p->iwin);
883     p->win = win->ftable;
884     p->wlen = win->flen;
885   } else p->win = NULL;
886   p->M = *p->iord;
887   p->N = N;
888 
889   if((N & (N - 1)) != 0)
890     return csound->InitError(csound, "input size not power of two\n");
891 
892   p->setup = csound->LPsetup(csound,N,p->M);
893   if(p->buf.auxp == NULL || Nbytes > p->buf.size)
894     csound->AuxAlloc(csound, Nbytes, &p->buf);
895   if(p->cbuf.auxp == NULL || Nbytes > p->cbuf.size)
896     csound->AuxAlloc(csound, Nbytes, &p->cbuf);
897   if(p->fftframe.auxp == NULL || Nbytes > p->fftframe.size)
898     csound->AuxAlloc(csound, Nbytes, &p->fftframe);
899 
900   p->fout->N = N;
901   p->fout->sliding = 0;
902   p->fout->NB =  0;
903   p->fout->overlap = (int32_t) *p->prd;
904   p->fout->winsize = N;
905   p->fout->wintype = PVS_WIN_HANN;
906   p->fout->format = PVS_AMP_FREQ;
907 
908   Nbytes = (N+2)*sizeof(float);
909   if(p->fout->frame.auxp == NULL || Nbytes > p->fout->frame.size)
910     csound->AuxAlloc(csound, Nbytes, &p->fout->frame);
911 
912   p->cp = 1;
913   p->bp = 0;
914   return OK;
915 }
916 
lpcpvs(CSOUND * csound,LPCPVS * p)917 int32_t lpcpvs(CSOUND *csound, LPCPVS *p){
918   MYFLT *buf = (MYFLT *) p->buf.auxp;
919   MYFLT *cbuf = (MYFLT *) p->cbuf.auxp;
920   MYFLT *in = p->in;
921   int32_t M = p->M;
922   int32_t N = p->N;
923   int32_t bp = p->bp, cp = p->cp;
924   uint32_t offset = p->h.insdshead->ksmps_offset;
925   uint32_t early  = p->h.insdshead->ksmps_no_end;
926   uint32_t n, nsmps = CS_KSMPS;
927   MYFLT *c;
928 
929   if (UNLIKELY(early))
930     nsmps -= early;
931 
932   for(n=offset; n < nsmps; n++) {
933     cbuf[bp] = in[n];
934     bp = bp != N - 1 ? bp + 1 : 0;
935     if(--cp == 0) {
936       MYFLT k, incr = p->wlen/N, g, sr = csound->GetSr(csound);
937       MYFLT *fftframe =  (MYFLT *) p->fftframe.auxp;
938       float *pvframe = (float *)p->fout->frame.auxp;
939       int32_t j,i;
940       for (j=bp,i=0, k=0; i < N; j++,i++,k+=incr) {
941         buf[i] = p->win == NULL ? cbuf[j%N] : p->win[(int)k]*cbuf[j%N];
942       }
943       c = csound->LPred(csound,p->setup,buf);
944       memset(fftframe,0,sizeof(MYFLT)*N);
945       memcpy(&fftframe[1], &c[1], sizeof(MYFLT)*M);
946       fftframe[0] = 1;
947       csound->RealFFT(csound,fftframe,N);
948       g =  SQRT(c[0])*csoundLPrms(csound,p->setup);
949       MYFLT cps = csoundLPcps(csound,p->setup);
950       int cpsbin = cps*N/sr;
951       for(i=0; i < N+2; i+=2) {
952         MYFLT a = 0., inv;
953         int bin = i/2;
954         if(i > 0 && i < N)
955           inv = sqrt(fftframe[i]*fftframe[i] + fftframe[i+1]*fftframe[i+1]);
956         else if(i == N) inv = fftframe[1];
957         else inv = fftframe[0];
958         if(inv > 0)
959           a = g/inv;
960         else a = g;
961         pvframe[i] = (float) a;
962         if(bin/cpsbin)
963           pvframe[i+1] = cps*bin/cpsbin;
964         else if ((bin+1)/cpsbin)
965           pvframe[i+1] = cps*(bin-1)/cpsbin;
966         else if ((bin-1)/cpsbin)
967           pvframe[i+1] = cps*(bin+1)/cpsbin;
968 
969       }
970       p->fout->framecount += 1;
971       cp = (int32_t) (*p->prd > 1 ? *p->prd : 1);
972     }
973   }
974   p->bp = bp;
975   p->cp = cp;
976   return OK;
977 }
978 
pvscoefs_init(CSOUND * csound,PVSCFS * p)979 int32_t pvscoefs_init(CSOUND *csound, PVSCFS *p) {
980   unsigned int Nbytes = (p->fin->N+2)*sizeof(MYFLT);
981   unsigned int Mbytes = (p->M+1)*sizeof(MYFLT);
982   p->N = p->fin->N;
983   p->M = *p->iord;
984   p->setup = csound->LPsetup(csound,0,p->M);
985   if(p->buf.auxp == NULL || Nbytes > p->buf.size)
986     csound->AuxAlloc(csound, Nbytes, &p->buf);
987   if(p->coef.auxp == NULL || Mbytes > p->coef.size)
988     csound->AuxAlloc(csound, Nbytes, &p->coef);
989   tabinit(csound,p->out,p->M);
990   p->mod = *p->imod;
991   return OK;
992 }
993 
pvscoefs(CSOUND * csound,PVSCFS * p)994 int pvscoefs(CSOUND *csound, PVSCFS *p){
995   MYFLT *c = (MYFLT *) p->coef.auxp;
996   if(p->fin->framecount > p->framecount){
997     MYFLT *buf = (MYFLT *) p->buf.auxp;
998     int32_t i;
999     MYFLT pow = 0;
1000     float *pvframe = (float *) p->fin->frame.auxp;
1001     memset(buf,0,sizeof(MYFLT)*(p->N+2));
1002     for(i=2; i < p->N; i+=2) buf[i] = pvframe[i];
1003     buf[0] = pvframe[0];
1004     buf[p->N] = pvframe[p->N];
1005     for(i=0; i < p->N+2; i+=2) pow += buf[i];
1006     p->rms = pow/(2*sqrt(2));
1007     if(p->rms > 0) {
1008       memset(c,0,sizeof(MYFLT)*(p->M+1));
1009       csoundPvs2RealCepstrum(csound, buf, p->N+2);
1010       csoundCepsLP(csound,c,buf,p->M,p->N);
1011       p->err = sqrt(c[0]);
1012       c = csoundStabiliseAllpole(csound,p->setup,c,p->mod);
1013       memcpy(p->out->data,&c[1],p->M*sizeof(MYFLT));
1014     }
1015     p->framecount = p->fin->framecount;
1016   }
1017   *p->kerr = p->err;
1018   *p->krms = p->rms;
1019   return OK;
1020 }
1021 
1022 /* coefficients to filter CF/BW */
coef2parm_init(CSOUND * csound,CF2P * p)1023 int32_t coef2parm_init(CSOUND *csound, CF2P *p) {
1024   p->M = p->in->sizes[0];
1025   p->setup = csound->LPsetup(csound,0,p->M);
1026   tabinit(csound,p->out,p->M);
1027   p->sum = 0.0;
1028   return OK;
1029 }
1030 
cmpfunc(const void * a,const void * b)1031 static int cmpfunc (const void * a, const void * b) {
1032   MYFLT v1 = (phsc(*((MYCMPLX *) a)));
1033   MYFLT v2 = (phsc(*((MYCMPLX *) b)));
1034   return (int)((v1 - v2)*100000);
1035 }
1036 
coef2parm(CSOUND * csound,CF2P * p)1037 int32_t coef2parm(CSOUND *csound, CF2P *p) {
1038   MYCMPLX *pl;
1039   MYFLT *c = p->in->data, pm, pf, sum = 0.0;
1040   MYFLT *pp = p->out->data, Nyq = csound->esr/2;
1041   int i,j;
1042   // simple check for new data
1043   for(i=0; i< p->M; i++) sum += c[i];
1044   if (sum != p->sum) {
1045     pl = csoundCoef2Pole(csound,p->setup,c);
1046     qsort(pl,p->M,sizeof(MYCMPLX),cmpfunc);
1047     memset(pp,0,sizeof(MYFLT)*p->M);
1048     for(i = j = 0; i < p->M; i++) {
1049       pm = magc(pl[i]);
1050       pf = phsc(pl[i])/csound->tpidsr;
1051       if(isnan(pf)) {
1052         pp[j] = 0;
1053         pp[j+1] = 0;
1054       }
1055       else {
1056         if(pf > 0 && pf < Nyq && j < p->M) {
1057           pp[j] = pf;
1058           pp[j+1] = -LOG(pm)*2/csound->tpidsr;
1059           j += 2;
1060         }
1061       }
1062     }
1063   }
1064   p->sum = sum;
1065   return OK;
1066 }
1067 
1068 
1069 /* resonator bank */
resonbnk_init(CSOUND * csound,RESONB * p)1070 int32_t resonbnk_init(CSOUND *csound, RESONB *p)
1071 {
1072   int32_t scale, siz;
1073   p->scale = scale = (int32_t) *p->iscl;
1074   p->ord =  p->kparm->sizes[0];
1075   siz = (p->ord+1)/2;
1076   if (!*p->istor && (p->y1m.auxp == NULL ||
1077                      (uint32_t)(siz*sizeof(double)) > p->y1m.size))
1078     csound->AuxAlloc(csound, (int32_t)(siz*sizeof(double)), &p->y1m);
1079   if (!*p->istor && (p->y2m.auxp == NULL ||
1080                      (uint32_t)(siz*sizeof(double)) > p->y2m.size))
1081     csound->AuxAlloc(csound, (int32_t)(siz*sizeof(double)), &p->y2m);
1082 
1083   if (!*p->istor && (p->y1o.auxp == NULL ||
1084                      (uint32_t)(siz*sizeof(double)) > p->y1o.size))
1085     csound->AuxAlloc(csound, (int32_t)(siz*sizeof(double)), &p->y1o);
1086   if (!*p->istor && (p->y2o.auxp == NULL ||
1087                      (uint32_t)(siz*sizeof(double)) > p->y2o.size))
1088     csound->AuxAlloc(csound, (int32_t)(siz*sizeof(double)), &p->y2o);
1089 
1090   if (!*p->istor && (p->y1c.auxp == NULL ||
1091                      (uint32_t)(siz*sizeof(double)) > p->y1c.size))
1092     csound->AuxAlloc(csound, (int32_t)(siz*sizeof(double)), &p->y1c);
1093   if (!*p->istor && (p->y2c.auxp == NULL ||
1094                      (uint32_t)(siz*sizeof(double)) > p->y2c.size))
1095     csound->AuxAlloc(csound, (int32_t)(siz*sizeof(double)), &p->y2c);
1096 
1097   if (UNLIKELY(scale && scale != 1 && scale != 2)) {
1098     return csound->InitError(csound, Str("illegal reson iscl value, %f"),
1099                              *p->iscl);
1100   }
1101   if (!(*p->istor)) {
1102     memset(p->y1m.auxp, 0, siz*sizeof(double));
1103     memset(p->y2m.auxp, 0, siz*sizeof(double));
1104     memset(p->y1o.auxp, 0, siz*sizeof(double));
1105     memset(p->y2o.auxp, 0, siz*sizeof(double));
1106     memset(p->y1c.auxp, 0, siz*sizeof(double));
1107     memset(p->y2c.auxp, 0, siz*sizeof(double));
1108   }
1109   p->kcnt = 0;
1110   return OK;
1111 }
1112 
resonbnk(CSOUND * csound,RESONB * p)1113 int32_t resonbnk(CSOUND *csound, RESONB *p)
1114 {
1115   uint32_t    offset = p->h.insdshead->ksmps_offset;
1116   uint32_t    early  = p->h.insdshead->ksmps_no_end;
1117   uint32_t    n, nsmps = CS_KSMPS;
1118   int32_t     j, k, ord = p->ord, mod = *p->imod;
1119   MYFLT       *ar,*asig;
1120   double      c3p1, c3t4, omc3, c2sqr, cosf,cc2,cc3;
1121   double      *yt1, *yt2, c1 = 1.,*c2,*c3, x, *c2o, *c3o;
1122   MYFLT bw, cf;
1123   MYFLT kcnt = p->kcnt, prd = *p->iprd, interp, fmin = *p->kmin, fmax = *p->kmax;
1124 
1125 
1126   ar   = p->ar;
1127   asig = p->asig;
1128   yt1  = (double*) p->y1m.auxp;
1129   yt2  = (double*) p->y2m.auxp;
1130   c2o  = (double*) p->y1o.auxp;
1131   c3o  = (double*) p->y2o.auxp;
1132   c2  = (double*) p->y1c.auxp;
1133   c3  = (double*) p->y2c.auxp;
1134 
1135   if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
1136   if (UNLIKELY(early)) {
1137     nsmps -= early;
1138     memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
1139   }
1140 
1141 
1142   for (n=offset; n<nsmps; n++) {
1143     x = asig[n];
1144     ar[n] = 0.;
1145     if(kcnt == prd) kcnt = 0;
1146     interp = kcnt/prd;
1147     for (k=j=0; k < ord; j++,k+=2) {
1148       if(mod) x = asig[n]; // parallel
1149       if(kcnt == 0) {
1150         c3o[j] = c3[j]; c2o[j] = c2[j];
1151         cf = p->kparm->data[k];
1152         bw = p->kparm->data[k+1];
1153         if(cf > fmin && cf < fmax) {
1154           cosf = cos(cf * (double)(csound->tpidsr));
1155           c3[j] = exp(bw * (double)(csound->mtpdsr));
1156           c3p1 = c3[j] + 1.0;
1157           c3t4 = c3[j] * 4.0;
1158           c2[j] = c3t4 * cosf / c3p1;
1159         }
1160       }
1161       cc2 = c2o[j] + (c2[j] - c2o[j])*interp;
1162       cc3 = c3o[j] + (c3[j] - c3o[j])*interp;
1163       if(p->scale) {
1164         omc3 = 1.0 - cc3;
1165         c2sqr = cc2*cc2;
1166         c3p1 = cc3 + 1.0;
1167         if (p->scale == 1)
1168           c1 = omc3 * sqrt(1.0 - (c2sqr / (4*cc3)));
1169         else if (p->scale == 2)
1170           c1 = sqrt((c3p1*c3p1-c2sqr) * omc3/c3p1);
1171       }
1172       x = c1 * x + cc2 * yt1[j] - cc3 * yt2[j];
1173       yt2[j] = yt1[j];
1174       yt1[j] = x;
1175       if(mod) ar[n] += x; // parallel
1176     }
1177     kcnt += 1;
1178     if(!mod) ar[n] = x;
1179   }
1180   p->kcnt = kcnt;
1181   return OK;
1182 }
1183