/*
 * Copyright (C) 2017 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/**
 * Tools for measuring latency and for detecting glitches.
 * These classes are pure math and can be used with any audio system.
 */

#ifndef AAUDIO_EXAMPLES_LOOPBACK_ANALYSER_H
#define AAUDIO_EXAMPLES_LOOPBACK_ANALYSER_H

#include <algorithm>
#include <assert.h>
#include <cctype>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include <audio_utils/sndfile.h>

// Tag for machine readable results as property = value pairs
#define LOOPBACK_RESULT_TAG      "RESULT: "

constexpr int32_t kDefaultSampleRate = 48000;
constexpr int32_t kMillisPerSecond   = 1000;
constexpr int32_t kMinLatencyMillis  = 4;    // arbitrary and very low
constexpr int32_t kMaxLatencyMillis  = 400;  // arbitrary and generous
constexpr double  kMaxEchoGain       = 10.0; // based on experiments, otherwise too noisy
constexpr double  kMinimumConfidence = 0.5;

static void printAudioScope(float sample) {
    const int maxStars = 80; // arbitrary, fits on one line
    char c = '*';
    if (sample < -1.0) {
        sample = -1.0;
        c = '$';
    } else if (sample > 1.0) {
        sample = 1.0;
        c = '$';
    }
    int numSpaces = (int) (((sample + 1.0) * 0.5) * maxStars);
    for (int i = 0; i < numSpaces; i++) {
        putchar(' ');
    }
    printf("%c\n", c);
}

/*

FIR filter designed with
http://t-filter.appspot.com

sampling frequency: 48000 Hz

* 0 Hz - 8000 Hz
  gain = 1.2
  desired ripple = 5 dB
  actual ripple = 5.595266169703693 dB

* 12000 Hz - 20000 Hz
  gain = 0
  desired attenuation = -40 dB
  actual attenuation = -37.58691566571914 dB

*/

#define FILTER_TAP_NUM 11

static const float sFilterTaps8000[FILTER_TAP_NUM] = {
        -0.05944219353343189f,
        -0.07303434839503208f,
        -0.037690487672689066f,
        0.1870480506596512f,
        0.3910337357836833f,
        0.5333672385425637f,
        0.3910337357836833f,
        0.1870480506596512f,
        -0.037690487672689066f,
        -0.07303434839503208f,
        -0.05944219353343189f
};

class LowPassFilter {
public:

    /*
     * Filter one input sample.
     * @return filtered output
     */
    float filter(float input) {
        float output = 0.0f;
        mX[mCursor] = input;
        // Index backwards over x.
        int xIndex = mCursor + FILTER_TAP_NUM;
        // Write twice so we avoid having to wrap in the middle of the convolution.
        mX[xIndex] = input;
        for (int i = 0; i < FILTER_TAP_NUM; i++) {
            output += sFilterTaps8000[i] * mX[xIndex--];
        }
        if (++mCursor >= FILTER_TAP_NUM) {
            mCursor = 0;
        }
        return output;
    }

    /**
     * @return true if PASSED
     */
    bool test() {
        // Measure the impulse of the filter at different phases so we exercise
        // all the wraparound cases in the FIR.
        for (int offset = 0; offset < (FILTER_TAP_NUM * 2); offset++ ) {
            // printf("LowPassFilter: cursor = %d\n", mCursor);
            // Offset by one each time.
            if (filter(0.0f) != 0.0f) {
                printf("ERROR: filter should return 0.0 before impulse response\n");
                return false;
            }
            for (int i = 0; i < FILTER_TAP_NUM; i++) {
                float output = filter((i == 0) ? 1.0f : 0.0f); // impulse
                if (output != sFilterTaps8000[i]) {
                    printf("ERROR: filter should return impulse response\n");
                    return false;
                }
            }
            for (int i = 0; i < FILTER_TAP_NUM; i++) {
                if (filter(0.0f) != 0.0f) {
                    printf("ERROR: filter should return 0.0 after impulse response\n");
                    return false;
                }
            }
        }
        return true;
    }

private:
    float   mX[FILTER_TAP_NUM * 2]{}; // twice as big as needed to avoid wrapping
    int32_t mCursor = 0;
};

// A narrow impulse seems to have better immunity against over estimating the
// latency due to detecting subharmonics by the auto-correlator.
static const float s_Impulse[] = {
        0.0f, 0.0f, 0.0f, 0.0f, 0.3f, // silence on each side of the impulse
        0.99f, 0.0f, -0.99f, // bipolar with one zero crossing in middle
        -0.3f, 0.0f, 0.0f, 0.0f, 0.0f
};

