#include "bmm150.h"
#include "main.h"
#include "string.h"


extern void My_Delay_Ms(uint32_t ms);

static uint8 bmm150_read_ready_flag = 0;

#define bmm150_buff_len             10
int16 bmm150_data_buff[bmm150_buff_len] = {0};

bool bmm150_data_run = false;

static void BMM150_ReadRegisterValue(uint8 Address, uint8 *toPtr,uint16 len)
{
    uint8 sBuffer[32]={0};
	uint8 rBuffer[32]={0};
    uint8 increment = 0;

    sBuffer[0] = Address | BMM150_SPIReadFlag;
    HAL_GPIO_WritePin(BMM150_CS_GPIO_Port, BMM150_CS_Pin, GPIO_PIN_RESET);
    HAL_SPI_TransmitReceive(&hspi1, sBuffer, rBuffer, 1+len, 15);
    HAL_GPIO_WritePin(BMM150_CS_GPIO_Port, BMM150_CS_Pin, GPIO_PIN_SET);
    memcpy(toPtr,rBuffer+1,len);
    
}

uint8 BMM150_RegisterReadBack(uint8 addr,uint8 varify, uint8 varify_mask)
{
    uint8 receive=0;
	BMM150_ReadRegisterValue(addr,&receive,1);
	if(varify == (receive & varify_mask))
	{
		return BMM150_OK;
	}
	return BMM150_ERROR;
}

void BMM150_ConfigRegister(uint8 Address, uint8 value)
{
	uint8 sBuffer[4]={0};
	uint8 rBuffer[4]={0};

    
	sBuffer[0] = Address;
	sBuffer[1] = value;
	HAL_GPIO_WritePin(BMM150_CS_GPIO_Port, BMM150_CS_Pin, GPIO_PIN_RESET);
	HAL_SPI_TransmitReceive(&hspi1, sBuffer, rBuffer, 2, 10);
	HAL_GPIO_WritePin(BMM150_CS_GPIO_Port, BMM150_CS_Pin, GPIO_PIN_SET);
}

uint8 bmm150_set_pwr_mode(uint8 pwr_value)
{
    uint8 receive=0;
#if NORMAL_WORK_MODE    
    BMM150_ReadRegisterValue(BMM150_REGISTER_0x4C,&receive,1);
	receive = (receive & (~BMM150_PWR_MODE_VRF_MASK)) | pwr_value;
#else
    receive = pwr_value;
#endif
    BMM150_ConfigRegister(BMM150_REGISTER_0x4C, receive);
	
#if (BMM150_READBACK_DEF && NORMAL_WORK_MODE)
    if(BMM150_OK == BMM150_RegisterReadBack(BMM150_REGISTER_0x4C, pwr_value, BMM150_PWR_MODE_VRF_MASK))
    {
        return BMM150_OK;
    }
    return BMM150_SET_PWR_MODE_ERR;
#else
    return BMM150_OK;
#endif
}

uint8 bmm150_read_pwr_control(void)
{
    uint8 bmm150_id = 0;
    BMM150_ReadRegisterValue(BMM150_REGISTER_0x4B, &bmm150_id, 1);
    if(BMM150_PWR_OTHER_MODE != bmm150_id)
    {
        return BMM150_READ_PWR_CONTROL_ERR;
    }
    return BMM150_OK;
}

uint8 bmm150_set_pwr_control(uint8 value)
{
    uint8 receive=0;
	BMM150_ReadRegisterValue(BMM150_REGISTER_0x4B,&receive,1);
	receive = (receive & (~BMM150_PWR_CONTROL_VRF_MASK)) | value;
    
    BMM150_ConfigRegister(BMM150_REGISTER_0x4B, receive);
    My_Delay_Ms(1);
#if BMM150_READBACK_DEF
    if(BMM150_OK == BMM150_RegisterReadBack(BMM150_REGISTER_0x4B, value, BMM150_PWR_CONTROL_VRF_MASK))
    {
        return BMM150_OK;
    }
    return BMM150_SET_PWR_MODE_ERR;
#else
    return BMM150_OK;
#endif
}

uint8 bmm150_soft_reset(void)
{
    uint8 receive=0;
	BMM150_ReadRegisterValue(BMM150_REGISTER_0x4B,&receive,1);
	receive = (receive & (~BMM150_SOFT_RESET_VRF_MASK)) | BMM150_SOFT_RESET;
	
    BMM150_ConfigRegister(BMM150_REGISTER_0x4B, receive);
    My_Delay_Ms(1);
    return BMM150_OK;
} 

