/*
 * Copyright (c) 2020 Great Master Wu. All rights reserved.
 * Author            Date          ChangeLog
 * Master Great.wu   2021-03-17    Initialize version
 * Master Great.wu   2021-03-31    Finished raise to wake(Not debug)
 * TODO: FIFO INIT,DRDY INIT in function independent,  APEX MOTION FUNCTIONS init 
 * indenpent, add convert function; data in data_buf cna convert according config;
 */
#include "icm4x6xx.h"
#include "sensor_error.h"


/* USER BANK 0 REGISTER MAP - DESCRIPTIONS */

// 14.1 device config
int icm4x6xx_set_spi_mode(inv_icm4x6xx_t *dev,
                          DEVICE_CONFIG_SPI_MODE_t mode)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[] = {REG_DEVICE_CONFIG, 0};
    sensor_serial_if_t *driver = dev->driver;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    buf[1] = mode;
    ret |= driver->write_reg(driver, buf, 1);

    return ret;
}


int icm4x6xx_reset(inv_icm4x6xx_t *dev)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t flag;
    uint8_t buf[] = {REG_DEVICE_CONFIG, 0};
    sensor_serial_if_t *driver = dev->driver;
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    ret |= driver->read_reg(driver, buf, 1);
    buf[0] = REG_DEVICE_CONFIG; // first bit is clear at nordic platform
    buf[1] |= CHIP_RESET_ENANLE;

    ret |= driver->write_reg(driver, buf, 1);

    sensor_delay_ms(100);
    ret |= icm4x6xx_get_int_status(dev, &flag);
    if(flag & RESET_DONE_INT_FLAG)
    {
        SENSOR_INFO("%s is reset done\r\n", dev->name);
    }
    else
    {
        SENSOR_ERR("%s is reset fail\r\n", dev->name);
    }

    return ret;
}


// 14.2 driver config
/*
 * Note: Strong slew rate on output pins could induce ground bounces,
 * user must make sure there are no glitches on AP_SCL and AP_SDA when
 * system is in I3C mode, as there is no glitch filter to shield AP_SCL
 * and AP_SDA from noise on the lines.
*/
int icm4x6xx_set_driver_config(inv_icm4x6xx_t *dev,
                               I2C_SLEW_RATE_TYPE i2c_slew,
                               SPI_SLEW_RATE_TYPE spi_slew)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[] = {REG_DRIVE_CONFIG, 0};
    sensor_serial_if_t *driver = dev->driver;
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    buf[1] = i2c_slew | spi_slew;

    ret = driver->write_reg(driver, buf, 1);

    return ret;
}

// 14.3 INT config
/*
 * @param config - combine ele in INT_CONFIG_TYPE_t
 * notice: not retain last config
*/
int icm4x6xx_set_interrupt_pin_state(inv_icm4x6xx_t *dev, uint8_t config)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[] = {REG_INT_CONFIG, 0};
    sensor_serial_if_t *driver = dev->driver;
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    buf[1] = config;

    ret = driver->write_reg(driver, buf, 1);

    return ret;
}


int icm4x6xx_get_int_config(inv_icm4x6xx_t *dev, uint8_t *config)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[] = {REG_INT_CONFIG, 0};
    sensor_serial_if_t *driver = dev->driver;
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    ret = driver->read_reg(driver, buf, 1);
    *config = buf[1];

    return ret;
}


// 14.4 FIFO config
int icm4x6xx_set_fifo_config(inv_icm4x6xx_t *dev,
                            FIFO_CONFIG_MODE_t mode)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[] = {REG_FIFO_CONFIG, 0};
    sensor_serial_if_t *driver = dev->driver;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    buf[1] = mode;
    ret |= driver->write_reg(driver, buf, 1);

    return ret;   
}


// 14.5  14,6 TEMP_DATA
int icm4x6xx_read_raw_temperature(inv_icm4x6xx_t *dev, int16_t *temp)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[] = {REG_TEMP_DATA1, 0};
    sensor_serial_if_t *driver = dev->driver;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    ret = driver->read_reg(driver, buf, 1);
    *temp = buf[1] << 8;

    buf[0] = REG_TEMP_DATA0;
    ret |= driver->read_reg(driver, buf, 1);
    *temp |= buf[1];

    return ret;
}


int icm4x6xx_read_degree_centigrade(inv_icm4x6xx_t *dev, float *degree)
{
    int16_t raw = 0;
    int ret = 0;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    ret = icm4x6xx_read_raw_temperature(dev, &raw);
    *degree = (raw / 132.48) + 25;

    return ret;
}


// 14.7/8/9/10/11/12/1314/15/16/17/18 ACCEL DATA,

/* when the interface is i2c,42686-P automatically increments the register
   address and loads the data to the appropriate register. The other
   interface is not comment this*/
/**
 * @brief read data register directly
*/
int icm4x6xx_read_raw_data(inv_icm4x6xx_t *dev)
{
    int ret;
    dev->data_buf[0] = REG_TEMP_DATA1;
    sensor_serial_if_t *driver = dev->driver;

    if(dev->dri_package_size == 0)
    {
        return SENSOR_VAR_NOT_INIT;
    }
    ret = driver->read_reg(driver, dev->data_buf, dev->dri_package_size);
    
    return ret;
}

/**
 * @brief read data after read flag, with the flag auto select read mode
*/
int icm4x6xx_read_raw_data_with_flag(inv_icm4x6xx_t *dev, acc_gyro_data_packet_t *data)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[sizeof(acc_gyro_data_packet_t) + 1] = {REG_TEMP_DATA1, 0};
    sensor_serial_if_t *driver = dev->driver;
    if(data == NULL)
    {
        SENSOR_ERR("Data packet NULL pointer\r\n");
        return SENSOR_ERROR_ARGS;
    }
    uint8_t int_flag;

    ret = icm4x6xx_get_int_status(dev, &int_flag);
    SENSOR_RESULT_CHECK(ret);
    if((int_flag & DRDY_INT_FLAG) && (dev->fifo_mode_en == DRDY_MODE))
    {
        icm4x6xx_read_raw_data(dev);
    }
    if(int_flag & FIFO_FULL_INT_FLAG)
    {
        // reset fifo
        ret = icm4x6xx_set_fifo_config(dev, FIFO_CONFIG_MODE_BYPASS);
        SENSOR_RESULT_CHECK(ret);
        // set fifo stream fifo
        ret = icm4x6xx_set_fifo_config(dev, FIFO_CONFIG_MODE_STREAM);
        SENSOR_RESULT_CHECK(ret);
        return SENSOR_ERROR_FIFO_FULL;
    }else if(int_flag & FIFO_THS_INT_FLAG)
    {
        ret = icm4x6xx_get_data_in_fifo(dev);
    }
    // TODO Convert data
