#include "MPU.h"
#include "hal_data.h"
//2- Accel & Gyro Scaling Factor
fsp_err_t i2c_write_byte(uint8_t mem_addr, uint8_t data);
fsp_err_t i2c_read(uint8_t mem_addr, uint8_t *p_buffer, uint8_t num_bytes);
fsp_err_t i2c_read_byte(uint8_t mem_addr, uint8_t *p_data);

#define RESET_VALUE        0x00
#define BUF_LEN            14    // 6 accel + 2 temp + 6 gyro
#define DELAY_MS(ms)     R_BSP_SoftwareDelay((ms), BSP_DELAY_UNITS_MILLISECONDS)
volatile i2c_master_event_t g_master_event = 0; // Global variable for I2C event
uint8_t read_buffer[BUF_LEN] = {RESET_VALUE};
static float accelScalingFactor, gyroScalingFactor;
//3- Bias variables
static float A_X_Bias = 0.0f;
static float A_Y_Bias = 0.0f;
static float A_Z_Bias = 0.0f;

static int16_t GyroRW[3];
/* Callback function */
void sci_i2c_master_callback(i2c_master_callback_args_t *p_args)
{
    /* TODO: add your own code here */
    if (p_args != NULL)
     {
         g_master_event = p_args->event;
     }
}

fsp_err_t  i2c_write_byte(uint8_t mem_addr, uint8_t data)
{
    fsp_err_t err;
    uint8_t tx_buf[2] = { mem_addr, data };

    g_master_event = I2C_MASTER_EVENT_ABORTED;

    err = R_SCI_I2C_Write(&g_i2c0_ctrl, tx_buf, sizeof(tx_buf), false);
    if (err != FSP_SUCCESS) return err;

    while (g_master_event != I2C_MASTER_EVENT_TX_COMPLETE) { /* Wait */ }

    return FSP_SUCCESS;
}
 fsp_err_t  i2c_read(uint8_t mem_addr, uint8_t *p_buffer, uint8_t num_bytes)
{
    fsp_err_t err;

    g_master_event = I2C_MASTER_EVENT_ABORTED;

    // Send register address first
    err = R_SCI_I2C_Write(&g_i2c0_ctrl, &mem_addr, 1, true); // restart = true
    if (err != FSP_SUCCESS) return err;

    while (g_master_event != I2C_MASTER_EVENT_TX_COMPLETE) { /* Wait */ }

    // Read data from slave
    g_master_event = I2C_MASTER_EVENT_ABORTED;

    err = R_SCI_I2C_Read(&g_i2c0_ctrl, p_buffer, num_bytes, false); // restart = false (stop after)
    if (err != FSP_SUCCESS) return err;

    while (g_master_event != I2C_MASTER_EVENT_RX_COMPLETE) { /* Wait */ }

    return FSP_SUCCESS;
}
fsp_err_t  i2c_read_byte(uint8_t mem_addr, uint8_t *p_data)
{
    fsp_err_t err;

    g_master_event = I2C_MASTER_EVENT_ABORTED;

    // Step 1: Send register address to read from
    err = R_SCI_I2C_Write(&g_i2c0_ctrl, &mem_addr, 1, true); // restart = true
    if (err != FSP_SUCCESS) return err;

    while (g_master_event != I2C_MASTER_EVENT_TX_COMPLETE) { /* wait */ }

    // Step 2: Read one byte
    g_master_event = I2C_MASTER_EVENT_ABORTED;

    err = R_SCI_I2C_Read(&g_i2c0_ctrl, p_data, 1, false); // restart = false (stop)
    if (err != FSP_SUCCESS) return err;

    while (g_master_event != I2C_MASTER_EVENT_RX_COMPLETE) { /* wait */ }

    return FSP_SUCCESS;
}

void MPU6050_Config(MPU_ConfigTypeDef *config)
{
    uint8_t Buffer = 0;
    //Clock Source
    //Reset Device

    i2c_write_byte(PWR_MAGT_1_REG, 0x80);

    DELAY_MS(100);

    Buffer = config ->ClockSource & 0x07; //change the 7th bits of register

    Buffer |= (config ->Sleep_Mode_Bit << 6) &0x40; // change only the 7th bit in the register

    i2c_write_byte(PWR_MAGT_1_REG, Buffer);

    DELAY_MS(100); // should wait 10ms after changeing the clock setting.

    //Set the Digital Low Pass Filter
    Buffer = 0;

    Buffer = config->CONFIG_DLPF & 0x07;

    i2c_write_byte(CONFIG_REG, Buffer);

    //Select the Gyroscope Full Scale Range
    Buffer = 0;

    Buffer = (config->Gyro_Full_Scale << 3) & 0x18;

    i2c_write_byte(GYRO_CONFIG_REG, Buffer);

    //Select the Accelerometer Full Scale Range
    Buffer = 0;

    Buffer = (config->Accel_Full_Scale << 3) & 0x18;

    i2c_write_byte(ACCEL_CONFIG_REG, Buffer);

    //Set SRD To Default
    MPU6050_Set_SMPRT_DIV(0x00);//chnage on 04-06-25 was 0x04


    //Accelerometer Scaling Factor, Set the Accelerometer and Gyroscope Scaling Factor
    switch (config->Accel_Full_Scale)
    {
        case AFS_SEL_2g:
            accelScalingFactor = (2000.0f/32768.0f);
            break;

        case AFS_SEL_4g:
            accelScalingFactor = (4000.0f/32768.0f);
                break;

        case AFS_SEL_8g:
            accelScalingFactor = (8000.0f/32768.0f);
            break;

        case AFS_SEL_16g:
            accelScalingFactor = (16000.0f/32768.0f);
            break;

        default:
            break;
    }
    //Gyroscope Scaling Factor
    switch (config->Gyro_Full_Scale)
    {
        case FS_SEL_250:
            gyroScalingFactor = 250.0f/32768.0f;
            break;

        case FS_SEL_500:
                gyroScalingFactor = 500.0f/32768.0f;
                break;

        case FS_SEL_1000:
            gyroScalingFactor = 1000.0f/32768.0f;
            break;

        case FS_SEL_2000:
            gyroScalingFactor = 2000.0f/32768.0f;
            break;

        default:
            break;
    }

}
//5- Get Sample Rate Divider
uint8_t MPU6050_Get_SMPRT_DIV(void)
{
    uint8_t Buffer = 0;

    i2c_read_byte(SMPLRT_DIV_REG,&Buffer);

    return Buffer;
}

