#include <ESP32Servo.h>

// --- Definição dos Pinos ---
#define BTN_VERMELHO 27 
#define BTN_VERDE    14 
#define BTN_AZUL     12 
#define BTN_AMARELO  26 
#define BTN_ROXO     25 

// LED RGB 
#define LED_R 16 
#define LED_G 2
#define LED_B 15

// Servo Motor
#define SERVO_PIN 4

Servo meuServo;

// Variável para controlar o tempo de separação
const int TEMPO_SEPARACAO = 1000; // 1 segundo para o servo ficar na posição

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

  // Botões
  pinMode(BTN_VERMELHO, INPUT_PULLUP);
  pinMode(BTN_VERDE, INPUT_PULLUP);
  pinMode(BTN_AZUL, INPUT_PULLUP);
  pinMode(BTN_AMARELO, INPUT_PULLUP);
  pinMode(BTN_ROXO, INPUT_PULLUP);

  // LEDs
  pinMode(LED_R, OUTPUT);
  pinMode(LED_G, OUTPUT);
  pinMode(LED_B, OUTPUT);

  // Servo
  meuServo.setPeriodHertz(50); 
  meuServo.attach(SERVO_PIN, 500, 2400); 
  
  // Inicia em 0
  meuServo.write(0);
  Serial.println("Sistema Pronto.");
}

void loop() {
  // Chamamos a função apenas se algum botão for pressionado
  // Se nenhum for pressionado, o loop roda vazio, mantendo o servo onde está (ou no 0)
  verificarBotoes();
  delay(50); 
}

// Função auxiliar para controlar o LED (usando digital para evitar conflito de timer)
void acenderLed(bool r, bool g, bool b) {
  digitalWrite(LED_R, r ? HIGH : LOW);
  digitalWrite(LED_G, g ? HIGH : LOW);
  digitalWrite(LED_B, b ? HIGH : LOW);
}

// Função para mover, esperar e resetar
void ativarSeparador(int angulo, String cor) {
    Serial.print("Separando cor: ");
    Serial.println(cor);
    
    // 1. Move o servo para a posição de descarte
    meuServo.write(angulo);
    
    // 2. Aguarda o objeto cair/separar (Bloqueante propositalmente)
    delay(TEMPO_SEPARACAO);
    
    // 3. Retorna para a posição inicial (Recolhe o braço)
    meuServo.write(0);
    
    // 4. Apaga o LED
    acenderLed(false, false, false);
}

void verificarBotoes() {
  // Nota: INPUT_PULLUP lê LOW quando pressionado

  if (digitalRead(BTN_VERMELHO) == LOW) {
    acenderLed(true, false, false); // Vermelho
    ativarSeparador(105, "VERMELHO");
  }
  else if (digitalRead(BTN_VERDE) == LOW) {
    acenderLed(false, true, false); // Verde
    ativarSeparador(70, "VERDE");
  }
  else if (digitalRead(BTN_AZUL) == LOW) {
    acenderLed(false, false, true); // Azul
    // Mudei de 0 para 135, pois 0 é a base. Se o azul for detectado, 
    // ele precisa ir para algum lugar diferente da base.
    ativarSeparador(35, "AZUL"); 
  }
  else if (digitalRead(BTN_AMARELO) == LOW) {
    acenderLed(true, true, false); // Amarelo (R+G)
    ativarSeparador(140, "AMARELO");
  }
  else if (digitalRead(BTN_ROXO) == LOW) {
    acenderLed(true, false, true); // Roxo (R+B)
    ativarSeparador(175, "ROXO");
  }
  // Removemos o 'else' que forçava o zero constantemente
}