/* Instrument tuner using the FRDM-KL25Z Board. Written for EEE212 Term Project.
 * Miraç L. Gülgönül - S. Çağatay Çelebi
 * 20 May 2019
 */

#include "mbed.h"
#include "NVIC_set_all_priorities.h"
#include <ctype.h>
#include "arm_math.h"
#include "arm_const_structs.h"
#include "FastAnalogIn.h"
#include "TextLCD.h"
#include <algorithm>
#include <string>
#include "Adafruit_GFX.h"
#include "Adafruit_GFX_Config.h"
#include "Adafruit_SSD1306.h"
#include "glcdfont.h"

using namespace std;

I2C comms(PTC9, PTC8);

Adafruit_SSD1306_I2c oled(comms, PTC3, 0x78, 64, 128);

FastAnalogIn   Audio(PTC2); // INPUT

// Dummy ISR for disabling NMI on PTA4.
// More info at https://mbed.org/questions/1387/How-can-I-access-the-FTFA_FOPT-register-/
extern "C" void NMI_Handler()
{
    DigitalIn test(PTA4);
}


// A guitar is at max from C2 to F4: which gives 65.41Hz to 349.23Hz
// Supported highest freq is 493 Hz, B4.
const int SAMPLE_RATE_HZ = 1000;
const int WINDOW_SIZE = 1024; // WAS 1024
const int NOTE_SIZE = 39;
float max_amp = 0;
int max_idx = 0;
int max_idxs[WINDOW_SIZE];
int window_idx = 0;
const int FFT_SIZE = 512; // Size of the FFT.

const char * const notes[NOTE_SIZE] = {
    "A1", "A#1", "B1", "C2", "C#2", "D2", "D#2", "E2", "F2", "F#2", "G2", "G#2",
    "A2", "A#2", "B2", "C3", "C#3", "D3", "D#3", "E3", "F3", "F#3", "G3", "G#3",
    "A3", "A#3", "B3", "C4", "C#4", "D4", "D#4", "E4", "F4", "F#4", "G4", "G#4",
    "A4", "A#4", "B4",
};

const int note_freqs[NOTE_SIZE] = {
    55, 58, 62, 65, 69, 73, 78, 82, 87, 92, 98, 103, 110, 116, 123,
    131, 139, 147, 156, 165, 175, 185, 196, 208, 220, 233, 246, 261,
    277, 294, 311, 330, 349, 370, 392, 415, 440, 466, 493
};

int differences[39];
int min_diff = 1000;
int min_diff_idx = 10;
string closest_note = notes[min_diff_idx];

const static arm_cfft_instance_f32 *S;
Ticker samplingTimer;
float samples[FFT_SIZE*2];
float magnitudes[FFT_SIZE];
int sampleCounter = 0;


////////////////////////////////////////////////////////////////////////////////
// SAMPLING FUNCTIONS
////////////////////////////////////////////////////////////////////////////////

void samplingCallback()
{
    // Read from the ADC and store the sample data
    samples[sampleCounter] = (1023 * Audio) - 511.0f;
    // Complex FFT functions require a coefficient for the imaginary part of the input.
    // Since we only have real data, set this coefficient to zero.
    samples[sampleCounter+1] = 0.0;
    // Update sample buffer position and stop after the buffer is filled
    sampleCounter += 2;
    if (sampleCounter >= FFT_SIZE*2) {
        samplingTimer.detach();
    }
}

void samplingBegin()
{
    // Reset sample buffer position and start callback at necessary rate.
    sampleCounter = 0;
    samplingTimer.attach_us(&samplingCallback, 1000000/SAMPLE_RATE_HZ);
}

bool samplingIsDone()
{
    return sampleCounter >= FFT_SIZE*2;
}



////////////////////////////////////////////////////////////////////////////////
// MAIN FUNCTION
////////////////////////////////////////////////////////////////////////////////

int main()
{
    NVIC_set_all_irq_priorities(1);
    NVIC_SetPriority(UART0_IRQn, 0);
    oled.setTextSize(3);
    // Begin sampling audio
    samplingBegin();

    S = & arm_cfft_sR_f32_len512;

    while(1) {

        // Calculate FFT if a full sample is available.
        if (samplingIsDone()) {
            // Does a Hanning on samples:
            for (int i = 0; i < FFT_SIZE*2; i++) {
                if (i % 2 == 0) {
                    samples[i] = samples[i] * (1-cos(2*3.141592653589793/(FFT_SIZE)));
                } else {
                    samples[i] = 0.0;
                }
            }
            // Run FFT on sample data.
            arm_cfft_f32(S, samples, 0, 1);
            // Calculate magnitude of complex numbers output by the FFT.
            arm_cmplx_mag_f32(samples, magnitudes, FFT_SIZE);

            // Restart audio sampling.
            samplingBegin();
        }

        max_amp = 0;
        max_idx = 0;

        for(int i = 1; i < FFT_SIZE; i++) {
            if(magnitudes[i] > max_amp && i < 256) {
                max_amp = magnitudes[i];
                max_idx = i;
            }
        }

        max_idxs[window_idx] = max_idx;

        if(window_idx == WINDOW_SIZE - 1) {
            window_idx = 0;
            // sorts the array
            sort(max_idxs, max_idxs+100);

            // finds the most repeated element
            int repeat = max_idxs[0];
            int mode = repeat;
            int mode_freq = mode;
            int count = 1;
            int count_mode = 1;

            for(int i = 1; i < WINDOW_SIZE; i++) {
                if(max_idxs[i] == repeat) {
                    ++count;
                } else {
                    if(count > count_mode) {
                        count_mode = count;
                        mode = repeat;
                    }
                    count = 1;
                    repeat = max_idxs[i];
                }
            }
            mode_freq = static_cast<int>(floor(mode*1.953125)); // for FFT_SIZE = 512, Fs = 1000.

            for(int i = 0; i < NOTE_SIZE; i++) {
                differences[i] = mode_freq - note_freqs[i];
            }

            min_diff = 1000;
            min_diff_idx = 10;

            for(int i = 0; i < NOTE_SIZE; i++) {
                if(abs(differences[i]) < min_diff) {
                    min_diff = differences[i];
                    min_diff_idx = i;
                }
            }

            closest_note = notes[min_diff_idx];
            char mode_freq_str[10];
            char closest_note_char[10];
            char min_diff_char[10];

            sprintf(closest_note_char, "%s", closest_note);
            sprintf(mode_freq_str, "%d", mode_freq);
            sprintf(min_diff_char, "%d", -1*min_diff);

            oled.clearDisplay();
            oled.setTextCursor(0,0);

            for(int i = 0; i < 3 ; i++) {
                oled.writeChar(mode_freq_str[i]);
            }
            oled.writeChar('\n');

            for(int i = 0; i < 3 ; i++) {
                oled.writeChar(closest_note_char[i]);
            }
            oled.writeChar(' ');
            oled.writeChar(' ');


            for(int i = 0; i < 2 ; i++) {
                oled.writeChar(min_diff_char[i]);
            }

            oled.display();

        } else {
            window_idx = window_idx + 1;
        }
    }
}