#include <stdint.h>
#include "sensor_common.h"
#include "wx_common.h"
#include "twi_api.h"
#include "bma456_an.h"
#include "common.h"
#include "nrf_drv_twi.h"


static struct bma4_dev bma = { 0 };
static struct bma4_accel_config accel_conf = { 0 };
static struct bma456_an_any_no_mot_config any_no_mot = { 0 };
static struct bma4_int_pin_config bma4_int_pin_config = { 0 };

//static float lsb_to_ms2(int16_t val, float g_range, uint8_t bit_width)
//{
//    double power = 2;

//    float half_scale = (float)((pow((double)power, (double)bit_width) / 2.0f));

//    return (GRAVITY_EARTH * val * g_range) / half_scale;
//}

void bma456_x_test(const void *param)
{
    struct bma4_accel sens_data = { 0 };

    bma4_read_accel_xyz(&sens_data, &bma);

    uint16_t init_state = 0;
    bma4_read_int_status(&init_state, &bma);
    rtt_app_print("accel x: %d, y: %d, z: %d, int_status: %d\n", sens_data.x, sens_data.y, sens_data.z, init_state);
}

void bma456_x_ctrl(uint8_t ctrl)
{
    if (ctrl == 0) {
        any_no_mot.axes_en = BMA456_AN_DIS_ALL_AXIS;
    } else {
        any_no_mot.axes_en = BMA456_AN_EN_ALL_AXIS;
    }
    bma456_an_set_any_mot_config(&any_no_mot, &bma);

    rtt_app_print("bma456_x_ctrl: %d\n", ctrl);
}

const nrf_drv_twi_t m_twi = NRF_DRV_TWI_INSTANCE(1);

//#define PWR_REG_RESET

void bma456_x_init(void)
{
    //OBS_REG(OBS_EVT_100MS, bma456_x_test);
    int8_t rslt;
	
	const nrf_drv_twi_config_t twi_config = {
        .scl                = 7,
        .sda                = 8,
        .frequency          = NRF_DRV_TWI_FREQ_400K,
        .interrupt_priority = APP_IRQ_PRIORITY_HIGH,
        .clear_bus_init     = false
    };
	//TWIM1
    nrf_drv_twi_init(&m_twi, &twi_config, NULL, NULL);
    nrf_drv_twi_enable(&m_twi);
	//hal_twi1_init();
	nrf_delay_ms(100);
	
	uint8_t who_am_i;
	twi_read_reg1(0x18, 0x00, &who_am_i, 1);
	rtt_app_print("who_am_i: 0x%x\n", who_am_i);
	
	nrf_delay_ms(100);

    bma4_interface_init(&bma, BMA4_I2C_INTF, BMA45X_VARIANT);

#if 1
    rslt = bma4_soft_reset(&bma);
    nrf_delay_us(2000);
	
	who_am_i = 0;
	twi_read_reg1(0x18, 0x00, &who_am_i, 1);
	rtt_app_print("who_am_i: 0x%x\n", who_am_i);
	
    if (rslt != BMA4_OK) {
        rtt_app_print("bma4_soft_reset failed: %d\n", rslt);
    }

    rslt = bma456_an_init(&bma);
    if (rslt != BMA4_OK) {
        rtt_app_print("bma456_an_init failed: %d\n", rslt);
    }

    rslt = bma456_an_write_config_file(&bma);
	if (rslt != BMA4_OK) {
		rtt_app_print("bma456_an_write_config failed: %d\n", rslt);
	}
    
    accel_conf.odr = BMA4_OUTPUT_DATA_RATE_25HZ;
    accel_conf.range = BMA4_ACCEL_RANGE_16G;
    accel_conf.bandwidth = BMA4_ACCEL_NORMAL_AVG4;
    accel_conf.perf_mode = BMA4_CONTINUOUS_MODE;
    rslt = bma4_set_accel_config(&accel_conf, &bma);
    if (rslt != BMA4_OK) {
        rtt_app_print("bma4_set_accel_config failed: %d\n", rslt);
    }
	
	bma4_set_advance_power_save(BMA4_DISABLE, &bma);

    rslt = bma4_set_accel_enable(1, &bma);  
    if (rslt != BMA4_OK) {
        rtt_app_print("bma4_set_accel_enable failed: %d\n", rslt);
    }

    bma4_int_pin_config.input_en = BMA4_INPUT_DISABLE;
    bma4_int_pin_config.output_en = BMA4_OUTPUT_ENABLE;
    bma4_int_pin_config.lvl = BMA4_ACTIVE_HIGH;
    bma4_int_pin_config.od = BMA4_PUSH_PULL;
    rslt = bma4_set_int_pin_config(&bma4_int_pin_config, BMA4_INTR1_MAP, &bma);
    if (rslt != BMA4_OK) {
        rtt_app_print("bma4_set_int_pin_config failed: %d\n", rslt);
    }

    rslt = bma456_an_map_interrupt(BMA4_INTR1_MAP, BMA456_AN_ANY_MOT_INT, BMA4_ENABLE, &bma);
    if (rslt != BMA4_OK) {
        rtt_app_print("bma456_an_map_interrupt failed: %d\n", rslt);
    }

    any_no_mot.threshold = 50; //1bit = 0.48mG
    any_no_mot.duration = 5;   
    any_no_mot.axes_en = BMA456_AN_EN_ALL_AXIS;
    rslt = bma456_an_set_any_mot_config(&any_no_mot, &bma);
    if (rslt != BMA4_OK) {
        rtt_app_print("bma456_an_set_any_mot_config failed: %d\n", rslt);
    }

    struct bma456_an_any_no_mot_config any_mot= { 0 };
    bma456_an_get_any_mot_config(&any_mot, &bma);
    rtt_app_print("any_mot threshold: %d, duration: %d, axes_en: %d\n", any_mot.threshold, any_mot.duration, any_mot.axes_en);
#endif
    rtt_app_print("BMA init finished\n");
	
#ifdef PWR_REG_RESET
	uint8_t pwr_conf = 0x03;
    uint8_t pwr_ctrl = 0x00;
    twi_write_data1(0x18, 0x7c, &pwr_conf, 1);
    twi_write_data1(0x18, 0x7d, &pwr_ctrl, 1);
#endif
	//acc_en=1, adv_power_save=0
	rslt = bma4_soft_reset(&bma);
    rtt_app_print("bma4_soft_reset ret: %d\n", rslt);
    nrf_delay_ms(10);
	
	//I2C bus is not working, cannot read ID
	who_am_i = 0;
	twi_read_reg1(0x18, 0x00, &who_am_i, 1);
	rtt_app_print("who_am_i1: 0x%x\n", who_am_i);

	who_am_i = 0;
	twi_read_reg1(0x18, 0x00, &who_am_i, 1);
	rtt_app_print("who_am_i2: 0x%x\n", who_am_i);
}