#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

// Inicializa o display LCD no endereço 0x27
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Configura os servos
Servo servoBase;
Servo servoEixo1;
Servo servoEixo2;
Servo servoBraco;
Servo servoPulso;
Servo servoGarra;
Servo servoGira;

const int servoBaseHome = 90;
const int servoEixoHome1e2 = 90;
const int servoBracoHome = 90;
const int servoPulsoHome = 90;
const int servoGarraHome = 90;
const int servoGiraHome = 90;

// Definições dos pinos
#define pinS0 22
#define pinS1 23
#define pinLED 24
#define pinS2 25
#define pinS3 26
#define pinOut 27
#define pinBotao 28 // Pino do botão

#define pinPotVelocidade A0 // Potenciômetro para controlar a velocidade

#define pinPotBase A1
#define pinPotEixo A2 // Potenciômetro para Eixo 1 e 2
#define pinPotBraco A3
#define pinPotPulso A4
#define pinPotGarra A5
#define pinPotGira A6

// Variáveis de controle
bool modoAutomatico = true; // Inicia no modo automático
bool ultimoEstadoBotao = LOW; // Para detectar mudanças de estado do botão

// Pinos dos LEDs
#define pinoLedVerm 2
#define pinoLedVerd 3
#define pinoLedAzul 4

unsigned int valorVermelho = 0;
unsigned int valorVerde = 0;
unsigned int valorAzul = 0;
unsigned int valorBranco = 0;

void detectaCor();
void moverParaHome();
void realizarMovimentosVermelho();
void realizarMovimentosVerde();
void realizarMovimentosAzul();
void moverServosGradualmente(Servo &servo1, Servo &servo2, int anguloDestino1);


void setup() {
    pinMode(pinS0, OUTPUT);
    pinMode(pinS1, OUTPUT);
    pinMode(pinS2, OUTPUT);
    pinMode(pinS3, OUTPUT);
    pinMode(pinLED, OUTPUT);
    pinMode(pinOut, INPUT);
    pinMode(pinBotao, INPUT_PULLUP); // Botão com resistor pull-up

    servoBase.attach(5);
    servoEixo1.attach(6);
    servoEixo2.attach(7);
    servoBraco.attach(8);
    servoPulso.attach(9);
    servoGarra.attach(10);
    servoGira.attach(11);

    pinMode(pinoLedVerm, OUTPUT);
    pinMode(pinoLedVerd, OUTPUT);
    pinMode(pinoLedAzul, OUTPUT);
 
    digitalWrite(pinS0, HIGH);
    digitalWrite(pinS1, LOW);
    delay(2000);
  
    digitalWrite(pinLED, HIGH);
  
    Serial.begin(9600);
    lcd.begin();
    lcd.backlight();

  
  
}

void loop() {
    // Lê o estado do botão
    bool estadoBotao = digitalRead(pinBotao);
    
   // Alterna o modo se o botão for pressionado
   if (estadoBotao == LOW && ultimoEstadoBotao == HIGH) {
        modoAutomatico = !modoAutomatico;
        Serial.print("Modo: ");
        Serial.println(modoAutomatico ? "Automático" : "Manual");
        
        if (modoAutomatico) {
           lcd.clear();
            delay(300);
           lcd.setCursor(2, 0);
           lcd.print("Aguardando...");
           lcd.setCursor(2, 1);
           lcd.print("Home");
            moverParaHome(); // Move para a posição HOME quando ativar o modo automático
        }
        
        delay(200); // Debounce
       lcd.clear();
        digitalWrite(pinLED, LOW);
         delay(400);
    }
    ultimoEstadoBotao = estadoBotao;
  
    // Modo automático
    if (modoAutomatico) {
        digitalWrite(pinLED, HIGH);
        lcd.setCursor(2, 0);
        lcd.print("Modo: Auto");
        lcd.setCursor(2, 1);
        lcd.print("Cor: ");
        detectaCor();

        Serial.print("Vermelho: ");
        Serial.print(valorVermelho);
        Serial.print(" Verde: ");
        Serial.print(valorVerde);
        Serial.print(" Azul: ");
        Serial.print(valorAzul);
        Serial.print(" Branco: ");
        Serial.println(valorBranco);

        if (valorVermelho < valorAzul && valorVermelho < valorVerde && valorBranco < 55) {
            lcd.setCursor(7, 1);
            lcd.print("Vermelho");
            digitalWrite(pinoLedVerm, HIGH);
            digitalWrite(pinoLedVerd, LOW);
            digitalWrite(pinoLedAzul, LOW);
            realizarMovimentosVermelho();
            delay(1000);
            moverParaHome();
            lcd.clear();
        } 
        else if (valorAzul < valorVermelho && valorAzul < valorVerde && valorBranco < 50) {
            lcd.setCursor(7, 1);
            lcd.print("Azul");
            digitalWrite(pinoLedVerm, LOW);
            digitalWrite(pinoLedVerd, LOW);
            digitalWrite(pinoLedAzul, HIGH);
            realizarMovimentosAzul();
            delay(1000);
            moverParaHome();
           lcd.clear();
        }
         else if (valorVerde < valorVermelho && valorVerde < valorAzul && valorBranco < 65) {
            lcd.setCursor(7, 1);
            lcd.print("Verde");
            digitalWrite(pinoLedVerm, LOW);
            digitalWrite(pinoLedVerd, HIGH);
            digitalWrite(pinoLedAzul, LOW);
            realizarMovimentosVerde();
            delay(1000);
            moverParaHome();
          lcd.clear();
        }

        delay(20);
        digitalWrite(pinoLedVerm, LOW);
        digitalWrite(pinoLedVerd, LOW);
        digitalWrite(pinoLedAzul, LOW);
    }
    // Modo manual
    else {
        lcd.setCursor(2, 0);
        lcd.print("Modo: Manual");
        // Ler os valores dos potenciômetros e controlar os servos
        int valorBase = analogRead(pinPotBase);
        int valorEixo = analogRead(pinPotEixo); // Potenciômetro único para Eixo 1 e 2
        int valorBraco = analogRead(pinPotBraco);
        int valorPulso = analogRead(pinPotPulso);
        int valorGarra = analogRead(pinPotGarra);
        int valorGira = analogRead(pinPotGira);
        int valorVelocidade = analogRead(pinPotVelocidade); // Velocidade

        // Mapeia os valores de 0-1023 para 0-180 graus
        servoBase.write(map(valorBase, 0, 1023, 0, 180));
        int anguloEixo = map(valorEixo, 0, 1023, 0, 180);
        servoEixo1.write(anguloEixo); // Eixo 1
        servoEixo2.write(180 - anguloEixo); // Eixo 2 invertido
        servoBraco.write(map(valorBraco, 0, 1023, 0, 180));
        servoPulso.write(map(valorPulso, 0, 1023, 0, 180));
        servoGarra.write(map(valorGarra, 0, 1023, 0, 180));
        servoGira.write(map(valorGira, 0, 1023, 0, 180));

        // Controla a velocidade dos movimentos
       // int delayTempo = map(valorVelocidade, 0, 1023, 15, 600);
        //delay(delayTempo); // Aguarda para controlar a velocidade

        delay(10); // Aguarda para evitar saturação
    }
}

