byzorgan Code

#include "vocproc.h"
#include <math.h>
#include <QDebug>
#include <algorithm>
#include <complex>
#include "synth.h"
#include "repitcher.h"

#define RAW_BUFFER_SIZE 4096
#define BUFFER_SIZE 16384
#define OUT_BUFFER_SIZE 16384

#define COMB_FILTER_BUFFER_SIZE 1024
#define COMB_FILTER_BUFFERS_COUNT 3

#define FFTLEN 2560
#define FFT_DETECT_BLOCK_SIZE 512

class CombFilter {
public:

    CombFilter() :
        amp(0),
        outptr(0),
        bn(0)
    {
        memset(period, 0, sizeof(period));
        memset(targetPeriod, 0, sizeof(targetPeriod));
        memset(aamp, 0, sizeof(aamp));
        memset(buffer, 0, sizeof (buffer));
    }
    ~CombFilter() { }

    inline void setAmplitude(int amplitude) {
        amp = (amplitude < 0 ? 0 : amplitude > 255 ? 255 : static_cast<uint>(amplitude));
    }
    inline void setPeriod(uint value) {
        if (value > ((COMB_FILTER_BUFFER_SIZE - 2) << 8))
            value = 0;
        if (value != period[bn]) {
            if (value != 0 && period[bn] != 0 && abs(static_cast<int>(value - period[bn])) < 2000) {
                targetPeriod[bn] = value;
            } else {
                if (period[bn] != 0) {
                    if (++bn >= COMB_FILTER_BUFFERS_COUNT)
                        bn = 0;
                }
                targetPeriod[bn] = period[bn] = value;
            }
            //qDebug() << bn << period[bn] << targetPeriod[bn] << aamp[bn];
        }
    }

    qint32 doSample(qint32 sample)
    {
        if (amp == 0)
            return sample;

        qint32 out = 0;
        int sumamp = 0;
        for(uint i = 0; i < COMB_FILTER_BUFFERS_COUNT; ++i) {
            qint32 s = 0;
            if (targetPeriod[i] > period[i]) {
                period[i] += 3;
                if (period[i] > targetPeriod[i])
                    period[i] = targetPeriod[i];
            } else if (targetPeriod[i] < period[i]) {
                period[i] -= 3;
                if (period[i] < targetPeriod[i])
                    period[i] = targetPeriod[i];
            }
            if (period[i] != 0) {
                uint ptrd = (outptr << 8) - period[i] + 1;
                uint a1 = amp;
                if (a1 > 256)
                    a1 = 0;       // overflow
                uint a = ((256 - (ptrd & 255)) * a1 + 128) >> 8;
                uint pt1 = (ptrd >> 8) & (COMB_FILTER_BUFFER_SIZE - 1);
                s += static_cast<int>(a) * buffer[i][pt1];
                pt1 = (pt1 + 1) & (COMB_FILTER_BUFFER_SIZE - 1);
                s += static_cast<int>(a1 - a) * buffer[i][pt1];
                //if (i == bn)
                //    s += sample * static_cast<int>(256 - amp);
            }
            if (i == bn && period[bn] != 0) {
                if (aamp[i] < 256)
                    aamp[i]++;
                if (aamp[i] > 256)
                    aamp[i] = 256;
            } else {
                if (aamp[i] > 0)
                    aamp[i]--;
                if (aamp[i] < 0)
                    aamp[i] = 0;
            }
            sumamp += aamp[i];
            s += ((sample * aamp[i] + 128) >> 8) * static_cast<int>(256 - amp);
            s = (s + 128) >> 8;
            buffer[i][outptr] = s;
            //qDebug() << i << amp << (period[i] >> 8) << s << sample;
            out += s;
        }
        if (sumamp < 256) {
            qint32 s = sample * (256 - sumamp);
            s = (s + 128) >> 8;
            out += s;

        }
        outptr = (outptr + 1) & (COMB_FILTER_BUFFER_SIZE - 1);
        return out;
    }
private:
    qint32 buffer[COMB_FILTER_BUFFERS_COUNT][COMB_FILTER_BUFFER_SIZE];
    uint amp;
    uint period[COMB_FILTER_BUFFERS_COUNT];
    uint targetPeriod[COMB_FILTER_BUFFERS_COUNT];
    int aamp[COMB_FILTER_BUFFERS_COUNT];
    uint outptr;
    uint bn;
};

inline float interpolateGet(float *array, uint index, uint denom) {
    uint ix = index / denom, iy = index % denom;
    return array[ix] + (array[ix + 1] - array[ix]) * iy / denom;
}

static inline uint nbits(uint x) {
    uint nb = 0;
    if (x & 0x80000000u) { nb |= 32; x &= 0x00000000u; }
    if (x & 0x7FFF8000u) { nb |= 16; x &= 0x7FFF0000u; }
    if (x & 0x7F807F80u) { nb |=  8; x &= 0x7F007F00u; }
    if (x & 0x78787878u) { nb |=  4; x &= 0x70707070u; }
    if (x & 0x66666666u) { nb |=  2; x &= 0x44444444u; }
    if (x & 0x55555555u) { nb |=  1; }
    return nb;
}

