#include <math.h>
#include "i2c.h"
#include "QMC5883L.h"

#ifndef PI
#define PI                 3.14159265358979f
#endif

typedef struct
{
    int16_t x_min;
    int16_t x_max;
    int16_t y_min;
    int16_t y_max;
    int16_t z_min;
    int16_t z_max;
} _QMC5883L_calibration_data;

static _QMC5883L_calibration_data calibration_data = {0};


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

bool QMC5883L_software_reset(void)
{
    return i2c_write_reg(QMC5883L_I2C_ADDR, QMC5883L_CTRL2_REG, 0x80);
}

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;
}

void QMC5883L_set_calibration_data(int16_t x_min, int16_t x_max, int16_t y_min, int16_t y_max, int16_t z_min, int16_t z_max)
{
    calibration_data.x_min = x_min;
    calibration_data.x_max = x_max;
    calibration_data.y_min = y_min;
    calibration_data.y_max = y_max;
    calibration_data.z_min = z_min;
    calibration_data.z_max = z_max;
}

QMC5883L_MData_t QMC5883L_get_MFields_raw(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;
}

QMC5883L_MData_calibrated_t QMC5883L_get_MFields_calibrated(void)
{
    uint8_t data[6];
    QMC5883L_MData_calibrated_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];
    }

    int x_offset = (calibration_data.x_min + calibration_data.x_max) / 2;
    int y_offset = (calibration_data.y_min + calibration_data.y_max) / 2;
    int z_offset = (calibration_data.z_min + calibration_data.z_max) / 2;
    int x_avg_delta = (calibration_data.x_max - calibration_data.x_min) / 2;
    int y_avg_delta = (calibration_data.y_max - calibration_data.y_min) / 2;
    int z_avg_delta = (calibration_data.z_max - calibration_data.z_min) / 2;

    int avg_delta = (x_avg_delta + y_avg_delta + z_avg_delta) / 3;
    
    float x_scale = (float)avg_delta / x_avg_delta;
	float y_scale = (float)avg_delta / y_avg_delta;
	float z_scale = (float)avg_delta / z_avg_delta;

    Mdata.MFieldX = (Mdata.MFieldX - x_offset) * x_scale;
    Mdata.MFieldY = (Mdata.MFieldY - y_offset) * y_scale;
    Mdata.MFieldZ = (Mdata.MFieldZ - z_offset) * z_scale;

    return Mdata;
}

int16_t QMC5883L_get_azimuth(const QMC5883L_MData_calibrated_t data)
{
    int16_t angle = atan2(data.MFieldY, data.MFieldX) * 180.0 / PI;
    return angle < 0 ? 360 + angle : angle;
}

bool QMC5883L_configure_1(QMC5883L_Output_Data_Rate_e ODR, QMC5883L_Full_Scale_e RNG, QMC5883L_Over_Sample_Ratio_e OSR)
{
    uint8_t data;

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

    data &= 0x03;
    data |= OSR << 6 | RNG << 4 | ODR << 2;

    return i2c_write_reg(QMC5883L_I2C_ADDR, QMC5883L_CTRL1_REG, data);
}

bool QMC5883L_configure_2(bool rolling_ptr, bool data_ready_int)
{
    return i2c_write_reg(QMC5883L_I2C_ADDR, QMC5883L_CTRL2_REG, rolling_ptr << 6 | data_ready_int);
}

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);
}