#include <U8g2lib.h>
//U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE);
U8G2_SH1106_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE);

#include "FspTimer.h"

FspTimer timer;

float sampleFrequency = 32000;

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

volatile int bufferFull = 0;
volatile int bufferIndx = 0;

void timer_callback(timer_callback_args_t __attribute((unused)) * p_args) {
  if (bufferIndx < bufferSize) {
    rawData[bufferIndx++] = analogRead(A0) >> 2;
  }
  if (bufferIndx >= bufferSize) bufferFull = 1;
}

bool beginTimer(float rate) {
  uint8_t timer_type = GPT_TIMER;
  int8_t tindex = FspTimer::get_available_timer(timer_type);
  if (tindex < 0) {
    tindex = FspTimer::get_available_timer(timer_type, true);
  }
  if (tindex < 0) {
    return false;
  }

  FspTimer::force_use_of_pwm_reserved_timer();

  if (!timer.begin(TIMER_MODE_PERIODIC, timer_type, tindex, rate, 0.0f, timer_callback)) {
    return false;
  }

  if (!timer.setup_overflow_irq()) {
    return false;
  }

  if (!timer.open()) {
    return false;
  }

  if (!timer.start()) {
    return false;
  }
  return true;
}

void setup() {

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

  u8g2.initDisplay();
  u8g2.setContrast(30);
  u8g2.setFlipMode(2);
  u8g2.setPowerSave(0);

  u8g2.clearBuffer();
  u8g2.drawRFrame(0, 0, 128, 64, 5);
  u8g2.setFont(u8g2_font_t0_22b_mf);
  u8g2.drawStr(4, 18, "samplingfrq");
  char strf[20];
  dtostrf(sampleFrequency, 0, 2, strf);
  u8g2.drawStr((128 - u8g2.getUTF8Width(strf)) / 2, 38, strf);
  u8g2.drawStr(53, 58, "Hz");
  u8g2.sendBuffer();
  delay(3000);

  beginTimer(sampleFrequency);
}

void loop() {

  static unsigned long elapse = micros();

  if (bufferFull) {

    elapse = micros() - elapse;

    timer.stop();

    digitalWrite(LED_BUILTIN, HIGH);

    float frequency = findFrequency(rawData, bufferSize, sampleFrequency);
    char * ptr = pitch(frequency);
    float dev = deviation(frequency);

    Serial.print(String(frequency, 2) + "Hz\t" + ptr + "\t" + String(dev) + "%\t");
    float test = 1e6 * bufferSize / elapse;
    Serial.println(test);

    static char strf[20];
    static char strq[99];

    u8g2.clearBuffer();
    u8g2.drawRFrame(0, 0, 128, 64, 5);
    u8g2.setFont(u8g2_font_t0_22b_mf);
    snprintf(strq, 99, "%s%s", dtostrf(frequency, 0, 2, strf), "Hz");
    u8g2.drawStr((128 - u8g2.getUTF8Width(strq)) / 2, 18, strq);
    snprintf(strq, 99, "%s", ptr);
    u8g2.drawStr((128 - u8g2.getUTF8Width(strq)) / 2, 38, strq);
    snprintf(strq, 99, "%s%s", dtostrf(dev, 0, 2, strf), "%");
    u8g2.drawStr((128 - u8g2.getUTF8Width(strq)) / 2, 58, strq);
    u8g2.sendBuffer();

    digitalWrite(LED_BUILTIN, LOW);

    bufferFull = 0;
    bufferIndx = 0;

    timer.start();

    elapse = micros();
  }
}

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));
  }
}