/**
* @file GP22_ATY.c
*
* @param Project DEVICE_GENERAL_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2023 MZ-ATY
*       - This code follows:
*           - MZ-ATY Various Contents Joint Statement -
*               <a href="https://mengze.top/MZ-ATY_VCJS">
*                        https://mengze.top/MZ-ATY_VCJS</a>
*           - CC 4.0 BY-NC-SA -
*               <a href="https://creativecommons.org/licenses/by-nc-sa/4.0/">
*                        https://creativecommons.org/licenses/by-nc-sa/4.0/</a>
*       - Your use will be deemed to have accepted the terms of this statement.
*
* @brief Base functions of GP22 for all embedded device
*
* @version
*       - 1_01_220901 > ATY
*           -# Preliminary version, first Release
*           - Undone
********************************************************************************
*/



#ifndef __GP22_ATY_C
#define __GP22_ATY_C

#include "GP22_ATY.h"

/******************************* For user *************************************/

/******************************************************************************/

/* Basic **********************************************************************/
/**
 * @brief   write data with spi
 * @param   data_t data to wirite
 */
void GP22_WriteData(uint8_t data_t)
{
    GP22_ENABLE;
    SPI_Write(&data_t, 1);
    GP22_DISABLE;
}

void GP22_WriteReg(uint8_t opcode_address, uint32_t reg_data)
{
    uint8_t temp_uint8[4];
    temp_uint8[0] = reg_data >> 24;
    temp_uint8[1] = reg_data >> 16;
    temp_uint8[2] = reg_data >> 8;
    temp_uint8[3] = reg_data;
    GP22_ENABLE;
    SPI_Write(&opcode_address, 1);
    SPI_Write(temp_uint8, 4);
    GP22_DISABLE;
}

uint32_t GP22_ReadReg(uint8_t opcode_address)
{
    uint8_t data_t[4] = {0};

    GP22_ENABLE;
    SPI_Write(&opcode_address, 1);
    SPI_Read(data_t, 4);
    GP22_DISABLE;

#ifdef __DEBUG_GP22_OPCODE_ATY
    printf("\r\nRead opcode %02X: 0x%02X%02X%02X%02X",
        opcode_address, data_t[0], data_t[1], data_t[2], data_t[3]);
#endif /* __DEBUG_GP22_OPCODE_ATY */

    return (((uint32_t)data_t[0] << 24) + ((uint32_t)data_t[1] << 16)
        + ((uint32_t)data_t[2] << 8) + ((uint32_t)data_t[3]));
}

void GP22_Reset(void)
{
    GP22_NORESET;
    DelayMs(1);
    GP22_RESET;
    DelayMs(1);
    GP22_NORESET;
    DelayMs(1);
    GP22_WriteData(GP22_OPCODE_RESET);
}

// TODO: set generic
uint8_t GP22_WaitInt(uint8_t step)
{
    uint32_t errTimeCount = 0;

    // GP22_ENABLE;
    while(GP22_READ_INT_H)
    {
        errTimeCount++;
        if(errTimeCount > 12000)     // 24000 About 36ms at 64M HCLK, 1.25MIPS/Mhz for arm
        {
            errTimeCount = 0;
#ifdef __DEBUG_GP22_ATY
            printf("\r\nERR - INT timeout %d", step);
#endif /* __DEBUG_GP22_ATY */
            return step;
        }
    }
    return 0;
}

uint8_t GP22_SpiTest(uint8_t data_t)
{
    uint8_t i = 0;
    uint32_t temp_uint32 = 0;

    for(i = 0; i < 5; i++)
    {
        GP22_Reset();

        GP22_WriteReg(GP22_OPCODE_WRITE_REG | 0x01, ((uint32_t)data_t << 24) | 0x00123456);
        DelayMs(2);
        temp_uint32 = GP22_ReadReg(GP22_OPCODE_READ_REG | GP22_REGADDR_TEST);
#ifdef __DEBUG_GP22_ATY
        printf("\r\nSPIT - 0x%08X", temp_uint32);
#endif /* __DEBUG_GP22_ATY */
        if((temp_uint32 >> 24) == data_t)
            return 0;
    }
#ifdef __DEBUG_GP22_ATY
    printf("\r\nERR - SPI Commucate Err!");
#endif /* __DEBUG_GP22_ATY */
    return 1;
}

