/////////////////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 A2
#define VY_IZQ A3
#define SEL_IZQ 13

#define VX_DER A0
#define VY_DER A1
#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;

int VEL_X,VEL_Y,POS_X,POS_Y,Stop_Der,Stop_Izq;

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;
}

bool Search_path(){
  bool Encontrado=false;
  bool Search=true;

  while(Search){//para que solo se ejecute 1 abusqueda
      for(int i=0;i<=170;i++){
        myservo.write(i);
        delay(20);
        distance= getDistance();
        if(distance>13&&distance<300){
            Search!=Search;
            Encontrado=true;
            Serial.println("Path encontrado...");
            break;//sale del bucle
        }else{
          Serial.println("Buscando Path...");
        }
      }
      if(Search){//si sale y no se ha encontrado 
        Serial.println("Path NO encontrado...");
        Encontrado=false;
        break;
      }
  }
  return Encontrado;
}


void setup() {
  pinMode(buzzerPin, OUTPUT);
///////////////////////////////
  pinMode(PIN_RED, 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);
  //////////////////////
}

void loop() {
  ///////////////LECTURA JOYSTICKS///////////
  VEL_X=map(analogRead(VX_IZQ),);
  VEL_Y=analogRead(VY_IZQ);

  POS_X=analogRead(VX_DER);
  POS_Y=analogRead(VY_DER);
 
  Stop_Der=analogRead(SEL_DER);
  Stop_Izq=analogRead(SEL_IZQ);
  if(POS_X>530){
    right();
  }
   if(POS_X<470){
    left();
  }
   if(POS_Y>550){
    forward();
  }
   if(POS_Y>490){
    back();
  }


 
  //////////////////////
  /*myservo.write(90);

  distance= getDistance();
  Serial.print("DISTANCIA: ");
  Serial.println(distance,2);
  delay(1000);
  if(distance<= 13){

      analogWrite(PIN_RED, 255); // ENCIENDE el LED rojo
      delay(1000);
      analogWrite(PIN_RED, 0);
      delay(1000);
      analogWrite(PIN_GREEN, 255);
      delay(1000);
      analogWrite(PIN_GREEN, 0);
      delay(1000);

      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]);
      }
      noTone(buzzerPin);
  }
*/
}

void imprimir_joystick(){
  Serial.print(" | velocidad X: ");
  Serial.print(VEL_X);
  Serial.print(" | velocidad Y: ");
  Serial.print(VEL_Y);
  Serial.print(" | BOTON IZQ: ");
  Serial.print(Stop_Der);

  Serial.print(" | posicion X: ");
  Serial.print(POS_X);
  Serial.print(" | posicion Y: ");
  Serial.print(POS_Y);
  Serial.print(" | BOTON DER: ");
  Serial.println(Stop_Der);
  delay(1000);
}