#include "i2c.h"
#include "QMC5883L.h"

bool QMC5883L_init(void)
{
    return i2c_write_reg(QMC5883L_I2C_ADDR, QMC5883L_SETRES_REG, 0x01);
}

float QMC5883L_get_temperature(void)
{
    uint8_t data;
    int16_t raw_temp;

    i2c_read_reg(QMC5883L_I2C_ADDR, QMC5883L_TEMP_MSB_REG, &data);
    
    raw_temp = data << 8;

    i2c_read_reg(QMC5883L_I2C_ADDR, QMC5883L_TEMP_LSB_REG, &data);

    raw_temp |= data;

    return (float) raw_temp / 100.0;
}

bool QMC5883L_is_data_available(void)
{
    uint8_t data;

    i2c_read_reg(QMC5883L_I2C_ADDR, QMC5883L_STATUS_REG, &data);

    return data & QMC5883L_DRDY_BIT ? true : false;
}

QMC5883L_MData_t QMC5883L_get_MFields(void)
{
    uint8_t data[6];
    QMC5883L_MData_t Mdata = {.MFieldX = -1, .MFieldY = -1, .MFieldZ = -1};

    if(i2c_read(QMC5883L_I2C_ADDR, QMC5883L_DATA_X_LSB_REG, data, sizeof data))
    {
        Mdata.MFieldX = (data[1] << 8) | data[0];
        Mdata.MFieldY = (data[3] << 8) | data[2];
        Mdata.MFieldZ = (data[5] << 8) | data[4];
    }

    return Mdata;
}

bool QMC5883L_set_power_mode(QMC5883L_Mode_Control_e MC)
{
    uint8_t data;

    if(!i2c_read_reg(QMC5883L_I2C_ADDR, QMC5883L_CTRL1_REG, &data))return false;

    data &= 0xFC;
    data |= MC;

    return i2c_write_reg(QMC5883L_I2C_ADDR, QMC5883L_CTRL1_REG, data);
}