#include <Wire.h>// Incluye la biblioteca del I2C
#include <PixyI2C.h>// Incluye la biblioteca de la camara
#include <Servo.h>// Incluye la biblioteca para el control de los servos
//envio de datos de la camara
PixyI2C pixy;//objeto principal de pixy
int inicio = 500;//inicio de trama
int final = 300;//final de trama
String s = "s";
String trama;//trama a enviar
int x, y, signature;//datos de los objetos detectados en los que se et interesado
uint16_t blocks;// numero de objetos detectados
//servos y electroiman
Servo servo1,servo2,servo3;//objetos de servo
int led = 13;//pin del electroiman
int button = 2;//botn para debug
int button_state = LOW;
//misc
int i, j=0, k;
//recepcion de tramas
int StartStop,servo,Degrees,sign;// datos de la trama que viene desde el PC
char lectura2,l2,l3;
static char lectura[20],CStartStop[20],CServo[20],CSign[20],CDegrees[20];


void setup(){
  pixy.init();//Inicializar la camara
  pinMode (led, OUTPUT);// poner el pin del electroiman en modo salida
  Serial.begin(115200);//Inicializar la comunicacin serie a 115200 baudios
  servo1.attach(8);//asignar el servo 1 a al pin 8
  servo2.attach(10);//asignar el servo 2 a al pin 10
  servo3.attach(11);//asignar el servo 3 a al pin 11
}

void loop(){
  if (Serial.available() > 10){// si se ha enviado algo desde el PC
    for (i=0;i<12;i++){//leelo
      lectura2 = Serial.read();
      lectura[i] = lectura2;// y guardalo en la variable lectura
    }
  }
  /////Start Stop////////////
  CStartStop[0]=lectura[0];
  StartStop = atoi(CStartStop);
  ///// N Servo/////////////
  CServo[0]=lectura[2];
  servo= atoi(CServo);
  ///// PositiveNeganitve////
  CSign[0]=lectura[4];
  sign=atoi(CSign);
  ///// Grados//////////////
  for(i=6;i<9;i++){
    CDegrees[i-6]=lectura[i];
  }
  Degrees=atoi(CDegrees);
  if (sign == 1){
    Degrees = -Degrees;
  }

  //solo para debug
  button_state = digitalRead(button);
  if (button_state == HIGH){
    Serial.print(StartStop); Serial.print(" "); Serial.println(servo);
  }

  switch (servo){// mueve los servos a la posicion correspondiente
    case 1: servo1.write(90-2-Degrees); break;//la posicin es 90 - factor de correccin de error de los servos - grados enviados desde el PC
    case 2: servo2.write(90-2-Degrees); break;
    case 3: servo3.write(90-2-Degrees); break;
  }

  if (StartStop == 1){//enciende o apaga el electroman
    digitalWrite(led, HIGH);
  }
  else {digitalWrite(led, LOW);}

  //leer de la camara
  blocks = pixy.getBlocks();//guarda el numero de objetos detectados en la bariable blocks
  if (blocks){// si se ha detectado algo
    j++;
    //como la camara envia 50 fps, para no saturar el arduino se hace que procese los datos una de cada 10 veces (cada 200 ms)
    if (j%10==0){
      //sacar los datos de las imagenes
      for (k=0; k<blocks; k++){//lo hace para cada uno de los objetos detectados
        signature = pixy.blocks[k].signature;//guarda en la variable signature la asignacin del objeto detectado (puede ir de 1 a 7)
        x = map(pixy.blocks[k].x, 0, 300, 0, 100);//coje la posicin x del objeto y lo pasa de un rango de 0 a 300 a un rango de 0 a 100
        y = map(pixy.blocks[k].y, 0, 200, 100, 0);//coje la posicin y del objeto y lo pasa de un rango de 0 a 200 a un rango de 0 a 100
        trama = s + " " + inicio + " " + signature + " " + x +     " " + y + " " + final;// construye una trama con la siguiente estructura: s inicio_de_trama asignacion_del_objeto posx posy final_de_trama
        //el inicio y final de trama estan puestos para evitar datos erroneos en la recepcion del PC
        Serial.println(trama);// envia la trama por serie al PC
      }
    }
  }
}
