#include <NewPing.h>
#include <Servo.h>
//Agregar el movimiento de los motores a la funcion de Medir distancias.

//Servos Configuraciones
int AnguloMin[8] = {50, 0, 0, 0, 0, 0, 0, 0};
int AnguloIntermedio1[8] = {85, 10, 10, 10, 10, 10, 10, 10};
int AnguloMedio[8] = {115,0,0,0,0,0,0,0};
int AnguloIntermedio2[8] = {125, 10, 10, 10, 10, 10, 10, 10};
int AnguloMax[8] = {180, 90, 45, 35, 25, 15, 10, 5};
int PinServos[8] = {13, 12, 11, 10, 9, 8, 7, 6};
Servo servos[8];  // Array para almacenar los objetos Servo


//Ultrasonicos Configuraciones
int DistanciaUltrasonicos[8] = {10,200,200,200,200,200,200,200};
unsigned int DistanciasMedidas[8] = {0,0,0,0,0,0,0,0};
int PinEcho[8] = {5, 4, 3, 2, A0, A1, A2, A3};
int PinTrig[8] = {A4,A5,A6,A7,A8,A9,A10,A11};

//NewPing Ultrasonico_1(PinTrig[0],PinEcho[0],DistanciaUltrasonicos[0]);
//NewPing Ultrasonico_2(PinTrig[1],PinEcho[1],DistanciaUltrasonicos[1]);
//NewPing Ultrasonico_3(PinTrig[2],PinEcho[2],DistanciaUltrasonicos[2]);
//NewPing Ultrasonico_4(PinTrig[3],PinEcho[3],DistanciaUltrasonicos[3]);
//NewPing Ultrasonico_5(PinTrig[4],PinEcho[4],DistanciaUltrasonicos[4]);
//NewPing Ultrasonico_6(PinTrig[5],PinEcho[5],DistanciaUltrasonicos[5]);
//NewPing Ultrasonico_7(PinTrig[6],PinEcho[6],DistanciaUltrasonicos[6]);
//NewPing Ultrasonico_8(PinTrig[7],PinEcho[7],DistanciaUltrasonicos[7]);


NewPing ultrasonicos[8] = {
  NewPing(PinTrig[0], PinEcho[0], DistanciaUltrasonicos[0]),
  NewPing(PinTrig[1], PinEcho[1], DistanciaUltrasonicos[1]),
  NewPing(PinTrig[2], PinEcho[2], DistanciaUltrasonicos[2]),
  NewPing(PinTrig[3], PinEcho[3], DistanciaUltrasonicos[3]),
  NewPing(PinTrig[4], PinEcho[4], DistanciaUltrasonicos[4]),
  NewPing(PinTrig[5], PinEcho[5], DistanciaUltrasonicos[5]),
  NewPing(PinTrig[6], PinEcho[6], DistanciaUltrasonicos[6]),
  NewPing(PinTrig[7], PinEcho[7], DistanciaUltrasonicos[7])
};

//Definimos las Luces.
int PinLuces[32]= {53,52,51,50,49,48,47,46,45,44,43,42,41,40,39,38,37,36,35,34,33,32,31,30,29,28,27,26,25,24,23,22};

//

int MedirCantidadVehiculos[8] = {};
void setup() {
  Serial.begin(9600);
  DeclararObjetos();
  EncenderLucesPruebas();
  EncenderServosPrueba();  

}

void loop() {
  // Tu código principal aquí, se ejecutará de forma repetitiva
//Serial.println("Hola mundo");
ContarCambioDistancia(); //Tu funcion
 // Serial.print("La cantidad de vehiculoe es: ");
  //Serial.println(MedirCantidadVehiculos[0]);
}

void contador(unsigned long tiempoEspera) {
  unsigned long tiempoInicial = millis(); // Tiempo inicial
  
  while (millis() - tiempoInicial < tiempoEspera) {
    // Realizar operaciones adicionales si es necesario
    
    // No hacer nada, esperar el tiempo especificado
  }
  
  // Realizar operaciones después de que ha pasado el tiempo de espera
  static int contador = 0; // Variable estática para mantener el valor del contador entre llamadas a la función
  contador++; // Incrementar el contador
  
}


