#include "ak9757w.h"
#include "ak9757w_def.h"
#include "app_i2c_master.h"
#include "app_log.h"
#include "sw_timer.h"
#include "app_oled.h"
#include "eeprom_address.h"



#define AK9757W_CHIPID_READ_WAIT_TIME   100
#define AK9757W_ERROR_PRINT_INTERVAL    1000
#define AK9757W_MEASUREMENT_INTERVAL    1000
#define AK9757W_MAGIC_KEY               0xA55A





static AK9757W_HANDLER  AK9757W_Handler;
static AK9757W_HANDLER* pHandler = &AK9757W_Handler; 
static PARAMETER_INFO_BUFF  ParameterInfo;

const uint8_t default_cntl_reg_data[] = {0x20, 0xFF, 0xFE, 0x4A, 0xE1, 0x00, 0x00, 0x00, 0xDF, 0xE0};
//const uint8_t default_cntl_reg_data[] = {0x20, 0xFF, 0xFE, 0x42, 0xE1, 0x00, 0x00, 0x00, 0xDF, 0xE0};
const uint8_t default_fcoef_reg_data[] = {0x29, 0xF3, 0x57, 0x30, 0x6B, 0xBE, 0x21, 0x61, 0x8D, 0x10, 0x8D, 0xBF, 0x01, 0x0C, 0xFE};
const uint8_t default_gcoef_reg_data[] = {0x37, 0x7D, 0x48, 0x40, 0xF1, 0xB7, 0x33, 0x83, 0x5C, 0x20, 0x40, 0xAB, 0x12, 0x70, 0x40};
const uint8_t default_xcoef_reg_data[] = {0x45, 0xC7, 0xA0, 0x32, 0x78, 0x42, 0x23, 0x75, 0x92, 0x16, 0xC5, 0x42, 0x08, 0x15, 0x60};
const uint8_t default_gir_oir_gts_ots_git_reg_data[] = {0x53, 0xE0, 0x94, 0x12, 0x00, 0x00, 0x0D, 0x14, 0x00, 0x00};
//const uint8_t default_gir_oir_gts_ots_git_reg_data[] = {0x53, 0xE0, 0x1C, 0x0E, 0x00, 0x00, 0xE1, 0x2F, 0x02, 0x00};




static bool AK9757W_Check_CompanyCode(void);
static bool AK9757W_Set_Operation_Mode(AK9757W_MODE mode);
static bool AK9757W_Set_ADC_Inverter(AK9757W_MODE mode);
static bool AK9757W_Set_CNTL1_9_Parameter(void);
static bool AK9757W_Set_FCOEF4_0_Parameter(void);
static bool AK9757W_Set_GCOEF4_0_Parameter(void);
static bool AK9757W_Set_XCOEF4_0_Parameter(void);
static bool AK9757W_Set_IR_TS_IT_Parameter(void);
static bool AK9757W_Calc_Parameter(void);
static bool AK9757W_Read_RawData(void);
static void AK9757W_Process(void);
static void AK9757W_Next_Step(AK9757W_STEP nextStep);
static uint16_t AK9757W_Get_Tout(uint16_t TOUT);
static int16_t AK9757W_14BitTo16Bit(uint16_t Data);
static int16_t AK9757W_12BitTo16Bit(uint16_t Data);
static int8_t AK9757W_6BitTo8it(uint8_t Data);







bool AK9757W_Initialization(void)
{

    AK9757W_Handler.PreStep = STEP_INIT;
    AK9757W_Handler.Step = STEP_INIT;
    AK9757W_Handler.isMeasurementStart = true;
    AK9757W_Handler.UpdateTime_Millisec = AK9757W_MEASUREMENT_INTERVAL;

    SW_Timer_Callback_Register(SW_TIMER_RUN_CONTINUE, 1, AK9757W_Process);
    return true;
}





