// FW_VERSION informativo — no hace OTA, se flashea por USB
#define FW_VERSION 20
#include <Servo.h>

// ==== Pines motores ====
const int ENA = D1;
const int ENB = D5;
const int IN1 = D6;
const int IN2 = D7;
const int IN3 = D2;
const int IN4 = D0;

// ==== Pines servos ====
const int SERVO_X_PIN = D3;
const int SERVO_Y_PIN = D4;

// ==== Pin buzzer ====
const int buzzerPin = D8;

Servo servoX;
Servo servoY;

int currentSpeed = 150;
int posX = 90;
int posY = 90;

bool espReady = false;

void setup() {
  // 1. Motores parados inmediatamente
  pinMode(ENA, OUTPUT); pinMode(ENB, OUTPUT);
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  stopMotors();

  // 2. Serial
  Serial.begin(9600);

  // 3. Periféricos
  analogWriteRange(255);
  analogWriteFreq(1000);
  servoX.attach(SERVO_X_PIN);
  servoY.attach(SERVO_Y_PIN);
  pinMode(buzzerPin, OUTPUT);
  servoX.write(posX);
  servoY.write(posY);

  // 4. Esperar a que el ESP32-CAM termine de arrancar (~20s)
  // El ESP32 necesita: WiFiManager + OTA check + arrancar servidor
  unsigned long elapsed = millis();
  if (elapsed < 20000) delay(20000 - elapsed);

  // 5. Descartar cualquier basura que haya llegado por Serial durante el arranque
  while (Serial.available()) Serial.read();

  // 6. Listo — 2 pitidos
  for (int i = 0; i < 2; i++) {
    digitalWrite(buzzerPin, HIGH); delay(180);
    digitalWrite(buzzerPin, LOW);  delay(180);
  }
  espReady = true;
}

// Watchdog: si no llega comando de movimiento en 1.5s → para motores
unsigned long lastMoveMs = 0;
bool motorRunning = false;

void loop() {
  if (motorRunning && (millis() - lastMoveMs > 1500)) {
    stopMotors();
    motorRunning = false;
  }

  if (!Serial.available()) return;
  char c = Serial.read();

  if (!espReady) return;

  if      (c == 'F') { forward();    lastMoveMs = millis(); motorRunning = true; }
  else if (c == 'B') { backward();   lastMoveMs = millis(); motorRunning = true; }
  else if (c == 'L') { left();       lastMoveMs = millis(); motorRunning = true; }
  else if (c == 'R') { right();      lastMoveMs = millis(); motorRunning = true; }
  else if (c == 'S') { stopMotors(); motorRunning = false; }
  else if (c == 'V') {
    int val = Serial.parseInt();
    if (val >= 0 && val <= 255) currentSpeed = val;
  }
  else if (c == 'X') {
    int val = Serial.parseInt();
    if (val >= 0 && val <= 180) { posX = val; servoX.write(posX); }
  }
  else if (c == 'Y') {
    int val = Serial.parseInt();
    if (val >= 0 && val <= 180) { posY = val; servoY.write(posY); }
  }
  else if (c == 'H') {
    int val = Serial.parseInt();
    digitalWrite(buzzerPin, val > 0 ? HIGH : LOW);
  }
}

// ==== Funciones de movimiento ====
void forward() {
  analogWrite(ENA, currentSpeed); analogWrite(ENB, currentSpeed);
  digitalWrite(IN1, LOW);  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);  digitalWrite(IN4, HIGH);
}
void backward() {
  analogWrite(ENA, currentSpeed); analogWrite(ENB, currentSpeed);
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
}
void left() {
  analogWrite(ENA, currentSpeed); analogWrite(ENB, currentSpeed);
  digitalWrite(IN1, LOW);  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
}
void right() {
  analogWrite(ENA, currentSpeed); analogWrite(ENB, currentSpeed);
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);  digitalWrite(IN4, HIGH);
}
void stopMotors() {
  analogWrite(ENA, 0); analogWrite(ENB, 0);
  digitalWrite(IN1, LOW); digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW); digitalWrite(IN4, LOW);
}