void DeclararObjetos() {
  for (int i = 0; i < 8; i++) {
    servos[i].attach(PinServos[i]);  // Adjuntar cada objeto Servo a un pin específico
  }
  delay(100);
  //Definimos los pines de las luces como salidas.
  int j=0;
  for(j=0;j<32;j++){
    pinMode(PinLuces[j],OUTPUT);
  }

//Definimos los pines analogicos como Pines DIGITALES de Salida de los triggers de los Ultrasonicos.
  for(int k=58; k<=69; k++){
    pinMode(k, OUTPUT); //Establece los pines como salida
  }

//Definimos los pines analogicos como PINES digitales de ENTRADA para los Ultrasonicos ECHO
  for(int a=54; a<=57; a++){
    pinMode(a, INPUT); //Establece los pines como entrada
  }
  //Definimos los Pines de Entrada Digital de los Echos normales.
  for(int b=2; b<=5; b++){
    pinMode(b, INPUT); //Establece los pines como entrada
  }
}

void EncenderServosPrueba(){
  //Esta funcion se debe correr solamenta al prender y apagar el arduino
 int j=0;
  for(j=0;j<8;j++){
    servos[j].write(180);
      contador(1000);
  }

  contador(1000);

    for(j=0;j<8;j++){
    servos[j].write(0);
      contador(1000);
  }

}

void EncenderLucesPruebas(){
  int j=0;
  for(j=0;j<32;j++){
    digitalWrite(PinLuces[j],HIGH);
  }
    contador(1000);
    for(j=0;j<32;j++){
    digitalWrite(PinLuces[j],LOW);
  }
    contador(1000);
}

unsigned int MedirDistancia(int Ultrasonico) {
  unsigned int distancia = 0;
  switch (Ultrasonico) {
    case 0:
  MoverMotor(Ultrasonico,0);
    MedirCantidadVehiculos[Ultrasonico] = 0;
    contador(500);
   AnalisisDeIntervaloUno(Ultrasonico, AnguloMin[Ultrasonico]);
    contador(500);
  AnalisisDeIntervaloDos(Ultrasonico, AnguloMedio[Ultrasonico], AnguloIntermedio1[Ultrasonico]);
      contador(500);
  AnalisisDeIntervaloTres(Ultrasonico, AnguloMax[Ultrasonico], AnguloIntermedio2[Ultrasonico]);
      contador(500);
      break;

    case 1:
    MedirCantidadVehiculos[Ultrasonico] = 0;
    contador(500);
   AnalisisDeIntervaloUno(Ultrasonico, AnguloMin[Ultrasonico]);
    contador(500);
  AnalisisDeIntervaloDos(Ultrasonico, AnguloMedio[Ultrasonico], AnguloIntermedio1[Ultrasonico]);
      contador(500);
  AnalisisDeIntervaloTres(Ultrasonico, AnguloMax[Ultrasonico], AnguloIntermedio2[Ultrasonico]);
      contador(500);
      break;

    case 2:
    MedirCantidadVehiculos[Ultrasonico] = 0;
    contador(500);
   AnalisisDeIntervaloUno(Ultrasonico, AnguloMin[Ultrasonico]);
    contador(500);
  AnalisisDeIntervaloDos(Ultrasonico, AnguloMedio[Ultrasonico], AnguloIntermedio1[Ultrasonico]);
      contador(500);
  AnalisisDeIntervaloTres(Ultrasonico, AnguloMax[Ultrasonico], AnguloIntermedio2[Ultrasonico]);
      contador(500);
      break;

    case 3:
    MedirCantidadVehiculos[Ultrasonico] = 0;
    contador(500);
   AnalisisDeIntervaloUno(Ultrasonico, AnguloMin[Ultrasonico]);
    contador(500);
  AnalisisDeIntervaloDos(Ultrasonico, AnguloMedio[Ultrasonico], AnguloIntermedio1[Ultrasonico]);
      contador(500);
  AnalisisDeIntervaloTres(Ultrasonico, AnguloMax[Ultrasonico], AnguloIntermedio2[Ultrasonico]);
      contador(500);
      break;
  
    case 4:
    MedirCantidadVehiculos[Ultrasonico] = 0;
    contador(500);
   AnalisisDeIntervaloUno(Ultrasonico, AnguloMin[Ultrasonico]);
    contador(500);
  AnalisisDeIntervaloDos(Ultrasonico, AnguloMedio[Ultrasonico], AnguloIntermedio1[Ultrasonico]);
      contador(500);
  AnalisisDeIntervaloTres(Ultrasonico, AnguloMax[Ultrasonico], AnguloIntermedio2[Ultrasonico]);
      contador(500);
      break;

    case 5:
    MedirCantidadVehiculos[Ultrasonico] = 0;
    contador(500);
   AnalisisDeIntervaloUno(Ultrasonico, AnguloMin[Ultrasonico]);
    contador(500);
  AnalisisDeIntervaloDos(Ultrasonico, AnguloMedio[Ultrasonico], AnguloIntermedio1[Ultrasonico]);
      contador(500);
  AnalisisDeIntervaloTres(Ultrasonico, AnguloMax[Ultrasonico], AnguloIntermedio2[Ultrasonico]);
      contador(500);
      break;

    case 6:
    MedirCantidadVehiculos[Ultrasonico] = 0;
    contador(500);
   AnalisisDeIntervaloUno(Ultrasonico, AnguloMin[Ultrasonico]);
    contador(500);
  AnalisisDeIntervaloDos(Ultrasonico, AnguloMedio[Ultrasonico], AnguloIntermedio1[Ultrasonico]);
      contador(500);
  AnalisisDeIntervaloTres(Ultrasonico, AnguloMax[Ultrasonico], AnguloIntermedio2[Ultrasonico]);
      contador(500);
      break;

    case 7:
    MedirCantidadVehiculos[Ultrasonico] = 0;
    contador(500);
   AnalisisDeIntervaloUno(Ultrasonico, AnguloMin[Ultrasonico]);
    contador(500);
  AnalisisDeIntervaloDos(Ultrasonico, AnguloMedio[Ultrasonico], AnguloIntermedio1[Ultrasonico]);
      contador(500);
  AnalisisDeIntervaloTres(Ultrasonico, AnguloMax[Ultrasonico], AnguloIntermedio2[Ultrasonico]);
      contador(500);
      break;

    default:
      // Realizar acciones correspondientes al estado por defecto
      break;
  }

  return distancia;
}