static void AK9757W_Process(void)
{
    switch(pHandler->Step)
    {
        case STEP_INIT:
            AK9757W_Handler.isInitComplete = false;
            AK9757W_Handler.SensorID = 0xFFFFFFFF;
            AK9757W_Next_Step(STEP_CHECK_COMPANY);
        break;

        case STEP_CHECK_COMPANY:
            if(AK9757W_Check_CompanyCode() == false){
                AK9757W_Next_Step(STEP_ERROR);      
            }else{
                AK9757W_Next_Step(STEP_CHECK_ID_POWER_ON);      
            }
        break;
        case STEP_CHECK_ID_POWER_ON:
            if(AK9757W_Set_Mode(AK9757W_MODE_CONTINUOUS_MODE) == false){
                AK9757W_Next_Step(STEP_ERROR);
            }else{
                pHandler->TickCount = millis();
                AK9757W_Next_Step(STEP_CHECK_ID_POWER_ON_WAIT);
            }
        break;
        case STEP_CHECK_ID_POWER_ON_WAIT:
            if((millis() - pHandler->TickCount) >= AK9757W_CHIPID_READ_WAIT_TIME)
                AK9757W_Next_Step(STEP_CHECK_ID_READ);
        break;
        case STEP_CHECK_ID_READ:
            if(AK9757W_Get_SensorID() == false){
                AK9757W_Next_Step(STEP_ERROR);
            }
            else{
                AK9757W_Next_Step(STEP_CHECK_ID_POWER_OFF);
            }    
        break;
        case STEP_CHECK_ID_POWER_OFF:
            if(AK9757W_Set_Mode(AK9757W_MODE_STAND_BY_MODE) == false){
                AK9757W_Next_Step(STEP_ERROR);
            }
            else{                
                AK9757W_Next_Step(STEP_WRITE_CNTL1_9);
            }
        break;
        case STEP_WRITE_CNTL1_9:
            if(AK9757W_Set_CNTL1_9_Parameter() == false){
                AK9757W_Next_Step(STEP_ERROR);
            }
            else{
                AK9757W_Next_Step(STEP_WRITE_FCOEF4_0);
            }
        break;
        case STEP_WRITE_FCOEF4_0:
            if(AK9757W_Set_FCOEF4_0_Parameter() == false){
                AK9757W_Next_Step(STEP_ERROR);
            }
            else{
                AK9757W_Next_Step(STEP_WRITE_GCOEF4_0);
            }
        break;
        case STEP_WRITE_GCOEF4_0:
            if(AK9757W_Set_GCOEF4_0_Parameter() == false){
                AK9757W_Next_Step(STEP_ERROR);
            }
            else{
                AK9757W_Next_Step(STEP_WRITE_XCOEF4_0);
            }
        break;
        case STEP_WRITE_XCOEF4_0:
            if(AK9757W_Set_XCOEF4_0_Parameter() == false){
                AK9757W_Next_Step(STEP_ERROR);
            }
            else{
                AK9757W_Next_Step(STEP_WRITE_IR_TS_IT);
            }
        break;
        case STEP_WRITE_IR_TS_IT:
            if(AK9757W_Set_IR_TS_IT_Parameter() == false){
                AK9757W_Next_Step(STEP_ERROR);
            }
            else{
                AK9757W_Next_Step(STEP_CALC_PARAMETER);
            }
        break;
        case STEP_CALC_PARAMETER:
            AK9757W_Calc_Parameter();
            AK9757W_Next_Step(STEP_WAIT_START);
        break;
        case STEP_WAIT_START:
            if(pHandler->isMeasurementStart == true)
                AK9757W_Next_Step(STEP_MEASUREMENT_START);
        break;
        case STEP_MEASUREMENT_START:
            if(AK9757W_Set_Mode(AK9757W_MODE_CONTINUOUS_MODE) == false){
                AK9757W_Next_Step(STEP_ERROR);
            }
            else{                
                pHandler->TickCount = millis();
                AK9757W_Next_Step(STEP_MEASUREMENT);
            }
        break;
        case STEP_MEASUREMENT:
            if((millis() - pHandler->TickCount) >= pHandler->UpdateTime_Millisec)
            {
                pHandler->TickCount = millis();
                AK9757W_Read_RawData();
            }
        break;


        case STEP_WAIT_STOP:
            if(AK9757W_Set_Mode(AK9757W_MODE_STAND_BY_MODE) == false){
                pHandler->isMeasurementStart = false;
                AK9757W_Next_Step(STEP_ERROR);
            }
            else{                
                pHandler->isMeasurementStart = false;
                AK9757W_Next_Step(STEP_CALC_PARAMETER);
            }
        break;

        
        case STEP_ERROR:
            if((millis() - pHandler->TickCount) >= AK9757W_ERROR_PRINT_INTERVAL)
            {
                pHandler->TickCount = millis();
                dbg_printf(LOG_LEVEL_DEBUG, "ak975f7w Step Error %d\r\n", pHandler->PreStep);
            }
            
        break;
    }

}

