err:
    SENSOR_ERR(sensor_error_str(ret));
    return ret;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
}
// 14.19/20 TMST FSYNCH/L
// TODO

// 14.21
/**
 * The bit clears to 0 after the register has been read.
 */
int icm4x6xx_get_int_status(inv_icm4x6xx_t *dev, uint8_t *flag)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[] = {REG_INT_STATUS, 0};
    sensor_serial_if_t *driver = dev->driver;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    ret = driver->read_reg(driver, buf, 1);
    *flag = buf[1];

    return ret; 
}


/**
 * notice: if you want read multiple flag, please use icm4x6xx_get_int_status
 */
bool icm4x6xx_is_fifo_full(inv_icm4x6xx_t *dev)
{
    uint8_t flag;
    bool result = false;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    if(icm4x6xx_get_int_status(dev, &flag) & FIFO_FULL_INT1_FLAG)
    {
        result = true;
    }

    return result;   
}

/**
 * notice: if you want read multiple flag, please use icm4x6xx_get_int_status
 */
bool icm4x6xx_is_fifo_ths(inv_icm4x6xx_t *dev)
{
    uint8_t flag;
    bool result = false;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    if(icm4x6xx_get_int_status(dev, &flag) & FIFO_THS_INT1_FLAG)
    {
        result = true;
    }

    return result;   
}

/**
 * notice: if you want read multiple flag, please use icm4x6xx_get_int_status
 */
bool icm4x6xx_is_agc_ready(inv_icm4x6xx_t *dev)
{
    uint8_t flag;
    bool result = false;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    if(icm4x6xx_get_int_status(dev, &flag) & AGC_RDY_INT1_FLAG)
    {
        result = true;
    }

    return result;
}


/**
 * 14.22/23
 * count indicates the number of records or bytes available in FIFO
 * according to FIFO_COUNT_REC setting. Reading FIFO_COUNTL latches
 * the data for both FIFO_COUNTH, and FIFO_COUNTL. 
 */
int icm4x6xx_get_fifo_count(inv_icm4x6xx_t *dev, uint16_t *count)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[] = {REG_FIFO_COUNTL, 0};
    sensor_serial_if_t *driver = dev->driver;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    ret = driver->read_reg(driver, buf, 1);
    *count = (buf[1] & 0x00FF) << 8;

    buf[0] = REG_FIFO_COUNTH;
    ret |= driver->read_reg(driver, buf, 1);
    *count |= buf[1];

    return ret;
}


// 14.24
/*
 * TODO:Notice - find the relationship between buffer in spi_event_handler
 * with buffer at here
*/
int icm4x6xx_get_data_in_fifo(inv_icm4x6xx_t *dev)
{
    DEV_ASSERT(dev);

    dev->data_buf[0] = REG_FIFO_DATA;
    sensor_serial_if_t *driver = dev->driver;
    
    if(dev->fifo_data_to_read <=0 || dev->fifo_data_to_read > ICM4X6XX_MAX_FIFO_SIZE)
    {
        return SENSOR_FIFO_COUNT_INVALID;
    }

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->read_reg(driver, dev->data_buf, dev->fifo_data_to_read);
}

// 14.25/26/27/28/29/30
// TODO


// 14.31
int icm4x6xx_get_int_status2(inv_icm4x6xx_t *dev, uint8_t *status)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[] = {REG_INT_STATUS2, 0};
    sensor_serial_if_t *driver = dev->driver;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    ret = driver->read_reg(driver, buf, 1);
    *status = buf[1];

    return ret;
}


// 14.32
int icm4x6xx_get_int_status3(inv_icm4x6xx_t *dev, uint8_t *status)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[] = {REG_INT_STATUS3, 0};
    sensor_serial_if_t *driver = dev->driver;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    ret = driver->read_reg(driver, buf, 1);
    *status = buf[1];

    return ret;
}


// 14.33 SIGNAL_PATH_RESET (4Bh)
/*
 * @param dmp_init_state - When this bit is set to 1, the DMP is enabled
 * @param dmp_mem_reset_state - When this bit is set to 1, the DMP memory is reset
 * @param reset - When this bit is set to 1, the signal path is reset by restarting
 *                the ODR counter and signal path controls.
 * @param tmst - When this bit is set to 1, the time stamp counter is latched
 *               into the time stamp register. This is a write on clear bit.
 * @param fifo_flush - When set to 1, FIFO will get flushed.
*/
int icm4x6xx_set_signal_path_reset(inv_icm4x6xx_t *dev,
                                   SIGNAL_PATH_RESET_DMP_INIT_t dmp_init_state,
                                   SIGNAL_PATH_RESET_DMP_MEM_RESET_t dmp_mem_reset_state,
                                   SIGNAL_PATH_ABORT_AND_RESET_t reset,
                                   SIGNAL_PATH_RESET_TMST_STROBE_t tmst,
                                   SIGNAL_PATH_RESET_FIFO_FLUSH_t fifo_flush)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_SIGNAL_PATH_RESET, 0};
    buf[1] = dmp_init_state | dmp_mem_reset_state | reset | tmst | fifo_flush;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    return driver->write_reg(driver, buf, 1);
}

int icm4x6xx_flush_fifo(inv_icm4x6xx_t *dev)
{
    DEV_ASSERT(dev);
    int ret;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_SIGNAL_PATH_RESET, 0};

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    ret = driver->read_reg(driver, buf, 1);
    if(ret == SENSOR_SUCCESS)
    {
        buf[1] |= SIGNAL_PATH_RESET_FIFO_FLUSH_EN;
    }

    ret |= driver->write_reg(driver, buf, 1);

    return ret;
}


