#include "lis2doc.h"
#include "gsensor_port.h"
#include "sys_config.h"
#include "sys_typedef.h"
#include "gsensor_api.h"
#include "service_gsensor.h"
#include <inttypes.h>

#define ENABLE_PRINT_INFO 1
#define ENABLE_DEBUG 0

#if ENABLE_PRINT_INFO
#define static_print_info(...) sys_gs_log_i(__VA_ARGS__)  // 一般信息打印宏控制
#define static_print_warn(...) sys_gs_log_w(__VA_ARGS__)  // 警告信息打印一般常开
#define static_print_error(...) sys_gs_log_e(__VA_ARGS__) // 错误信息打印一般常开
#if ENABLE_DEBUG
#define static_print_debug(...) sys_gs_log_d(__VA_ARGS__) // debug
#else
#define static_print_debug(...)
#endif
#else
#define static_print_info(...)
#define static_print_warn(...)
#define static_print_error(...)
#endif

static lis2doc_mode_t pm;
stmdev_ctx_t dev_ctx;
int16_t data_raw_acceleration[3];

uint32_t lis2doc_read_data_polling_fun(void)
{
    uint8_t val, i;
    gsen_data_t x, y, z;
    
    lis2doc_fifo_data_level_get(&dev_ctx, &val);

    for (i = 0; i < val; i++) {
        /* Read acceleration data */
        memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
        lis2doc_acceleration_raw_get(&dev_ctx, data_raw_acceleration);
        tjd_service_gs_add_xyz_fifo(i,data_raw_acceleration[0], data_raw_acceleration[1], data_raw_acceleration[2]);

        static_print_debug("raw [mg]:X=%d  Y=%d  Z=%d", data_raw_acceleration[0], data_raw_acceleration[1],
                            data_raw_acceleration[2]);

    }
    
    return val;
}

int32_t lis2doc_init(void)
{
    /* Initialize mems driver interface. */
    uint32_t ret;
    uint8_t val;
    lis2doc_reg_t int_route;
    dev_ctx.write_reg = gsensor_i2c_write_reg;
    dev_ctx.read_reg = gsensor_i2c_read_reg;
    dev_ctx.handle = NULL;
    /* Check device ID */
    uint8_t whoamI, rst;
    ret = lis2doc_device_id_get(&dev_ctx, &whoamI);
    if (ret != 0) {
        static_print_error("iic lis2doc_device_id_get fail");
        return ret;
    }

    if (whoamI != LIS2DOC_ID)
        static_print_error("lis2doc_device_id_get fail id:%x", whoamI);

    /* Restore default configuration */
    ret = lis2doc_reset_set(&dev_ctx, PROPERTY_ENABLE);
    if (ret != 0) {
        static_print_error("iic lis2doc_reset_set fail");
        return ret;
    }

    for (int i = 0; i < 100; i++) {
        ret = lis2doc_reset_get(&dev_ctx, &rst);
        if (ret != 0) {
            static_print_error("iic lis2doc_reset_get fail");
            return ret;
        }

        if (rst == 0) {
            static_print_info("lis2doc_reset_get succ.");
            break;
        }
        static_print_error("lis2doc_reset_get fail.");
    }

    /* Enable Block Data Update */
    ret = lis2doc_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
    if (ret != 0) {
        static_print_error("iic lis2doc_block_data_update_set fail");
        return ret;
    }

    /* Set full scale */
    ret = lis2doc_full_scale_set(&dev_ctx, LIS2DOC_8g);
    if (ret != 0) {
        static_print_error("iic lis2doc_full_scale_set fail");
        return ret;
    }

    /* Configure filtering chain
     * Accelerometer - filter path / bandwidth
     */
    ret = lis2doc_filter_path_set(&dev_ctx, LIS2DOC_LPF_ON_OUT);
    if (ret != 0) {
        static_print_error("iic lis2doc_filter_path_set fail");
        return ret;
    }
    ret = lis2doc_filter_bandwidth_set(&dev_ctx, LIS2DOC_ODR_DIV_4);
    if (ret != 0) {
        static_print_error("iic lis2doc_filter_bandwidth_set fail");
        return ret;
    }
    
    /* Configure FIFO */
    ret = lis2doc_fifo_watermark_set(&dev_ctx, 4);
    if (ret != 0) {
        static_print_error("iic lis2doc_fifo_watermark_set fail");
        return ret;
    }
    ret = lis2doc_fifo_mode_set(&dev_ctx, LIS2DOC_STREAM_MODE);
    if (ret != 0) {
        static_print_error("iic lis2doc_fifo_mode_set fail");
        return ret;
    }

    /* Configure power mode */
    memset(&pm, 0, sizeof(lis2doc_mode_t));
    pm.low_noise = 1;
    pm.lpm = 1;
    ret = lis2doc_power_mode_set(&dev_ctx, pm);
    if (ret != 0) {
        static_print_error("iic lis2doc_power_mode_set fail");
        return ret;
    }
    return 0;
}

int32_t lis2doc_open(void)
{
    /* Set Output Data Rate */
    return lis2doc_data_rate_set(&dev_ctx, LIS2DOC_XL_ODR_25Hz);
}

int32_t lis2doc_low_power(void)
{
    /* Configure power mode */
    memset(&pm, 0, sizeof(lis2doc_mode_t));
    pm.low_noise = 1;
    return lis2doc_power_mode_set(&dev_ctx, pm);
}

int32_t lis2doc_standard_power(void)
{
    /* Configure power mode */
    memset(&pm, 0, sizeof(lis2doc_mode_t));
    pm.hpm = 1;
    pm.low_noise = 1;
    return lis2doc_power_mode_set(&dev_ctx, pm);
}

int32_t lis2doc_power_down(void)
{
    return lis2doc_data_rate_set(&dev_ctx, LIS2DOC_XL_ODR_OFF);
}