#include "bma253_defs.h"

typedef struct _bma253_frame_
{
    int16_t acc_x;
    int16_t acc_y;
    int16_t acc_z;
} bma253_frame_t;

uint8_t bma253_read_regs(  uint8_t reg, uint8_t len, uint8_t *buf)
{
    if(HAL_I2C_Mem_Read(&hi2c2, (uint16_t)BMA253_DEFAULT_ADDR<<1, reg, 
                               I2C_MEMADD_SIZE_8BIT, buf, len, BMA253_I2C_TIMEOUT )!= HAL_OK)
    {
        return -1;
    }
    
    return 0;
}

uint8_t bma253_write_regs(  uint8_t reg, uint8_t len, uint8_t *buf)
{
    if(HAL_I2C_Mem_Write(&hi2c2, (uint16_t)BMA253_DEFAULT_ADDR<<1, reg, 
                               I2C_MEMADD_SIZE_8BIT, buf, len, BMA253_I2C_TIMEOUT )!= HAL_OK)
    {
        return -1;
    }
    
    return 0;
}

uint32_t bma253_driver_read( void *buffer,uint32_t size)
{

    uint8_t frame_len = sizeof(bma253_frame_t);

    uint8_t buf[6];
    uint16_t acc_x, acc_y, acc_z;
    bma253_frame_t frame;

    for (uint8_t i = 0; i < size; i++)
    {
        if (bma253_read_regs( BMA253_REG_FIFO_DATA, 6, buf) != 0){
            printf("BMA253 Error \r\n");
            return -1;
        }

        acc_x = (((uint16_t)buf[1] << 8) | (buf[0] & (_BMA253_ACCD12_LSB_MASK << _BMA253_ACCD12_LSB_SHIFT))) >> _BMA253_ACCD12_LSB_SHIFT;
        acc_y = (((uint16_t)buf[3] << 8) | (buf[2] & (_BMA253_ACCD12_LSB_MASK << _BMA253_ACCD12_LSB_SHIFT))) >> _BMA253_ACCD12_LSB_SHIFT;
        acc_z = (((uint16_t)buf[5] << 8) | (buf[4] & (_BMA253_ACCD12_LSB_MASK << _BMA253_ACCD12_LSB_SHIFT))) >> _BMA253_ACCD12_LSB_SHIFT;
        
        frame.acc_x = (int16_t)(acc_x>2048?(int16_t)acc_x-4096:acc_x);
//        printf("frame.acc_x : %d \r\n",frame.acc_x);
        frame.acc_y = (int16_t)(acc_y>2048?(int16_t)acc_y-4096:acc_y);
//        printf("frame.acc_y : %d \r\n",frame.acc_y);
        frame.acc_z = (int16_t)(acc_z>2048?(int16_t)acc_z-4096:acc_z);
//        printf("frame.acc_z : %d \r\n",frame.acc_z);
        printf(" ======================\r\n");
        printf("||X       | Y     | Z    |\r\n");
        printf("||%d   |%d    |%d    |\r\n",frame.acc_x,frame.acc_y,frame.acc_z);
        printf("=======================\r\n");
        memcpy((uint8_t *)buffer+frame_len * i, (uint8_t *)&frame, frame_len);
    }

    return size * frame_len;
}

int8_t bma253_reset(void)
{
    uint8_t reg = BMA253_RESET_BYTE;

    if (bma253_write_regs(BMA253_REG_BGW_SOFTRESET, 1, &reg) != 0){
        printf("BMA253 Error \r\n");
        return -1;
    }

    HAL_Delay(1000);

    return 0;
}

void i2c_check(uint8_t result,uint8_t* res){
    switch(result){
        case HAL_OK:
        printf("HAL_OK : %d Result : %02x\r\n",result,res[0]);
        break;
        case HAL_ERROR:
        printf("HAL_ERROR : %d\r\n",result);
        break;
        case HAL_BUSY:
        printf("HAL_BUSY : %d\r\n",result);
        break;
        case HAL_TIMEOUT:
        printf("HAL_TIMEOUT : %d\r\n",result);
        break;
    }

}

int bma253_get_chip_id(void){
    uint8_t res[1]={0,};
    i2c_check(HAL_I2C_Mem_Read(&hi2c2, (uint16_t)BMA253_DEFAULT_ADDR<<1, BMA253_REG_BGW_CHIPID,I2C_MEMADD_SIZE_8BIT, res, I2C_MEMADD_SIZE_8BIT, 1000 ),res);
}