/*
 * PortAudio Portable Real-Time Audio Library
 * Latest Version at: http://www.portaudio.com
 *
 * Copyright (c) 1999-2010 Phil Burk and Ross Bencina
 *
 * 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.
 */

/*
 * The text above constitutes the entire PortAudio license; however,
 * the PortAudio community also makes the following non-binding requests:
 *
 * Any person wishing to distribute modifications to the Software is
 * requested to send the modifications to the original developer so that
 * they can be incorporated into the canonical version. It is also
 * requested that these non-binding requests be included along with the
 * license above.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include "qa_tools.h"
#include "audio_analyzer.h"
#include "write_wav.h"

#define PAQA_POP_THRESHOLD  (0.04)

/*==========================================================================================*/
double PaQa_GetNthFrequency( double baseFrequency, int index )
{
    // Use 13 tone equal tempered scale because it does not generate harmonic ratios.
    return baseFrequency * pow( 2.0, index / 13.0 );
}

/*==========================================================================================*/
void PaQa_EraseBuffer( float *buffer, int numFrames, int samplesPerFrame )
{
    int i;
    int numSamples = numFrames * samplesPerFrame;
    for( i=0; i<numSamples; i++ )
    {
        *buffer++ = 0.0;
    }
}

/*==========================================================================================*/
void PaQa_SetupSineGenerator( PaQaSineGenerator *generator, double frequency, double amplitude, double frameRate )
{
    generator->phase = 0.0;
    generator->amplitude = amplitude;
    generator->frequency = frequency;
    generator->phaseIncrement = 2.0 * frequency * MATH_PI / frameRate;
}

/*==========================================================================================*/
void PaQa_MixSine( PaQaSineGenerator *generator, float *buffer, int numSamples, int stride )
{
    int i;
    for( i=0; i<numSamples; i++ )
    {
        float value = sinf( (float) generator->phase ) * generator->amplitude;
        *buffer += value; // Mix with existing value.
        buffer += stride;
        // Advance phase and wrap around.
        generator->phase += generator->phaseIncrement;
        if (generator->phase > MATH_TWO_PI)
        {
            generator->phase -= MATH_TWO_PI;
        }
    }
}

/*==========================================================================================*/
void PaQa_GenerateCrackDISABLED( float *buffer, int numSamples, int stride )
{
    int i;
    int offset = numSamples/2;
    for( i=0; i<numSamples; i++ )
    {
        float phase = (MATH_TWO_PI * 0.5 * (i - offset)) / numSamples;
        float cosp = cosf( phase );
        float cos2 = cosp * cosp;
        // invert second half of signal
        float value = (i < offset) ? cos2 : (0-cos2);
        *buffer = value;
        buffer += stride;
    }
}


/*==========================================================================================*/
int PaQa_InitializeRecording( PaQaRecording *recording, int maxFrames, int frameRate )
{
    int numBytes = maxFrames * sizeof(float);
    recording->buffer = (float*)malloc(numBytes);
    QA_ASSERT_TRUE( "Allocate recording buffer.", (recording->buffer != NULL) );
    recording->maxFrames = maxFrames;    recording->sampleRate = frameRate;
    recording->numFrames = 0;
    return 0;
error:
    return 1;
}

/*==========================================================================================*/
void PaQa_TerminateRecording( PaQaRecording *recording )
{
    if (recording->buffer != NULL)
    {
        free( recording->buffer );
        recording->buffer = NULL;
    }
    recording->maxFrames = 0;
}

/*==========================================================================================*/
int PaQa_WriteRecording( PaQaRecording *recording, float *buffer, int numFrames, int stride )
{
    int i;
    int framesToWrite;
    float *data = &recording->buffer[recording->numFrames];

    framesToWrite = numFrames;
    if ((framesToWrite + recording->numFrames) > recording->maxFrames)
    {
        framesToWrite = recording->maxFrames - recording->numFrames;
    }

    for( i=0; i<framesToWrite; i++ )
    {
        *data++ = *buffer;
        buffer += stride;
    }
    recording->numFrames += framesToWrite;
    return (recording->numFrames >= recording->maxFrames);
}