static bool AK9757W_Check_CompanyCode(void)
{
    int error;
    uint8_t TxBuff;
    uint8_t RxBuff[10];
    
    TxBuff = AK9757W_REG_RO_COMPANY_CODE;
    error = I2C_Master_Write(TEMP_I2C_ADDRESS, &TxBuff, 1);
    if(error != E_NO_ERROR)
    {
        dbg_printf(LOG_LEVEL_DEBUG, "I2C Write Error %d\r\n", error);
        return false;
    }    

    error = I2C_Master_Read(TEMP_I2C_ADDRESS, &RxBuff[0], 2);
    if(error != E_NO_ERROR)
    {
        dbg_printf(LOG_LEVEL_DEBUG, "I2C Read Error %d\r\n", error);
        return false;
    }

    if(!(RxBuff[0] == AK9757W_COMPANY_CODE_VALUE && RxBuff[1] == AK9757W_DEVICE_ID_VALUE)) 
    {
        dbg_printf(LOG_LEVEL_DEBUG, "Read Data %X, %X\r\n", RxBuff[0], RxBuff[1]);
        return false;
    }
    
    dbg_printf(LOG_LEVEL_DEBUG, "Company %XH, %XH\r\n", RxBuff[0], RxBuff[1]);
    return true;
}


bool AK9757W_Set_Mode(AK9757W_MODE mode)
{
    int error;
    uint8_t TxBuff[10];
    uint8_t RxBuff[10];

    TxBuff[0] = AK9757W_REG_RW_CNTL9;    
    error = I2C_Master_WriteRead(TEMP_I2C_ADDRESS, &TxBuff[0], 1, &RxBuff[0], 1);    
    if(error != E_NO_ERROR)
    {
        dbg_printf(LOG_LEVEL_DEBUG, "I2C Read Error %d\r\n", error);
        return false;
    }

    //#define CLEARBIT(ADDRESS,BIT)	    (ADDRESS &= ~(1<<BIT))
    dbg_printf(LOG_LEVEL_DEBUG, "read data %x\r\n", RxBuff[0]);
    TxBuff[1] = RxBuff[0] & ~(AK9757W_CNTL9_MODE_MSK << AK9757W_CNTL9_MODE_POS);
    TxBuff[1] |= (mode << AK9757W_CNTL9_MODE_POS);    
    dbg_printf(LOG_LEVEL_DEBUG, "write data %x\r\n", TxBuff[1]);

    error = I2C_Master_Write(TEMP_I2C_ADDRESS, &TxBuff[0], 2);
    if(error != E_NO_ERROR)
    {
        dbg_printf(LOG_LEVEL_DEBUG, "I2C write Error %d\r\n", error);
        return false;
    }
    return true;
}

bool AK9757W_Set_AD_OutputDataTyte(AK9757W_IR_ADC_INVERT type)
{

}


static bool AK9757W_Set_CNTL1_9_Parameter(void)
{
    int error;
    uint8_t i;    
    
    memcpy(&pHandler->Cntl_Info.CNTL_Buff[0], &default_cntl_reg_data[1], (sizeof(default_cntl_reg_data)-1));
    error = I2C_Master_Write(TEMP_I2C_ADDRESS, &default_cntl_reg_data[0], sizeof(default_cntl_reg_data));
    if(error != E_NO_ERROR)
    {
        dbg_printf(LOG_LEVEL_DEBUG, "I2C CTL1_9 write Error %d\r\n", error);
        return false;
    }

    

    
    return true;    
}

static bool AK9757W_Set_FCOEF4_0_Parameter(void)
{
    int error;
    uint8_t i;    
    uint8_t RxBuff[20];


    memcpy(&pHandler->Fcoef_Info.FCOEF_Buff[0], &default_fcoef_reg_data[1], (sizeof(default_fcoef_reg_data)-1));
    error = I2C_Master_Write(TEMP_I2C_ADDRESS, &default_fcoef_reg_data[0], sizeof(default_fcoef_reg_data));
    if(error != E_NO_ERROR)
    {
        dbg_printf(LOG_LEVEL_DEBUG, "I2C FCOEF4_0 write Error %d\r\n", error);
        return false;
    }

    dbg_printf(LOG_LEVEL_DEBUG, "I2C FCOEF4_0 write Success\r\n");
    return true;    
}

static bool AK9757W_Set_GCOEF4_0_Parameter(void)
{
    int error;
    uint8_t i;    
    memcpy(&pHandler->Gcoef_Info.GCOEF_Buff[0], &default_gcoef_reg_data[1], (sizeof(default_gcoef_reg_data)-1));
    error = I2C_Master_Write(TEMP_I2C_ADDRESS, &default_gcoef_reg_data[0], sizeof(default_gcoef_reg_data));
    if(error != E_NO_ERROR)
    {
        dbg_printf(LOG_LEVEL_DEBUG, "I2C GCOEF4_0 write Error %d\r\n", error);
        return false;
    }
    dbg_printf(LOG_LEVEL_DEBUG, "I2C GCOEF4_0 write Success\r\n");
    return true;    
}