constexpr int32_t kImpulseSizeInFrames = (int32_t)(sizeof(s_Impulse) / sizeof(s_Impulse[0]));

class PseudoRandom {
public:
    PseudoRandom() {}
    PseudoRandom(int64_t seed)
            :    mSeed(seed)
    {}

    /**
     * Returns the next random double from -1.0 to 1.0
     *
     * @return value from -1.0 to 1.0
     */
     double nextRandomDouble() {
        return nextRandomInteger() * (0.5 / (((int32_t)1) << 30));
    }

    /** Calculate random 32 bit number using linear-congruential method. */
    int32_t nextRandomInteger() {
        // Use values for 64-bit sequence from MMIX by Donald Knuth.
        mSeed = (mSeed * (int64_t)6364136223846793005) + (int64_t)1442695040888963407;
        return (int32_t) (mSeed >> 32); // The higher bits have a longer sequence.
    }

private:
    int64_t mSeed = 99887766;
};


typedef struct LatencyReport_s {
    double latencyInFrames;
    double confidence;
} LatencyReport;

static double calculateCorrelation(const float *a,
                                   const float *b,
                                   int windowSize)
{
    double correlation = 0.0;
    double sumProducts = 0.0;
    double sumSquares = 0.0;

    // Correlate a against b.
    for (int i = 0; i < windowSize; i++) {
        float s1 = a[i];
        float s2 = b[i];
        // Use a normalized cross-correlation.
        sumProducts += s1 * s2;
        sumSquares += ((s1 * s1) + (s2 * s2));
    }

    if (sumSquares >= 0.00000001) {
        correlation = (float) (2.0 * sumProducts / sumSquares);
    }
    return correlation;
}

static int measureLatencyFromEchos(const float *data,
                                   int32_t numFloats,
                                   int32_t sampleRate,
                                   LatencyReport *report) {
    // Allocate results array
    const int minReasonableLatencyFrames = sampleRate * kMinLatencyMillis / kMillisPerSecond;
    const int maxReasonableLatencyFrames = sampleRate * kMaxLatencyMillis / kMillisPerSecond;
    int32_t maxCorrelationSize = maxReasonableLatencyFrames * 3;
    int numCorrelations = std::min(numFloats, maxCorrelationSize);
    float *correlations = new float[numCorrelations]{};
    float *harmonicSums = new float[numCorrelations]{};

    // Perform sliding auto-correlation.
    // Skip first frames to avoid huge peak at zero offset.
    for (int i = minReasonableLatencyFrames; i < numCorrelations; i++) {
        int32_t remaining = numFloats - i;
        float correlation = (float) calculateCorrelation(&data[i], data, remaining);
        correlations[i] = correlation;
        // printf("correlation[%d] = %f\n", ic, correlation);
    }

    // Apply a technique similar to Harmonic Product Spectrum Analysis to find echo fundamental.
    // Add higher harmonics mapped onto lower harmonics. This reinforces the "fundamental" echo.
    const int numEchoes = 8;
    for (int partial = 1; partial < numEchoes; partial++) {
        for (int i = minReasonableLatencyFrames; i < numCorrelations; i++) {
            harmonicSums[i / partial] += correlations[i] / partial;
        }
    }

    // Find highest peak in correlation array.
    float maxCorrelation = 0.0;
    int peakIndex = 0;
    for (int i = 0; i < numCorrelations; i++) {
        if (harmonicSums[i] > maxCorrelation) {
            maxCorrelation = harmonicSums[i];
            peakIndex = i;
            // printf("maxCorrelation = %f at %d\n", maxCorrelation, peakIndex);
        }
    }
    report->latencyInFrames = peakIndex;
/*
    {
        int32_t topPeak = peakIndex * 7 / 2;
        for (int i = 0; i < topPeak; i++) {
            float sample = harmonicSums[i];
            printf("%4d: %7.5f ", i, sample);
            printAudioScope(sample);
        }
    }
*/

    // Calculate confidence.
    if (maxCorrelation < 0.001) {
        report->confidence = 0.0;
    } else {
        // Compare peak to average value around peak.
        int32_t numSamples = std::min(numCorrelations, peakIndex * 2);
        if (numSamples <= 0) {
            report->confidence = 0.0;
        } else {
            double sum = 0.0;
            for (int i = 0; i < numSamples; i++) {
                sum += harmonicSums[i];
            }
            const double average = sum / numSamples;
            const double ratio = average / maxCorrelation; // will be < 1.0
            report->confidence = 1.0 - sqrt(ratio);
        }
    }

    delete[] correlations;
    delete[] harmonicSums;
    return 0;
}

class AudioRecording
{
public:
    AudioRecording() {
    }
    ~AudioRecording() {
        delete[] mData;
    }