uint8_t GP22_ReadId(void)
{
    uint8_t temp_uint8 = GP22_OPCODE_READ_ID;
    uint8_t id[8] = {0};
    GP22_ENABLE;
    SPI_Write(&temp_uint8, 1);
    SPI_Read(id, 8);
    GP22_DISABLE;

#ifdef __DEBUG_GP22_ATY
    printf("\r\nRID - Read id: 0x%02X%02X%02X%02X%02X%02X%02X%02X",
        id[0], id[1], id[2], id[3], id[4], id[5], id[6], id[7]);
    // printf("\r\nRID - Save id: 0x%02X%02X%02X%02X%02X%02X%02X%02X",
    //     GP22_Ids[0], GP22_Ids[1], GP22_Ids[2], GP22_Ids[3], GP22_Ids[4],
    //     GP22_Ids[5], GP22_Ids[6], GP22_Ids[7]);
#endif /* __DEBUG_GP22_ATY */

    if((id[0] == GP22_Ids[0]) && (id[1] == GP22_Ids[1]) && (id[2] == GP22_Ids[2])
        && (id[3] == GP22_Ids[3]) && (id[4] == GP22_Ids[4]) && (id[5] == GP22_Ids[5])
        && (id[6] == GP22_Ids[6]))
    {
        return 0;
    }
    else
    {
#ifdef __DEBUG_GP22_ATY
        printf("\r\nERR - Read wrong id number!");
#endif /* __DEBUG_GP22_ATY */
        return 1;
    }

}

uint16_t GP22_AnalyseErrCode(void)
{
    uint16_t statusCode = 0;

    statusCode = GP22_ReadReg(GP22_OPCODE_READ_REG | GP22_REGADDR_STATUS) >> 16;

#ifdef __DEBUG_GP22_ATY
    printf("\r\nSTAT - Code: 0x%04X | ALU - %d | HIT1 - %d | HIT2 - %d",
        statusCode,
        statusCode & 0x0007,
        (statusCode >> 3) & 0x0007,
        (statusCode >> 6) & 0x0007);
    //Bit9: Timeout_TDC
    if((statusCode & 0x0200) == 0x0200)
        printf("\r\nERR - Indicates an overflow of the TDC unit");
    //Bit10: Timeout_Precounter
    if((statusCode & 0x0400) == 0x0400)
        printf("\r\nERR - Indicates an overflow of the 14 bit precounter in MR 2");
    //Bit11: Error_open
    if((statusCode & 0x0800) == 0x0800)
        printf("\r\nERR - Indicates an open sensor at temperature measurement");
    //Bit12: Error_short
    if((statusCode & 0x1000) == 0x1000)
        printf("\r\nERR - Indicates a shorted sensor at temperature measurement");
    //Bit13: EEPROM_eq_CREG
    if((statusCode & 0x2000) != 0x2000)
        printf("\r\nERR - The content of the configuration registers not equals the EEPROM");
    //Bit14: EEPROM_DED
    if((statusCode & 0x4000) == 0x4000)
        printf("\r\nERR - Double error detection. A multiple error has been detected whcich can not be corrected");
    //Bit15: EEPROM_Error
    if((statusCode & 0x8000) == 0x8000)
        printf("\r\nERR - Single error in EEPROM which has been corrected");
#endif /* __DEBUG_GP22_ATY */

    return statusCode;
}

// TODO: E2 tested is not reality, state changed with power time
uint8_t GP22_CompareE2(void)
{
    uint8_t errCount = 5;
    uint16_t statusCode = 0x0000;

    while(errCount != 0)
    {
        errCount--;
        GP22_WriteData(GP22_OPCODE_COMPARE_E2);
        statusCode = GP22_ReadReg(GP22_OPCODE_READ_REG | GP22_REGADDR_STATUS);
        if((statusCode & 0x2000) != 0x2000)
        {
            GP22_WriteData(GP22_OPCODE_WRITE_E2);
            DelayMs(100);
            if(GP22_WaitInt(GP22_STEP_CALC_HSC) != 0)
            {
#ifdef __DEBUG_GP22_ATY
                printf("\r\nERR - Write E2 timeout");
#endif /* __DEBUG_GP22_ATY */
                return 1;
            }
        }
        else
        {
#ifdef __DEBUG_GP22_ATY
            printf("\r\nCE2 - Compare E2 success");
#endif /* __DEBUG_GP22_ATY */
            return 0;
        }
    }
#ifdef __DEBUG_GP22_ATY
    printf("\r\nERR - Compare E2 failed");
#endif /* __DEBUG_GP22_ATY */
    return 1;
}