uint8 bmm150_read_id(void)
{
    uint8 bmm150_id = 0;
    BMM150_ReadRegisterValue(BMM150_REGISTER_CHIP_ID, &bmm150_id, 1);
    if(BMM150_CHIP_ID != bmm150_id)
    {
        return BMM150_READ_ID_ERR;
    }
    return BMM150_OK;
}

uint8 bmm150_set_rate(uint8 value)
{
    uint8 receive=0;
    
    BMM150_ReadRegisterValue(BMM150_REGISTER_0x4C,&receive,1);
	receive = (receive & (~BMM150_RATE_VRF_MASK)) | value;

	BMM150_ConfigRegister(BMM150_REGISTER_0x4C, receive);
    My_Delay_Ms(1);
#if BMM150_READBACK_DEF
    if(BMM150_OK == BMM150_RegisterReadBack(BMM150_REGISTER_0x4C, value, BMM150_RATE_VRF_MASK))
    {
        return BMM150_OK;
    }
    return BMM150_SET_RATE_ERR;
#else
    return BMM150_OK;
#endif
} 

uint8 bmm150_set_ready_en(uint8 value)
{
    uint8 receive=0;
	BMM150_ReadRegisterValue(BMM150_REGISTER_0x4E,&receive,1);
	receive = (receive & (~BMM150_READY_EN_VRF_MASK)) | value;

    BMM150_ConfigRegister(BMM150_REGISTER_0x4E, receive);
    My_Delay_Ms(1);
#if BMM150_READBACK_DEF
    if(BMM150_OK == BMM150_RegisterReadBack(BMM150_REGISTER_0x4E, value, BMM150_READY_EN_VRF_MASK))
    {
        return BMM150_OK;
    }
    return BMM150_SET_READY_EN_ERR;
#else
    return BMM150_OK;
#endif
}

uint8 bmm150_set_self_test(uint8 switch_value)
{
    uint8 receive=0;
	BMM150_ReadRegisterValue(BMM150_REGISTER_0x4C,&receive,1);
	receive = (receive & (~BMM150_SELF_TEST_VRF_MASK)) | switch_value;
	
    BMM150_ConfigRegister(BMM150_REGISTER_0x4C, receive);
    My_Delay_Ms(1);
#if BMM150_READBACK_DEF
    if(BMM150_OK == BMM150_RegisterReadBack(BMM150_REGISTER_0x4C, switch_value, BMM150_SELF_TEST_VRF_MASK))
    {
        return BMM150_OK;
    }
    return BMM150_SELF_TEST_PERFORMING;
#else
    return BMM150_OK;
#endif
}

uint8 bmm150_set_channel(uint8 value)
{
    uint8 receive=0;
	BMM150_ReadRegisterValue(BMM150_REGISTER_0x4E,&receive,1);
	receive = (receive & (~BMM150_CHANNEL_VRF_MASK)) | value;
	
    BMM150_ConfigRegister(BMM150_REGISTER_0x4E, value);
    My_Delay_Ms(1);
#if BMM150_READBACK_DEF
    if(BMM150_OK == BMM150_RegisterReadBack(BMM150_REGISTER_0x4E, value, BMM150_CHANNEL_VRF_MASK))
    {
        return BMM150_OK;
    }
    return BMM150_SET_CHANNEL_ERR;
#else
    return BMM150_OK;
#endif
} 

uint8 bmm150_set_repxy(uint8 value)
{
    BMM150_ConfigRegister(BMM150_REGISTER_0x51, value);
    My_Delay_Ms(1);
#if BMM150_READBACK_DEF
    if(BMM150_OK == BMM150_RegisterReadBack(BMM150_REGISTER_0x51, value, BMM150_REPXY_VRF_MASK))
    {
        return BMM150_OK;
    }
    return BMM150_SET_REPXY_ERR;
#else
    return BMM150_OK;
#endif
} 

uint8 bmm150_set_repz(uint8 value)
{
    BMM150_ConfigRegister(BMM150_REGISTER_0x52, value);
    My_Delay_Ms(1);
#if BMM150_READBACK_DEF
    if(BMM150_OK == BMM150_RegisterReadBack(BMM150_REGISTER_0x52, value, BMM150_REPZ_VRF_MASK))
    {
        return BMM150_OK;
    }
    return BMM150_SET_REPZ_ERR;
#else
    return BMM150_OK;
#endif
} 