static inline uint ilog(uint x) {
    uint nb = nbits(x);
    if (nb <= 6)
        return x;
    else
        return ((nb - 6) << 5) + (x >> (nb - 6));
}

VocProc::VocProc(QObject *parent) : QIODevice (parent),
    xv0(0), xv1(0), yv0(0), yv1(0),
    peakLow(0), peakHigh(0), threshold(0),
    sampleRate(44100),
    wptr(0), ewptr(0),
    lowpt0(0), lowpt(0),
    highpt0(0), highpt(0),
    resetpt(0),
    lastKey(0), lastKey2(0), lastKey3(0),
    outptr(0), outcount(0),
    actualPeriod(0),
    maxPeriod(0),
    outVolume(16384), echoVolume(16384),
    gateOnAmp(0), gateOffAmp(-1),
    gateOpen(false),
    useFFT(false),
    useEqualizer(false),
    fftwInitialized(false),
    devId(0),
    lowestKey(128), highestKey(-1),
    lowestPeriod(2000000000), highestPeriod(0),
    keyToPress(0), keyPressed(0),
    peakState(false),
    enableCorrection(false),
    enableVoiceIson(false),
    enableNotchFilter(false),
    enhancedVoiceIson(false),
    loAmp(0), hiAmp(0), currentAmp(0),
    notchFilterFreq(50), notchFilterWidth(200),
    notch(nullptr)
{
    audioRaw = static_cast<qint32 *>(malloc(RAW_BUFFER_SIZE*sizeof(*audioRaw)));
    audioIn = static_cast<qint32 *>(malloc(BUFFER_SIZE*sizeof(*audioIn)));
    audioOut = static_cast<qint16 *>(malloc(OUT_BUFFER_SIZE*sizeof(*audioOut)));
    memset(xa, 0, sizeof(xa));
    memset(xb, 0, sizeof(xb));
    memset(xc, 0, sizeof(xc));
    memset(xd, 0, sizeof(xd));
    memset(xe, 0, sizeof(xe));
    memset(eqa, 0, sizeof(eqa));
    memset(eqb, 0, sizeof(eqb));

    pcc[0] = new RepitchPSOLA(this);
    pcc[1] = nullptr;

    //pcc2 = new RepitchSynth(this);
    //pcc3 = new RepitchSynth(this);

    keys.resize(128);

    combFilter2 = new CombFilter();

}

VocProc::~VocProc()
{
#ifdef CONFIG_FFTW
    if (fftwInitialized)
        fftwRelease();
#endif
    for(uint i = 0; i < sizeof(pcc) / sizeof(*pcc); ++i) {
        if (pcc[i]) {
            delete pcc[i];
            pcc[i] = nullptr;
        }
    }
    free(audioIn);
    free(audioRaw);
    free(audioOut);
    delete combFilter2;
}

int VocProc::currentLevel()
{
    return ampToDb(currentAmp) + 10;
}

int VocProc::ampToDb(int amp)
{
    if (amp < 128)
        return -14;
    int db;
    for(db = -14; amp >= 256; db += 6, amp >>= 1)
        ;
    if (amp > 181) {
        if (amp > 228)
            db += 5;
        else if (amp > 203)
            db += 4;
        else
            db += 3;
    } else if (amp > 143) {
        if (amp > 161)
            db += 2;
        else
            db += 1;
    }
    return db;
}

void VocProc::setSampleRate(quint32 rate) {
    sampleRate = rate;
    if (notch)
        notch->setPeriod((sampleRate + notchFilterFreq / 2) / notchFilterFreq);
}

void VocProc::setKeyTuning(int key, quint32 period)
{
    int pkey = periodToKey.value(keys[key].period, -1);
    if (pkey == key)
        periodToKey.remove(keys[key].period);
    pkey = periodToKey.value(period, -1);
    if (pkey == -1 || pkey > key)
        periodToKey.insert(period, key);

    //qDebug() << "VocProc::setKeyTuning" << key << period << periodToKey.count();
    keys[key].period = period;
    if (key == lowestKey || key == highestKey)
        setupHystogram();
    if (key != 0 && key == static_cast<int>(keyPressed))
        tuneVoiceIson(key);
}

void VocProc::setKeyActive(int key, bool state)
{
    bool needUpdate = false;
    if (keys[key].active && !state) {
        int i;
        if (key == lowestKey) {
            for(i = key + 1; i < 128; ++i)
                if (keys[i].active)
                    break;
            lowestKey = i;
            needUpdate = true;
        }
        if (key == highestKey) {
            for(i = key - 1; i >= 0; --i)
                if (keys[i].active)
                    break;
            highestKey = i;
            needUpdate = true;
        }
    }
    keys[key].active = state;
    if (state) {
        if (key < lowestKey) {
            lowestKey = key;
            needUpdate = true;
        }
        if (key > highestKey) {
            highestKey = key;
            needUpdate = true;
        }
    }
    if (needUpdate)
        setupHystogram();
}

void VocProc::releaseKeys()
{
    if (lastKey != 0) {
        emit keyDown(lastKey, 0);
        //emit keyNotify(lastKey, 0);
        lastKey = 0;
    }
    if (lastKey2 != 0) {
        emit keyNotify(lastKey2, 0);
        lastKey2 = 0;
    }
    if (lastKey3 != 0) {
        emit keyNotify(lastKey3, 0);
        lastKey3 = 0;
    }
}