static bool AK9757W_Set_XCOEF4_0_Parameter(void)
{
    int error;
    uint8_t i;    
    memcpy(&pHandler->Xcoef_Info.XCOEF_Buff[0], &default_xcoef_reg_data[1], (sizeof(default_xcoef_reg_data)-1));
    error = I2C_Master_Write(TEMP_I2C_ADDRESS, &default_xcoef_reg_data[0], sizeof(default_xcoef_reg_data));
    if(error != E_NO_ERROR)
    {
        dbg_printf(LOG_LEVEL_DEBUG, "I2C XCOEF4_0 write Error %d\r\n", error);
        return false;
    }
    dbg_printf(LOG_LEVEL_DEBUG, "I2C XCOEF4_0 write Success\r\n");
    return true;    
}

static bool AK9757W_Set_IR_TS_IT_Parameter(void)
{
    int error;
    uint8_t i;    

    Eeprom_Get_Parameter(&ParameterInfo);
    if(ParameterInfo.Parameter_Info.isSaveParameter == AK9757W_MAGIC_KEY)
    {
        uint16_t CheckSum = 0;
        uint32_t readChipID;

        readChipID = (uint32_t)(ParameterInfo.Parameter_Info.Parameter_ID_H << 16) | (uint32_t)ParameterInfo.Parameter_Info.Parameter_ID_L;

        for(i = 0 ; i < EE_INDEX_CHECKSUM ; i++)
        {
            CheckSum += ParameterInfo.Parameter_Buff[i];
        }

        if(ParameterInfo.Parameter_Info.CheckSum == CheckSum && readChipID == pHandler->SensorID)
        {       
            
            for(i = 0 ; i < 9 ; i ++)
            {
                pHandler->IR_TS_IT_Info.IR_TS_IT_Buff[1+i] = ParameterInfo.Parameter_Buff[EE_INDEX_PA_53 + i];
            }
            dbg_printf(LOG_LEVEL_DEBUG, "Parameter write eeprom\r\n");
        }
        else
        {
            memcpy(&pHandler->IR_TS_IT_Info.IR_TS_IT_Buff[0], &default_gir_oir_gts_ots_git_reg_data[0], (sizeof(default_gir_oir_gts_ots_git_reg_data)));        
        }
    }
    else
    {
        memcpy(&pHandler->IR_TS_IT_Info.IR_TS_IT_Buff[0], &default_gir_oir_gts_ots_git_reg_data[0], (sizeof(default_gir_oir_gts_ots_git_reg_data)));        
    }
    
    
    pHandler->IR_TS_IT_Info.IR_TS_IT_Buff[0] = AK9757W_REG_PARAMETER;
    pHandler->IR_TS_IT_Info.IR_TS_IT_Buff[1] = 0xE0;    // default value

    error = I2C_Master_Write(TEMP_I2C_ADDRESS, &pHandler->IR_TS_IT_Info.IR_TS_IT_Buff[0], sizeof(pHandler->IR_TS_IT_Info.IR_TS_IT_Buff));
    if(error != E_NO_ERROR)
    {
        dbg_printf(LOG_LEVEL_DEBUG, "I2C IR_TS_IT write Error %d\r\n", error);
        return false;
    }
    dbg_printf(LOG_LEVEL_DEBUG, "I2C IR_TS_IT write Success\r\n");
    return true;    
}


