/**
 * @file magnetometer_read.h
 * @author Woon Jun Wei
 * @brief This file contains the functions to read the data
 * from the LSM303DLHC accelerometer and magnetometer sensor
 */

#ifndef MAGNETOMETER_READ_H
#define MAGNETOMETER_READ_H
#include "magnetometer_init.h"

/**
 * @brief Read Data with I2C, given the address and register
 * @param addr  Address of the device
 * @param reg   Register to read from
 * @return      1 piece of data read from the register
 */
static inline int
read_data(uint8_t addr, uint8_t reg) {
    uint8_t data[1];

    // Send the register address to read from
    i2c_write_blocking(i2c_default, addr, &reg, 1, true);

    // Read the data
    i2c_read_blocking(i2c_default, addr, data, 1, false);

    return data[0];
}

/**
 * @brief Read Accelerometer Data
 * @param accelerometer   Accelerometer Data
 */
static inline void
read_accelerometer(int16_t accelerometer[3]) {
    uint8_t buffer[6];

    buffer[0] = read_data(ACCEL_ADDR, LSM303_OUT_X_L_A);
    buffer[1] = read_data(ACCEL_ADDR, LSM303_OUT_X_H_A);
    buffer[2] = read_data(ACCEL_ADDR, LSM303_OUT_Y_L_A);
    buffer[3] = read_data(ACCEL_ADDR, LSM303_OUT_Y_H_A);
    buffer[4] = read_data(ACCEL_ADDR, LSM303_OUT_Z_L_A);
    buffer[5] = read_data(ACCEL_ADDR, LSM303_OUT_Z_H_A);

    // Combine high and low bytes

    // xAcceleration
    accelerometer[0] = (int16_t) ((buffer[1] << 8) | buffer[0]);

    // yAcceleration
    accelerometer[1] = (int16_t) ((buffer[3] << 8) | buffer[2]);

    // zAcceleration
    accelerometer[2] = (int16_t) ((buffer[5] << 8) | buffer[4]);

}

/**
 * @brief Read Magnetometer Data
 * @param magnetometer  Magnetometer Data
 */
static inline void
read_magnetometer(int16_t magnetometer[3]) {
    uint8_t buffer[6];

    buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M);
    buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M);
    buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M);
    buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M);
    buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M);
    buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M);

    magnetometer[0] = (int16_t) (buffer[0] << 8 | buffer[1]); //xMag

    magnetometer[1] = (int16_t) (buffer[2] << 8 | buffer[3]); //yMag

    magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag
}

#endif