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