    void allocate(int maxFrames) {
        delete[] mData;
        mData = new float[maxFrames];
        mMaxFrames = maxFrames;
    }

    // Write SHORT data from the first channel.
    int write(int16_t *inputData, int inputChannelCount, int numFrames) {
        // stop at end of buffer
        if ((mFrameCounter + numFrames) > mMaxFrames) {
            numFrames = mMaxFrames - mFrameCounter;
        }
        for (int i = 0; i < numFrames; i++) {
            mData[mFrameCounter++] = inputData[i * inputChannelCount] * (1.0f / 32768);
        }
        return numFrames;
    }

    // Write FLOAT data from the first channel.
    int write(float *inputData, int inputChannelCount, int numFrames) {
        // stop at end of buffer
        if ((mFrameCounter + numFrames) > mMaxFrames) {
            numFrames = mMaxFrames - mFrameCounter;
        }
        for (int i = 0; i < numFrames; i++) {
            mData[mFrameCounter++] = inputData[i * inputChannelCount];
        }
        return numFrames;
    }

    int size() {
        return mFrameCounter;
    }

    float *getData() {
        return mData;
    }

    void setSampleRate(int32_t sampleRate) {
        mSampleRate = sampleRate;
    }

    int32_t getSampleRate() {
        return mSampleRate;
    }

    int save(const char *fileName, bool writeShorts = true) {
        SNDFILE *sndFile = nullptr;
        int written = 0;
        SF_INFO info = {
                .frames = mFrameCounter,
                .samplerate = mSampleRate,
                .channels = 1,
                .format = SF_FORMAT_WAV | (writeShorts ? SF_FORMAT_PCM_16 : SF_FORMAT_FLOAT)
        };

        sndFile = sf_open(fileName, SFM_WRITE, &info);
        if (sndFile == nullptr) {
            printf("AudioRecording::save(%s) failed to open file\n", fileName);
            return -errno;
        }

        written = sf_writef_float(sndFile, mData, mFrameCounter);

        sf_close(sndFile);
        return written;
    }

    int load(const char *fileName) {
        SNDFILE *sndFile = nullptr;
        SF_INFO info;

        sndFile = sf_open(fileName, SFM_READ, &info);
        if (sndFile == nullptr) {
            printf("AudioRecording::load(%s) failed to open file\n", fileName);
            return -errno;
        }

        assert(info.channels == 1);
        assert(info.format == SF_FORMAT_FLOAT);

        setSampleRate(info.samplerate);
        allocate(info.frames);
        mFrameCounter = sf_readf_float(sndFile, mData, info.frames);

        sf_close(sndFile);
        return mFrameCounter;
    }

    /**
     * Square the samples so they are all positive and so the peaks are emphasized.
     */
    void square() {
        for (int i = 0; i < mFrameCounter; i++) {
            const float sample = mData[i];
            mData[i] = sample * sample;
        }
    }

    /**
     * Low pass filter the recording using a simple FIR filter.
     * Note that the lowpass filter cutoff tracks the sample rate.
     * That is OK because the impulse width is a fixed number of samples.
     */
    void lowPassFilter() {
        for (int i = 0; i < mFrameCounter; i++) {
            mData[i] = mLowPassFilter.filter(mData[i]);
        }
    }

    /**
     * Remove DC offset using a one-pole one-zero IIR filter.
     */
    void dcBlocker() {
        const float R = 0.996; // narrow notch at zero Hz
        float x1 = 0.0;
        float y1 = 0.0;
        for (int i = 0; i < mFrameCounter; i++) {
            const float x = mData[i];
            const float y = x - x1 + (R * y1);
            mData[i] = y;
            y1 = y;
            x1 = x;
        }
    }

private:
    float        *mData = nullptr;
    int32_t       mFrameCounter = 0;
    int32_t       mMaxFrames = 0;
    int32_t       mSampleRate = kDefaultSampleRate; // common default
    LowPassFilter mLowPassFilter;
};

// ====================================================================================
class LoopbackProcessor {
public:
    virtual ~LoopbackProcessor() = default;


    virtual void reset() {}

    virtual void process(float *inputData, int inputChannelCount,
                 float *outputData, int outputChannelCount,
                 int numFrames) = 0;


    virtual void report() = 0;

    virtual void printStatus() {};

    int32_t getResult() {
        return mResult;
    }

    void setResult(int32_t result) {
        mResult = result;
    }

    virtual bool isDone() {
        return false;
    }

    virtual int save(const char *fileName) {
        (void) fileName;
        return AAUDIO_ERROR_UNIMPLEMENTED;
    }