void set_bmm150_read_ready(void)
{
    bmm150_read_ready_flag = 1;
}

void clr_bmm150_read_ready(void)
{
    bmm150_read_ready_flag = 0;
}


uint8 bmm150_init(void)
{
    uint8 init_ret = BMM150_ERROR;
    uint8 self_test_fault_cnt = 0;

    init_ret = bmm150_read_pwr_control();
    if(BMM150_OK != init_ret)
    {
        init_ret = bmm150_set_pwr_control(BMM150_PWR_OTHER_MODE);
        if(BMM150_OK != init_ret)
        {
            return init_ret;
        }
    }

    //bmm150_soft_reset();

    init_ret = bmm150_read_id();
    if(BMM150_OK != init_ret)
    {
        return init_ret;
    }


#if (!NORMAL_WORK_MODE)

    init_ret = bmm150_set_pwr_mode(BMM150_PWR_SLEEP_MODE);
    if(BMM150_OK != init_ret)
    {
        return init_ret;
    }

    init_ret = bmm150_set_ready_en(BMM150_READY_EN);
    if(BMM150_OK != init_ret)
    {
        return init_ret;
    }
    
    init_ret = bmm150_set_repxy(BMM150_REPXY_LOWPWR);
    if(BMM150_OK != init_ret)
    {
        return init_ret;
    }

    init_ret = bmm150_set_repz(BMM150_REPZ_LOWPWR);
    if(BMM150_OK != init_ret)
    {
        return init_ret;
    }
#else

    init_ret = bmm150_set_ready_en(BMM150_READY_EN);
    if(BMM150_OK != init_ret)
    {
        return init_ret;
    }

    init_ret = bmm150_set_rate(BMM150_DATA_RATE_10HZ);
    if(BMM150_OK != init_ret)
    {
        return init_ret;
    }

    init_ret = bmm150_set_repxy(BMM150_REPXY_REGULAR);
    if(BMM150_OK != init_ret)
    {
        return init_ret;
    }

    init_ret = bmm150_set_repz(BMM150_REPZ_REGULAR);
    if(BMM150_OK != init_ret)
    {
        return init_ret;
    }

    init_ret = bmm150_set_pwr_mode(BMM150_PWR_NORMAL_MODE);
    if(BMM150_OK != init_ret)
    {
        return init_ret;
    }
    
    
#endif
    return BMM150_OK;
}

void read_bmm150_mag(float *p_read_buff)
{
    //while(1)
    {
#if (NORMAL_WORK_MODE == 1)
        if(bmm150_read_ready_flag)
        {
            clr_bmm150_read_ready();
            BMM150_ReadRegisterValue(BMM150_REGISTER_X_MAG_LSB, (uint8 *)bmm150_data_buff, 6);


            bmm150_data_buff[0] = (bmm150_data_buff[0] >> 3);
            bmm150_data_buff[1] = (bmm150_data_buff[1] >> 3);
            bmm150_data_buff[2] = (bmm150_data_buff[2] >> 1);

            *p_read_buff = (float)bmm150_data_buff[0];
            *(p_read_buff+1) = (float)bmm150_data_buff[1];
            *(p_read_buff+2) = (float)bmm150_data_buff[2];

            memset(bmm150_data_buff, 0, bmm150_buff_len);

            bmm150_data_run = true;
        }
#else        
        bmm150_set_pwr_mode(BMM150_PWR_FORCAD_MODE);
        BMM150_ReadRegisterValue(BMM150_REGISTER_X_MAG_LSB, (uint8 *)bmm150_data_buff, 6);


        bmm150_data_buff[0] = (bmm150_data_buff[0] >> 3);
        bmm150_data_buff[1] = (bmm150_data_buff[1] >> 3);
        bmm150_data_buff[2] = (bmm150_data_buff[2] >> 1);

        *p_read_buff = (float)bmm150_data_buff[0];
        *(p_read_buff+1) = (float)bmm150_data_buff[1];
        *(p_read_buff+2) = (float)bmm150_data_buff[2];

        memset(bmm150_data_buff, 0, bmm150_buff_len);

        bmm150_data_run = true;
#endif


    }
    
}