/*
 * enable/disable dmp
*/
int icm4x6xx_dmp_state(inv_icm4x6xx_t *dev, SIGNAL_PATH_RESET_DMP_INIT_t state)
{
    DEV_ASSERT(dev);
    int ret;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_SIGNAL_PATH_RESET, 0};

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    ret = driver->read_reg(driver, buf, 1);
    if(ret == SENSOR_SUCCESS)
    {
        buf[1] &= (~BIT_SIGNAL_PATH_RESET_DMP_INIT_MASK); // clear this bit
        buf[1] |= state;
    }

    ret |= driver->write_reg(driver, buf, 1);

    return ret;
}

// 14.34 INTF_CONFIG0
/**
 * @param config - the combine ele in INTF_CONFIG_TYPE
 */
int icm4x6xx_set_intf_config0(inv_icm4x6xx_t *dev, uint8_t config)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_INTF_CONFIG0, 0};

    buf[1] = config;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->write_reg(driver, buf, 1);
}


int icm4x6xx_get_intf_config0(inv_icm4x6xx_t *dev, uint8_t *status)
{
    DEV_ASSERT(dev);
    int ret;
    uint8_t buf[] = {REG_INTF_CONFIG0, 0};
    sensor_serial_if_t *driver = dev->driver;

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    ret = driver->read_reg(driver, buf, 1);
    *status = buf[1];

    return ret;
}


// 14.35 INTF_CONFIG1(4D)
/*
 * @param lp_clk - 0: Accelerometer LP mode uses Wake Up oscillator clock
 *                 1: Accelerometer LP mode uses RC oscillator clock
 * @param rtc_mode - 0: No input RTC clock is required
 *                   1: RTC clock input is required
 * @param clksel - 00: Always select internal RC oscillator
 *                 01: Select PLL when available, else select RC oscillator (default)
 *                 10: Reserved
 *                 11: Disable all clocks
*/
int icm4x6xx_set_intf_config1(inv_icm4x6xx_t *dev,
                              INTF_CONFIG1_ACCEL_LP_CLK_t lp_clk,
                              INTF_CONFIG1_RTC_MODE_t rtc_mode,
                              INTF_CONFIG1_CLKSEL_t clksel)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_PWR_MGMT_0, 0};

    buf[1] = lp_clk | rtc_mode | clksel;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->write_reg(driver, buf, 1);
}

//14.36
int icm4x6xx_set_pwr_mgmt0(inv_icm4x6xx_t *dev,
                           PWR_MGMT0_TEMP_t temp_state,
                           PWR_MGMT0_IDLE_t idle,
                           PWR_MGMT0_GYRO_MODE_t gyro_mode,
                           PWR_MGMT0_ACCEL_MODE_t accel_mode)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_PWR_MGMT_0, 0};
    SENSOR_INFO("%s\r\n", __func__);

    buf[1] = temp_state | idle | gyro_mode | accel_mode;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->write_reg(driver, buf, 1);
}


int icm4x6xx_get_pwr_mgmt0(inv_icm4x6xx_t *dev, uint8_t* config)
{
    DEV_ASSERT(dev);
    int ret = 0;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_PWR_MGMT_0, 0};
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    ret = driver->read_reg(driver, buf, 1);
    *config = buf[1];

    return ret;
}


// 14.37
int icm4266x_set_gyro_config0(inv_icm4x6xx_t *dev,
                              GYRO_CONFIG0_FS_SEL_t sel,
                              GYRO_CONFIG0_ODR_t odr)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_GYRO_CONFIG0, 0};
    SENSOR_INFO("%s\r\n", __func__);

    buf[1] = sel | odr;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->write_reg(driver, buf, 1);
}


int icm4266x_get_gyro_config0(inv_icm4x6xx_t *dev,uint8_t *config)
{
    DEV_ASSERT(dev);
    int ret;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_GYRO_CONFIG0, 0};
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    ret = driver->read_reg(driver, buf, 1);
    *config = buf[1];

    return ret;
}


// 14.38
int icm4266x_set_accel_config0(inv_icm4x6xx_t *dev,
                               ACCEL_CONFIG0_FS_SEL_t sel,
                               ACCEL_CONFIG0_ODR_t odr)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_ACCEL_CONFIG0, 0};
    SENSOR_INFO("%s\r\n", __func__);

    buf[1] = sel | odr;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->write_reg(driver, buf, 1);
}


int icm4266x_get_accel_config0(inv_icm4x6xx_t *dev, uint8_t *config)
{
    DEV_ASSERT(dev);
    int ret;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_ACCEL_CONFIG0, 0};
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    ret = driver->read_reg(driver, buf, 1);
    *config = buf[1];

    return ret;
}


// 14.39
// TODO

// 14.40
int icm4x6xx_set_gyro_accel_config0(inv_icm4x6xx_t *dev,
                                    GYRO_ACCEL_CONFIG0_ACCEL_FILT_AVG_t acc_filt,
                                    GYRO_ACCEL_CONFIG0_GYRO_FILT_BW_t gyro_filt)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_ACCEL_GYRO_CONFIG0, acc_filt|gyro_filt};

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->write_reg(driver, buf, 1);
}

// 14.41
// TODO

// 14.42
/**
 * @param regs_en - 0: TMST_VALUE[19:0] read always returns 0s
 *                  1: TMST_VALUE[19:0] read returns timestamp value
 * @param res - Time Stamp resolution: When set to 0,time stamp resolution is 1 µs.(default)  
 *              When set to 1, resolution is 16µs.
 * @param delta - Time Stamp delta enable: When set to 1, the time stamp field 
 *                contains the measurement of time since the last occurrence of ODR.
 *                Note: This functionality is only available for timestamp in FIFO.
 * @param fsync - Time Stamp register FSYNC enable (default). 
 *                When set to 1, the contents of the Timestamp feature of FSYNC is enabled. 
 *                The user also needs to select FIFO_TMST_FSYNC_EN in order to propagate 
 *                the timestamp value to the FIFO.
 * @param tmst_en - 0: Time Stamp register disable
 *                  1: Time Stamp register enable (default)
*/
int icm4x6xx_set_tmst_config(inv_icm4x6xx_t *dev,
                             TMST_CONFIG_TMST_TO_REGS_EN_t regs_en,
                             TMST_CONFIG_RESOL_t res,
                             TMST_CONFIG_DELTA_t delta,
                             TMST_CONFIG_TMST_FSYNC_EN_t fsync,
                             TMST_CONFIG_TMST_EN_t tmst_en)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_TMST_CONFIG, 0};

    buf[1] = regs_en | res | delta | fsync | tmst_en;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->write_reg(driver, buf, 1);
}

