#include "MPU6050.h"
#include "board.h"
#include "delay.h"
#include "print.h"

MPU6050::MPU6050(uint8_t address):devAddr(address)
{
}

void MPU6050::initialize()
{
    Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x80); // 设备复位
    delay_ms(5);
    Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x01); //设置设备时钟源
    delay_ms(5);
    Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_CONFIG , MPU6050_DLPF_BW_42);    //低通滤波 acc: 44hz, gyro: 42hz
    delay_ms(5);
    Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x00);
    delay_ms(5);
    Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GYRO_FS_2000<<3);
    delay_ms(5);
    Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACCEL_FS_8<<3);
    delay_ms(5);

    Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_USER_CTRL,0X00);
    delay_ms(5);
    Board::get()->i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x02);
    delay_ms(5);
}

bool MPU6050::testConnection()
{
    uint8_t id = getDeviceID();
    return (id == 0x39) || (id == 0x34);
}

void MPU6050::setRate(uint8_t rate)
{
    Board::get()->i2c_write_byte(devAddr, MPU6050_RA_SMPLRT_DIV, rate);
}

void MPU6050::setExternalFrameSync(uint8_t sync)
{
    Board::get()->i2c_write_bits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync);
}

void MPU6050::setFullScaleGyroRange(uint8_t range)
{
    Board::get()->i2c_write_bits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}

void MPU6050::setFullScaleAccelRange(uint8_t range)
{
    Board::get()->i2c_write_bits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}

void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
{
    Board::get()->i2c_read_buf(devAddr, MPU6050_RA_ACCEL_XOUT_H, buffer, 14);
    *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
    *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
    *az = (((int16_t)buffer[4]) << 8) | buffer[5];
    *gx = (((int16_t)buffer[8]) << 8) | buffer[9];
    *gy = (((int16_t)buffer[10]) << 8) | buffer[11];
    *gz = (((int16_t)buffer[12]) << 8) | buffer[13];
}

void MPU6050::reset()
{
    Board::get()->i2c_write_bits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1, 1);
}

uint8_t MPU6050::getDeviceID()
{
    buffer[0]=0xff;
    Board::get()->i2c_read_bits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
    log("MPU6050 id: 0x%02x", buffer[0]);
    return buffer[0];
}