bool VocProc::gateState()
{
    return gateOpen;
}

qint32 VocProc::getInterpolatedSample(uint index)
{
    quint32 ptr1 = (index >> 16);
    qint32 amp1 = (index >> 4) & 4095;
    qint32 data = audioIn[ptr1 & (BUFFER_SIZE - 1)] * (4096 - amp1);
    data += audioIn[(ptr1 + 1) & (BUFFER_SIZE - 1)] * amp1;
    return (data + 2048) >> 12;
}


void VocProc::setVoiceVolume(int volume)
{
    if (volume == 0)
        outVolume = 0;
    else
        outVolume = Synthesizer::volumeToAmp(volume + 2) >> 14;
}

void VocProc::setEchoVolume(int volume)
{
    if (volume == 0)
        echoVolume = 0;
    else
        echoVolume = Synthesizer::volumeToAmp(volume + 2) >> 14;
}

void VocProc::setGateLevel(int value)
{
    gateOnAmp = Synthesizer::volumeToAmp(value + 4) >> 15;
    gateOffAmp = gateOnAmp * 15 / 16;
}

void VocProc::setEnableCorrection(bool state)
{
    if (state && !enableCorrection && pcc[0])
        pcc[0]->reset();
    enableCorrection = state;
}

void VocProc::setEnableVoiceIson(bool state)
{
    if (state && !enableVoiceIson && pcc[1])
        pcc[1]->reset();
    enableVoiceIson = state;
}

void VocProc::setCombFilterAmp(int amp)
{
    combFilter2->setAmplitude(amp);
}

void VocProc::setEnableNotchFilter(bool state)
{
    if (state && !notch) {
        notch = new NotchFilter();
        notch->setPeriod((sampleRate + notchFilterFreq / 2) / notchFilterFreq);
        notch->setAmp(notchFilterWidth);
    }
    enableNotchFilter = state;
}

void VocProc::setEnableFFT(bool state)
{
#ifdef CONFIG_FFTW
    if (state && !fftwInitialized)
        fftwInitialize();
    useFFT = state;
    releaseKeys();
#endif
}

void VocProc::setEnhancedVoiceIson(bool state)
{
#ifdef CONFIG_FFTW
    if (pcc[1] && state == enhancedVoiceIson)
        return;
    enhancedVoiceIson = state;
    if (pcc[1]) {
        delete pcc[1];
        pcc[1] = nullptr;
    }
    if (state) {
        pcc[1] = new RepitchVocoder(this);
    } else {
        pcc[1] = new RepitchPSOLA(this);
    }
    pcc[1]->setGateEnabled(true);
    pcc[1]->setErrorRate(15);
    pcc[1]->setSpectrumLength(FFTLEN / 2);
#endif
}

void VocProc::setPortamentoRate(int rate)
{
    for(uint i = 0; i < sizeof(pcc) / sizeof(*pcc); ++i)
        pcc[i]->setPortamentoRate(static_cast<uint>(rate));
}

void VocProc::keyDown(int channel, uint key, bool state)
{
    //qDebug() << "VocProc::keyDown" << channel << key << state;
    if (channel == 1) {
        int targetKey = -1;
        if (state) {
            //if (keyPressed != 0) {
            //    keyToPress = key;
            //} else {
                targetKey = static_cast<int>(key);
            //}
        } else {
            //if (keyToPress != 0) {
            //    targetKey = static_cast<int>(keyToPress);
            //    keyToPress = 0;
            //} else {
                targetKey = 0;
            //}
        }
        if (targetKey >= 0) {
            tuneVoiceIson(targetKey);
            keyPressed = static_cast<uint>(targetKey);
        }
    }
}

//void VocProc::userSetting(int value)
//{
//    pcc[1]->setUserValue(value);
//}

void VocProc::setNotchFilterFreq(int freq)
{
    notchFilterFreq = static_cast<uint>(freq);
    if (notch)
        notch->setPeriod((sampleRate + notchFilterFreq / 2) / notchFilterFreq);
}

void VocProc::setNotchFilterWidth(int value)
{
    notchFilterWidth = qMax(qMin(value, 255), 1);
    if (notch)
        notch->setAmp(notchFilterWidth);
}

bool VocProc::open(QIODevice::OpenMode mode)
{
    bool state = QIODevice::open(mode);
    if (!state)
        return false;
    outptr = outcount = 0;
    wptr = 0;

    if (pcc[1] == nullptr)
        setEnableNotchFilter(enhancedVoiceIson);

    for(uint i = 0; i < sizeof(pcc) / sizeof(*pcc); ++i)
        pcc[i]->reset();
    return true;
}

bool VocProc::reset()
{
    bool state = QIODevice::reset();
    if (!state)
        return false;
    outcount = 0;
    return true;
}

qint64 VocProc::readData(char *data, qint64 maxSize)
{
    uint count = static_cast<uint>(qMin<qint64>(maxSize >> 1, outcount));
    uint ptr1 = (outptr - outcount) & (OUT_BUFFER_SIZE - 1);
    uint outc = 0;
    qint16 sample = 0;
    while(count > 0) {
        sample = audioOut[ptr1];
        reinterpret_cast<qint16 *>(data)[outc] = sample;
        ptr1 = (ptr1 + 1) & (OUT_BUFFER_SIZE - 1);
        ++outc;
        --count;
        --outcount;
    }

    count = static_cast<uint>(maxSize >> 1) - outc;
    while (count > 0) {
        reinterpret_cast<qint16 *>(data)[outc] = sample;
        ++outc;
        --count;
    }
    return outc * sizeof(qint16);
}

