#include "Yin.h"
Yin yin;

#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.0;

const int bufferSize = 2048;
int16_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);
    //rawData[bufferIndx++] = 120 * sin(bufferIndx * 2.0 * PI * 440.0 / sampleFrequency);
  }
  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);

  Yin_init(&yin, bufferSize, sampleFrequency, 0.15);

  beginTimer(sampleFrequency);
}

void loop() {

  static unsigned long elapse = micros();

  if (bufferFull) {

    elapse = micros() - elapse;

    timer.stop();

    digitalWrite(LED_BUILTIN, HIGH);

    float frequency = Yin_getPitch(&yin, rawData);
    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();
  }
}

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