#include "ms5607.h"
#include "i2c.h"
#include  "filter.h"
#include "anemometer_dev.h"
#include "uart_dev.h"

static void MS56XX_Reset(void)
{
    uint8_t _cmd[] = {0x1e};
    HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _cmd, 1, 0xff);
    HAL_Delay(5);
}

static void MS56XX_Read_PromData(uint16_t *VAL_C)
{
    uint8_t _cmd[] = {0xa0};
    uint8_t temp_buff[2] = {0};
    for(uint8_t i = 0; i < 8; i++)
    {
        HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _cmd, 1, 0xff);
        HAL_I2C_Master_Receive(&hi2c3, MS5607_ADDRESS, temp_buff, 2, 0xff);
        *(VAL_C + i) = (temp_buff[0] << 8)|temp_buff[1];
        _cmd[0] += 2;
        osDelay(1000);
    }
    
}

static uint16_t MS56XX_C_Value[8] = {0};
void MS56XX_Init(void)
{
    MS56XX_Reset();
    MS56XX_Read_PromData(MS56XX_C_Value);
}

static uint32_t MS56XX_ReadD1_Press(void)
{
    uint8_t D1_Value_Buff[3] = {0xFF, 0xFF, 0xFF};
    uint8_t _cmd[] = {0x48};
    uint8_t _addr[] = {0x00};
    HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _cmd, 1, 0xff);
    osDelay(10);
    HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _addr, 1, 0xff);
    HAL_I2C_Master_Receive(&hi2c3, MS5607_ADDRESS, D1_Value_Buff, 3, 0xff);
    
    uint32_t press;
    press = D1_Value_Buff[0];
    press <<= 8;
    press += D1_Value_Buff[1];
    press <<= 8;
    press += D1_Value_Buff[2];
    term_printf("%x",press);
    return press;
}

static uint32_t MS56XX_ReadD2_Temp(void)
{
    uint8_t D2_Value_Buff[3] = {0x00, 0x00, 0x00};
    uint8_t _cmd[] = {0x58};
    uint8_t _addr[] = {0x00};
    HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _cmd, 1, 0xff);
    osDelay(10);
    HAL_I2C_Master_Transmit(&hi2c3, MS5607_ADDRESS, _addr, 1, 0xff);
    HAL_I2C_Master_Receive(&hi2c3, MS5607_ADDRESS, D2_Value_Buff, 3, 0xff);
    
    uint32_t Temp;
    Temp = D2_Value_Buff[0];
    Temp <<= 8;
    Temp += D2_Value_Buff[1];
    Temp <<= 8;
    Temp += D2_Value_Buff[2];
    return Temp;
}

static void MS56XX_GetTemperature (int32_t *dT, int32_t *MS56XX_Temperature)     //计算温度
{
    uint32_t D2_Value = MS56XX_ReadD2_Temp();      //循环读取 D2
    
    if(D2_Value > ((uint32_t)MS56XX_C_Value[5] * (0x00000001 << 8) ))
    {
        *dT  = D2_Value - (( uint32_t )MS56XX_C_Value[5] * (0x00000001 << 8));  //公式 dT = D2 - TREF = D2 - C5 * 2^8
    }
    else
    {
        *dT  = (( uint32_t ) MS56XX_C_Value[5] * (0x00000001 << 8) ) - D2_Value;     
        *dT *= -1;
    }
    *MS56XX_Temperature = 2000 + ( *dT * MS56XX_C_Value[6] ) / (0x00000001 << 23); //算出温度值的100倍，2001表示20.01°  公式TEMP =20°C + dT * TEMPSENS =2000 + dT * C6 / 2^23

    if(*MS56XX_Temperature < -6000) 
    {
        *MS56XX_Temperature=-6000;
    }
    if(*MS56XX_Temperature > 8500) 
    {
        *MS56XX_Temperature=8500;
    }
}

uint8_t MS56XX_GetPressureTemp (float *Temp, float *Press)  //计算温度补偿压力
{
    int32_t dT;
    int32_t MS56XX_Temperature;
    uint32_t D1_Value = MS56XX_ReadD1_Press();      //循环读取 D1
    
    MS56XX_GetTemperature(&dT, &MS56XX_Temperature);//读取dT，Temp
    
    int64_t OFF =  ((int64_t)MS56XX_C_Value[2] * (0x00000001 << 17)) + ((int64_t)MS56XX_C_Value[4] * dT )/ 64.0;    //公式 OFF = OFFT1 + TCO *dT = C2 *2^17 +(C4 *dT )/ 2^6
    int64_t SENS = ((int64_t)MS56XX_C_Value[1] * (0x00000001 << 16)) + ((int64_t)MS56XX_C_Value[3] * dT )/ 128.0; //公式SENS = SENST1 + TCS* dT= C1 * 2^16 + (C3 * dT )/ 2^7

    //温度补偿部分    逻辑看芯片手册   
    int32_t T2;
    int64_t OFF2;
    int64_t SENS2;
    if (MS56XX_Temperature < 2000 ) // second order temperature compensation when under 20 degrees C
    {
        T2 = ( dT * dT ) / (( uint64_t )0x0000000001 << 31);
        OFF2 = 61 *  (( MS56XX_Temperature - 2000 ) * ( MS56XX_Temperature - 2000 )) / 16;
        SENS2 = 2 * (( MS56XX_Temperature - 2000 ) * ( MS56XX_Temperature - 2000 )) ;
        if ( MS56XX_Temperature < -1500 )
        {
            OFF2 = OFF2 + 15 * (( MS56XX_Temperature + 1500 ) * ( MS56XX_Temperature + 1500 ));
            SENS2 = SENS2 + 8 * (( MS56XX_Temperature + 1500 ) * ( MS56XX_Temperature + 1500 ));
        }
    }
    else        //(Temperature > 2000)
    {
        T2 = 0;
        OFF2 = 0;
        SENS2 = 0;
    }

    MS56XX_Temperature = MS56XX_Temperature - T2;
    OFF = OFF - OFF2;
    SENS = SENS - SENS2;
    
    
    int32_t Tmp_Pressure = (D1_Value * SENS / (0x00000001 << 21) - OFF ) / (0x00000001 << 15); //公式 P = D1 * SENS - OFF = (D1 * SENS / 2^21 - OFF) / 2^15

    *Temp = ((float)MS56XX_Temperature)/100;          //单位  ℃     
    *Press = ((float)Tmp_Pressure)/100;           //单位  mbar 
    term_printf("%f", *Press);

    if(*Temp<-60) 
    {
        *Temp=-60;
        return 0;
    }
    if(*Temp>85)
    {
        *Temp=85;
        return 0;
    }
    if(*Press<10)
    {
        *Press=10;
        return 0;
    }
    if(*Press>1200)
    {
        *Press=1200;
        return 0;
    }
    return 1;
}