#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);

//    FILE: frequencyDSP.ino
//  AUTHOR: rob tillaart (org version akellyirl)
// VERSION: 0.1.03
// PURPOSE: frequency analysis using DSP
//    DATE: 2013-10-27
//     URL: http://www.instructables.com/id/Reliable-Frequency-Detection-Using-DSP-Techniques

float frequency;
String pitchhhh;
float abweichung;

const uint16_t size = 500;
char rawData[size];

volatile int index = 0;

ISR(ADC_vect) {  //when new ADC value ready

  if (index < size) {
    rawData[index++] = ADCH - 128;
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("  ");

  lcd.init();
  lcd.clear();
  lcd.backlight();

  cli();  //diable interrupts

  //set up continuous sampling of analog pin 0 at 38.5kHz

  ADCSRA = 0;  //clear ADCSRA register
  ADCSRB = 0;  //clear ADCSRB register

  ADMUX |= (1 << REFS0);  //set reference voltage
  ADMUX |= (1 << ADLAR);  //left align the ADC value- so we can read highest 8 bits from ADCH register only

  ADCSRA |= (1 << ADPS2) | (1 << ADPS0);  //set ADC clock with 32 prescaler- 16mHz/32=500kHz
  ADCSRA |= (1 << ADATE);                 //enabble auto trigger
  ADCSRA |= (1 << ADIE);                  //enable interrupts when measurement complete
  ADCSRA |= (1 << ADEN);                  //enable ADC
  ADCSRA |= (1 << ADSC);                  //start ADC measurements

  sei();  //enable interrupts
}

void loop() {

  intsim(rawData, size, 38462);


  if (index >= size) {
    cli();  //diable interrupts
    frequency = autoCorrelateFrequency(rawData, size, 38462);
    pitchhhh = pitch(frequency);
    abweichung = abweich(frequency);

    Serial.println(frequency);
    Serial.println(pitchhhh);
    Serial.println(abweichung);

    index = 0;

    sei();  //enable interrupts
  }

  lcd.setCursor(0, 0);
  lcd.print("frequ:");
  lcd.print(frequency, 2);
  lcd.print("Hz");
  lcd.print("              ");

  lcd.setCursor(0, 1);
  lcd.print("pitch:");
  lcd.print(pitchhhh);
  lcd.print(" ");
  lcd.print(abweichung, 2);
  lcd.print("%");
  lcd.print("              ");

}

void intsim(char* sample, int len, float sampleFreq) {

  float freq = 1000.0;

  if (index >= len) return;

  for (index = 0; index < len; index++) {
    sample[index] = 127.0 * sin(index * 2.0 * PI * freq / sampleFreq);
    ;
  }
}

float autoCorrelateFrequency(char* sample, int len, float sampleFreq) {
  long sum = 0;
  long sum_old = 0;
  int thresh = 0;
  byte pd_state = 0;
  int period = 0;  // 0 results in inf

  // Autocorrelation
  for (int i = 0; i < len && (pd_state != 3); i++) {
    sum_old = sum;
    sum = 0;

    for (int k = 0; k < len - i; k++) {
      sum += sample[k] * sample[k + i];
    }
    sum /= 256;

    // Peak Detect State Machine
    // 0: initial, set threshold
    // 1: detect positive slope
    // 2: detect negative slope
    // 3: peak found
    if (pd_state == 0) {
      thresh = sum / 2;
      pd_state = 1;
    } else if (pd_state == 1 && (sum > thresh) && (sum - sum_old) > 0) {
      pd_state = 2;
    } else if (pd_state == 2 && (sum - sum_old) <= 0) {
      period = i;
      pd_state = 3;
    }
  }

  return sampleFreq / period;
}


String pitch(float frequency) {

  static const String pitch1[] = {
    "G#", "A", "A#", "B", "C", "C#", "D", "D#", "E", "F", "F#", "G"
  };

  static const String pitch2[] = {
    "Ab", "A", "Bb", "B", "C", "Db", "D", "Eb", "E", "F", "Gb", "G"
  };

  float n = 49 + 12 * log(frequency / 440.0) / log(2);

  if (n < 0) {
    return pitch1[11 + (int(n + 0.5) % 12)] + String((int(n + 0.5) - 4) / 12);
  } else {
    return pitch1[int(n + 0.5) % 12] + String((int(n + 0.5) + 8) / 12);
  }
}



float abweich(float frequency) {

  float n = 49 + 12 * log(frequency / 440.0) / log(2);

  //  if (n >= 0) {
  float center = 100 * (n - int(n + 0.5));
  //}

  return center;
}