#define _GNU_SOURCE
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include "mpu6050.h"
#include "registers.h"
#include "i2c.h"

int main() {

    uint8_t id;
    uint8_t gyro, acc;
    uint8_t int_status = 0;
    uint8_t data[6];
    int16_t gx, gy, gz;
    int16_t ax, ay, az;
    float lsb_conv_gyro;
    float lsb_conv_acc;
    int err = 0;
    
    if (i2c_init()) {
        exit(1);
    }

    if (i2c_read(REG_WHO_AM_I, &id, 1)) {
        exit(1);
    }

    if (id == 0x68) {
        puts("Found MPU-6050");
    }

    /* reset all signal paths */
    /* enable gyro, acc and temp */
    if (i2c_write(REG_SIGNAL_PATH_RESET, 0x07)) {
        exit(1);
    }

    /* set sample rate divisor to 150 */
    /* if LP-filter = 0 or 7 => sample rate = 8 Khz, otherwise 1 Khz */
    if (i2c_write(REG_SMPLRT_DIV, 150)) {
        exit(1);
    }

    /* set up digital LP-filter */
    if (i2c_write(REG_CONFIG, 0x05)) {
        exit(1);
    }

    /* set sleep to 0 */
    if (i2c_write(REG_PWR_MGMT1, 0x00)) {
        exit(1);
    }

    /* enable DATA_RDY interrupt (polling for now) */
    /* active when internal writes to all out data regs are done */
    if (i2c_write(REG_INT_ENABLE, 0x01)) {
        exit(1);
    }

    gyro = MPU6050_GYRO_FS_250 << 3;
    lsb_conv_gyro = 131.f;
    if (i2c_write(REG_GYRO_CONFIG, gyro)) {
        exit(1);
    }

    acc = MPU6050_ACC_FS_2G << 3;
    lsb_conv_acc = 16384.f;
    if (i2c_write(REG_ACCEL_CONFIG, acc)) {
        exit(1);
    }

    usleep(200 * 1000);

LOOP:
    do {
        err |= i2c_read(REG_INT_STATUS, &int_status, 1);
        usleep(1000);
    } while(!err && !(int_status & 1));

    if (i2c_read(REG_GYRO_XOUT_H, data, 6)) {
        exit(1);
    }

    gx = (data[0] << 8 | data[1]);
    gy = (data[2] << 8 | data[3]);
    gz = (data[4] << 8 | data[5]);

    printf("[GYRO °/s] x: % -3.4f y: %-3.4f z: %-3.4f ",
        (float)(gx)/lsb_conv_gyro,
        (float)(gy)/lsb_conv_gyro,
        (float)(gz)/lsb_conv_gyro);

    err = 0;
    int_status = 0;

    do {
        err |= i2c_read(REG_INT_STATUS, &int_status, 1);
        usleep(1000);
    } while(!err && !(int_status & 1));

    if (i2c_read(REG_ACCEL_XOUT_H, data, 6)) {
        exit(1);
    }

    ax = (data[0] << 8 | data[1]);
    ay = (data[2] << 8 | data[3]);
    az = (data[4] << 8 | data[5]);

    printf("[ACCEL g] x: % -3.4f y: %-3.4f z: %-3.4f\n",
        (float)(ax)/lsb_conv_acc,
        (float)(ay)/lsb_conv_acc,
        (float)(az)/lsb_conv_acc);
    
goto LOOP;

    i2c_deinit();

    return 0;
}