void ContarCambioDistancia() {
  for (int i = 0; i <=7; i++) {
    unsigned int distanciaActual = MedirDistancia(i);
  }
}

int MoverMotor(int motor ,int AnguloServo){
  servos[motor].write(AnguloServo);
}

void AnalisisDeIntervaloUno(int Ultrasonico, int AnguloMin) {
  int ContadorIntervalo1 = 0;
  int distancias = 0;
  bool FlagPrimerIntervalo = false;
  MoverMotor(Ultrasonico, ContadorIntervalo1);
  contador(500);
    for (ContadorIntervalo1 = 0; ContadorIntervalo1 <= AnguloMin; ContadorIntervalo1++) {
    contador(10);
    MoverMotor(Ultrasonico, ContadorIntervalo1);
    contador(10);
    distancias = ultrasonicos[Ultrasonico].ping_cm();
    Serial.println(distancias);
    if (distancias != 0 && FlagPrimerIntervalo == false) {
      MedirCantidadVehiculos[Ultrasonico]++;
      Serial.println(MedirCantidadVehiculos[Ultrasonico]);
      FlagPrimerIntervalo = true;
      break;
    }
  }
  contador(100);
}

void AnalisisDeIntervaloDos(int Ultrasonico, int AnguloMedio, int AnguloIntermedio1) {
  int distancias = 0;
  bool FlagSegundoIntervalo = false;
  int ContadorIntervalo2 = AnguloIntermedio1;
// Bucle del Segundo intervalo.
  MoverMotor(Ultrasonico, ContadorIntervalo2);
  contador(500);
  for (int ContadorIntervalo2 = AnguloIntermedio1; ContadorIntervalo2 <= AnguloMedio; ContadorIntervalo2++) {
   contador(10);
  MoverMotor(Ultrasonico, ContadorIntervalo2);
   contador(10);
    distancias = ultrasonicos[Ultrasonico].ping_cm();
  Serial.println(distancias);
  if (distancias != 0 && FlagSegundoIntervalo == false) {
    MedirCantidadVehiculos[Ultrasonico]++;
    FlagSegundoIntervalo = true;
   Serial.println(MedirCantidadVehiculos[Ultrasonico]);
 break;
     }
  }
  contador(100);
}

  
void AnalisisDeIntervaloTres(int Ultrasonico, int AnguloMaximo, int AnguloIntermedio2) {
  int distancias = 0;
  bool FlagTercerIntervalo = false;
  int ContadorIntervalo3 = AnguloIntermedio2;
// Bucle del Segundo intervalo.
  MoverMotor(Ultrasonico, ContadorIntervalo3);
  contador(500);
  for (int ContadorIntervalo3 = AnguloIntermedio2; ContadorIntervalo3 <= AnguloMaximo; ContadorIntervalo3++) {
   contador(10);
  MoverMotor(Ultrasonico, ContadorIntervalo3);
   contador(10);
    distancias = ultrasonicos[Ultrasonico].ping_cm();
  Serial.println(distancias);
  if (distancias != 0 && FlagTercerIntervalo == false) {
    MedirCantidadVehiculos[Ultrasonico]++;
    FlagTercerIntervalo = true;
   Serial.println(MedirCantidadVehiculos[Ultrasonico]);
 break;
     }
  }
  contador(100);
}