static bool AK9757W_Calc_Parameter(void)
{
    int error;
    uint8_t uint8_TempValue;
    int8_t int8_TempValue;
    uint16_t uint16_TempValue;    
    int16_t int16_TempValue;
    uint8_t TxData;
    uint8_t* pRxBuff;


    TxData = AK9757W_REG_PARAMETER;
    pRxBuff = &pHandler->IR_TS_IT_Info.IR_TS_IT_Buff[0];
    error = I2C_Master_WriteRead(TEMP_I2C_ADDRESS, &TxData, 1, pRxBuff, 9);
    if(error != E_NO_ERROR)
        return false;

    TxData = AK9757W_REG_RW_FCOEF4L;
    pRxBuff = &pHandler->Fcoef_Info.FCOEF_Buff[0];
    error = I2C_Master_WriteRead(TEMP_I2C_ADDRESS, &TxData, 1, pRxBuff, 14);
    if(error != E_NO_ERROR)
        return false;


    TxData = AK9757W_REG_RW_GCOEF4L;
    pRxBuff = &pHandler->Gcoef_Info.GCOEF_Buff[0];
    error = I2C_Master_WriteRead(TEMP_I2C_ADDRESS, &TxData, 1, pRxBuff, 14);
    if(error != E_NO_ERROR)
        return false;


    TxData = AK9757W_REG_RW_XCOEF4L;
    pRxBuff = &pHandler->Xcoef_Info.XCOEF_Buff[0];
    error = I2C_Master_WriteRead(TEMP_I2C_ADDRESS, &TxData, 1, pRxBuff, 14);
    if(error != E_NO_ERROR)
        return false;



    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    pHandler->calc_gts = ((double)((int8_t)pHandler->IR_TS_IT_Info.IR_TS_IT.gts) * (double)TWO_POW_M10);
    
    uint16_TempValue = (uint16_t)(pHandler->IR_TS_IT_Info.IR_TS_IT.ots_h << 8) | (uint16_t)pHandler->IR_TS_IT_Info.IR_TS_IT.ots_l;
    pHandler->calc_ots = AK9757W_14BitTo16Bit(uint16_TempValue);

    pHandler->calc_git = (double)((double)((int8_t)pHandler->IR_TS_IT_Info.IR_TS_IT.git) * (double)TWO_POW_M23);

    uint16_TempValue = (uint16_t)(pHandler->IR_TS_IT_Info.IR_TS_IT.gir_h << 8) | pHandler->IR_TS_IT_Info.IR_TS_IT.gir_l;
    pHandler->calc_gir = (double)((double)uint16_TempValue* (double)TWO_POW_M14);

    uint16_TempValue = (uint16_t)(pHandler->IR_TS_IT_Info.IR_TS_IT.oir_h << 8) | pHandler->IR_TS_IT_Info.IR_TS_IT.oir_l;
    pHandler->calc_oir = AK9757W_12BitTo16Bit(uint16_TempValue);




    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    int16_TempValue = (pHandler->Fcoef_Info.FCOEF.fcoef4_h << 8) | pHandler->Fcoef_Info.FCOEF.fcoef4_l;
    uint8_TempValue = pHandler->Fcoef_Info.FCOEF.fcoef4_ex;
    pHandler->calc_FC4 = (double)int16_TempValue * pow(2, ((15+uint8_TempValue) * -1));

    int16_TempValue = (pHandler->Fcoef_Info.FCOEF.fcoef3_h << 8) | pHandler->Fcoef_Info.FCOEF.fcoef3_l;
    uint8_TempValue = pHandler->Fcoef_Info.FCOEF.fcoef3_ex;
    pHandler->calc_FC3 = (double)int16_TempValue * pow(2, ((15+uint8_TempValue) * -1));

    int16_TempValue = (pHandler->Fcoef_Info.FCOEF.fcoef2_h << 8) | pHandler->Fcoef_Info.FCOEF.fcoef2_l;
    uint8_TempValue = pHandler->Fcoef_Info.FCOEF.fcoef2_ex & 0x3F;
    pHandler->calc_FC2 = (double)int16_TempValue * pow(2, ((14+uint8_TempValue) * -1));

    int16_TempValue = (pHandler->Fcoef_Info.FCOEF.fcoef1_h << 8) | pHandler->Fcoef_Info.FCOEF.fcoef1_l;
    uint8_TempValue = pHandler->Fcoef_Info.FCOEF.fcoef1_ex & 0x1F;
    pHandler->calc_FC1 = (double)int16_TempValue * pow(2, ((14+uint8_TempValue) * -1));

    int16_TempValue = (pHandler->Fcoef_Info.FCOEF.fcoef0_h << 8) | pHandler->Fcoef_Info.FCOEF.fcoef0_l;
    pHandler->calc_FC0 = (double)int16_TempValue;




    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    int16_TempValue = (pHandler->Gcoef_Info.GCOEF.gcoef4_h << 8) | pHandler->Gcoef_Info.GCOEF.gcoef4_l;
    uint8_TempValue = pHandler->Gcoef_Info.GCOEF.gcoef4_ex;
    pHandler->calc_GC4 = (double)int16_TempValue * pow(2, ((15+uint8_TempValue) * -1));

    int16_TempValue = (pHandler->Gcoef_Info.GCOEF.gcoef3_h << 8) | pHandler->Gcoef_Info.GCOEF.gcoef3_l;
    uint8_TempValue = pHandler->Gcoef_Info.GCOEF.gcoef3_ex;
    pHandler->calc_GC3 = (double)int16_TempValue * pow(2, ((15+uint8_TempValue) * -1));

    int16_TempValue = (pHandler->Gcoef_Info.GCOEF.gcoef2_h << 8) | pHandler->Gcoef_Info.GCOEF.gcoef2_l;
    uint8_TempValue = pHandler->Gcoef_Info.GCOEF.gcoef2_ex & 0x3F;
    pHandler->calc_GC2 = (double)int16_TempValue * pow(2, ((15+uint8_TempValue) * -1));

    int16_TempValue = (pHandler->Gcoef_Info.GCOEF.gcoef1_h << 8) | pHandler->Gcoef_Info.GCOEF.gcoef1_l;
    uint8_TempValue = pHandler->Gcoef_Info.GCOEF.gcoef1_ex & 0x1F;
    pHandler->calc_GC1 = (double)int16_TempValue * pow(2, ((14+uint8_TempValue) * -1));

    int16_TempValue = (pHandler->Gcoef_Info.GCOEF.gcoef0_h << 8) | pHandler->Gcoef_Info.GCOEF.gcoef0_l;
    pHandler->calc_GC0 = (double)int16_TempValue * TWO_POW_M14;



    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    int16_TempValue = (pHandler->Xcoef_Info.XCOEF.xcoef4_h << 8) | pHandler->Xcoef_Info.XCOEF.xcoef4_l;
    uint8_TempValue = pHandler->Xcoef_Info.XCOEF.xcoef4_ex;
    pHandler->calc_XC4 = (double)int16_TempValue * pow(2, ((15+uint8_TempValue) * -1));

    int16_TempValue = (pHandler->Xcoef_Info.XCOEF.xcoef3_h << 8) | pHandler->Xcoef_Info.XCOEF.xcoef3_l;
    uint8_TempValue = pHandler->Xcoef_Info.XCOEF.xcoef3_ex;
    pHandler->calc_XC3 = (double)int16_TempValue * pow(2, ((15+uint8_TempValue) * -1));

    int16_TempValue = (pHandler->Xcoef_Info.XCOEF.xcoef2_h << 8) | pHandler->Xcoef_Info.XCOEF.xcoef2_l;
    uint8_TempValue = pHandler->Xcoef_Info.XCOEF.xcoef2_ex & 0x3F;
    pHandler->calc_XC2 = (double)int16_TempValue * pow(2, ((15+uint8_TempValue) * -1));

    int16_TempValue = (pHandler->Xcoef_Info.XCOEF.xcoef1_h << 8) | pHandler->Xcoef_Info.XCOEF.xcoef1_l;
    uint8_TempValue = pHandler->Xcoef_Info.XCOEF.xcoef1_ex & 0x1F;
    pHandler->calc_XC1 = (double)int16_TempValue * pow(2, ((14+uint8_TempValue) * -1));

    uint16_TempValue = (pHandler->Xcoef_Info.XCOEF.xcoef0_h << 8) | pHandler->Xcoef_Info.XCOEF.xcoef0_l;
    pHandler->calc_XC0 = (double)uint16_TempValue * TWO_POW_M10;








}


