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

float sampleFrequency;

const int bufferSize = 1024;
uint8_t rawData[bufferSize];  // Buffer for ADC capture

volatile int sampleCnt = 0;
volatile int timerstop = 0;

uint8_t SIMULATION() {

  float a1 = 20, a2 = 20, a3 = 20, a4 = 20;      //amplitude
  float f1 = 220, f2 = 330, f3 = 440, f4 = 550;  //frequency

  f1 = a1 * sin(sampleCnt * 2.0 * PI * f1 / sampleFrequency);
  f2 = a2 * sin(sampleCnt * 2.0 * PI * f2 / sampleFrequency);
  f3 = a3 * sin(sampleCnt * 2.0 * PI * f3 / sampleFrequency);
  f4 = a4 * sin(sampleCnt * 2.0 * PI * f4 / sampleFrequency);

  float f = f1 + f2 + f3 + f4;

  //Serial.println(f);

  if (f < -127) f = -127;
  else if (f > 127) f = 127;

  return 128 + f;
}

ISR(ADC_vect) {

  if (sampleCnt < bufferSize) {
    //rawData[sampleCnt] = ADCH;
    rawData[sampleCnt] = SIMULATION();
    sampleCnt++;
  } else timerstop = 1;

  // Start the next conversion
  ADCSRA |= (1 << ADSC);
}

void setup() {

  Serial.begin(115200);
  Serial.println("  ");
  pinMode(LED_BUILTIN, OUTPUT);

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

  cli();  //disable interrupts

  //set up sampling of analog pin 0
  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

  switch (32) {

    case 16:
      sampleFrequency = 76923.0769;
      ADCSRA |= (1 << ADPS2);  //set ADC clock with 16 prescaler- 16mHz/16=1000kHz/13 ->76923 sps
      break;
    case 32:
      sampleFrequency = 38461.5384;
      ADCSRA |= (1 << ADPS2) | (1 << ADPS0);  //set ADC clock with 32 prescaler- 16mHz/32=500kHz/13 ->38461 sps
      break;
    case 64:
      sampleFrequency = 19230.7692;
      ADCSRA |= (1 << ADPS2) | (1 << ADPS1);  //set ADC clock with 64 prescaler- 16mHz/64=250kHz/13 ->19230 sps
      break;
    case 128:
      sampleFrequency = 9615.3846;
      ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);  // ADC clock 128 prescaler- 16mHz/128=125kHz/13->9615 sps
      break;
  }

  ADCSRA |= (1 << ADATE);  //enable 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() {

  if (timerstop) {

    digitalWrite(LED_BUILTIN, HIGH);

    float frequency = findFrequency(rawData, bufferSize, sampleFrequency);
    Serial.println(String(frequency, 2) + "Hz\t" + pitch(frequency) + "\t" + String(deviation(frequency)) + "%");

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

    lcd.setCursor(0, 1);
    lcd.print(pitch(frequency));
    lcd.print(F(" "));
    lcd.print(deviation(frequency));
    lcd.print(F("%               "));

    digitalWrite(LED_BUILTIN, LOW);

    sampleCnt = 0;
    timerstop = 0;
  }
}

float findFrequency(uint8_t* rawData, int bufferSize, float sampleFrequency) {

  // Calculate mean to remove DC offset
  long meanSum = 0;
  for (int k = 0; k < bufferSize; k++) {
    meanSum += rawData[k];
  }
  uint8_t mean = meanSum / bufferSize;

  // Autocorrelation
  long twoPreviousSum = 0;
  long previousSum = 0;
  long currentSum = 0;
  long threshold = 0;
  int pdState = 0;
  float period = 0;

  for (int i = 0; i < bufferSize && (pdState != 3); i++) {
    // Autocorrelation
    twoPreviousSum = previousSum;
    previousSum = currentSum;
    currentSum = 0;
    for (int k = 0; k < bufferSize - i; k++) {
      currentSum += (rawData[k] - mean) * (rawData[k + i] - mean);
    }
    // Peak detection
    switch (pdState) {
      case 0:  // Set threshold based on zero lag autocorrelation
        threshold = currentSum / 2;
        pdState = 1;
        break;
      case 1:  // Look for over threshold and increasing
        if ((currentSum > threshold) && (currentSum - previousSum) > 0)
          pdState = 2;
        break;
      case 2:  // Look for decreasing (past peak over threshold)
        if ((currentSum - previousSum) <= 0) {
          // quadratic interpolation
          float interpolationValue = 0.5 * (currentSum - twoPreviousSum) / (2 * previousSum - twoPreviousSum - currentSum);
          period = i - 1 + interpolationValue;
          pdState = 3;
        }
        break;
      default:
        pdState = 3;
        break;
    }
  }

  if (period <= 0) return 0;
  float frequency = sampleFrequency / period;
  if (threshold < 100 || frequency > 10000) return 0;
  return frequency;
}

char* pitch(float frequency) {

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

  if (frequency <= 0.0) return "no pitch";

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

  int notenr;
  int octave;

  if (n < 0) {
    notenr = 11 + ((int)(n + 0.5) % 12);
    octave = ((int)(n + 0.5) - 4) / 12;
  } else {
    notenr = (int)(n + 0.5) % 12;
    octave = ((int)(n + 0.5) + 8) / 12;
  }
  static char MMM[99];

  switch (notenr) {
    case 1:
    case 3:
    case 4:
    case 6:
    case 8:
    case 9:
    case 11:
      snprintf(MMM, 99, "%s%d", pitch1[notenr], octave);
      return MMM;

    case 0:
    case 2:
    case 5:
    case 7:
    case 10:
      snprintf(MMM, 99, "%s%d/%s%d", pitch1[notenr], octave, pitch2[notenr], octave);
      return MMM;
    default:
      return "error";
  }
}

float deviation(float frequency) {

  if (frequency <= 0.0) return 0;

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

  if (n >= 0) {
    return 100 * (n - (int)(n + 0.5));
  } else {
    return 100 * (n - (int)(n - 0.5));
  }
}