#include <ESP32Servo.h>

// ==========================================
// CONFIGURAÇÃO PARA SIMULAÇÃO (WOKWI)
// ==========================================

Servo sGarra;
Servo sBase;
Servo sOmbro;
Servo sCotovelo;

// Pinos Diretos no ESP32 (Sem PCA9685)
#define PIN_GARRA    13
#define PIN_BASE     12
#define PIN_OMBRO    14
#define PIN_COTOVELO 27

// --- SEUS LIMITES ---
#define BASE_MIN       0  
#define BASE_MAX       170 
#define GARRA_FECHADA  70  
#define GARRA_ABERTA   100 
#define OMBRO_CIMA     70
#define OMBRO_BAIXO    160 
#define COT_BAIXO      60
#define COT_CIMA       130

#define PASSO_MOVIMENTO 10 // Aumentei um pouco para o simulador ser ágil

// Variáveis Globais
int posBase = 90;
int posOmbro = 90;
int posCotovelo = 90;
int posGarra = 90;

bool modoDemoAtivo = false;
int passoDemo = 0;

// ==========================================
// SETUP
// ==========================================
void setup() {
  Serial.begin(115200);
  Serial.println("--- ROBO INICIADO (SIMULACAO) ---");
  Serial.println("Comandos disponiveis:");
  Serial.println("  Demo,ON   -> Inicia Demo");
  Serial.println("  Demo,OFF  -> Para Demo");
  Serial.println("  Home,GO   -> Voltar ao Inicio");
  Serial.println("  Base,LEFT / Base,RIGHT");
  Serial.println("---------------------------------");

  // Anexa os servos aos pinos
  sGarra.attach(PIN_GARRA);
  sBase.attach(PIN_BASE);
  sOmbro.attach(PIN_OMBRO);
  sCotovelo.attach(PIN_COTOVELO);

  // Posicao inicial
  aplicarServo(1, posGarra);
  aplicarServo(2, posBase);
  aplicarServo(3, posOmbro);
  aplicarServo(4, posCotovelo);
}

// ==========================================
// FUNÇÕES AUXILIARES
// ==========================================

// Função wrapper para simular o comportamento do código original
void aplicarServo(int canal, int angulo) {
  if(canal == 1) sGarra.write(angulo);
  if(canal == 2) sBase.write(angulo);
  if(canal == 3) sOmbro.write(angulo);
  if(canal == 4) sCotovelo.write(angulo);
}

// Verifica se tem comandos na Serial (Simulando o Wi-Fi)
void verificarSerial() {
  if (Serial.available() > 0) {
    String texto = Serial.readStringUntil('\n');
    texto.trim(); // Remove espaços e enters
    
    // Separa por virgula
    int virgula = texto.indexOf(',');
    if (virgula > 0) {
      String key = texto.substring(0, virgula);
      String val = texto.substring(virgula + 1);
      
      if (key == "Demo") {
        if (val == "ON") { modoDemoAtivo = true; passoDemo = 0; Serial.println("Demo: LIGADO"); }
        else { modoDemoAtivo = false; Serial.println("Demo: DESLIGADO"); }
      }
      else if (key == "Home") {
        modoDemoAtivo = false;
        Serial.println("Indo para Home...");
        moverParaHome(1, 90, 10);
        moverParaHome(2, 90, 10);
        moverParaHome(3, 90, 10);
        moverParaHome(4, 90, 10);
      }
      else {
        processarManual(key, val);
      }
    }
  }
}

// Movimento Suave Demo
void moverSuaveDemo(int canal, int anguloAlvo, int delayMs) {
  int *posAtual;
  if (canal == 2) posAtual = &posBase;
  else if (canal == 3) posAtual = &posOmbro;
  else if (canal == 4) posAtual = &posCotovelo;
  else if (canal == 1) posAtual = &posGarra;
  
  if (*posAtual == anguloAlvo) return;
  int step = (*posAtual < anguloAlvo) ? 1 : -1;

  while (*posAtual != anguloAlvo) {
    verificarSerial(); // Checa se pediu para parar
    if (!modoDemoAtivo) return;

    *posAtual += step;
    aplicarServo(canal, *posAtual);
    delay(delayMs);
  }
}

// Movimento Suave Home (Forçado)
void moverParaHome(int canal, int anguloAlvo, int delayMs) {
  int *posAtual;
  if (canal == 2) posAtual = &posBase;
  else if (canal == 3) posAtual = &posOmbro;
  else if (canal == 4) posAtual = &posCotovelo;
  else if (canal == 1) posAtual = &posGarra;
  
  if (*posAtual == anguloAlvo) return;
  int step = (*posAtual < anguloAlvo) ? 1 : -1;

  while (*posAtual != anguloAlvo) {
    *posAtual += step;
    aplicarServo(canal, *posAtual);
    delay(delayMs);
  }
}

