/* MPU6050 device I2C library code for ARM STM32F103xx is placed under the MIT license
   Copyright (c) 2012 Harinadha Reddy Chintalapalli

   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included in
   all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   THE SOFTWARE.
*/

#include "mpu6050.h"
#include "i2c.h"

/** @defgroup MPU_Library
 * @{
 */

/** Power on and prepare for general usage.
 * This will activate the device and take it out of sleep mode (which must be done
 * after start-up). This function also sets both the accelerometer and the gyroscope
 * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
 * the clock source to use the X Gyro for reference, which is slightly better than
 * the default internal clock source.
 */
void mpu_init()
{
    mpu_reg_write(MPU_RA_PWR_MGMT1, MPU_CLOCK_PLL_XGYRO);
    mpu_set_gyro_fs(MPU_GYRO_FS_250);
    mpu_set_accel_fs(MPU_ACCEL_FS_2);
}

void mpu_init_low_power(uint8_t wake_freq, bool enable_interrupt)
{
    mpu_set_gyro_fs(MPU_GYRO_FS_250);
    mpu_set_accel_fs(MPU_ACCEL_FS_2);
    /* For these two regs, see pwr mgmt reg 2 desc in reg map ref manual, pg. 42 */
    mpu_reg_write(MPU_RA_PWR_MGMT1, MPU_PWR_MGMT1_CYCLE | MPU_PWR_MGMT1_TEMP_DIS | MPU_CLOCK_INTERNAL);
    mpu_reg_write(MPU_RA_PWR_MGMT2, MPU_PWR_MGMT2_STBY_XG | MPU_PWR_MGMT2_STBY_YG | MPU_PWR_MGMT2_STBY_ZG | wake_freq);

    if (enable_interrupt) {
        mpu_reg_write(MPU_RA_INT_PIN_CFG, MPU_INT_PIN_CFG_RD_CLEAR);
        mpu_reg_write(MPU_RA_INT_ENABLE, MPU_INT_ENABLE_DATA_RDY);
    }
}

/** Verify the I2C connection.
 * Make sure the device is connected and responds as expected.
 * @return True if connection is valid, FALSE otherwise
 */
bool mpu_test_connection()
{
    return mpu_device_id() == MPU_DEVICE_ID;
}

/** Get Device ID.
 * This register is used to verify the identity of the device (0b110100).
 * @return Device ID (should be 0x68, 104 dec, 150 oct)
 * @see MPU_RA_WHO_AM_I
 * @see MPU_WHO_AM_I_BIT
 * @see MPU_WHO_AM_I_LENGTH
 */
uint8_t mpu_device_id()
{
    return mpu_reg_read(MPU_RA_WHO_AM_I) >> MPU_WHO_AM_I_Pos;
}

/** Set full-scale gyroscope range.
 * @param range New full-scale gyroscope range value
 * @see MPU_GetFullScaleGyroRange()
 * @see MPU_GYRO_FS_250
 * @see MPU_RA_GYRO_CONFIG
 * @see MPU_GCONFIG_FS_SEL_BIT
 * @see MPU_GCONFIG_FS_SEL_LENGTH
 */
void mpu_set_gyro_fs(uint8_t range)
{
    mpu_reg_write(MPU_RA_GYRO_CONFIG, range);
}


/** Get full-scale gyroscope range.
 * The FS_SEL parameter allows setting the full-scale range of the gyro sensors,
 * as described in the table below.
 *
 * <pre>
 * 0 = +/- 250 degrees/sec
 * 1 = +/- 500 degrees/sec
 * 2 = +/- 1000 degrees/sec
 * 3 = +/- 2000 degrees/sec
 * </pre>
 *
 * @return Current full-scale gyroscope range setting
 * @see MPU_GYRO_FS_250
 * @see MPU_RA_GYRO_CONFIG
 * @see MPU_GCONFIG_FS_SEL_BIT
 * @see MPU_GCONFIG_FS_SEL_LENGTH
 */
uint8_t mpu_get_gyro_fs()
{
    return mpu_reg_read(MPU_RA_GYRO_CONFIG) & MPU_GYRO_FS_SEL_Msk;
}

/** Get full-scale accelerometer range.
 * The FS_SEL parameter allows setting the full-scale range of the accelerometer
 * sensors, as described in the table below.
 *
 * <pre>
 * 0 = +/- 2g
 * 1 = +/- 4g
 * 2 = +/- 8g
 * 3 = +/- 16g
 * </pre>
 *
 * @return Current full-scale accelerometer range setting
 * @see MPU_ACCEL_FS_2
 * @see MPU_RA_ACCEL_CONFIG
 */
uint8_t mpu_get_accel_fs()
{
    return mpu_reg_read(MPU_RA_ACCEL_CONFIG) & MPU_ACCEL_CONFIG_FS_SEL_Msk;
}

