#include "mpu6500.h"

#define MPU6500_REG_GYRO_CONFIG       0x1BU
#define MPU6500_REG_ACCEL_CONFIG      0x1CU
#define MPU6500_REG_ACCEL_XOUT_H      0x3BU
#define MPU6500_REG_WHO_AM_I          0x75U
#define MPU6500_REG_SIGNAL_PATH_RESET 0x68U
#define MPU6500_REG_PWR_MGMT_1        0x6BU
#define mPU6500_REG_PWR_MGMT_2        0x6CU

#define MPU6500_CONST_WHOAMI 0x70U

#define MPU6500_BTW_CPU(h, l) (((h & 0xFFU) << 8U) | (l & 0xFFU))

/**
 * @brief Reset MPU6500.
 * Refer to description field of the PWR_MGMT1 register in RegMap manual
 * @param mpu Pointer to mpu6500_t struct
 * @return mpu6500_ret_t MPU6500_RET_SUCCESS for success, others for failure.
 */
static mpu6500_ret_t mpu6500_reset(mpu6500_t *mpu) {
    mpu6500_ret_t ret = MPU6500_RET_SUCCESS;

    uint8_t buf[1];

    ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_PWR_MGMT_1, buf, 0x01);
    if (ret != MPU6500_RET_SUCCESS) {
        return ret;
    }

    buf[0] |= 0x80; /* H_RESET */
    ret = mpu->ops.write_register(mpu->user_data, MPU6500_REG_PWR_MGMT_1, buf, 0x01);
    if (ret != MPU6500_RET_SUCCESS) {
        return ret;
    }

    ret = mpu->ops.delay(mpu->user_data, 100);
    if (ret != MPU6500_RET_SUCCESS) {
        return ret;
    }

    buf[0] = 0x07; /* GYRO_RST | ACCEL_RST | TEMP_RST */
    ret    = mpu->ops.write_register(mpu->user_data, MPU6500_REG_SIGNAL_PATH_RESET, buf, 0x01);
    if (ret != MPU6500_RET_SUCCESS) {
        return ret;
    }

    ret = mpu->ops.delay(mpu->user_data, 100);
    return ret;
}

mpu6500_ret_t mpu6500_init(mpu6500_t *mpu) {
    mpu6500_ret_t ret = MPU6500_RET_SUCCESS;

    uint8_t rx_buf[1];

    ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_WHO_AM_I, rx_buf, 0x01);
    if (ret != MPU6500_RET_SUCCESS) {
        return ret;
    }

    ret = mpu6500_reset(mpu);
    return ret;
}

mpu6500_ret_t mpu6500_read_channel(mpu6500_t *mpu, mpu6500_channel_t ch, int16_t *result) {
    mpu6500_ret_t ret = MPU6500_RET_SUCCESS;

    uint8_t buf[2];

    ret = mpu->ops.read_register(mpu->user_data, ch, buf, 0x02);
    if (ret != MPU6500_RET_SUCCESS) {
        return ret;
    }

    *result = MPU6500_BTW_CPU(buf[0], buf[1]);

    return ret;
}

mpu6500_ret_t mpu6500_read_result(mpu6500_t *mpu, mpu6500_result_t *result) {
    mpu6500_ret_t ret = MPU6500_RET_SUCCESS;

    uint8_t buf[14];
    ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_ACCEL_XOUT_H, buf, 14);
    if (ret != MPU6500_RET_SUCCESS) {
        return ret;
    }

    result->accel_x_raw = MPU6500_BTW_CPU(buf[0], buf[1]);
    result->accel_y_raw = MPU6500_BTW_CPU(buf[2], buf[3]);
    result->accel_z_raw = MPU6500_BTW_CPU(buf[4], buf[5]);
    result->temp_raw    = MPU6500_BTW_CPU(buf[6], buf[7]);
    result->gyro_x_raw  = MPU6500_BTW_CPU(buf[8], buf[9]);
    result->gyro_y_raw  = MPU6500_BTW_CPU(buf[10], buf[11]);
    result->gyro_z_raw  = MPU6500_BTW_CPU(buf[12], buf[13]);

    return ret;
}

mpu6500_ret_t mpu6500_get_accel_fullscale(mpu6500_t *mpu, uint16_t *f_scale) {
    mpu6500_ret_t ret = MPU6500_RET_SUCCESS;

    uint8_t buf[1];
    ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_ACCEL_CONFIG, buf, 0x01);
    if (ret != MPU6500_RET_SUCCESS) {
        return ret;
    }

    uint8_t fs = (buf[0] >> 3U) & 0x03U;

    *f_scale = 1 << (fs + 1); /* 00: 2g, 01: 4g, 10: 8g, 11: 16g */

    return ret;
}

mpu6500_ret_t mpu6500_get_gyro_fullscale(mpu6500_t *mpu, uint16_t *f_scale) {
    mpu6500_ret_t ret = MPU6500_RET_SUCCESS;

    uint8_t buf[1];
    ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_GYRO_CONFIG, buf, 0x01);
    if(ret != MPU6500_RET_SUCCESS) {
        return ret;
    }

    uint8_t fs = (buf[0] >> 3U) & 0x03U;

    *f_scale = 250 * (1 << fs);

    return ret;
}