    virtual int load(const char *fileName) {
        (void) fileName;
        return AAUDIO_ERROR_UNIMPLEMENTED;
    }

    virtual void setSampleRate(int32_t sampleRate) {
        mSampleRate = sampleRate;
    }

    int32_t getSampleRate() {
        return mSampleRate;
    }

    // Measure peak amplitude of buffer.
    static float measurePeakAmplitude(float *inputData, int inputChannelCount, int numFrames) {
        float peak = 0.0f;
        for (int i = 0; i < numFrames; i++) {
            const float pos = fabs(*inputData);
            if (pos > peak) {
                peak = pos;
            }
            inputData += inputChannelCount;
        }
        return peak;
    }


private:
    int32_t mSampleRate = kDefaultSampleRate;
    int32_t mResult = 0;
};

class PeakDetector {
public:
    float process(float input) {
        float output = mPrevious * mDecay;
        if (input > output) {
            output = input;
        }
        mPrevious = output;
        return output;
    }

private:
    float  mDecay = 0.99f;
    float  mPrevious = 0.0f;
};

// ====================================================================================
/**
 * Measure latency given a loopback stream data.
 * Uses a state machine to cycle through various stages including:
 *
 */
class EchoAnalyzer : public LoopbackProcessor {
public:

    EchoAnalyzer() : LoopbackProcessor() {
        mAudioRecording.allocate(2 * getSampleRate());
        mAudioRecording.setSampleRate(getSampleRate());
    }

    void setSampleRate(int32_t sampleRate) override {
        LoopbackProcessor::setSampleRate(sampleRate);
        mAudioRecording.setSampleRate(sampleRate);
    }

    void reset() override {
        mDownCounter = getSampleRate() / 2;
        mLoopCounter = 0;
        mMeasuredLoopGain = 0.0f;
        mEchoGain = 1.0f;
        mState = STATE_INITIAL_SILENCE;
    }

    virtual bool isDone() {
        return mState == STATE_DONE || mState == STATE_FAILED;
    }

    void setGain(float gain) {
        mEchoGain = gain;
    }

    float getGain() {
        return mEchoGain;
    }

    bool testLowPassFilter() {
        LowPassFilter filter;
        return filter.test();
    }

    void report() override {
        printf("EchoAnalyzer ---------------\n");
        if (getResult() != 0) {
            printf(LOOPBACK_RESULT_TAG "result          = %d\n", getResult());
            return;
        }

        // printf("LowPassFilter test %s\n", testLowPassFilter() ? "PASSED" : "FAILED");

        printf(LOOPBACK_RESULT_TAG "measured.gain          = %8f\n", mMeasuredLoopGain);
        printf(LOOPBACK_RESULT_TAG "echo.gain              = %8f\n", mEchoGain);
        printf(LOOPBACK_RESULT_TAG "test.state             = %8d\n", mState);
        printf(LOOPBACK_RESULT_TAG "test.state.name        = %8s\n", convertStateToText(mState));

        if (mState == STATE_WAITING_FOR_SILENCE) {
            printf("WARNING - Stuck waiting for silence. Input may be too noisy!\n");
            setResult(ERROR_NOISY);
        } else if (mMeasuredLoopGain >= 0.9999) {
            printf("   ERROR - clipping, turn down volume slightly\n");
            setResult(ERROR_CLIPPING);
        } else if (mState != STATE_DONE && mState != STATE_GATHERING_ECHOS) {
            printf("WARNING - Bad state. Check volume on device.\n");
            setResult(ERROR_INVALID_STATE);
        } else {
            // Cleanup the signal to improve the auto-correlation.
            mAudioRecording.dcBlocker();
            mAudioRecording.square();
            mAudioRecording.lowPassFilter();

            printf("Please wait several seconds for auto-correlation to complete.\n");
            measureLatencyFromEchos(mAudioRecording.getData(),
                                    mAudioRecording.size(),
                                    getSampleRate(),
                                    &mLatencyReport);

            double latencyMillis = kMillisPerSecond * (double) mLatencyReport.latencyInFrames
                                   / getSampleRate();
            printf(LOOPBACK_RESULT_TAG "latency.frames         = %8.2f\n",
                   mLatencyReport.latencyInFrames);
            printf(LOOPBACK_RESULT_TAG "latency.msec           = %8.2f\n",
                   latencyMillis);
            printf(LOOPBACK_RESULT_TAG "latency.confidence     = %8.6f\n",
                   mLatencyReport.confidence);
            if (mLatencyReport.confidence < kMinimumConfidence) {
                printf("   ERROR - confidence too low!\n");
                setResult(ERROR_CONFIDENCE);
            }
        }
    }

