#include "MSE5611.h"

#define MSE5611_Slave 0xEC
//#define MSE5611_Slave 0xEE

#define MSE5611_Reset 0x1E
#define Convert_D1_256 0x40
#define Convert_D1_512 0x42
#define Convert_D1_1024 0x44
#define Convert_D1_2048 0x46
#define Convert_D1_4096 0x48
#define Convert_D2_256 0x50
#define Convert_D2_512 0x52
#define Convert_D2_1024 0x54
#define Convert_D2_2048 0x56
#define Convert_D2_4096 0x58
#define ADC_Read 0x00

#define PROM_Read_0 0xA0
#define PROM_Read_1 0xA2
#define PROM_Read_2 0xA4
#define PROM_Read_3 0xA6
#define PROM_Read_4 0xA8
#define PROM_Read_5 0xAA
#define PROM_Read_6 0xAC
#define PROM_Read_7 0xAE

#define Sea_Level_Pressure 101325.0
uint16_t Reserve = 0;
uint16_t CFG[6] = {0};
uint16_t MSE5611_CRC = 0;
uint32_t D[2] = {0};

int32_t Temp = 0;
int32_t Press = 0;
float Height = 0;

int MSE5611_register_write_len(uint8_t addr,uint8_t register_address, uint8_t len,uint8_t *buf)
{
	uint8_t w2_data[50],i;
	w2_data[0] = register_address;
	for(i=0;i<len;i++){
		w2_data[i+1] = *(buf+i);
	}
	if(twi_master_transfer(addr, w2_data, len+1, TWI_ISSUE_STOP) == true) return 0;
	else return -1;
}

int MSE5611_register_read_len(uint8_t addr,uint8_t register_address, uint8_t number_of_bytes,uint8_t * destination )
{
    bool transfer_succeeded;
    transfer_succeeded  = twi_master_transfer(addr, &register_address, 1, TWI_DONT_ISSUE_STOP);
    transfer_succeeded &= twi_master_transfer(addr|TWI_READ_BIT, destination, number_of_bytes, TWI_ISSUE_STOP);
    
	  if(transfer_succeeded == true)return 0;
	  else return -1;
}

void Reset_MSE5611(void)
{
	uint8_t dat = 0;
	//I2C_WriteByte(I2C1,MSE5611_Slave,MSE5611_Reset); 
	MSE5611_register_write_len(MSE5611_Slave, MSE5611_Reset, 0, &dat);
	nrf_delay_ms(20);
}
void ReadPressure_Pre(void)
{
	uint8_t dat = 0;
	//I2C_WriteByte(I2C1,MSE5611_Slave,Convert_D1_4096);
	MSE5611_register_write_len(MSE5611_Slave, Convert_D1_256, 0, &dat);
}
uint32_t ReadPressure(void)
{
	uint8_t dd[3] = {0,0,0};
	uint8_t R = 0;
	
	MSE5611_register_read_len(MSE5611_Slave,R,3,dd);
	D[0] = ((uint32_t)(dd[0]<<16)|(dd[1]<<8)|(dd[2]<<0));
//	printf("D[0]=%d\r\n",D[0]);
	
	return D[0];
	//I2C_ReadBytes(I2C1,MSE5611_Slave,ADC_Read,3,(uint8_t*)D,MSB);
//	MSE5611_register_read_len(MSE5611_Slave,&R,3,(uint8_t*)&D[0]);
}
void Single_Read_Pressure(void)
{
	ReadPressure_Pre();
	nrf_delay_ms(2);
	ReadPressure();
}
void ReadTemp_Pre(void)
{
	uint8_t dat = 0;
	//I2C_WriteByte(I2C1,MSE5611_Slave,Convert_D2_4096);
	MSE5611_register_write_len(MSE5611_Slave, Convert_D2_4096, 0, &dat);
}
void ReadTemp(void)
{
	//I2C_ReadBytes(I2C1,MSE5611_Slave,ADC_Read,3,(uint8_t*)( D + 1 ),MSB);
	MSE5611_register_read_len(MSE5611_Slave,ADC_Read,3,(uint8_t*)&D[1]);
	printf("T:%d\n",D[1]);
}
void Single_Read_Temp(void) 
{
	ReadTemp_Pre();
	nrf_delay_ms(2);
	ReadTemp();
}
float Pressure_Caculate(void)
{
	int64_t OFF = 0,OFF2 = 0,SENS = 0, SENS2 = 0,T2 = 0;
	int64_t dT = 0;
	dT = D[1] - (((int64_t)CFG[4]) << 8);
	Temp = 2000 + (((int64_t)dT * CFG[5])>>23);
	if(Temp < 2000)
	{
		T2 = ( dT*dT )>>31;
		OFF2 = 5 * (Temp - 2000) * (Temp - 2000) >> 1;
		SENS2 = 5 * (Temp - 2000) * (Temp - 2000) >> 2;
	}
	if(Temp < -1500)
	{
		OFF2 = OFF2 + 7 * (Temp + 1500) * (Temp + 1500);
		SENS2 = SENS2 + ( 11 * (Temp +1500) * (Temp +1500) >> 1 );
	}
	OFF = (((int64_t)CFG[1]) << 16) + ((CFG[3] * dT) >> 7);
	SENS = (((int64_t)CFG[0]) << 15) + ((CFG[2] * dT) >> 8);
	OFF -= OFF2;
	Temp -= T2;
	SENS -= SENS2;
	Press = (((D[0] * SENS)>>21) - OFF)>>15;
	Height = 44330 * (1 - pow(Press/Sea_Level_Pressure ,1 / 5.225));
	return Height;
}
void MSE5611_Config(void)
{
	uint8_t i;
	uint8_t dd[2];
	
	Reset_MSE5611();
	
	MSE5611_register_read_len(MSE5611_Slave,PROM_Read_0,2,dd);
	Reserve = ((uint16_t)dd[0]<<8|dd[1]);
	printf("Reserve=%x\r\n",Reserve);
	
	for(i=0;i<6;i++){
		MSE5611_register_read_len(MSE5611_Slave,PROM_Read_1+i*2,2,dd);
		CFG[i] = ((uint16_t)dd[0]<<8 | dd[1]);
		printf("CFG[%d]=%x\r\n",i,CFG[i]);
	}
	
	MSE5611_register_read_len(MSE5611_Slave,PROM_Read_7,2,dd);
	MSE5611_CRC = ((uint16_t)dd[0]<<8|dd[1]);
	printf("MSE5611_CRC=%x\r\n",MSE5611_CRC);
}