uint8_t GP22_CalcHSC(void)
{
    uint8_t errCount = 5;
    uint32_t hscReadValue;
    float correctionFactor;
    mbP_GVWP = 0;

    while(errCount != 0)
    {
        errCount--;
        GP22_WriteData(GP22_OPCODE_INIT);
        // EN_START need to set high
        GP22_WriteData(GP22_OPCODE_CALC_HSC);
        if(GP22_WaitInt(GP22_STEP_START_TEMP) != 0) return GP22_STEP_CALC_HSC;

        hscReadValue = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x00);
        // CLKHS periods * 1000 / 32.768 us
        // 0: 2 periods = 61.03515625us
        // 1: 4 periods = 122.0703125us
        // 2: 8 periods = 244.140625us
        // 3: 16 periods = 488.28125us
        correctionFactor = ((1 << (GP22_CLKHS_PERIOD + 1)) * 1000 / 32.768)
            / (hscReadValue / 65536) * GP22_CLKHS_FREQ;
        GP22_CALC_CLKHS_FREQ =
            GP22_CLKHS_FREQ;
            // correctionFactor * GP22_CLKHS_FREQ;
#ifdef __DEBUG_GP22_ATY
        printf("\r\nCHSC - Theoretical value: 0x%08X - %1.6f",
            hscReadValue, (float)hscReadValue / 65536);
        printf("\r\nCHSC - Correction factor for clock: %.6f", correctionFactor);
        printf("\r\nCHSC - Calibrated internal clock frequency: %.6fMHz - %.6fns",
            GP22_CALC_CLKHS_FREQ, 1000 / GP22_CALC_CLKHS_FREQ);
#endif /* __DEBUG_GP22_ATY */
        return 0;
    }

    return 1;
}

uint8_t GP22_StartTemp(void)
{
    uint8_t errCount = 5;
    uint32_t temp_uint32[4] = {0};
    float tempRefResFactor = 0.0;
    float tempRealRes[2] = {0.0};

    while(errCount != 0)
    {
        errCount--;
        GP22_WriteData(GP22_OPCODE_INIT);
        GP22_WriteData(GP22_OPCODE_START_TEMP);
        if(GP22_WaitInt(GP22_STEP_START_TEMP) != 0) continue;
        // GP22_AnalyseErrCode();

        temp_uint32[0] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x00);
        temp_uint32[1] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x01);
        temp_uint32[2] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x02);
        temp_uint32[3] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x03);
        tempRefResFactor =
            (float)(temp_uint32[2] + temp_uint32[3]) / 2
            / GP22_CALC_CLKHS_FREQ / GP22_TEMP_REF_RES;
        // tempRealRes[0] = (float)temp_uint32[0] / GP22_CALC_CLKHS_FREQ / tempRefRes;
        // tempRealRes[0] =
        //     (GP22_TEMP_REF_RES * tempRealRes[0]) / (GP22_TEMP_REF_RES - tempRealRes[0]);
        // tempRealRes[1] = (float)temp_uint32[1] / GP22_CALC_CLKHS_FREQ / tempRefRes;
        // tempRealRes[1] =
        //     (GP22_TEMP_REF_RES * tempRealRes[1]) / (GP22_TEMP_REF_RES - tempRealRes[1]);
        // tempRealValue[1] = ALGO_ResToKelvinTemp(tempRealRes[1], 10, 3937);
        tempRealRes[0] = (float)temp_uint32[0] / GP22_CALC_CLKHS_FREQ / tempRefResFactor;
        tempRealValue[0] = ALGO_ResToKelvinTemp(tempRealRes[0], 10, 3950);
        tempRealRes[1] = (float)temp_uint32[1] / GP22_CALC_CLKHS_FREQ / tempRefResFactor;
        tempRealValue[1] = ALGO_ResToKelvinTemp(tempRealRes[1], 10, 3950);
#ifdef __DEBUG_GP22_ATY
        printf("\r\nSTEMP - Temp reg: 0x%08X - 0x%08X - 0x%08X - 0x%08X",
            temp_uint32[0], temp_uint32[1], temp_uint32[2], temp_uint32[3]);
        printf("\r\nSTEMP - Temp reg: %d - %d - %d - %d",
            temp_uint32[0], temp_uint32[1], temp_uint32[2], temp_uint32[3]);
        printf("\r\nSTEMP - PT1 res: %f KOhm - %f C", tempRealRes[0], tempRealValue[0]);
        printf("\r\nSTEMP - PT2 res: %f KOhm - %f C", tempRealRes[1], tempRealValue[1]);
#endif /* __DEBUG_GP22_ATY */
        return 0;
    }

    return 1;
}







/* Use ************************************************************************/
float GP22_CALC_CLKHS_FREQ = GP22_CLKHS_FREQ;     // MHz
uint8_t GP22_Ids[8] = {0xF0, 0xF1, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6};
uint32_t GP22_REG[7] = {0};

