// Bbot MPU6050 driver

#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyro;  // MPU-6050 object

int16_t ax, ay, az;  // Accel readings
int16_t gx, gy, gz;  // Gyro readings

double arctan2(int y, int x);

void initSensors()
{                                       // Set zero sensor values
	Wire.begin();

	// initialize device
	Serial.println("Initializing MPU-6050...");
	accelgyro.initialize();
//	accelgyro.setDLPFMode(MPU6050_DLPF_BW_256); // Set filter mode to off
	accelgyro.setRate(40); // Set sample rate to 200Hz (8KHz/40)
//	accelgyro.setDLPFMode(MPU6050_DLPF_BW_5); // Set filter mode to off
//	accelgyro.setRate(5); // Set sample rate to 200Hz (1KHz/5)
}

// data acquisition
void updateSensors()
{
	// Get raw accel/gyro measurements from device
	accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
}

// Gyro rate in degs/second
double getGyroRate()
{
	// Gyro returns full scale of 32766 = 250 deg/sec
	return ((double)gy / 131.064); // In deg/sec
}

// Accel angle in degs
double getAccAngle() {                      
  return (arctan2(-az, -ax) + 90.0);  // In deg, rotated 90deg
}

// http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
// Returns the angle in degs
double arctan2(int y, int x)
{
	float coeff_1 = 0.7853975; // Pi/4
	float coeff_2 = 2.3561925; // 3*coeff_1
	float abs_y = abs(y)+1e-10;  // prevent 0/0 condition
	float r, angle;

	if (x >= 0)
	{
		r = (x - abs_y) / (x + abs_y);
		angle = coeff_1 - coeff_1 * r;
	}
	else
	{
		r = (x + abs_y) / (abs_y - x);
		angle = coeff_2 - coeff_1 * r;
	}

	// Convert radians to degs
	angle = angle * 57.295827; // angle * 180 / pi

	if (y < 0)
		return(-angle);  // negate if in quad III or IV
	else
		return(angle);
}

