#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
const int MPU2 = 0x69, MPU1=0x68;
#include <SPI.h>                      //the communication interface with the modem
#include "RF24.h"

long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ, gyroX, gyroY, gyroZ,rotX, rotY, rotZ;
long accelX2, accelY2, accelZ2;
float gForceX2, gForceY2, gForceZ2;

#define ESPERA_INICIO 10
#define ESPERA_FINAL 10

#define MAX_ACE 100
#define MIN_ACE 30

unsigned long tiempoAnterior = 0;
unsigned long etapa = 1;

bool tocando = 0;

// La dirección del MPU6050 puede ser 0x68 o 0x69, dependiendo
// del estado de AD0. Si no se especifica, 0x68 estará implicito
MPU6050 sensor;

// Valores RAW (sin procesar) del acelerometro y giroscopio en los ejes x,y,z
int ax, ay, az;
int gx, gy, gz;

RF24 radio(7,8);                     //5 and 10 are a digital pin numbers to which signals CE and CSN are connected.

const uint64_t pipe = 0xE8E8F0F0E1LL;

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU1);
  Wire.write(0x6B);
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(MPU1);
  Wire.write(0x1B);
  Wire.write(0x00000000);
  Wire.endTransmission();
  Wire.beginTransmission(MPU1);
  Wire.write(0x1C);
  Wire.write(0b00000000);
  Wire.endTransmission();

  Wire.begin();
  Wire.beginTransmission(MPU2);
  Wire.write(0x6B);
  Wire.write(0b00000000);
  Wire.endTransmission();
  Wire.beginTransmission(MPU2);
  Wire.write(0x1B);
  Wire.write(0x00000000);
  Wire.endTransmission();
  Wire.beginTransmission(MPU2);
  Wire.write(0x1C);
  Wire.write(0b00000000);
  Wire.endTransmission();

  sensor.initialize();    //Iniciando el sensor
  Serial.begin(9600);
  radio.begin();                      //it activates the modem.
  radio.openWritingPipe(pipe);
}

void loop() {

  GetMpuValue(MPU1);
  //Serial.print("\t ||| \t");
  GetMpuValue(MPU2);
  //Serial.println("");

  int x,y,z;
  int xCode, yCode, zCode;
  unsigned long diferencia = 0;
  int mensaje = 1;

   x = abs(x);
   y = abs(y);
   z = abs(z);


  switch (etapa)
  {
    case 1:
      if ((x > MAX_ACE) or (y > MAX_ACE) or (z > MAX_ACE))
      {
        tiempoAnterior = millis();
        radio.write(&etapa, sizeof(etapa));
        etapa++;
      }

      break;

    case 2:
      diferencia = millis() - tiempoAnterior;
      if (diferencia > ESPERA_INICIO)
      {
         if ((x > MAX_ACE) or (y > MAX_ACE) or (z > MAX_ACE))
         {
            //confirma que hay un movimiento, y pasa INMEDIATAMENTE a la siguiente etapa
            radio.write(&etapa, sizeof(etapa));

            tiempoAnterior = millis();
            etapa++;
         }
         else
         {
            etapa = 1;
         }
      }

      break;

    case 3: //etapa en la cual enviamos x, y, z
      if ((x < MIN_ACE) && (y < MIN_ACE) && (z < MIN_ACE))
      {
        tiempoAnterior = millis();
        radio.write(&etapa, sizeof(etapa));
        etapa++;
      }
         xCode = x + 1000;
         yCode = y + 2000;
         zCode = z + 3000;
        radio.write(&xCode, sizeof(xCode));
         radio.write(&yCode, sizeof(yCode));
          radio.write(&zCode, sizeof(zCode));
          //Serial.println(yCode);

      break;

    case 4:
      diferencia = millis() - tiempoAnterior;
      if (diferencia > ESPERA_FINAL)
      {
         if ((x < MIN_ACE) && (z < MIN_ACE))
         {
            //confirmar el positivo y negativo
            mensaje = 1;
            radio.write(&etapa, sizeof(etapa));
            etapa = 1;
         }
         else
         {
            etapa = 3;
         }
      }
      break;
  }
}



void GetMpuValue(const int MPU){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(MPU,6);
  while(Wire.available() < 6);
  accelX = Wire.read()<<8|Wire.read();
  accelY = Wire.read()<<8|Wire.read();
  accelZ = Wire.read()<<8|Wire.read();

  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(MPU,6);
  while(Wire.available() < 6);
  gyroX = Wire.read()<<8|Wire.read();
  gyroY = Wire.read()<<8|Wire.read();
  gyroZ = Wire.read()<<8|Wire.read();


  gForceX = accelX / 16384.0;
  gForceY = accelY / 16384.0;
  gForceZ = accelZ / 16384.0;
  rotX = gyroX / 131.0;
  rotY = gyroY / 131.0;
  rotZ = gyroZ / 131.0;
  //Serial.print("gyro\t");
  Serial.print(rotX);
  /*Serial.print("\t");
  Serial.print(rotY);
  Serial.print("\t");
  Serial.print(rotZ);
  Serial.print("\tAcc\t");*/
  Serial.print(gForceX);
 /* Serial.print("\t");
  Serial.print(gForceY);
  Serial.print("\t");
  Serial.print(gForceZ);*/
  delay(100);
}