// ampanlzr.cc
/*
    An amplitude analyzer class for the vocoder.
    Copyright (C) 2002 Jarno Wuolijoki and Pilvari Pirtola
    Partially based on bwlowpas.cc Copyright (C) 1994-1998 Kai Lassfolk

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Library General Public
    License as published by the Free Software Foundation; either
    version 2 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public
    License along with this library; if not, write to the Free
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/
#include "vocoder.h"
#include "ampanlzr.h"
#include <math.h>
#include <string.h>


SPKitAmplitudeAnalyzer::SPKitAmplitudeAnalyzer() {
    index=0; setCutOffFreq(100); buf_a=0;
}


void SPKitAmplitudeAnalyzer::setCutOffFreq(SPKitFloat f) {
    cutOff = f;
    if (input) {
        SPKitFloat C = 1.0/tan(M_PI*cutOff/inputSamplingRate);
        k2 = 1.0/(1.0 + sqrt(2.0)*C + C*C);
        k1 = 2.0*k2;
        k0 = k2;
        km1 = -2*(1.0 - C*C)*k2;
        km2 = -(1.0 - sqrt(2.0)*C + C*C)*k2;
    }
}

SPKitError SPKitAmplitudeAnalyzer::setInput(SPKitProcessor *input) {
    SPKitProcessor::setInput(input);
    int size=inputChannelCount*10;
    if (size<256) size=256;
    index=0;
    buf_a=new SPKitFloat[size];
    buf_b=buf_a+inputChannelCount;
    buf_c=buf_b+inputChannelCount;
    buf_d=buf_c+inputChannelCount;
    buf_e=buf_d+inputChannelCount;
    indmax=size-4*inputChannelCount-2;
    for (int i=0; i<size; i++) buf_a[i]=0;
    if (cutOff) setCutOffFreq(cutOff);
    return 0;
}




int SPKitAmplitudeAnalyzer::getSample(SPKitSample& os, SPKitProcessor* caller) {
    if (!input->getSample(os, this)) return 0;
    convertSample(os);
    return 1;
}


inline void SPKitAmplitudeAnalyzer::convertSample(SPKitSample& smp) {
    if (index>indmax) {
      for (SPKitFloat *p=buf_a; p<=buf_e; p++) *p=p[index];
      index=0;
    }

    // square
    buf_e[index]=smp*smp;

    // butterworth lowpass "inlined" for speed
    buf_c[index]=buf_a[index]*km2+buf_b[index]*km1+buf_c[index]*k0+buf_d[index]*k1+buf_e[index]*k2;

    // sqrt
    smp=sqrt(fabs(buf_c[index]));

    index++;
}


int SPKitAmplitudeNormalizer::getSample(SPKitSample& os, SPKitProcessor* caller) {
    if (!input->getSample(os, this)) return 0;
    SPKitFloat amp=os;
    convertSample(amp);
    os/=(amp<.000001?.000001:amp);
    return 1;
}