static bool AK9757W_Read_RawData(void)
{
    int error;    
    uint8_t TxData = AK9757W_REG_RO_STATUS;
    uint8_t RxBuff[9];
    uint16_t uint16_TempValue;    
    int16_t int16_TempValue;
    uint16_t ToutTemp;

    float fTs;
    float fTout;

    error = I2C_Master_WriteRead(TEMP_I2C_ADDRESS, &TxData, 1, &RxBuff[0], 9);
    if(error != E_NO_ERROR)
    {
        dbg_printf(LOG_LEVEL_DEBUG, "I2C W/R Error %d\r\n", error);
        return false;
    }
    
    pHandler->RAWDATA.Status.Status = RxBuff[0];
    if(pHandler->RAWDATA.Status.Status_Bit.DRDY == true)
    {
        pHandler->RAWDATA.RAW_TO = (RxBuff[2] << 8) | RxBuff[1];    
        pHandler->RAWDATA.RAW_TS = RxBuff[4] << 8 | RxBuff[3];
        pHandler->RAWDATA.RAW_IR = RxBuff[6] << 8 | RxBuff[5];   

        ToutTemp = AK9757W_Get_Tout(pHandler->RAWDATA.RAW_TO);

        //fTs = (-0.1075 * pow(pHandler->RAWDATA.RAW_TS, 3)) + (0.9811 * pow(pHandler->RAWDATA.RAW_TS, 2)) - (3.0914 * pHandler->RAWDATA.RAW_TS) + 29.404;
        //fTs = 26.02 + (pHandler->RAWDATA.RAW_TS * 0.002);

        fTs = -0.000000003402565*pHandler->RAWDATA.RAW_TS*pHandler->RAWDATA.RAW_TS+0.001985926*pHandler->RAWDATA.RAW_TS+26.02365;
        fTout = ToutTemp / 100.0;
        dbg_printf(LOG_LEVEL_DEBUG, "IR = %d, TS = %d, TOUT = %d, Tout = %.2f, Ts = %.2f\r\n", pHandler->RAWDATA.RAW_IR, pHandler->RAWDATA.RAW_TS, pHandler->RAWDATA.RAW_TO, fTout, fTs);
        //dbg_printf(LOG_LEVEL_DEBUG, "IR = %X, TS = %X, TOUT = %X, Tout = %.2f, Ts = %.2f\r\n", pHandler->RAWDATA.RAW_IR, pHandler->RAWDATA.RAW_TS, pHandler->RAWDATA.RAW_TO, fTout, fTs);
        Oled_SetTemperature(ToutTemp);
    }
    
#if 0
    Oled_SetTemperature(pHandler->RAWDATA.RAW_TO/10);
    TS = (1.0 + pHandler->calc_gts) * pHandler->RAWDATA.RAW_TS + pHandler->calc_ots;
    GIR = pHandler->calc_git * TS;
    IR = (pHandler->calc_gir + pHandler->calc_gir * GIR) * pHandler->RAWDATA.RAW_IR + pHandler->calc_oir;
    F_TS = pHandler->calc_FC4 * pow(TS, 4) + pHandler->calc_FC3 * pow(TS, 3) + pHandler->calc_FC2 * pow(TS, 2) + pHandler->calc_FC1 * TS + pHandler->calc_FC0;
    G_TS = pHandler->calc_GC4 * pow(TS, 4) + pHandler->calc_GC3 * pow(TS, 3) + pHandler->calc_GC2 * pow(TS, 2) + pHandler->calc_GC1 * TS + pHandler->calc_GC0;
    IM_IR = (IR - F_TS) * G_TS;
    TO = pHandler->calc_XC4 * pow(IM_IR, 4) + pHandler->calc_XC3 * pow(IM_IR, 3) + pHandler->calc_XC2 * pow(IM_IR, 2) + pHandler->calc_XC1 * IM_IR + pHandler->calc_XC0;

    dbg_printf(LOG_LEVEL_DEBUG, "%d,%d,%d, %f, %f\r\n", pHandler->RAWDATA.RAW_IR, pHandler->RAWDATA.RAW_TS, pHandler->RAWDATA.RAW_TO, TO, TS);    
#endif
    
    return true;
    
}


