// notes
  // HW & connections - 
    // SSD1306 OLED
      // Pins (ard): VCC|+5V, GND|GND, SCL|A5, SDA|A4
      // our OLED screens may not be using SSD1306 ICs
      // w/ 2 oscs, it may not update fast enough, could try SPI
      // could also try faster microcontroller
    // VCOs
      // each VCO will have an output
      // perhaps add a button/toggle (from MIDI input) to set both VCOs to C3 or C4 for tuning?
  // to do
    // port over the freq detection algorithm from the automatic tuner

// global
  // libs
    #include <Wire.h>
    #include <Adafruit_GFX.h>
    #include <Adafruit_SSD1306.h>
  // userDefs
    //#define SERIAL_ON
  // pins
    const uint8_t testIn1 = A0, testIn2 = A1;
  // defs
    const uint16_t SCREEN_WIDTH = 128, SCREEN_HEIGHT = 64; // OLED width&height (pixels)
    //const uint16_t noteFreqs[109] = {33,35,37,39,41,44,46,49,52,55,58,62,65,69,73,78,82,87,93,98,104,110,117,123,131,139,147,156,165,175,185,196,208,220,233,247,262,277,294,311,330,349,370,392,415,440,466,494,523,554,587,622,659,698,740,784,831,880,932,988,1047,1109,1175,1245,1319,1397,1480,1568,1661,1760,1865,1976,2093,2217,2349,2489,2637,2794,2960,3136,3322,3520,3729,3951,4186,4435,4699,4978,5274,5588,5920,6272,6645,7040,7459,7902,8372,8870,9397,9956,10548,11175,11840,12544,13290,14080,14917,15804,16744};
    const PROGMEM float noteFreqs[109] = { 32.70, 34.65, 36.71, 38.89, 41.20, 43.65, 46.25, 49.00, 51.91, 55.00, 58.27, 61.74, 65.41, 69.30, 73.42, 77.78, 82.41, 87.31, 92.50, 98.00, 103.83, 110.00, 116.54, 123.47, 130.81, 138.59, 146.83, 155.56, 164.81, 174.61, 185.00, 196.00, 207.65, 220.00, 233.08, 246.94, 261.63, 277.18, 293.66, 311.13, 329.63, 349.23, 369.99, 392.00, 415.30, 440.00, 466.16, 493.88, 523.25, 554.37, 587.33, 622.25, 659.25, 698.46, 739.99, 783.99, 830.61, 880.00, 932.33, 987.77, 1046.50, 1108.73, 1174.66, 1244.51, 1318.51, 1396.91, 1479.98, 1567.98, 1661.22, 1760.00, 1864.66, 1975.53, 2093.00, 2217.46, 2349.32, 2489.02, 2637.02, 2793.83, 2959.96, 3135.96, 3322.44, 3520.00, 3729.31, 3951.07, 4186.01, 4434.92, 4698.64, 4978.03, 5274.04, 5587.65, 5919.91, 6271.93, 6644.88, 7040.00, 7458.62, 7902.13, 8372.02, 8869.84, 9397.27, 9956.06, 10548.08, 11175.30, 11839.82, 12543.85, 13290, 14080, 14917, 15804, 16744 };
      const char noteLetters[] = { 'C', 'C', 'D', 'E', 'E', 'F', 'F', 'G', 'G', 'A', 'B', 'B' };
      const char noteSuffixes[12] = { ' ', '#', ' ', 'b', ' ', ' ', '#', ' ', '#', ' ', 'b', ' ' };
  // vars
    uint16_t lastFreq1 = 0, lastFreq2 = 0;
    uint8_t lastNoteNum = 0; // prob not needed, clean this out
  // init
    Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1); // decl SSD1306 SDI, I2C pins

void setup() {
  bool displayLaunched = display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // Address 0x3D for 128x64
  #ifdef SERIAL_ON
    Serial.begin(9600);
    if (!displayLaunched) Serial.println(F("SSD1306 allocation failed"));
  #endif
  //if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) Serial.println(F("SSD1306 allocation failed")); // Address 0x3D for 128x64
  
  display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    #ifdef TEST_PRINT
      display.println("Hello, World!");
      display.display(); 
    #endif
}

void loop() {
  // read in new frequency. if it's changed from last read, update the tuner
    uint16_t newFreq1 = analogRead(testIn1)+2400, newFreq2 = analogRead(testIn2)+640;
    if (newFreq1!=lastFreq1 || newFreq2!=lastFreq2) { // if freq
      display.clearDisplay();
      lastFreq1=newFreq1;
      lastFreq2=newFreq2;
      handleFreq(lastFreq1, false);
      handleFreq(lastFreq2, true);
      display.display();
    }

  // end of loop cleanup
    
    #ifdef WAIT_AT_END
      delay(2000);
    #endif
}