/** Set full-scale accelerometer range.
 * @param range New full-scale accelerometer range setting
 * @see MPU_GetFullScaleAccelRange()
 */
void mpu_set_accel_fs(uint8_t range)
{
    mpu_reg_write(MPU_RA_ACCEL_CONFIG, range);
}

/** Get sleep mode status.
 * Setting the SLEEP bit in the register puts the device into very low power
 * sleep mode. In this mode, only the serial interface and internal registers
 * remain active, allowing for a very low standby current. Clearing this bit
 * puts the device back into normal mode. To save power, the individual standby
 * selections for each of the gyros should be used if any gyro axis is not used
 * by the application.
 * @return Current sleep mode bit
 * @see MPU_RA_PWR_MGMT_1
 * @see MPU_PWR1_SLEEP_BIT
 */
bool mpu_get_sleep_mode()
{
    return mpu_reg_read(MPU_RA_PWR_MGMT1) & MPU_PWR_MGMT1_SLEEP;
}

/** Set sleep mode status.
 * @param enabled New sleep mode enabled status
 * @see MPU_GetSleepModeStatus()
 * @see MPU_RA_PWR_MGMT_1
 * @see MPU_PWR1_SLEEP_BIT
 */
void mpu_set_sleep_mode(bool val)
{
    int tmp = mpu_reg_read(MPU_RA_PWR_MGMT1);
    tmp = (tmp & ~MPU_PWR_MGMT1_SLEEP) | (val ? MPU_PWR_MGMT1_SLEEP : 0);
    mpu_reg_write(MPU_RA_PWR_MGMT1, tmp);
}

/** Get raw 6-axis motion sensor readings (accel/gyro).
 * Retrieves all currently available motion sensor values.
 * @see MPU_RA_ACCEL_XOUT_H
 */
void mpu_read_accel_gyro(struct mpu_raw_data *out)
{
    struct mpu_raw_data buf;
    mpu_reg_read_multiple(MPU_RA_ACCEL_XOUT_H, (uint8_t *)&buf, sizeof(buf));

    for (uint8_t i=0; i<ARRAY_SIZE(buf.channels); i++)
        out->channels[i] = (int16_t)be16toh((uint16_t)buf.channels[i]);
}

int16_t mpu_read_temp() {
    uint16_t buf;
    mpu_reg_read_multiple(MPU_RA_TEMP_OUT_H, (uint8_t *)&buf, sizeof(buf));
    return (int16_t)be16toh(buf);
}

void mpu_read_accel(struct mpu_accel_data *out)
{
    struct mpu_accel_data buf;
    mpu_reg_read_multiple(MPU_RA_ACCEL_XOUT_H, (uint8_t *)&buf, sizeof(buf));

    for (uint8_t i=0; i<ARRAY_SIZE(buf.channels); i++)
        out->channels[i] = (int16_t)be16toh((uint16_t)buf.channels[i]);
}

void mpu_read_gyro(struct mpu_gyro_data *out)
{
    struct mpu_gyro_data buf;
    mpu_reg_read_multiple(MPU_RA_GYRO_XOUT_H, (uint8_t *)&buf, sizeof(buf));

    for (uint8_t i=0; i<ARRAY_SIZE(buf.channels); i++)
        out->channels[i] = (int16_t)be16toh((uint16_t)buf.channels[i]);
}

/**
 * @brief  Writes one byte to the  MPU6050.
 * @param  reg : address of the register in which the data will be written
 * @param  val : the data to be written to the MPU6050.
 * @return None
 */
void mpu_reg_write(uint8_t reg, uint8_t val)
{
    uint8_t tx[2] = {reg, val};
    i2c_transmit(MPU_I2C_PERIPH, MPU_DEFAULT_ADDRESS, tx, sizeof(tx), I2C_GENSTOP_YES);
}

/**
 * @brief  Reads a block of data from the MPU6050.
 * @param  buf : pointer to the buffer that receives the data read from the MPU6050.
 * @param  addr : MPU6050's internal address to read from.
 * @param  len : number of bytes to read from the MPU6050
 * @return None
 */
void mpu_reg_read_multiple(uint8_t addr, uint8_t* buf, size_t len)
{
    i2c_transmit(MPU_I2C_PERIPH, MPU_DEFAULT_ADDRESS, &addr, 1, I2C_GENSTOP_NO);
    i2c_receive(MPU_I2C_PERIPH, MPU_DEFAULT_ADDRESS, buf, len);
}

uint8_t mpu_reg_read(uint8_t addr)
{
    uint8_t buf;
    i2c_transmit(MPU_I2C_PERIPH, MPU_DEFAULT_ADDRESS, &addr, 1, I2C_GENSTOP_NO);
    i2c_receive(MPU_I2C_PERIPH, MPU_DEFAULT_ADDRESS, &buf, 1);
    return buf;
}
/**
 * @}
 *//* end of group MPU_Library */