/*
 * HybridPlay sensor firmware 1.0.1 | 04/2016
 * ------------------------- 
 *  
 * Read HybridPlay sensors and
 * send the data via serial/bluetooth
 *
 * rev 09:
 * - Merging everything (RFID, IR, GYROSCOPE with QUATERNIONS, RGB LED, SPEAKER ---> BTLE RN4020)
 *
 * PINs Mapping:
 * 
 *  pin 11 digital --> speaker
 *  pin 6  digital PWM --> power LED R
 *  pin 10 digital PWM --> power LED G
 *  pin 5  digital PWM --> power LED B
 *  pin 5  digital PWM --> battery
 *
 * (cc) 2016 Lalalab | Emanuele Mazza aka n3m3da
 * http://www.lalalab.org
 * http://www.hybridplay.com
 * http://www.d3cod3.org
 */

// External Libraries
#include <Wire.h>

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

// DEBUG vars
boolean debugMode = true;

////////////////////////////////////////////////
// Hardware interface
#define speaker     11
#define ledRed      6
#define ledGreen    10
#define ledBlue     5
#define batteryPin  5

#define CMD         4 // Command input BLE RN4020

long  timer = 0;   //general purpuse timer
long  timerBattery = 0;
long  timerBT = 0;
int   batteryCapacity = 1000; // mAh

int   powerLedBrightness = 0;
int   powerLedFade = 1;
float powerLedFadeSpeed = 4.0; // min. 1.0, max. 20.0
int   powerLedFadeRange = (int)round(10000/powerLedFadeSpeed);
bool  ledR = false;
bool  ledG = false;
bool  ledB = false;
////////////////////////////////////////////////

////////////////////////////////////////////////
// MPU6050 GYROSCOPE/ACCELEROMETER
#define EPS 1.1920928955078125E-7f

MPU6050 mpu;

bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}

int16_t ax, ay, az;
int16_t gx, gy, gz;
uint8_t teapotPacket[8] = { 0,0,0,0,0,0,0,0 };
uint8_t prevteapotPacket[8] = { 0,0,0,0,0,0,0,0 };
uint8_t teapotPacketTest[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; // for standard serial testing
////////////////////////////////////////////////

////////////////////////////////////////////////
// GP2Y0E02B IR proximity sensor Variables
#define IR_ADDRESS       0x80 >> 1  // Arduino uses 7 bit addressing so we shift address right one bit
#define IR_DISTANCE_REG  0x5E
#define IR_SHIFT         0x35

int ir_distance = 0;                // Stores the calculated distance 
byte ir_high,ir_low = 0;            // High and low byte of distance
int ir_shift = 0;                   // Value in shift bit register
////////////////////////////////////////////////

////////////////////////////////////////////////
// Serial packet
#define SERIAL_BAUD_VEL 38400
////////////////////////////////////////////////

////////////////////////////////////////////////
// 2D DIRECTIONS

////////////////////////////////////////////////

////////////////////////////////////////////////
// TILT SENSOR
unsigned long last_read_time;
int16_t    last_x_angle;
int16_t    last_y_angle;
int16_t    last_z_angle;
int16_t    last_gyro_x_angle;
int16_t    last_gyro_y_angle;
int16_t    last_gyro_z_angle;
int16_t    base_x_accel;
int16_t    base_y_accel;
int16_t    base_z_accel;
int16_t    base_x_gyro;
int16_t    base_y_gyro;
int16_t    base_z_gyro;

int tiltReading = 0;
int tolerance = 10; // Sensitivity of the Alarm
boolean calibrated = false; // When accelerometer is calibrated - changes to true
boolean moveDetected = false; // When motion is detected - changes to true
boolean prevMoveDetected = false;
unsigned long timeTilt;
int tiltWait = 200;
int xMin; //Minimum x Value
int xMax; //Maximum x Value
int xVal; //Current x Value

int yMin; //Minimum y Value
int yMax; //Maximum y Value
int yVal; //Current y Value

int zMin; //Minimum z Value
int zMax; //Maximum z Value
int zVal; //Current z Value
////////////////////////////////////////////////

// IR SENSOR
int valIR = 0;
int prevValIR = -1;
// BATTERY
int valBattery = 0;
int prevValBattery = -1;

void setup() {

  // Serial
  initSerial();

  // BTLE RN4020
  pinMode(CMD,OUTPUT);
  initBTLERN4020();

  // MPU6050 & GP2Y0E02B IR Sensors Init
  initSensor();
  
  // RGB Power Led Mode
  pinMode(ledGreen,OUTPUT);
  pinMode(ledRed,  OUTPUT);
  pinMode(ledBlue, OUTPUT);

  // RGB Power LED
  ledYELLOW();

  // Battery
  readBattery();
  
  // Time
  timer = millis();
  timerBattery = millis();
  timerBT = millis();
  delay(20);
}

void loop() {  
  if((millis()-timer) >= 40 && dmpReady){  // Main loop runs at 25Hz (same as 25 FPS)

    // MPU6050
    updateMPU6050();

    // IR
    updateIR();

    // TILT
    updateTILT();

    // data send via BLUETOOTH (Serial1)
    updateBTServices();

    // print on usb (Serial)
    if(debugMode){
      receiveBluetoothSerial();
    }
    
  }
}