#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
//#include <Adafruit_BME280.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

struct Point_t {
  int x, y;
};

struct Timer_t {
  long iAcc, iSp, iDc;
  bool bRunning, bLoop;

  Timer_t(long sp = 0, long dc = 0, bool l = false) {
    iAcc = 0;
    iSp = sp;
    iDc = dc;
    bRunning = false;
    bLoop = l;
  }

  bool loop(bool run) {
    if (!bRunning && run) {
      bRunning = true;
      iAcc = millis();
    }

    if (!run)
      bRunning = false;

    if (bRunning && !bLoop) {
      if (millis() - iAcc >= iSp)
        return true;
      return false;
    }

    if (bRunning && bLoop) {
      if (millis() - iAcc >= iSp)
        iAcc = millis();

      if (millis() - iAcc >= iDc)
        return true;
      return false;
    }

    return false;
  }
};

Point_t gear_pos[7] = {
  {-74, -24}, 
  {-37, -24}, {-37, 24}, 
  {  0, -24}, {  0, 24}, 
  { 37, -24}, { 37, 24}
};

Timer_t timer_enable(3000);
Timer_t timer_next(20);
Timer_t timer_blink(1000, 500, true);

int iCalibrate = -1;
bool bButtonBack = false;

void setup()
{
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(6, INPUT_PULLUP);

  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) 
    for (;;);
  
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
}

void loop() 
{
  if (timer_enable.loop(!digitalRead(6)) && iCalibrate < 0)
    iCalibrate = 8;
  bool bBlinker = timer_blink.loop(iCalibrate >= 0);

  int px = 0;
  int py = 0;

  if (iCalibrate == -1) {
    bButtonBack = false;
  }
  else if (iCalibrate == 8) {
    if (digitalRead(6))
      iCalibrate = 0;
  }
  else if (iCalibrate >= 0) {
    bool button = timer_next.loop(!digitalRead(6));

    if (button && !bButtonBack)
      iCalibrate++;
    
    bButtonBack = button;
    if (iCalibrate >= 7) {
      iCalibrate = -1;
      return;
    }
    
    px = gear_pos[iCalibrate].x;
    py = gear_pos[iCalibrate].y;
  }

  display.clearDisplay();
  
  display.setCursor(3, 1);
  display.write("R");
  if (iCalibrate >= 0) {
    display.setTextSize(1);
    display.setCursor(0, 57);
    display.write("Cal");
    display.setTextSize(2);
  }
  display.setCursor(40, 1);
  display.write("1");
  display.setCursor(40, 49);
  display.write("2");
  display.setCursor(77, 1);
  display.write("3");
  display.setCursor(77, 49);
  display.write("4");
  display.setCursor(114, 1);
  display.write("5");
  display.setCursor(114, 49);
  display.write("6");

  display.drawLine(  7, 16,   7, 31, SSD1306_WHITE);
  display.drawLine( 45, 16,  45, 46, SSD1306_WHITE);
  display.drawLine( 82, 16,  82, 46, SSD1306_WHITE);
  display.drawLine(119, 16, 119, 46, SSD1306_WHITE);
  display.drawLine(  7, 31, 119, 31, SSD1306_WHITE);

  if ((iCalibrate >= 0 && bBlinker) || iCalibrate < 0) {
    display.fillCircle(px + 82, py + 31, 9, SSD1306_INVERSE);
  }
    
  display.display();

  delay(10);
}