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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define SERVO_PIN 4
Servo servo;

#define BUTTON_LOOP_PIN 2
#define BUTTON_MANUAL_PIN 23
#define BUTTON_STOP_PIN 13

#define POTENTIOMETER_PIN 34

bool lastButtonLoopState = HIGH;  // Inicializado para HIGH em função do pull-up interno
bool lastButtonManualState = HIGH;  // Inicializado para HIGH em função do pull-up interno
bool lastButtonStopState = HIGH;  // Inicializado para HIGH em função do pull-up interno
int modo;

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // Inicialização do display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("Falha ao iniciar o display OLED"));
    for (;;);
  }

  display.display();
  delay(2000);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  servo.attach(SERVO_PIN);
  pinMode(BUTTON_LOOP_PIN, INPUT_PULLUP);  // Utiliza pull-up interno
  pinMode(BUTTON_MANUAL_PIN, INPUT_PULLUP);  // Utiliza pull-up interno
  pinMode(BUTTON_STOP_PIN, INPUT_PULLUP);  // Utiliza pull-up interno
}

void loop() {
  static bool autoMode = false;
  static bool manualMode = false;
  static bool stopRequested = false;

  // Leitura dos botões para a detecção de borda de subida
  bool currentButtonLoopState = digitalRead(BUTTON_LOOP_PIN);
  bool currentButtonManualState = digitalRead(BUTTON_MANUAL_PIN);
  bool currentButtonStopState = digitalRead(BUTTON_STOP_PIN);

  if (currentButtonLoopState == LOW && lastButtonLoopState == HIGH) {
    autoMode = true;
    manualMode = false;
    stopRequested = false;
  }

  if (currentButtonManualState == LOW && lastButtonManualState == HIGH) {
    manualMode = true;
    autoMode = false;
  }

  if (currentButtonStopState == LOW && lastButtonStopState == HIGH) {
    autoMode = false;
    manualMode = false;
    stopRequested = true;
    servo.write(0); // Para o servo
    Serial.println("Servo parado");
    display.clearDisplay();
    display.setCursor(0, 0);
    display.print("Servo parado");
    display.display();
  }

  lastButtonLoopState = currentButtonLoopState;
  lastButtonManualState = currentButtonManualState;
  lastButtonStopState = currentButtonStopState;

  if (autoMode) {
    // Modo automático
    static unsigned long lastMoveTime = 0;
    static int angle = 0;
    static int direction = 1; // 1 para movimento crescente, -1 para decrescente

    if (millis() - lastMoveTime > 100) {
      lastMoveTime = millis();
      angle += direction * 1;

      if (angle >= 180) {
        direction = -1;
        delay(100);
        modo = 0;
      } 
      else if (angle <= 0) {
        direction = 1;
        delay(200);
        modo = 1;
      }

      servo.write(map(angle, 0, 180, 500, 2500));
      updateDisplay1("Modo automatico e sentido", angle, modo);
      Serial.print("Modo automatico | Angle: ");
      Serial.println(angle);
      Serial.print("Sentido: ");
      Serial.println(modo);
    }
  }

  if (manualMode) {
    // Modo manual
    int potValue = analogRead(POTENTIOMETER_PIN);
    int angle = map(potValue, 0, 4095, 0, 180);
    servo.write(map(angle, 0, 180, 500, 2500));
    updateDisplay2("Modo manual", angle);
    Serial.print("Modo manual | Angle: ");
    Serial.println(angle);
  }
}
void updateDisplay2(String mode, int angle) {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print(mode);
  display.setCursor(0, 10);
  display.print("Angle: ");
  display.print(angle);
  display.display();
}

void updateDisplay1(String mode, int angle, int modo) {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print(mode);
  display.setCursor(0, 20);
  display.print("Angle: ");
  display.print(angle);
  display.setCursor(0, 30);
  display.print("Sentido: ");
  if(modo == 0){
    display.print("Horario");
  }
  else if(modo == 1){
    display.print("Anti-horario");
  }
  display.display();
}