int vel = 200;
int velfast = 300;
int velslow = 140;
int velgiro = 400;

//PINS
int ledPin = 13;
int ddPin = 2;
int sensorRPin = 1;
int sensorLPin = 2;
int ultraSoundPin = 5;

//variables para drive
int straight = 0x8000;		//Valor de ir recto

//variables para play y song 
char song0[12] = {60,10,120,13,92,15,113,21,74,31,85,15};                //IR left
char song1[12] = {94,10,93,10,94,10,93,10,94,10,93,10};                  //usound
char song2[16] = {90,10,130,10,90,10,130,10,60,10,130,10,60,10,130,10};  //wall
char song3[12] = {72,10,132,13,104,15,125,21,86,31,97,15};               //IR right
char song4[12] = {70,10,60,10,120,30,70,10,60,10,120,10};
char song5[10] = {80,10,70,10,80,10,70,10,60,20};

//variables para updateSensors
int packet = 0;			//paquete de sensores.
char sensorbytes[10];
boolean irDetect = false;

#define bumpright	(sensorbytes[0] & 0x01)
#define bumpleft	(sensorbytes[0] & 0x02)
#define wall	        (sensorbytes[6] & 0x01)

//variables para charge
long chargelow = 500;
int detection = 0;

/************************ INCIO ****************************/
void setup()
{
  Serial.begin(19200);    //velocidad de transmission.
  inicio();
  loadsong(0,4,song0);
  play(0);
  delay(1000);
  loadsong(1,6,song1);
  play(1);
  delay(1000);
  loadsong(2,8,song2);
  play(2);
  delay(1000);
  loadsong(3,4,song3);
  play(3);
  delay(1000);
  loadsong(4,6,song4);
  play(4);
  delay(1000);
}

void inicio()
{
  pinMode(ddPin, OUTPUT);
  digitalWrite (ddPin,HIGH);
  delay(100);
  digitalWrite (ddPin,LOW);
  delay(500);
  digitalWrite (ddPin,HIGH);
  delay(200);

  Serial.print(128,BYTE);	//START
  delay(50);
  Serial.print(130,BYTE);	//CONTROL
  delay(50);
  pinMode(sensorRPin,INPUT);
  pinMode(sensorLPin,INPUT);

  leds(0,0,0);
}

/************************ MOVIMIENTO ****************************/
void drive(int velocity, int radius, int del)
{
  int velocityhigh = (velocity & 0xFF00) >> 8;  //Valores entre -500 i 500 mm/s
  int velocitylow = velocity & 0x00FF;
  int radiushigh = (radius & 0xFF00) >> 8;      //Valores entre -2000 i 2000 mm
  int radiuslow = radius & 0x00FF;
  Serial.print (137,BYTE);
  Serial.print (velocityhigh,BYTE);
  Serial.print (velocitylow,BYTE);
  Serial.print (radiushigh,BYTE);
  Serial.print (radiuslow,BYTE);
  delay(del);
}

void moveBack(int rad){
  drive(-velfast,straight,500);
  drive(vel,rad,1000);
  drive(vel,straight,100);
}

void moveForward(int rad){
  drive(vel,rad,200);
  drive(vel,straight,100);
  leds(0x00,0,0);
  detection = 0;
}

void moveForwardSlow(int rad){
  drive(velslow,rad,200);
  drive(velslow,straight,200);
  leds(0x00,0,0);
  detection = 0;
}

/************************ LUCES ****************************/
void leds(byte LEDsel, byte powerColor, byte powerIntensity)
{
  Serial.print (139, BYTE);
  Serial.print (LEDsel, BYTE);          //byte con el estado de los leds
  Serial.print (powerColor, BYTE);     //0 = green y 255 = red
  Serial.print (powerIntensity, BYTE); //intensidad del boton power, 0=off y 255=full
}

/************************ SONIDOS ****************************/
void play(int nSong)
{
  Serial.print(141, BYTE);
  Serial.print(nSong,BYTE);
}

void loadsong(int nSong, int nNotes, char song[])
{
  int i = 0;
  Serial.print (140, BYTE);
  Serial.print (nSong, BYTE);
  Serial.print (nNotes, BYTE);
  for (i=0; i<(nNotes*2); i++)
  {
    Serial.print (song[i], BYTE);
    i++;
    Serial.print (song[i], BYTE);
  }
}