uint32_t tofValue[4] = {0};
float tempRealValue[2] = {0.0};
float echoTimeDetect[2] = {0};
float echoTimeCalc[2] = {0};
float speedWave[2] = {0};
uint32_t PW1ST_ValueA = 0;

float machineDelayTime = 0;
float usDistance = 0;

volatile uint8_t pulseGenNum = 3;
volatile uint8_t pulseNum = 4;
volatile uint8_t firstWaveEnable = 1;
volatile uint16_t ignoreTime = 1200;
volatile uint16_t offsetValue = 0;
volatile uint8_t startPulseNum = 6;


// #define GP22_MEASURE_US_ANALOG
// #define GP22_MEASURE_US_DIGITAL
#define GP22_MEASURE_UsWave1MHz
// #define GP22_MEASURE_UsWave1MHz

/******************************************************************************/
#if defined(GP22_MEASURE_UsWave1MHz)
void GP22_RegInit_UsWave1MHz(void)
{
    GP22_REG[0] = GP22_CFG_KEEP_DEFAULT0
        | GP22_CFG0_ANZ_FIRE_0 * pulseGenNum    // 3-6 pulse
        | GP22_CFG0_DIV_FIRE_0 * 1              // 0CLK | 4 Div for 1MHz pulse
        | GP22_CFG0_ANZ_PER_CALRES_0 * GP22_CLKHS_PERIOD
        | GP22_CFG0_DIV_CLKHS_0 * GP22_CLKHS_DIV
        | GP22_CFG0_START_CLKHS_0 * 1
        | GP22_CFG0_CALIBRATE * 1
        | GP22_CFG0_NO_CAL_AUTO * 0             // 1 to disable, enable is necessary at mode 2
        | GP22_CFG0_MESSB2 * 1

        | GP22_CFG0_ANZ_PORT * 1
        | GP22_CFG0_TCYCLE * 1
        | GP22_CFG0_ANZ_FAKE * 0
        | GP22_CFG0_SEL_ECLK_TEMP * 1
        | GP22_Ids[0];
    GP22_REG[1] = GP22_CFG_KEEP_DEFAULT1
        | GP22_CFG1_HIT2_0 * 2
        | GP22_CFG1_HIT1_0 * 1
        | GP22_CFG1_HITIN2_0 * 0
        | GP22_CFG1_HITIN1_0 * pulseNum
        | GP22_CFG1_SEL_START_FIRE * 1
        | GP22_CFG1_SEL_TSTO2_0 * 2
        | GP22_CFG1_SEL_TSTO1_0 * 6
        // | GP22_CFG1_EN_FAST_INIT * 1
        | GP22_Ids[1];
    GP22_REG[2] = GP22_CFG_KEEP_DEFAULT2
        | GP22_CFG2_EN_INT_TDC_TIMEOUT * 1
        | GP22_CFG2_EN_INT_HITS * 1
        | GP22_CFG2_EN_INT_ALU * 1
        // | GP22_CFG2_RFEDGE2 * 0             // 0: rising or falling, 1: and
        | GP22_CFG2_RFEDGE1 * 0             // 0: rising or falling, 1: and
        | GP22_CFG2_DELVAL1_0 * ignoreTime / (1 << GP22_CLKHS_DIV)
        | GP22_Ids[2];
    GP22_REG[3] = GP22_CFG_KEEP_DEFAULT3
        | GP22_CFG3FW_EN_AUTOCALC_MB2 * 1
        | GP22_CFG3FW_EN_ERR_VAL * 1
        | GP22_CFG3FW_EN_FIRST_WAVE * firstWaveEnable
        | GP22_CFG3FW_SEL_TIMO_MB2_0 * 1
        // | GP22_CFG3FW_SEL_TIMO_MB2_0 * 0
        | GP22_CFG3FW_DELREL3_0 * (startPulseNum + 2)
        | GP22_CFG3FW_DELREL2_0 * (startPulseNum + 1)
        | GP22_CFG3FW_DELREL1_0 * startPulseNum
        | GP22_Ids[3];
    GP22_REG[4] = GP22_CFG_KEEP_DEFAULT4
        | GP22_CFG4FW_DIS_PW * 0            // 1: disable PW
        | GP22_CFG4FW_OFFSRNG1 * (((uint16_t)offsetValue % 10) & 0x01)
        | GP22_CFG4FW_OFFSRNG2 * (((uint16_t)offsetValue % 10) & 0x02)
        | GP22_CFG4FW_EDGE_FW * (((uint16_t)offsetValue % 10) & 0x04)
        | GP22_CFG4FW_OFFS_0 * ((uint16_t)offsetValue / 10)
        // above 4 only take effect at first wave enable mode
        | GP22_Ids[4];
    GP22_REG[5] = GP22_CFG_KEEP_DEFAULT5
        | GP22_CFG5_CON_FIRE_DOWN * 1
        | GP22_CFG5_EN_STARTNOISE * 1
        | GP22_CFG5_DIS_PHASESHIFT * 1
        | GP22_Ids[5];
    GP22_REG[6] = GP22_CFG_KEEP_DEFAULT6
        | GP22_CFG6_EN_ANALOG * 1
        | GP22_CFG6_EN_INT_END * 1
        | GP22_CFG0_START_CLKHS_0 * 1
        | GP22_CFG6_TW2_0 * 3
        | GP22_CFG6_QUAD_RES * 1
        // | GP22_CFG6_DOUBLE_RES * 1
        // | GP22_CFG6_DA_KORR_0 * 8

        | GP22_CFG6_NEG_STOP_TEMP * 1       // Must set handly
        // | GP22_CFG6_FIREO_DEF * 1
        // | GP22_CFG6_TEMP_PORTDIR * 1
        | GP22_Ids[6];

    GP22_WriteReg(0x80, GP22_REG[0]);
    GP22_WriteReg(0x81, GP22_REG[1]);
    GP22_WriteReg(0x82, GP22_REG[2]);
    GP22_WriteReg(0x83, GP22_REG[3]);
    GP22_WriteReg(0x84, GP22_REG[4]);
    GP22_WriteReg(0x85, GP22_REG[5]);
    GP22_WriteReg(0x86, GP22_REG[6]);

#ifdef __DEBUG_GP22_ATY
    printf("\r\nREGI - 0: %08X\r\nREGI - 1: %08X\r\nREGI - 2: %08X\
        \r\nREGI - 3: %08X\r\nREGI - 4: %08X\r\nREGI - 5: %08X\r\nREGI - 6: %08X",
        GP22_REG[0], GP22_REG[1], GP22_REG[2],
        GP22_REG[3], GP22_REG[4], GP22_REG[5], GP22_REG[6]);
#endif /* __DEBUG_GP22_ATY */
}