/*==========================================================================================*/
int PaQa_WriteSilence( PaQaRecording *recording, int numFrames )
{
    int i;
    int framesToRecord;
    float *data = &recording->buffer[recording->numFrames];

    framesToRecord = numFrames;
    if ((framesToRecord + recording->numFrames) > recording->maxFrames)
    {
        framesToRecord = recording->maxFrames - recording->numFrames;
    }

    for( i=0; i<framesToRecord; i++ )
    {
        *data++ = 0.0f;
    }
    recording->numFrames += framesToRecord;
    return (recording->numFrames >= recording->maxFrames);
}

/*==========================================================================================*/
int PaQa_RecordFreeze( PaQaRecording *recording, int numFrames )
{
    int i;
    int framesToRecord;
    float *data = &recording->buffer[recording->numFrames];

    framesToRecord = numFrames;
    if ((framesToRecord + recording->numFrames) > recording->maxFrames)
    {
        framesToRecord = recording->maxFrames - recording->numFrames;
    }

    for( i=0; i<framesToRecord; i++ )
    {
        // Copy old value forward as if the signal had frozen.
        data[i] = data[i-1];
    }
    recording->numFrames += framesToRecord;
    return (recording->numFrames >= recording->maxFrames);
}

/*==========================================================================================*/
/**
 * Write recording to WAV file.
 */
int PaQa_SaveRecordingToWaveFile( PaQaRecording *recording, const char *filename )
{
    WAV_Writer writer;
    int result = 0;
#define NUM_SAMPLES  (200)
    short data[NUM_SAMPLES];
    const int samplesPerFrame = 1;
    int numLeft = recording->numFrames;
    float *buffer = &recording->buffer[0];

    result =  Audio_WAV_OpenWriter( &writer, filename, recording->sampleRate, samplesPerFrame );
    if( result < 0 ) goto error;

    while( numLeft > 0 )
    {
        int i;
        int numToSave = (numLeft > NUM_SAMPLES) ? NUM_SAMPLES : numLeft;
        // Convert double samples to shorts.
        for( i=0; i<numToSave; i++ )
        {
            double fval = *buffer++;
            // Convert float to int and clip to short range.
            int ival = fval * 32768.0;
            if( ival > 32767 ) ival = 32767;
            else if( ival < -32768 ) ival = -32768;
            data[i] = ival;
        }
        result =  Audio_WAV_WriteShorts( &writer, data, numToSave );
        if( result < 0 ) goto error;
        numLeft -= numToSave;
    }

    result =  Audio_WAV_CloseWriter( &writer );
    if( result < 0 ) goto error;

    return 0;

error:
    printf("ERROR: result = %d\n", result );
    return result;
#undef NUM_SAMPLES
}

/*==========================================================================================*/

double PaQa_MeasureCrossingSlope( float *buffer, int numFrames )
{
    int i;
    double slopeTotal = 0.0;
    int slopeCount = 0;
    float previous;
    double averageSlope = 0.0;

    previous = buffer[0];
    for( i=1; i<numFrames; i++ )
    {
        float current = buffer[i];
        if( (current > 0.0) && (previous < 0.0) )
        {
            double delta = current - previous;
            slopeTotal += delta;
            slopeCount += 1;
        }
        previous = current;
    }
    if( slopeCount > 0 )
    {
        averageSlope = slopeTotal / slopeCount;
    }
    return averageSlope;
}

/*==========================================================================================*/
/*
 * We can't just measure the peaks cuz they may be clipped.
 * But the zero crossing should be intact.
 * The measured slope of a sine wave at zero should be:
 *
 *   slope = sin( 2PI * frequency / sampleRate )
 *
 */
