/*Programar como Arduino Pro Mini con CPU ATMEGA168 a 3.3v y 8MHz*/
#include <Wire.h> 
#include "HMC6352.h"
#include <Servo.h>
//#include <SoftwareServo.h>

 // TONES  ==========================================
// Start by defining the relationship between 
//       note, period, &  frequency. 
#define  c     3830    // 261 Hz 
#define  d     3400    // 294 Hz 
#define  e     3038    // 329 Hz 
#define  f     2864    // 349 Hz 
#define  g     2550    // 392 Hz 
#define  a     2272    // 440 Hz 
#define  b     2028    // 493 Hz 
#define  C     1912    // 523 Hz 
// Define a special note, 'R', to represent a rest
#define  R     0

boolean demo = true;

int melodyStart[] = {  C,  b,  g, C,  R};
//int melodyStart[] = {  c, d, e, f, g, a, b, C, R };
int beatsStart[]  = { 16, 16, 16, 8, 32};

int melodyStop[] = {  b,   e};
int beatsStop[]  = {  8,  16};

int melodyNo[] = {  c };
int beatsNo[]  = {  32};

int MAX_COUNT1 = sizeof(melodyStart)/2; // Melody length, for looping.
int MAX_COUNT2 = sizeof(melodyStop)/2; // Melody length, for looping.
int MAX_COUNT3 = sizeof(melodyNo)/2; // Melody length, for looping.

// Set overall tempo
long tempo = 10000;
// Set length of pause between notes
int pause = 1000;
// Loop variable to increase Rest length
int rest_count = 100; //<-BLETCHEROUS HACK; See NOTES

// Initialize core variables
int tono = 0;
int beat = 0;
long duration  = 0;


//SoftwareServo servoA;
//SoftwareServo servoB;

Servo servoA;
Servo servoB;

//Control Shift register
#define latchPin 13
#define clockPin 12
#define dataPin 11

//Salidas Shift register
#define IR 0  //Resistencia 56 Ohm
int idpins[4]  = { 5, 6, 7, 8 };
#define INPUT1 1
#define INPUT2 2
#define INPUT3 3
#define INPUT4 4
#define servo_on 5
#define camara_on 6
#define buzzer 7

#define min_level 702  //Nivel minimo de bateria
#define  max_level 984  //Nivel maximo de bateria

#define  MAX_HOR 180   //MAXIMO IZQUIERDA
#define  MED_HOR 90   //MEDIO
#define  MIN_HOR 0  //MAXIMO DERECHA

#define  MIN_VER 85  //MAXIMO ABAJO
#define  MED_VER 97  //MEDIO
#define  MAX_VER 120  //MAXIMO ARRIBA

float  distancia=150; //Distancia deteccion ultrasonido

//Entradas digitales
#define CTS 4

//Salidas digitales
#define speed1 10
#define speed2 9

//Entradas analogicas
#define vbat 3
#define sensor1 7
#define sensor2 0
#define sensor3 1
#define sensor4 2


int val_shift = 0x00; //Valor del shift register

float angulo = 0;
float angulo_ant = 0;
float angulo_act = 0;
float angulo_neg = 0;

byte val = 0x00; //Valor entrada serie
int n_bat = 0;

int Command_ok=0;
int mensaje_ok=0;

int modo=0;
byte dat = 0;
int command = 0;
int count = 0;

int frec = 0;
int activa = 0;
boolean flag_giro = false;

unsigned long time=0;
unsigned long time_correc=0;

int datd = 0;
int dati = 0;
int pos = 90;

int inc = 1;
int neg_sig=0;
int pos_sig=0;

int flag_avanza_stop=1;
int flag_avanza=1;

int IR1=0;

boolean camera_on = false;

boolean scan_mode = false;

#define time_robot_max 10000
unsigned long time_robot_count = 0;

// defines for setting and clearing register bits
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif

void setup()                    
{
  pinMode(latchPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(dataPin, OUTPUT);
  servoA.attach(2);
  servoB.attach(3);
  stop_motores();
  analogWrite(speed1, 0);
  analogWrite(speed2, 0);
  Wire.begin();
  Serial.begin(19200); 

  pinMode(CTS, INPUT); //Pin de control de flujo de datos

  //Lectura del identificador de la placa
  for (int i=0; i<4; i++) pinMode(idpins[i], INPUT);
    
  digital_shift(servo_on, LOW);
  servoA.write(MED_VER);  //Vertical
  servoB.write(MED_HOR); //Horizontal
  delay(15);
  
  if (id()<2) 
    {
      digital_shift(camara_on,  HIGH);
      camera_on = true;
    }
  else digital_shift(camara_on,  LOW);
  delay(4000);
  while(digitalRead(CTS));
  Serial.print("Soy el robot ");
  Serial.println(id());
  delay(1000);
  HMC6352.Wake();
  angulo_act = HMC6352.GetHeading();
  HMC6352.Sleep();
  angulo=0;
  time_correc=millis();
  pitidoPositivo();
  time_robot_count = millis();
  //if (demo) maximun_gyro();
}

float limite_min = 0;
float limite_max = 0;

void maximun_gyro()
  {
    float limite_izquierdo = 0;
    float limite_derecho = 0;
    giro_der(255,255);
    delay(1000);
    Serial.println("Calibrando derecha...");
    HMC6352.Wake();
    float angulo_int = HMC6352.GetHeading();
    float angulo_int_ant = 0;
    while (angulo_int!=angulo_int_ant)
      {
        angulo_int_ant = angulo_int;
        angulo_int = HMC6352.GetHeading();
        delay(100);
      }
    stop_motores();
    delay(1000);
    limite_derecho = angulo_int;
    Serial.println(limite_derecho);
    giro_iz(255,255);
    delay(1000);
    angulo_int = HMC6352.GetHeading();
    Serial.println("Calibrando izquierda...");
    while (angulo_int!=angulo_int_ant)
      {
        angulo_int_ant = angulo_int;
        angulo_int = HMC6352.GetHeading();
        delay(100);
      }
    stop_motores();
    delay(1000);
    limite_izquierdo = angulo_int;
    Serial.println(limite_izquierdo);
    if (limite_derecho>limite_izquierdo) {limite_max=limite_derecho; limite_min=limite_izquierdo;}
    else {limite_max=limite_izquierdo; limite_min=limite_derecho;}
    angulo = (limite_max - limite_min)/2 + limite_min;
    Serial.println("Centrando...");
    Serial.println(angulo);
    flag_giro=1;
    while (flag_giro==1) correccion_ang(); //Giro del robot
    HMC6352.Wake();
    angulo_act = HMC6352.GetHeading();
    HMC6352.Sleep();
    Serial.println(angulo_act);
    //HMC6352.Sleep();
  }
  
unsigned long time_scan = 0;

void loop()                      //run over and over again
{
  if (demo) scan();
  if (flag_giro==1) correccion_ang(); //Giro del robot
  if ((measure(sensor1)<=19)&&(flag_avanza_stop==1))//Para ULTRASONIDO
  {
    flag_avanza_stop=0;
    stop_motores();
    pitidoParada();
  }
  else if (analogRead(sensor1)>50) flag_avanza_stop=1;
  Serial_read();


}

void Serial_read()
  {
      if (Serial.available()) {
    val = Serial.read();
    if ((val&0xF0)==0xF0)
    {
      mensaje_ok=1;
      count=1;  //OTRO ID PUEDE RESETEAR EL ACTUAL...
      command=0x00;
      dat=0x00;
      modo=val;
    }
    else
    { 
      switch (modo) {
            case 0xF0: 
                if ((((val>>4)==id())||((val>>4)==0x0E))&&(count==1))
                 {
                   command=val&0x0F;
                   count=2; // SE INCREMENTA EL CONTADOR DE BYTES SOLO SI EL ID ES EL CORRECTO  
                 }
                else if ((((val>>4)==id())||((val>>4)==0x0E))&&(count==2))
                 {
                   dat = (val&0x0F)<<4;
                   count=3;    
                 }
                else if ((((val>>4)==id())||((val>>4)==0x0E))&&(count==3))
                 {
                   dat = dat|(val&0x0F);
                   count=4;
                   //Serial.print(0xF3, BYTE);
                 }     
             break;
             case 0xF2: 
                //if (((val>>4)==id())&&(count==1))
                if (count==1)
                { 
                  count=0; 
                  time_robot_count = millis();
                  //Serial.print(0xF4, BYTE);
                }
             break;
      }
    }
  } // ENDIF DEL AVAILABLESERIAL

  if ((count==4)&&(mensaje_ok==1)) // MENSAJE RECIBIDO, VAMOS A PROCESAR EL COMANDO
  {
    count=0;
    mensaje_ok = 0;
    //Serial.print(command, BYTE); 
    //Serial.print(dat, BYTE); 

    switch (command) {

    case 0x00: //Velocidad hacia delante
        if (flag_avanza_stop==1) avanza(dat);
      break;
    case 0x01: //Velocidad hacia atras
        retrocede(dat);
      break;
    case 0x02: //GIRO ROBOT ABSOLUTO
      {
        angulo_ant = angulo;
        angulo=map(dat, 0, 180, 0, 359);
        if ((angulo_ant==angulo)&&(flag_giro==0)) flag_giro=0;
        else flag_giro=1;
        //Serial.print(angulo);
        //Serial.print(angulo_ant);
      }
      break;
    case 0x03: //CAMARA ON/OFF
      {
          if (dat==0x01) 
            {
              digital_shift(camara_on,  HIGH);
              camera_on = true;
            }
          else if (dat==0x00)
            {
              digital_shift(camara_on,  LOW);
              camera_on = false;
            }
      }
      break;
    case 0x04://Calibracion Brujula
      {
        if (dat==0x01) 
        {
          HMC6352.startcal() ;
          giro_der(255,255);
          Serial.println("Calibrando...");
          delay(15000);
          HMC6352.stopcal() ;
          stop_motores();
          Serial.println("Fin de la calibracion.");
        }
        else if (dat==0x00) 
        {
          HMC6352.stopcal() ;
          stop_motores();
          Serial.println("Fin de la calibracion.");
        }
      }
      break;
    case 0x05: //ZUMBADOR
      {
        if (dat==0x00) pitidoParada();
        else if (dat==0x01) pitidoNegativo();
      }
      break;
    case 0x06: //IR
      {
        if (dat==0x01) digital_shift(IR,  HIGH);
        else digital_shift(IR,  LOW);  
        
        n_bat=analogRead(vbat);
        n_bat=map(n_bat,min_level,max_level,0,100);
        IR1=analogRead(sensor1);
        IR1=map(IR1,0,1023,0,255);
        HMC6352.Wake();
        angulo_act = HMC6352.GetHeading();
        HMC6352.Sleep();
        byte id_temp = id();
        while(digitalRead(CTS));
        Serial.write(0xF1);
        while(digitalRead(CTS));
        Serial.write(id_temp<<4|0x00);
        while(digitalRead(CTS));
        Serial.write(id_temp<<4|((int (angulo_act/2))>>4));
        while(digitalRead(CTS));
        Serial.write(id_temp<<4|((int (angulo_act/2))&0x0F));
        while(digitalRead(CTS));
        Serial.write((id_temp<<4)|(n_bat>>4));
        while(digitalRead(CTS));
        Serial.write((id_temp<<4)|(n_bat&0x0F));
        while(digitalRead(CTS));
        Serial.write((id_temp<<4)|(IR1>>4));
        while(digitalRead(CTS));
        Serial.write((id_temp<<4)|(IR1&0x0F));
      }
      break;
    case 0x07:  //GIRO ROBOT RELATIVO
      {
//        if (dat<=127) giro_iz(map(dat,0,127,0,255), map(dat,0,127,0,255));
//        else if (dat>=129) giro_der(map(dat,129,255,0,255), map(dat,129,255,0,255));
        if (dat<=127) giro_iz(255, 255);
        else if (dat>=129) giro_der(255, 255);
        else stop_motores();
      }
      break;
    case 0x08:  //Stop Motores
      {
        stop_motores();
      }
      break;
    case 0x09:  //Posicion Inicial Cuello
      {
        digital_shift(servo_on,  LOW);
        servoA.write(60);
        servoB.write(90);
        delay(15);
      }
      break;
    case 0x0A:  //Giro cuello vertical
      {
        digital_shift(servo_on,  LOW);
        //servoA.write(map(dat, 0, 180, 0, 114)+66);
        if (dat>180) dat=180;
        servoA.write(map(dat, 0, 180, MAX_VER, MIN_VER));
        delay(15);
      }
      break;
    case 0x0B:  //Giro cuello Horizontal
      {
        digital_shift(servo_on,  LOW);
        if (dat>180) dat=180;
        servoB.write(map(dat, 0, 180, MIN_HOR, MAX_HOR));
        //servoB.write(dat);
        delay(15);
      }
      break;
    case 0x0C:  //Apagar servos
      {
        digital_shift(servo_on,  HIGH);
      }
      break;
    case 0x0D:  //Apagar servos
      {
        scan_mode=true;
        time_scan = millis();
      }
      break;
    case 0x0E:  //Apagar servos
      {
        demo=false;
      }
      break;
    default:
    break;
    }
    //Comando de vuelta
  }
  }
  
void digital_shift(int pin,int state)
{
  switch (pin) {
  case 0:
    if (state==1) val_shift= B00000001|val_shift;
    else val_shift= B11111110&val_shift;
    break;
  case 1:
    if (state==1) val_shift= B00000010|val_shift;
    else val_shift= B11111101&val_shift;
    break;
  case 2:
    if (state==1) val_shift= B00000100|val_shift;
    else val_shift= B11111011&val_shift;
    break;
  case 3:
    if (state==1) val_shift= B00001000|val_shift;
    else val_shift= B11110111&val_shift;
    break;
  case 4:
    if (state==1) val_shift= B00010000|val_shift;
    else val_shift= B11101111&val_shift;
    break;
  case 5:
    if (state==1) val_shift= B00100000|val_shift;
    else val_shift= B11011111&val_shift;
    break;
  case 6:
    if (state==1) val_shift= B01000000|val_shift;
    else val_shift= B10111111&val_shift;
    break;
  case 7:
    if (state==1) val_shift= B10000000|val_shift;
    else val_shift= B01111111&val_shift;
    break;
  } 
  digitalWrite(latchPin, 0);
  shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
  digitalWrite(latchPin, 1);

}

void stop_motores()
{
  val_shift= B11100001&val_shift;
  digitalWrite(latchPin, 0);
  shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
  digitalWrite(latchPin, 1);
  analogWrite(speed1, 0);
  analogWrite(speed2, 0);
}

void giro_iz(int iz, int der) 
{ 
  val_shift= B11101101&val_shift;
  val_shift= B00001100|val_shift;
  digitalWrite(latchPin, 0);
  shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
  digitalWrite(latchPin, 1);
  analogWrite(speed1, iz);
  analogWrite(speed2, der);
}

void giro_der(int iz, int der) 
{ 
  val_shift= B1110011&val_shift;
  val_shift= B0010010|val_shift;
  digitalWrite(latchPin, 0);
  shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
  digitalWrite(latchPin, 1);
  analogWrite(speed1, iz);
  analogWrite(speed2, der);
}

void avanza(int av)
{ 
  //stop_motores();
  val_shift= B11101011&val_shift;
  val_shift= B00001010|val_shift;
  digitalWrite(latchPin, 0);
  shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
  digitalWrite(latchPin, 1);
  analogWrite(speed2, av);
  analogWrite(speed1, av);
} 

void retrocede(int re) 
{ 
  //stop_motores();
  val_shift= B11110101&val_shift;
  val_shift= B00010100|val_shift;
  digitalWrite(latchPin, 0);
  shiftOut(dataPin, clockPin, MSBFIRST, val_shift);
  digitalWrite(latchPin, 1);
  analogWrite(speed2, re);
  analogWrite(speed1, re);
} 

void correccion_ang( ) 
{ 
  #define res 3
  int sentido_giro=0;
  
  digital_shift(IR,  LOW);
  
  if ((millis()-time_correc)>=15)
  {
    angulo_act=0;
    time_correc=millis();
    HMC6352.Wake();
    angulo_act = HMC6352.GetHeading();
    HMC6352.Sleep();
    //Serial.println(angulo_act);
    if ((angulo<angulo_act)&&((angulo_act-angulo)>180)) angulo_neg=angulo+360;
    else angulo_neg=angulo;

    if (((abs(angulo_act-angulo)<abs(360-angulo+angulo_act))&&(angulo_act<=angulo))||((abs(360-angulo_act+angulo)<abs(angulo_act-angulo))&&(angulo_act>angulo))) sentido_giro=1;
    else sentido_giro=0;

    if (abs(angulo_act-angulo_neg)<=res)
    {
      flag_giro=0;
      stop_motores();
    }
    else 
    {
      if (sentido_giro==0) giro_iz(255,255);
      else giro_der(255,255);
      delay(20);
    }

  }
}

// PLAY TONE  ==============================================
// Pulse the speaker to play a tone for a particular duration
void playTone() {
  long elapsed_time = 0;
  if (tono > 0) { // if this isn't a Rest beat, while the tone has 
    //  played less long than 'duration', pulse speaker HIGH and LOW
    while (elapsed_time < duration) {

      digital_shift(buzzer,HIGH);
      delayMicroseconds(tono / 2);

      // DOWN
      digital_shift(buzzer,LOW);
      delayMicroseconds(tono / 2);

      // Keep track of how long we pulsed
      elapsed_time += (tono);
    } 
  }
  else { // Rest beat; loop times delay
    for (int j = 0; j < rest_count; j++) { // See NOTE on rest_count
      delayMicroseconds(duration);  
    }                                
  }                                 
}


void pitidoPositivo() {
  
   for (int i=0; i<MAX_COUNT1; i++) {
    tono = melodyStart[i];
    beat = beatsStart[i];
                
    duration = beat * tempo; // Set up timing
    playTone(); 
    // A pause between notes...
    delayMicroseconds(pause);
    }
}

void pitidoParada(){
            
  for (int i=0; i<MAX_COUNT2; i++) {
    tono = melodyStop[i];
    beat = beatsStop[i];
        
    duration = beat * tempo; // Set up timing
    playTone(); 
    // A pause between notes...
    delayMicroseconds(pause);
    }
           
}

void pitidoNegativo() {
  
  for (int i=0; i<MAX_COUNT3; i++) {
    tono = melodyNo[i];
    beat = beatsNo[i];
        
    duration = beat * tempo; // Set up timing
    playTone(); 
    // A pause between notes...
    delayMicroseconds(pause);
    }
}

int id()
  {
    int id = 0;
    for(int i=0; i<4; i++)
      for (byte i=0; i<4; i++) bitWrite(id, i, !digitalRead(idpins[i]));
    return id;
  }

#define RIGHT true
#define LEFT  false

boolean turn = RIGHT;
int angle = MIN_HOR;
int angle_ant = 0;

boolean message_send0 = true;
boolean message_send = true;

void scan()
  {
    
    if ((millis()-time_robot_count)> time_robot_max)
      {
        servoA.write(MED_VER);  //Vertical
        servoB.write(MED_HOR); //Horizontal
        delay(15);
        for (int i=0; i<100; i++)
          {
            Serial_read();
            delay(10);
          }
        digital_shift(servo_on, HIGH);
        float prox = measure(sensor1);
        if (prox <= distancia) time_robot_count = millis();
        if (id()==2) send_message(0xF0, 0x03, 0x00, 0x01);
        else if (id()==3) send_message(0xF0, 0x13, 0x10, 0x11);
      }
    else
    {
        digital_shift(servo_on, LOW);
        HMC6352.Wake();
        angulo_act = HMC6352.GetHeading();
        HMC6352.Sleep();
        float prox = measure(sensor1);
        if (message_send0)
                    {
                      message_send0 = false;
                      if (id()==2) send_message(0xF0, 0x03, 0x00, 0x01);
                      else if (id()==3) send_message(0xF0, 0x13, 0x10, 0x11);
                    }
        //Serial.println(prox);
        if (turn == RIGHT)
          {
            servoB.write(angle);  //Horizontal
            delay(15); 
            if (camera_on==false) digital_shift(camara_on,  LOW);
            else digital_shift(camara_on,  HIGH);
            while (prox <= distancia)
            {
              message_send0 = true;
              send_message();
              delay(500);
              time_robot_count = millis();
              prox = measure(sensor1);
              Serial_read();
              if (((id()<2)&&(camera_on))||(id()>=2)) 
                {
                  if (message_send)
                    {
                      message_send = false;
                      if (id()==2) send_message(0xF0, 0x03, 0x00, 0x00);
                      else if (id()==3) send_message(0xF0, 0x13, 0x10, 0x10);
                    }
                  digital_shift(camara_on,  HIGH);
                }
              delay(500);
              //if (angle!=MED_HOR) correccion_cuerpo();
            }
            message_send = true;
            angle++;
            if (angle>=MAX_HOR) turn = LEFT;
          }
        else if (turn == LEFT)
          {
            servoB.write(angle);  //Horizontal
            delay(15); 
            if (camera_on==false) digital_shift(camara_on,  LOW);
            else digital_shift(camara_on,  HIGH);
            while (prox <= distancia)
            {
              message_send0 = true;
              send_message();
              delay(500);
              time_robot_count = millis();
              prox = measure(sensor1);
              Serial_read();
               if (((id()<2)&&(camera_on))||(id()>=2)) 
                {
                   if (message_send)
                    {
                      message_send = false;
                      if (id()==2) send_message(0xF0, 0x03, 0x00, 0x00);
                      else if (id()==3) send_message(0xF0, 0x13, 0x10, 0x10);
                    }
                   digital_shift(camara_on,  HIGH);
                }
              delay(500);
              //if (angle!=MED_HOR) correccion_cuerpo();
            }
            message_send = true;
            angle--;
            if (angle<=MIN_HOR) turn = RIGHT;
          }
    }
      
  }

void send_message(byte val0, byte val1, byte val2, byte val3)
  {
    for(int i=0; i<3; i++)
      {
        while(digitalRead(CTS));
        Serial.write(val0);
        while(digitalRead(CTS));
        Serial.write(val1);
        while(digitalRead(CTS));
        Serial.write(val2);
        while(digitalRead(CTS));
        Serial.write(val3);
      }
  }

void send_message()
  {
    for(int i=0; i<3; i++)
      {
        while(digitalRead(CTS));
        Serial.write(0xF2);
        while(digitalRead(CTS));
        Serial.write(0x00);
      }
  }
              
              
boolean correccion_cuerpo()
  {
     int angle_turn = MED_HOR - angle;
     angulo = angulo_act - angle_turn;
     Serial.print(angulo);
     Serial.print(" ");
     if (angulo<0) angulo = 360 + angulo;
     if (angulo<limite_min) angulo = limite_min;
     else if (angulo>limite_max) angulo = limite_max;     
     Serial.println(angulo);
     digital_shift(camara_on,  HIGH);//Para ULTRASONIDO
     flag_giro = true;
     HMC6352.Wake();
     angulo_act = HMC6352.GetHeading();
     HMC6352.Sleep();
     if (angulo!=angulo_act)
      {
         angle = MED_HOR;
         servoB.write(angle);  //Horizontal
         delay(15); 
         while (flag_giro==1) 
          {
            correccion_ang(); //Giro del robot
          }
      }
  }
  
float num = 10;
float measure(int sensor)
  {
     float value = 0;
     for (int i=0; i<num; i++) value = value + analogRead(sensor);
     value= value/num; 
     float voltage = value*(5000/1023.);
     float cm = voltage*2.54/9.8;
     return cm; 
  }