/*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

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
#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

//Entradas digitales
#define CTS 4
#define bit0 5
#define bit1 6
#define bit2 7
#define bit3 8

//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

int mac = 0; //Identificador de la placa

int id=0;

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;
int flag_giro=1;

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;

// 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()                    
{
//  // set prescale to 16
//  sbi(ADCSRA,ADPS2) ;
//  cbi(ADCSRA,ADPS1) ;
//  cbi(ADCSRA,ADPS0) ;
  pinMode(latchPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(dataPin, OUTPUT);
  stop_motores();

  servoA.attach(2);
  //  servoA.writeMicroseconds(1500);
  servoB.attach(3);
  //  servoB.writeMicroseconds(1500);


  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
  pinMode(bit0, INPUT);
  pinMode(bit1, INPUT);
  pinMode(bit2, INPUT);
  pinMode(bit3, INPUT);
  
  if (digitalRead(bit0)) mac=1;
  if (digitalRead(bit1)) mac=mac+2;
  if (digitalRead(bit2)) mac=mac+4;
  if (digitalRead(bit3)) mac=mac+8;
   
  id=mac;
  
  digital_shift(servo_on,LOW);
  servoA.write(MED_VER);  //Vertical
  servoB.write(MED_HOR); //Horizontal
  delay(15);
  
  digital_shift(camara_on,  LOW);
  Serial.print("Soy el robot ");
  Serial.println(mac);
  delay(1000);
  HMC6352.Wake();
  angulo_act = HMC6352.GetHeading();
  HMC6352.Sleep();
  angulo=0;
  time_correc=millis();
  pitidoPositivo();
}

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

  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)&&(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)&&(count==2))
                 {
                   dat = (val&0x0F)<<4;
                   count=3;    
                 }
                else if (((val>>4)==id)&&(count==3))
                 {
                   dat = dat|(val&0x0F);
                   count=4;
                   //Serial.print(0xF3, BYTE);
                 }     
             break;
             case 0xF2: 
                if (((val>>4)==mac)&&(count==1))
                { 
                  id=val&0x0F;
                  count=0; 
                  //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);
          else if (dat==0x00) digital_shift(camara_on,  LOW);
      }
      break;
    case 0x04://Calibracion Brujula
      {
        if (dat==0x01) 
        {
          //HMC6352.startcal() ;
          giro_der(200,200);
        }
        else if (dat==0x00) 
        {
          //HMC6352.stopcal() ;
          stop_motores();
        }
      }
      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();
        
        while(digitalRead(CTS));
        Serial.write(0xF1);
        while(digitalRead(CTS));
        Serial.write(id<<4|0x00);
        while(digitalRead(CTS));
        Serial.write(id<<4|((int (angulo_act/2))>>4));
        while(digitalRead(CTS));
        Serial.write(id<<4|((int (angulo_act/2))&0x0F));
        while(digitalRead(CTS));
        Serial.write((id<<4)|(n_bat>>4));
        while(digitalRead(CTS));
        Serial.write((id<<4)|(n_bat&0x0F));
        while(digitalRead(CTS));
        Serial.write((id<<4)|(IR1>>4));
        while(digitalRead(CTS));
        Serial.write((id<<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));
        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;
    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);
    }
}