// 14.43
/**
 * @param dmp - 0: DMP power save mode not active
 *              1: DMP power save mode active (default)
 * @param tap - 0: Tap Detection not enabled
 *              1: Tap Detection enabled when accelerometer ODR
 *                 is set to one of the ODR values supported by
 *                 Tap Detection (200Hz, 500Hz, 1kHz)
 * @param ped - 0: Pedometer not enabled; 1: Pedometer enabled
 * @param tilt - 0: Tilt Detection not enabled; 1: Tilt Detection enabled
 * @param r2w - 0: Raise to Wake/Sleep not enabled
 *              1: Raise to Wake/Sleep enabled
 *              Notice-accel odr should bigger than 50hz, DMP_ODR=50hz
 * @param odr - 00: 25Hz; 01: Reserved ;10: 50Hz;11: Reserved
*/
int icm4x6xx_set_apex_config0(inv_icm4x6xx_t *dev, 
                              APEX_CONFIG0_DMP_POWER_SAVE_t dmp,
                              APEX_CONFIG0_TAP_ENABLE_t tap,
                              APEX_CONFIG0_PEDO_EN_t ped,
                              APEX_CONFIG0_TILT_EN_t tilt,
                              APEX_CONFIG0_R2W_EN_t r2w,
                              APEX_CONFIG0_DMP_ODR_t odr)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_APEX_CONFIG0, 0};

    buf[1] = dmp | tap | ped | tilt | r2w | odr;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->write_reg(driver, buf, 1);
}


int icm4x6xx_set_raise_to_wake_state(inv_icm4x6xx_t *dev, APEX_CONFIG0_R2W_EN_t state)
{
    DEV_ASSERT(dev);
    int ret;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_APEX_CONFIG0, 0};

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    ret = driver->read_reg(driver, buf, 1);
    buf[1] &= (~BIT_APEX_CONFIG0_R2W_STATE_MASK);
    buf[1] |= state;
    ret |= driver->write_reg(driver, buf, 1);

    return ret;
}


int icm4x6xx_set_dmp_odr(inv_icm4x6xx_t *dev, APEX_CONFIG0_DMP_ODR_t odr)
{
    DEV_ASSERT(dev);
    int ret;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_APEX_CONFIG0, 0};

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    ret = driver->read_reg(driver, buf, 1);
    buf[1] &= (~BIT_APEX_CONFIG0_DMP_ODR_MASK);
    buf[1] |= odr;
    ret |= driver->write_reg(driver, buf, 1);

    return ret;
}

// 14.44
// TODO

// 14.45
/**
 *
 * @param wm - Trigger FIFO watermark interrupt on every ODR 
 *             (DMA write) if FIFO_COUNT ≥ FIFO_WM_TH.
 * @param hires - Enable 3 bytes of extended 20-bits accel, gyro data + 1 byte of extended 
 *                16-bit temperature sensor data to be placed into the FIFO.
 * @param fsync - Must be set to 1 for all FIFO use cases when FSYNC is used.
 * @param temp - Enable temperature sensor packets to go to FIFO.
 * @param gyro - Enable gyroscope packets to go to FIFO
 * @param accel - Enable accelerometer packets to go to FIFO
*/
int icm4x6xx_set_fifo_config1(inv_icm4x6xx_t *dev,
                              FIFO_CONFIG1_PARTIAL_RD_t partial,
                              FIFO_CONFIG1_WM_GT_t wm,
                              FIFO_CONFIG1_HIRES_t hires,
                              FIFO_CONFIG1_TMST_FSYNC_t fsync,
                              FIFO_CONFIG1_TEMP_t temp,
                              FIFO_CONFIG1_GYRO_t gyro,
                              FIFO_CONFIG1_ACCEL_t accel)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_FIFO_CONFIG1, 0}; 

    buf[1] = partial | wm | hires | fsync | temp | gyro | accel;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->write_reg(driver, buf, 1);  
}


// 14.46/47
/**
 * Generate interrupt when the FIFO reaches or exceeds FIFO_WM size in bytes 
 * or records according to FIFO_COUNT_REC setting. FIFO_WM_EN must be zero 
 * before writing this register. Interrupt only fires once. This register 
 * should be set to non-zero value, before choosing this interrupt source. 
 */
int icm4x6xx_set_fifo_watermark(inv_icm4x6xx_t *dev, uint16_t fifo_wm)
{
    DEV_ASSERT(dev);
    int ret = 0;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_FIFO_CONFIG2, 0}; 

    buf[1] = fifo_wm & 0xFF;
    ret = driver->write_reg(driver, buf, 1);

    buf[0] = REG_FIFO_CONFIG3; 
    buf[1] = (fifo_wm >> 8) & 0xFF;

    ret = driver->write_reg(driver, buf, 1);
    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);    

    return ret;   
}


int icm4x6xx_get_fifo_watermark(inv_icm4x6xx_t *dev, uint16_t *fifo_wm)
{
    DEV_ASSERT(dev);
    int ret = 0;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_FIFO_CONFIG2, 0}; 

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    ret = driver->read_reg(driver, buf, 1);
    *fifo_wm = buf[1];

    buf[0] = REG_FIFO_CONFIG3;
    ret |= driver->read_reg(driver, buf, 1);
    *fifo_wm |= ((buf[1] & 0x00FF) << 8);

    return ret;
}


// 14.48 FSYNC_CONFIG
// TODO


// 14.49 INT CONFIG0
// TODO


// 14.50 INT CONFIG1
/*
 * User should change setting to 0 from default setting of 1, for proper INT1
 * and INT2 pin operation
*/
int icm4x6xx_set_int_config1(inv_icm4x6xx_t *dev, INT_CONFIG1_ASY_RST_t state)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_INT_CONFIG1, state};
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->write_reg(driver, buf, 1);
}


int icm4x6xx_get_int_config1(inv_icm4x6xx_t *dev, uint8_t *state)
{
    DEV_ASSERT(dev);
    int ret = 0;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_INT_CONFIG1, 0x00};
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    ret = driver->read_reg(driver, buf, 1);
    *state = buf[1];

    return ret;
}