// note funcs
  void handleFreq(float newFreq, bool isVco2) {
    uint8_t upperBound = getUpperBound(newFreq);
    uint8_t lowerBound = upperBound; // not needed if upperBound==0 || upperBound==109
      bool freqIsLower=false, freqIsHigher=false;
      uint8_t numTicks = 0;
      float freqLog = log(newFreq), lowerLog, upperLog, lowerDif, upperDif; // calc logs
    if (upperBound==0) { // can't happen in test case
      freqIsLower=true;
      upperLog = log(pgm_read_float_near(noteFreqs + upperBound));
      upperDif = upperLog-freqLog;
    }
    else if (upperBound==109) { // can't happen in test case
      lowerBound = 108;
      freqIsHigher=true;
      lowerLog = log(pgm_read_float_near(noteFreqs + lowerBound));
      lowerDif = freqLog-lowerLog;
    }
    else {
      lowerBound = upperBound-1;
        lowerLog = log(pgm_read_float_near(noteFreqs + lowerBound));
        upperLog = log(pgm_read_float_near(noteFreqs + upperBound));
      lowerDif = freqLog-lowerLog;
        upperDif = upperLog-freqLog;
      freqIsLower = lowerDif>=upperDif;
        freqIsHigher = !freqIsLower;
      lastNoteNum = freqIsLower ? upperBound : lowerBound;
    }
    display.setCursor(55, 10+(isVco2*20));
    printNote(lastNoteNum);
    display.println();
    //display.print(F("test_test"));
    if (freqIsLower) { // input freq lower than the note we're trying to get to
      numTicks = 10*upperDif/0.025155486;
      numTicks = numTicks>10 ? 10 : numTicks; // yes, this is cheating
      for (uint8_t i=10; i>0; i--) {
        if (i<=numTicks) display.print(F("|"));
        else display.print(F(" "));
      }
      display.print(F("O          "));
    }
    else { // if freqIsHigher
      numTicks = 10*lowerDif/0.025155486;
      numTicks = numTicks>10 ? 10 : numTicks; // yes, this is cheating
      display.print(F("          O"));
      for (uint8_t i=1; i<11; i++) if (i<=numTicks) display.print(F("|"));
    }
    #ifdef SERIAL_ON
    Serial.println(); Serial.print(F("newFreq=")); Serial.print(newFreq); Serial.print(F("Hz, log(newFreq)=")); Serial.print(freqLog, 6);
      Serial.println(); Serial.print(F("lowerLog=")); Serial.print(lowerLog, 6); Serial.print(F(", lowerDif=")); Serial.print(lowerDif, 6);
      Serial.println(); Serial.print(F("upperLog=")); Serial.print(upperLog, 6); Serial.print(F(", upperDif=")); Serial.print(upperDif, 6);
      Serial.println();    
      if (freqIsLower) Serial.print(F("Freq is lower than nearest note "));
        else Serial.print(F("Freq is higher than nearest note "));
        printNoteSerial(lastNoteNum); Serial.print(F(" {noteNum=")); Serial.print(lastNoteNum); 
        Serial.print(F("} {")); Serial.print(F("Ticks=")); Serial.print(numTicks); Serial.print(F("}"));
    #endif
  }
  void printNote(uint8_t noteNumToPrint) {
    uint8_t twelveTone = noteNumToPrint % 12;
    display.print(noteLetters[twelveTone]);
    if(!isSpace(noteSuffixes[twelveTone])) display.print(noteSuffixes[twelveTone]);
    uint8_t testVar; testVar=noteNumToPrint/12;
    display.print(testVar);
  }
  uint8_t getUpperBound(uint16_t freq) {
    for (uint8_t i=0; i<109; i++) if (freq<pgm_read_float_near(noteFreqs + i)) return i;
    return 109;  
  }


// test(/old) funcs
  #ifdef SERIAL_ON
  void printNoteSerial(uint8_t noteNumToPrint) {
    uint8_t twelveTone = noteNumToPrint % 12;
    Serial.print(noteLetters[twelveTone]);
    if(!isSpace(noteSuffixes[twelveTone])) Serial.print(noteSuffixes[twelveTone]);
    uint8_t testVar; testVar=noteNumToPrint/12;
    Serial.print(testVar);
  }
  #endif
  void printSetupComplete() {
    display.clearDisplay();

    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    // Display static text
    //display.println("Hello, World!");
    display.println();
      display.print(F("Current Note ")); // so 21 across are avail
      display.println();
      //uint8_t noteNum = analogRead(testIn1)>>3;
      uint8_t noteNum = lastNoteNum;
      uint8_t twelveTone = noteNum % 12;
      display.print(noteLetters[twelveTone]);
      if(!isSpace(noteSuffixes[twelveTone])) display.print(noteSuffixes[twelveTone]);
      uint8_t testVar; testVar=noteNum/12;
      display.print(testVar);
    display.display();
  }
  
  void oldLoop() {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 10);
    display.println();
      display.print(F("Hello, World!"));
    display.display();
  }