1 /*
2     ugens5.c:
3 
4     Copyright (C) 1991 Barry Vercoe, John ffitch, Gabriel Maldonado
5 
6     This file is part of Csound.
7 
8     The Csound Library is free software; you can redistribute it
9     and/or modify it under the terms of the GNU Lesser General Public
10     License as published by the Free Software Foundation; either
11     version 2.1 of the License, or (at your option) any later version.
12 
13     Csound is distributed in the hope that it will be useful,
14     but WITHOUT ANY WARRANTY; without even the implied warranty of
15     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16     GNU Lesser General Public License for more details.
17 
18     You should have received a copy of the GNU Lesser General Public
19     License along with Csound; if not, write to the Free Software
20     Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
21     02110-1301 USA
22 */
23 
24 #include "csoundCore.h"         /*                      UGENS5.C        */
25 #include "ugens5.h"
26 #include <math.h>
27 #include <inttypes.h>
28 
29 /*
30  * LPC storage slots
31  */
32 
33 #define MAX_LPC_SLOT 20
34 
35 
porset(CSOUND * csound,PORT * p)36 int32_t porset(CSOUND *csound, PORT *p)
37 {
38    IGN(csound);
39     p->c2 = pow(0.5, (double)CS_ONEDKR / *p->ihtim);
40     p->c1 = 1.0 - p->c2;
41     if (LIKELY(*p->isig >= FL(0.0)))
42       p->yt1 = (double)(*p->isig);
43     p->ihtim_old = *p->ihtim;
44     return OK;
45 }
kporset(CSOUND * csound,PORT * p)46 int32_t kporset(CSOUND *csound, PORT *p) { return porset(csound, p); }
47 
port(CSOUND * csound,PORT * p)48 int32_t port(CSOUND *csound, PORT *p)
49 {
50     IGN(csound);
51     p->yt1 = p->c1 * (double)*p->ksig + p->c2 * p->yt1;
52     *p->kr = (MYFLT)p->yt1;
53     return OK;
54 }
55 
kport(CSOUND * csound,KPORT * p)56 int32_t kport(CSOUND *csound, KPORT *p)
57 {
58     IGN(csound);
59     if (p->ihtim_old != *p->ihtim) {
60       p->c2 = pow(0.5, (double)CS_ONEDKR / *p->ihtim);
61       p->c1 = 1.0 - p->c2;
62       p->ihtim_old = *p->ihtim;
63     }
64     p->yt1 = p->c1 * (double)*p->ksig + p->c2 * p->yt1;
65     *p->kr = (MYFLT)p->yt1;
66     return OK;
67 }
68 
tonset(CSOUND * csound,TONE * p)69 int32_t tonset(CSOUND *csound, TONE *p)
70 {
71     double b;
72     p->prvhp = (double)*p->khp;
73     b = 2.0 - cos((double)(p->prvhp * csound->tpidsr));
74     p->c2 = b - sqrt(b * b - 1.0);
75     p->c1 = 1.0 - p->c2;
76 
77     if (LIKELY(!(*p->istor)))
78       p->yt1 = 0.0;
79     return OK;
80 }
81 
ktonset(CSOUND * csound,TONE * p)82 int32_t ktonset(CSOUND *csound, TONE *p) {
83     IGN(csound);
84     double b;
85     p->prvhp = (double)*p->khp;
86     b = 2.0 - cos((double)(p->prvhp * CS_ONEDKR *TWOPI));
87     p->c2 = b - sqrt(b * b - 1.0);
88     p->c1 = 1.0 - p->c2;
89 
90     if (LIKELY(!(*p->istor)))
91       p->yt1 = 0.0;
92     return OK;
93  }
94 
ktone(CSOUND * csound,TONE * p)95 int32_t ktone(CSOUND *csound, TONE *p)
96 {
97     IGN(csound);
98     double      c1 = p->c1, c2 = p->c2;
99     double      yt1 = p->yt1;
100 
101     if (*p->khp != (MYFLT)p->prvhp) {
102       double b;
103       p->prvhp = (double)*p->khp;
104       b = 2.0 - cos((double)(p->prvhp * CS_ONEDKR *TWOPI));
105       p->c2 = c2 = b - sqrt(b * b - 1.0);
106       p->c1 = c1 = 1.0 - c2;
107     }
108     yt1 = c1 * (double)(*p->asig) + c2 * yt1;
109     *p->ar = (MYFLT)yt1;
110     p->yt1 = yt1;
111     return OK;
112 }
113 
tone(CSOUND * csound,TONE * p)114 int32_t tone(CSOUND *csound, TONE *p)
115 {
116     IGN(csound);
117     MYFLT       *ar, *asig;
118     uint32_t offset = p->h.insdshead->ksmps_offset;
119     uint32_t early  = p->h.insdshead->ksmps_no_end;
120     uint32_t n, nsmps = CS_KSMPS;
121     double      c1 = p->c1, c2 = p->c2;
122     double      yt1 = p->yt1;
123 
124     if (*p->khp != (MYFLT)p->prvhp) {
125       double b;
126       p->prvhp = (double)*p->khp;
127       b = 2.0 - cos((double)(p->prvhp * csound->tpidsr));
128       p->c2 = c2 = b - sqrt(b * b - 1.0);
129       p->c1 = c1 = 1.0 - c2;
130     }
131     ar = p->ar;
132     asig = p->asig;
133     if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
134     if (UNLIKELY(early)) {
135       nsmps -= early;
136       memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
137     }
138     for (n=offset; n<nsmps; n++) {
139       yt1 = c1 * (double)(asig[n]) + c2 * yt1;
140       ar[n] = (MYFLT)yt1;
141     }
142     p->yt1 = yt1;
143     return OK;
144 }
145 
tonsetx(CSOUND * csound,TONEX * p)146 int32_t tonsetx(CSOUND *csound, TONEX *p)
147 {                   /* From Gabriel Maldonado, modified for arbitrary order */
148     {
149       double b;
150       p->prvhp = *p->khp;
151       b = 2.0 - cos((double)(*p->khp * csound->tpidsr));
152       p->c2 = b - sqrt(b * b - 1.0);
153       p->c1 = 1.0 - p->c2;
154     }
155     if (UNLIKELY((p->loop = (int32_t) (*p->ord + FL(0.5))) < 1)) p->loop = 4;
156     if (!*p->istor && (p->aux.auxp == NULL ||
157                     (uint32_t)(p->loop*sizeof(double)) > p->aux.size))
158         csound->AuxAlloc(csound, (int32_t)(p->loop*sizeof(double)), &p->aux);
159     p->yt1 = (double*)p->aux.auxp;
160     if (LIKELY(!(*p->istor))) {
161     memset(p->yt1, 0, p->loop*sizeof(double)); /* Punning zero and 0.0 */
162     }
163     return OK;
164 }
165 
tonex(CSOUND * csound,TONEX * p)166 int32_t tonex(CSOUND *csound, TONEX *p)      /* From Gabriel Maldonado, modified */
167 {
168     MYFLT       *ar = p->ar;
169     double      c2 = p->c2, *yt1 = p->yt1,c1 = p->c1;
170     uint32_t    offset = p->h.insdshead->ksmps_offset;
171     uint32_t    early  = p->h.insdshead->ksmps_no_end;
172     uint32_t    n, nsmps = CS_KSMPS;
173     int32_t     j, lp = p->loop;
174 
175     if (*p->khp != p->prvhp) {
176       double b;
177       p->prvhp = (double)*p->khp;
178       b = 2.0 - cos(p->prvhp * (double)csound->tpidsr);
179       p->c2 = b - sqrt(b * b - 1.0);
180       p->c1 = 1.0 - p->c2;
181     }
182 
183     memmove(ar,p->asig,sizeof(MYFLT)*nsmps);
184     if (UNLIKELY(offset))  memset(ar, '\0', offset*sizeof(MYFLT));
185     if (UNLIKELY(early)) {
186       nsmps -= early;
187       memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
188     }
189     for (j=0; j< lp; j++) {
190       /* Should *yt1 be reset to something?? */
191       for (n=0; n<nsmps; n++) {
192         double x = c1 * ar[n] + c2 * yt1[j];
193         yt1[j] = x;
194         ar[n] = (MYFLT)x;
195       }
196     }
197     return OK;
198 }
199 
katone(CSOUND * csound,TONE * p)200 int32_t katone(CSOUND *csound, TONE *p)
201 {
202     IGN(csound);
203     double     sig, x;
204     double      c2 = p->c2, yt1 = p->yt1;
205 
206     if (*p->khp != p->prvhp) {
207       double b;
208       p->prvhp = *p->khp;
209       b = 2.0 - cos((double)(*p->khp * CS_ONEDKR *TWOPI));
210       p->c2 = c2 = b - sqrt(b * b - 1.0);
211 /*      p->c1 = c1 = 1.0 - c2; */
212     }
213       sig = *p->asig;
214       x = yt1 = c2 * (yt1 + sig);
215       *p->ar = (MYFLT)x;
216       yt1 -= sig;               /* yt1 contains yt1-xt1 */
217 
218     p->yt1 = yt1;
219     return OK;
220 }
221 
222 
atone(CSOUND * csound,TONE * p)223 int32_t atone(CSOUND *csound, TONE *p)
224 {
225     MYFLT       *ar, *asig;
226     uint32_t    offset = p->h.insdshead->ksmps_offset;
227     uint32_t    early  = p->h.insdshead->ksmps_no_end;
228     uint32_t    n, nsmps = CS_KSMPS;
229     double      c2 = p->c2, yt1 = p->yt1;
230 
231     if (*p->khp != p->prvhp) {
232       double b;
233       p->prvhp = *p->khp;
234       b = 2.0 - cos((double)(*p->khp * csound->tpidsr));
235       p->c2 = c2 = b - sqrt(b * b - 1.0);
236 /*      p->c1 = c1 = 1.0 - c2; */
237     }
238     ar = p->ar;
239     if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
240     if (UNLIKELY(early)) {
241       nsmps -= early;
242       memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
243     }
244     asig = p->asig;
245     for (n=offset; n<nsmps; n++) {
246       double sig = (double)asig[n];
247       double x = yt1 = c2 * (yt1 + sig);
248       ar[n] = (MYFLT)x;
249       yt1 -= sig;               /* yt1 contains yt1-xt1 */
250     }
251     p->yt1 = yt1;
252     return OK;
253 }
254 
atonex(CSOUND * csound,TONEX * p)255 int32_t atonex(CSOUND *csound, TONEX *p)      /* Gabriel Maldonado, modified */
256 {
257     MYFLT       *ar = p->ar;
258     double      c2 = p->c2, *yt1 = p->yt1;
259     uint32_t    offset = p->h.insdshead->ksmps_offset;
260     uint32_t    early  = p->h.insdshead->ksmps_no_end;
261     uint32_t    n, nsmps = CS_KSMPS;
262     int32_t     j, lp = p->loop;
263 
264     if (*p->khp != p->prvhp) {
265       double b;
266       p->prvhp = *p->khp;
267       b = 2.0 - cos((double)(*p->khp * csound->tpidsr));
268       p->c2 = b - sqrt(b * b - 1.0);
269       /*p->c1 = 1. - p->c2;*/
270     }
271 
272     memmove(ar,p->asig,sizeof(MYFLT)*nsmps);
273     if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
274     if (UNLIKELY(early)) {
275       nsmps -= early;
276       memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
277     }
278     for (j=1; j<lp; j++) {
279       for (n=offset; n<nsmps; n++) {
280         double sig = (double)ar[n];
281         double x = c2 * (yt1[j] + sig);
282         yt1[j] = x - sig;            /* yt1 contains yt1-xt1 */
283         ar[n] = (MYFLT)x;
284       }
285     }
286     return OK;
287 }
288 
rsnset(CSOUND * csound,RESON * p)289 int32_t rsnset(CSOUND *csound, RESON *p)
290 {
291     int32_t scale;
292     p->scale = scale = (int32_t)*p->iscl;
293     if (UNLIKELY(scale && scale != 1 && scale != 2)) {
294       return csound->InitError(csound, Str("illegal reson iscl value, %f"),
295                                        *p->iscl);
296     }
297     p->prvcf = p->prvbw = -100.0;
298     if (!(*p->istor))
299       p->yt1 = p->yt2 = 0.0;
300     p->asigf = IS_ASIG_ARG(p->kcf);
301     p->asigw = IS_ASIG_ARG(p->kbw);
302 
303     return OK;
304 }
305 
krsnset(CSOUND * csound,RESON * p)306 int32_t krsnset(CSOUND *csound, RESON *p){ return rsnset(csound,p); }
307 
kreson(CSOUND * csound,RESON * p)308 int32_t kreson(CSOUND *csound, RESON *p)
309 {
310     uint32_t flag = 0;
311     double      c3p1, c3t4, omc3, c2sqr;
312     double      yt0, yt1, yt2, c1 = p->c1, c2 = p->c2, c3 = p->c3;
313     IGN(csound);
314 
315     if (*p->kcf != (MYFLT)p->prvcf) {
316       p->prvcf = (double)*p->kcf;
317       p->cosf = cos(p->prvcf * (double)(CS_ONEDKR *TWOPI));
318       flag = 1;                 /* Mark as changed */
319     }
320     if (*p->kbw != (MYFLT)p->prvbw) {
321       p->prvbw = (double)*p->kbw;
322       c3 = p->c3 = exp(p->prvbw * (double)(-CS_ONEDKR *TWOPI));
323       flag = 1;                /* Mark as changed */
324     }
325     if (flag) {
326       c3p1 = c3 + 1.0;
327       c3t4 = c3 * 4.0;
328       omc3 = 1.0 - c3;
329       c2 = p->c2 = c3t4 * p->cosf / c3p1;               /* -B, so + below */
330       c2sqr = c2 * c2;
331       if (p->scale == 1)
332         c1 = p->c1 = omc3 * sqrt(1.0 - c2sqr / c3t4);
333       else if (p->scale == 2)
334         c1 = p->c1 = sqrt((c3p1*c3p1-c2sqr) * omc3/c3p1);
335       else c1 = p->c1 = 1.0;
336     }
337 
338     yt1 = p->yt1; yt2 = p->yt2;
339     yt0 = c1 * ((double)*p->asig) + c2 * yt1 - c3 * yt2;
340     *p->ar = (MYFLT)yt0;
341     p->yt1 = yt0; p->yt2 = yt1; /* Write back for next cycle */
342     return OK;
343 }
344 
345 
reson(CSOUND * csound,RESON * p)346 int32_t reson(CSOUND *csound, RESON *p)
347 {
348     uint32_t    offset = p->h.insdshead->ksmps_offset;
349     uint32_t    early  = p->h.insdshead->ksmps_no_end;
350     uint32_t    flag = 0, n, nsmps = CS_KSMPS;
351     MYFLT       *ar, *asig;
352     double      c3p1, c3t4, omc3, c2sqr;
353     double      yt1, yt2, c1 = p->c1, c2 = p->c2, c3 = p->c3;
354     int32_t     asigf = p->asigf;
355     int32_t     asigw = p->asigw;
356 
357     asig = p->asig;
358     ar = p->ar;
359     if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
360     if (UNLIKELY(early)) {
361       nsmps -= early;
362       memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
363     }
364     yt1 = p->yt1; yt2 = p->yt2;
365     for (n=offset; n<nsmps; n++) {
366       double yt0;
367       MYFLT cf = asigf ? p->kcf[n] : *p->kcf;
368       MYFLT bw = asigw ? p->kbw[n] : *p->kbw;
369       if (cf != (MYFLT)p->prvcf) {
370         p->prvcf = (double)cf;
371         p->cosf = cos(cf * (double)(csound->tpidsr));
372         flag = 1;                 /* Mark as changed */
373       }
374       if (bw != (MYFLT)p->prvbw) {
375         p->prvbw = (double)bw;
376         c3 = p->c3 = exp(bw * (double)(csound->mtpdsr));
377         flag = 1;                /* Mark as changed */
378       }
379       if (flag) {
380         c3p1 = c3 + 1.0;
381         c3t4 = c3 * 4.0;
382         omc3 = 1.0 - c3;
383         c2 = p->c2 = c3t4 * p->cosf / c3p1;               /* -B, so + below */
384         c2sqr = c2 * c2;
385         if (p->scale == 1)
386           c1 = p->c1 = omc3 * sqrt(1.0 - c2sqr / c3t4);
387         else if (p->scale == 2)
388           c1 = p->c1 = sqrt((c3p1*c3p1-c2sqr) * omc3/c3p1);
389         else c1 = p->c1 = 1.0;
390         flag = 0;
391       }
392       yt0 = c1 * ((double)asig[n]) + c2 * yt1 - c3 * yt2;
393       ar[n] = (MYFLT)yt0;
394       yt2 = yt1;
395       yt1 = yt0;
396     }
397     p->yt1 = yt1; p->yt2 = yt2; /* Write back for next cycle */
398     return OK;
399 }
400 
rsnsetx(CSOUND * csound,RESONX * p)401 int32_t rsnsetx(CSOUND *csound, RESONX *p)
402 {                               /* Gabriel Maldonado, modifies for arb order */
403     int32_t scale;
404     p->scale = scale = (int32_t) *p->iscl;
405     if ((p->loop = (int32_t) (*p->ord + FL(0.5))) < 1)
406       p->loop = 4; /* default value */
407     if (!*p->istor && (p->aux.auxp == NULL ||
408                        (uint32_t)(p->loop*2*sizeof(double)) > p->aux.size))
409       csound->AuxAlloc(csound, (int32_t)(p->loop*2*sizeof(double)), &p->aux);
410     p->yt1 = (double*)p->aux.auxp; p->yt2 = (double*)p->aux.auxp + p->loop;
411     if (UNLIKELY(scale && scale != 1 && scale != 2)) {
412       return csound->InitError(csound, Str("illegal reson iscl value, %f"),
413                                        *p->iscl);
414     }
415     p->prvcf = p->prvbw = -100.0;
416 
417     if (!(*p->istor)) {
418       memset(p->yt1, 0, p->loop*sizeof(double));
419       memset(p->yt2, 0, p->loop*sizeof(double));
420     }
421     return OK;
422 }
423 
resonx(CSOUND * csound,RESONX * p)424 int32_t resonx(CSOUND *csound, RESONX *p)   /* Gabriel Maldonado, modified  */
425 {
426     uint32_t    offset = p->h.insdshead->ksmps_offset;
427     uint32_t    early  = p->h.insdshead->ksmps_no_end;
428     uint32_t    flag = 0, n, nsmps = CS_KSMPS;
429     int32_t     j;
430     MYFLT       *ar;
431     double      c3p1, c3t4, omc3, c2sqr;
432     double      *yt1, *yt2, c1,c2,c3;
433     int32_t     asgf = IS_ASIG_ARG(p->kcf);
434     int32_t     asgw = IS_ASIG_ARG(p->kbw);
435 
436     ar   = p->ar;
437     c1   = p->c1;
438     c2   = p->c2;
439     c3   = p->c3;
440     yt1  = p->yt1;
441     yt2  = p->yt2;
442     ar = p->ar;
443     memmove(ar,p->asig,sizeof(MYFLT)*nsmps);
444     if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
445     if (UNLIKELY(early)) {
446       nsmps -= early;
447       memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
448     }
449     for (j=0; j< p->loop; j++) {
450       for (n=offset; n<nsmps; n++) {
451         double x;
452         MYFLT cf = asgf ? p->kcf[n] : *p->kcf;
453         MYFLT bw = asgw ? p->kbw[n] : *p->kbw;
454         if (cf != (MYFLT)p->prvcf) {
455           p->prvcf = (double)cf;
456           p->cosf = cos(cf * (double)(csound->tpidsr));
457           flag = 1;
458         }
459         if (bw != (MYFLT)p->prvbw) {
460           p->prvbw = (double)bw;
461           c3 = exp(bw * (double)(csound->mtpdsr));
462           flag = 1;
463         }
464         if (flag) {
465           c3p1 = c3 + 1.0;
466           c3t4 = c3 * 4.0;
467           omc3 = 1.0 - c3;
468           c2 = c3t4 * p->cosf / c3p1;            /* -B, so + below */
469           c2sqr = c2 * c2;
470           if (p->scale == 1)
471             c1 = omc3 * sqrt(1.0 - (c2sqr / c3t4));
472           else if (p->scale == 2)
473             c1 = sqrt((c3p1*c3p1-c2sqr) * omc3/c3p1);
474           else c1 = 1.0;
475           flag =0;
476         }
477         x = c1 * ((double)ar[n]) + c2 * yt1[j] - c3 * yt2[j];
478         yt2[j] = yt1[j];
479         ar[n] = (MYFLT)x;
480         yt1[j] = x;
481       }
482     }
483     p->c1 = c1; p->c2 = c2; p->c3 = c3;
484     return OK;
485 }
486 
kareson(CSOUND * csound,RESON * p)487 int32_t kareson(CSOUND *csound, RESON *p)
488 {
489     uint32_t    flag = 0;
490     double      c3p1, c3t4, omc3, c2sqr; //, D = 2.0; /* 1/RMS = root2 (rand) */
491                                                    /*      or 1/.5  (sine) */
492     double      yt1, yt2, c1, c2, c3;
493     IGN(csound);
494 
495     if (*p->kcf != (MYFLT)p->prvcf) {
496       p->prvcf = (double)*p->kcf;
497       p->cosf = cos(p->prvcf * (double)(CS_ONEDKR *TWOPI));
498       flag = 1;
499     }
500     if (*p->kbw != (MYFLT)p->prvbw) {
501       p->prvbw = (double)*p->kbw;
502       p->c3 = exp(p->prvbw * (double)(-CS_ONEDKR *TWOPI));
503       flag = 1;
504     }
505     if (flag) {
506       c3p1 = p->c3 + 1.0;
507       c3t4 = p->c3 * 4.0;
508       omc3 = 1.0 - p->c3;
509       p->c2 = c3t4 * p->cosf / c3p1;
510       c2sqr = p->c2 * p->c2;
511       if (p->scale == 1)                        /* i.e. 1 - A(reson) */
512         p->c1 = 1.0 - omc3 * sqrt(1.0 - c2sqr / c3t4);
513       else if (p->scale == 2)                 /* i.e. D - A(reson) */
514         p->c1 = 2.0 - sqrt((c3p1*c3p1-c2sqr)*omc3/c3p1);
515       else p->c1 = 0.0;                        /* cannot tell        */
516     }
517 
518     c1 = p->c1; c2 = p->c2; c3 = p->c3; yt1 = p->yt1; yt2 = p->yt2;
519     if (p->scale == 1 || p->scale == 0) {
520         double sig = (double) *p->asig;
521         double ans = c1 * sig + c2 * yt1 - c3 * yt2;
522         yt2 = yt1;
523         yt1 = ans - sig;  /* yt1 contains yt1-xt1 */
524         *p->ar = (MYFLT)ans;
525     }
526     else if (p->scale == 2) {
527         double sig = (double) *p->asig;
528         double ans = c1 * sig + c2 * yt1 - c3 * yt2;
529         yt2 = yt1;
530         yt1 = ans - 2.0 * sig;      /* yt1 contains yt1-D*xt1 */
531         *p->ar = (MYFLT)ans;
532     }
533     p->yt1 = yt1; p->yt2 = yt2;
534     return OK;
535 }
536 
areson(CSOUND * csound,RESON * p)537 int32_t areson(CSOUND *csound, RESON *p)
538 {
539     uint32_t    offset = p->h.insdshead->ksmps_offset;
540     uint32_t    early  = p->h.insdshead->ksmps_no_end;
541     uint32_t    flag = 0, n, nsmps = CS_KSMPS;
542     MYFLT       *ar, *asig;
543     double      c3p1, c3t4, omc3, c2sqr;//, D = 2.0; /* 1/RMS = root2 (rand) */
544                                                    /*      or 1/.5  (sine) */
545     double      yt1, yt2, c1, c2, c3;
546 
547     if (*p->kcf != (MYFLT)p->prvcf) {
548       p->prvcf = (double)*p->kcf;
549       p->cosf = cos(p->prvcf * (double)(csound->tpidsr));
550       flag = 1;
551     }
552     if (*p->kbw != (MYFLT)p->prvbw) {
553       p->prvbw = (double)*p->kbw;
554       p->c3 = exp(p->prvbw * (double)(csound->mtpdsr));
555       flag = 1;
556     }
557     if (flag) {
558       c3p1 = p->c3 + 1.0;
559       c3t4 = p->c3 * 4.0;
560       omc3 = 1.0 - p->c3;
561       p->c2 = c3t4 * p->cosf / c3p1;
562       c2sqr = p->c2 * p->c2;
563       if (p->scale == 1)                        /* i.e. 1 - A(reson) */
564         p->c1 = 1.0 - omc3 * sqrt(1.0 - c2sqr / c3t4);
565       else if (p->scale == 2)                 /* i.e. D - A(reson) */
566         p->c1 = 2.0 - sqrt((c3p1*c3p1-c2sqr)*omc3/c3p1);
567       else p->c1 = 0.0;                        /* cannot tell        */
568     }
569     asig = p->asig;
570     ar = p->ar;
571     if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
572     if (UNLIKELY(early)) {
573       nsmps -= early;
574       memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
575     }
576     c1 = p->c1; c2 = p->c2; c3 = p->c3; yt1 = p->yt1; yt2 = p->yt2;
577     if (p->scale == 1 || p->scale == 0) {
578       for (n=offset; n<nsmps; n++) {
579         double sig = (double)asig[n];
580         double ans = c1 * sig + c2 * yt1 - c3 * yt2;
581         yt2 = yt1;
582         yt1 = ans - sig;  /* yt1 contains yt1-xt1 */
583         ar[n] = (MYFLT)ans;
584       }
585     }
586     else if (p->scale == 2) {
587       for (n=offset; n<nsmps; n++) {
588         double sig = (double)asig[n];
589         double ans = c1 * sig + c2 * yt1 - c3 * yt2;
590         yt2 = yt1;
591         yt1 = ans - 2.0 * sig;      /* yt1 contains yt1-D*xt1 */
592         ar[n] = (MYFLT)ans;
593       }
594     }
595     p->yt1 = yt1; p->yt2 = yt2;
596     return OK;
597 }
598 
599 /*
600  *
601  * LPREAD opcode : initialisation phase
602  *
603  *
604  */
605 
lprdset_(CSOUND * csound,LPREAD * p,int32_t stringname)606 int32_t lprdset_(CSOUND *csound, LPREAD *p, int32_t stringname)
607 {
608     LPHEADER *lph;
609     MEMFIL   *mfp;
610     int32_t  magic;
611     int32_t  totvals;
612     char     lpfilname[MAXNAME];
613 
614     /* Store adress of opcode for other lpXXXX init to point to */
615     if (csound->lprdaddr == NULL ||
616         csound->currentLPCSlot >= csound->max_lpc_slot) {
617       csound->max_lpc_slot = csound->currentLPCSlot + MAX_LPC_SLOT;
618       csound->lprdaddr = csound->ReAlloc(csound,
619                                   csound->lprdaddr,
620                                   csound->max_lpc_slot * sizeof(LPREAD*));
621     }
622     ((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot] = p;
623 
624     /* Build file name */
625     if (stringname) strNcpy(lpfilname, ((STRINGDAT*)p->ifilcod)->data, MAXNAME-1);
626     else if (csound->ISSTRCOD(*p->ifilcod))
627       strNcpy(lpfilname, get_arg_string(csound, *p->ifilcod), MAXNAME-1);
628     else csound->strarg2name(csound, lpfilname, p->ifilcod, "lp.", 0);
629 
630     /* Do not reload existing file ? */
631     if (UNLIKELY((mfp = p->mfp) != NULL && strcmp(mfp->filename, lpfilname) == 0))
632       goto lpend;                             /* rtn if file prv known */
633     /* Load analysis in memory file */
634     /* else read file  */
635     if (UNLIKELY((mfp = ldmemfile2withCB(csound, lpfilname, CSFTYPE_LPC, NULL))
636                  == NULL)) {
637       return csound->InitError(csound, Str("LPREAD cannot load %s"), lpfilname);
638     }
639     /* Store memory file location in opcode */
640     p->mfp = mfp;                                   /*  & record facts   */
641     /* Take a peek to the header if exisiting. Else take input arguments */
642     lph = (LPHEADER *) mfp->beginp;
643 
644     magic=lph->lpmagic;
645     if (LIKELY((magic==LP_MAGIC)||(magic==LP_MAGIC2))) {
646       p->storePoles = (magic==LP_MAGIC2);
647 
648       csound->Warning(csound, Str("Using %s type of file.\n"),
649                       p->storePoles?Str("pole"):Str("filter coefficient"));
650       /* Store header length */
651       p->headlen = lph->headersize;
652       /* Check if input values where available */
653       if (*p->inpoles || *p->ifrmrate) {
654         csound->Warning(csound, Str("lpheader overriding inputs"));
655       }
656       /* Check orc/analysis sample rate compatibility */
657       if (lph->srate != csound->esr) {
658         csound->Warning(csound, Str("lpfile srate != orch sr"));
659       }
660       p->npoles = lph->npoles;                /* note npoles, etc. */
661       /* Store header info in opcode */
662       p->nvals = lph->nvals;
663       p->framrat16 = lph->framrate * FL(65536.0);/* scaled framno cvt */
664     }
665     else if (UNLIKELY(BYTREVL(lph->lpmagic) == LP_MAGIC)) { /* Header reversed: */
666       return csound->InitError(csound, Str("file %s bytes are in wrong order"),
667                                        lpfilname);
668     }
669     else {                                    /* No Header on file:*/
670       p->headlen = 0;
671       p->npoles = (int32_t)*p->inpoles;          /*  data from inargs */
672       p->nvals = p->npoles + 4;
673       p->framrat16 = *p->ifrmrate * FL(65536.0);
674       if (UNLIKELY(!p->npoles || !p->framrat16)) {
675         return csound->InitError(csound,
676                                  Str("insufficient args and no file header"));
677       }
678     }
679     /* Check pole number */
680     csound->AuxAlloc(csound, (int32_t)(p->npoles*8*sizeof(MYFLT)), &p->aux);
681     p->kcoefs = (MYFLT*)p->aux.auxp;
682     /* if (UNLIKELY(p->npoles > MAXPOLES)) { */
683     /*   return csound->InitError(csound, Str("npoles > MAXPOLES")); */
684     /* } */
685     /* Look for total frame data size (file size - header) */
686     totvals = (mfp->length - p->headlen)/sizeof(MYFLT);
687     /* Store the size of a frame in integer */
688     p->lastfram16 = (((totvals - p->nvals) / p->nvals) << 16) - 1;
689     if (UNLIKELY(csound->oparms->odebug))
690       csound->Message(csound, Str(
691                  "npoles %"PRIi32", nvals %"PRIi32", totvals %"PRIi32
692                  ", lastfram16 = %"PRIi32"x\n"),
693              p->npoles, p->nvals, totvals, p->lastfram16);
694  lpend:
695     p->lastmsg = 0;
696     return OK;
697 }
698 
lprdset(CSOUND * csound,LPREAD * p)699 int32_t lprdset(CSOUND *csound, LPREAD *p){
700     return lprdset_(csound, p, 0);
701 }
702 
lprdset_S(CSOUND * csound,LPREAD * p)703 int32_t lprdset_S(CSOUND *csound, LPREAD *p){
704     return lprdset_(csound, p, 1);
705 }
706 
707 
708 #ifdef TRACE_POLES
709 static void
DumpPolesF(int32_t poleCount,MYFLT * part1,MYFLT * part2,int32_t isMagn,char * where)710   DumpPolesF(int32_t poleCount, MYFLT *part1, MYFLT *part2,
711              int32_t isMagn, char *where)
712 {
713     int32_t i;
714 
715     csound->Message(csound, "%s\n", where);
716     for (i=0; i<poleCount; i++) {
717       if (isMagn)
718         csound->Message(csound, Str("magnitude: %f   Phase: %f\n"),
719                                 part1[i], part2[i]);
720       else
721         csound->Message(csound, Str("Real: %f   Imag: %f\n"),
722                                 part1[i], part2[i]);
723     }
724 }
725 #endif
726 
SortPoles(int32_t poleCount,MYFLT * poleMagn,MYFLT * polePhas)727 static void SortPoles(int32_t poleCount, MYFLT *poleMagn, MYFLT *polePhas)
728 {
729     int32_t i, j;
730     MYFLT   diff, fTemp;
731     int32_t shouldSwap;
732 
733     /*  DumpPolesF(poleCount, poleMagn, polePhas, 1, "Before sort"); */
734 
735     for (i=1; i<poleCount; i++) {
736       for (j=0; j<i; j++) {
737 
738         shouldSwap = 0;
739 
740         diff = FABS(polePhas[j])-FABS(polePhas[i]);
741         if (diff>FL(1.0e-10))
742           shouldSwap = 1;
743         else if (diff>-FL(1.0e-10)) {
744           diff = poleMagn[j]-poleMagn[i];
745 
746           if (diff>FL(1.0e-10))
747             shouldSwap = 1;
748           else if (diff>-FL(1.0e-10))
749             {
750               if (polePhas[j]>polePhas[i])
751                 shouldSwap = 1;
752             }
753         }
754         if (shouldSwap) {
755           fTemp = poleMagn[i];
756           poleMagn[i] = poleMagn[j];
757           poleMagn[j] = fTemp;
758 
759           fTemp = polePhas[i];
760           polePhas[i] = polePhas[j];
761           polePhas[j] = fTemp;
762         }
763       }
764     }
765 /*  DumpPolesF(poleCount, poleMagn, polePhas, 1, "After sort"); */
766 }
767 
DoPoleInterpolation(int poleCount,MYFLT * pm1,MYFLT * pp1,MYFLT * pm2,MYFLT * pp2,MYFLT factor,MYFLT * outMagn,MYFLT * outPhas)768 static int32_t DoPoleInterpolation(int poleCount,
769                                MYFLT *pm1, MYFLT *pp1,
770                                MYFLT *pm2, MYFLT *pp2,
771                                MYFLT factor, MYFLT *outMagn, MYFLT *outPhas)
772 {
773     int32_t i;
774 
775     if (UNLIKELY(poleCount%2!=0)) {
776       printf (Str("Cannot handle uneven pole count yet\n"));
777       return (0);
778     }
779 
780     for (i=0; i<poleCount; i++) {
781       if (FABS(FL(FABS(pp1[i])-PI))<FL(1.0e-5)) {
782         pm1[i] = -pm1[i];
783         pp1[i] = FL(0.0);
784       }
785 
786       if (FABS(FL(FABS(pp2[i])-PI))<FL(1.0e-5)) {
787         pm2[i] = -pm2[i];
788         pp2[i] = FL(0.0);
789       }
790     }
791 
792     /* Sort poles according to abs(phase) */
793 
794     SortPoles(poleCount, pm1, pp1);
795     SortPoles(poleCount, pm2, pp2);
796 
797         /*      DumpPolesF(poleCount, pm1, pp1, 1, "Sorted poles 1"); */
798         /*      DumpPolesF(poleCount, pm2, pp2, 1, "Sorted poles 2"); */
799 
800         /*      printf ("factor=%f\n", factor); */
801 
802     for (i=0; i<poleCount; i++) {
803       outMagn[i] = pm1[i]+(pm2[i]-pm1[i])*factor;
804       outPhas[i] = pp1[i]+(pp2[i]-pp1[i])*factor;
805     }
806 
807 /*     DumpPolesF(poleCount, outMagn, outPhas, 1, "Interpolated poles"); */
808 
809     return(1);
810 }
811 
InvertPoles(int32_t count,double * real,double * imag)812 static inline void InvertPoles(int32_t count, double *real, double *imag)
813 {
814     int32_t    i;
815     double     pr,pi,mag;
816 
817     for (i=0; i<count; i++) {
818       pr = real[i];
819       pi = imag[i];
820       mag = pr*pr+pi*pi;
821       real[i] = pr/mag;
822       imag[i] = -pi/mag;
823     }
824 }
825 
826 /*
827  *
828  * Resynthetize filter coefficients from poles values
829  *
830  */
831 
832 static inline void
synthetize(int32_t poleCount,double * poleReal,double * poleImag,double * polyReal,double * polyImag)833     synthetize(int32_t    poleCount,
834                double *poleReal, double *poleImag,
835                double *polyReal, double *polyImag)
836 {
837     int32_t    j, k;
838     double     pr, pi, cr, ci;
839 
840     polyReal[0] = 1;
841     polyImag[0] = 0;
842 
843     for (j=0; j<poleCount; j++) {
844       polyReal[j+1] = 1;
845       polyImag[j+1] = 0;
846 
847       pr = poleReal[j];
848       pi = poleImag[j];
849 
850       for (k=j; k>=0; k--) {
851         cr = polyReal[k];
852         ci = polyImag[k];
853 
854         polyReal[k] = -(cr*pr-ci*pi);
855         polyImag[k] = -(ci*pr+cr*pi);
856 
857         if (LIKELY(k>0)) {
858             polyReal[k] += polyReal[k-1];
859             polyImag[k] += polyImag[k-1];
860         }
861       }
862     }
863 
864     /* Makes it 1+a1.x+...+anXn */
865 
866     pr = polyReal[0];
867     for (j=0; j<=poleCount; j++)
868       polyReal[j] /= pr;
869 }
870 
871 /*
872  *
873  * LPREAD k/a time access. This will setup current pole values
874  *
875  */
876 
lpread(CSOUND * csound,LPREAD * p)877 int32_t lpread(CSOUND *csound, LPREAD *p)
878 {
879     MYFLT   *bp, *np, *cp;
880     int32_t  nn, framphase;
881     MYFLT   fract;
882     int32_t i, status;
883     MYFLT   *poleMagn1 = p->kcoefs + 2*p->npoles;
884     MYFLT   *polePhas1 = poleMagn1 + p->npoles;
885     MYFLT   *poleMagn2 = polePhas1 + p->npoles;
886     MYFLT   *polePhas2 = poleMagn2 + p->npoles;
887     MYFLT   *interMagn = polePhas2 + p->npoles;
888     MYFLT   *interPhas = interMagn + p->npoles;
889 
890 
891     if (UNLIKELY(p->mfp==NULL)) {
892       return csound->PerfError(csound, &(p->h),
893                                Str("lpread: not initialised"));
894     }
895     /* Locate frame position range */
896     if (UNLIKELY((framphase = (int32)(*p->ktimpt*p->framrat16)) < 0)) {
897       /* for kfram reqd*/
898       return csound->PerfError(csound, &(p->h),
899                                Str("lpread timpnt < 0"));
900     }
901     if (framphase > p->lastfram16) {                /* not past last one */
902       framphase = p->lastfram16;
903       if (UNLIKELY(!p->lastmsg)) {
904         p->lastmsg = 1;
905         csound->Warning(csound, Str("lpread ktimpnt truncated to last frame"));
906       }
907     }
908     /* Locate frames bounding current time */
909     bp = (MYFLT *)(p->mfp->beginp + p->headlen); /* locate begin frame data */
910     nn = (framphase >> 16) * p->nvals;
911     bp = bp + nn;                                /* locate begin this frame */
912     np = bp + p->nvals;                          /* & interp betw adj frams */
913     fract = (framphase & 0x0FFFFL) / FL(65536.0);
914     /* Interpolate freq/amplpitude and store in opcode */
915     *p->krmr = *bp + (*np - *bp) * fract;   bp++;   np++; /* for 4 rslts */
916     *p->krmo = *bp + (*np - *bp) * fract;   bp++;   np++;
917     *p->kerr = *bp + (*np - *bp) * fract;   bp++;   np++;
918     *p->kcps = *bp + (*np - *bp) * fract;   bp++;   np++;
919 
920    /* Interpolate filter or poles coef values and store in opcode */
921 
922     cp = p->kcoefs;      /* This is where the coefs get stored */
923     if (p->storePoles) {
924       for (i=0; i<p->npoles; i++) {
925         poleMagn1[i] = *bp++;
926         polePhas1[i] = *bp++;
927         poleMagn2[i] = *np++;
928         polePhas2[i] = *np++;
929       }
930 
931       status =
932         DoPoleInterpolation(p->npoles,poleMagn1,polePhas1,poleMagn2,
933                             polePhas2,fract,interMagn,interPhas);
934       if (UNLIKELY(!status)) {
935         return csound->PerfError(csound, &(p->h),
936                                  Str("Interpolation failed"));
937       }
938       for (i=0; i<p->npoles; i++) {
939         *cp++ =  interMagn[i];
940         *cp++ =  interPhas[i];
941       }
942     }
943     else {
944       for (nn = 0; nn< p->npoles; nn++) {
945         cp[nn] = bp[nn] + (np[nn] - bp[nn]) * fract;
946       }
947     }
948 /*  if (csound->oparms->odebug) {
949       csound->Message(csound,
950           "phase:%lx fract:%6.2f rmsr:%6.2f rmso:%6.2f kerr:%6.2f kcps:%6.2f\n",
951           framphase,fract,*p->krmr,*p->krmo,*p->kerr,*p->kcps);
952       cp = p->kcoefs;
953       nn = p->npoles;
954       do {
955         csound->Message(csound, " %6.2f",*cp++);
956       } while (--nn);
957       csound->Message(csound, "\n");
958     }  */
959     return OK;
960 }
961 
962 
lpformantset(CSOUND * csound,LPFORM * p)963 int32_t lpformantset(CSOUND *csound, LPFORM *p)
964 {
965     LPREAD *q;
966 
967    /* connect to previously loaded lpc analysis */
968    /* get adr lpread struct */
969     p->lpread = q = ((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot];
970     csound->AuxAlloc(csound, p->lpread->npoles*sizeof(MYFLT), &p->aux);
971     return OK;
972 }
973 
lpformant(CSOUND * csound,LPFORM * p)974 int32_t lpformant(CSOUND *csound, LPFORM *p)
975 {
976     LPREAD  *q = p->lpread;
977     MYFLT   *coefp, sr = csound->esr;
978     MYFLT   *cfs = (MYFLT*)p->aux.auxp;
979     MYFLT   *bws = cfs+p->lpread->npoles/2;
980     int32_t i, j, ndx = *p->kfor;
981     double  pm,pp;
982 
983     if (q->storePoles) {
984       coefp = q->kcoefs;
985       for (i=2,j=0; i<q->npoles*2; i+=4, j++) {
986         pm = coefp[i];
987         pp = coefp[i+1];
988         cfs[j] = pp*sr/TWOPI;
989         /* if (pm > 1.0) csound->Message(csound,
990                                         Str("warning unstable pole %f\n"), pm); */
991         bws[j] = -LOG(pm)*sr/PI;
992       }
993     }
994     else {
995       csound->PerfError(csound, &(p->h),
996                         Str("this opcode only works with LPC "
997                             "pole analysis type (-a)\n"));
998       return NOTOK;
999     }
1000 
1001     j = (ndx < 1 ? 1 :
1002          (ndx >= p->lpread->npoles/2 ? p->lpread->npoles/2 : ndx)) - 1;
1003     if (bws[j] > sr/2 || isnan(bws[j])) bws[j] = sr/2;
1004     if (bws[j] < 1.0) bws[j] = 1.0;
1005     if (cfs[j] > sr/2 || isnan(cfs[j])) cfs[j] = sr/2;
1006     cfs[j] = FABS(cfs[j]);
1007     *p->kcf = cfs[j];
1008     *p->kbw = bws[j];
1009 
1010     return OK;
1011 }
1012 
1013 
1014 /*
1015  *
1016  * LPRESON: initialisation time
1017  *
1018  *
1019  */
lprsnset(CSOUND * csound,LPRESON * p)1020 int32_t lprsnset(CSOUND *csound, LPRESON *p)
1021 {
1022     LPREAD *q;
1023 
1024    /* connect to previously loaded lpc analysis */
1025    /* get adr lpread struct */
1026     p->lpread = q = ((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot];
1027 
1028     csound->AuxAlloc(csound, (int32)((q->npoles<<1)*sizeof(MYFLT)), &p->aux);
1029    /* Initialize pointer to circulat buffer (for filtering) */
1030     p->circjp = p->circbuf = (MYFLT*)p->aux.auxp;
1031     p->jp2lim = p->circbuf + (q->npoles << 1);  /* npoles det circbuflim */
1032     return OK;
1033 }
1034 
1035 /*
1036  *
1037  * LPRESON: k & a time access. Will actually filter the signal
1038  *                  Uses a circular buffer to store previous signal values.
1039  */
1040 
lpreson(CSOUND * csound,LPRESON * p)1041 int32_t lpreson(CSOUND *csound, LPRESON *p)
1042 {
1043     IGN(csound);
1044     LPREAD   *q = p->lpread;
1045     uint32_t offset = p->h.insdshead->ksmps_offset;
1046     uint32_t early  = p->h.insdshead->ksmps_no_end;
1047     uint32_t n, nsmps = CS_KSMPS;
1048     MYFLT    *coefp, *pastp, *jp, *jp2, *rslt = p->ar, *asig = p->asig;
1049     MYFLT    x;
1050     double   poleReal[MAXPOLES], poleImag[MAXPOLES];
1051     double   polyReal[MAXPOLES+1], polyImag[MAXPOLES+1];
1052     int32_t  i, nn;
1053     double   pm,pp;
1054 
1055     jp = p->circjp;
1056     jp2 = jp + q->npoles;
1057 
1058     /* If we where using poles, we have to compute filter coefs now */
1059     if (q->storePoles) {
1060       coefp = q->kcoefs;
1061       for (i=0; i<q->npoles; i++) {
1062         pm = *coefp++;
1063         pp = *coefp++;
1064         /*       csound->Message(csound, "pole %d, fr=%.2f, BW=%.2f\n", i,
1065                         pp*(csound->esr)/6.28, -csound->esr*log(pm)/3.14);
1066         */
1067         if (fabs(pm)>0.999999)
1068           pm = 1/pm;
1069         poleReal[i] = pm*cos(pp);
1070         poleImag[i] = pm*sin(pp);
1071       }
1072       /*     DumpPoles(q->npoles,poleReal,poleImag,0,"About to filter"); */
1073       InvertPoles(q->npoles,poleReal,poleImag);
1074       synthetize(q->npoles,poleReal,poleImag,polyReal,polyImag);
1075       coefp = q->kcoefs;
1076       for (i=0; i<q->npoles; i++) {
1077         /* MR_WHY - somthing with the atan2 ? */
1078         coefp[i] = -(MYFLT)polyReal[q->npoles-i];
1079       }
1080     }
1081 
1082     if (UNLIKELY(offset)) memset(rslt, '\0', offset*sizeof(MYFLT));
1083     if (UNLIKELY(early)) {
1084       nsmps -= early;
1085       memset(&rslt[nsmps], '\0', early*sizeof(MYFLT));
1086     }
1087     /* For each sample */
1088     for (n=offset; n<nsmps; n++) {
1089       /* Compute Xn = Yn + CkXn-k */
1090 
1091 #ifdef TRACE_FILTER
1092       csound->Message(csound, "Asig=%f\n", *asig);
1093 #endif
1094       x = asig[n];
1095       coefp = q->kcoefs;              /* using lpread interp coefs */
1096       pastp = jp;
1097       nn = q->npoles;
1098       do {
1099 #ifdef TRACE_FILTER
1100         csound->Message(csound, "\t%f,%f\n", *coefp, *pastp);
1101 #endif
1102         x += *coefp++ * *pastp++;
1103       } while (--nn);
1104 #ifdef TRACE_FILTER
1105       csound->Message(csound, "result=%f\n", x);
1106 #endif
1107       /* Store result signal in circular and output buffers */
1108 
1109       *jp++ = *jp2++ = x;
1110       rslt[n] = x;
1111 
1112       /* Check if end of buffer reached */
1113       if (jp2 >= p->jp2lim) {
1114         jp2 = jp;
1115         jp = p->circbuf;
1116       }
1117     }
1118     p->circjp = jp;
1119     return OK;
1120 }
1121 
1122 /*
1123  *
1124  * LPFRESON : Initialisation time
1125  *
1126  */
lpfrsnset(CSOUND * csound,LPFRESON * p)1127 int32_t lpfrsnset(CSOUND *csound, LPFRESON *p)
1128 {
1129 
1130    /* Connect to previously loaded analysis file */
1131 
1132     if (((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot]->storePoles) {
1133       return csound->InitError(csound, Str("Pole file not supported "
1134                                            "for this opcode !"));
1135     }
1136 
1137 
1138     p->lpread = ((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot];
1139     if(p->lpread->npoles < 2) {
1140       return csound->InitError(csound, Str("Too few poles (> 2)"));
1141     }
1142 
1143     p->prvratio = FL(1.0);
1144     p->d = FL(0.0);
1145     p->prvout = FL(0.0);
1146     csound->AuxAlloc(csound, (int32)(p->lpread->npoles*sizeof(MYFLT)), &p->aux);
1147     p->past = (MYFLT*)p->aux.auxp;
1148 
1149     return OK;
1150 }
1151 
1152 /*
1153  *
1154  * LPFRESON : k & a time : actually filters the data
1155  *
1156  */
lpfreson(CSOUND * csound,LPFRESON * p)1157 int32_t lpfreson(CSOUND *csound, LPFRESON *p)
1158 {
1159     LPREAD   *q = p->lpread;
1160     uint32_t offset = p->h.insdshead->ksmps_offset;
1161     uint32_t early  = p->h.insdshead->ksmps_no_end;
1162     uint32_t nn, n, nsmps = CS_KSMPS;
1163     MYFLT    *coefp, *pastp, *pastp1, *rslt = p->ar, *asig = p->asig;
1164     MYFLT    x, temp1, temp2, ampscale, cq;
1165 
1166     if (*p->kfrqratio != p->prvratio) {             /* for new freqratio */
1167       if (*p->kfrqratio <= FL(0.0)) {
1168         return csound->PerfError(csound, &(p->h),
1169                                  Str("illegal frqratio, %5.2f"),
1170                                          *p->kfrqratio);
1171       }                                             /*      calculate d  */
1172       p->d = (*p->kfrqratio - FL(1.0)) / (*p->kfrqratio + FL(1.0));
1173       p->prvratio = *p->kfrqratio;
1174     }
1175     if (p->d != FL(0.0)) {                          /* for non-zero d,   */
1176       coefp = q->kcoefs;
1177       nn = q->npoles - 1;
1178       do {
1179         temp1 = p->d * *coefp++;                    /*    shift formants */
1180         *coefp += temp1;
1181       }
1182       while (--nn);
1183       ampscale = FL(1.0) / (FL(1.0) - p->d * *coefp); /*    & reset scales */
1184       cq = (FL(1.0) - p->d * p->d) * ampscale;
1185     }
1186     else {
1187       cq = FL(1.0);
1188       ampscale = FL(1.0);
1189     }
1190     x = p->prvout;
1191     if (UNLIKELY(offset)) memset(rslt, '\0', offset*sizeof(MYFLT));
1192     if (UNLIKELY(early)) {
1193       nsmps -= early;
1194       memset(&rslt[nsmps], '\0', early*sizeof(MYFLT));
1195     }
1196     for (n=offset; n<nsmps; n++) {
1197       nn = q->npoles - 1;
1198       pastp  = pastp1 = p->past + nn;
1199       temp1 = *pastp;
1200       *pastp = cq * x - p->d * *pastp;
1201       pastp--;
1202       do {
1203         temp2 = *pastp;
1204         *pastp = (*pastp1 - *pastp) * p->d + temp1;
1205         pastp--;   pastp1--;
1206         temp1 = temp2;
1207       } while (--nn);
1208       x = asig[n];
1209       pastp = p->past;
1210       coefp = q->kcoefs;
1211       nn = q->npoles;
1212       do  {
1213         x += *coefp++ * *pastp++;
1214       } while (--nn);
1215       rslt[n] = x * ampscale;
1216     }
1217     p->prvout = x;
1218     return OK;
1219 }
1220 
rmsset(CSOUND * csound,RMS * p)1221 int32_t rmsset(CSOUND *csound, RMS *p)
1222 {
1223     double   b;
1224 
1225     b = 2.0 - cos((double)(*p->ihp * csound->tpidsr));
1226     p->c2 = b - sqrt(b*b - 1.0);
1227     p->c1 = 1.0 - p->c2;
1228     if (!*p->istor)
1229       p->prvq = 0.0;
1230     return OK;
1231 }
1232 
gainset(CSOUND * csound,GAIN * p)1233 int32_t gainset(CSOUND *csound, GAIN *p)
1234 {
1235     double   b;
1236 
1237     b = 2.0 - cos((double)(*p->ihp * csound->tpidsr));
1238     p->c2 = b - sqrt(b*b - 1.0);
1239     p->c1 = 1.0 - p->c2;
1240     if (!*p->istor)
1241       p->prvq = p->prva = 0.0;
1242     return OK;
1243 }
1244 
balnset(CSOUND * csound,BALANCE * p)1245 int32_t balnset(CSOUND *csound, BALANCE *p)
1246 {
1247     double   b;
1248 
1249     b = 2.0 - cos((double)(*p->ihp * csound->tpidsr));
1250     p->c2 = b - sqrt(b*b - 1.0);
1251     p->c1 = 1.0 - p->c2;
1252     if (!*p->istor)
1253       p->prvq = p->prvr = p->prva = 0.0;
1254     return OK;
1255 }
1256 
rms(CSOUND * csound,RMS * p)1257 int32_t rms(CSOUND *csound, RMS *p)
1258 {
1259     IGN(csound);
1260     uint32_t offset = p->h.insdshead->ksmps_offset;
1261     uint32_t early  = p->h.insdshead->ksmps_no_end;
1262     uint32_t n, nsmps = CS_KSMPS;
1263     MYFLT    *asig;
1264     double   q;
1265     double   c1 = p->c1, c2 = p->c2;
1266 
1267     q = p->prvq;
1268     asig = p->asig;
1269     if (UNLIKELY(early)) nsmps -= early;
1270     for (n=offset; n<nsmps; n++) {
1271       double as = (double)asig[n];
1272       q = c1 * as * as + c2 * q;
1273     }
1274     p->prvq = q;
1275     *p->kr = (MYFLT) sqrt(q);
1276     return OK;
1277 }
1278 
gain(CSOUND * csound,GAIN * p)1279 int32_t gain(CSOUND *csound, GAIN *p)
1280 {
1281     IGN(csound);
1282     uint32_t offset = p->h.insdshead->ksmps_offset;
1283     uint32_t early  = p->h.insdshead->ksmps_no_end;
1284     uint32_t n, nsmps = CS_KSMPS;
1285     MYFLT    *ar, *asig;
1286     double   q, a, m, diff, inc;
1287     double   c1 = p->c1, c2 = p->c2;
1288 
1289     q = p->prvq;
1290     asig = p->asig;
1291     if (UNLIKELY(early)) nsmps -= early;
1292     for (n = offset; n < nsmps-early; n++) {
1293       double as = (double)asig[n];
1294       q = c1 * as * as + c2 * q;
1295     }
1296     p->prvq = q;
1297     if (q > 0.0)
1298       a = *p->krms / sqrt(q);
1299     else
1300       a = *p->krms;
1301     ar = p->ar;
1302     if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
1303     if (UNLIKELY(early)) {
1304       nsmps -= early;
1305       memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
1306     }
1307     if ((diff = a - p->prva) != 0.0) {
1308       m = p->prva;
1309       inc = diff / (double)(nsmps-offset);
1310       for (n = offset; n < nsmps; n++) {
1311         ar[n] = asig[n] * m;
1312         m += inc;
1313       }
1314       p->prva = a;
1315     }
1316     else {
1317       for (n = offset; n < nsmps; n++) {
1318         ar[n] = asig[n] * a;
1319       }
1320     }
1321     return OK;
1322 }
1323 
balance(CSOUND * csound,BALANCE * p)1324 int32_t balance(CSOUND *csound, BALANCE *p)
1325 {
1326     IGN(csound);
1327     uint32_t offset = p->h.insdshead->ksmps_offset;
1328     uint32_t early  = p->h.insdshead->ksmps_no_end;
1329     uint32_t n, nsmps = CS_KSMPS;
1330     MYFLT    *ar, *asig, *csig;
1331     double   q, r, a, m, diff, inc;
1332     double   c1 = p->c1, c2 = p->c2;
1333 
1334     q = p->prvq;
1335     r = p->prvr;
1336     asig = p->asig;
1337     csig = p->csig;
1338     ar = p->ar;
1339     if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
1340     if (UNLIKELY(early)) {
1341       nsmps -= early;
1342       memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
1343     }
1344     for (n = offset; n < nsmps; n++) {
1345       double as = (double)asig[n];
1346       double cs = (double)csig[n];
1347       q = c1 * as * as + c2 * q;
1348       r = c1 * cs * cs + c2 * r;
1349     }
1350     p->prvq = q;
1351     p->prvr = r;
1352     if (LIKELY(q != 0.0))
1353       a = sqrt(r/q);
1354     else
1355       a = sqrt(r);
1356     if ((diff = a - p->prva) != 0.0) {
1357       m = p->prva;
1358       inc = diff / (double)(nsmps-offset);
1359       for (n = offset; n < nsmps; n++) {
1360         ar[n] = asig[n] * m;
1361         m += inc;
1362       }
1363       p->prva = a;
1364     }
1365     else {
1366       for (n = offset; n < nsmps; n++) {
1367         ar[n] = asig[n] * a;
1368       }
1369     }
1370     return OK;
1371 }
1372 
1373 
balance2(CSOUND * csound,BALANCE * p)1374 int32_t balance2(CSOUND *csound, BALANCE *p)
1375 {
1376     IGN(csound);
1377     uint32_t offset = p->h.insdshead->ksmps_offset;
1378     uint32_t early  = p->h.insdshead->ksmps_no_end;
1379     uint32_t n, nsmps = CS_KSMPS;
1380     MYFLT    *ar, *asig, *csig;
1381     double   q, r, a;
1382     double   c1 = p->c1, c2 = p->c2;
1383 
1384     q = p->prvq;
1385     r = p->prvr;
1386     asig = p->asig;
1387     csig = p->csig;
1388     ar = p->ar;
1389     if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
1390     if (UNLIKELY(early)) {
1391       nsmps -= early;
1392       memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
1393     }
1394     for (n = offset; n < nsmps; n++) {
1395       double as = (double)asig[n];
1396       double cs = (double)csig[n];
1397       q = c1 * as * as + c2 * q;
1398       r = c1 * cs * cs + c2 * r;
1399       if (LIKELY(q != 0.0))
1400         a = sqrt(r/q);
1401       else
1402         a = sqrt(r);
1403       ar[n] = asig[n] * a;
1404     }
1405     p->prvq = q;
1406     p->prvr = r;
1407     return OK;
1408 }
1409 
1410 /*
1411  *   Set current lpc slot
1412  */
lpslotset(CSOUND * csound,LPSLOT * p)1413 int32_t lpslotset(CSOUND *csound, LPSLOT *p)
1414 {
1415     int32_t n;
1416 
1417     n = (int32_t) *(p->islotnum);
1418     if (UNLIKELY(n < 0))
1419       return csound->InitError(csound, Str("lpslot number should be positive"));
1420     else {
1421       if (n >= csound->max_lpc_slot) {
1422         csound->max_lpc_slot = n + MAX_LPC_SLOT;
1423         csound->lprdaddr = csound->ReAlloc(csound,
1424                                     csound->lprdaddr,
1425                                     csound->max_lpc_slot * sizeof(LPREAD**));
1426       }
1427       csound->currentLPCSlot = n;
1428     }
1429     return OK;
1430 }
1431 
lpitpset(CSOUND * csound,LPINTERPOL * p)1432 int32_t lpitpset(CSOUND *csound, LPINTERPOL *p)
1433 {
1434 
1435     if (UNLIKELY((uint32_t) ((int32_t) *(p->islot1))
1436                  >= (uint32_t) csound->max_lpc_slot ||
1437                  (uint32_t) ((int32_t) *(p->islot2))
1438                  >= (uint32_t) csound->max_lpc_slot))
1439       return csound->InitError(csound, Str("LPC slot is not allocated"));
1440   /* Get lpread pointers */
1441     p->lp1 = ((LPREAD**) csound->lprdaddr)[(int32_t) *(p->islot1)];
1442     p->lp2 = ((LPREAD**) csound->lprdaddr)[(int32_t) *(p->islot2)];
1443 
1444   /* Check if workable */
1445 
1446     if (UNLIKELY((!p->lp1->storePoles) || (!p->lp2->storePoles))) {
1447       return csound->InitError(csound, Str("lpinterpol works only "
1448                                            "with poles files.."));
1449     }
1450     if (UNLIKELY(p->lp1->npoles != p->lp2->npoles)) {
1451       return csound->InitError(csound, Str("The poles files "
1452                                            "have different pole count"));
1453     }
1454 
1455 #if 0                   /* This is incorrect C */
1456     if (&p->kcoefs-p != &p->lp1->kcoefs-p->lp1)
1457       return csound->InitError(csound, Str("padding error"));
1458 #endif
1459 
1460     p->npoles = p->lp1->npoles;
1461     csound->AuxAlloc(csound, (int32)(p->npoles*8*sizeof(MYFLT)), &p->aux);
1462     p->kcoefs = (MYFLT*)p->aux.auxp;
1463     p->storePoles = 1;
1464     ((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot] = (LPREAD*) p;
1465     return OK;
1466 }
1467 
lpinterpol(CSOUND * csound,LPINTERPOL * p)1468 int32_t lpinterpol(CSOUND *csound, LPINTERPOL *p)
1469 {
1470     int32_t i,status;
1471     MYFLT   *cp,*cp1,*cp2;
1472     MYFLT   *poleMagn1 = p->kcoefs + 2*p->npoles;
1473     MYFLT   *polePhas1 = poleMagn1 + p->npoles;
1474     MYFLT   *poleMagn2 = polePhas1 + p->npoles;
1475     MYFLT   *polePhas2 = poleMagn2 + p->npoles;
1476     MYFLT   *interMagn = polePhas2 + p->npoles;
1477     MYFLT   *interPhas = interMagn + p->npoles;
1478 
1479     /* RWD: guessing this... */
1480     if (UNLIKELY(p->lp1==NULL || p->lp2==NULL)) {
1481       return csound->PerfError(csound, &(p->h),
1482                                Str("lpinterpol: not initialised"));
1483     }
1484     cp1 =  p->lp1->kcoefs;
1485     cp2 =  p->lp2->kcoefs;
1486 
1487     for (i=0; i<p->npoles; i++) {
1488       poleMagn1[i] = *cp1++;
1489       polePhas1[i] = *cp1++;
1490       poleMagn2[i] = *cp2++;
1491       polePhas2[i] = *cp2++;
1492     }
1493 
1494     status = DoPoleInterpolation(p->npoles,poleMagn1,polePhas1,poleMagn2,
1495                                      polePhas2,*p->kmix,interMagn,interPhas);
1496     if (UNLIKELY(!status)) {
1497       return csound->PerfError(csound, &(p->h),
1498                                Str("Interpolation failed"));
1499     }
1500 
1501     cp = p->kcoefs;      /* This is where the coefs get stored */
1502     for (i=0; i<p->npoles; i++) {
1503       *cp++ = interMagn[i];
1504       *cp++ = interPhas[i];
1505     }
1506     return OK;
1507 }
1508 
1509 
klimit(CSOUND * csound,LIMIT * p)1510 int32_t klimit(CSOUND *csound, LIMIT *p)
1511 {
1512     IGN(csound);
1513     MYFLT       sig=*p->sig, min=*p->min, max=*p->max;
1514     if (LIKELY((sig <= max) && (sig >= min))) {
1515       *p->ans = sig;
1516     }
1517     else {
1518      if ( min >= max) {
1519         *p->ans = FL(0.5) * (min + max);
1520       }
1521       else {
1522         if (sig > max)
1523           *p->ans = max;
1524         else
1525           *p->ans = min;
1526       }
1527     }
1528     return OK;
1529 }
1530 
limit(CSOUND * csound,LIMIT * p)1531 int32_t limit(CSOUND *csound, LIMIT *p)
1532 {
1533     IGN(csound);
1534     MYFLT       *ans, *asig;
1535     MYFLT       min=*p->min, max=*p->max, aver;
1536     uint32_t    offset = p->h.insdshead->ksmps_offset;
1537     uint32_t    early  = p->h.insdshead->ksmps_no_end;
1538     uint32_t    n, nsmps = CS_KSMPS;
1539     ans = p->ans;
1540     asig  = p->sig;
1541 
1542     if (UNLIKELY(offset)) memset(ans, '\0', offset*sizeof(MYFLT));
1543     if (UNLIKELY(early)) {
1544       nsmps -= early;
1545       memset(&ans[nsmps], '\0', early*sizeof(MYFLT));
1546     }
1547     if (min >= max) {
1548       aver = (min + max) * FL(0.5);
1549       for (n=offset; n<nsmps; n++) {
1550         ans[n] = aver;
1551       }
1552     }
1553     else
1554       for (n=offset; n<nsmps; n++) {
1555         if ((asig[n] <= max) && (asig[n] >= min)) {
1556           ans[n] = asig[n];
1557         }
1558         else {
1559           if (asig[n] > max)
1560             ans[n] = max;
1561           else
1562             ans[n] = min;
1563         }
1564       }
1565     return OK;
1566 }
1567