/*
 * 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)
 */


#ifndef __ICM426XX_DEVICE_H__
#define __ICM426XX_DEVICE_H__
#ifdef __cplusplus
extern "C" {
#endif

#include <stdint.h>
#include <string.h>
#include "platform_if.h"
#include "icm4x6xx_def.h"
#include "sensor_type.h"


typedef struct{
    uint8_t accel_x1;
    uint8_t accel_x0;

    uint8_t accel_y1;
    uint8_t accel_y0;

    uint8_t accel_z1;
    uint8_t accel_z0;

    uint8_t gyro_x1;
    uint8_t gyro_x0;

    uint8_t gyro_y1;
    uint8_t gyro_y0;

    uint8_t gyro_z1;
    uint8_t gyro_z0;
}imu_byte_data;


typedef struct{
    int16_t accel_x;
    int16_t accel_y;
    int16_t accel_z;

    int16_t gyro_x;
    int16_t gyro_y;
    int16_t gyro_z;
}imu_wh_data;

#define ICM426XX_DATA_SIZE     sizeof(imu_wh_data)

typedef union icm4x6xx_data{
    imu_byte_data data8;
    imu_wh_data data16;
    /* notice big endian or little endian */
    uint8_t fifo[ICM426XX_DATA_SIZE];
}icm4x6xx_data_t;

typedef struct
{
    uint8_t x1;
    uint8_t x0;

    uint8_t y1;
    uint8_t y0;

    uint8_t z1;
    uint8_t z0;
}accel_data;

typedef struct
{
    uint8_t x1;
    uint8_t x0;

    uint8_t y1;
    uint8_t y0;

    uint8_t z1;
    uint8_t z0;
}gyro_data;

typedef struct
{
    uint8_t header;
    accel_data accel;
    uint8_t temperature;
}fifo_packet1;


typedef struct
{
    uint8_t header;
    gyro_data gyro;
    uint8_t temperature;
}fifo_packet2;


typedef struct
{
    uint8_t header;
    accel_data accel;
    gyro_data gyro;
    uint8_t temperature;
    uint16_t timestamp;
}fifo_packet3;


typedef struct
{
    uint8_t header;
    accel_data accel;
    gyro_data gyro;
    uint16_t temperature;
    uint16_t timestamp;
    uint32_t extension:20;
}fifo_packet4;


typedef enum
{
    FIFO_PACKET_FMT1 = 0,
    FIFO_PACKET_FMT2,
    FIFO_PACKET_FMT3,
    FIFO_PACKET_FMT4,
}FIFO_PACKET_TYPE_t;


typedef struct
{
    /* Full scale select for accelerometer UI interface output */
    ACCEL_CONFIG0_FS_SEL_t accel_full_scale;
    ACCEL_CONFIG0_ODR_t accel_odr;

    /* PWR_MGMT0 - 0x4E */
    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;
    /* intf_config1 - 0x4D */
    INTF_CONFIG1_ACCEL_LP_CLK_t lp_clk;
    INTF_CONFIG1_RTC_MODE_t rtc_mode;
    INTF_CONFIG1_CLKSEL_t clksel;
    /* apex_config0 - 0x56*/
    APEX_CONFIG0_DMP_ODR_t dmp_odr;
}typical_init_t;


typedef struct
{
    /* 0x4B */
    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;
    
    /* 0x43 */
    APEX_CONFIG4_TILT_WAIT_TIME_t tilt_wait_time;
    /* 0x4D BANK4, combine type in ROUTED_TO_INT1_STATE_t */
    uint8_t int_source6_flag;
    /* 0x56 */
    APEX_CONFIG0_R2W_EN_t r2w_state;
}apex_init_t;

#define ICM4X6XX_MAX_FIFO_SIZE        2000  

typedef enum
{
    ICM_I2C = 0,
    ICM_SPI,
}INTERFACE_TYPE_t;

#define MAX_RECV_PACKET                      100

struct acc_gyro_data {
    uint8_t sens_type;
    float x, y, z;
    uint64_t timestamp;
};

typedef struct acc_gyro_data_packet {
    uint8_t acc_out_size;
    uint8_t gyro_out_size;
    uint64_t timestamp;
    float temperature;
    struct acc_gyro_data out_buf[MAX_RECV_PACKET];
    uint32_t magic_num;
}acc_gyro_data_packet_t;

/************** 17 USER BANK 4 REGISTER MAP type start******************/
typedef struct
{
    /* Gyro offset programmed by user. Max value is ±128 dps,
       resolution is 1/16 dps.*/
    uint8_t gyro_x_l;
    uint8_t gyro_x_h:4;

    uint8_t gyro_y_l;
    uint8_t gyro_y_h:4;

    uint8_t gyro_z_l;
    uint8_t gyro_z_h:4;

    /* Accel offset programmed by user. Max value is ±2g,
       resolution is 1mg */
    uint8_t accel_x_l;
    uint8_t accel_x_h:4;

    uint8_t accel_y_l;
    uint8_t accel_y_h:4;

    uint8_t accel_z_l;
    uint8_t accel_z_h:4;
}user_acc_gyro_offset_t;

#define COMBINE_OFFSET_BYTE(h, l)    ((h << 4)|(l))
#define SPLIT_BYTE_LOWER(d)          ((d) & 0x0F)
#define SPLIT_BYTE_HIGHER(d)         (((d) & 0xF0) >> 4)

/************** 17 USER BANK 4 REGISTER MAP type end******************/
typedef struct inv_icm4x6xx
{
    sensor_serial_if_t *driver;
    char *name;

    CHIP_TYPE_t product;
    float chip_temper;
    uint8_t interface; /* combine ele in INTERFACE_TYPE_t */
    uint8_t int_cfg; /* combin ele in INT_CONFIG_TYPE_t */

    /* For save reg status */
    uint8_t  int_src0;
    uint8_t  pwr_sta;
    uint8_t  acc_cfg0;
    uint8_t  gyro_cfg0;

    /* For fifo */
    uint16_t watermark;
    /* Caching raw data, first byte is register */
    uint8_t  data_buf[ICM4X6XX_MAX_FIFO_SIZE];
    uint32_t fifo_data_to_read;  /* mult fifo_package_size */
    DATA_READY_MODE_t fifo_mode_en;
    uint16_t fifo_package_size;
    uint16_t dri_package_size;

    bool init_cfg;
    /* only used in driver, user needn't configure it */
    BANK_SEL_DEF_t bank;

    FIFO_PACKET_TYPE_t packet_type;
    uint16_t fifo_watermark;
}inv_icm4x6xx_t;


/* 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);

int icm4x6xx_reset(inv_icm4x6xx_t *dev);

// 14.3 INT config
/*
 * INT1 and INT2 can be push-pull or open drain
 * Level or pulse mode
 * Active high or active low
 * @param config - combine ele in INT_CONFIG_TYPE_t
*/
int icm4x6xx_set_interrupt_pin_state(inv_icm4x6xx_t *dev, uint8_t config);

int icm4x6xx_get_int_config(inv_icm4x6xx_t *dev, uint8_t *config);

// 14.4 FIFO config
int icm4x6xx_set_fifo_config(inv_icm4x6xx_t *dev, 
                             FIFO_CONFIG_MODE_t mode);

// 14.4 FIFO config
int icm4x6xx_set_fifo_config(inv_icm4x6xx_t *dev, 
                            FIFO_CONFIG_MODE_t mode);

int icm4x6xx_read_degree_centigrade(inv_icm4x6xx_t *dev, float *degree);

// 14.7/8/9/10/11/12/1314/15/16/17/18 ACCEL DATA,
int icm4x6xx_read_raw_data(inv_icm4x6xx_t *dev);
// 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);


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

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

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

/**
 * 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);

// 14.24
int icm4x6xx_get_data_in_fifo(inv_icm4x6xx_t *dev);

// 14.31
int icm4x6xx_get_int_status2(inv_icm4x6xx_t *dev, uint8_t *status);

// 14.32
int icm4x6xx_get_int_status3(inv_icm4x6xx_t *dev, uint8_t *status);

// 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);

int icm4x6xx_flush_fifo(inv_icm4x6xx_t *dev);

/*
 * enable/disable dmp
*/
int icm4x6xx_dmp_state(inv_icm4x6xx_t *dev, SIGNAL_PATH_RESET_DMP_INIT_t state);


// 14.34

int icm4x6xx_set_intf_config0(inv_icm4x6xx_t *dev, uint8_t config);

int icm4x6xx_get_intf_config0(inv_icm4x6xx_t *dev, uint8_t *status);


// 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);


//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);

int icm4x6xx_get_pwr_mgmt0(inv_icm4x6xx_t *dev, uint8_t* config);

// 14.37
int icm4266x_set_gyro_config0(inv_icm4x6xx_t *dev,
                              GYRO_CONFIG0_FS_SEL_t sel,
                              GYRO_CONFIG0_ODR_t odr);

int icm4266x_get_gyro_config0(inv_icm4x6xx_t *dev,uint8_t *config);

// 14.38
int icm4266x_set_accel_config0(inv_icm4x6xx_t *dev,
                               ACCEL_CONFIG0_FS_SEL_t sel,
                               ACCEL_CONFIG0_ODR_t odr);

int icm4266x_get_accel_config0(inv_icm4x6xx_t *dev, uint8_t *config);

// 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);

// 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);


// 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);

int icm4x6xx_set_raise_to_wake_state(inv_icm4x6xx_t *dev, APEX_CONFIG0_R2W_EN_t state);

int icm4x6xx_set_dmp_odr(inv_icm4x6xx_t *dev, APEX_CONFIG0_DMP_ODR_t odr);

// 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);

// 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);

int icm4x6xx_get_fifo_watermark(inv_icm4x6xx_t *dev, uint16_t *fifo_wm);

int icm4x6xx_set_int_config1(inv_icm4x6xx_t *dev, INT_CONFIG1_ASY_RST_t state);

int icm4x6xx_enable_int_pin_operation(inv_icm4x6xx_t *dev);

int icm4x6xx_get_int_config1(inv_icm4x6xx_t *dev, uint8_t *state);

// 14.51 INT SOURCE0
/**
 * @param source - 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 source);

int icm4x6xx_disable_source0_interrupt(inv_icm4x6xx_t *dev, uint8_t flag);

int icm4x6xx_get_int_source0(inv_icm4x6xx_t *dev, uint8_t *flag);

// 14.58
bool icm4x6xx_get_who_am_i(inv_icm4x6xx_t *dev);

// 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);


// 15.19 INTF_CONFIG6
int icm4x6xx_set_intf_config(inv_icm4x6xx_t *dev, uint8_t config);

int icm4x6xx_get_intf_config(inv_icm4x6xx_t *dev, uint8_t *config);
/*
 * @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);

/************** 17 USER BANK 4 REGISTER MAP – DESCRIPTIONS **********************/

// 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);


int icm4x6xx_clear_int_config6(inv_icm4x6xx_t *dev, uint8_t flag);

int icm4x6xx_set_user_offset(inv_icm4x6xx_t *dev, user_acc_gyro_offset_t *offset);

int icm4x6xx_get_user_offset(inv_icm4x6xx_t *dev, user_acc_gyro_offset_t *offset);
/* register function end */


/* Function init API */
int icm4x6xx_typical_init(inv_icm4x6xx_t *dev, typical_init_t *init);

int icm4x6xx_apex_init(inv_icm4x6xx_t *dev, apex_init_t *init);

/*
 * initialize sensor according to the spec in 8.5 RAISE TO WAKE/SLEEP PROGRAMMING
*/
int icm4x6xx_raise_to_wake_init(inv_icm4x6xx_t *dev);

/*
 * 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 icm4x6xx_device_fifo_init(inv_icm4x6xx_t *self);

int icm4x6xx_init_config(inv_icm4x6xx_t *dev);


int icm4x6xx_initialize(inv_icm4x6xx_t *dev);

#ifdef __cplusplus
}
#endif


#endif /* __ICM426XX_DEVICE_H__ */


