#include "ESP32Servo.h"
#include <math.h>

#define STEP_DISTANCIA 1

Servo servo1;
Servo servo2;

#define servoPin1 16
#define servoPin2 17
#define led1     25
#define led2     26
#define led3     27

void setup() {
  Serial.begin(9600);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo1.write(0);
  servo2.write(0);
  Serial.println("Catapulta pronta. Digite a distancia (1-4):");
}

void loop() {
  if (Serial.available()) {
    int distancia = Serial.parseInt();
    while (Serial.available()) Serial.read();

    distancia = STEP_DISTANCIA * floor(distancia / STEP_DISTANCIA);

    int ang = 0;
    switch (distancia) {
      case 1:  ang = 10; break;
      case 2:  ang = 20; break;
      case 3:  ang = 30; break;
      case 4:  ang = 40; break;
      default: ang = 0;  break;
    }

    Serial.print("Distancia: ");
    Serial.print(distancia);
    Serial.print(" cm -> Angulo: ");
    Serial.println(ang);

    digitalWrite(led1, HIGH);
    servo1.write(ang);
    delay(500);

    digitalWrite(led2, HIGH);
    delay(200);

    for (int pos = 0; pos <= 180; pos += 10) {  // passo de 10°
      servo2.write(pos);
      delay(10);  // quanto menor, mais rápido
    }
    delay(300);

    digitalWrite(led3, HIGH);
    delay(200);

    servo1.write(0);
    servo2.write(0);
    delay(500);

    digitalWrite(led1, LOW);
    digitalWrite(led2, LOW);
    digitalWrite(led3, LOW);
    Serial.println("Pronto. Digite a proxima distancia:");
  }
  delay(10);
}