int icm4x6xx_enable_int_pin_operation(inv_icm4x6xx_t *dev)
{
    return icm4x6xx_set_int_config1(dev, INT_CONFIG1_ASY_RST_ENABLED);
}

// 14.51 INT SOURCE0
/**
 * @param flag - the type of INT_SOURCE0_FLAG_t;, can multiple value combine with '|'
 *                 when you want set multiple interrupt source
*/
int icm4x6xx_enable_source0_interrupt(inv_icm4x6xx_t *dev, uint8_t flag)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_INT_SOURCE0, flag};
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    return driver->write_reg(driver, buf, 1);
}


/**
 * @param flag - the type of INT_SOURCE0_FLAG_t;, can multiple value combine with '|'
 *                 when you want set multiple interrupt source
*/
int icm4x6xx_disable_source0_interrupt(inv_icm4x6xx_t *dev, uint8_t flag)
{
    DEV_ASSERT(dev);
    uint8_t temp;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_INT_SOURCE0, flag};
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    driver->read_reg(driver, buf, 1);
    temp = buf[1];
    
    temp &= ~flag;
    buf[0] = REG_INT_SOURCE0;

    return driver->write_reg(driver, buf, 1);
}


int icm4x6xx_get_int_source0(inv_icm4x6xx_t *dev, uint8_t *flag)
{
    DEV_ASSERT(dev);
    int ret = 0;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_INT_SOURCE0, 0x00};
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);

    ret = driver->read_reg(driver, buf, 1);
    *flag = buf[1];

    return ret;
}

// 14.52 INT SOURCE1
// TODO

// 14.53 INT SOURCE3
// TODO

// 14.54 INT SOURCE4
// TODO

// 14.55 FIFO_LOST_PKT0
// TODO

// 14.56 FIFO_LOST_PKT1
// TODO


// 14.57 SELF_TEST_CONFIG
// TODO

// 14.58
bool icm4x6xx_get_who_am_i(inv_icm4x6xx_t *dev)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_WHO_AM_I, 0};

    icm4x6xx_bank_check(dev, BANK_SEL_BANK0);
    driver->read_reg(dev->driver, buf, 1);

    return (ICM_WHOAMI == buf[1]) ? true: false;
}


// 14.59 REG_BANK_SEL
/**
 *
 * Note:This register is accessible from all register banks
*/
int icm4x6xx_set_bank_sel(inv_icm4x6xx_t *dev, BANK_SEL_DEF_t bank)
{
    DEV_ASSERT(dev);
    int ret;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_REG_BANK_SEL, bank};
    SENSOR_INFO("%s\r\n", __func__);

    ret = driver->write_reg(driver, buf, 1);
    if(ret == SENSOR_SUCCESS)
    {
        dev->bank = bank;
    }

    return ret;
}


/*
 * check the bank before R/W every register, if current bank is not
 * select for this register, change it.
*/
int icm4x6xx_bank_check(inv_icm4x6xx_t *dev, BANK_SEL_DEF_t bank)
{
    int ret = SENSOR_SUCCESS;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_REG_BANK_SEL, 0};
    SENSOR_INFO("%s\r\n", __func__);

    ret = driver->read_reg(driver, buf, 1);
    if(ret == SENSOR_SUCCESS && buf[1] != bank)
    {
        ret = icm4x6xx_set_bank_sel(dev, bank);
    }

    return ret;
}


/* USER BANK 1 REGISTER MAP – DESCRIPTIONS */
// 15.1 SENSOR_CONFIG0
// TODO

// 15.2 GYRO_CONFIG_STATIC2
// TODO

// 15.3 GYRO_CONFIG_STATIC3
// TODO

// 15.4 GYRO_CONFIG_STATIC4
// TODO

// 15.5 GYRO_CONFIG_STATIC5
// TODO

// 15.6 GYRO_CONFIG_STATIC6
// TODO

// 15.7 GYRO_CONFIG_STATIC7
// TODO

// 15.8 GYRO_CONFIG_STATIC8
// TODO

// 15.9 GYRO_CONFIG_STATIC9
// TODO

// 15.10 GYRO_CONFIG_STATIC10
// TODO

// 15.11 XG_ST_DATA
// TODO

// 15.12 YG_ST_DATA
// TODO

// 15.13 ZG_ST_DATA
// TODO

// 15.14 TMSTVAL0
// TODO

// 15.15 TMSTVAL1
// TODO

// 15.16 TMSTVAL2
// TODO

// 15.17 INTF_CONFIG4
// TODO

// 15.18 INTF_CONFIG5
// TODO

// 15.19 INTF_CONFIG6
/*
 * @param config - combine ele in INTF_CONFIG6_TYPE
*/
int icm4x6xx_set_intf_config(inv_icm4x6xx_t *dev, uint8_t config)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_INTF_CONFIG6_B1, config};
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK1);

    return driver->write_reg(dev->driver, buf, 1);
}


int icm4x6xx_get_intf_config(inv_icm4x6xx_t *dev, uint8_t *config)
{
    DEV_ASSERT(dev);
    int ret = 0;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_INTF_CONFIG6_B1, 0};
    SENSOR_INFO("%s\r\n", __func__);

    icm4x6xx_bank_check(dev, BANK_SEL_BANK1);

    ret = driver->read_reg(dev->driver, buf, 1);
    *config = buf[1];

    return ret;
}

/* 16 USER BANK 2 REGISTER MAP – DESCRIPTIONS */
// 16.1 ACCEL_CONFIG_STATIC2
// TODO

// 16.2 ACCEL_CONFIG_STATIC3
// TODO

// 16.3 ACCEL_CONFIG_STATIC4
// TODO

// 16.4 XA_ST_DATA
// TODO

// 16.5 YA_ST_DATA
// TODO

// 16.6 ZA_ST_DATA
// TODO

/************** 17 USER BANK 4 REGISTER MAP – DESCRIPTIONS **********************/
// 17.1 APEX_CONFIG1
// TODO

// 17.2 APEX_CONFIG2
// TODO

// 17.3 APEX_CONFIG3
// TODO