uint8_t GP22_StartTof_UsWave1MHz(void)
{
    float wavePeriod = 0.0;
    float waveFreq = 0.0;

    GP22_WriteData(GP22_OPCODE_INIT);
    GP22_WriteData(GP22_OPCODE_START_TOF);

    if(GP22_WaitInt(GP22_STEP_START_TOF) != 0) return 1;

    // GP22_AnalyseErrCode();
    tofValue[0] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x00);
    tofValue[1] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x01);
    tofValue[2] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x02);
    tofValue[3] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x03);
    if((GP22_REG[3] & GP22_CFG3FW_EN_FIRST_WAVE) != 0)
        PW1ST_ValueA = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x08);

    wavePeriod =
        (float)((tofValue[2] - tofValue[1]) + (tofValue[1] - tofValue[0])) / 2
        / GP22_CALC_CLKHS_FREQ / 65536;     // ns
    waveFreq = 1 / wavePeriod;              // MHz
    echoTimeDetect[0] = ((float)tofValue[1] / GP22_CALC_CLKHS_FREQ / 65536);
        // - (wavePeriod * 2);
    // if(echoTimeDetect[0] <= 0)
    //     echoTimeDetect[0] = machineDelayTime + 1;
    echoTimeCalc[0] = echoTimeDetect[0] - machineDelayTime;
    speedWave[0] = usDistance * 1000.0 / echoTimeCalc[0];

#ifdef __DEBUG_GP22_ATY
    if((GP22_REG[3] & GP22_CFG3FW_EN_FIRST_WAVE) != 0)
        printf("\r\nSTOF - PW1ST value: 0x%02X - %f",
            PW1ST_ValueA, (float)(PW1ST_ValueA >> 24) / 128);
    printf("\r\nSTOF - TOF reg: 0x%08X - 0x%08X - 0x%08X - 0x%08X",
        tofValue[0], tofValue[1], tofValue[2], tofValue[3]);
    printf("\r\nSTOF - TOF reg: %d - %d - %d - %d",
        tofValue[0] / 65536, tofValue[1] / 65536,
        tofValue[2] / 65536, tofValue[3] / 65536);
    printf("\r\nSTOF - TOF reg: %f - %f - %f - %f",
        (float)tofValue[0] / GP22_CALC_CLKHS_FREQ / 65536,
        (float)tofValue[1] / GP22_CALC_CLKHS_FREQ / 65536,
        (float)tofValue[2] / GP22_CALC_CLKHS_FREQ / 65536,
        (float)tofValue[3] / GP22_CALC_CLKHS_FREQ / 65536);
    printf("\r\nSTOF - Wave: %f us - %f MHz", wavePeriod, waveFreq);
    printf("\r\nSTOF - Time: %f us - %f m/s", echoTimeCalc[0], speedWave[0]);
    printf("\r\nStep %02d-A done! ----------------------------------------------",
        GP22_STEP_START_TOF);
