 BatReader battery;
 
 
+
+
 //-- First step: Configure the pins where the servos are attached
 /*
 

}
*/

void requestDistance()
{
    int distance=Distance(PIN_TriggerBat,PIN_EchoBat);
    Serial.print("&&");
    Serial.print("D ");
    Serial.print(distance);
    Serial.println("%%");
    delay(10);
}

void requestNoise()
{
    int microphone=analogRead(PIN_NoiseSensor);
    Serial.print("&&");
    Serial.print("N ");
    Serial.print(microphone);
    Serial.println("%%");
    delay(10);
}

void requestBattery()
{
       //The first read of the batery is often a wrong reading, so we read it two times. 
    double batteryLevel= battery.readBatPercent();
   batteryLevel= battery.readBatPercent();

    Serial.print("&&");
    Serial.print("B ");
    Serial.print(batteryLevel);
    Serial.println("%%");
    Serial.flush();
}

void requestProgramId()
{
    Serial.print("&&");
    Serial.print("I ");
    Serial.print(programID);
    Serial.println("%%");
    delay(10);
}

void lineaLeds(int linea){
    for (int i=1;i<7;i++){
      ledmatrix.setLed(linea, i);
    }
  }
                                                                             
void lineaLedsOff(int linea){
    for (int i=1;i<7;i++){
      ledmatrix.unsetLed(linea, i);
    }
  }

int noiseLevel(int PinNoiseSensor){
    int i = map(analogRead(PinNoiseSensor),400,800,1,6);
    return i;
  }

void noiseToLed(int noiseLevel){
    switch (noiseLevel){
      case 5: 
      lineaLeds(1);
      case 4: 
      lineaLeds(2);   
      case 3: 
      lineaLeds(3);    
      case 2: 
      lineaLeds(4);    
      case 1: 
      lineaLeds(5);
      break;
      default: break;
    }
    switch (noiseLevel){
      case 1: 
      lineaLedsOff(4);
      case 2: 
      lineaLedsOff(3);
      case 3: 
      lineaLedsOff(2);
      case 4: 
      lineaLedsOff(1);
      break;
      default: break;
    }
  }

   //bqBAT
long TP_init(int trigger_pin, int echo_pin)
   {
    digitalWrite(trigger_pin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigger_pin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigger_pin, LOW);
    long microseconds = pulseIn(echo_pin ,HIGH,100000);
    return microseconds;
  }
long Distance(int trigger_pin, int echo_pin)
  {
    long microseconds = TP_init(trigger_pin, echo_pin);
    long distance;
    distance = microseconds/29/2;
    if (distance == 0){
      distance = 999;
    }
    return distance;
  }


      delay(4000);
      ledmatrix.clearMatrix();
    } 

  ZowiTone(PIN_Buzzer, frec, duration, duration);   
  // tone(PIN_Buzzer,frec,duration);
  // delay(duration);
  // noTone(PIN_Buzzer);
}



//Ultrasound Sensor
long TP_init(int trigger_pin, int echo_pin)
   {
    digitalWrite(trigger_pin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigger_pin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigger_pin, LOW);
    long microseconds = pulseIn(echo_pin ,HIGH,100000);
    return microseconds;
}

long Distance(int trigger_pin, int echo_pin)
  {
    long microseconds = TP_init(trigger_pin, echo_pin);
    long distance;
    distance = microseconds/29/2;
    if (distance == 0){
      distance = 999;
    }
    return distance;
}


void requestDistance()
{
    int distance=Distance(PIN_TriggerBat,PIN_EchoBat);
    Serial.print("&&");
    Serial.print("D ");
    Serial.print(distance);
    Serial.println("%%");
    Serial.flush();
}

void requestNoise()
{
    int microphone=analogRead(PIN_NoiseSensor);
    Serial.print("&&");
    Serial.print("N ");
    Serial.print(microphone);
    Serial.println("%%");
    Serial.flush();
}

void requestBattery()
{
       //The first read of the batery is often a wrong reading, so we read it two times. 
    double batteryLevel= battery.readBatPercent();
    batteryLevel= battery.readBatPercent();

    Serial.print("&&");
    Serial.print("B ");
    Serial.print(batteryLevel);
    Serial.println("%%");
    Serial.flush();
}

void requestProgramId()
{
    Serial.print("&&");
    Serial.print("I ");
    Serial.print(programID);
    Serial.println("%%");
    Serial.flush();
}

void sendAck()
{
    Serial.print("&&");
    Serial.print("A");
    Serial.println("%%");
    Serial.flush();
}


void ZowiTone(int speakerPin, float noteFrequency, long noteDuration, long silentDuration)
{    
  int x;
  // Convert the frequency to microseconds
  float microsecondsPerWave = 1000000/noteFrequency;
  // Calculate how many HIGH/LOW cycles there are per millisecond
  float millisecondsPerCycle = 1000/(microsecondsPerWave * 2);
  // Multiply noteDuration * number or cycles per millisecond
  float loopTime = noteDuration * millisecondsPerCycle;
  // Play the note for the calculated loopTime.
  for (x=0;x<loopTime;x++)   
  {   
              digitalWrite(speakerPin,HIGH); 
              delayMicroseconds(microsecondsPerWave); 
              digitalWrite(speakerPin,LOW); 
              delayMicroseconds(microsecondsPerWave); 
  }
  delay(silentDuration); 
}  




//Pruebas sonidoo
//---
void ZowiMelody(float melodyNotes[], int melodySize){

     for (int i=0;i<=melodySize;i=i+3) //por cada "fila"
     {
        ZowiTone(PIN_Buzzer, melodyNotes[i],melodyNotes[i+1],melodyNotes[i+2]);
     }

}

void conexion (){
          ZowiTone(PIN_Buzzer, note_E6,50,30);
          ZowiTone(PIN_Buzzer, note_E7,50,30);
          ZowiTone(PIN_Buzzer, note_A7,50,30);
}

void desconexion (){
          ZowiTone(PIN_Buzzer, note_E6,50,30);
          ZowiTone(PIN_Buzzer, note_A7,50,30);
          ZowiTone(PIN_Buzzer, note_E7,50,30);
}








//Constant feedback of the sensors. F 1 = Feedback ON  F 0 = Feedback 0
/*void sensorFeedback()
