#include <wiringPi.h>
#include <math.h>

#include <libgy521.h>

//Constants

#define MPU6050_GYRO_XOUT_H 0x43 //R
#define MPU6050_GYRO_YOUT_H 0x45 //R
#define MPU6050_GYRO_ZOUT_H 0x47 //R

#define MPU6050_ACCEL_XOUT_H 0x3B //R
#define MPU6050_ACCEL_YOUT_H 0x3D //R
#define MPU6050_ACCEL_ZOUT_H 0x3F //R

#define MPU6050_PWR_MGMT_1 0x6B //R/W
#define MPU6050_I2C_ADDRESS 0x68 //I2C

int id;

int setupgy521(void) {
    if (id = wiringPiI2CSetup(MPU6050_I2C_ADDRESS) != -1) {
        wiringPiI2CReadReg8(id, MPU6050_PWR_MGMT_1);
        wiringPiI2CWriteReg16(id, MPU6050_PWR_MGMT_1, 0);
        return EXIT_SUCCESS;
    } else {
        return EXIT_FAILURE
    }
}

int getGyroX(void) {
    return wiringPiI2CReadReg8(id, MPU6050_GYRO_XOUT_H);
}

int getGyroY(void) {
    return wiringPiI2CReadReg8(id, MPU6050_GYRO_YOUT_H);
}

int getGyroZ(void) {
    return wiringPiI2CReadReg8(id, MPU6050_GYRO_ZOUT_H);
}

int getAccelX(void) {
    return wiringPiI2CReadReg8(id, MPU6050_ACCEL_XOUT_H);
}

int getAccelY(void) {
    return wiringPiI2CReadReg8(id, MPU6050_ACCEL_YOUT_H);
}

int getAccelZ(void) {
    return wiringPiI2CReadReg8(id, MPU6050_ACCEL_ZOUT_H);
}

float getAngleX(void) {
    int x = getAccelX();
    int y = getAccelY();
    int z = getAccelZ();
    return atan(x / (sqrt(pow(y, 2) + pow(z, 2)))) * 180.0 / M_PI;
}

float getAngleY(void) {
    int x = getAccelX();
    int y = getAccelY();
    int z = getAccelZ();
    return atan(y / (sqrt(pow(x, 2) + pow(z, 2)))) * 180.0 / M_PI;
}

float getAngleZ(void) {
    int x = getAccelX();
    int y = getAccelY();
    int z = getAccelZ();
    return atan((sqrt(po(y, 2) + pow(x, 2))) / z) * 180.0 / M_PI;
}