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




#define AK9757W_ERROR_PRINT_INTERVAL    1000





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

const uint8_t default_cntl_reg_data[] = {0x20, 0xFF, 0xFE, 0x4A, 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};




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_Process(void);
static void AK9757W_Next_Step(AK9757W_STEP nextStep);







bool AK9757W_Initialization(void)
{

    AK9757W_Handler.PreStep = STEP_INIT;
    AK9757W_Handler.Step = STEP_INIT;

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

#if 0
    if(AK9757W_Check_CompanyCode() == false)
    {
        return false;
    }

    if(AK9757_Set_Mode(AK9757W_MODE_CONTINUOUS_MODE) == false)
    {
        return false;
    }

    Delay_ms(200);

    if(AK9757_Get_SensorID() == false)
    {
        return false;
    }

    AK9757W_Handler.isInitComplete = true;
#endif    


}





static bool 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_ERROR);
            }
        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;    
    memcpy(&pHandler->IR_TS_IT_Info.IR_TS_IT_Buff[0], &default_gir_oir_gts_ots_git_reg_data[1], (sizeof(default_gir_oir_gts_ots_git_reg_data)-1));
    error = I2C_Master_Write(TEMP_I2C_ADDRESS, &default_gir_oir_gts_ots_git_reg_data[0], sizeof(default_gir_oir_gts_ots_git_reg_data));
    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;    
}






























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;
}