static uint16_t AK9757W_Get_Tout(uint16_t TOUT)
{
    uint16_t RetTemp = 0;
    uint16_t Temp;
    double dbTemp;
    RetTemp = (TOUT >> 10) * 100;
    Temp = TOUT & 0x3FF;
    dbTemp = (double)Temp / 1000.0;
    RetTemp += (uint16_t)(dbTemp * 100.0);

    return RetTemp;
}


static int16_t AK9757W_14BitTo16Bit(uint16_t Data)
{
    int16_t RetData = 0;
    Data &= 0x3FFF;

    if(Data & 0x2000)
        Data |= 0xC000;
    RetData |= Data;
    return RetData;
}

static int16_t AK9757W_12BitTo16Bit(uint16_t Data)
{
    int16_t RetData = 0;
    Data &= 0x0FFF;

    if(Data & 0x0800)
        Data |= 0xF000;
    RetData |= Data;
    return RetData;
}

static int8_t AK9757W_6BitTo8it(uint8_t Data)
{
    int8_t RetData = 0;
    Data &= 0x3F;

    if(Data & 0x20)
        Data |= 0xC0;
    RetData |= Data;
}
























bool AK9757W_Get_SensorID(void)
{
    int error;
    uint8_t TxBuff;
    uint8_t RxBuff[10];
    
    TxBuff = AK9757W_REG_RO_CHIPID;
    error = I2C_Master_WriteRead(TEMP_I2C_ADDRESS, &TxBuff, 1, &RxBuff[0], 4);    
    if(error != E_NO_ERROR)
    {
        dbg_printf(LOG_LEVEL_DEBUG, "I2C Read Error %d\r\n", error);
        return false;
    }
    AK9757W_Handler.SensorID = (RxBuff[0] << 24) | (RxBuff[1] << 16) | (RxBuff[2] << 8) | (RxBuff[3] << 0);
    dbg_printf(LOG_LEVEL_DEBUG, "Read ChipID %XH\r\n", AK9757W_Handler.SensorID);    

    return true;
}





static void AK9757W_Next_Step(AK9757W_STEP nextStep)
{
    AK9757W_Handler.PreStep = AK9757W_Handler.Step;
    AK9757W_Handler.Step = nextStep;
}






bool AK9757W_Start_Stop(bool isStartStop)
{
    if(isStartStop == true)
    {
        if(pHandler->isMeasurementStart == false)
        {
            pHandler->isMeasurementStart = true;
            AK9757W_Next_Step(STEP_INIT);
        }
    }
    else
    {
        if(pHandler->isMeasurementStart == true)
            AK9757W_Next_Step(STEP_WAIT_STOP);
    }
    return true;
}