static inline qint32 nullFilter1(qint32 in, float out[]) {
    out[1] = out[0];
    out[0] = in;
    return in;
}

static inline qint32 nullFilter3(qint32 in, float out[]) {
    out[3] = out[2]; out[2] = out[1]; out[1] = out[0];
    out[0] = in;
    return in;
}

static inline qint32 highPassFilter2(float in[], float out[], float k0, float k1) {
    out[2] = out[1]; out[1] = out[0];
    out[0] = (in[0] + in[2]) - 2 * in[1] + k0 * out[2] + k1 * out[1];
    return static_cast<qint32>(roundf(out[0]));
}
/*
static inline qint32 highPassFilter3(float in[], float out[], float k0, float k1, float k2) {
    out[3] = out[2]; out[2] = out[1]; out[1] = out[0];
    out[0] = (in[0] - in[3]) + 3 * (in[2] - in[1]) + k0 * out[3] + k1 * out[2] + k2 * out[1];
    return static_cast<qint32>(roundf(out[0]));
}
*/
static inline qint32 lowPassFilter1(float in[], float out[], float k0) {
    out[1] = out[0];
    out[0] = (in[1] + in[0]) + k0 * out[1];
    return static_cast<qint32>(roundf(out[0]));
}

/*
static inline float lowPassFilter3(float in[], float out[], float k0, float k1, float k2) {
    out[3] = out[2]; out[2] = out[1]; out[1] = out[0];
    out[0] = (in[0] + in[3]) + 3 * (in[2] + in[1]) + k0 * out[3] + k1 * out[2] + k2 * out[1];
    return out[0];
}
*/


qint64 VocProc::writeData(const char *data, qint64 maxSize)
{

    //qDebug() << "W:" << maxSize;
    uint sampleCount = static_cast<uint>(maxSize >> 1);
    for(uint i = 0; i < sampleCount; ++i) {
        int sample = reinterpret_cast<const qint16 *>(data)[i];

        //static float pt = 0.0f;
        //float x = 2 * M_PI * (pt += 1.0f) / sampleRate;
        //float x = wptr % 400;
        //sample = 1000 * (cosf(23.4575 * 5 * x) );
        //sample = 4000 * (sinf(2 * M_PI * wptr * 2.5 / FFTLEN) );
        //float x = static_cast<float>(wptr) / sampleRate;
        //float x = wptr / 300.0f; x -= floorf(x);
        //x *= 2*M_PI;
        //sample = 4000 * cosf(((wptr / sampleRate) * 2 + 50) * x);
        //sample = 4000 * cosf(((wptr / sampleRate) * 2 + 50) * x);
        //sample = 4000 * cosf(x) + 4000 * cosf(2*x);
        //sample = 4000 * (cosf(2 * M_PI * wptr / 400.0f) );
        //sample = ((wptr / 24000) % 2) * 4000 * (sinf(2 * M_PI * wptr / 400.0f) );
        //sample = 4000 * (sinf(x * (((wptr / 24000) % 2) * 100.0f + 300.0f)) );  // bitonal
        //sample = 4000 * (sinf(2 * M_PI * (wptr % 400) / (1.0f * sinf(2 * M_PI * wptr / 24000) + 300.f)) );  // FM
//        sample = 4000 * (sinf(2 * M_PI * (wptr % 400) / 400) );  // FM

        //sample = eqset[1]->doSample(sample);

        // Preliminary high-pass filter
        nullFilter3(sample, xa);
        highPassFilter2(xa, xb, -0.9907866988f, 1.9907440595f);
        sample = highPassFilter2(xb, xc, -0.9907866988f, 1.9907440595f);

        //sample = highPassFilter3(xa, xb, 0.9858534011f, -2.9716062045f, 2.9857524444f);       // unstable
        //sample = highPassFilter3(xb, xc, 0.9858534011f, -2.9716062045f, 2.9857524444f);

        if (enableNotchFilter)
            sample = (notch->doSample(sample << 7) + 64) >> 7;

        if (sample < loAmp)
            loAmp = sample;
        if (sample > hiAmp)
            hiAmp = sample;

        //qint32 sample2 = (sample * gateAmp + 2048) >> 12;
        qint32 sample2 = sample;
        //int aptr = wptr & (BUFFER_SIZE - 1);
        audioRaw[wptr & (RAW_BUFFER_SIZE - 1)] = sample2;
        audioIn[wptr & (BUFFER_SIZE - 1)] = combFilter2->doSample(sample2);

        if (!useFFT) {
            nullFilter1(sample2, xe);
            int fsample = lowPassFilter1(xe, xd, 0.9929014614f);

            // Check for peak
            quint32 period = checkPeriod(fsample);

            // Store period to hystogram
            if (period != 0)
                updateHystogram(period);

            // No period found - shift down threshold
            if ((wptr - resetpt) >= 2048) {
                resetpt = wptr;
                threshold -= threshold >> 2;
            }
        }

        ++wptr;

        // Update current amplitude
        if ((wptr & 2047) == 0) {
            currentAmp = (hiAmp - loAmp + 3 * currentAmp) / 4;
            loAmp = hiAmp = sample;
            emit captureLevel(currentLevel());
            if (currentAmp > gateOnAmp) {
                if (!gateOpen) {
                    emit gateStateChanged(true, devId);
                    gateOpen = true;
                }
            }
            else if (currentAmp < gateOffAmp) {
                if (gateOpen) {
                    emit gateStateChanged(false, devId);
                    gateOpen = false;
                }
            }
        }

        // Detect current key
#ifdef CONFIG_FFTW
        if (useFFT) {
            if ((wptr & (FFT_DETECT_BLOCK_SIZE - 1)) == 0) {
                uint period = dofftDetection();
                processPeriod(period, spectrum);
            }
        } else
#endif
        if ((wptr & 2047) == 0) {
            uint period = doSimpleDetection();
            processPeriod(period);
        }

        // Process pitch correction
        qint32 s1 = (enableCorrection ? pcc[0]->getSample() : 0);
        qint32 s2 = (enableVoiceIson ? pcc[1]->getSample() : 0);
        //s2 = pcc2->getSample();
        writeProcessedData(s1, s2);
    }

    return maxSize;
}

