#include <Servo.h>

Servo Garra;

int bt_sobe     = A0;
int bt_desce    = A1;
int bt_frente   = A2;
int bt_tras     = A3;
int bt_esquerda = A4;
int bt_direita  = A5;
int bt_garra    = 2;
int zero_M1     = 6;
int zero_M2     = 5;
int zero_M3     = 4;

int stp1 = 13;
int dir1 = 12;
int stp2 = 11;
int dir2 = 10;
int stp3 = 9;
int dir3 = 8;

int encoder_M1 = 0;
int encoder_M2 = 0;
int encoder_M3 = 0;

void setup() {
  Serial.begin(9600);  // serial ativo para monitorar encoders durante testes
  Garra.attach(7);

  pinMode(stp1, OUTPUT);
  pinMode(dir1, OUTPUT);
  pinMode(stp2, OUTPUT);
  pinMode(dir2, OUTPUT);
  pinMode(stp3, OUTPUT);
  pinMode(dir3, OUTPUT);

  pinMode(bt_sobe,     INPUT_PULLUP);
  pinMode(bt_desce,    INPUT_PULLUP);
  pinMode(bt_frente,   INPUT_PULLUP);
  pinMode(bt_tras,     INPUT_PULLUP);
  pinMode(bt_esquerda, INPUT_PULLUP);
  pinMode(bt_direita,  INPUT_PULLUP);
  pinMode(bt_garra,    INPUT_PULLUP);
  pinMode(zero_M1,     INPUT_PULLUP);
  pinMode(zero_M2,     INPUT_PULLUP);
  pinMode(zero_M3,     INPUT_PULLUP);

  Garra.write(0);  // fecha a garra antes de iniciar o homing

  // ZERO M1 — com timeout de segurança (evita loop infinito)
  int timeout = 0;
  while (digitalRead(zero_M1) == HIGH && timeout < 500) {
    digitalWrite(dir1, LOW);
    digitalWrite(stp1, HIGH);
    delay(50);
    digitalWrite(stp1, LOW);
    delay(50);
    timeout++;
  }
  Serial.println("M1 zero encontrado");

  // ZERO M2 — com timeout de segurança
  timeout = 0;
  while (digitalRead(zero_M2) == HIGH && timeout < 500) {
    digitalWrite(dir2, LOW);
    digitalWrite(stp2, HIGH);
    delay(50);
    digitalWrite(stp2, LOW);
    delay(50);
    timeout++;
  }
  Serial.println("M2 zero encontrado");

  // ZERO M3 — com timeout de segurança
  timeout = 0;
  while (digitalRead(zero_M3) == HIGH && timeout < 500) {
    digitalWrite(dir3, LOW);
    digitalWrite(stp3, HIGH);
    delay(50);
    digitalWrite(stp3, LOW);
    delay(50);
    timeout++;
  }
  Serial.println("M3 zero encontrado");

  // RETORNA M1 PARA CENTRO
  for (int i = 0; i <= 100; i++) {
    digitalWrite(dir1, HIGH);
    digitalWrite(stp1, HIGH);
    delay(10);
    digitalWrite(stp1, LOW);
    delay(10);
  }
  Serial.println("M1 no centro");

  // RETORNA M2 PARA CENTRO
  for (int i = 0; i <= 120; i++) {
    digitalWrite(dir2, HIGH);
    digitalWrite(stp2, HIGH);
    delay(10);
    digitalWrite(stp2, LOW);
    delay(10);
  }
  Serial.println("M2 no centro");

  // RETORNA M3 PARA CENTRO
  for (int i = 0; i <= 270; i++) {
    digitalWrite(dir3, HIGH);
    digitalWrite(stp3, HIGH);
    delay(10);
    digitalWrite(stp3, LOW);
    delay(10);
  }
  Serial.println("M3 no centro — sistema pronto");
}

void loop() {

  // motor 1 - SOBE/DESCE
  if (digitalRead(bt_sobe) == LOW && digitalRead(bt_desce) == HIGH && encoder_M1 < 100) {
    digitalWrite(dir1, HIGH);
    digitalWrite(stp1, HIGH);
    delay(10);
    digitalWrite(stp1, LOW);
    delay(10);
    encoder_M1++;  // incrementa após pulso completo
  }

  if (digitalRead(bt_sobe) == HIGH && digitalRead(bt_desce) == LOW && encoder_M1 > -100) {
    digitalWrite(dir1, LOW);
    digitalWrite(stp1, HIGH);
    delay(10);
    digitalWrite(stp1, LOW);
    delay(10);
    encoder_M1--;  // decrementa após pulso completo
  }

  // motor 2 - FRENTE/TRAS
  if (digitalRead(bt_frente) == LOW && digitalRead(bt_tras) == HIGH && encoder_M2 < 120) {
    digitalWrite(dir2, HIGH);
    digitalWrite(stp2, HIGH);
    delay(10);
    digitalWrite(stp2, LOW);
    delay(10);
    encoder_M2++;
  }

  if (digitalRead(bt_frente) == HIGH && digitalRead(bt_tras) == LOW && encoder_M2 > -120) {
    digitalWrite(dir2, LOW);
    digitalWrite(stp2, HIGH);
    delay(10);
    digitalWrite(stp2, LOW);
    delay(10);
    encoder_M2--;
  }

  // motor 3 - ESQUERDA/DIREITA
  if (digitalRead(bt_esquerda) == LOW && digitalRead(bt_direita) == HIGH && encoder_M3 < 270) {
    digitalWrite(dir3, HIGH);
    digitalWrite(stp3, HIGH);
    delay(10);
    digitalWrite(stp3, LOW);
    delay(10);
    encoder_M3++;
  }

  if (digitalRead(bt_esquerda) == HIGH && digitalRead(bt_direita) == LOW && encoder_M3 > -270) {
    digitalWrite(dir3, LOW);
    digitalWrite(stp3, HIGH);
    delay(10);
    digitalWrite(stp3, LOW);
    delay(10);
    encoder_M3--;
  }

  // GARRA
  if (digitalRead(bt_garra) == LOW)
    Garra.write(180);  // abre
  else
    Garra.write(0);    // fecha

  // monitor serial — exibe posição dos 3 motores
  Serial.print("M1="); Serial.print(encoder_M1);
  Serial.print(" | M2="); Serial.print(encoder_M2);
  Serial.print(" | M3="); Serial.println(encoder_M3);
  delay(100);
}