bool AK9757W_UpdateTime(uint32_t Millisec)
{
    if(pHandler->isMeasurementStart == true)
        return false;    

    pHandler->UpdateTime_Millisec = Millisec;
    return true;
}


bool AK9757W_ReadReg(uint8_t RegAddr, uint32_t readLen, uint8_t* pReadBuff)
{
    int error;    

    if(pHandler->isMeasurementStart == true)
        return false;    

    error = I2C_Master_WriteRead(TEMP_I2C_ADDRESS, &RegAddr, 1, &pReadBuff[0], readLen);    
    if(error != E_NO_ERROR)
    {        
        return false;
    }    
    return true;
}


bool AK9757W_WriteReg(uint8_t WriteAddr, uint8_t WriteData)
{
    uint8_t TxBuff[2];
    int error;
    
    if(pHandler->isMeasurementStart == true)
        return false;

    TxBuff[0] = WriteAddr;
    TxBuff[1] = WriteData;
    
    error = I2C_Master_Write(TEMP_I2C_ADDRESS, &TxBuff[0], 2);    
    if(error != E_NO_ERROR)
    {        
        return false;
    }    
    return true;
}


uint32_t AK9757W_GetID(void)
{
    return pHandler->SensorID;
}

bool AK9757W_SaveParameter(void)
{
    uint8_t i;
    int error;

    uint8_t RegAddr;
    uint8_t RxBuff[9];

    uint16_t CheckSum;

    if(pHandler->isMeasurementStart == true)
        return false;    
    RegAddr = AK9757W_REG_PARAMETER;
    error = I2C_Master_WriteRead(TEMP_I2C_ADDRESS, &RegAddr, 1, &RxBuff[0], 9);    
    if(error != E_NO_ERROR)
    {        
        return false;
    }    

    CheckSum = 0x0000;

    
    


    ParameterInfo.Parameter_Info.isSaveParameter = AK9757W_MAGIC_KEY;
    ParameterInfo.Parameter_Info.Parameter_ID_H = (pHandler->SensorID >> 16) & 0xFFFF;
    ParameterInfo.Parameter_Info.Parameter_ID_L = (pHandler->SensorID >> 0) & 0xFFFF;        
    ParameterInfo.Parameter_Info.Parameter_PARA_53 = RxBuff[0];
    ParameterInfo.Parameter_Info.Parameter_PARA_54 = RxBuff[1];
    ParameterInfo.Parameter_Info.Parameter_PARA_55 = RxBuff[2];
    ParameterInfo.Parameter_Info.Parameter_PARA_56 = RxBuff[3];
    ParameterInfo.Parameter_Info.Parameter_PARA_57 = RxBuff[4];
    ParameterInfo.Parameter_Info.Parameter_PARA_58 = RxBuff[5];
    ParameterInfo.Parameter_Info.Parameter_PARA_59 = RxBuff[6];
    ParameterInfo.Parameter_Info.Parameter_PARA_5A = RxBuff[7];
    ParameterInfo.Parameter_Info.Parameter_PARA_5B = RxBuff[8];

    for(i = 0 ; i < EE_INDEX_CHECKSUM ; i++)
    {
        CheckSum += ParameterInfo.Parameter_Buff[i];
    }
    ParameterInfo.Parameter_Info.CheckSum = CheckSum;

    Eeprom_Save_Parameter(&ParameterInfo);
    return true;
    
}

bool AK9757W_DeleteParameter(void)
{
    uint8_t i;
    for(i = 0 ; i < EE_INDEX_MAX ; i++)
    {
        ParameterInfo.Parameter_Buff[i] = 0x0000;
    }
    Eeprom_Save_Parameter(&ParameterInfo);
}

bool AK9757W_DispalyParameter(void)
{
    uint8_t i;
    Eeprom_Get_Parameter(&ParameterInfo);
    printf("MagicKey = 0x%04X\r\n", ParameterInfo.Parameter_Info.isSaveParameter);
    printf("SensorID = 0x%04X%04X\r\n", ParameterInfo.Parameter_Info.Parameter_ID_H, ParameterInfo.Parameter_Info.Parameter_ID_L);

    for(i = 0 ; i < 9 ; i++)
    {
        printf("Reg(%02X) = %02X\r\n", 0x53+i, ParameterInfo.Parameter_Buff[EE_INDEX_PA_53 + i]);
    }
    printf("CheckSum = 0x%04X\r\n", ParameterInfo.Parameter_Info.CheckSum);


    
        
    
}