#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#define OLED_ADDR   0x3C
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// 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_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


#define encoder0PinA  2
#define encoder0PinB  3
volatile unsigned int encoder0Pos = 0;

int valA;
int valB;
int valC;

byte clk;
byte menuCount = 1;
byte dir = 0;
bool runState = false;

void setup() {
  pinMode(6,OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  
  Serial.begin(9600);

  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
  display.display();
  display.clearDisplay();

  attachInterrupt(0, doEncoder, CHANGE);  // encoder pin on interrupt 0 - pin 2
}

void loop() {
  clk = digitalRead(4);
  ledControl();
  menuCheck();
  staticMenu();

  display.clearDisplay();
  delay(50);
}

void staticMenu() {
  display.setTextSize(2);

  display.setTextColor(WHITE);

  display.setCursor(10, 0);
  display.println("yyRobotic");

  //---------------------------------
  display.setTextSize(1);
  display.setCursor(10,
  20);
  display.println("Value A:");
  display.setCursor(60, 20);
  display.println(valA);


  display.setCursor(10, 30);
  display.println("Value B:");
  display.setCursor(60,
  30);
  display.println(valB);

  display.setCursor(10, 40);
  display.println("Value C:");
  display.setCursor(60, 40);
  display.println(valC);

  display.setCursor(10,
  50);
  display.println("Start:");
  display.setCursor(45, 50);
  if (encoder0Pos
  > 5 && menuCount == 4) {
    display.println("Run!");
    runState = true;

  } else {
    display.println("Idle");
    runState = false;
  }

  display.setCursor(2, (menuCount * 10) + 10);
  display.println(">");


  display.display();
}

void ledControl() {
  if (runState == true)
  {
    analogWrite(6, valA);
    analogWrite(9, valB);
    analogWrite(10,
  valC);
  }
}
void menuCheck() {
  if (clk == LOW && menuCount < 5) {

    menuCount++;
    encoder0Pos = 0;
  }
  if (clk == LOW && menuCount
  >= 5) {
    menuCount = 1;
  }

  if (menuCount == 1) {
    valA
  = encoder0Pos;
  }
  if (menuCount == 2) {
    valB = encoder0Pos;

  }
  if (menuCount == 3) {
    valC = encoder0Pos;
  }
}

void
  doEncoder() {
  if (digitalRead(encoder0PinA) == HIGH) {
    if (digitalRead(encoder0PinB)
  == LOW && encoder0Pos > 0) {
      encoder0Pos = encoder0Pos - 1;
      dir
  = 0;
    }
    else {
      encoder0Pos = encoder0Pos + 1;
      dir
  = 1;
    }
  }
  else
  {
    if (digitalRead(encoder0PinB) == LOW
  ) {
      encoder0Pos = encoder0Pos + 1;
      dir = 1;
    }
    else
  {
      if (encoder0Pos > 0) {
        encoder0Pos = encoder0Pos - 1;

        dir = 0;
      }
    }
  }
}