1/*
2 * Copyright 2012, Gerasim Troeglazov (3dEyes**), 3dEyes@gmail.com.
3 * All rights reserved.
4 * Distributed under the terms of the MIT License.
5 */
6
7#include <stdlib.h>
8#include <math.h>
9#include <string.h>
10
11#include "Equalizer.h"
12
13Equalizer::Equalizer()
14{
15	fActivated = false;
16	CleanUp();
17}
18
19Equalizer::~Equalizer()
20{
21}
22
23void
24Equalizer::SetFormat(int channels, float framerate)
25{
26    fChannels = channels;
27    fRate = framerate;
28
29    memset(fWDataVector[0][0], 0, sizeof(fWDataVector));
30
31    fActiveBands = BandCount();
32    while(fFrequency[fActiveBands-1] * 2.0 > fRate) {
33    	fActiveBands--;
34    }
35
36    for(int i = 0; i < fActiveBands; i++) {
37        BandPassFilterCalcs(fAWeights[i], fBWeights[i],
38        					fFrequency[i] / (float)fRate);
39    }
40}
41
42void
43Equalizer::SetPreAmp(double value)
44{
45	fPreAmp = value;
46	RecalcGains();
47}
48
49double
50Equalizer::PreAmp(void)
51{
52	return fPreAmp;
53}
54
55void
56Equalizer::SetBand(int band, double value)
57{
58	if (band < 0 || band >= BandCount())
59		return;
60
61	fBands[band] = value;
62	RecalcGains();
63}
64
65double
66Equalizer::Band(int band)
67{
68	if (band < 0 || band >= BandCount())
69		return 0.0;
70
71	return fBands[band];
72}
73
74int
75Equalizer::BandCount(void)
76{
77	return EQ_BANDS;
78}
79
80float
81Equalizer::BandFrequency(int band)
82{
83	if (band < 0 || band >= BandCount())
84		return 0.0;
85
86	return fFrequency[band];
87}
88
89void
90Equalizer::ProcessBuffer(float* buffer, int samples)
91{
92   if (!fActivated || samples <= 0)
93        return;
94
95   for(int i = 0; i < fChannels; i++) {
96		float *g = fGainVector[i];
97      	float *end = buffer + samples;
98        for(float *fptr = buffer + i; fptr < end; fptr += fChannels) {
99            float yt = *fptr;
100            for (int k = 0; k < fActiveBands; k ++) {
101                float *Wq = fWDataVector[i][k];
102                float w = yt * fBWeights[k][0] +
103                		Wq[0] * fAWeights[k][0] +
104                		Wq[1] * fAWeights[k][1];
105                yt += (w + Wq[1] * fBWeights[k][1]) * g[k];
106                Wq[1] = Wq[0];
107                Wq[0] = w;
108            }
109            *fptr = yt;
110        }
111    }
112}
113
114void
115Equalizer::CleanUp()
116{
117	float freq[EQ_BANDS] = {31.25, 62.5, 125, 250, 500, 1000, 2000, 4000, 8000, 16000};
118
119	for(int i = 0; i < BandCount(); i++) {
120		fFrequency[i] = freq[i];
121		fBands[i] = 0.0;
122	}
123
124	fPreAmp = 0.0;
125	RecalcGains();
126}
127
128void
129Equalizer::RecalcGains(void)
130{
131    float adjust[EQ_BANDS];
132
133    for(int i = 0; i < BandCount(); i++)
134        adjust[i] = fPreAmp + fBands[i];
135
136    for(int c = 0; c < MAX_CHANNELS; c++)
137    	for(int i = 0; i<BandCount(); i++)
138     	   fGainVector[c][i] = pow(10, adjust[i] / 20) - 1;
139
140    fActivated = true;
141}
142
143void
144Equalizer::BandPassFilterCalcs(float *a, float *b, float f)
145{
146	float q = 1.2247449;
147    float theta = 2.0 * M_PI * f;
148    float c_value = (1 - tanf(theta * q / 2.0)) / (1 + tanf(theta * q / 2.0));
149
150    a[0] = (1 + c_value) * cosf(theta);
151    a[1] = -c_value;
152    b[0] = (1 - c_value) / 2.0;
153    b[1] = -1.005;
154}
155