#ifdef CONFIG_FFTW
void VocProc::fftwInitialize()
{
    fftWindow = static_cast<float *>(fftwf_malloc (FFTLEN * sizeof (float)));
    fftCorr = static_cast<float *>(fftwf_malloc ((FFTLEN / 2) * sizeof (float)));
    fftTdata = static_cast<float *>(fftwf_malloc (FFTLEN * sizeof (float)));
    fftTavg = static_cast<float *>(fftwf_malloc ((FFTLEN / 2 + 1) * sizeof (float)));
    fftFdata = static_cast<fftwf_complex *>(fftwf_malloc ((FFTLEN / 2 + 1) * sizeof (fftwf_complex)));
    fftFdata2 = static_cast<float *>(fftwf_malloc ((FFTLEN / 2) * sizeof (float)));
    spectrum = static_cast<float *>(fftwf_malloc ((FFTLEN / 2) * sizeof (float)));

    fwdPlan = fftwf_plan_dft_r2c_1d (FFTLEN, fftTdata, fftFdata, FFTW_ESTIMATE);
    //invPlan = fftwf_plan_dft_c2r_1d (FFTLEN, fftFdata, fftTdata, FFTW_ESTIMATE);
    invPlan = fftwf_plan_r2r_1d (FFTLEN/2, fftFdata2, fftTdata, FFTW_REDFT01, FFTW_ESTIMATE);

    for (int i = 0; i < FFTLEN; ++i) {
        //fftWindow[i] = sinf(M_PI * (i + 0.5f) / FFTLEN);
        fftWindow[i] = 0.5f * (1 - cosf(2.0f * static_cast<float>(M_PI) * i / FFTLEN));
        //if (i < FFTLEN / 4 || i >= 3 * FFTLEN / 4)
        //    fftWindow[i] = 0.5f * (1 - cosf(4.0f * static_cast<float>(M_PI) * i / FFTLEN));
        //else
        //    fftWindow[i] = 1.0f;
    }

    // Compute window autocorrelation and normalize it.
    fftwf_execute_dft_r2c (fwdPlan, fftWindow, fftFdata);
    int half = FFTLEN / 2;
    for (int i = 0; i < half; i++) {
        float x = fftFdata [i][0],
              y = fftFdata [i][1];
        fftFdata2[i] = sqrt(x * x + y * y);
    }
    fftwf_execute_r2r(invPlan, fftFdata2, fftCorr);
    float t = fftCorr[0];

    //QString s;
    for (int i = 0; i < FFTLEN / 2; i++) {
        fftCorr[i] /= t;
    //    s += " " + QString::number(fftCorr[i],'f',3);
    }
    //qDebug() << "auw" << s;

    for (int i = 0; i <= half; i++)
        fftTavg[i] = 0;

    pcc[0]->setSpectrumLength(FFTLEN / 2);

    fftwInitialized = true;
}

void VocProc::fftwRelease()
{
    fftwf_free(spectrum);
    fftwf_free(fftWindow);
    fftwf_free(fftCorr);
    fftwf_free(fftTdata);
    fftwf_free(fftFdata);
    fftwf_free(fftFdata2);
    fftwf_free(fftTavg);
    fftwf_destroy_plan (fwdPlan);
    fftwf_destroy_plan (invPlan);
    fftwInitialized = false;
}
#endif

quint32 VocProc::checkPeriod(const qint32 &sample)
{
    quint32 period = 0;
    if (peakState) {
        if (sample < peakLow) {
            peakLow = sample;
            threshold = (peakHigh * 2 + peakLow) / 3;
            lowpt = wptr;
        }
        if (sample > threshold) {
            peakState = false;
            period = lowpt - lowpt0;
            lowpt0 = lowpt;
            peakHigh = sample;
            threshold = (peakHigh * 2 + peakLow) / 3;
            resetpt = highpt = wptr;
        }
    } else {
        if (sample > peakHigh) {
            peakHigh = sample;
            threshold = (peakHigh + 2 * peakLow) / 3;
            highpt = wptr;
        }
        if (sample < threshold) {
            peakState = true;
            period = highpt - highpt0;
            highpt0 = highpt;
            peakLow = sample;
            threshold = (peakHigh + 2 * peakLow) / 3;
            resetpt = lowpt = wptr;
        }
    }
    return period;
}