#endif /* __DEBUG_GP22_ATY */
    return 0;
}

uint8_t GP22_Process_UsWave1MHz(uint8_t cmdCode)
{
    // cmdCode = 0xFF;
    uint8_t errCode = 0;

    if(cmdCode & GP22_STEP_RESET)
        GP22_Reset();
    if(cmdCode & GP22_STEP_SPI_TEST)
        if(GP22_SpiTest(0x68)) return GP22_STEP_SPI_TEST;
    if(cmdCode & GP22_STEP_REG_INIT
        || cmdCode & GP22_STEP_READ_ID){
        GP22_RegInit_UsWave1MHz();
        if(GP22_ReadId()) return GP22_STEP_READ_ID;
    }
    // if(cmdCode & GP22_STEP_COMPARE_E2)
    //     if(GP22_CompareE2()) return GP22_STEP_COMPARE_E2;

    if(cmdCode & GP22_STEP_CALC_HSC)
        if(GP22_CalcHSC()) return GP22_STEP_CALC_HSC;
    // GP22_CALC_CLKHS_FREQ = GP22_CLKHS_FREQ;
    if(cmdCode & GP22_STEP_START_TEMP)
        if(GP22_StartTemp()) errCode = GP22_STEP_START_TEMP;
    if(cmdCode & GP22_STEP_START_TOF)
        if(GP22_StartTof_UsWave1MHz()) errCode = GP22_STEP_START_TOF;

    return errCode;
}



uint8_t GP22_Process_Run(uint8_t cmdCode)
{
    uint8_t errCode = 0;
#ifdef __DEBUG_GP22_ATY
    printf("\r\nStart");
#endif /* __DEBUG_GP22_ATY */
    // GP22_WAKE;
    errCode = GP22_Process_UsWave1MHz(cmdCode);
    // GP22_AnalyseErrCode();
#ifdef __DEBUG_GP22_ATY
    printf("\r\nStep %02d done! ------------------------------------------------", errCode);
    printf("\r\nOver!");
#endif /* __DEBUG_GP22_ATY */

    return errCode;
}





