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

// Definição do display OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Definição dos servos
Servo baseServo, garraServo, esquerdaServo, direitaServo;

// Pinos dos servos
const int servoBasePin = 9;
const int servoDireitaPin = 10;
const int servoEsquerdaPin = 11;
const int servoGarraPin = 12;

// Pinos do Joystick e Botões
const int eixoX = A0;  // Eixo X controla o servo da base
const int eixoY = A1;  // Eixo Y controla o servo da esquerda
const int botaoA = 2;  // Decrementa servo direita
const int botaoB = 3;  // Incrementa servo garra
const int botaoC = 4;  // Incrementa servo direita
const int botaoD = 5;  // Decrementa servo garra
const int botaoJoystick = 6;  // Troca de tela

// Zona neutra para os joysticks
const int ZONA_NEUTRA_MIN = 450;
const int ZONA_NEUTRA_MAX = 550;

// Variáveis para ângulos dos servos
int anguloBase = 90, anguloDireita = 90, anguloEsquerda = 90, anguloGarra = 90;
int telaAtual = 0; // Tela atual do display

// Função para inicializar o display
void inicializarDisplay() {
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("Falha ao inicializar o display OLED"));
    for (;;);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
}

// Função para inicializar os servos
void inicializarServos() {
  baseServo.attach(servoBasePin);
  direitaServo.attach(servoDireitaPin);
  esquerdaServo.attach(servoEsquerdaPin);
  garraServo.attach(servoGarraPin);

  // Posicionar inicialmente em 90 graus
  baseServo.write(anguloBase);
  direitaServo.write(anguloDireita);
  esquerdaServo.write(anguloEsquerda);
  garraServo.write(anguloGarra);
}

// Função para ler o joystick e controlar os servos
void controlarServos() {
  int leituraX = analogRead(eixoX);
  int leituraY = analogRead(eixoY);

  // Controle do servo da base (eixo X)
  if (leituraX < ZONA_NEUTRA_MIN) {
    anguloBase = max(0, anguloBase - 1);
  } else if (leituraX > ZONA_NEUTRA_MAX) {
    anguloBase = min(180, anguloBase + 1);
  }
  baseServo.write(anguloBase);

  // Controle do servo da esquerda (eixo Y)
  if (leituraY < ZONA_NEUTRA_MIN) {
    anguloEsquerda = max(0, anguloEsquerda - 1);
  } else if (leituraY > ZONA_NEUTRA_MAX) {
    anguloEsquerda = min(180, anguloEsquerda + 1);
  }
  esquerdaServo.write(anguloEsquerda);

  // Controle dos botões para o servo da direita
  if (digitalRead(botaoA) == LOW) {
    anguloDireita = max(0, anguloDireita - 1);
  }
  if (digitalRead(botaoC) == LOW) {
    anguloDireita = min(180, anguloDireita + 1);
  }
  direitaServo.write(anguloDireita);

  // Controle dos botões para o servo da garra
  if (digitalRead(botaoB) == LOW) {
    anguloGarra = min(180, anguloGarra + 1);
  }
  if (digitalRead(botaoD) == LOW) {
    anguloGarra = max(0, anguloGarra - 1);
  }
  garraServo.write(anguloGarra);
}

// Função para alternar entre as telas do display
void alternarTelas() {
  if (digitalRead(botaoJoystick) == LOW) {
    telaAtual = (telaAtual + 1) % 4; // Alterna entre 4 telas
    delay(300); // Pequeno debounce para o botão
  }
}

// Função para exibir as telas
void exibirTela() {
  display.clearDisplay();

  // Título centralizado
  display.drawRect(0, 0, 128, 16, SSD1306_WHITE);
  display.setCursor(35, 4);

  // Desenha retângulo para exibir informações do servo
  display.drawRect(10, 24, 108, 32, SSD1306_WHITE);

  if (telaAtual == 0) {
    // Tela para o servo da base
    display.print(F("Servo Base"));
    display.setCursor(20, 35);
    display.print(F("Angulo: "));
    display.print(anguloBase);
  } else if (telaAtual == 1) {
    // Tela para o servo da esquerda
    display.print(F("Servo Esq"));
    display.setCursor(20, 35);
    display.print(F("Angulo: "));
    display.print(anguloEsquerda);
  } else if (telaAtual == 2) {
    // Tela para o servo da direita
    display.print(F("Servo Dir"));
    display.setCursor(20, 35);
    display.print(F("Angulo: "));
    display.print(anguloDireita);
  } else if (telaAtual == 3) {
    // Tela para o servo da garra
    display.print(F("Servo Garra"));
    display.setCursor(20, 35);
    display.print(F("Angulo: "));
    display.print(anguloGarra);
  }

  display.display();
}

void setup() {
  Serial.begin(9600);
  
  // Inicializa o display e os servos
  inicializarDisplay();
  inicializarServos();

  // Define os pinos dos botões como entrada
  pinMode(botaoA, INPUT_PULLUP);
  pinMode(botaoB, INPUT_PULLUP);
  pinMode(botaoC, INPUT_PULLUP);
  pinMode(botaoD, INPUT_PULLUP);
  pinMode(botaoJoystick, INPUT_PULLUP);
}

void loop() {
  controlarServos();
  alternarTelas();
  exibirTela();

  delay(15);
}