double PaQa_MeasureSineAmplitudeBySlope( PaQaRecording *recording,
                                         double frequency, double frameRate,
                                         int startFrame, int numFrames )
{
    float *buffer = &recording->buffer[startFrame];
    double measuredSlope = PaQa_MeasureCrossingSlope( buffer, numFrames );
    double unitySlope = sin( MATH_TWO_PI * frequency / frameRate );
    double estimatedAmplitude = measuredSlope / unitySlope;
    return estimatedAmplitude;
}

/*==========================================================================================*/
double PaQa_CorrelateSine( PaQaRecording *recording, double frequency, double frameRate,
                           int startFrame, int numFrames, double *phasePtr )
{
    double magnitude = 0.0;
    int numLeft = numFrames;
    double phase = 0.0;
    double phaseIncrement = 2.0 * MATH_PI * frequency / frameRate;
    double sinAccumulator = 0.0;
    double cosAccumulator = 0.0;
    float *data = &recording->buffer[startFrame];

    QA_ASSERT_TRUE( "startFrame out of bounds", (startFrame < recording->numFrames) );
    QA_ASSERT_TRUE( "numFrames out of bounds", ((startFrame+numFrames) <= recording->numFrames) );

    while( numLeft > 0 )
    {
        double sample = (double) *data++;
        sinAccumulator += sample * sin( phase );
        cosAccumulator += sample * cos( phase );
        phase += phaseIncrement;
        if (phase > MATH_TWO_PI)
        {
            phase -= MATH_TWO_PI;
        }
        numLeft -= 1;
    }
    sinAccumulator = sinAccumulator / numFrames;
    cosAccumulator = cosAccumulator / numFrames;
    // TODO Why do I have to multiply by 2.0? Need it to make result come out right.
    magnitude = 2.0 * sqrt( (sinAccumulator * sinAccumulator) + (cosAccumulator * cosAccumulator ));
    if( phasePtr != NULL )
    {
        double phase = atan2( cosAccumulator, sinAccumulator );
        *phasePtr = phase;
    }
    return magnitude;
error:
    return -1.0;
}

/*==========================================================================================*/
void PaQa_FilterRecording( PaQaRecording *input, PaQaRecording *output, BiquadFilter *filter )
{
    int numToFilter = (input->numFrames > output->maxFrames) ? output->maxFrames : input->numFrames;
    BiquadFilter_Filter( filter, &input->buffer[0], &output->buffer[0], numToFilter );
    output->numFrames = numToFilter;
}

/*==========================================================================================*/
/** Scan until we get a correlation of a single that goes over the tolerance level,
 * peaks then drops to half the peak.
 * Look for inverse correlation as well.
 */
double PaQa_FindFirstMatch( PaQaRecording *recording, float *buffer, int numFrames, double threshold  )
{
    int ic,is;
    // How many buffers will fit in the recording?
    int maxCorrelations = recording->numFrames - numFrames;
    double maxSum = 0.0;
    int peakIndex = -1;
    double inverseMaxSum = 0.0;
    int inversePeakIndex = -1;
    double location = -1.0;

    QA_ASSERT_TRUE( "numFrames out of bounds", (numFrames < recording->numFrames) );

    for( ic=0; ic<maxCorrelations; ic++ )
    {
        int pastPeak;
        int inversePastPeak;

        double sum = 0.0;
        // Correlate buffer against the recording.
        float *recorded = &recording->buffer[ ic ];
        for( is=0; is<numFrames; is++ )
        {
            float s1 = buffer[is];
            float s2 = *recorded++;
            sum += s1 * s2;
        }
        if( (sum > maxSum) )
        {
            maxSum = sum;
            peakIndex = ic;
        }
        if( ((-sum) > inverseMaxSum) )
        {
            inverseMaxSum = -sum;
            inversePeakIndex = ic;
        }
        pastPeak = (maxSum > threshold) && (sum < 0.5*maxSum);
        inversePastPeak = (inverseMaxSum > threshold) && ((-sum) < 0.5*inverseMaxSum);
        //printf("PaQa_FindFirstMatch: ic = %4d, sum = %8f, maxSum = %8f, inverseMaxSum = %8f\n", ic, sum, maxSum, inverseMaxSum );
        if( pastPeak && inversePastPeak )
        {
            if( maxSum > inverseMaxSum )
            {
                location = peakIndex;
            }
            else
            {
                location = inversePeakIndex;
            }
            break;
        }

    }
    //printf("PaQa_FindFirstMatch: location = %4d\n", (int)location );
    return location;
error:
    return -1.0;
}