#elif defined(GP22_MEASURE_US_DIGITAL)
void GP22_RegInitUs_Digital(void)
{
    GP22_REG[0] = GP22_CFG_KEEP_DEFAULT0
        | GP22_CFG0_ANZ_FIRE_0 * 6
        // | GP22_CFG0_ANZ_FIRE_0 * 14
        | GP22_CFG0_DIV_FIRE_0 * 11
        | GP22_CFG0_ANZ_PER_CALRES_0 * 3
        | GP22_CFG0_DIV_CLKHS_0 * 2
        | GP22_CFG0_START_CLKHS_0 * 1
        | GP22_CFG0_CALIBRATE * 1
        | GP22_CFG0_MESSB2 * 1
        | GP22_Ids[0];
    GP22_REG[1] = GP22_CFG_KEEP_DEFAULT1
        | GP22_CFG1_HIT2_0 * 2
        | GP22_CFG1_HIT1_0 * 1
        | GP22_CFG1_HITIN2_0 * 0
        | GP22_CFG1_HITIN1_0 * 4
        | GP22_CFG1_SEL_START_FIRE * 1
        | GP22_CFG1_SEL_TSTO2_0 * 2
        | GP22_CFG1_SEL_TSTO1_0 * 1
        | GP22_Ids[1];
    GP22_REG[2] = GP22_CFG_KEEP_DEFAULT2
        | GP22_CFG2_EN_INT_TDC_TIMEOUT * 1
        | GP22_CFG2_EN_INT_HITS * 1
        | GP22_CFG2_EN_INT_ALU * 1
        | GP22_CFG2_RFEDGE1 * 1
        | GP22_Ids[2];
    GP22_REG[3] = GP22_CFG_KEEP_DEFAULT3
        | GP22_CFG3FW_EN_AUTOCALC_MB2 * 1
        // | GP22_CFG3FW_EN_FIRST_WAVE * 1
        | GP22_CFG3FW_EN_ERR_VAL * 1
        | GP22_CFG3FW_SEL_TIMO_MB2_0 * 3
        | GP22_CFG3FW_DELREL3_0 * 5
        | GP22_CFG3FW_DELREL2_0 * 4
        | GP22_CFG3FW_DELREL1_0 * 3
        | GP22_Ids[3];
    GP22_REG[4] = GP22_CFG_KEEP_DEFAULT4
        // | GP22_CFG4FW_DIS_PW * 1

        // | GP22_CFG4FW_OFFSRNG2 * 1
        // | GP22_CFG4FW_OFFSRNG1 * 1
        // | GP22_CFG4FW_EDGE_FW * 1
        // | GP22_CFG4FW_OFFS_0 * 15
        // | GP22_CFG4FW_OFFS_0 * 16
        | GP22_Ids[4];
    GP22_REG[5] = GP22_CFG_KEEP_DEFAULT5
        | GP22_CFG5_CON_FIRE_0 * 1
        | GP22_CFG5_EN_STARTNOISE * 1
        | GP22_CFG5_PHFIRE_0 * 0x5555
        | GP22_Ids[5];
    GP22_REG[6] = GP22_CFG_KEEP_DEFAULT6
        // | GP22_CFG6_NEG_STOP_TEMP * 1

        // | GP22_CFG6_DOUBLE_RES * 1
        | GP22_CFG6_QUAD_RES * 1
        // | GP22_CFG6_EN_ANALOG * 1
        // | GP22_CFG6_TW2_0 * 3
        | GP22_CFG6_EN_INT_END * 1
        // | GP22_CFG6_FIREO_DEF * 0
        | GP22_CFG6_ANZ_FIRE_END_0 * 0
        | GP22_Ids[6];

#ifdef __DEBUG_GP22_ATY
    printf("\r\nREGI - 0: %08X\r\nREGI - 1: %08X\r\nREGI - 2: %08X\
        \r\nREGI - 3: %08X\r\nREGI - 4: %08X\r\nREGI - 5: %08X\r\nREGI - 6: %08X",
        GP22_REG[0], GP22_REG[1], GP22_REG[2],
        GP22_REG[3], GP22_REG[4], GP22_REG[5], GP22_REG[6]);
#endif /* __DEBUG_GP22_ATY */

    GP22_WriteReg(0x80, GP22_REG[0]);
    GP22_WriteReg(0x81, GP22_REG[1]);
    GP22_WriteReg(0x82, GP22_REG[2]);
    GP22_WriteReg(0x83, GP22_REG[3]);
    GP22_WriteReg(0x84, GP22_REG[4]);
    GP22_WriteReg(0x85, GP22_REG[5]);
    GP22_WriteReg(0x86, GP22_REG[6]);
}

uint8_t gp22StartTofFlag = 0;
uint32_t tofValue[4] = {0};
uint8_t GP22_StartTofUs_Digital(void)
{
    uint32_t tofReadValue[4] = {0};
    uint32_t PW1ST_Value = 0;

    gp22StartTofFlag = 1;
    GP22_WriteData(GP22_OPCODE_INIT);
    GP22_WriteData(GP22_OPCODE_START_TOF);
    // GP22_WriteData(GP22_OPCODE_START_TOF2);
    if(GP22_WaitInt(GP22_STEP_START_TOF) != 0) return 1;
    gp22StartTofFlag = 0;

    tofReadValue[0] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x00);
    tofReadValue[1] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x01);
    tofReadValue[2] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x02);
    tofReadValue[3] = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x03);
    tofValue[0] = tofReadValue[0];
    tofValue[1] = tofReadValue[1];
    tofValue[2] = tofReadValue[2];
    tofValue[3] = tofReadValue[3];
    PW1ST_Value = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x08);
    PW1ST_Value = PW1ST_Value * 1;
#ifdef __DEBUG_GP22_ATY
    printf("\r\nSTOF - TOF reg: 0x%08X - 0x%08X - 0x%08X - 0x%08X",
        tofReadValue[0], tofReadValue[1], tofReadValue[2], tofReadValue[3]);
    printf("\r\nSTOF - TOF reg: %d - %d - %d - %d",
        tofReadValue[0], tofReadValue[1], tofReadValue[2], tofReadValue[3]);
    printf("\r\nSTOF - TOF reg: %.2f - %.2f - %.2f - %.2f",
        (float)tofReadValue[0] / 65536, (float)tofReadValue[1] / 65536,
        (float)tofReadValue[2] / 65536, (float)tofReadValue[3] / 65536);
    printf("\r\nSTOF - PW1ST value: 0x%02X - %f",
        PW1ST_Value, (float)(PW1ST_Value >> 24) / 128);
