1 /*
2 * Copyright (c) 2018 Fedor Uporov
3 *
4 * Permission is hereby granted, free of charge, to any person obtaining a copy
5 * of this software and associated documentation files (the "Software"), to deal
6 * in the Software without restriction, including without limitation the rights
7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8 * copies of the Software, and to permit persons to whom the Software is
9 * furnished to do so, subject to the following conditions:
10 *
11 * The above copyright notice and this permission notice shall be included in all
12 * copies or substantial portions of the Software.
13 *
14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
20 * SOFTWARE.
21 */
22
23 #pragma once
24
25 #include <cmath>
26 #include <vector>
27 #include <complex>
28 #include <limits>
29 #include <numeric>
30 #include <algorithm>
31 #include <functional>
32
33 namespace OrfanidisEq {
34
35 /*
36 * Just version.
37 */
38 static const char* eq_version = "0.02";
39
40 /*
41 * The float type usage here could be cause the lack of precession.
42 */
43 typedef double eq_double_t;
44
45 /*
46 * Eq configuration constants.
47 * The defaultEqBandPassFiltersOrder should be more then 2.
48 */
49 static const eq_double_t defaultSampleFreqHz = 48000;
50 static const size_t defaultEqBandPassFiltersOrder = 4;
51
52 /* Default frequency values to get frequency grid. */
53 static const eq_double_t lowestGridCenterFreqHz = 31.25;
54 static const eq_double_t bandsGridCenterFreqHz = 1000;
55 static const eq_double_t lowestAudioFreqHz = 20;
56 static const eq_double_t highestAudioFreqHz = 20000;
57
58 /* Default gain values for every type of filter channel. */
59 static const eq_double_t eqGainRangeDb = 40;
60 static const eq_double_t eqGainStepDb = 1;
61 static const eq_double_t eqDefaultGainDb = 0;
62
63 /*
64 * Allow to convert values between different representations.
65 * Also, fast conversions between linear and logarithmic scales are included.
66 */
67 class Conversions {
68 std::vector<eq_double_t> linGains;
69
linGainsIndex(eq_double_t x)70 int linGainsIndex(eq_double_t x)
71 {
72 int inTx = (int)x;
73 int range = linGains.size() / 2;
74
75 if ((x >= -range) && (x < range - 1))
76 return range + inTx;
77
78 return range;
79 }
80
81 Conversions();
82 Conversions(const Conversions&);
83 Conversions& operator= (const Conversions&);
84
85 public:
Conversions(int range)86 Conversions(int range)
87 {
88 int step = -range;
89
90 while (step <= range)
91 linGains.push_back(db2Lin(step++));
92 }
93
fastDb2Lin(eq_double_t x)94 eq_double_t fastDb2Lin(eq_double_t x)
95 {
96 int intPart = x;
97 eq_double_t fractPart = x - intPart;
98
99 return linGains.at(linGainsIndex(intPart)) * (1-fractPart) +
100 linGains.at(linGainsIndex(intPart + 1)) * fractPart;
101 }
102
fastLin2Db(eq_double_t x)103 eq_double_t fastLin2Db(eq_double_t x)
104 {
105 if ((x >= linGains[0]) && (x < linGains[linGains.size() - 1]))
106 for (size_t i = 0; i < linGains.size() - 2; i++)
107 if ((x >= linGains[i]) && (x < linGains[i + 1]))
108 return i - linGains.size() / 2.0 + (x - (int)(x));
109
110 return 0;
111 }
112
db2Lin(eq_double_t x)113 static eq_double_t db2Lin(eq_double_t x)
114 {
115 return pow(10, x/20.0);
116 }
117
lin2Db(eq_double_t x)118 static eq_double_t lin2Db(eq_double_t x)
119 {
120 return 20.0*log10(x);
121 }
122
rad2Hz(eq_double_t x,eq_double_t fs)123 static eq_double_t rad2Hz(eq_double_t x, eq_double_t fs)
124 {
125 return 2.0*M_PI/x*fs;
126 }
127
hz2Rad(eq_double_t x,eq_double_t fs)128 static eq_double_t hz2Rad(eq_double_t x, eq_double_t fs)
129 {
130 return 2.0*M_PI*x/fs;
131 }
132 };
133
134 /*
135 * Filter frequency band representation.
136 */
137 class Band {
138 public:
139 eq_double_t minFreq;
140 eq_double_t centerFreq;
141 eq_double_t maxFreq;
142
Band()143 Band() : minFreq(0), centerFreq(0), maxFreq(0) {}
Band(eq_double_t fl,eq_double_t fc,eq_double_t fh)144 Band(eq_double_t fl, eq_double_t fc, eq_double_t fh)
145 : minFreq(fl), centerFreq(fc), maxFreq(fh) {}
146 };
147
148 /* Basic eq errors handling. */
149 typedef enum {
150 no_error,
151 invalid_input_data_error,
152 processing_error
153 } eq_error_t;
154
155 /*
156 * Frequency grid representation.
157 * Allow to calculate all required bandpass filters frequencies
158 * for equalizer construction.
159 */
160 class FrequencyGrid {
161 std::vector<Band> freqs;
162
163 public:
FrequencyGrid()164 FrequencyGrid() {}
165
setBand(eq_double_t fl,eq_double_t fc,eq_double_t fh)166 eq_error_t setBand(eq_double_t fl, eq_double_t fc, eq_double_t fh)
167 {
168 freqs.clear();
169
170 return addBand(fl, fc, fh);
171 }
172
addBand(eq_double_t fl,eq_double_t fc,eq_double_t fh)173 eq_error_t addBand(eq_double_t fl, eq_double_t fc, eq_double_t fh)
174 {
175 if (fl < fc && fc < fh) {
176 freqs.push_back(Band(fl, fc, fh));
177
178 return no_error;
179 }
180
181 return invalid_input_data_error;
182 }
183
addBand(eq_double_t fc,eq_double_t df)184 eq_error_t addBand(eq_double_t fc, eq_double_t df)
185 {
186 if (fc >= df /2 ) {
187 freqs.push_back(Band(fc - df / 2, fc, fc + df / 2));
188
189 return no_error;
190 }
191
192 return invalid_input_data_error;
193 }
194
195 eq_error_t set5Bands(eq_double_t fc = bandsGridCenterFreqHz)
196 {
197 freqs.clear();
198
199 if (lowestAudioFreqHz < fc && fc < highestAudioFreqHz) {
200 /* Find lowest center frequency in the band. */
201 eq_double_t lowestFc = fc;
202
203 while (lowestFc > lowestGridCenterFreqHz)
204 lowestFc /= 4.0;
205
206 if (lowestFc < lowestGridCenterFreqHz)
207 lowestFc *= 4.0;
208
209 /* Calculate frequencies. */
210 eq_double_t f0 = lowestFc;
211 for (size_t i = 0; i < 5 ; i++) {
212 freqs.push_back(Band(f0 / 2, f0, f0 * 2));
213 f0 *= 4;
214 }
215
216 return no_error;
217 }
218
219 return invalid_input_data_error;
220 }
221
222 eq_error_t set10Bands(eq_double_t fc = bandsGridCenterFreqHz)
223 {
224 freqs.clear();
225
226 if (lowestAudioFreqHz < fc && fc < highestAudioFreqHz) {
227 /* Find lowest center frequency in the band. */
228 eq_double_t lowestFc = fc;
229
230 while (lowestFc > lowestGridCenterFreqHz)
231 lowestFc /= 2;
232
233 if (lowestFc < lowestGridCenterFreqHz)
234 lowestFc *= 2;
235
236 /* Calculate frequencies. */
237 eq_double_t f0 = lowestFc;
238 for (size_t i = 0; i < 10; i++) {
239 freqs.push_back(
240 Band(f0 / pow(2, 0.5), f0, f0 * pow(2, 0.5)));
241
242 f0 *= 2;
243 }
244
245 return no_error;
246 }
247
248 return invalid_input_data_error;
249 }
250
251 eq_error_t set20Bands(eq_double_t fc = bandsGridCenterFreqHz)
252 {
253 freqs.clear();
254
255 if (lowestAudioFreqHz < fc && fc < highestAudioFreqHz) {
256 /* Find lowest center frequency in the band. */
257 eq_double_t lowestFc = fc;
258
259 while (lowestFc >= lowestAudioFreqHz)
260 lowestFc /= pow(2, 0.5);
261
262 if (lowestFc < lowestAudioFreqHz)
263 lowestFc *= pow(2, 0.5);
264
265 /* Calculate frequencies. */
266 eq_double_t f0 = lowestFc;
267 for (size_t i = 0; i < 20; i++) {
268 freqs.push_back(Band(f0 / pow(2, 0.25),
269 f0, f0 * pow(2, 0.25)));
270
271 f0 *= pow(2, 0.5);
272 }
273
274 return no_error;
275 }
276
277 return invalid_input_data_error;
278 }
279
280 eq_error_t set30Bands(eq_double_t fc = bandsGridCenterFreqHz)
281 {
282 freqs.clear();
283
284 if (lowestAudioFreqHz < fc && fc < highestAudioFreqHz) {
285 /* Find lowest center frequency in the band. */
286 eq_double_t lowestFc = fc;
287 while (lowestFc > lowestAudioFreqHz)
288 lowestFc /= pow(2.0, 1.0/3.0);
289
290 if (lowestFc < lowestAudioFreqHz)
291 lowestFc *= pow(2.0, 1.0/3.0);
292
293 /* Calculate frequencies. */
294 eq_double_t f0 = lowestFc;
295 for (size_t i = 0; i < 30; i++) {
296 freqs.push_back(Band(f0 / pow(2.0, 1.0/6.0),
297 f0, f0 * pow(2.0, 1.0/6.0)));
298
299 f0 *= pow(2, 1.0/3.0);
300 }
301
302 return no_error;
303 }
304
305 return invalid_input_data_error;
306 }
307
getNumberOfBands()308 size_t getNumberOfBands()
309 {
310 return freqs.size();
311 }
312
getFreqs()313 std::vector<Band> getFreqs()
314 {
315 return freqs;
316 }
317
getFreq(size_t index)318 size_t getFreq(size_t index)
319 {
320 if (index < freqs.size())
321 return freqs[index].centerFreq;
322 else
323 return 0;
324 }
325
getRoundedFreq(size_t index)326 size_t getRoundedFreq(size_t index)
327 {
328 if (index < freqs.size()) {
329 size_t freq = freqs[index].centerFreq;
330 if (freq < 100) {
331 return freq;
332 } else if (freq < 1000) {
333 size_t rest = freq % 10;
334 if (rest < 5)
335 return freq - rest;
336 else
337 return freq - rest + 10;
338 } else if (freq < 10000) {
339 size_t rest = freq % 100;
340 if (rest < 50)
341 return freq - rest;
342 else
343 return freq - rest + 100;
344 } else if (freq >= 10000) {
345 size_t rest = freq%1000;
346 if (rest < 500)
347 return freq - rest;
348 else
349 return freq - rest + 1000;
350 }
351 }
352
353 return 0;
354 }
355 };
356
357 /*
358 * Second order biquad section representation.
359 */
360 struct SOSection
361 {
362 eq_double_t b0, b1, b2;
363 eq_double_t a0, a1, a2;
364 };
365
366 /*
367 * Fourth order biquad section representation.
368 */
369 class FOSection {
370 protected:
371 eq_double_t b0, b1, b2, b3, b4;
372 eq_double_t a0, a1, a2, a3, a4;
373
374 eq_double_t numBuf[4];
375 eq_double_t denumBuf[4];
376
df1FOProcess(eq_double_t in)377 eq_double_t df1FOProcess(eq_double_t in)
378 {
379 eq_double_t out = 0;
380
381 out+= b0*in;
382 out+= (b1*numBuf[0] - denumBuf[0]*a1);
383 out+= (b2*numBuf[1] - denumBuf[1]*a2);
384 out+= (b3*numBuf[2] - denumBuf[2]*a3);
385 out+= (b4*numBuf[3] - denumBuf[3]*a4);
386
387 numBuf[3] = numBuf[2];
388 numBuf[2] = numBuf[1];
389 numBuf[1] = numBuf[0];
390 *numBuf = in;
391
392 denumBuf[3] = denumBuf[2];
393 denumBuf[2] = denumBuf[1];
394 denumBuf[1] = denumBuf[0];
395 *denumBuf = out;
396
397 return out;
398 }
399
400 public:
FOSection()401 FOSection() : b0(1), b1(0), b2(0), b3(0), b4(0),
402 a0(1), a1(0), a2(0), a3(0), a4(0)
403 {
404 memset(numBuf, 0, sizeof(numBuf));
405 memset(denumBuf, 0, sizeof(denumBuf));
406 }
407
FOSection(std::vector<eq_double_t> & b,std::vector<eq_double_t> a)408 FOSection(std::vector<eq_double_t>& b, std::vector<eq_double_t> a)
409 {
410 memset(numBuf, 0, sizeof(numBuf));
411 memset(denumBuf, 0, sizeof(denumBuf));
412
413 b0 = b[0]; b1 = b[1]; b2 = b[2]; b3 = b[3]; b4 = b[4];
414 a0 = a[0]; a1 = a[1]; a2 = a[2]; a3 = a[3]; a4 = a[4];
415 }
416
process(eq_double_t in)417 eq_double_t process(eq_double_t in)
418 {
419 return df1FOProcess(in);
420 }
421 };
422
423 /*
424 * Bandpass filter representation.
425 */
426 class BPFilter {
427 public:
BPFilter()428 BPFilter() {}
~BPFilter()429 virtual ~BPFilter() {}
430
431 virtual eq_double_t process(eq_double_t in) = 0;
432 };
433
434 class ButterworthBPFilter : public BPFilter {
435 std::vector<FOSection> sections;
436
ButterworthBPFilter()437 ButterworthBPFilter() {}
438 public:
ButterworthBPFilter(ButterworthBPFilter & f)439 ButterworthBPFilter(ButterworthBPFilter& f)
440 {
441 this->sections = f.sections;
442 }
443
ButterworthBPFilter(size_t N,eq_double_t w0,eq_double_t wb,eq_double_t G,eq_double_t Gb)444 ButterworthBPFilter(size_t N,
445 eq_double_t w0, eq_double_t wb,
446 eq_double_t G, eq_double_t Gb)
447 {
448 /* Case if G == 0 : allpass. */
449 if (G == 0) {
450 sections.push_back(FOSection());
451 return;
452 }
453
454 /* Get number of analog sections. */
455 size_t r = N % 2;
456 size_t L = (N - r) / 2;
457
458 /* Convert gains to linear scale. */
459 eq_double_t G0 = Conversions::db2Lin(0.0);
460 G = Conversions::db2Lin(G);
461 Gb = Conversions::db2Lin(Gb);
462
463 eq_double_t e = sqrt((G*G - Gb*Gb) / (Gb*Gb - G0*G0));
464 eq_double_t g = pow(G, 1.0 / N);
465 eq_double_t g0 = pow(G0, 1.0 / N);
466 eq_double_t beta = pow(e, -1.0 / N) * tan(wb / 2.0);
467 eq_double_t c0 = cos(w0);
468
469 /* Calculate every section. */
470 for (size_t i = 1; i <= L; i++) {
471 eq_double_t ui = (2.0 * i - 1) / N;
472 eq_double_t si = sin(M_PI * ui / 2.0);
473 eq_double_t Di = beta*beta + 2*si*beta + 1;
474
475 std::vector<eq_double_t> B;
476 B.push_back((g*g*beta*beta + 2*g*g0*si*beta + g0*g0)/Di);
477 B.push_back(-4*c0*(g0*g0 + g*g0*si*beta)/Di);
478 B.push_back(2*(g0*g0*(1 + 2*c0*c0) - g*g*beta*beta)/Di);
479 B.push_back(-4*c0*(g0*g0 - g*g0*si*beta)/Di);
480 B.push_back((g*g*beta*beta - 2*g*g0*si*beta + g0*g0)/Di);
481
482 std::vector<eq_double_t> A;
483 A.push_back(1);
484 A.push_back(-4*c0*(1 + si*beta)/Di);
485 A.push_back(2*(1 + 2*c0*c0 - beta*beta)/Di);
486 A.push_back(-4*c0*(1 - si*beta)/Di);
487 A.push_back((beta*beta - 2*si*beta + 1)/Di);
488
489 sections.push_back(FOSection(B, A));
490 }
491 }
492
~ButterworthBPFilter()493 ~ButterworthBPFilter() {}
494
computeBWGainDb(eq_double_t gain)495 static eq_double_t computeBWGainDb(eq_double_t gain)
496 {
497 eq_double_t bwGain = 0;
498 if (gain < -3)
499 bwGain = gain + 3;
500 else if (gain >= -3 && gain < 3)
501 bwGain = gain / sqrt(2);
502 else if (gain >= 3)
503 bwGain = gain - 3;
504
505 return bwGain;
506 }
507
process(eq_double_t in)508 virtual eq_double_t process(eq_double_t in)
509 {
510 eq_double_t p0 = in, p1 = 0;
511
512 /* Process FO sections in serial connection. */
513 for (size_t i = 0; i < sections.size(); i++) {
514 p1 = sections[i].process(p0);
515 p0 = p1;
516 }
517
518 return p1;
519 }
520 };
521
522 class ChebyshevType1BPFilter : public BPFilter {
523 std::vector<FOSection> sections;
524
ChebyshevType1BPFilter()525 ChebyshevType1BPFilter() {}
526 public:
ChebyshevType1BPFilter(size_t N,eq_double_t w0,eq_double_t wb,eq_double_t G,eq_double_t Gb)527 ChebyshevType1BPFilter(size_t N,
528 eq_double_t w0, eq_double_t wb,
529 eq_double_t G, eq_double_t Gb)
530 {
531 /* Case if G == 0 : allpass. */
532 if(G == 0) {
533 sections.push_back(FOSection());
534 return;
535 }
536
537 /* Get number of analog sections. */
538 size_t r = N % 2;
539 size_t L = (N - r) / 2;
540
541 /* Convert gains to linear scale. */
542 eq_double_t G0 = Conversions::db2Lin(0.0);
543 G = Conversions::db2Lin(G);
544 Gb = Conversions::db2Lin(Gb);
545
546 eq_double_t e = sqrt((G*G - Gb*Gb) / (Gb*Gb - G0*G0));
547 eq_double_t g0 = pow(G0, 1.0 / N);
548 eq_double_t alfa = pow(1.0 / e + pow(1 + pow(e, -2.0), 0.5), 1.0 / N);
549 eq_double_t beta = pow(G / e + Gb*pow(1 + pow(e, -2.0), 0.5),1.0 / N);
550 eq_double_t a = 0.5 * (alfa - 1.0 / alfa);
551 eq_double_t b = 0.5*(beta - g0*g0*(1 / beta));
552 eq_double_t tetta_b = tan(wb / 2);
553 eq_double_t c0 = cos(w0);
554
555 /* Calculate every section. */
556 for (size_t i = 1; i <= L; i++) {
557 eq_double_t ui = (2.0*i - 1.0) / N;
558 eq_double_t ci = cos(M_PI * ui / 2.0);
559 eq_double_t si = sin(M_PI * ui / 2.0);
560 eq_double_t Di = (a*a + ci*ci) * tetta_b * tetta_b +
561 2.0 * a * si * tetta_b + 1;
562
563 std::vector<eq_double_t> B;
564 B.push_back(((b*b + g0*g0*ci*ci)*tetta_b*tetta_b + 2*g0*b*si*tetta_b + g0*g0)/Di);
565 B.push_back(-4*c0*(g0*g0 + g0*b*si*tetta_b)/Di);
566 B.push_back(2*(g0*g0*(1 + 2*c0*c0) - (b*b + g0*g0*ci*ci)*tetta_b*tetta_b)/Di);
567 B.push_back(-4*c0*(g0*g0 - g0*b*si*tetta_b)/Di);
568 B.push_back(((b*b + g0*g0*ci*ci)*tetta_b*tetta_b - 2*g0*b*si*tetta_b + g0*g0)/Di);
569
570 std::vector<eq_double_t> A;
571 A.push_back(1);
572 A.push_back(-4*c0*(1 + a*si*tetta_b)/Di);
573 A.push_back(2*(1 + 2*c0*c0 - (a*a + ci*ci)*tetta_b*tetta_b)/Di);
574 A.push_back(-4*c0*(1 - a*si*tetta_b)/Di);
575 A.push_back(((a*a + ci*ci)*tetta_b*tetta_b - 2*a*si*tetta_b + 1)/Di);
576
577 sections.push_back(FOSection(B, A));
578 }
579 }
580
~ChebyshevType1BPFilter()581 ~ChebyshevType1BPFilter() {}
582
computeBWGainDb(eq_double_t gain)583 static eq_double_t computeBWGainDb(eq_double_t gain)
584 {
585 eq_double_t bwGain = 0;
586 if (gain < 0)
587 bwGain = gain + 0.1;
588 else
589 bwGain = gain - 0.1;
590
591 return bwGain;
592 }
593
process(eq_double_t in)594 eq_double_t process(eq_double_t in)
595 {
596 eq_double_t p0 = in, p1 = 0;
597
598 /* Process FO sections in serial connection. */
599 for (size_t i = 0; i < sections.size(); i++) {
600 p1 = sections[i].process(p0);
601 p0 = p1;
602 }
603
604 return p1;
605 }
606 };
607
608 class ChebyshevType2BPFilter : public BPFilter {
609 std::vector<FOSection> sections;
610
ChebyshevType2BPFilter()611 ChebyshevType2BPFilter() {}
612 public:
ChebyshevType2BPFilter(size_t N,eq_double_t w0,eq_double_t wb,eq_double_t G,eq_double_t Gb)613 ChebyshevType2BPFilter(size_t N,
614 eq_double_t w0, eq_double_t wb,
615 eq_double_t G, eq_double_t Gb)
616 {
617 /* Case if G == 0 : allpass. */
618 if (G == 0) {
619 sections.push_back(FOSection());
620 return;
621 }
622
623 /* Get number of analog sections. */
624 size_t r = N % 2;
625 size_t L = (N - r) / 2;
626
627 /* Convert gains to linear scale. */
628 eq_double_t G0 = Conversions::db2Lin(0.0);
629 G = Conversions::db2Lin(G);
630 Gb = Conversions::db2Lin(Gb);
631
632 eq_double_t e = sqrt((G*G - Gb*Gb) / (Gb*Gb - G0*G0));
633 eq_double_t g = pow(G, 1.0 / N);
634 eq_double_t eu = pow(e + sqrt(1 + e*e), 1.0 / N);
635 eq_double_t ew = pow(G0*e + Gb*sqrt(1 + e*e), 1.0 / N);
636 eq_double_t a = (eu - 1.0 / eu) / 2.0;
637 eq_double_t b = (ew - g*g / ew) / 2.0;
638 eq_double_t tetta_b = tan(wb / 2);
639 eq_double_t c0 = cos(w0);
640
641 /* Calculate every section. */
642 for (size_t i = 1; i <= L; i++) {
643 eq_double_t ui = (2.0 * i - 1.0) / N;
644 eq_double_t ci = cos(M_PI * ui / 2.0);
645 eq_double_t si = sin(M_PI * ui / 2.0);
646 eq_double_t Di = tetta_b*tetta_b + 2*a*si*tetta_b +
647 a*a + ci*ci;
648
649 std::vector<eq_double_t> B;
650 B.push_back((g*g*tetta_b*tetta_b + 2*g*b*si*tetta_b + b*b + g*g*ci*ci)/Di);
651 B.push_back(-4*c0*(b*b + g*g*ci*ci + g*b*si*tetta_b)/Di);
652 B.push_back(2*((b*b + g*g*ci*ci)*(1 + 2*c0*c0) - g*g*tetta_b*tetta_b)/Di);
653 B.push_back(-4*c0*(b*b + g*g*ci*ci - g*b*si*tetta_b)/Di);
654 B.push_back((g*g*tetta_b*tetta_b - 2*g*b*si*tetta_b + b*b + g*g*ci*ci)/Di);
655
656 std::vector<eq_double_t> A;
657 A.push_back(1);
658 A.push_back(-4*c0*(a*a + ci*ci + a*si*tetta_b)/Di);
659 A.push_back(2*((a*a + ci*ci)*(1 + 2*c0*c0) - tetta_b*tetta_b)/Di);
660 A.push_back(-4*c0*(a*a + ci*ci - a*si*tetta_b)/Di);
661 A.push_back((tetta_b*tetta_b - 2*a*si*tetta_b + a*a + ci*ci)/Di);
662
663 sections.push_back(FOSection(B, A));
664 }
665 }
666
~ChebyshevType2BPFilter()667 ~ChebyshevType2BPFilter(){}
668
computeBWGainDb(eq_double_t gain)669 static eq_double_t computeBWGainDb(eq_double_t gain)
670 {
671 eq_double_t bwGain = 0;
672 if (gain < 0)
673 bwGain = -1;
674 else
675 bwGain = 1;
676
677 return bwGain;
678 }
679
process(eq_double_t in)680 eq_double_t process(eq_double_t in)
681 {
682 eq_double_t p0 = in, p1 = 0;
683
684 /* Process FO sections in serial connection. */
685 for (size_t i = 0; i < sections.size(); i++) {
686 p1 = sections[i].process(p0);
687 p0 = p1;
688 }
689
690 return p1;
691 }
692 };
693
694 class EllipticTypeBPFilter : public BPFilter {
695 private:
696 /* complex -1. */
697 std::complex<eq_double_t> j;
698 std::vector<FOSection> sections;
699
EllipticTypeBPFilter()700 EllipticTypeBPFilter() {}
701
arccos(std::complex<eq_double_t> z)702 std::complex<eq_double_t> arccos(std::complex<eq_double_t> z)
703 {
704 return -j*log(z + sqrt(z*z - 1.0));
705 }
706
707 /*
708 * Landen transformations of an elliptic modulus.
709 */
landen(eq_double_t k,eq_double_t tol)710 std::vector<eq_double_t> landen(eq_double_t k, eq_double_t tol)
711 {
712 std::vector<eq_double_t> v;
713
714 if (k == 0 || k == 1.0)
715 v.push_back(k);
716
717 if (tol < 1) {
718 while (k > tol) {
719 k = pow(k/(1.0 + sqrt(1.0 - k*k)), 2);
720 v.push_back(k);
721 }
722 } else {
723 eq_double_t M = tol;
724 for (size_t i = 1; i <= M; i++) {
725 k = pow(k/(1.0 + sqrt(1.0 - k*k)), 2);
726 v.push_back(k);
727 }
728 }
729
730 return v;
731 }
732
733 /*
734 * Complete elliptic integral.
735 */
ellipk(eq_double_t k,eq_double_t tol,eq_double_t & K,eq_double_t & Kprime)736 void ellipk(eq_double_t k, eq_double_t tol, eq_double_t& K, eq_double_t& Kprime)
737 {
738 eq_double_t kmin = 1e-6;
739 eq_double_t kmax = sqrt(1 - kmin*kmin);
740
741 if (k == 1.0) {
742 K = std::numeric_limits<eq_double_t>::infinity();
743 } else if (k > kmax) {
744 eq_double_t kp = sqrt(1.0 - k*k);
745 eq_double_t L = -log(kp / 4.0);
746 K = L + (L - 1) * kp*kp / 4.0;
747 } else {
748 std::vector<eq_double_t> v = landen(k, tol);
749
750 std::transform(v.begin(), v.end(), v.begin(),
751 bind2nd(std::plus<eq_double_t>(), 1.0));
752
753 K = std::accumulate(v.begin(), v.end(),
754 1, std::multiplies<eq_double_t>()) * M_PI/2.0;
755 }
756
757 if (k == 0.0) {
758 Kprime = std::numeric_limits<eq_double_t>::infinity();
759 } else if (k < kmin) {
760 eq_double_t L = -log(k / 4.0);
761 Kprime = L + (L - 1.0) * k*k / 4.0;
762 } else {
763 eq_double_t kp = sqrt(1.0 - k*k);
764 std::vector<eq_double_t> vp = landen(kp, tol);
765
766 std::transform(vp.begin(), vp.end(), vp.begin(),
767 bind2nd(std::plus<eq_double_t>(), 1.0));
768
769 Kprime = std::accumulate(vp.begin(), vp.end(),
770 1.0, std::multiplies<eq_double_t>()) * M_PI/2.0;
771 }
772 }
773
774 /*
775 * Solves the degree equation in analog elliptic filter design.
776 */
ellipdeg2(eq_double_t n,eq_double_t k,eq_double_t tol)777 eq_double_t ellipdeg2(eq_double_t n, eq_double_t k, eq_double_t tol)
778 {
779 const size_t M = 7;
780 eq_double_t K, Kprime;
781
782 ellipk(k, tol, K, Kprime);
783 eq_double_t q = exp(-M_PI * Kprime / K);
784 eq_double_t q1 = pow(q, n);
785 eq_double_t s1 = 0, s2 = 0;
786
787 for (size_t i = 1; i <= M; i++)
788 {
789 s1 += pow(q1, i*(i+1));
790 s2 += pow(q1, i*i);
791 }
792
793 return 4 * sqrt(q1) * pow((1.0 + s1) / (1.0 + 2 * s2), 2);
794 }
795
srem(eq_double_t x,eq_double_t y)796 eq_double_t srem(eq_double_t x, eq_double_t y)
797 {
798 eq_double_t z = remainder(x, y);
799
800 return z - y * copysign(1.0, z) * ((eq_double_t)(abs(z) > y / 2.0));
801 }
802
803 /*
804 * Inverse of cd elliptic function.
805 */
acde(std::complex<eq_double_t> w,eq_double_t k,eq_double_t tol)806 std::complex<eq_double_t> acde(std::complex<eq_double_t> w, eq_double_t k,
807 eq_double_t tol)
808 {
809 std::vector<eq_double_t> v = landen(k, tol);
810
811 for (size_t i = 0; i < v.size(); i++) {
812 eq_double_t v1;
813 if (i == 0)
814 v1 = k;
815 else
816 v1 = v[i - 1];
817
818 w = w / (1.0 + sqrt(1.0 - w*w * v1*v1)) * 2.0/(1 + v[i]);
819 }
820
821 std::complex<eq_double_t> u = 2.0 / M_PI * arccos(w);
822 eq_double_t K, Kprime;
823 ellipk(k ,tol, K, Kprime);
824
825 return srem(real(u), 4) + j*srem(imag(u), 2*(Kprime/K));
826 }
827
828 /*
829 * Inverse of sn elliptic function.
830 */
asne(std::complex<eq_double_t> w,eq_double_t k,eq_double_t tol)831 std::complex<eq_double_t> asne(std::complex<eq_double_t> w, eq_double_t k,
832 eq_double_t tol)
833 {
834 return 1.0 - acde(w, k, tol);
835 }
836
837 /*
838 * cd elliptic function with normalized complex argument.
839 */
cde(std::complex<eq_double_t> u,eq_double_t k,eq_double_t tol)840 std::complex<eq_double_t> cde(std::complex<eq_double_t> u, eq_double_t k,
841 eq_double_t tol)
842 {
843 std::vector<eq_double_t> v = landen(k, tol);
844 std::complex<eq_double_t> w = cos(u * M_PI / 2.0);
845
846 for (int i = v.size() - 1; i >= 0; i--)
847 w = (1 + v[i]) * w / (1.0 + v[i] * pow(w, 2));
848
849 return w;
850 }
851
852 /*
853 * sn elliptic function with normalized complex argument.
854 */
sne(const std::vector<eq_double_t> & u,eq_double_t k,eq_double_t tol)855 std::vector<eq_double_t> sne(const std::vector<eq_double_t> &u,
856 eq_double_t k, eq_double_t tol)
857 {
858 std::vector<eq_double_t> v = landen(k, tol);
859 std::vector<eq_double_t> w;
860
861 for (size_t i = 0; i < u.size(); i++)
862 w.push_back(sin(u[i] * M_PI / 2.0));
863
864 for (int i = v.size() - 1; i >= 0; i--)
865 for (size_t j = 0; j < w.size(); j++)
866 w[j] = ((1 + v[i])*w[j])/(1 + v[i]*w[j]*w[j]);
867
868 return w;
869 }
870
871 /*
872 * Solves the degree equation in analog elliptic filter design.
873 */
ellipdeg(size_t N,eq_double_t k1,eq_double_t tol)874 eq_double_t ellipdeg(size_t N, eq_double_t k1, eq_double_t tol)
875 {
876 eq_double_t L = floor(N / 2);
877 std::vector<eq_double_t> ui;
878 for (size_t i = 1; i <= L; i++)
879 ui.push_back((2.0*i - 1.0) / N);
880
881 eq_double_t kmin = 1e-6;
882 if (k1 < kmin) {
883 return ellipdeg2(1.0 / N, k1, tol);
884 } else {
885 eq_double_t kc = sqrt(1 - k1*k1);
886 std::vector<eq_double_t> w = sne(ui, kc, tol);
887 eq_double_t prod = std::accumulate(w.begin(), w.end(),
888 1.0, std::multiplies<eq_double_t>());
889 eq_double_t kp = pow(kc, N) * pow(prod, 4);
890
891 return sqrt(1 - kp*kp);
892 }
893 }
894
895 /*
896 * Bilinear transformation of analog second-order sections.
897 */
blt(const std::vector<SOSection> & aSections,eq_double_t w0,std::vector<FOSection> & sections)898 void blt(const std::vector<SOSection>& aSections, eq_double_t w0,
899 std::vector<FOSection>& sections)
900 {
901 eq_double_t c0 = cos(w0);
902 size_t K = aSections.size();
903
904 std::vector<std::vector<eq_double_t> > B, A, Bhat, Ahat;
905 for (size_t i = 0; i < K; i++) {
906 B.push_back(std::vector<eq_double_t>(5));
907 A.push_back(std::vector<eq_double_t>(5));
908 Bhat.push_back(std::vector<eq_double_t>(3));
909 Ahat.push_back(std::vector<eq_double_t>(3));
910 }
911
912 std::vector<eq_double_t> B0(3), B1(3), B2(3), A0(3), A1(3), A2(3);
913 B0[0] = aSections[0].b0; B0[1] = aSections[1].b0; B0[2] = aSections[2].b0;
914 B1[0] = aSections[0].b1; B1[1] = aSections[1].b1; B1[2] = aSections[2].b1;
915 B2[0] = aSections[0].b2; B2[1] = aSections[1].b2; B2[2] = aSections[2].b2;
916 A0[0] = aSections[0].a0; A0[1] = aSections[1].a0; A0[2] = aSections[2].a0;
917 A1[0] = aSections[0].a1; A1[1] = aSections[1].a1; A1[2] = aSections[2].a1;
918 A2[0] = aSections[0].a2; A2[1] = aSections[1].a2; A2[2] = aSections[2].a2;
919
920 /* Find 0th-order sections (i.e., gain sections). */
921 std::vector<size_t> zths;
922 for (size_t i = 0; i < B0.size(); i++)
923 if ((B1[i] == 0 && A1[i] == 0) && (B2[i] == 0 && A2[i] == 0))
924 zths.push_back(i);
925
926 for (size_t i = 0; i < zths.size(); i++) {
927 size_t j = zths[i];
928 Bhat[j][0] = B0[j] / A0[j];
929 Ahat[j][0] = 1;
930 B[j][0] = Bhat[j][0];
931 A[j][0] = 1;
932 }
933
934 /* Find 1st-order analog sections. */
935 std::vector<size_t> fths;
936 for (size_t i = 0; i < B0.size(); i++)
937 if ((B1[i] != 0 || A1[i] != 0) && (B2[i] == 0 && A2[i] == 0))
938 fths.push_back(i);
939
940 for (size_t i = 0; i < fths.size(); i++) {
941 size_t j = fths[i];
942 eq_double_t D = A0[j] + A1[j];
943 Bhat[j][0] = (B0[j] + B1[j]) / D;
944 Bhat[j][1] = (B0[j] - B1[j]) / D;
945 Ahat[j][0] = 1;
946 Ahat[j][1] = (A0[j] - A1[j]) / D;
947
948 B[j][0] = Bhat[j][0];
949 B[j][1] = c0 * (Bhat[j][1] - Bhat[j][0]);
950 B[j][2] = -Bhat[j][1];
951 A[j][0] = 1;
952 A[j][1] = c0 * (Ahat[j][1] - 1);
953 A[j][2] = -Ahat[j][1];
954 }
955
956 /* Find 2nd-order sections. */
957 std::vector<size_t> sths;
958 for (size_t i = 0; i < B0.size(); i++)
959 if (B2[i] != 0 || A2[i] != 0)
960 sths.push_back(i);
961
962 for (size_t i = 0; i < sths.size(); i++) {
963 size_t j = sths[i];
964 eq_double_t D = A0[j] + A1[j] + A2[j];
965 Bhat[j][0] = (B0[j] + B1[j] + B2[j]) / D;
966 Bhat[j][1] = 2 * (B0[j] - B2[j]) / D;
967 Bhat[j][2] = (B0[j] - B1[j] + B2[j]) / D;
968 Ahat[j][0] = 1;
969 Ahat[j][1] = 2 * (A0[j] - A2[j]) / D;
970 Ahat[j][2] = (A0[j] - A1[j] + A2[j]) /D;
971
972 B[j][0] = Bhat[j][0];
973 B[j][1] = c0 * (Bhat[j][1] - 2 * Bhat[j][0]);
974 B[j][2] = (Bhat[j][0] - Bhat[j][1] + Bhat[j][2]) *c0*c0 - Bhat[j][1];
975 B[j][3] = c0 * (Bhat[j][1] - 2 * Bhat[j][2]);
976 B[j][4] = Bhat[j][2];
977
978 A[j][0] = 1;
979 A[j][1] = c0 * (Ahat[j][1] - 2);
980 A[j][2] = (1 - Ahat[j][1] + Ahat[j][2])*c0*c0 - Ahat[j][1];
981 A[j][3] = c0 * (Ahat[j][1] - 2*Ahat[j][2]);
982 A[j][4] = Ahat[j][2];
983 }
984
985 /* LP or HP shelving filter. */
986 if (c0 == 1 || c0 == -1) {
987 for (size_t i = 0; i < Bhat.size(); i++) {
988 B[i] = Bhat[i];
989 A[i] = Ahat[i];
990 }
991
992 for (size_t i = 0; i < B.size(); i++) {
993 B[i][1] *= c0;
994 A[i][1] *= c0;
995
996 B[i][3] = 0; B[i][4] = 0;
997 A[i][3] = 0; A[i][4] = 0;
998 }
999 }
1000
1001 for (size_t i = 0; i < B.size(); i++)
1002 sections.push_back(FOSection(B[i], A[i]));
1003 }
1004
1005 public:
EllipticTypeBPFilter(size_t N,eq_double_t w0,eq_double_t wb,eq_double_t G,eq_double_t Gb)1006 EllipticTypeBPFilter(size_t N,
1007 eq_double_t w0, eq_double_t wb,
1008 eq_double_t G, eq_double_t Gb) :
1009 j(std::complex<long double>(0.0L, 1.0L))
1010 {
1011 /* Case if G == 0 : allpass. */
1012 if(G == 0) {
1013 sections.push_back(FOSection());
1014 return;
1015 }
1016
1017 const eq_double_t tol = 2.2e-16;
1018 eq_double_t Gs = G - Gb;
1019
1020 /* Get number of analog sections. */
1021 size_t r = N % 2;
1022 size_t L = (N - r) / 2;
1023
1024 /* Convert gains to linear scale. */
1025 eq_double_t G0 = Conversions::db2Lin(0.0);
1026 G = Conversions::db2Lin(G);
1027 Gb = Conversions::db2Lin(Gb);
1028 Gs = Conversions::db2Lin(Gs);
1029
1030 eq_double_t WB = tan(wb / 2.0);
1031 eq_double_t e = sqrt((G*G - Gb*Gb) / (Gb*Gb - G0*G0));
1032 eq_double_t es = sqrt((G*G - Gs*Gs) / (Gs*Gs - G0*G0));
1033 eq_double_t k1 = e / es;
1034 eq_double_t k = ellipdeg(N, k1, tol);
1035 std::complex<eq_double_t> ju0 = asne(j*G / e / G0, k1, tol) / (eq_double_t)N;
1036 std::complex<eq_double_t> jv0 = asne(j / e, k1, tol) / (eq_double_t)N;
1037
1038 /* Initial initialization of analog sections. */
1039 std::vector<SOSection> aSections;
1040 if (r == 0) {
1041 SOSection ba = {Gb, 0, 0, 1, 0, 0};
1042 aSections.push_back(ba);
1043 } else if (r == 1) {
1044 eq_double_t A00, A01, B00, B01;
1045 if (G0 == 0.0 && G != 0.0) {
1046 B00 = G * WB;
1047 B01 = 0.0;
1048 } else {
1049 eq_double_t z0 = std::real(j* cde(-1.0 + ju0, k, tol));
1050 B00 = G * WB;
1051 B01 = -G / z0;
1052 }
1053 A00 = WB;
1054 A01 = -1 / std::real(j * cde(-1.0 + jv0, k, tol));
1055 SOSection ba = {B00, B01, 0, A00, A01, 0};
1056 aSections.push_back(ba);
1057 }
1058
1059 if (L > 0) {
1060 for (size_t i = 1; i <= L; i++) {
1061 eq_double_t ui = (2.0 * i - 1) / N;
1062 std::complex<eq_double_t> poles, zeros;
1063
1064 if (G0 == 0.0 && G != 0.0)
1065 zeros = j / (k * cde(ui, k, tol));
1066 else if (G0 != 0.0 && G == 0.0)
1067 zeros = j * cde(ui, k, tol);
1068 else
1069 zeros = j * cde(ui - ju0, k ,tol);
1070
1071 poles = j * cde(ui - jv0, k, tol);
1072
1073 SOSection sa = {
1074 WB*WB, -2*WB*std::real(1.0/zeros), pow(abs(1.0/zeros), 2),
1075 WB*WB, -2*WB*std::real(1.0/poles), pow(abs(1.0/poles), 2)};
1076
1077 aSections.push_back(sa);
1078 }
1079 }
1080
1081 blt(aSections, w0, sections);
1082
1083 }
1084
~EllipticTypeBPFilter()1085 ~EllipticTypeBPFilter(){}
1086
computeBWGainDb(eq_double_t gain)1087 static eq_double_t computeBWGainDb(eq_double_t gain)
1088 {
1089 eq_double_t bwGain = 0;
1090 if (gain < 0)
1091 bwGain = gain + 0.05;
1092 else
1093 bwGain = gain - 0.05;
1094
1095 return bwGain;
1096 }
1097
process(eq_double_t in)1098 eq_double_t process(eq_double_t in)
1099 {
1100 eq_double_t p0 = in, p1 = 0;
1101
1102 /* Process FO sections in serial connection. */
1103 for (size_t i = 0; i < sections.size(); i++) {
1104 p1 = sections[i].process(p0);
1105 p0 = p1;
1106 }
1107
1108 return p1;
1109 }
1110 };
1111
1112 /*
1113 * The next filter types are supported.
1114 */
1115 typedef enum {
1116 none,
1117 butterworth,
1118 chebyshev1,
1119 chebyshev2,
1120 elliptic
1121 } filter_type;
1122
1123 /*
1124 * Representation of single precomputed equalizer channel
1125 * contain vector of filters for every band gain value.
1126 */
1127 class EqChannel {
1128 eq_double_t f0;
1129 eq_double_t fb;
1130 eq_double_t samplingFrequency;
1131 eq_double_t gainRangeDb;
1132 eq_double_t gainStepDb;
1133
1134 size_t currentFilterIndex;
1135 eq_double_t currentGainDb;
1136
1137 std::vector<BPFilter*> filters;
1138 filter_type currentChannelType;
1139
EqChannel()1140 EqChannel() {}
1141
getFltIndex(eq_double_t gainDb)1142 size_t getFltIndex(eq_double_t gainDb)
1143 {
1144 size_t numberOfFilters = filters.size();
1145 eq_double_t scaleCoef = gainDb / gainRangeDb;
1146
1147 return (numberOfFilters / 2) + (numberOfFilters / 2) * scaleCoef;
1148 }
1149
cleanupFiltersArray()1150 void cleanupFiltersArray()
1151 {
1152 for(size_t j = 0; j < filters.size(); j++)
1153 delete filters[j];
1154 }
1155
1156 public:
1157 EqChannel(filter_type ft,
1158 eq_double_t fs, eq_double_t f0, eq_double_t fb,
1159 eq_double_t gainRangeDb = eqGainRangeDb,
1160 eq_double_t gainStepDb = eqGainStepDb)
1161 {
1162 samplingFrequency = fs;
1163 this->f0 = f0;
1164 this->fb = fb;
1165 this->gainRangeDb = gainRangeDb;
1166 this->gainStepDb = gainStepDb;
1167 currentGainDb = 0;
1168 currentFilterIndex = 0;
1169 currentChannelType = ft;
1170
1171 setChannel(currentChannelType, samplingFrequency);
1172 }
1173
~EqChannel()1174 ~EqChannel()
1175 {
1176 cleanupFiltersArray();
1177 }
1178
setChannel(filter_type ft,eq_double_t fs)1179 eq_error_t setChannel(filter_type ft, eq_double_t fs)
1180 {
1181 (void)fs;
1182
1183 eq_double_t wb = Conversions::hz2Rad(fb, samplingFrequency);
1184 eq_double_t w0 = Conversions::hz2Rad(f0, samplingFrequency);
1185
1186 for (eq_double_t gain = -gainRangeDb; gain <= gainRangeDb;
1187 gain+= gainStepDb) {
1188 switch(ft) {
1189 case (butterworth): {
1190 eq_double_t bw_gain =
1191 ButterworthBPFilter::computeBWGainDb(gain);
1192
1193 ButterworthBPFilter* bf =
1194 new ButterworthBPFilter(
1195 defaultEqBandPassFiltersOrder, w0,
1196 wb, gain, bw_gain);
1197
1198 filters.push_back(bf);
1199 break;
1200 }
1201
1202 case (chebyshev1): {
1203 eq_double_t bwGain =
1204 ChebyshevType1BPFilter::computeBWGainDb(gain);
1205
1206 ChebyshevType1BPFilter* cf1 =
1207 new ChebyshevType1BPFilter(
1208 defaultEqBandPassFiltersOrder, w0,
1209 wb, gain, bwGain);
1210
1211 filters.push_back(cf1);
1212 break;
1213 }
1214
1215 case (chebyshev2): {
1216 eq_double_t bwGain =
1217 ChebyshevType2BPFilter::computeBWGainDb(gain);
1218
1219 ChebyshevType2BPFilter* cf2 =
1220 new ChebyshevType2BPFilter(
1221 defaultEqBandPassFiltersOrder, w0,
1222 wb, gain, bwGain);
1223
1224 filters.push_back(cf2);
1225 break;
1226 }
1227
1228 case (elliptic): {
1229 eq_double_t bwGain =
1230 EllipticTypeBPFilter::computeBWGainDb(gain);
1231
1232 EllipticTypeBPFilter* e =
1233 new EllipticTypeBPFilter(
1234 defaultEqBandPassFiltersOrder, w0,
1235 wb, gain, bwGain);
1236
1237 filters.push_back(e);
1238 break;
1239 }
1240
1241 default: {
1242 currentChannelType = none;
1243 return invalid_input_data_error;
1244 }
1245 }
1246 }
1247
1248 /* Get current filter index. */
1249 currentGainDb = 0;
1250 currentFilterIndex = getFltIndex(currentGainDb);
1251
1252 return no_error;
1253 }
1254
setGainDb(eq_double_t db)1255 eq_error_t setGainDb(eq_double_t db)
1256 {
1257 if (db > -gainRangeDb && db < gainRangeDb) {
1258 currentGainDb = db;
1259 currentFilterIndex = getFltIndex(db);
1260
1261 return no_error;
1262 }
1263
1264 return invalid_input_data_error;
1265 }
1266
SBSProcess(eq_double_t * in,eq_double_t * out)1267 eq_error_t SBSProcess(eq_double_t *in, eq_double_t *out)
1268 {
1269 *out = filters[currentFilterIndex]->process(*in);
1270
1271 return no_error;
1272 }
1273 };
1274
getFilterName(filter_type type)1275 static const char *getFilterName(filter_type type)
1276 {
1277 switch(type) {
1278 case butterworth:
1279 return "butterworth";
1280 case chebyshev1:
1281 return "chebyshev1";
1282 case chebyshev2:
1283 return "chebyshev2";
1284 case elliptic:
1285 return "elliptic";
1286 default:
1287 return "none";
1288 }
1289 }
1290
1291 /*
1292 * Main equalizer class.
1293 */
1294 class Eq {
1295 Conversions conv;
1296 eq_double_t samplingFrequency;
1297 FrequencyGrid freqGrid;
1298 std::vector<EqChannel*> channels;
1299 filter_type currentEqType;
1300
cleanupChannelsArray()1301 void cleanupChannelsArray()
1302 {
1303 for(size_t j = 0; j < channels.size(); j++)
1304 delete channels[j];
1305 }
1306
1307 public:
Eq(FrequencyGrid & fg,filter_type eq_t)1308 Eq(FrequencyGrid &fg, filter_type eq_t) : conv(46)
1309 {
1310 samplingFrequency = defaultSampleFreqHz;
1311 freqGrid = fg;
1312 currentEqType = eq_t;
1313 setEq(freqGrid, currentEqType);
1314 }
1315
~Eq()1316 ~Eq()
1317 {
1318 cleanupChannelsArray();
1319 }
1320
setEq(const FrequencyGrid & fg,filter_type ft)1321 eq_error_t setEq(const FrequencyGrid& fg, filter_type ft)
1322 {
1323 cleanupChannelsArray();
1324 channels.clear();
1325
1326 freqGrid = fg;
1327 currentEqType = ft;
1328
1329 for (size_t i = 0; i < freqGrid.getNumberOfBands(); i++) {
1330 Band bFres = freqGrid.getFreqs()[i];
1331
1332 EqChannel* eq_ch = new EqChannel(ft, samplingFrequency,
1333 bFres.centerFreq, bFres.maxFreq - bFres.minFreq);
1334
1335 channels.push_back(eq_ch);
1336 channels[i]->setGainDb(eqDefaultGainDb);
1337 }
1338
1339 return no_error;
1340 }
1341
setEq(filter_type ft)1342 eq_error_t setEq(filter_type ft)
1343 {
1344 return setEq(freqGrid, ft);
1345 }
1346
setSampleRate(eq_double_t sr)1347 eq_error_t setSampleRate(eq_double_t sr)
1348 {
1349 samplingFrequency = sr;
1350
1351 return setEq(currentEqType);
1352 }
1353
changeGains(const std::vector<eq_double_t> & bandGains)1354 eq_error_t changeGains(const std::vector<eq_double_t>& bandGains)
1355 {
1356 if (channels.size() == bandGains.size())
1357 for(size_t j = 0; j < channels.size(); j++)
1358 channels[j]->setGainDb(conv.fastLin2Db(bandGains[j]));
1359 else
1360 return invalid_input_data_error;
1361
1362 return no_error;
1363 }
1364
changeGainsDb(const std::vector<eq_double_t> & bandGains)1365 eq_error_t changeGainsDb(const std::vector<eq_double_t>& bandGains)
1366 {
1367 if (channels.size() == bandGains.size())
1368 for(size_t j = 0; j < channels.size(); j++)
1369 channels[j]->setGainDb(bandGains[j]);
1370 else
1371 return invalid_input_data_error;
1372
1373 return no_error;
1374 }
1375
changeBandGain(size_t bandNumber,eq_double_t bandGain)1376 eq_error_t changeBandGain(size_t bandNumber, eq_double_t bandGain)
1377 {
1378 if (bandNumber < channels.size())
1379 channels[bandNumber]->setGainDb(conv.fastLin2Db(bandGain));
1380 else
1381 return invalid_input_data_error;
1382
1383 return no_error;
1384 }
1385
changeBandGainDb(size_t bandNumber,eq_double_t bandGain)1386 eq_error_t changeBandGainDb(size_t bandNumber, eq_double_t bandGain)
1387 {
1388 if (bandNumber < channels.size())
1389 channels[bandNumber]->setGainDb(bandGain);
1390 else
1391 return invalid_input_data_error;
1392
1393 return no_error;
1394 }
1395
SBSProcessBand(size_t bandNumber,eq_double_t * in,eq_double_t * out)1396 eq_error_t SBSProcessBand(size_t bandNumber, eq_double_t *in,
1397 eq_double_t *out)
1398 {
1399 if (bandNumber < getNumberOfBands())
1400 channels[bandNumber]->SBSProcess(in, out);
1401 else
1402 return invalid_input_data_error;
1403
1404 return no_error;
1405 }
1406
SBSProcess(eq_double_t * in,eq_double_t * out)1407 eq_error_t SBSProcess(eq_double_t *in, eq_double_t *out)
1408 {
1409 eq_error_t err = no_error;
1410 eq_double_t inOut = *in;
1411
1412 for (size_t i = 0; i < getNumberOfBands(); i++) {
1413 err = SBSProcessBand(i, &inOut, &inOut);
1414 if (err)
1415 return err;
1416 }
1417
1418 *out = inOut;
1419
1420 return no_error;
1421 }
1422
getEqType()1423 filter_type getEqType()
1424 {
1425 return currentEqType;
1426 }
1427
getStringEqType()1428 const char* getStringEqType()
1429 {
1430 return getFilterName(currentEqType);
1431 }
1432
getNumberOfBands()1433 size_t getNumberOfBands()
1434 {
1435 return freqGrid.getNumberOfBands();
1436 }
1437
getVersion()1438 const char *getVersion()
1439 {
1440 return eq_version;
1441 }
1442 };
1443
1444 } //namespace OrfanidisEq
1445