#define LED_BUILTIN 2

#include "Yin.h"
Yin yin;

#include <driver/adc.h>
#include <driver/timer.h>

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

hw_timer_t* timer = NULL;

float sampleFrequency = 22000.0;

const int bufferSize = 2048;
int16_t rawData[bufferSize];  // Buffer for ADC capture

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

void IRAM_ATTR onTimer() {
  if (bufferIndx < bufferSize) {
    rawData[bufferIndx++] = adc1_get_raw(ADC1_CHANNEL_6);  // direkter Rohwert (0–4095) (GPIO 34)
  }
  if (bufferIndx >= bufferSize) bufferFull = 1;
}
void setup() {

  Serial.begin(115200);
  Serial.println("  ");

  pinMode(LED_BUILTIN, OUTPUT);

  u8g2.initDisplay();
  u8g2.setContrast(30);
  u8g2.setFlipMode(0);
  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();

  adc1_config_width(ADC_WIDTH_BIT_12);                         // 0 - 4095 range
  adc1_config_channel_atten(ADC1_CHANNEL_6, ADC_ATTEN_DB_11);  //0.0 – 3.9 V (GPIO 34)

  delay(2000);

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

  timer = timerBegin(20000000);
  timerAttachInterrupt(timer, &onTimer);
  timerAlarm(timer, 20000000 / sampleFrequency, true, 0);
}

void loop() {

  static unsigned long elapse = micros();

  if (bufferFull) {

    elapse = micros() - elapse;

    timerStop(timer);

    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;

    timerStart(timer);

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