/*==========================================================================================*/
// Measure the area under the curve by summing absolute value of each value.
double PaQa_MeasureArea( float *buffer, int numFrames, int stride  )
{
    int is;
    double area = 0.0;
    for( is=0; is<numFrames; is++ )
    {
        area += fabs( *buffer );
        buffer += stride;
    }
    return area;
}

/*==========================================================================================*/
// Measure the area under the curve by summing absolute value of each value.
double PaQa_MeasureRootMeanSquare( float *buffer, int numFrames )
{
    int is;
    double area = 0.0;
    double root;
    for( is=0; is<numFrames; is++ )
    {
        float value = *buffer++;
        area += value * value;
    }
    root = sqrt( area );
    return root / numFrames;
}


/*==========================================================================================*/
// Compare the amplitudes of these two signals.
// Return ratio of recorded signal over buffer signal.

double PaQa_CompareAmplitudes( PaQaRecording *recording, int startAt, float *buffer, int numFrames )
{
    QA_ASSERT_TRUE( "startAt+numFrames out of bounds", ((startAt+numFrames) < recording->numFrames) );

    {
        double recordedArea = PaQa_MeasureArea( &recording->buffer[startAt], numFrames, 1 );
        double bufferArea = PaQa_MeasureArea( buffer, numFrames, 1 );
        if( bufferArea == 0.0 ) return 100000000.0;
        return recordedArea / bufferArea;
    }
error:
    return -1.0;
}


/*==========================================================================================*/
double PaQa_ComputePhaseDifference( double phase1, double phase2 )
{
    double delta = phase1 - phase2;
    while( delta > MATH_PI )
    {
        delta -= MATH_TWO_PI;
    }
    while( delta < -MATH_PI )
    {
        delta += MATH_TWO_PI;
    }
    return delta;
}

/*==========================================================================================*/
int PaQa_MeasureLatency( PaQaRecording *recording, PaQaTestTone *testTone, PaQaAnalysisResult *analysisResult )
{
    double threshold;
    PaQaSineGenerator generator;
#define MAX_BUFFER_SIZE 2048
    float buffer[MAX_BUFFER_SIZE];
    double period = testTone->sampleRate / testTone->frequency;
    int cycleSize = (int) (period + 0.5);
    //printf("PaQa_AnalyseRecording: frequency = %8f, frameRate = %8f, period = %8f, cycleSize = %8d\n",
    //       testTone->frequency, testTone->sampleRate, period, cycleSize );
    analysisResult->latency = -1;
    analysisResult->valid = (0);

    // Set up generator to find matching first cycle.
    QA_ASSERT_TRUE( "cycleSize out of bounds", (cycleSize < MAX_BUFFER_SIZE) );
    PaQa_SetupSineGenerator( &generator, testTone->frequency, testTone->amplitude, testTone->sampleRate );
    PaQa_EraseBuffer( buffer, cycleSize, testTone->samplesPerFrame );
    PaQa_MixSine( &generator, buffer, cycleSize, testTone->samplesPerFrame );

    threshold = cycleSize * 0.02;
    analysisResult->latency = PaQa_FindFirstMatch( recording, buffer, cycleSize, threshold );
    QA_ASSERT_TRUE( "Could not find the start of the signal.", (analysisResult->latency >= 0) );
    analysisResult->amplitudeRatio = PaQa_CompareAmplitudes( recording, analysisResult->latency, buffer, cycleSize );
    return 0;
error:
    return -1;
}

