#include <stdio.h>
#include <string.h>
#include "common.h"
#include "bmi270.h"
#include "uart.h"



static uint8_t i2c_bus;
struct bmi2_dev bmi270dev;

int8_t Open_BMI270_ACC(struct bmi2_dev *dev)
{
	int8_t rslt = BMI2_OK;
	uint8_t sensor_list = BMI2_ACCEL;
	/* Sensor configuration structure */
	struct bmi2_sens_config config = { 0 };

	/* Enable the selected sensors */
	rslt = bmi270_sensor_enable(&sensor_list, 1, dev);

	config.type = sensor_list;
	
	/* Get the previous or default configuration settings */
		/* Update all or any of the accelerometer
		configurations */
        config.cfg.acc.odr      = BMI2_ACC_ODR_200HZ;
        config.cfg.acc.range     = BMI2_ACC_RANGE_2G ;

        #if defined(SUPPORT_LOWPOWER)
        config.cfg.acc.filter_perf = BMI2_POWER_OPT_MODE;
        config.cfg.acc.bwp        = BMI2_ACC_OSR4_AVG1;
        #else
        config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;
        config.cfg.acc.bwp        = BMI2_ACC_NORMAL_AVG4;
        #endif
		/* Set the configurations */
		rslt = bmi2_set_sensor_config(&config, 1, dev);
		if (rslt != BMI2_OK) 
		{
			TRACE("ACC configuration failed, rslt=%d", rslt);
		} 
		else
		{
			TRACE("ACC configuration set successfully");

			/* Get the configuration settings for validation */
			rslt = bmi270_get_sensor_config(&config, 1, dev);
			if (rslt == BMI2_OK) 
			{
				TRACE("Get BMI2_ACCEL Configuration successful");
				TRACE("Performance Mode = %d", config.cfg.acc.filter_perf);
				TRACE("Bandwidth = %d", config.cfg.acc.bwp);
				TRACE("ODR = %d", config.cfg.acc.odr);
				TRACE("Range = %d", config.cfg.acc.range);
			}
		}


	return rslt;
}

int8_t Open_BMI270_GYRO(struct bmi2_dev *dev)
{
	int8_t rslt = BMI2_OK;
	uint8_t sensor_list = BMI2_GYRO;
	/* Sensor configuration structure */
	struct bmi2_sens_config config = { 0 };

	/* Enable the selected sensors */
	rslt = bmi270_sensor_enable(&sensor_list, 1, dev);
	TRACE("rslt2=%d",rslt);

	config.type = sensor_list;
	
	/* Get the previous or default configuration settings */
	rslt = bmi270_get_sensor_config(&config, 1, dev);
	if (rslt == BMI2_OK) 
	{
		config.cfg.gyr.odr = BMI2_GYR_ODR_100HZ;
		config.cfg.gyr.range = BMI2_GYR_RANGE_2000;
		config.cfg.gyr.ois_range = BMI2_GYR_OIS_2000;
		
		#if defined(SUPPORT_LOWPOWER)
		config.cfg.gyr.bwp = BMI2_GYR_OSR4_MODE;
		config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;
		config.cfg.gyr.filter_perf = BMI2_POWER_OPT_MODE;
		#else
		config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;
		config.cfg.gyr.noise_perf = BMI2_PERF_OPT_MODE;
		config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;
		#endif
	
		/* Set the configurations */
		rslt = bmi2_set_sensor_config(&config, 1, dev);
		if (rslt != BMI2_OK) 
		{
			TRACE("GYRO configuration failed");
		} 
		else
		{
			TRACE("GYRO configuration set successfully");
			/* Get the configuration settings for validation */
			rslt = bmi2_get_sensor_config(&config, 1, dev);
			/*if (rslt == BMI2_OK)
			{
				TRACE("Get BMI2_GYRO Configuration successful");
				TRACE("Bandwidth = %d", config.cfg.gyr.bwp);
				TRACE("ODR = %d", config.cfg.gyr.odr);
				TRACE("Range = %d", config.cfg.gyr.range);
				TRACE("OIS Range = %d", config.cfg.gyr.ois_range);
				TRACE("Noise_perf = %d", config.cfg.gyr.noise_perf);
				TRACE("Filter_perf = %d", config.cfg.gyr.filter_perf);
			}*/
		}
	}

	return rslt;
}

struct bmi2_sensor_data sensor_data = { 0 };

int Init_BMI270(void)
{

	int8_t rslt = BMI2_OK;


	/* Interface reference is given as a parameter
	* For I2C : BMI2_I2C_INTF
	* For SPI : BMI2_SPI_INTF
	*/


	i2c_bus = BMI2_I2C_PRIM_ADDR;
	bmi270dev.intf_ptr = &i2c_bus;
	bmi270dev.intf = BMI2_I2C_INTF;
	bmi270dev.read = bmi2xy_i2c_bus_read;
	bmi270dev.write = bmi2xy_i2c_bus_write;
	bmi270dev.read_write_len =2;
	bmi270dev.delay_us = bmi2_delay_us;
	bmi270dev.config_file_ptr = NULL;
	
	rslt = bmi270_init(&bmi270dev);

	/* Disable advanced power save */
	//rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi270dev);


	Open_BMI270_GYRO(&bmi270dev);
    //Open_BMI270_ACC(&bmi270dev);
	  rslt = bmi2_get_sensor_data(&sensor_data, 1, &bmi270dev);
	                        TRACE("Gyrx= %d", sensor_data.sens_data.gyr.x);
	                        TRACE("Gyry= %d", sensor_data.sens_data.gyr.y);
	                        TRACE("Gyrz= %d", sensor_data.sens_data.gyr.z);

	return rslt;
}
    void read()
    {   ;

    }