void processarManual(String parte, String acao) {
  if (modoDemoAtivo) { modoDemoAtivo = false; Serial.println("Manual ativado. Demo parado."); }

  Serial.print("Manual: "); Serial.print(parte); Serial.print(" -> "); Serial.println(acao);

  if (parte == "Base") {
    if (acao == "LEFT")  posBase += PASSO_MOVIMENTO;
    if (acao == "RIGHT") posBase -= PASSO_MOVIMENTO;
    posBase = constrain(posBase, BASE_MIN, BASE_MAX);
    aplicarServo(2, posBase);
  }
  else if (parte == "Shoulder") {
    if (acao == "UP")    posOmbro -= PASSO_MOVIMENTO; 
    if (acao == "DOWN")  posOmbro += PASSO_MOVIMENTO;
    posOmbro = constrain(posOmbro, OMBRO_CIMA, OMBRO_BAIXO);
    aplicarServo(3, posOmbro);
  }
  else if (parte == "Elbow") {
    if (acao == "UP")    posCotovelo += PASSO_MOVIMENTO;
    if (acao == "DOWN")  posCotovelo -= PASSO_MOVIMENTO;
    posCotovelo = constrain(posCotovelo, COT_BAIXO, COT_CIMA);
    aplicarServo(4, posCotovelo);
  }
  else if (parte == "Gripper") {
    if (acao == "OPEN")  posGarra = GARRA_ABERTA;
    if (acao == "CLOSE") posGarra = GARRA_FECHADA;
    aplicarServo(1, posGarra);
  }
}

// ==========================================
// LOOP
// ==========================================
void loop() {
  verificarSerial(); // Substitui o wifi.cleanup

  if (modoDemoAtivo) {
    // Velocidades para Simulação
    int velLenta  = 15; 
    int velMedia  = 10;  
    int velRapida = 5;  

    int ombroChao = 150; 
    int cotoveloChao = COT_BAIXO; 

    // Canais mapeados: 1=Garra, 2=Base, 3=Ombro, 4=Cotovelo
    switch (passoDemo) {
      case 0: // Preparar
        moverSuaveDemo(1, GARRA_ABERTA, velRapida);
        moverSuaveDemo(3, 90, velLenta);
        moverSuaveDemo(4, 90, velLenta);
        moverSuaveDemo(2, 90, velLenta);
        delay(200); break;
      case 1: // Esq + Chão
        moverSuaveDemo(2, BASE_MAX, velLenta);
        moverSuaveDemo(4, cotoveloChao, velMedia);
        moverSuaveDemo(3, ombroChao, velLenta); 
        delay(300); break;
      case 2: // Pegar
        moverSuaveDemo(1, GARRA_FECHADA, velRapida);
        delay(300); break;
      case 3: // Levantar
        moverSuaveDemo(3, OMBRO_CIMA, velLenta);
        moverSuaveDemo(4, 90, velMedia);
        delay(200); break;
      case 4: // Levar Direita
        moverSuaveDemo(2, BASE_MIN, velLenta);
        delay(200); break;
      case 5: // Baixar Direita
        moverSuaveDemo(4, cotoveloChao, velMedia);
        moverSuaveDemo(3, ombroChao, velLenta); 
        delay(300); break;
      case 6: // Soltar
        moverSuaveDemo(1, GARRA_ABERTA, velRapida);
        delay(300); break;
      case 7: // Afastar
        moverSuaveDemo(3, 110, velLenta);
        delay(200); break;
      case 8: // Voltar p/ Pegar
        moverSuaveDemo(3, ombroChao, velLenta);
        delay(300); break;
      case 9: // Re-Pegar
        moverSuaveDemo(1, GARRA_FECHADA, velRapida);
        delay(300); break;
      case 10: // Levantar
        moverSuaveDemo(3, OMBRO_CIMA, velLenta);
        moverSuaveDemo(4, 90, velMedia);
        delay(200); break;
      case 11: // Voltar Esq
        moverSuaveDemo(2, BASE_MAX, velLenta);
        delay(200); break;
      case 12: // Baixar Origem
        moverSuaveDemo(4, cotoveloChao, velMedia);
        moverSuaveDemo(3, ombroChao, velLenta);
        delay(300); break;
      case 13: // Soltar Final
        moverSuaveDemo(1, GARRA_ABERTA, velRapida);
        delay(300); break;
      case 14: // Reiniciar
        moverSuaveDemo(3, 90, velLenta);
        moverSuaveDemo(4, 90, velLenta);
        delay(500); break;
    }
    
    if (modoDemoAtivo) {
      passoDemo++;
      if (passoDemo > 14) passoDemo = 1; 
    }
  }
}