/*==========================================================================================*/
// Apply cosine squared window.
void PaQa_FadeInRecording( PaQaRecording *recording, int startFrame, int count )
{
    int is;
    double phase = 0.5 * MATH_PI;
    // Advance a quarter wave
    double phaseIncrement = 0.25 * 2.0 * MATH_PI / count;

    assert( startFrame >= 0 );
    assert( count > 0 );

    /* Zero out initial part of the recording. */
    for( is=0; is<startFrame; is++ )
    {
        recording->buffer[ is ] = 0.0f;
    }
    /* Fade in where signal begins. */
    for( is=0; is<count; is++ )
    {
        double c = cos( phase );
        double w = c * c;
        float x = recording->buffer[ is + startFrame ];
        float y = x * w;
        //printf("FADE %d : w=%f, x=%f, y=%f\n", is, w, x, y );
        recording->buffer[ is + startFrame ] = y;

        phase += phaseIncrement;
    }
}


/*==========================================================================================*/
/** Apply notch filter and high pass filter then detect remaining energy.
 */
int PaQa_DetectPop( PaQaRecording *recording, PaQaTestTone *testTone, PaQaAnalysisResult *analysisResult )
{
    int result = 0;
    int i;
    double maxAmplitude;
    int maxPosition;

    PaQaRecording     notchOutput = { 0 };
    BiquadFilter      notchFilter;

    PaQaRecording     hipassOutput = { 0 };
    BiquadFilter      hipassFilter;

    int frameRate = (int) recording->sampleRate;

    analysisResult->popPosition = -1;
    analysisResult->popAmplitude = 0.0;

    result = PaQa_InitializeRecording( &notchOutput, recording->numFrames, frameRate );
    QA_ASSERT_EQUALS( "PaQa_InitializeRecording failed", 0, result );

    result = PaQa_InitializeRecording( &hipassOutput, recording->numFrames, frameRate );
    QA_ASSERT_EQUALS( "PaQa_InitializeRecording failed", 0, result );

    // Use notch filter to remove test tone.
    BiquadFilter_SetupNotch( &notchFilter, testTone->frequency / frameRate, 0.5 );
    PaQa_FilterRecording( recording, &notchOutput, &notchFilter );
    //result = PaQa_SaveRecordingToWaveFile( &notchOutput, "notch_output.wav" );
    //QA_ASSERT_EQUALS( "PaQa_SaveRecordingToWaveFile failed", 0, result );

    // Apply fade-in window.
    PaQa_FadeInRecording( &notchOutput, (int) analysisResult->latency, 500 );

    // Use high pass to accentuate the edges of a pop. At higher frequency!
    BiquadFilter_SetupHighPass( &hipassFilter, 2.0 * testTone->frequency / frameRate, 0.5 );
    PaQa_FilterRecording( &notchOutput, &hipassOutput, &hipassFilter );
    //result = PaQa_SaveRecordingToWaveFile( &hipassOutput, "hipass_output.wav" );
    //QA_ASSERT_EQUALS( "PaQa_SaveRecordingToWaveFile failed", 0, result );

    // Scan remaining signal looking for peak.
    maxAmplitude = 0.0;
    maxPosition = -1;
    for( i=(int) analysisResult->latency; i<hipassOutput.numFrames; i++ )
    {
        float x = hipassOutput.buffer[i];
        float mag = fabs( x );
        if( mag > maxAmplitude )
        {
            maxAmplitude = mag;
            maxPosition = i;
        }
    }

    if( maxAmplitude > PAQA_POP_THRESHOLD )
    {
        analysisResult->popPosition = maxPosition;
        analysisResult->popAmplitude = maxAmplitude;
    }

    PaQa_TerminateRecording( &notchOutput );
    PaQa_TerminateRecording( &hipassOutput );
    return 0;

error:
    PaQa_TerminateRecording( &notchOutput );
    PaQa_TerminateRecording( &hipassOutput );
    return -1;
}

