#include <ESP32Servo.h>

int servo_A_Pin = 18;   // pin conectado al servo A
int servo_B_Pin = 17;   // pin conectado al servo B
int led_rojo = 23;      // pin conectado al led Rojo
int led_amarillo = 22;  // pin conectado al led amarillo
int led_verde = 21;     // pin conectado al led verde

Servo servo_A; //servo A
Servo servo_B; //servo B
/* contadores */
unsigned long previousMillis_rojo=0;
unsigned long previousMillis_amarillo=0;
unsigned long previousMillis_verde=0;
unsigned long previousMillis_servo_A=0;
unsigned long previousMillis_servo_B=0;
/* intervalos de cuenta*/
unsigned long interval_rojo=100;
unsigned long interval_amarillo=1000;
unsigned long interval_verde=2000;
unsigned long interval_servo_A=500;
unsigned long interval_servo_B=1000;
/* semaforos */
int led_State_rojo=LOW; // bandera inicial en LOW
int led_State_amarillo=LOW; // bandera inicial en LOW
int led_State_verde=LOW; // bandera inicial en LOW
int led_State_servo_A=LOW;
int led_State_servo_B=LOW;

void mueve_servo_A(int sentido);
void mueve_servo_B(int sentido);
void led_change(int led, int estado);


void setup() {
  pinMode(led_rojo, OUTPUT);
  pinMode(led_amarillo, OUTPUT);
  pinMode(led_verde, OUTPUT);
  servo_A.attach(servo_A_Pin, 500, 2400);
  servo_B.attach(servo_B_Pin, 500, 2400);
  Serial.begin(115200);
  Serial.println("Hello, SuperLoop V3");
 
}

void loop() {
  unsigned long currentMillis = millis();
  
  if(currentMillis - previousMillis_rojo > interval_rojo) 
  {
    previousMillis_rojo = currentMillis;// save the last time you blinked the LED
    if (led_State_rojo == LOW)
      led_State_rojo = HIGH;
    else
      led_State_rojo = LOW;
    led_change(led_rojo, led_State_rojo);// set the LED with the ledState of the variable:
  }
  if(currentMillis - previousMillis_amarillo > interval_amarillo) 
  {
    previousMillis_amarillo = currentMillis;// save the last time you blinked the LED
    if (led_State_amarillo == LOW)
      led_State_amarillo = HIGH;
    else
      led_State_amarillo = LOW;
    led_change(led_amarillo, led_State_amarillo);// set the LED with the ledState of the variable:
  }
  if(currentMillis - previousMillis_verde > interval_verde) 
  {
    previousMillis_verde = currentMillis;// save the last time you blinked the LED
    if (led_State_verde == LOW)
      led_State_verde = HIGH;
    else
      led_State_verde = LOW;
    led_change(led_verde, led_State_verde);// set the LED with the ledState of the variable:
  }
  if(currentMillis - previousMillis_servo_A > interval_servo_A) 
  {
    previousMillis_servo_A = currentMillis;// save the last time you blinked the LED
    if (led_State_servo_A == LOW)
      led_State_servo_A = HIGH;
    else
      led_State_servo_A = LOW;
    mueve_servo_A(led_State_servo_A);
  }
  if(currentMillis - previousMillis_servo_B > interval_servo_B) 
  {
    previousMillis_servo_B = currentMillis;// save the last time you blinked the LED
    if (led_State_servo_B == LOW)
      led_State_servo_B = HIGH;
    else
      led_State_servo_B = LOW;
    mueve_servo_B(led_State_servo_B);
  }

  /*mueve_servo_A();
  mueve_servo_B();
  */
}// fin de loop
/* funcion cambio de estado del led */
void led_change(int led, int estado)
{
  if (led == 23)
    Serial.print("LED Rojo:");
  if (led == 22)
    Serial.print("LED Amarillo:");
  if (led == 21)
    Serial.print("LED verde:");
  digitalWrite(led, estado);
  Serial.println(estado);
}
/*funcilon mueve servo_A */
void mueve_servo_A(int sentido)
{
  int pos = 0;
  if(sentido == 1){
    Serial.println("Servo_A de 0 a 180");
    for (pos = 0; pos <= 180; pos += 1) {
      servo_A.write(pos);
    }
  }
  else{
    Serial.println("Servo_A de 180 a 0");
    for (pos = 180; pos >= 0; pos -= 1) {
      servo_A.write(pos);
    }
  }
}
/*funcilon mueve servo_B */
void mueve_servo_B(int sentido)
{
  int pos = 0;
  if(sentido == 1){
    Serial.println("Servo_B de 0 a 180");
    for (pos = 0; pos <= 180; pos += 1) {
      servo_B.write(pos);
    }
  }
  else{
    Serial.println("Servo_B de 180 a 0");
    for (pos = 180; pos >= 0; pos -= 1) {
      servo_B.write(pos);
    }
  }
}