void VocProc::setupHystogram()
{
    if (lowestKey > highestKey)
        return;
    quint32 hp = keys[lowestKey].period * 9 / 8;
    quint32 lp = keys[highestKey].period * 8 / 9;
    if (hp == 0 || lp == 0)
        return;
    if (highestPeriod < lowestPeriod) {
        highestPeriod = lowestPeriod = keys[lowestKey].period;
        hystogram.append(HystogramItem(highestPeriod));
    }
    if (hp > highestPeriod) {
        while(hp > highestPeriod) {
            highestPeriod = highestPeriod * 9 / 8;
            hystogram.append(HystogramItem(highestPeriod));
        }
        uint pp = (highestPeriod + 256) >> 8;
        Repitcher::setLargetsPeriod(pp);
    }
    while(lp < lowestPeriod) {
        lowestPeriod = lowestPeriod * 8 / 9;
        hystogram.prepend(HystogramItem(lowestPeriod));
    }
    //qDebug() << lowestKey << highestKey << lowestPeriod << highestPeriod << hystogram.count() << lowLimit << highLimit;
}

void VocProc::tuneVoiceIson(int key)
{
    quint32 targetPeriod = (key != 0 ? keys[key].period : 0);
    pcc[1]->setTargetPeriod(targetPeriod);
}

void VocProc::updateHystogram(const quint32 &period)
{
    // Search for nearest key
    //qDebug() << period;
    uint pp = period << 8;
    if (!hystogram.empty()) {
        QVector<HystogramItem>::iterator it =
                std::lower_bound(hystogram.begin(), hystogram.end(), pp, HystogramItemCompare());
        if (it > hystogram.begin() && it < hystogram.end() - 1) {
            //qDebug() << "HIS" << pp << it[-1].period << it[0].period;
            if (pp - it[-1].period < it[0].period - pp)
                --it;
            it->hits++;
            it->total += period;
        }
    }
    //qDebug() << pp;
}

void VocProc::processPeriod(uint period, float *spectrum)
{
    if (!mutexKeys.tryLock())
        return;

    uint key = 0, key2 = 0, key3 = 0;
    uint keyvel2 = 0, keyvel3 = 0;
    actualPeriod = 0;
    //targetPeriod = 0;
    //rate = 65536;
    if (gateOpen && period && period > lowestPeriod && period < highestPeriod) {
        actualPeriod = period;
        QMap<uint,int>::iterator it = periodToKey.lowerBound(actualPeriod);
        QMap<uint,int>::iterator it1 = it, it2 = it;

/*
        QVector<KeyItem>::iterator it =
                std::lower_bound(keys.begin() + lowestKey,
                                 keys.begin() + highestKey + 1,
                                 actualPeriod,
                                 KeyItemCompare());
        QVector<KeyItem>::iterator it1, it2;
*/
        // Search for nearest active key

        if (it1 != periodToKey.begin()) {
            for(--it1; it1 != periodToKey.begin() && !keys[it1.value()].active; --it1)
                ;
        }
        for(; it2 != periodToKey.end() && !keys[it2.value()].active; ++it2)
            ;

        if (it1 == periodToKey.begin()) {
            if (it2 != periodToKey.end() && keys[it2.value()].active)
                key = it2.value();
        } else if (it2 == periodToKey.end()) {
            if (it1 != periodToKey.begin() && keys[it1.value()].active)
                key = it1.value();
        } else if (keys[it1.value()].active && keys[it2.value()].active) {
            uint a1 = actualPeriod - it1.key();
            uint a2 = it2.key() - actualPeriod;
            uint i1 = it1.value();
            uint i2 = it2.value();
            if (i1 == lastKey)
                a1 >>= 1;
            if (i2 == lastKey)
                a2 >>= 1;
            key = (a1 < a2) ? i1 : i2;
        }
        if (enableCorrection)
            pcc[0]->setTargetPeriod(keys[static_cast<int>(key)].period);

        // Harmonize
        /*
        int i;
        for(i = 3, it1 = keys.begin() + key; it1 >= keys.begin(); --it1)
            if (it1->active)
                if (--i == 0)
                    break;
        pcc2->setTargetPeriod(it1 >= keys.begin() ? it1->period : 0);
        for(i = 3, it1 = keys.begin() + key; it1 < keys.end(); ++it1)
            if (it1->active)
                if (--i == 0)
                    break;
        pcc3->setTargetPeriod(it1 < keys.end() ? it1->period : 0);
        */

        // search for nearest keys (may be inactive)
        if (it != periodToKey.begin() && it != periodToKey.end()) {
            it1 = it - 1;
            it2 = it;
            if (it1 != periodToKey.begin() && it2 != periodToKey.end()) {
                key2 = it1.value();
                key3 = it2.value();
                uint a2 = actualPeriod - keys[key2].period;
                uint a3 = keys[key3].period - actualPeriod;
                keyvel2 = qMin<int>(a3 * 80 / (a2 + a3), 80);
                keyvel3 = qMin<int>(a2 * 80 / (a2 + a3), 80);
            }
        }
    } else {
        if (enableCorrection)
            pcc[0]->setTargetPeriod(0);
    }
    if (enableCorrection)
        pcc[0]->setActualPeriod(actualPeriod, spectrum);
    if (enableVoiceIson)
        pcc[1]->setActualPeriod(actualPeriod, spectrum);
    //pcc2->setActualPeriod(actualPeriod);
    //pcc3->setActualPeriod(actualPeriod);

    combFilter2->setPeriod(actualPeriod);
    mutexKeys.unlock();

    // Generating key signals
    //uint keyvel = hitPower >> 12, keyvel2 = qMin<uint>(hitPower2 >> 12, 80);
    //qDebug() << keyvel << keyvel2;
    if (lastKey != key) {
        if (lastKey != 0) {
            emit keyDown(lastKey, 0);
        }
        if (key != 0) {
            emit keyDown(key, 127);
        }
        lastKey = key;
    }
    if (lastKey2 != key && lastKey2 != key2 && lastKey2 != key3) {
        emit keyNotify(lastKey2, 0);
        lastKey2 = 0;
    }
    if (lastKey3 != key && lastKey3 != key2 && lastKey3 != key3) {
        emit keyNotify(lastKey3, 0);
        lastKey3 = 0;
    }
    if (key2 != key) {
        emit keyNotify(key2, keyvel2);
    }
    if (key3 != key)
        emit keyNotify(key3, keyvel3);
    lastKey2 = key2;
    lastKey3 = key3;
    //qDebug() << hitIndex << secondHit << hitPower << hitPower2 << s;
}