#endif /* __DEBUG_GP22_ATY */

    return 0;
}


// Mode 1
/* Laser distance calc ***********************************************************/
#elif defined(GP22_MEASURE_UsWave1MHz1)
// ---------------------------------------------------
//                 T_ref
// Time_Value = ----------- * measured_RAW_Value
//               Cal2-Cal1
// 3*10^8 m/s light speed(0.3m/ns)
// ---------------------------------------------------
uint8_t GP22_StartTofLaserCal(void)
{
    uint32_t tofValue = 0, calValue = 0;

    GP22_WriteReg(0x80, GP22_REG[0]
        | GP22_CFG0_CALIBRATE * 1);
    GP22_WriteReg(0x82, GP22_REG[2]
        & (~GP22_CFG2_EN_INT_ALU)
        & (~GP22_CFG2_EN_INT_TDC_TIMEOUT)
        | (GP22_CFG2_EN_INT_HITS * 1));
    GP22_WriteData(GP22_OPCODE_INIT);
    GP22_WriteData(GP22_OPCODE_START_CAL_TOF);
    if(GP22_WaitInt(GP22_STEP_START_TOF) != 0) return 1;
#ifdef __DEBUG_GP22_ATY
    printf("\r\nTOF - Laser: Cal over");
#endif /* __DEBUG_GP22_ATY */
    GP22_WriteReg(0x82, GP22_REG[2]
        & (~GP22_CFG2_EN_INT_HITS)
        | (GP22_CFG2_EN_INT_ALU * 1)
        | (GP22_CFG2_EN_INT_TDC_TIMEOUT * 1));
    GP22_WriteData(GP22_OPCODE_INIT);
    GP22_SIGNAL_L;
    GP22_SIGNAL_H;
    GP22_SIGNAL_L;
    // for(uint8_t i = 0; i < 2; i++){}
    GP22_SIGNAL_A_L;
    GP22_SIGNAL_A_H;
    GP22_SIGNAL_A_L;
    if(GP22_WaitInt(GP22_STEP_START_TOF) != 0) return 1;
    tofValue = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x00);
#ifdef __DEBUG_GP22_ATY
    printf("\r\nTOF - Laser: 0x%08X - %d", tofValue, tofValue);
#endif /* __DEBUG_GP22_ATY */
    GP22_WriteReg(0x81, GP22_REG[1]
        & (~GP22_CFG1_HIT2_0)
        & (~GP22_CFG1_HIT2_1)
        & (~GP22_CFG1_HIT2_2)
        & (~GP22_CFG1_HIT2_3)
        & (~GP22_CFG1_HIT1_0)
        & (~GP22_CFG1_HIT1_1)
        & (~GP22_CFG1_HIT1_2)
        & (~GP22_CFG1_HIT1_3)
        | GP22_CFG1_HIT2_0 * 6
        | GP22_CFG1_HIT1_0 * 7);        // Cal1 - Cal2, EN Stop2/Stop1 1 hit
    calValue = GP22_ReadReg(GP22_OPCODE_READ_REG | 0x01);
#ifdef __DEBUG_GP22_ATY
    printf("\r\nTOF - Laser: 0x%08X - %d", calValue, calValue / 65536);
    printf("\r\nTOF - Laser: %f ns - %f m",
        (float)tofValue / ((float)calValue / 65536) * 16,       // TODO: Time calculate wrong
        (float)tofValue / ((float)calValue / 65536) * 16 * 0.23);
#endif /* __DEBUG_GP22_ATY */

    return 0;
}

void GP22_Process(uint32_t* gp22ProcessValue)
{
    if(!GP22_Step(GP22_STEP_RESET)
        && !GP22_Step(GP22_STEP_SPI_TEST)
        && !GP22_Step(GP22_STEP_REG_INIT)
        && !GP22_Step(GP22_STEP_READ_ID)
        && !GP22_Step(GP22_STEP_COMPARE_E2)
        && !GP22_Step(GP22_STEP_START_TOF)
        // && !GP22_Step(GP22_STEP_CALC_HSC)
        // && !GP22_Step(GP22_STEP_START_TEMP)
        )
    {
        // gp22ProcessValue[0] = tofValue[0];
        // gp22ProcessValue[1] = tofValue[1];
        // gp22ProcessValue[2] = tofValue[2];
        // gp22ProcessValue[3] = tofValue[3];
    }
    GP22_AnalyseErrCode();
#ifdef __DEBUG_GP22_ATY
    printf("\r\n");
#endif /* __DEBUG_GP22_ATY */
}
#endif /* if defined*/









#endif /* __GP22_ATY_C */

/******************************** End Of File *********************************/