/*
 * @brief 17.4 APEX_CONFIG4
 * Configures duration of delay after tilt is detected before
 * interrupt is triggered
*/
int icm4x6xx_set_apex_config4(inv_icm4x6xx_t *dev, APEX_CONFIG4_TILT_WAIT_TIME_t time)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_APEX_CONFIG4_B4, 0};

    buf[1] = time;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK4);

    return driver->write_reg(driver, buf, 1);
}


// 17.5 APEX_CONFIG7
// REG_APEX_CONFIG5_B4

// 17.6 APEX_CONFIG8
// TODO

// 17.7 APEX_CONFIG9
// TODO

// 17.8 ACCEL_WOM_X_THR
// TODO

// 17.9 ACCEL_WOM_Y_THR
// TODO

// 17.10 ACCEL_WOM_Z_THR
// TODO

// 17.11 INT_SOURCE6
/*
 * @param flag - combine the type in ROUTED_TO_INT1_STATE_t with '|'
*/
int icm4x6xx_set_int_config6(inv_icm4x6xx_t *dev, uint8_t flag)
{
    DEV_ASSERT(dev);
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_INT_SOURCE6_B4, 0};

    buf[1] = flag;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK4);

    return driver->write_reg(driver, buf, 1);
}


int icm4x6xx_clear_int_config6(inv_icm4x6xx_t *dev, uint8_t flag)
{
    DEV_ASSERT(dev);
    int ret;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_INT_SOURCE6_B4, 0};

    buf[1] = flag;
    icm4x6xx_bank_check(dev, BANK_SEL_BANK4);

    ret = driver->read_reg(driver, buf, 1);
    buf[1] &= (~flag);  // clear
    ret |= driver->write_reg(driver, buf, 1);

    return ret;
}

// 17.12 INT_SOURCE7
// TODO

// 17.13 INT_SOURCE8
// TODO

// 17.14 INT_SOURCE9
// TODO

// 17.15 INT_SOURCE10
// TODO

// 17.16/17.17/17.18/17.19/17.20/17.21/17.22/17.23/17.24
int icm4x6xx_set_user_offset(inv_icm4x6xx_t *dev, user_acc_gyro_offset_t *offset)
{
    DEV_ASSERT(dev);
    int ret;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[] = {REG_OFFSET_USER_0_B4, offset->gyro_x_l,                    /*0x77*/
                     COMBINE_OFFSET_BYTE(offset->gyro_y_h, offset->gyro_x_h),   /*0x78*/
                     offset->gyro_y_l, offset->gyro_z_l,                        /* 0x79, 0x7A*/
                     COMBINE_OFFSET_BYTE(offset->accel_x_h, offset->accel_z_h), /* 0x7B */
                     offset->accel_x_l, offset->accel_y_l,                      /* 0x7C, 0x7D */
                     COMBINE_OFFSET_BYTE(offset->accel_z_h, offset->accel_y_h), /* 0x7E */
                     offset->accel_z_l};                                        /* 0x7F */

    return driver->write_reg(driver, buf, sizeof(user_acc_gyro_offset_t));
}

int icm4x6xx_get_user_offset(inv_icm4x6xx_t *dev, user_acc_gyro_offset_t *offset)
{
    DEV_ASSERT(dev);
    int ret;
    sensor_serial_if_t *driver = dev->driver;
    uint8_t buf[sizeof(user_acc_gyro_offset_t) + 1] = {REG_OFFSET_USER_0_B4};

    ret = driver->read_reg(driver, buf, sizeof(user_acc_gyro_offset_t));
    offset->gyro_x_l = buf[1];
    offset->gyro_y_h = SPLIT_BYTE_HIGHER(buf[2]);
    offset->gyro_x_h = SPLIT_BYTE_LOWER(buf[2]);
    offset->gyro_y_l = buf[3];
    offset->gyro_z_l = buf[4];
    offset->gyro_z_h = SPLIT_BYTE_LOWER(buf[5]);

    offset->accel_x_h = SPLIT_BYTE_HIGHER(buf[5]);
    offset->accel_x_l = buf[6];

    offset->accel_y_l = buf[7];
    offset->accel_z_h = SPLIT_BYTE_HIGHER(buf[8]);
    offset->accel_y_h = SPLIT_BYTE_LOWER(buf[8]);

    offset->accel_z_l = buf[9];

    return ret;
}

/* BANK 4 Register is finished */


int icm4x6xx_typical_init(inv_icm4x6xx_t *dev, typical_init_t *init)
{
    int ret = SENSOR_SUCCESS;

    if(init == NULL)
    {
        return SENSOR_ERROR_ARGS;
    }
    /* 0x50, Set accelerometer ODR */
    ret = icm4266x_set_accel_config0(dev,
                                     init->accel_full_scale,
                                     init->accel_odr);
    SENSOR_RESULT_CHECK(ret);

    ret = icm4x6xx_set_pwr_mgmt0(dev,
                                 init->temp_state,
                                 init->idle,
                                 init->gyro_mode,
                                 init->accel_mode);// 4E
    SENSOR_RESULT_CHECK(ret);
    ret = icm4x6xx_set_intf_config1(dev,
                                    init->lp_clk,
                                    init->rtc_mode,
                                    init->clksel); // 4d
    SENSOR_RESULT_CHECK(ret);
    ret = icm4x6xx_set_dmp_odr(dev, init->dmp_odr); // 56
    SENSOR_RESULT_CHECK(ret);

    return SENSOR_SUCCESS;
err:
    return ret;
}


int icm4x6xx_apex_init(inv_icm4x6xx_t *dev, apex_init_t *init)
{
    int ret = SENSOR_SUCCESS;

    if(init == NULL)
    {
        return SENSOR_ERROR_ARGS;
    }
    icm4x6xx_set_signal_path_reset(dev,
                                   init->dmp_init_state,
                                   init->dmp_mem_reset_state,
                                   init->reset,
                                   init->tmst,
                                   init->fifo_flush);
    sensor_delay_ms(1);

    ret = icm4x6xx_set_apex_config4(dev, init->tilt_wait_time);
    SENSOR_RESULT_CHECK(ret);
    sensor_delay_ms(1);
    ret = icm4x6xx_dmp_state(dev, SIGNAL_PATH_RESET_DMP_INIT_EN);
    SENSOR_RESULT_CHECK(ret);
    sensor_delay_ms(1);
    ret = icm4x6xx_set_int_config6(dev, init->int_source6_flag);
    SENSOR_RESULT_CHECK(ret);
    sensor_delay_ms(50);
    ret = icm4x6xx_set_raise_to_wake_state(dev, init->r2w_state);
    SENSOR_RESULT_CHECK(ret);

    return SENSOR_SUCCESS;
err:
    return ret;
}


