/* AudioFilterEqualizer_F32.cpp
 *
 * Bob Larkin,  W7PUA  8 May 2020
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "AudioFilterEqualizer_F32.h"

void AudioFilterEqualizer_F32::update(void)  {
    audio_block_f32_t *block, *block_new;

#if TEST_TIME_EQ
  if (iitt++ >1000000) iitt = -10;
  uint32_t t1, t2; 
  t1 = tElapse;
#endif

    block = AudioStream_F32::receiveReadOnly_f32();
    if (!block) return;

    // If there's no coefficient table, give up.  
    if (cf32f == NULL) {
      AudioStream_F32::release(block);
      return;
    }

	block_new = AudioStream_F32::allocate_f32(); 	// get a block for the FIR output
	if (block_new) {
		//apply the FIR
		arm_fir_f32(&fir_inst, block->data, block_new->data, block->length);
		AudioStream_F32::transmit(block_new); // send the FIR output
		AudioStream_F32::release(block_new);
	}
	AudioStream_F32::release(block);
	
#if TEST_TIME_EQ   
  t2 = tElapse;
  if(iitt++ < 0) {Serial.print("At AnalyzePhase end, microseconds = ");  Serial.println (t2 - t1); }  
  t1 = tElapse;
#endif   
}

/* equalizerNew() calculates the Equalizer FIR filter coefficients. Works from: 
 * uint16_t equalizerNew(uint16_t _nBands, float32_t *feq, float32_t *adb,
                      uint16_t _nFIR, float32_t *_cf32f, float32_t kdb)
 *   nBands   Number of equalizer bands
 *   feq      Pointer to array feq[] of nBands breakpoint frequencies, fractions of sample rate, Hz
 *   adb      Pointer to array aeq[] of nBands levels, in dB, for the feq[] defined frequency bands
 *   nFIR     The number of FIR coefficients (taps) used in the equalzer
 *   cf32f    Pointer to an array of float to hold FIR coefficients
 *   kdb      A parameter that trades off sidelobe levels for sharpness of band transition.
 *            kdb=30 sharp cutoff, poor sidelobes
 *            kdb=60 slow cutoff, low sidelobes
 * 
 * The arrays, feq[], aeq[] and cf32f[] are supplied by the calling .INO
 *
 * Returns: 0 if successful, or an error code if not.
 * Errors:  1 = Too many bands, 50 max
 *          2 = sidelobe level out of range, must be > 0
 *          3 = nFIR out of range
 *
 * Note - This function runs at setup time, and there is no need to fret about
 * processor speed.  Likewise, local arrays are created on the stack and are
 * available for other use when this function closes.
 */
