#include <Servo.h>  // Biblioteca para controlar servos
#include <Wire.h>   // Biblioteca para comunicação I2C
#include <Adafruit_GFX.h>  // Biblioteca gráfica
#include <Adafruit_SSD1306.h>  // Biblioteca para OLED

#define SCREEN_WIDTH 128  // Largura do display OLED
#define SCREEN_HEIGHT 32  // Altura do display OLED
#define OLED_RESET -1     // Reset não utilizado (conectado internamente)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define VERT_PIN A0  // Eixo vertical do joystick
#define HORZ_PIN A1  // Eixo horizontal do joystick
#define SEL_PIN  2   // Botão do joystick

// Definimos 5 servos
Servo servos[5];
int servoPins[5] = {3, 5, 6, 9, 10};  // Pinos dos servos

int activeServo = 0;  // Servo que está sendo controlado (0 a 4)
bool buttonPressed = false;  // Evitar múltiplas leituras do botão

void setup() {
  // Inicializa o display OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {  // Endereço padrão I2C: 0x3C
    for (;;) {
      // Trava o sistema se o display não for encontrado
    }
  }
  display.clearDisplay();  // Limpa o display
  display.setTextSize(2);  // Define o tamanho do texto
  display.setTextColor(SSD1306_WHITE);  // Cor do texto (branco)

  // Conectar cada servo ao pino correspondente
  for (int i = 0; i < 5; i++) {
    servos[i].attach(servoPins[i]);
    servos[i].write(90);  // Posição inicial neutra (90 graus)
  }

  pinMode(SEL_PIN, INPUT_PULLUP);  // Configura o botão como INPUT_PULLUP

  atualizarDisplay();  // Exibe o servo inicial no OLED
}

void loop() {
  // Leitura dos eixos do joystick
  int horz = analogRead(HORZ_PIN);
  int vert = analogRead(VERT_PIN);

  // Mapeia a leitura do joystick para um ângulo entre 0 e 180 graus
  int angle = map(vert, 0, 1023, 0, 180);

  // Controla o servo ativo com o ângulo calculado
  servos[activeServo].write(angle);

  // Verifica se o botão foi pressionado
  if (digitalRead(SEL_PIN) == LOW && !buttonPressed) {
    buttonPressed = true;  // Marca que o botão foi pressionado
    // Alterna para o próximo servo (volta ao primeiro após o último)
    activeServo = (activeServo + 1) % 5;
    atualizarDisplay();  // Atualiza o display com o novo servo ativo
  }

  // Libera o estado do botão quando ele for solto
  if (digitalRead(SEL_PIN) == HIGH) {
    buttonPressed = false;
  }

  delay(100);  // Atraso para suavizar a leitura e resposta
}

// Função para atualizar o display OLED
void atualizarDisplay() {
  display.clearDisplay();  // Limpa o display
  display.setCursor(0, 0);  // Define a posição do cursor
  display.print("Servo ativo: ");
  display.println(activeServo + 1);  // Exibe o número do servo (1 a 5)
  display.display();  // Exibe no display
}