uint VocProc::doSimpleDetection()
{
    //QString s;
    if (!mutexKeys.tryLock())
        return 0;

    // Process hystogram: find first and second maximums
    uint maxHit = 0, sumHit = 0;
    int hitIndex = -1;
    for(int i = 0; i < hystogram.count(); ++i) {
        if (hystogram[i].hits > maxHit) {
            maxHit = hystogram[i].hits;
            hitIndex = i;
        }
    }
    if (hitIndex >= 0) {
        sumHit = hystogram[hitIndex].total;
        if (hitIndex > 0) {
            maxHit += hystogram[hitIndex - 1].hits;
            sumHit += hystogram[hitIndex - 1].total;
        }
        if (hitIndex < hystogram.count() - 1) {
            maxHit += hystogram[hitIndex + 1].hits;
            sumHit += hystogram[hitIndex + 1].total;
        }
    }
    for(int i = 0; i < hystogram.count(); ++i) {
        quint16 oldHits = hystogram[i].hits;
        quint16 newHits = oldHits >> 1;
        hystogram[i].hits = newHits;
        if (newHits == 0)
            hystogram[i].total = 0;
        else
            hystogram[i].total = (hystogram[i].total * newHits + oldHits / 2) / oldHits;
    }

    uint period = 0, hitPower = 0;
    if (hitIndex >= 0) {
        hitPower = maxHit * hystogram[hitIndex].period;
        if (hitPower >= 1100000) {
            period = ((sumHit << 8) + maxHit / 2) / maxHit;
        }
    }
    mutexKeys.unlock();
    return period;
}


