#include <Servo.h>
#undef int
#undef abs
#undef double
#undef float
#undef round

Servo servoA, servoB, servoC;

int IR = 8; 
int zumbador =11;
int camara = 4;
int girocam = 2;
int bat = 4;
int bit1 = 14;
int bit2 = 15;
int bit3 = 16;
int bit4 = 17;
int led =3;
int laser =5;
int act_serv = 7;
int speed1 = 9;
int speed2 = 10;

int id = 0;
int val1 = 0;
int val2 = 0;
int val3 = 0;
int val4 = 0;
byte val = 0;
unsigned long time=0;
unsigned long time_ant=0;
int frec = 0;
int activa=0;
int Command_ok=0;
int mensaje_ok=0;
int dat = 0;
int datd = 0;
int dati = 0;
int command = 0;
int count = 0;

void izquierda(int iz) 
{ 
  servoA.write(iz);
}

void derecha(int der) 
{ 
  servoB.write(der);
}

void setup() 
{ 
  Serial.begin(19200); 
  delay(100); 

  /*Serial.print("ER_CMD#U5");
   delay(200); 
   Serial.print("ACK");
   delay(200); 
   
   Serial.begin(38400);*/

  analogWrite(zumbador,0);

  servoA.attach(speed1);
  servoB.attach(speed2);
  servoC.attach(girocam);

  digitalWrite(act_serv,  HIGH); //Servos apagados

  servoC.write(93); //Parado

  pinMode(IR, OUTPUT);
  pinMode(zumbador, OUTPUT);
  pinMode(camara, OUTPUT);
  pinMode(14, INPUT);
  pinMode(15, INPUT);
  pinMode(16, INPUT);
  pinMode(17, INPUT);
  digitalWrite(14, HIGH);
  digitalWrite(15, HIGH);
  digitalWrite(16, HIGH);
  digitalWrite(17, HIGH);
  if (digitalRead(bit1)) val1=1;
  else val1=0;
  if (digitalRead(bit2)) val2=2;
  else val2=0;
  if (digitalRead(bit3)) val3=4;
  else val3=0;
  if (digitalRead(bit4)) val4=8;
  else val4=0;
  id=val1+val2+val3+val4;
  Serial.print("Hola, soy el robot numero ");
  Serial.println(id);
  digitalWrite(camara,  LOW); //Desactiva camara
  digitalWrite(act_serv,  HIGH); //Desactiva servomotores
  digitalWrite(led,  LOW); //Apaga leds
  digitalWrite(laser,  LOW); //Apaga laser

  //Serial.print(id<<4|0x00, BYTE);
  //Serial.print(id<<4|analogRead(bat)/68, BYTE);

} 

void loop() 
{ 
  Servo::refresh();  
  if (Serial.available()) {

    val = Serial.read();

    if (val==0xF0) 
    {
      mensaje_ok=1;
      count=1;
    }
    else if (mensaje_ok==1) count++;


    if (count==2) 
    {
      if ((val>>4)==id) command=val&0x0F;
      else 
      {
        mensaje_ok=0;
        count=0;
      }
    }
    else if (count==3) 
    {
      if ((val>>4)==id) dat = val&0x0F;
      else 
      {
        mensaje_ok=0;
        count=0;
      }
    }
    //else if (count>3) Serial.print(0xF1, BYTE); //
  }

  if ((count==3)&&(mensaje_ok==1))
  {
    count=0;
    mensaje_ok=0;

    //map(pos, 0,180,5,200);
    delay(10);

    switch (command) {
    case 0x00: //Servo derecho
      switch (dat) {
      case 0x00:
        datd=180;
        break;
      case 0x01:
        datd=100;
        break;
      case 0x02:
        datd=90;
        break;
      case 0x03:
        datd=80;
        break;
      case 0x04:
        datd=70;
        break;
      case 0x05:
        datd=60; //
        break;
      case 0x06:
        datd=50;
        break;
      case 0x07:
        datd=38;
        break;
      case 0x08:
        datd=38;
        break;
      case 0x09:
        datd=25;
        break;
      case 0x0A:
        datd=15;
        break;
      case 0x0B:
        datd=10;
        break;
      case 0x0C:
        datd=8;
        break;
      case 0x0D:
        datd=6;
        break;
      case 0x0E:
        datd=3;
        break;
      case 0x0F:
        datd=0;
        break;
      }
      //if (dat==7) digitalWrite(act_serv,  HIGH);
      digitalWrite(act_serv,  LOW);
      derecha(datd);
      break;
    case 0x01: //Servo izquierdo
      switch (dat) {
      case 0x00:
        dati=0;
        break;
      case 0x01:
        dati=3;
        break;
      case 0x02:
        dati=6;
        break;
      case 0x03:
        dati=8;
        break;
      case 0x04:
        dati=10;
        break;
      case 0x05:
        dati=15;
        break;
      case 0x06:
        dati=25;
        break;
      case 0x07:
        dati=38;
        break;
      case 0x08:
        dati=38;
        break;
      case 0x09:
        dati=50;
        break;
      case 0x0A:
        dati=60;
        break;
      case 0x0B:
        dati=70;
        break;
      case 0x0C:
        dati=80;
        break;
      case 0x0D:
        dati=90;
        break;
      case 0x0E:
        dati=100;
        break;
      case 0x0F:
        dati=180;
        break;
      }
      //if (dat==7) digitalWrite(act_serv,  HIGH);
      digitalWrite(act_serv,  LOW);
      izquierda(dati);
      break;
    case 0x02: //CAMARA ON/OFF
      {
        if (dat==0x01) digitalWrite(camara,  HIGH);
        else digitalWrite(camara,  LOW);
      }
      break;
    case 0x03: //GIRO CAMARA
      {
        digitalWrite(act_serv,  LOW);
        servoC.write(11*dat);
      }
      break;
    case 0x04://LASER ON/OFF
      {
        if (dat==0x01) digitalWrite(laser,  HIGH);
        else digitalWrite(laser,  LOW);
      }
      break;
    case 0x05: //ZUMBADOR
      {
        if (dat==0x01)
        {
          frec= 100;
          activa=1;
          time_ant=millis();
        }
        else
        {
          activa=0;
          analogWrite(zumbador,0);
        }
      }
      break;
    case 0x06: //IR
      {
        if (dat==0x01) digitalWrite(IR,  HIGH);
        else digitalWrite(IR,  LOW);
      }
      break;
    case 0x07:  //LEDS BLANCOS
      {
        if (dat==0x01) digitalWrite(led,  HIGH); //Enciende leds
        else digitalWrite(led,  LOW);
      }
      break;
    case 0x08:  //
      {
        //if (dat==0x01) digitalWrite(act_serv,  HIGH); //Apaga servo
      }
      break;
    case 0x09:  //comando off servos
      {
        digitalWrite(act_serv,  HIGH); //STOP SERVO
      }
      break;
    }
    //Comando de vuelta
    //Serial.print(id<<4|command, BYTE);
    //Serial.print(id<<4|analogRead(bat)/68, BYTE);
  }
  time = millis();
  if (activa==1)
  {
    if (time-time_ant >= frec)
    {
      analogWrite(zumbador,255);
      if (time-time_ant >= 2*frec) time_ant=time;
    }
    else analogWrite(zumbador,0);
  }
  
  
}