    void printStatus() override {
        printf("st = %d, echo gain = %f ", mState, mEchoGain);
    }

    void sendImpulses(float *outputData, int outputChannelCount, int numFrames) {
        while (numFrames-- > 0) {
            float sample = s_Impulse[mSampleIndex++];
            if (mSampleIndex >= kImpulseSizeInFrames) {
                mSampleIndex = 0;
            }

            *outputData = sample;
            outputData += outputChannelCount;
        }
    }

    void sendOneImpulse(float *outputData, int outputChannelCount) {
        mSampleIndex = 0;
        sendImpulses(outputData, outputChannelCount, kImpulseSizeInFrames);
    }

    // @return number of frames for a typical block of processing
    int32_t getBlockFrames() {
        return getSampleRate() / 8;
    }

    void process(float *inputData, int inputChannelCount,
                 float *outputData, int outputChannelCount,
                 int numFrames) override {
        int channelsValid = std::min(inputChannelCount, outputChannelCount);
        float peak = 0.0f;
        int numWritten;
        int numSamples;

        echo_state nextState = mState;

        switch (mState) {
            case STATE_INITIAL_SILENCE:
                // Output silence at the beginning.
                numSamples = numFrames * outputChannelCount;
                for (int i = 0; i < numSamples; i++) {
                    outputData[i] = 0;
                }
                mDownCounter -= numFrames;
                if (mDownCounter <= 0) {
                    nextState = STATE_MEASURING_GAIN;
                    //printf("%5d: switch to STATE_MEASURING_GAIN\n", mLoopCounter);
                    mDownCounter = getBlockFrames() * 2;
                }
                break;

            case STATE_MEASURING_GAIN:
                sendImpulses(outputData, outputChannelCount, numFrames);
                peak = measurePeakAmplitude(inputData, inputChannelCount, numFrames);
                // If we get several in a row then go to next state.
                if (peak > mPulseThreshold) {
                    mDownCounter -= numFrames;
                    if (mDownCounter <= 0) {
                        //printf("%5d: switch to STATE_WAITING_FOR_SILENCE, measured peak = %f\n",
                        //       mLoopCounter, peak);
                        mDownCounter = getBlockFrames();
                        mMeasuredLoopGain = peak;  // assumes original pulse amplitude is one
                        mSilenceThreshold = peak * 0.1; // scale silence to measured pulse
                        // Calculate gain that will give us a nice decaying echo.
                        mEchoGain = mDesiredEchoGain / mMeasuredLoopGain;
                        if (mEchoGain > kMaxEchoGain) {
                            printf("ERROR - loop gain too low. Increase the volume.\n");
                            nextState = STATE_FAILED;
                        } else {
                            nextState = STATE_WAITING_FOR_SILENCE;
                        }
                    }
                } else if (numFrames > kImpulseSizeInFrames){ // ignore short callbacks
                    mDownCounter = getBlockFrames();
                }
                break;

            case STATE_WAITING_FOR_SILENCE:
                // Output silence and wait for the echos to die down.
                numSamples = numFrames * outputChannelCount;
                for (int i = 0; i < numSamples; i++) {
                    outputData[i] = 0;
                }
                peak = measurePeakAmplitude(inputData, inputChannelCount, numFrames);
                // If we get several in a row then go to next state.
                if (peak < mSilenceThreshold) {
                    mDownCounter -= numFrames;
                    if (mDownCounter <= 0) {
                        nextState = STATE_SENDING_PULSE;
                        //printf("%5d: switch to STATE_SENDING_PULSE\n", mLoopCounter);
                        mDownCounter = getBlockFrames();
                    }
                } else {
                    mDownCounter = getBlockFrames();
                }
                break;

            case STATE_SENDING_PULSE:
                mAudioRecording.write(inputData, inputChannelCount, numFrames);
                sendOneImpulse(outputData, outputChannelCount);
                nextState = STATE_GATHERING_ECHOS;
                //printf("%5d: switch to STATE_GATHERING_ECHOS\n", mLoopCounter);
                break;

            case STATE_GATHERING_ECHOS:
                numWritten = mAudioRecording.write(inputData, inputChannelCount, numFrames);
                peak = measurePeakAmplitude(inputData, inputChannelCount, numFrames);
                if (peak > mMeasuredLoopGain) {
                    mMeasuredLoopGain = peak;  // AGC might be raising gain so adjust it on the fly.
                    // Recalculate gain that will give us a nice decaying echo.
                    mEchoGain = mDesiredEchoGain / mMeasuredLoopGain;
                }
                // Echo input to output.
                for (int i = 0; i < numFrames; i++) {
                    int ic;
                    for (ic = 0; ic < channelsValid; ic++) {
                        outputData[ic] = inputData[ic] * mEchoGain;
                    }
                    for (; ic < outputChannelCount; ic++) {
                        outputData[ic] = 0;
                    }
                    inputData += inputChannelCount;
                    outputData += outputChannelCount;
                }
                if (numWritten  < numFrames) {
                    nextState = STATE_DONE;
                }
                break;

            case STATE_DONE:
            case STATE_FAILED:
            default:
                break;
        }

        mState = nextState;
        mLoopCounter++;
    }