/*==========================================================================================*/
int PaQa_DetectPhaseError( PaQaRecording *recording, PaQaTestTone *testTone, PaQaAnalysisResult *analysisResult )
{
    int i;
    double period = testTone->sampleRate / testTone->frequency;
    int cycleSize = (int) (period + 0.5);

    double maxAddedFrames = 0.0;
    double maxDroppedFrames = 0.0;

    double previousPhase = 0.0;
    double previousFrameError = 0;
    int loopCount = 0;
    int skip = cycleSize;
    int windowSize = cycleSize;

    // Scan recording starting with first cycle, looking for phase errors.
    analysisResult->numDroppedFrames = 0.0;
    analysisResult->numAddedFrames = 0.0;
    analysisResult->droppedFramesPosition = -1.0;
    analysisResult->addedFramesPosition = -1.0;

    for( i=analysisResult->latency; i<(recording->numFrames - windowSize); i += skip )
    {
        double expectedPhase = previousPhase + (skip * MATH_TWO_PI / period);
        double expectedPhaseIncrement = PaQa_ComputePhaseDifference( expectedPhase, previousPhase );

        double phase = 666.0;
        double mag = PaQa_CorrelateSine( recording, testTone->frequency, testTone->sampleRate, i, windowSize, &phase );
        if( (loopCount > 1) && (mag > 0.0) )
        {
            double phaseDelta = PaQa_ComputePhaseDifference( phase, previousPhase );
            double phaseError = PaQa_ComputePhaseDifference( phaseDelta, expectedPhaseIncrement );
            // Convert phaseError to equivalent number of frames.
            double frameError = period * phaseError / MATH_TWO_PI;
            double consecutiveFrameError = frameError + previousFrameError;
//            if( fabs(frameError) > 0.01 )
//            {
//                printf("FFFFFFFFFFFFF frameError = %f, at %d\n", frameError, i );
//            }
            if( consecutiveFrameError > 0.8 )
            {
                double droppedFrames = consecutiveFrameError;
                if (droppedFrames > (maxDroppedFrames * 1.001))
                {
                    analysisResult->numDroppedFrames = droppedFrames;
                    analysisResult->droppedFramesPosition = i + (windowSize/2);
                    maxDroppedFrames = droppedFrames;
                }
            }
            else if( consecutiveFrameError < -0.8 )
            {
                double addedFrames = 0 - consecutiveFrameError;
                if (addedFrames > (maxAddedFrames * 1.001))
                {
                    analysisResult->numAddedFrames = addedFrames;
                    analysisResult->addedFramesPosition = i + (windowSize/2);
                    maxAddedFrames = addedFrames;
                }
            }
            previousFrameError = frameError;


            //if( i<8000 )
            //{
            //    printf("%d: phase = %8f, expected = %8f, delta = %8f, frameError = %8f\n", i, phase, expectedPhaseIncrement, phaseDelta, frameError );
            //}
        }
        previousPhase = phase;
        loopCount += 1;
    }
    return 0;
}

/*==========================================================================================*/
int PaQa_AnalyseRecording( PaQaRecording *recording, PaQaTestTone *testTone, PaQaAnalysisResult *analysisResult )
{
    int result = 0;

    memset( analysisResult, 0, sizeof(PaQaAnalysisResult) );
    result = PaQa_MeasureLatency( recording, testTone, analysisResult );
    QA_ASSERT_EQUALS( "latency measurement", 0, result );

    if( (analysisResult->latency >= 0) && (analysisResult->amplitudeRatio > 0.1) )
    {
        analysisResult->valid = (1);

        result = PaQa_DetectPop( recording, testTone, analysisResult );
        QA_ASSERT_EQUALS( "detect pop", 0, result );

        result = PaQa_DetectPhaseError( recording, testTone, analysisResult );
        QA_ASSERT_EQUALS( "detect phase error", 0, result );
    }
    return 0;
error:
    return -1;
}