void detectaCor() {
    // Vermelho
    digitalWrite(pinS2, LOW);
    digitalWrite(pinS3, LOW);
    valorVermelho = pulseIn(pinOut, digitalRead(pinOut) == HIGH ? LOW : HIGH);

    // Branco
    digitalWrite(pinS2, HIGH);
    valorBranco = pulseIn(pinOut, digitalRead(pinOut) == HIGH ? LOW : HIGH);

    // Azul
    digitalWrite(pinS2, LOW);
    digitalWrite(pinS3, HIGH);
    valorAzul = pulseIn(pinOut, digitalRead(pinOut) == HIGH ? LOW : HIGH);

    // Verde
    digitalWrite(pinS2, HIGH);
    valorVerde = pulseIn(pinOut, digitalRead(pinOut) == HIGH ? LOW : HIGH);
}

// Função para mover servos para a posição "HOME"
void moverParaHome() {
    moverServoGradualmente(servoBase, 90);
    moverServosGradualmente(servoEixo1, servoEixo2, 90);
    moverServoGradualmente(servoBraco, 90);
    moverServoGradualmente(servoPulso, 90);
    moverServoGradualmente(servoGarra, 90);
    moverServoGradualmente(servoGira, 90);
    delay(1000);  // Aguarda estabilização
}

void realizarMovimentosVermelho() {
    moverServoGradualmente(servoBase, 15);
    moverServosGradualmente(servoEixo1, servoEixo2, 150); // Movendo Eixo 1 e 2
    delay(1000);
    moverServosGradualmente(servoEixo1, servoEixo2, 2); // Movendo Eixo 1 e 2
    moverServoGradualmente(servoBase, 180);
    moverServoGradualmente(servoBase, 100); // Movendo Base
    delay(1000);
}

void realizarMovimentosVerde() {
    moverServoGradualmente(servoBase, 50);
    moverServosGradualmente(servoEixo1, servoEixo2, 150); // Movendo Eixo 1 e 2
    delay(1000);
    moverServosGradualmente(servoEixo1, servoEixo2, 165); // Movendo Eixo 1 e 2
    moverServoGradualmente(servoBase, 150);
    delay(1000);
}

void realizarMovimentosAzul() {
    moverServoGradualmente(servoBase, 135);
    delay(1000);
    moverServosGradualmente(servoEixo1, servoEixo2, 150); // Movendo Eixo 1 e 2
    delay(1000);
    moverServosGradualmente(servoEixo1, servoEixo2, 15); // Movendo Eixo 1 e 2
    delay(1000);
    moverServoGradualmente(servoBase, 10);
    delay(1000);
    moverServosGradualmente(servoEixo1, servoEixo2, 170); // Movendo Eixo 1 e 2
    delay(1000);
}

// Função para mover os dois servos a uma velocidade específica
void moverServosGradualmente(Servo &servo1, Servo &servo2, int anguloDestino1) {
    int anguloAtual1 = servo1.read();
    int passo1 = (anguloDestino1 > anguloAtual1) ? 1 : -1;

    while (anguloAtual1 != anguloDestino1) {
        int velocidade = map(analogRead(pinPotVelocidade), 0, 1023, 15, 600);
        int delayTempo = 1000 / velocidade;

        anguloAtual1 += passo1;
        servo1.write(anguloAtual1);

        int anguloAtual2 = 180 - anguloAtual1;
        servo2.write(anguloAtual2);

        delay(delayTempo);
    }

    servo1.write(anguloDestino1);
    servo2.write(180 - anguloDestino1);
}

void moverServoGradualmente(Servo &servo, int anguloDestino) {
    int anguloAtual = servo.read();
    int passo = (anguloDestino > anguloAtual) ? 1 : -1;

    while (anguloAtual != anguloDestino) {
        int velocidade = map(analogRead(pinPotVelocidade), 0, 1023, 15, 600);
        int delayTempo = 1000 / velocidade;

        anguloAtual += passo;
        servo.write(anguloAtual);

        delay(delayTempo);
    }

    servo.write(anguloDestino);
}