/*
 * initialize sensor according to the spec in 8.5 RAISE TO WAKE/SLEEP PROGRAMMING
*/
int icm4x6xx_raise_to_wake_init(inv_icm4x6xx_t *dev)
{
    int ret;
    apex_init_t apex;
    typical_init_t typical;

    /* Set accelerometer ODR (Register 0x50h in Bank 0)
       ACCEL_ODR = 10 for 25 Hz*/
    typical.accel_full_scale = ACCEL_CONFIG0_FS_SEL_4g;
    typical.accel_odr = ACCEL_CONFIG0_ODR_500_HZ;

    typical.temp_state = PWR_MGMT0_TEMP_DIS;
    /* when Accel and Gyro are powered off, the chip will go to
       OFF state, since the RC oscillator will also be powered off*/
    typical.idle = PWR_MGMT0_IDLE_EN;
    typical.gyro_mode = PWR_MGMT0_GYRO_MODE_OFF;
    /* Set Accel to Low Power mode (Register 0x4Eh in Bank 0)
       ACCEL_MODE = 2 and (Register 0x4Dh in Bank 0),
       ACCEL_LP_CLK_SEL = 0, for low power mode*/
    typical.accel_mode = PWR_MGMT0_ACCEL_MODE_LP;

    //  ACCEL_LP_CLK_SEL = 0, for low power mode
    typical.lp_clk = INTF_CONFIG1_ACCEL_LP_CLK_WUOSC;
    typical.rtc_mode = INTF_CONFIG1_NO_RTC_INPUT;
    typical.clksel = INTF_CONFIG1_INTERNAL_RC_OSC;

    /* Set DMP ODR */
    typical.dmp_odr = APEX_CONFIG0_DMP_ODR_50Hz;

    /* Set DMP_MEM_RESET_EN to 1 (Register 0x4Bh in Bank 0) */
    apex.dmp_mem_reset_state = SIGNAL_PATH_RESET_DMP_MEM_RESET_EN;
    /* Configures duration of delay after tilt is detected before
       interrupt is triggered----no wait*/
    apex.tilt_wait_time = APEX_CONFIG4_TILT_WAIT_TIME_0;
    /* Enable Raise to Wake/Sleep, source for INT1 by setting bit 2,1
       in register INT_SOURCE6 (Register 0x4Dh in Bank 4)*/
    apex.int_source6_flag = SOURCE6_WAKE_DET_INT1_FLAG | SOURCE6_SLEEP_DET_INT1_FLAG;
    /* Turn on Raise to Wake/Sleep feature */
    apex.r2w_state = APEX_CONFIG0_R2W_STATE_EN;

    ret = icm4x6xx_typical_init(dev, &typical);
    SENSOR_RESULT_CHECK(ret);
    ret = icm4x6xx_apex_init(dev, &apex);
    SENSOR_RESULT_CHECK(ret);

    icm4x6xx_enable_int_pin_operation(dev);
    return SENSOR_SUCCESS;
err:
    return ret;
}


int icm4x6xx_device_fifo_init(inv_icm4x6xx_t *dev)
{
    int ret = 0;

    if(!icm4x6xx_get_who_am_i(dev))
    {
        return SENSOR_ERROR_CHECK_ID;
    }
    /* Configure  what data is placed into the FIFO*/
    FIFO_CONFIG1_HIRES_t hires;
    FIFO_CONFIG1_TMST_FSYNC_t fsync = FIFO_CONFIG1_TMST_FSYNC_DIS;
    FIFO_CONFIG1_TEMP_t temp;
    FIFO_CONFIG1_GYRO_t gyro;
    FIFO_CONFIG1_ACCEL_t accel;
    
    if(dev->packet_type == FIFO_PACKET_FMT1)
    {
        hires = FIFO_CONFIG1_HIRES_DIS;
        temp = FIFO_CONFIG1_TEMP_EN;
        gyro = FIFO_CONFIG1_GYRO_DIS;
        accel = FIFO_CONFIG1_ACCEL_DIS;
    }
    else if(dev->packet_type == FIFO_PACKET_FMT2)
    {
        hires = FIFO_CONFIG1_HIRES_DIS;
        temp = FIFO_CONFIG1_TEMP_EN;
        gyro = FIFO_CONFIG1_GYRO_EN;
        accel = FIFO_CONFIG1_ACCEL_DIS;
    }
    else if(dev->packet_type == FIFO_PACKET_FMT3)
    {
        hires = FIFO_CONFIG1_HIRES_DIS;
        temp = FIFO_CONFIG1_TEMP_EN;
        gyro = FIFO_CONFIG1_GYRO_EN;
        accel = FIFO_CONFIG1_ACCEL_EN;
        fsync = FIFO_CONFIG1_TMST_FSYNC_EN;
    }
    else if(dev->packet_type == FIFO_PACKET_FMT4)
    {
        hires = FIFO_CONFIG1_HIRES_EN;
        temp = FIFO_CONFIG1_TEMP_EN;
        gyro = FIFO_CONFIG1_GYRO_EN;
        accel = FIFO_CONFIG1_ACCEL_EN;
        fsync = FIFO_CONFIG1_TMST_FSYNC_EN;
    }

    /* fifo packet configure */
    ret = icm4x6xx_set_fifo_config1(dev,
              FIFO_CONFIG1_PARTIAL_DIS,
              FIFO_CONFIG1_WM_GT_TH_EN,
              hires, fsync, temp, gyro, accel);
    /* this function place here is avoid warning:  #546-D: transfer of control
       bypasses initialization of:*/
    TMST_CONFIG_TMST_FSYNC_EN_t fsync_en = TMST_CONFIG_TMST_FSYNC_DIS;
    SENSOR_RESULT_CHECK(ret);
    
    if(fsync == FIFO_CONFIG1_TMST_FSYNC_EN)
    {
        fsync_en = TMST_CONFIG_TMST_FSYNC_EN;
    }
    ret = icm4x6xx_set_tmst_config(dev,
             TMST_CONFIG_TMST_TO_REGS_EN,
             TMST_CONFIG_RESOL_1us,
             TMST_CONFIG_DELTA_EN,
             fsync_en,
             TMST_CONFIG_TMST_EN);
    SENSOR_RESULT_CHECK(ret);
    /* set watermark for fifo*/
    ret = icm4x6xx_set_fifo_watermark(dev, dev->fifo_watermark);
    SENSOR_RESULT_CHECK(ret);
    /* configure interrupt source */
    ret = icm4x6xx_enable_source0_interrupt(dev, FIFO_THS_INT1_FLAG);
    SENSOR_RESULT_CHECK(ret);
    /* configure raise wakeup */

    SENSOR_RESULT_CHECK(ret);

    return ret;
err:
    SENSOR_ERR(sensor_error_str(ret));
    return ret;
}