void MPU6050_Set_SMPRT_DIV(uint8_t SMPRTvalue)
{
    i2c_write_byte(SMPLRT_DIV_REG, SMPRTvalue);
}

uint8_t MPU6050_Get_FSYNC(void)
{
    uint8_t Buffer = 0;

    i2c_read_byte(CONFIG_REG, &Buffer);
    Buffer &= 0x38;
    return (Buffer>>3);
}
//8- Set External Frame Sync.
void MPU6050_Set_FSYNC(enum EXT_SYNC_SET_ENUM ext_Sync)
{
    uint8_t Buffer = 0;
    i2c_read_byte(CONFIG_REG, &Buffer);
    Buffer &= (uint8_t)(~0x38u);


    Buffer |= (ext_Sync <<3);
    i2c_write_byte(CONFIG_REG, Buffer);

}
//9- Get Accel Raw Data
void MPU6050_Get_Accel_RawData(RawData_Def *rawDef)
{
    uint8_t state;
    uint8_t AcceArr[6], GyroArr[6];

    i2c_read_byte(INT_STATUS_REG, &state);


    if((state&&0x01))
    {
        i2c_read(ACCEL_XOUT_H_REG, AcceArr, 6);

        //Accel Raw Data
        rawDef->x = (int16_t)(((int)AcceArr[0] << 8) | (int)AcceArr[1]);
        rawDef->y = (int16_t)(((int)AcceArr[2] << 8) | (int)AcceArr[3]);
        rawDef->z = (int16_t)(((int)AcceArr[4] << 8) | (int)AcceArr[5]);
        //Gyro Raw Data
        i2c_read(GYRO_XOUT_H_REG, GyroArr, 6);
        GyroRW[0] = (int16_t)(((int)GyroArr[0] << 8) | (int)GyroArr[1]);
        GyroRW[1] = (int16_t)(((int)GyroArr[2] << 8) | (int)GyroArr[3]);
        GyroRW[2] = (int16_t)(((int)GyroArr[4] << 8) | (int)GyroArr[5]);
    }
}
//10- Get Accel scaled data (g unit of gravity, 1g = 9.81m/s2)
void MPU6050_Get_Accel_Scale(ScaledData_Def *scaledDef)
{

    RawData_Def AccelRData;
    MPU6050_Get_Accel_RawData(&AccelRData);

    //Accel Scale data
    scaledDef->x = ((AccelRData.x+0.0f)*accelScalingFactor)/1000;
    scaledDef->y = ((AccelRData.y+0.0f)*accelScalingFactor)/1000;
    scaledDef->z = ((AccelRData.z+0.0f)*accelScalingFactor)/1000;
}
//11- Get Accel calibrated data
void MPU6050_Get_Accel_Cali(ScaledData_Def *CaliDef)
{
    ScaledData_Def AccelScaled;
    MPU6050_Get_Accel_Scale(&AccelScaled);

    //Accel Scale data
    CaliDef->x = (AccelScaled.x) - A_X_Bias; // x-Axis
    CaliDef->y = (AccelScaled.y) - A_Y_Bias;// y-Axis
    CaliDef->z = (AccelScaled.z) - A_Z_Bias;// z-Axis
}
//12- Get Gyro Raw Data
void MPU6050_Get_Gyro_RawData(RawData_Def *rawDef)
{

    //Accel Raw Data
    rawDef->x = GyroRW[0];
    rawDef->y = GyroRW[1];
    rawDef->z = GyroRW[2];

}

//13- Get Gyro scaled data
void MPU6050_Get_Gyro_Scale(ScaledData_Def *scaledDef)
{
    RawData_Def myGyroRaw;
    MPU6050_Get_Gyro_RawData(&myGyroRaw);

    //Gyro Scale data
    scaledDef->x = (myGyroRaw.x)*gyroScalingFactor; // x-Axis
    scaledDef->y = (myGyroRaw.y)*gyroScalingFactor; // y-Axis
    scaledDef->z = (myGyroRaw.z)*gyroScalingFactor; // z-Axis
}
//14- Accel Calibration
void _Accel_Cali(float x_min, float x_max, float y_min, float y_max, float z_min, float z_max)
{
    //1* X-Axis calibrate
    A_X_Bias        = (x_max + x_min)/2.0f;

    //2* Y-Axis calibrate
    A_Y_Bias        = (y_max + y_min)/2.0f;

    //3* Z-Axis calibrate
    A_Z_Bias        = (z_max + z_min)/2.0f;
}