#ifdef CONFIG_FFTW
//#define DEBUG_FFT_DETECTION
uint VocProc::dofftDetection()
{
    uint j = wptr - FFTLEN;

    for (uint i = 0; i < FFTLEN; i++) {
        float sample = audioRaw[(j + i) & (RAW_BUFFER_SIZE - 1)];
        fftTdata[i] = fftWindow[i] * sample;
    }

#ifdef DEBUG_FFT_DETECTION
    QString s;
#endif

    fftwf_execute_dft_r2c (fwdPlan, fftTdata, fftFdata);
    //float f = sampleRate / (FFTLEN * 3e3f);
    for (uint i = 0; i < FFTLEN / 2; i++) {
        float x = fftFdata [i][0],
              y = fftFdata [i][1];
        //float m = i * f;
        //fftFdata2[i] = (x * x + y * y) / (1 + m * m);
        //fftFdata2[i] = sqrtf(sqrtf(x * x + y * y));
        float amp2 = x * x + y * y;
        spectrum[i] = amp2;
        fftFdata2[i] = sqrt(amp2);
    }
    fftwf_execute_r2r(invPlan, fftFdata2, fftTdata);

    uint limit = sampleRate / 60 + 1;
    if (limit > FFTLEN * 2 / 5)
        limit = FFTLEN * 2 / 5;

    float t = fftTdata[0] + 0.1f;
    for (uint i = 0; i < limit; i++) {
        float a = (fftTdata[i] / (t * fftCorr[i]));
        if (a < 0.0f)
            a = 0.0f;
        fftTdata[i] = (a);
    };


    // Aliases suppression
    const uint hset[] = { 2, 3, 5, 7 }; //, 3, 5, 7, 11 };
    for(uint hi = 0; hi < sizeof(hset) / sizeof(*hset); ++hi) {
        uint hno = hset[hi];
        for(uint i = limit; i-- > 0 ;) {
            float a = fftTdata[i] - interpolateGet(fftTdata, i, hno);
            if (a < 0.0f)
                a = 0.0f;
            fftTdata[i] = a;
        }
    }


    uint avgmax = ilog(limit) + 2;
    for(uint i = 0; i < avgmax; ++i)
        fftTavg[i] *= 0.75f;

    uint i = 1;
    while(i < limit && fftTdata[i] > 0.0f)
        ++i;

    uint peakIndex = 0;
    float globalPeak = 0.0f, secondPeak = 0.0f;
    while(i < limit) {
        while(i < limit && fftTdata[i] <= 0.0f)
            ++i;
        if (i >= limit)
            break;

        // Search for next peak
        uint jm = i;
        float apeak = fftTdata[i];
        while (i < limit && fftTdata[i] > 0.0f) {
            float aa = fftTdata[i];
            if (aa > apeak) {
                apeak = aa;
                jm = i;
            }
            ++i;
        }

        if (jm >= (lowestPeriod >> 8) && apeak > 0.1f) {
            // Next peak found
            uint aindex = ilog(jm) + 1;
            float cpeak = apeak * fftTavg[aindex];

#ifdef DEBUG_FFT_DETECTION
            s += " " + QString::number(cpeak, 'f', 3) + "(" + QString::number(jm) + ")";
#endif

            /*
            bool isMultiple = false;
            if (peakIndex != 0) {
                uint muFrac = jm % (peakIndex);
                if (2 * muFrac > peakIndex)
                    muFrac = peakIndex - muFrac;
                if (muFrac * 20 < jm) {
                    isMultiple = true;
#ifdef DEBUG_FFT_DETECTION
                    s += "*";
#endif
                }
            }
            */

            //if (!isMultiple || (cpeak > globalPeak * 1.1f) ) {
                if (cpeak > globalPeak) {
                    //if (!isMultiple)
                        secondPeak = globalPeak;
                    globalPeak = cpeak;
                    peakIndex = jm;
                } else if (cpeak > secondPeak) {
                    secondPeak = cpeak;
                }
            //}

            fftTavg[aindex] += apeak;
            for(uint k = 1; k <= 6; ++k) {
                float m = apeak / powf(1.1f, k * k);
                if (aindex >= k)
                    fftTavg[aindex - k] += m;
                if (aindex + k < 256)
                    fftTavg[aindex + k] += m;
            }
        }
    }

    if (peakIndex && globalPeak > 0.1f && secondPeak / globalPeak < qMin(globalPeak * 0.6f + 0.14f, 0.5f)) {
        float x = fftTdata [peakIndex - 1];
        float y = fftTdata [peakIndex];
        float z = fftTdata [peakIndex + 1];
        float period = peakIndex + 0.5f + 0.5f * (x - z) / (z - 2 * y + x - 1e-9f);
#ifdef DEBUG_FFT_DETECTION
        qDebug() << QString::number(period,'f',1) << globalPeak << secondPeak << s;
#endif
        if (period > sampleRate / 60 || period < sampleRate / 1200)
            return 0;
        return static_cast<uint>(period * 256.0f + 0.5f);
    }
#ifdef DEBUG_FFT_DETECTION
    qDebug() << 0 << globalPeak << secondPeak << s;
#endif
    return 0;
}

#endif

void VocProc::writeProcessedData(qint32 sample, qint32 sample2)
{
    qint32 out1 = (sample + 32) >> 6;
    qint32 out2 = 0;
    qint32 out3 = (sample2 + 32) >> 6;

   if (echoVolume > 0) {
        //qint32 echo = audioIn[(wptr - 1) & (BUFFER_SIZE - 1)];
        qint32 echo = audioIn[(wptr - 1024) & (BUFFER_SIZE - 1)];
        echo = (echo * echoVolume + 128) >> 8;
        out2 = (echo + 32) >> 6;
    }

    if (out1 > 32767) out1 = 32767; else if (out1 < -32767) out1 = -32767;
    audioOut[outptr] = static_cast<qint16>(out1);
    outptr = (outptr + 1) & (OUT_BUFFER_SIZE - 1);

    if (out2 > 32767) out2 = 32767; else if (out2 < -32767) out2 = -32767;
    audioOut[outptr] = static_cast<qint16>(out2);
    outptr = (outptr + 1) & (OUT_BUFFER_SIZE - 1);

    if (out3 > 32767) out3 = 32767; else if (out3 < -32767) out3 = -32767;
    audioOut[outptr] = static_cast<qint16>(out3);
    outptr = (outptr + 1) & (OUT_BUFFER_SIZE - 1);

    if (outcount < OUT_BUFFER_SIZE) {
        outcount += 3;
        if (outcount > OUT_BUFFER_SIZE)
            outcount = OUT_BUFFER_SIZE;
    }
}

NotchFilter::NotchFilter() : buffer(nullptr), amp(0), ptr(0), bufferSize(0) { }

NotchFilter::~NotchFilter() {
    if (buffer)
        free(buffer);
}

void NotchFilter::setPeriod(uint period) {
    if (buffer)
        free(buffer);
    bufferSize = period;
    buffer = static_cast<qint32*>(calloc(bufferSize, sizeof(*buffer)));
    ptr = 0;
}