#include <Wire.h>
#include <SPI.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#define SCREEN_WIenc_dtH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
//Adafruit_SSD1306 display(OLED_RESET);
#define  OLED_ADDR   0x3C

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIenc_dtH, SCREEN_HEIGHT, &Wire, OLED_RESET);
#define TEXTSIZE 2 //Set this back to 1 for smaller text

#define pinEnc_clk 2  //This Pin must support interrupts eg 2 or 3 on UNO R3
#define enc_dt 3
#define SW 6
int v1;
int v2;
int menuCount = 0;
int btnState;


#define DEBOUNCE 20 //Minimum time between rotary clicks being recognised
volatile int counter = 0, aState, aLastState  ;
volatile unsigned long lastChange = 0 ;
bool changed = false;
unsigned long lastButtonPress = 0;

void setup() {
  Serial.begin (115200);
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
  display.display();
  display.clearDisplay();

  pinMode (SW, INPUT_PULLUP);
  pinMode (pinEnc_clk, INPUT_PULLUP);
  pinMode (enc_dt, INPUT_PULLUP);

  
  // Reads the initial state of the pinEnc_clk
  aLastState = digitalRead(pinEnc_clk);
  attachInterrupt(digitalPinToInterrupt(pinEnc_clk), encoderInterrupt, CHANGE);
  Serial.println("Set up complete");

  staticMenu();
}

void loop() {

  if (changed) {
    changed = false;
    Serial.print("Position: ");
    Serial.println(counter);
    updateValue();
    staticMenu();

  }
  btnState = digitalRead(SW);
  menuCheck();

  delay(1);
 
}

/*
 //set up interrupt driven rotary encoder connections
  volatile int pinEnc_pinEnc_clk = 2;   // Sets rotary encoder clock pin variable
  volatile int enc_enc_dt = 3;    // Sets rotary encoder enc_dt pin variable
  int col_col_select = 4;         // Sets rotary encoder PB switch pin variable
  volatile int count = 0;               // IniencoderPinpinEnc_clktialises count variable to 0
  volatile int enc_pinEnc_clkLast = LOW;       // Sets comparator variable
  volatile int enc_pinEnc_clkCurrent = LOW;    // Sets comparator variable

  void encoderInterrupt() {
  enc_pinEnc_clkCurrent = digitalRead(pinEnc_pinEnc_clk);
  if ((enc_pinEnc_clkLast == LOW) && (enc_pinEnc_clkCurrent == HIGH)) {
    if (digitalRead(enc_enc_dt) == HIGH) {
      count--;
    }
    else {
      count++;
    }
    //Serial.println(count);
  }
  enc_pinEnc_clkLast = enc_pinEnc_clkCurrent;
  }
*/

void encoderInterrupt() {

  aState = digitalRead(pinEnc_clk); // Reads the "current" state of the pinEnc_clk
  // If the previous and the current state of the pinEnc_clk are different, that means a Pulse has occured
  if (aState != aLastState) {
    // If the enc_dt state is different to the pinEnc_clk state, that means the encoder is rotating clockwise
    if ((millis() - lastChange) > DEBOUNCE) { //Ignore repeated bounce of contacts
      changed = true; //Set flag to say change has happened
      if (digitalRead(enc_dt) != aState  ) {
        counter ++;
      } else {
        counter --;
      }
    }
  }
  lastChange = millis();
  aLastState = aState; // Updates the previous state of the pinEnc_clk with the current state
}

void updateValue() {
  switch (menuCount) {
    case 0:
      v1 = counter;
      break;
    case 1:
      // statements
      v2 = counter;
      break;
    default:
      // statements
      break;
  }

}


void menuCheck() {

  if (btnState == LOW ) {
    if (millis() - lastButtonPress < 30) {
      return;
    }
    lastButtonPress = millis();
    Serial.println("Button press");
    menuCount = ++menuCount % 3; //Values 0,1,2

    Serial.print("Menu now ");
    Serial.println(menuCount);
    counter = 0;
    staticMenu();
  }
}

void staticMenu() {
  display.setTextSize(TEXTSIZE);
  display.setTextColor(WHITE, BLACK);
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("   MENU ");
  display.println(menuCount);

  if (menuCount == 0) {
    display.setTextColor(BLACK, WHITE);
  } else {
    display.setTextColor(WHITE, BLACK);
  }

  display.print("value 1:");
  display.println(v1);

  if (menuCount == 1) {
    display.setTextColor(BLACK, WHITE);
  } else {
    display.setTextColor(WHITE, BLACK);
  }
  display.print("value 2:");
  display.println(v2);

  if (menuCount == 2) {
    display.setTextColor(BLACK, WHITE);
  } else {
    display.setTextColor(WHITE, BLACK);
  }
  display.println("Ready!");

  display.display();
}