    int save(const char *fileName) override {
        return mAudioRecording.save(fileName);
    }

    int load(const char *fileName) override {
        int result = mAudioRecording.load(fileName);
        setSampleRate(mAudioRecording.getSampleRate());
        mState = STATE_DONE;
        return result;
    }

private:

    enum error_code {
        ERROR_OK = 0,
        ERROR_NOISY = -99,
        ERROR_CLIPPING,
        ERROR_CONFIDENCE,
        ERROR_INVALID_STATE
    };

    enum echo_state {
        STATE_INITIAL_SILENCE,
        STATE_MEASURING_GAIN,
        STATE_WAITING_FOR_SILENCE,
        STATE_SENDING_PULSE,
        STATE_GATHERING_ECHOS,
        STATE_DONE,
        STATE_FAILED
    };

    const char *convertStateToText(echo_state state) {
        const char *result = "Unknown";
        switch(state) {
            case STATE_INITIAL_SILENCE:
                result = "INIT";
                break;
            case STATE_MEASURING_GAIN:
                result = "GAIN";
                break;
            case STATE_WAITING_FOR_SILENCE:
                result = "SILENCE";
                break;
            case STATE_SENDING_PULSE:
                result = "PULSE";
                break;
            case STATE_GATHERING_ECHOS:
                result = "ECHOS";
                break;
            case STATE_DONE:
                result = "DONE";
                break;
            case STATE_FAILED:
                result = "FAILED";
                break;
        }
        return result;
    }


    int32_t         mDownCounter = 500;
    int32_t         mLoopCounter = 0;
    int32_t         mSampleIndex = 0;
    float           mPulseThreshold = 0.02f;
    float           mSilenceThreshold = 0.002f;
    float           mMeasuredLoopGain = 0.0f;
    float           mDesiredEchoGain = 0.95f;
    float           mEchoGain = 1.0f;
    echo_state      mState = STATE_INITIAL_SILENCE;

    AudioRecording  mAudioRecording; // contains only the input after the gain detection burst
    LatencyReport   mLatencyReport;
    // PeakDetector    mPeakDetector;
};


// ====================================================================================
/**
 * Output a steady sinewave and analyze the return signal.
 *
 * Use a cosine transform to measure the predicted magnitude and relative phase of the
 * looped back sine wave. Then generate a predicted signal and compare with the actual signal.
 */
class SineAnalyzer : public LoopbackProcessor {
public:

    void report() override {
        printf("SineAnalyzer ------------------\n");
        printf(LOOPBACK_RESULT_TAG "peak.amplitude     = %8f\n", mPeakAmplitude);
        printf(LOOPBACK_RESULT_TAG "sine.magnitude     = %8f\n", mMagnitude);
        printf(LOOPBACK_RESULT_TAG "peak.noise         = %8f\n", mPeakNoise);
        printf(LOOPBACK_RESULT_TAG "rms.noise          = %8f\n", mRootMeanSquareNoise);
        float amplitudeRatio = mMagnitude / mPeakNoise;
        float signalToNoise = amplitudeRatio * amplitudeRatio;
        printf(LOOPBACK_RESULT_TAG "signal.to.noise    = %8.2f\n", signalToNoise);
        float signalToNoiseDB = 10.0 * log(signalToNoise);
        printf(LOOPBACK_RESULT_TAG "signal.to.noise.db = %8.2f\n", signalToNoiseDB);
        if (signalToNoiseDB < MIN_SNRATIO_DB) {
            printf("ERROR - signal to noise ratio is too low! < %d dB. Adjust volume.\n", MIN_SNRATIO_DB);
            setResult(ERROR_NOISY);
        }
        printf(LOOPBACK_RESULT_TAG "frames.accumulated = %8d\n", mFramesAccumulated);
        printf(LOOPBACK_RESULT_TAG "sine.period        = %8d\n", mSinePeriod);
        printf(LOOPBACK_RESULT_TAG "test.state         = %8d\n", mState);
        printf(LOOPBACK_RESULT_TAG "frame.count        = %8d\n", mFrameCounter);
        // Did we ever get a lock?
        bool gotLock = (mState == STATE_LOCKED) || (mGlitchCount > 0);
        if (!gotLock) {
            printf("ERROR - failed to lock on reference sine tone\n");
            setResult(ERROR_NO_LOCK);
        } else {
            // Only print if meaningful.
            printf(LOOPBACK_RESULT_TAG "glitch.count       = %8d\n", mGlitchCount);
            printf(LOOPBACK_RESULT_TAG "max.glitch         = %8f\n", mMaxGlitchDelta);
            if (mGlitchCount > 0) {
                printf("ERROR - number of glitches > 0\n");
                setResult(ERROR_GLITCHES);
            }
        }
    }