uint16_t AudioFilterEqualizer_F32::equalizerNew(uint16_t _nBands, float32_t *feq, float32_t *adb,
                      uint16_t _nFIR, float32_t *_cf32f, float32_t kdb)  {
    uint16_t i, j;
    uint16_t nHalfFIR;
    float32_t beta, kbes;
    float32_t q, xj2, scaleXj2, WindowWt;
    float32_t fNorm[50];   // Normalized to the sampling frequency
    float32_t aVolts[50];  // Convert from dB to "quasi-Volts"
    mathDSP_F32 mathEqualizer;  // For Bessel function

    // Make private copies
    cf32f = _cf32f;
    nFIR = _nFIR;
    nBands = _nBands;

    // Check range of nFIR
    if (nFIR<5 || nFIR>EQUALIZER_MAX_COEFFS)
        return ERR_EQ_NFIR;

    // The number of FIR coefficients needs to be odd
    if (2*(nFIR/2) == nFIR)
        nFIR -= 1;  // We just won't use the last element of the array
    nHalfFIR = (nFIR - 1)/2;  // If nFIR=199, nHalfFIR=99
 
    for (int kk = 0; kk<nFIR; kk++)  // To be sure, zero the coefficients
      cf32f[kk] = 0.0f;
      
    // Convert dB to Voltage ratios, frequencies to fractions of sampling freq
    if(nBands <2 || nBands>50)  return ERR_EQ_BANDS;
    for (i=0; i<nBands; i++)  {
       aVolts[i]=powf(10.0, (0.05*adb[i]));
       fNorm[i]=feq[i]/sample_rate_Hz;
    }

    /* Find FIR coefficients, the Fourier transform of the frequency
     * response. This is done by dividing the response into a sequence
     * of nBands rectangular frequency blocks, each of a different level.
     * We can precalculate the Fourier transform for each rectangular band.
     * The linearity of the Fourier transform allows us to sum the transforms
     * of the individual blocks to get pre-windowed coefficients.  As follows
     *
     * Numbering example for nFIR==199:
     * Subscript 0 to 98 is 99 taps;  100 to 198 is 99 taps;  99+1+99=199 taps
     * The center coef ( for nFIR=199 taps, nHalfFIR=99 ) is a 
     * special case that comes from sin(0)/0 and treated first:
     */
    cf32f[nHalfFIR] = 2.0f*(aVolts[0]*fNorm[0]);  // Coefficient "99"
    for(i=1; i<nBands; i++) {
       cf32f[nHalfFIR] += 2.0f*aVolts[i]*(fNorm[i]-fNorm[i-1]);
    }
    for (j=1; j<=nHalfFIR; j++) {          // Coefficients "100 to 198"
        q = MF_PI*(float32_t)j;
        // First, deal with the zero frequency end band that is "low-pass."
        cf32f[j+nHalfFIR] = aVolts[0]*sinf(fNorm[0]*2.0*q)/q;
        //  and then the rest of the bands that have low and high frequencies
        for(i=1; i<nBands; i++)
           cf32f[j+nHalfFIR] += aVolts[i]*( (sinf(fNorm[i]*2.0*q)/q) - (sinf(fNorm[i-1]*2.0*q)/q) );
    }

    /* At this point, the cf32f[] coefficients are simply truncated sin(x)/x shapes, creating
     * very high sidelobe responses. To reduce the sidelobes, a windowing function is applied.
     * This has the side affect of increasing the rate of cutoff for sharp frequency changes.
     * The only windowing function available here is that of James Kaiser.  This has a number
     * of desirable features.  The tradeoff of sidelobe level versus cutoff rate is variable. 
     * We specify it in terms of kdb, the highest sidelobe, in dB, next to a sharp cutoff. For
     * calculating the windowing vector, we need a parameter beta, found as follows:
     */
    if (kdb<0) return ERR_EQ_SIDELOBES;
    if (kdb>50)
        beta = 0.1102*(kdb-8.7);
    else if  (kdb>20.96 && kdb<=50.0)
        beta = 0.58417*powf((kdb-20.96), 0.4) + 0.07886*(kdb-20.96);
    else
        beta=0.0;
    // Note: i0f is the floating point in & out zero'th order Bessel function (see mathDSP_F32.h)
    kbes = 1.0f / mathEqualizer.i0f(beta);      // An additional derived parameter used in loop
  
    // Apply the Kaiser window
    scaleXj2 = 2.0f/(float32_t)nFIR;
    scaleXj2 *= scaleXj2;
    for (j=0; j<=nHalfFIR; j++) {  // For 199 Taps, this is 0 to 99
         xj2 = (int16_t)(0.5f+(float32_t)j);
         xj2 = scaleXj2*xj2*xj2;
         WindowWt=kbes*(mathEqualizer.i0f(beta*sqrt(1.0-xj2)));
         cf32f[nHalfFIR + j] *= WindowWt;          // Apply the Kaiser window to upper half
         cf32f[nHalfFIR - j] = cf32f[nHalfFIR +j]; // and create the lower half
    }
    // And fill in the members of fir_inst
    arm_fir_init_f32(&fir_inst, nFIR, (float32_t *)cf32f,  &StateF32[0], (uint32_t)block_size);
    return 0;
}

/* Calculate response in dB.  Leave nFreq point result in array rdb[] supplied
 * by the calling .INO  See Parks and Burris, "Digital Filter Design," p27 (Type 1).
 */
void AudioFilterEqualizer_F32::getResponse(uint16_t nFreq, float32_t *rdb)  {
	uint16_t i, j;
	float32_t bt;
	float32_t piOnNfreq;
	uint16_t nHalfFIR;

    nHalfFIR = (nFIR - 1)/2;
    piOnNfreq = MF_PI / (float32_t)nFreq;
    for (i=0; i<nFreq; i++) {
        bt = cf32f[nHalfFIR];//bt = 0.5f*cf32f[nHalfFIR];  // Center coefficient
        for (j=0; j<nHalfFIR; j++)  // Add in the others twice, as they are symmetric
            bt += 2.0f*cf32f[j]*cosf(piOnNfreq*(float32_t)((nHalfFIR-j)*i));
        rdb[i] = 20.0f*log10f(fabsf(bt));     // Convert to dB
    }
}