//PINS
int ledPinR1 = 7;
int ledPinR2= 9;
int ledPinL1 = 10;
int ledPinL2 = 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;
int detection = 0;

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

//TIMER
int pause = 1000; //delay en ms
int r1 = 10;     //segundos minimos de intervalo
int r2 = 60;     //segundos maximos de intervalo
unsigned long time, period, randNum, count;
int estado;

/************************ 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);
  loadsong(3,6,song3);
  play(3);
  delay(1000);
  loadsong(4,5,song4);
  play(4);
  delay(1000);
  pinMode(ledPinR1, OUTPUT);
  pinMode(ledPinR2, OUTPUT);
  pinMode(ledPinL1, OUTPUT);
  pinMode(ledPinL2, OUTPUT);
  initTimer();
}

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,255);
  detection = 0;
}

void moveForwardSlow (int rad){
  drive(100,rad,200);
  drive(100,straight,100);
  leds(0x00,255,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;
  }
}

/******************** 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()
{
  timer();
  updateSensors();
  
  if (wall){
    play(2);
    drive(200,1,2000);
    drive(200,straight,100);
  }

  if (estado == 0){
    //lo que había hasta ahora
    drive(200,straight,0);  //???
    packet = 1;
    detection++;

    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
      irDetect = true;
      leds(0x10,0,0);
      moveForward (-1);
      play(0);
      detectUsound();
      ledLeft();
    }
    else if (irReadRight()<170) {
      //detección por sensor IR derecha
      irDetect = true;
      leds(0x01,100,0);
      moveForwardSlow(1);
      play(0);
      detectUsound();
      ledRight();
    }
    else if (irDetect) {
      //perdimos la persona, a ver si sta a la izquierda
      irDetect = false;
      leds(0x01,0,100);
      moveForward (-1);
      play(0);
      detectUsound();
    }
    else {
      irDetect = false;
    }
  } else if (estado == 1){
    //PARA
    leds(0,255,0);
    drive(0,-1,pause);
    leds(0,0,0);
    play(1);
    delay(500);
  } else if (estado == 2){
    //ADELANTE
    ledLeft();
    play(2);
    delay(500);
    moveForward(straight);
    ledRight();
  } else if (estado == 3){
    //ATRAS
    ledRight();
    play(3);
    delay(500);
    moveBack(straight);
    ledLeft();
  } else{
    //ALEATORIO
    play(4);
    delay(500);
    leds(0x11,0,100);
    drive(200,straight,200);
    drive(200,-1,200);
    drive(200,straight,200);
    drive(200,1,200);
    drive(200,straight,200);
    leds(0,0,0);
  }
}

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

void ledRight(){
  digitalWrite (ledPinL1,LOW);
  digitalWrite (ledPinL2,HIGH);
  delay(100);
  digitalWrite (ledPinL1,HIGH);
  digitalWrite (ledPinL2,LOW);
  delay(100);        
}

/****************TIMER******************/
void initTimer(){
  randomSeed(analogRead(5));
  r1 = r1*1000/pause;
  r2 = r2*1000/pause;
  //randNum = random(r1,r2);
  randNum = 10;
  count = 0;
  period = 0;
  estado = 0;
}

void timer(){
  time = millis()/(pause+65)-period;
  //reset timer
  if (count >= randNum){
    period = millis()/(pause+65);
    randNum = random(r1,r2);
    count = 0;
    estado = random(5);
  } else {
    count++;
  }
}