//#include <wiringPi.h>
#include <wiringPiI2C.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "libgy521.h"

//Constants

//######## Obsolete ########//

#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

//######## End Obsolete ########//

#define GRAVITIY_MS2                9.80665

// Scale Modifiers
#define ACCEL_SCALE_MODIFIER_2G     16384.0
#define ACCEL_SCALE_MODIFIER_4G     8192.0
#define ACCEL_SCALE_MODIFIER_8G     4096.0
#define ACCEL_SCALE_MODIFIER_16G    2048.0

#define GYRO_SCALE_MODIFIER_250DEG  131.0
#define GYRO_SCALE_MODIFIER_500DEG  65.5
#define GYRO_SCALE_MODIFIER_1000DEG 32.8
#define GYRO_SCALE_MODIFIER_2000DEG 16.4

// Pre-defined ranges
#define ACCEL_RANGE_2G              0x00
#define ACCEL_RANGE_4G              0x08
#define ACCEL_RANGE_8G              0x10
#define ACCEL_RANGE_16G             0x18

#define GYRO_RANGE_250DEG           0x00
#define GYRO_RANGE_500DEG           0x08
#define GYRO_RANGE_1000DEG          0x10
#define GYRO_RANGE_2000DEG          0x18

// MPU-6050 Registers
#define PWR_MGMT_1                  0x6B //R/W
#define PWR_MGMT_2                  0x6C

#define SELF_TEST_X                 0x0D
#define SELF_TEST_Y                 0x0E
#define SELF_TEST_Z                 0x0F
#define SELF_TEST_A                 0x10

#define ACCEL_XOUT0                 0x3B
#define ACCEL_XOUT1                 0x3C
#define ACCEL_YOUT0                 0x3D
#define ACCEL_YOUT1                 0x3E
#define ACCEL_ZOUT0                 0x3F
#define ACCEL_ZOUT1                 0x40

#define TEMP_OUT0                   0x41
#define TEMP_OUT1                   0x42

#define GYRO_XOUT0                  0x43 //R
#define GYRO_XOUT1                  0x44
#define GYRO_YOUT0                  0x45 //R
#define GYRO_YOUT1                  0x46
#define GYRO_ZOUT0                  0x47 //R
#define GYRO_ZOUT1                  0x48

#define ACCEL_CONFIG                0x1C
#define GYRO_CONFIG                 0x1B

int id;

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

int read_i2c_word(int reg) {
    return wiringPiI2CReadReg8(id, reg);
}

int read_i2c_word16(int reg) {
    return wiringPiI2CReadReg16(id, reg);
}

float getTemp(void) {
    /**
     * Formula given in
     * MPU-6050 Register Map and Descriptions revision 4.2, page 30
     */
    return (read_i2c_word16(TEMP_OUT0) / 340.0) + 36.53; //16 might be bullshit
}

void setAccelerometerRange(int range) {
    //First set data to 0x00
    wiringPiI2CWriteReg8(id, ACCEL_CONFIG, 0x00);
    //Then write the correct value to the register
    wiringPiI2CWriteReg8(id, ACCEL_CONFIG, range);
}

int readAccelerometerRange(int raw) {
    int rawData = wiringPiI2CReadReg8(id, ACCEL_CONFIG);
    int range;
    if (raw != 0) {
        range = rawData;
    } else {
        switch (rawData) {
            case ACCEL_RANGE_2G:
                range = 2;
                break;
            case ACCEL_RANGE_4G:
                range = 4;
                break;
            case ACCEL_RANGE_8G:
                range = 8;
                break;
            case ACCEL_RANGE_16G:
                range = 16;
                break;
            default:
                range = -1; //Error
                break; //Good manner ;)
        }
    }
    
    return range;
}

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(pow(y, 2) + pow(x, 2))) / z) * 180.0 / M_PI;
}