/////////////////BUZZER////////////
#define C 261
#define D 294
#define E 329
#define F 349
#define G 392
#define A 440
#define B 494
#define SILENCIO 0
int buzzerPin = 4; // Cambia el número según el pin que estás usando
int sirenNotes[] = {C, SILENCIO, G, SILENCIO, C, SILENCIO, G, SILENCIO}; // Alternando entre C, G y silencios
int noteDurations[] = {500, 200, 500, 200, 500, 200, 500, 200}; // Duraciones de las notas y los silencios
///////////////////RGB////////////
#define PIN_RED 10
#define PIN_GREEN 3
//////////////////JOYSTICKS///////
#define VX_IZQ A0
#define VY_IZQ A1
#define SEL_IZQ 13

#define VX_DER A2
#define VY_DER A3
#define SEL_DER 12
///////////////////SMART CAR///////
//    The direction of the car's movement
//  ENA   ENB   IN1   IN2   IN3   IN4   Description  
//  HIGH  HIGH  HIGH  LOW   LOW   HIGH  Car is runing forward
//  HIGH  HIGH  LOW   HIGH  HIGH  LOW   Car is runing back
//  HIGH  HIGH  LOW   HIGH  LOW   HIGH  Car is turning left
//  HIGH  HIGH  HIGH  LOW   HIGH  LOW   Car is turning right
//  HIGH  HIGH  LOW   LOW   LOW   LOW   Car is stoped
//  HIGH  HIGH  HIGH  HIGH  HIGH  HIGH  Car is stoped
//  LOW   LOW   N/A   N/A   N/A   N/A   Car is stoped
//define L298n module IO Pin
#define ENA 6
#define ENB 5
#define IN1 11
#define IN2 9
#define IN3 8
#define IN4 11
/////////////ULTRASONIC SENSOR/////////
#include <Servo.h>  //servo library
Servo myservo;      // create servo object to control servo

int Echo = A4;
int Trig = A5;
float distance=0;
bool Encontrar=false;

void stop(){
  digitalWrite(ENA,LOW); //enable L298n A channel
  digitalWrite(ENB,LOW); //enable L298n B channel
  Serial.println("DETENER");
}
void forward(){ 
  digitalWrite(ENA,HIGH); //enable L298n A channel
  digitalWrite(ENB,HIGH); //enable L298n B channel
  digitalWrite(IN1,HIGH); //set IN1 hight level
  digitalWrite(IN2,LOW);  //set IN2 low level
  digitalWrite(IN3,LOW);  //set IN3 low level
  digitalWrite(IN4,HIGH); //set IN4 hight level
  Serial.println("AVANZAR");//send message to serial monitor
}

void back(){
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);
  Serial.println("RETROCEDER");
}

void left(){
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH); 
  Serial.println("IZQUIERDA");
}

void right(){
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);
  Serial.println("DERECHA");
}

void speed_control(){
  for(int i = 255; i >= 0; i--){ 
    analogWrite(ENB,i);//MOTORES DERECHA 
    analogWrite(ENA,i);//MOTORES IZQUIERDA
    delay(20);
  }
}

float getDistance() {
    digitalWrite(Trig, LOW);
    delayMicroseconds(2);
    digitalWrite(Trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(Trig, LOW);
    return (float)pulseIn(Echo, HIGH) / 58.0;
}


void setup() {
  pinMode(buzzerPin, OUTPUT);
///////////////////////////////
  pinMode(PIN_RED, OUTPUT); 
  pinMode(PIN_GREEN, OUTPUT); 
  pinMode(PIN_GREEN, OUTPUT); 
 //////////////////////////////
  pinMode(VX_IZQ, INPUT); 
  pinMode(VY_IZQ, INPUT); 
  pinMode(SEL_IZQ, INPUT_PULLUP); 
//////////////////////////////////
  pinMode(VX_DER, INPUT); 
  pinMode(VY_DER, INPUT); 
  pinMode(SEL_DER, INPUT_PULLUP); 
//////////////////////////////////////
  attachInterrupt(SEL_IZQ,stop,FALLING);
  attachInterrupt(SEL_DER,stop,FALLING);
/////////////////////////////////////////
Serial.begin(9600);//open serial and set the baudrate

  pinMode(IN1,OUTPUT);//before useing io pin, pin mode must be set first 
  pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
  ////////////////////////////////////////////
  myservo.attach(3);
  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT);
  //////////////////////
  analogWrite(PIN_RED, 0);  // Enciende el LED rojo
  delay(1000);
  analogWrite(PIN_RED, 255); // Apaga el LED rojo
  delay(1000);
  analogWrite(PIN_GREEN, 0); // Enciende el LED verde
  delay(1000);
  analogWrite(PIN_GREEN, 255); // Apaga el LED verde
}

void loop() {
  ///////////////ALARMA ALERTA///////////
 /* for (int i = 0; i < 8; i++) {
    if (sirenNotes[i] != SILENCIO) {
      tone(buzzerPin, sirenNotes[i], noteDurations[i]);
    } else {
      noTone(buzzerPin); // Apaga el buzzer para el silencio
    }
    delay(noteDurations[i]);
  }*/
  //////////////////////
  myservo.write(90);

  distance= getDistance();
  Serial.print("DISTANCIA: ");
  Serial.println(distance,2);
  delay(1000);
  if(distance<= 13){
    stop();
    Encontrar=Search_path();
    if(Encontrar){
        
    }else{
      back();
      delay(3000);
      break;
    }
  }
}

bool Search_path(){
  while(Path){
      for(i=0,i<=170,i++){
        myservo.write(i);
        delay(20);
        distance= getDistance();
        if(distance>13&&distance<300){
            Path!=Path;
            Encontrado=true;
            Serial.println("Path encontrado...");
            break;
        }else{
          Serial.println("Buscando Path...");
        }
      }
      if(Path){
        Serial.println("Path NO encontrado...");
        break;
        Encontrado=false;
      }
  }
  return Encontrado;
}