    void printStatus() override {
        printf("st = %d, #gl = %3d,", mState, mGlitchCount);
    }

    double calculateMagnitude(double *phasePtr = NULL) {
        if (mFramesAccumulated == 0) {
            return 0.0;
        }
        double sinMean = mSinAccumulator / mFramesAccumulated;
        double cosMean = mCosAccumulator / mFramesAccumulated;
        double magnitude = 2.0 * sqrt( (sinMean * sinMean) + (cosMean * cosMean ));
        if( phasePtr != NULL )
        {
            double phase = M_PI_2 - atan2( sinMean, cosMean );
            *phasePtr = phase;
        }
        return magnitude;
    }

    /**
     * @param inputData contains microphone data with sine signal feedback
     * @param outputData contains the reference sine wave
     */
    void process(float *inputData, int inputChannelCount,
                 float *outputData, int outputChannelCount,
                 int numFrames) override {
        mProcessCount++;

        float peak = measurePeakAmplitude(inputData, inputChannelCount, numFrames);
        if (peak > mPeakAmplitude) {
            mPeakAmplitude = peak;
        }

        for (int i = 0; i < numFrames; i++) {
            bool sineEnabled = true;
            float sample = inputData[i * inputChannelCount];

            float sinOut = sinf(mPhase);

            switch (mState) {
                case STATE_IDLE:
                    sineEnabled = false;
                    mDownCounter--;
                    if (mDownCounter <= 0) {
                        mState = STATE_MEASURE_NOISE;
                        mDownCounter = NOISE_FRAME_COUNT;
                    }
                    break;
                case STATE_MEASURE_NOISE:
                    sineEnabled = false;
                    mPeakNoise = std::max(abs(sample), mPeakNoise);
                    mNoiseSumSquared += sample * sample;
                    mDownCounter--;
                    if (mDownCounter <= 0) {
                        mState = STATE_WAITING_FOR_SIGNAL;
                        mRootMeanSquareNoise = sqrt(mNoiseSumSquared / NOISE_FRAME_COUNT);
                        mTolerance = std::max(MIN_TOLERANCE, mPeakNoise * 2.0f);
                        mPhase = 0.0; // prevent spike at start
                    }
                    break;

                case STATE_IMMUNE:
                    mDownCounter--;
                    if (mDownCounter <= 0) {
                        mState = STATE_WAITING_FOR_SIGNAL;
                    }
                    break;

                case STATE_WAITING_FOR_SIGNAL:
                    if (peak > mThreshold) {
                        mState = STATE_WAITING_FOR_LOCK;
                        //printf("%5d: switch to STATE_WAITING_FOR_LOCK\n", mFrameCounter);
                        resetAccumulator();
                    }
                    break;

                case STATE_WAITING_FOR_LOCK:
                    mSinAccumulator += sample * sinOut;
                    mCosAccumulator += sample * cosf(mPhase);
                    mFramesAccumulated++;
                    // Must be a multiple of the period or the calculation will not be accurate.
                    if (mFramesAccumulated == mSinePeriod * PERIODS_NEEDED_FOR_LOCK) {
                        mPhaseOffset = 0.0;
                        mMagnitude = calculateMagnitude(&mPhaseOffset);
                        if (mMagnitude > mThreshold) {
                            if (fabs(mPreviousPhaseOffset - mPhaseOffset) < 0.001) {
                                mState = STATE_LOCKED;
                                //printf("%5d: switch to STATE_LOCKED\n", mFrameCounter);
                            }
                            mPreviousPhaseOffset = mPhaseOffset;
                        }
                        resetAccumulator();
                    }
                    break;

                case STATE_LOCKED: {
                    // Predict next sine value
                    float predicted = sinf(mPhase + mPhaseOffset) * mMagnitude;
                    // printf("    predicted = %f, actual = %f\n", predicted, sample);

                    float diff = predicted - sample;
                    float absDiff = fabs(diff);
                    mMaxGlitchDelta = std::max(mMaxGlitchDelta, absDiff);
                    if (absDiff > mTolerance) {
                        mGlitchCount++;
                        //printf("%5d: Got a glitch # %d, predicted = %f, actual = %f\n",
                        //       mFrameCounter, mGlitchCount, predicted, sample);
                        mState = STATE_IMMUNE;
                        mDownCounter = mSinePeriod * PERIODS_IMMUNE;
                    }

                    // Track incoming signal and slowly adjust magnitude to account
                    // for drift in the DRC or AGC.
                    mSinAccumulator += sample * sinOut;
                    mCosAccumulator += sample * cosf(mPhase);
                    mFramesAccumulated++;
                    // Must be a multiple of the period or the calculation will not be accurate.
                    if (mFramesAccumulated == mSinePeriod) {
                        const double coefficient = 0.1;
                        double phaseOffset = 0.0;
                        double magnitude = calculateMagnitude(&phaseOffset);
                        // One pole averaging filter.
                        mMagnitude = (mMagnitude * (1.0 - coefficient)) + (magnitude * coefficient);
                        resetAccumulator();
                    }
                } break;
            }

            float output = 0.0f;
            // Output sine wave so we can measure it.
            if (sineEnabled) {
                output = (sinOut * mOutputAmplitude)
                         + (mWhiteNoise.nextRandomDouble() * mNoiseAmplitude);
                // printf("%5d: sin(%f) = %f, %f\n", i, mPhase, sinOut,  mPhaseIncrement);
                // advance and wrap phase
                mPhase += mPhaseIncrement;
                if (mPhase > M_PI) {
                    mPhase -= (2.0 * M_PI);
                }
            }
            outputData[i * outputChannelCount] = output;


            mFrameCounter++;
        }
    }