int icm4x6xx_init_config(inv_icm4x6xx_t *dev)
{
    int ret = 0;
    uint8_t data;

    SENSOR_INFO("%s\r\n", __func__);

    ret = icm4x6xx_reset(dev);
    SENSOR_RESULT_CHECK(ret);
    /* en byte mode & little endian mode & disable spi or i2c interface */
    if(dev->interface == ICM_SPI)
    {
        SENSOR_INFO("SPI BUS\r\n");
        ret = icm4x6xx_set_driver_config(dev, I2C_SLEW_RATE_20NS_60NS,
                                         SPI_SLEW_RATE_SMALL_TAHN_2NS);
        SENSOR_RESULT_CHECK(ret);
        ret = icm4x6xx_get_intf_config(dev, &data);
        SENSOR_RESULT_CHECK(ret);
        ret = icm4x6xx_set_intf_config(dev, I3C_SDR_EN | I3C_DDR_EN | data);
        SENSOR_RESULT_CHECK(ret);
        data = SIFS_CFG_DISABLE_I2C | FIFO_HOLD_LAST_EN;
    }else if(dev->interface ==  ICM_I2C)
    {
        SENSOR_INFO("I2C BUS\r\n");
        ret = icm4x6xx_get_intf_config(dev, &data);
        SENSOR_RESULT_CHECK(ret);
        data &=(uint8_t) ~(I3C_SDR_EN | I3C_DDR_EN);
        ret = icm4x6xx_set_intf_config(dev, data);
        SENSOR_RESULT_CHECK(ret);
        data = SIFS_CFG_DISABLE_SPI | FIFO_HOLD_LAST_EN;
    }
    else
    {
        SENSOR_ERR("bus set error\r\n");
        return SENSOR_ERROR_ARGS;
    }
    ret = icm4x6xx_set_intf_config0(dev, data);
    SENSOR_RESULT_CHECK(ret);

    ret = icm4x6xx_set_interrupt_pin_state(dev, dev->int_cfg);
    SENSOR_RESULT_CHECK(ret);
    /* gyr odr & fs */
    ret = icm4266x_set_gyro_config0(dev, GYRO_CONFIG0_FS_SEL_2000dps, GYRO_CONFIG0_ODR_500_HZ);
    SENSOR_RESULT_CHECK(ret);
    /* acc odr & fs */
    ret = icm4266x_set_accel_config0(dev, ACCEL_CONFIG0_FS_SEL_8g, ACCEL_CONFIG0_ODR_50_HZ);
    SENSOR_RESULT_CHECK(ret);
    /* acc & gyro BW */
    // 14.40
    ret = icm4x6xx_set_gyro_accel_config0(dev,
                                    GYRO_ACCEL_CONFIG0_ACCEL_FILT_AVG_1,
                                    GYRO_ACCEL_CONFIG0_GYRO_FILT_BW_4);
    SENSOR_RESULT_CHECK(ret);
    /* acc & gyro UI filter @ 3th order */
    // ignore
    if(dev->fifo_mode_en == FIFO_MODE)
    {
        SENSOR_INFO("fifo mode\r\n");
        ret = icm4x6xx_set_fifo_config1(dev, FIFO_CONFIG1_PARTIAL_DIS,
                                      FIFO_CONFIG1_WM_GT_TH_EN,
                                      FIFO_CONFIG1_HIRES_DIS,
                                      FIFO_CONFIG1_TMST_FSYNC_EN,
                                      FIFO_CONFIG1_TEMP_EN,
                                      FIFO_CONFIG1_GYRO_EN,
                                      FIFO_CONFIG1_ACCEL_EN);
        SENSOR_RESULT_CHECK(ret);
        /* set wm_th=16 as default for byte mode */
        ret = icm4x6xx_set_fifo_watermark(dev, 16);
        SENSOR_RESULT_CHECK(ret);
        ret = icm4x6xx_set_fifo_config(dev, FIFO_CONFIG_MODE_BYPASS);
        SENSOR_RESULT_CHECK(ret);
        ret = icm4x6xx_enable_source0_interrupt(dev, FIFO_FULL_INT1_FLAG | FIFO_THS_INT1_FLAG);
        SENSOR_RESULT_CHECK(ret);
        dev->fifo_package_size = FIFO_16BYTES_PACKET_SIZE;
    }else
    {
        SENSOR_INFO("drdy mode\r\n");
        ret = icm4x6xx_enable_source0_interrupt(dev, DATA_DRDY_INT1_FLAG);
        SENSOR_RESULT_CHECK(ret);
        dev->dri_package_size = DRI_14BYTES_PACKET_SIZE;
    }
    /* async reset */
    icm4x6xx_enable_int_pin_operation(dev);
    SENSOR_RESULT_CHECK(ret);
    ret = icm4x6xx_get_pwr_mgmt0(dev, &dev->pwr_sta);
    SENSOR_RESULT_CHECK(ret);
err:
    SENSOR_ERR(sensor_error_str(ret));
    return ret;
}


int icm4x6xx_initialize(inv_icm4x6xx_t *dev)
{
    int ret;

    ret = icm4x6xx_init_config(dev);
    if(ret == SENSOR_SUCCESS)
    {
        dev->init_cfg = true;
        SENSOR_INFO("%s:Initialize success\r\n", dev->name);
    }
    else
    {
        ret = SENSOR_INIT_FAILURE;
    }

    return 0;
}