/************************ SENSORES ****************************/
void updateSensors()
{	
  int i = 0;
  Serial.print (142, BYTE);
  Serial.print (packet, BYTE);
  delay(100);
  while (Serial.available())
  {
    int c = Serial.read();
    if (c== -1)
    {
      for (i=0; i<5; i++)
      {
        digitalWrite (ledPin,HIGH);	//ledPin se puede substituir por un led del roomba
        delay(50);
        digitalWrite (ledPin,LOW);
        delay(50);
      }
    }
    sensorbytes[i++] = c;
  }
}

/******************** Ultra Sound **********************/
long ultraSound(){
  int val = 0;                         // Variable auxiliar
  long ultrasoundValue = 0;             // Valor del senros de ultrasonidos
  long timecount = 0;                   // Contador del eco

  pinMode(ultraSoundPin, OUTPUT);   // Conmuta el PIN de la señal a salida digital

  //Envía un pulso bajo-alto-bajo para lanzar el pulso de activación del sensor 
  digitalWrite(ultraSoundPin, LOW); // Envía un pulso bajo
  delayMicroseconds(2);                // Espera dos microsegundos
  digitalWrite(ultraSoundPin, HIGH);// Envía un pulso alto
  delayMicroseconds(5);                // Espera 5 segundos
  digitalWrite(ultraSoundPin, LOW); // Se queda en espera

  //En espera, escuchando el pulso de eco
  pinMode(ultraSoundPin, INPUT);    // Conmuta el PIN de la señal a entrada digital
  val = digitalRead(ultraSoundPin); // Recoge el valor del PIN de señal
  while(val == LOW) {                  // Espera a que el PIN de señal sea alto
    val = digitalRead(ultraSoundPin);
  }

  while(val == HIGH) {                   // Espera a que el PIN de señal sea bajo
    val = digitalRead(ultraSoundPin);
    timecount = timecount +1;            // Cuenta el tiempo que dura el pulso alto (amplitud)
  }

  ultrasoundValue = timecount;           // El contador es el valor obtenido por el sensor
  return ultrasoundValue;
}

void detectUsound(){
  if(ultraSound() < 600) {
    //distancia mas pequeña que x
    leds(0x00, 255, 255); 
    play(1);
    drive(velgiro, -1, 3000);
  }
}

int irReadRight(){
  return analogRead(sensorRPin);
}  
  
int irReadLeft(){
  return analogRead(sensorLPin);
}

/************************ LOOP **************************/
void loop()
{
  packet = 1;
  updateSensors();
  detection++;

  if (wall){
    play(2);
    drive(vel,1,2000);
    drive(vel,straight,100);
  }
  else if(bumpleft && (detection >= 25)) {
    //esto es si pasan 25 ciclos sin chocar izq., choca y se vuelve hacia atrás
    moveBack(-1);
  }
  else if(bumpright && (detection >= 25)) {
    //esto es si pasan 25 ciclos sin chocar der., choca y se vuelve hacia atrás
    moveBack(1);
  }
  else if (detection < 25 && (bumpleft || bumpright)) {
    //esto es si choca contra una persona
    drive(-vel,straight,500);
    drive(0,-1,100);

    play(4);
    for (int i=0;i<5;i++) {
      leds(0x0E,0,255);
      delay(500);
      leds(0x00,0,0);
      delay(500);
    }
    delay(1000);
    drive(vel,1,2000);
    drive(vel,straight,100);
  }
  else if ((irReadRight()<190) && (irReadLeft()<200)){
    //detección por sensor IR derecha & izquierda
    leds(0x11,0,0);
    moveForwardSlow(straight);
    detectUsound();
  }
  else if(irReadLeft()<200) {
    //detección por sensor IR izquierda
    irDetect = true;
    leds(0x10,0,0);
    moveForwardSlow(1);
    play(0);
    detectUsound();
  }
  else if (irReadRight()<190) {
    //detección por sensor IR derecha
    irDetect = true;
    leds(0x01,0,0);
    moveForwardSlow(-1);
    play(3);
    detectUsound();
  }
  else if (irDetect) {
    //perdimos la persona, a ver si sta a la izquierda
    irDetect = false;
    leds(0x01,0,0);
    moveForwardSlow(1);
    play(2);
    detectUsound();
  }
  else {
    irDetect = false;
    drive(velfast,straight,0);
  }
}