    void resetAccumulator() {
        mFramesAccumulated = 0;
        mSinAccumulator = 0.0;
        mCosAccumulator = 0.0;
    }

    void reset() override {
        mGlitchCount = 0;
        mState = STATE_IDLE;
        mDownCounter = IDLE_FRAME_COUNT;
        mPhaseIncrement = 2.0 * M_PI / mSinePeriod;
        printf("phaseInc = %f for period %d\n", mPhaseIncrement, mSinePeriod);
        resetAccumulator();
        mProcessCount = 0;
        mPeakNoise = 0.0f;
        mNoiseSumSquared = 0.0;
        mRootMeanSquareNoise = 0.0;
        mPhase = 0.0f;
        mMaxGlitchDelta = 0.0;
    }

private:

    enum error_code {
        OK,
        ERROR_NO_LOCK = -80,
        ERROR_GLITCHES,
        ERROR_NOISY
    };

    enum sine_state_t {
        STATE_IDLE,
        STATE_MEASURE_NOISE,
        STATE_IMMUNE,
        STATE_WAITING_FOR_SIGNAL,
        STATE_WAITING_FOR_LOCK,
        STATE_LOCKED
    };

    enum constants {
        // Arbitrary durations, assuming 48000 Hz
        IDLE_FRAME_COUNT = 48 * 100,
        NOISE_FRAME_COUNT = 48 * 600,
        PERIODS_NEEDED_FOR_LOCK = 8,
        PERIODS_IMMUNE = 2,
        MIN_SNRATIO_DB = 65
    };

    static constexpr float MIN_TOLERANCE = 0.01;

    int     mSinePeriod = 79;
    double  mPhaseIncrement = 0.0;
    double  mPhase = 0.0;
    double  mPhaseOffset = 0.0;
    double  mPreviousPhaseOffset = 0.0;
    double  mMagnitude = 0.0;
    double  mThreshold = 0.005;
    double  mTolerance = MIN_TOLERANCE;
    int32_t mFramesAccumulated = 0;
    int32_t mProcessCount = 0;
    double  mSinAccumulator = 0.0;
    double  mCosAccumulator = 0.0;
    float   mMaxGlitchDelta = 0.0f;
    int32_t mGlitchCount = 0;
    double  mPeakAmplitude = 0.0;
    int     mDownCounter = IDLE_FRAME_COUNT;
    int32_t mFrameCounter = 0;
    float   mOutputAmplitude = 0.75;

    // measure background noise
    float   mPeakNoise = 0.0f;
    double  mNoiseSumSquared = 0.0;
    double  mRootMeanSquareNoise = 0.0;

    PseudoRandom  mWhiteNoise;
    float   mNoiseAmplitude = 0.00; // Used to experiment with warbling caused by DRC.

    sine_state_t  mState = STATE_IDLE;
};

#undef LOOPBACK_RESULT_TAG

#endif /* AAUDIO_EXAMPLES_LOOPBACK_ANALYSER_H */