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

//Servos Configuraciones
int AnguloMin[8] = {10, 0, 0, 0, 0, 0, 0, 0};
int AnguloIntermedio1[8] = {25, 10, 10, 10, 10, 10, 10, 10};
int AnguloMedio[8] = {35,0,0,0,0,0,0,0};
int AnguloIntermedio2[8] = {55, 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); // Any baud rate should work
  DeclararObjetos();
  //EncenderLucesPruebas();
  //EncenderServosPrueba();  

}

void loop() {
  // Tu código principal aquí, se ejecutará de forma repetitiva
  Serial.println("Hola mundo");
ContarCambioDistancia();
  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:
      contador(1000);
    AnalisisDeIntervalos(Ultrasonico, AnguloMin[0], AnguloIntermedio1[0],AnguloMedio[0], AnguloIntermedio2[0], AnguloMax[0]);
      contador(1000);
      break;

    case 1:
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMax[Ultrasonico]);
      distancia = ultrasonicos[1].ping_cm();
      DistanciasMedidas[1] = distancia;
      Serial.print("La distancia medida por el Sensor 2 es: ");
      Serial.println(DistanciasMedidas[1]);
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMin[Ultrasonico]);
      break;

    case 2:
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMax[Ultrasonico]);
      distancia = ultrasonicos[2].ping_cm();
      DistanciasMedidas[2] = distancia;
      Serial.print("La distancia medida por el Sensor 3 es: ");
      Serial.println(DistanciasMedidas[2]);
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMin[Ultrasonico]);
      break;

    case 3:
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMax[Ultrasonico]);
      distancia = ultrasonicos[3].ping_cm();
      DistanciasMedidas[3] = distancia;
      Serial.print("La distancia medida por el Sensor 4 es: ");
      Serial.println(DistanciasMedidas[3]);
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMin[Ultrasonico]);
      break;
  
    case 4:
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMax[Ultrasonico]);
      distancia = ultrasonicos[4].ping_cm();
      DistanciasMedidas[4] = distancia;
      Serial.print("La distancia medida por el Sensor 5 es: ");
      Serial.println(DistanciasMedidas[4]);
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMin[Ultrasonico]);
      break;

    case 5:
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMax[Ultrasonico]);
      distancia = ultrasonicos[5].ping_cm();
      DistanciasMedidas[5] = distancia;
      Serial.print("La distancia medida por el Sensor 6 es: ");
      Serial.println(DistanciasMedidas[5]);
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMin[Ultrasonico]);
      break;

    case 6:
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMax[Ultrasonico]);
      distancia = ultrasonicos[6].ping_cm();
      DistanciasMedidas[6] = distancia;
      Serial.print("La distancia medida por el Sensor 7 es: ");
      Serial.println(DistanciasMedidas[6]);
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMin[Ultrasonico]);
      break;

    case 7:
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMax[Ultrasonico]);
      distancia = ultrasonicos[7].ping_cm();
      DistanciasMedidas[7] = distancia;
      Serial.print("La distancia medida por el Sensor 8 es: ");
      Serial.println(DistanciasMedidas[7]);
      contador(1000);
      MoverMotor(Ultrasonico, AnguloMin[Ultrasonico]);
      break;

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

  return distancia;
}

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

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

void AnalisisDeIntervalos(int Ultrasonico, int AnguloMin, int AnguloIntermedio1, int AnguloMedio, int AnguloIntermedio2, int AnguloMax) {
  int ContadorIntervalo1 = 0;
  int distancias = 0;
  bool FlagPrimerIntervalo = false;
  bool FlagSegundoIntervalo = false;
  bool FlagTercerIntervalo = false;

  for (ContadorIntervalo1 = 0; ContadorIntervalo1 <= AnguloMin; ContadorIntervalo1++) {
    MoverMotor(Ultrasonico, ContadorIntervalo1);
    distancias = ultrasonicos[Ultrasonico].ping_cm();
    Serial.println(distancias);
    if (distancias != 0 && FlagPrimerIntervalo == false) {
      MedirCantidadVehiculos[Ultrasonico]++;
      Serial.println(MedirCantidadVehiculos[Ultrasonico]);
      FlagPrimerIntervalo = true;
      break;
    }
  }
  
  // Bucle del Segundo intervalo.
  for (int ContadorIntervalo2 = AnguloMin + 1; ContadorIntervalo2 <= AnguloMedio; ContadorIntervalo2++) {
    MoverMotor(Ultrasonico, ContadorIntervalo2);
    distancias = ultrasonicos[Ultrasonico].ping_cm();
    Serial.println(distancias);
    if (distancias != 0 && FlagSegundoIntervalo == false) {
      MedirCantidadVehiculos[Ultrasonico]++;
      FlagSegundoIntervalo = true;
      Serial.println(MedirCantidadVehiculos[Ultrasonico]);
      break;
    }
  }
  
  // Bucle del Tercer intervalo.
  for (int ContadorIntervalo3 = AnguloMedio + 1; ContadorIntervalo3 <= AnguloMax; ContadorIntervalo3++) {
    MoverMotor(Ultrasonico, ContadorIntervalo3);
    distancias = ultrasonicos[Ultrasonico].ping_cm();
    Serial.println(distancias);
    if (distancias != 0 && FlagTercerIntervalo == false) {
      MedirCantidadVehiculos[Ultrasonico]++;
      FlagTercerIntervalo = true;
      Serial.println(MedirCantidadVehiculos[Ultrasonico]);
      break;
    }
  }
  if(MedirCantidadVehiculos[Ultrasonico] > 3 ){
    MedirCantidadVehiculos[Ultrasonico] = 3;
  }
}