/*
 * Company: Blackaddr Audio
 * Effect Name: Noise Gate
 * Description: A simple noise gate wtih controllable threshold, opening and closing times.
 *
 * This file was auto-generated by Aviate Audio Effect Creator for the Multiverse.
 */
#include <cmath>
#include "Aviate/EfxPrint.h"
#include "Aviate/TunerManager.h"
#include "Tuner.h"

using namespace Aviate;

namespace BlackaddrAudio_Tuner {

#define TUNER_A4STANDARD		440.0f
#define TUNER_FREQ_MIN			10.0f
#define TUNER_FREQ_MAX			1500.0f
#define TUNER_TOLERENCE			4.10f

constexpr unsigned STARTUP_CALIB_TIME_SECONDS = 2;

// NOTE FINDER PROTOTYPES
const char *noteToLabel (const uint8_t note);
static void noteBuildTable (const float A4);
const float noteToFreq (const uint8_t note, const uint8_t octave);
float noteFindNearest (float freqIn, uint8_t *noteOut, uint8_t *octaveOut);
float noteGetLower (uint8_t *note, uint8_t *octave);
float noteGetHigher (uint8_t *note, uint8_t *octave);
static void buildOctave (float pitchStandard, int octave);
static void noteBuildTable (const float A4);

Tuner::Tuner()
: AudioStream(NUM_INPUTS, m_inputQueueArray)
{

}

Tuner::~Tuner()
{

}

void Tuner::update(void)
{
    if (!m_initialized) { init(0.1f); }
    audio_block_t *inputAudioBlock = receiveWritable(); // get the next block of input samples
    inputAudioBlock = m_basicInputCheck(inputAudioBlock, 0); // check for disable mode, bypass, or invalid inputs. Transmit to channel 0 in bypass

    if (!inputAudioBlock) {  // EFX level bypass
        // Tuner reset
        for (unsigned i=0; i < AUDIO_GUITARTUNER_BLOCKS; i++) {
            if (blocklist1[i]) { release(blocklist1[i]); blocklist1[i] = nullptr; }
            if (blocklist2[i]) { release(blocklist2[i]); blocklist2[i] = nullptr; }
        }
        tunerReset();
        sendValueReport(TunerCents_e, -50.0f);
        sendValueReport(TunerNote_e,   static_cast<unsigned>(TunerNote::NO_DETECT));
        return;
    } // no further processing for this update() call

    // You must call m_updateInputPeak() before processing the audio
    m_updateInputPeak(inputAudioBlock);

    //////////////////////////////
    // START OF TUNER PROCESSING
    //////////////////////////////
    if (m_tunerEnabled) {
        audio_block_t *block = allocate();
        memcpy((void*)&block->data[0], (void*)&inputAudioBlock->data[0], sizeof(int16_t)*AUDIO_SAMPLES_PER_BLOCK);

        if ( next_buffer ) {
            blocklist1[state++] = block;
            if ( !first_run && process_buffer ) process( );
        } else {
            blocklist2[state++] = block;
            if ( !first_run && process_buffer ) process( );
        }

        if ( state >= AUDIO_GUITARTUNER_BLOCKS ) {
            if ( next_buffer ) {
                if ( !first_run && process_buffer ) process( );
                for ( unsigned i = 0; i < AUDIO_GUITARTUNER_BLOCKS; i++ ) copy_buffer( AudioBuffer+( i * 0x80 ), blocklist1[i]->data );
                for ( unsigned i = 0; i < AUDIO_GUITARTUNER_BLOCKS; i++ ) release( blocklist1[i] );
                next_buffer = false;
            } else {
                if ( !first_run && process_buffer ) process( );
                for ( unsigned i = 0; i < AUDIO_GUITARTUNER_BLOCKS; i++ ) copy_buffer( AudioBuffer+( i * 0x80 ), blocklist2[i]->data );
                for ( unsigned i = 0; i < AUDIO_GUITARTUNER_BLOCKS; i++ ) release( blocklist2[i] );
                next_buffer = true;
            }
            process_buffer = true;
            first_run = false;
            state = 0;
        }

        bool updated = available();
        if (updated) {
            float notef = read();
            if (notef < TUNER_FREQ_MIN)
            	notef = 0.01f;
            else if (notef > TUNER_FREQ_MAX)
            	notef = TUNER_FREQ_MAX;

            // calc surrounding notes
            uint8_t noteNearest;
            uint8_t octaveNearest;
            float delta = noteFindNearest(notef, &noteNearest, &octaveNearest);
            float freqN = noteToFreq(noteNearest, octaveNearest);

            uint8_t noteOutL = noteNearest;
            uint8_t octaveOutL = octaveNearest;
            float freqL = noteGetLower(&noteOutL, &octaveOutL);

            uint8_t noteOutH = noteNearest;
            uint8_t octaveOutH = octaveNearest;
            float freqH = noteGetHigher(&noteOutH, &octaveOutH);


            // calc note cent off from nearest note
            float cent = 0.0f;
            if (delta > 0.0f)
                cent = ((1.0f / (freqH - freqN)) * delta) * 100.0f;
            else if (delta < 0.0f)
                cent = -((1.0f / (freqL - freqN)) * delta) * 100.0f;

            //efxLogger.printf("Note: %s - cent: %f skipCount:%d\n", noteToLabel(noteNearest), cent, m_skipUpdateCount);
            if (noteNearest < 0 || noteNearest > 11) { noteNearest = 12; }  // 12 is the 'INVALID' note.
            if (tunerManagerPtr) { tunerManagerPtr->sendNoteUpdate(static_cast<Aviate::TunerNote>(noteNearest), cent, !m_signalDetect); }

            float newCentValue;
            if (cent < -50.0f) { newCentValue = -50.0f; }
            else if (cent > 50.0f) { newCentValue = 50.0f; }
            else { newCentValue = cent; }
            m_tunercents = m_tunercents*m_ALPHA + newCentValue*m_ONE_MINUS_ALPHA;

            sendValueReport(TunerCents_e, m_tunercents);
            sendValueReport(TunerNote_e,  noteNearest);
            m_signalDetect = true;
            m_skipUpdateCount = 0;

        } else if (m_skipUpdateCount > (AUDIO_SAMPLE_RATE_HZ / AUDIO_SAMPLES_PER_BLOCK) / 4) {
            if (tunerManagerPtr) { tunerManagerPtr->sendNoteUpdate(TunerNote::NO_DETECT, 0.0, true /* no filtering */); }
            sendValueReport(TunerCents_e, -50.0f);
            sendValueReport(TunerNote_e, static_cast<unsigned>(TunerNote::NO_DETECT));
            m_skipUpdateCount = 0;
            m_signalDetect = false;
        } else {
            m_skipUpdateCount++;
        }
    } else {
        // Tuner reset
        for (unsigned i=0; i < AUDIO_GUITARTUNER_BLOCKS; i++) {
            if (blocklist1[i]) { release(blocklist1[i]); blocklist1[i] = nullptr; }
            if (blocklist2[i]) { release(blocklist2[i]); blocklist2[i] = nullptr; }
        }
        tunerReset();
        sendValueReport(TunerCents_e, -50.0f);
        sendValueReport(TunerNote_e,   static_cast<unsigned>(TunerNote::NO_DETECT));
    }
    ////////////////////////////
    // END OF TUNER PROCESSING
    ////////////////////////////

    for (size_t i=0; i < AUDIO_SAMPLES_PER_BLOCK; i++) {
        inputAudioBlock->data[i] = m_silentEnabled ? 0 : std::round((float)inputAudioBlock->data[i] * m_volume);
    }

    m_updateOutputPeak(inputAudioBlock); // you must call m_upateOutputPeak() at the end of update() before transmit
    transmit(inputAudioBlock);
    release(inputAudioBlock);
}

void Tuner::volume(float value)
{
    float volDbValue = -40.0f + (value * 50.0f);  // remap the normalized value to represent -40dB to +10dB
    volumeDb(volDbValue);  // AudioEffectWrapper has built-in volume function in dB
}

void Tuner::tunercents(float value)
{
    // this is a value monitor control, we ignore any incoming requests to set this parameter
}

void Tuner::tunernote(float value)
{
    // this is a value monitor control, we ignore any incoming requests to set this parameter
}

void Tuner::tunersilent(float value)
{
    m_tunersilent = value;
    m_silentEnabled = m_tunersilent ? true : false;
}

void Tuner::bypass(bool value)
{
    m_bypass = value;
    m_tunerEnabled = !(value > 0.0f);
    if (tunerManagerPtr) { tunerManagerPtr->setTunerMode(m_tunerEnabled); }
}

//////////
// TUNER
//////////
#define HALF_BLOCKS AUDIO_GUITARTUNER_BLOCKS * 64
#define NOTE_OCTAVES			6		// number of octaves covered

enum _notes {
	NOTE_C = 0,
	NOTE_C_SHARP,
	NOTE_D,
	NOTE_D_SHARP,
	NOTE_E,
	NOTE_F,
	NOTE_F_SHARP,
	NOTE_G,
	NOTE_G_SHARP,
	NOTE_A,
	NOTE_A_SHARP,
	NOTE_B,
    NO_DETECT
};

typedef struct {
	uint8_t note;
	char label[3];
	float freq[NOTE_OCTAVES];
}notetable_t;

static notetable_t notetable[12] = {
 {NOTE_C,       "C", {}},
 {NOTE_C_SHARP, "C#",{}},
 {NOTE_D,       "D", {}},
 {NOTE_D_SHARP, "D#",{}},
 {NOTE_E,       "E", {}},
 {NOTE_F,       "F", {}},
 {NOTE_F_SHARP, "F#",{}},
 {NOTE_G,       "G", {}},
 {NOTE_G_SHARP, "G#",{}},
 {NOTE_A,       "A", {}},
 {NOTE_A_SHARP, "A#",{}},
 {NOTE_B,       "B", {}},
};

/**
 *  Copy internal blocks of data to class buffer
 *
 *  @param destination destination address
 *  @param source      source address
 */
void Tuner::copy_buffer(void *destination, const void *source) {
    const uint16_t *src = ( const uint16_t * )source;
    uint16_t *dst = (  uint16_t * )destination;
    for (int i=0; i < AUDIO_BLOCK_SAMPLES; i++)  *dst++ = (*src++);
}

/**
 *  Start the Yin algorithm
 *
 *  TODO: Significant speed up would be to use spectral domain to find fundamental frequency.
 *  This paper explains: https://aubio.org/phd/thesis/brossier06thesis.pdf -> Section 3.2.4
 *  page 79. Might have to downsample for low fundmental frequencies because of fft buffer
 *  size limit.
 */
void Tuner::process( void ) {

    const int16_t *p;
    p = AudioBuffer;

    uint16_t cycles = 64;
    uint16_t tau = tau_global;
    do {
        uint16_t x   = 0;
        uint64_t  sum = 0;
        do {
            int16_t current, lag, delta;
            lag = *( ( int16_t * )p + ( x+tau ) );
            current = *( ( int16_t * )p+x );
            delta = ( current-lag );
            sum += delta * delta;
            x += 4;

            lag = *( ( int16_t * )p + ( x+tau ) );
            current = *( ( int16_t * )p+x );
            delta = ( current-lag );
            sum += delta * delta;
            x += 4;

            lag = *( ( int16_t * )p + ( x+tau ) );
            current = *( ( int16_t * )p+x );
            delta = ( current-lag );
            sum += delta * delta;
            x += 4;

            lag = *( ( int16_t * )p + ( x+tau ) );
            current = *( ( int16_t * )p+x );
            delta = ( current-lag );
            sum += delta * delta;
            x += 4;
        } while ( x < HALF_BLOCKS );

        uint64_t rs = running_sum;
        rs += sum;
        yin_buffer[yin_idx] = sum*tau;
        rs_buffer[yin_idx] = rs;
        running_sum = rs;
        yin_idx = ( ++yin_idx >= 5 ) ? 0 : yin_idx;
        tau = estimate( yin_buffer, rs_buffer, yin_idx, tau );

        if ( tau == 0 ) {
            process_buffer  = false;
            new_output      = true;
            yin_idx         = 1;
            running_sum     = 0;
            tau_global      = 1;
            return;
        }
    } while ( --cycles );

    if ( tau >= HALF_BLOCKS ) {
        process_buffer  = false;
        new_output      = false;
        yin_idx         = 1;
        running_sum     = 0;
        tau_global      = 1;
        return;
    }
    tau_global = tau;
}

/**
 *  check the sampled data for fundamental frequency
 *
 *  @param yin  buffer to hold sum*tau value
 *  @param rs   buffer to hold running sum for sampled window
 *  @param head buffer index
 *  @param tau  lag we are currently working on gets incremented
 *
 *  @return tau
 */
uint16_t Tuner::estimate( uint64_t *yin, uint64_t *rs, uint16_t head, uint16_t tau ) {
    const uint64_t *y = ( uint64_t * )yin;
    const uint64_t *r = ( uint64_t * )rs;
    uint16_t _tau, _head;
    const float thresh = yin_threshold;
    _tau = tau;
    _head = head;

    if ( _tau > 4 ) {

        uint16_t idx0, idx1, idx2;
        idx0 = _head;
        idx1 = _head + 1;
        idx1 = ( idx1 >= 5 ) ? 0 : idx1;
        idx2 = head + 2;
        idx2 = ( idx2 >= 5 ) ? 0 : idx2;

        float s0, s1, s2;
        s0 = ( ( float )*( y+idx0 ) / *( r+idx0 ) );
        s1 = ( ( float )*( y+idx1 ) / *( r+idx1 ) );
        s2 = ( ( float )*( y+idx2 ) / *( r+idx2 ) );

        if ( s1 < thresh && s1 < s2 ) {
            uint16_t period = _tau - 3;
            periodicity = 1 - s1;
            data = period + 0.5f * ( s0 - s2 ) / ( s0 - 2.0f * s1 + s2 );
            return 0;
        }
    }
    return _tau + 1;
}

void Tuner::tunerReset() {
    process_buffer = false;
    periodicity    = 0.0f;
    next_buffer    = true;
    running_sum    = 0;
    tau_global     = 1;
    first_run      = true;
    yin_idx        = 1;
    state          = 0;
    data           = 0.0f;
}

void Tuner::init( float thresholdIn ) {
    tunerReset();

    yin_threshold  = thresholdIn;
    process_buffer = false;

    noteBuildTable(TUNER_A4STANDARD);

    m_initialized = true;
}

/**
 *  available
 *
 *  @return true if data is ready else false
 */
bool Tuner::available( void ) {
    bool flag = new_output;
    if ( flag ) new_output = false;
    return flag;
}

/**
 *  read processes the data samples for the Yin algorithm.
 *
 *  @return frequency in hertz
 */
float Tuner::read( void ) {
    float d = data;
    return AUDIO_SAMPLE_RATE_HZ / d;
}

/**
 *  Periodicity of the sampled signal from Yin algorithm from read function.
 *
 *  @return periodicity
 */
float Tuner::probability( void ) {
    float p = periodicity;
    return p;
}

// NOTE FINDER
const char *noteToLabel (const uint8_t note)
{
	return notetable[note].label;
}

const float noteToFreq (const uint8_t note, const uint8_t octave)
{
	return notetable[note].freq[octave];
}

float noteFindNearest (float freqIn, uint8_t *noteOut, uint8_t *octaveOut)
{
	float delta = 999999.0f;
	uint8_t note = -1;
	uint8_t octave = -1;

	for (int n = 0; n < 12; n++){
		for (int o = 0; o < NOTE_OCTAVES; o++){
			const float diff = fabsf(noteToFreq(n, o) - freqIn);
			if (diff < delta){
				delta = diff;
				note = n;
				octave = o;
			}
		}
	}

	if (note != -1){
		*noteOut = note;
		*octaveOut = octave;

		if (freqIn > noteToFreq(note, octave))
			return delta;
		else
			return -delta;
	}else{
		*noteOut = -1;
		return 0.0f;
	}
}

float noteGetLower (uint8_t *note, uint8_t *octave)
{
	int8_t octaveLower = *octave;
	int8_t noteLower = *note - 1;

	if (noteLower < NOTE_C){
		noteLower = NOTE_B;
		if (--octaveLower < 0)
			noteLower = -1;
	}

	if (noteLower != -1){
		float freq = noteToFreq(noteLower, octaveLower);
		*note = noteLower;
		*octave = octaveLower;
		return freq;
	}

	return 0.0f;
}

float noteGetHigher (uint8_t *note, uint8_t *octave)
{
	int8_t octaveHigher = *octave;
	int8_t noteHigher = *note + 1;

	if (noteHigher > NOTE_B){
		noteHigher = NOTE_C;
		if (++octaveHigher >= NOTE_OCTAVES){
			octaveHigher = -1;
			noteHigher = -1;
		}
	}

	if (noteHigher != -1){
		float freq = noteToFreq(noteHigher, octaveHigher);
		*note = noteHigher;
		*octave = octaveHigher;
		return freq;
	}

	return 0.0f;
}

static void buildOctave (float pitchStandard, int octave)
{
	octave -= 4;
	if (octave < -4){
		return;
	}else if (octave < 0){
		for (int d = octave+1; d <= 0; d++)
			pitchStandard /= 2.0f;
	}else if (octave > 0){
		for (int d = 0; d < octave; d++)
			pitchStandard *= 2.0f;
	}

	int noteIdx = 11;

	// A# and B
	for (float n = 2; n >= 1; n -= 1.0f){
		float freq = pitchStandard * exp2f(n/12.0f);
		notetable[noteIdx--].freq[octave+4] = freq;
	}

	// C to A
	for (float n = 0; n <= 9; n += 1.0f){
		float freq = pitchStandard / exp2f(n/12.0f);
		notetable[noteIdx--].freq[octave+4] = freq;
	}
}

static void noteBuildTable (const float A4)
{
	for (int o = 0; o < NOTE_OCTAVES; o++){
		buildOctave(A4, o);
	}
}

}
