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

#define POTENTIOMETER_PIN  35  // ESP32 pin GPIO35 (ADC0) connected to Potentiometer pin
#define LED_PIN_RIGHT            18  // ESP32 pin GPIO18 connected to LED's pin
#define LED_PIN_LEFT             19  // ESP32 pin GPIO19 connected to LED's pin

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

Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// the setup routine runs once when you press reset:
void setup() {
  // initialize serial communication at 9600 bits per second:
  Serial.begin(9600);

  // declare LED pin to be an output:
  pinMode(LED_PIN_RIGHT, OUTPUT);
  pinMode(LED_PIN_LEFT, OUTPUT);


  //For OLED
  // initialize OLED display with I2C address 0x3C
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("failed to start SSD1306 OLED"));
    while (1);
  }

  delay(2000);         // wait two seconds for initializing
  oled.clearDisplay(); // clear display

  oled.setTextSize(3);         // set text size
  oled.setTextColor(WHITE);    // set text color
  oled.setCursor(0, 2);       // set position to display (x,y)
  oled.println("Hello");
  
  oled.println("World");
  oled.display();              // display on OLED
}

// the loop routine runs over and over again forever:
void loop() {
  // reads the input on analog pin A0 (value between 0 and 4095)
  int analogValue = analogRead(POTENTIOMETER_PIN);

  // scales it to brightness (value between 0 and 255)
  int rotation = map(analogValue, 0, 4095, 0, 100);


  if (rotation > 70) {
    digitalWrite(LED_PIN_RIGHT, HIGH); // turn off the LED if the sensor value is less than 512
    oled.clearDisplay();
    oled.setTextSize(2);
    oled.setTextColor(WHITE);
    oled.setCursor(0, 2);       // set position to display (x,y)
    // oled.print("Rotation: ");
    // oled.println(rotation);
    oled.println();
    oled.println("Right Turn");
    oled.display();
  } 
  else {
    digitalWrite(LED_PIN_RIGHT, LOW); 
    oled.clearDisplay();
    oled.display();
  }
  if (rotation < 30) {
    digitalWrite(LED_PIN_LEFT, HIGH); // turn on the LED if the sensor value is greater than or equal to 512
    oled.clearDisplay();
    oled.setTextSize(2);
    oled.setTextColor(WHITE);
    oled.setCursor(0, 2);       // set position to display (x,y)
    // oled.print("Rotation: ");
    // oled.println(rotation);
    oled.println();
    oled.println("Left Turn");
    oled.display();
  }
  else {
    digitalWrite(LED_PIN_LEFT, LOW); 
    oled.clearDisplay();
    oled.display();
  }

  delay(500);
}