//PINS
int ledPinR = 7;
int ledPinr= 9;
int ledPinL = 10;
int ledPinl = 8;
int ddPin = 13;
int sensorRPin = 0;
int sensorLPin = 2;
int ultraSoundPin = 12;

//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
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] = {70,10,60,10,120,30,70,10,60,10,120,10};
char song4[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,2,song0);
  play(0);
  delay(1000);
  loadsong(1,6,song1);
  play(1);
  delay(1000);
  loadsong(2,8,song2);
  play(2);
  delay(1000);
  pinMode(ledPinR, OUTPUT);
  pinMode(ledPinr, OUTPUT);
  pinMode(ledPinL, OUTPUT);
  pinMode(ledPinl, OUTPUT);
}

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(-200,straight,500);
  drive(200,rad,1000);
  drive(200,straight,100);
}

void moveForward (int rad){
  drive(200,rad,200);
  drive(200,straight,100);
  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++)
      {
      }
    }*/
    sensorbytes[i++] = c;
  }
}

void charge()
{        
  boolean on = 0;
  digitalWrite (ledPinR,LOW);        
  long charge = 0;
  packet = 3;
  updateSensors();
  charge = (sensorbytes[6] << 8) | sensorbytes [7];
  if (charge < chargelow) 
  {
    Serial.print(135, BYTE);		//CLEAN para poder ejecutar force-dock
    delay(2000);
    Serial.print(143,BYTE);		//Force-dock
    digitalWrite(ledPinL, HIGH);
    while(sensorbytes[0] ==0)
    {
      packet = 3;
      updateSensors();
      delay(500);
    }
        digitalWrite (ledPinL,HIGH);	//ledPin se puede substituir por un led del roomba
        delay(50);
        digitalWrite (ledPinl,HIGH);
        delay(50);
        digitalWrite (ledPinr,LOW);
        delay(50);
        digitalWrite (ledPinR,LOW);
    while(sensorbytes[0] ==2)
    {
      packet = 3;
      updateSensors();
      delay(5000);
      if (on = 1){
        digitalWrite(ledPinR, LOW);
        digitalWrite (ledPinr, HIGH); 
        on = 0;
      }
      else {
        digitalWrite (ledPinL, HIGH);
     digitalWrite (ledPinl,- HIGH);    
        on = 1;
      }
    }
    digitalWrite(ledPinR, LOW);
    Serial.print(135, BYTE);		//CLEAN para q salga de la base dock
    delay(5000);	
    inicio();
  }
}
/******************** 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(500, -1, 3000);
  }
}

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

/************************ LOOP **************************/
void loop()
{
  drive(200,straight,0);  //???
  packet = 1;
  updateSensors();
  detection++;

  if (wall){
    play(2);
    drive(200,1,2000);
    drive(200,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 detecta una persona
    drive(-500,straight,500);
    drive(0,-1,100);

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

void ledLeft(){
          digitalWrite (ledPinR,LOW);	//ledPin se puede substituir por un led del roomba
        digitalWrite (ledPinr,HIGH);
        delay(100);
          digitalWrite (ledPinR,LOW);	//ledPin se puede substituir por un led del roomba
        digitalWrite (ledPinr,HIGH);
        delay(50);
}

void ledRight(){
        digitalWrite (ledPinL,LOW);
         digitalWrite (ledPinl,HIGH);
        delay(100);
        digitalWrite (ledPinL,LOW);
         digitalWrite (ledPinl,HIGH);
        delay(100);        
}