#include <SoftwareSerial.h>
#include <Servo.h>

// Pines
const int BT_RX_PIN   = 10;
const int BT_TX_PIN   = 11;
const int BASE_PIN    = 9;
const int HOMBRO_PIN  = 6;
const int MANO_PIN    = 5;

// Servo motores
Servo baseServo;
Servo hombroServo;
Servo manoServo;

// Comunicación serial Bluetooth
//SoftwareSerial Serial(BT_RX_PIN, BT_TX_PIN);

// Variables de estado
char dato;
int estado = 0;
unsigned long previousMillis = 0;
const unsigned long interval = 15; // Intervalo de tiempo para los movimientos

// Ángulos actuales de los servos
int ang1 = 0;
int ang2 = 0;
int ang3 = 90;

// Función de configuración
void setup() {
  baseServo.attach(BASE_PIN);
  hombroServo.attach(HOMBRO_PIN);
  manoServo.attach(MANO_PIN);

  baseServo.write(ang1);
  hombroServo.write(ang2);
  manoServo.write(ang3);

  Serial.begin(115200);
  Serial.println("Iniciando.");
}

// Función principal
void loop() {
  // Verificamos si hay datos disponibles en la comunicación Bluetooth
  if (Serial.available()) {
    dato = Serial.read();
    if (dato == 'A') {
      estado = 1;
    } else if (dato == 'B') {
      resetServos();
    }
  }

  // Control de la máquina de estados
  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    switch (estado) {
      case 1:
        moverBase();
        break;
      case 2:
        moverHombro();
        break;
      case 3:
        moverMano();
        break;
      case 4:
        regresarHombro();
        break;
      case 5:
        regresarBase();
        break;
      default:
        // Estado por defecto, no hacer nada
        break;
    }
  }
}

// Funciones de movimiento por estado
void moverBase() {
  if (ang1 < 130) {
    ang1 += 2;
    baseServo.write(ang1);
    if (ang1 == 100) {
      estado = 2;
    }
  } else {
    estado = 4; // Ir directamente a regresar el hombro si ya alcanzó el ángulo máximo
  }
}

void moverHombro() {
  if (ang2 < 90) {
    ang2 += 2;
    hombroServo.write(ang2);
    if (ang2 == 90) {
      estado = 3;
    }
  }
}

void moverMano() {
  if (ang3 > 0) {
    ang3 -= 2;
    manoServo.write(ang3);
    if (ang3 == 0) {
      estado = 4;
    }
  }
}

void regresarHombro() {
  if (ang2 > 0) {
    ang2 -= 2;
    hombroServo.write(ang2);
    if (ang2 == 0) {
      estado = 5;
    }
  }
}

void regresarBase() {
  if (ang1 > 0) {
    ang1 -= 2;
    baseServo.write(ang1);
  } else {
    estado = 0; // Volver al estado inicial
  }
}

void resetServos() {
  ang1 = 0;
  ang2 = 0;
  ang3 = 90;
  baseServo.write(ang1);
  hombroServo.write(ang2);
  manoServo.write(ang3);
  estado = 0;
}