#include 
#include "boards.h"
#include "app_util_platform.h"
#include "app_error.h"
#include "nrf_drv_twi.h"
#include "nrf_delay.h"


#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"

#include "bmi160.h"

/* TWI instance ID. */
#define TWI_INSTANCE_ID     0

struct bmi160_dev sensor;
struct bmi160_sensor_data accel;
struct bmi160_sensor_data gyro;

int8_t rslt = BMI160_OK;

/* Mode for LM75B. */
#define NORMAL_MODE 0U

/* Indicates if operation on TWI has ended. */
static volatile bool m_xfer_done = false;

/* TWI instance. */
static const nrf_drv_twi_t m_twi = NRF_DRV_TWI_INSTANCE(TWI_INSTANCE_ID);

/* Buffer for samples read from temperature sensor. */
static uint8_t m_sample;



/**
 * @brief UART initialization.
 */
void twi_init (void)
{
   // NRF_LOG_INFO("in twi\r\n");
    ret_code_t err_code;

    const nrf_drv_twi_config_t twi_lm75b_config = {
       .scl                = 12,//ARDUINO_SCL_PIN,
       .sda                = 11,//ARDUINO_SDA_PIN,
       .frequency          = NRF_DRV_TWI_FREQ_100K,
       .interrupt_priority = APP_IRQ_PRIORITY_HIGH,
       .clear_bus_init     = false
    };

    err_code = nrf_drv_twi_init(&m_twi, &twi_lm75b_config, NULL, NULL);
    APP_ERROR_CHECK(err_code);
       if (NRF_SUCCESS == err_code)
	{
		nrf_drv_twi_enable(&m_twi);
		NRF_LOG_INFO("TWI init success...");	
	}
nrf_delay_ms(5);
}

int8_t Acc_i2c_Write(uint8_t dev_id, uint8_t reg_addr, uint8_t *reg_data, uint16_t len)
{

      //  NRF_LOG_INFO("WRITE: dev_id: %x reg_addr: %x reg_data: %x len: %i\n", dev_id, reg_addr, *reg_data, len);
	int8_t rslt = 0;
	uint8_t data[len + 1];
	data[0] = reg_addr;
	for (uint16_t i = 0; i < len; i++) {
		data[i + 1] = reg_data[i];
	}
	
	rslt = nrf_drv_twi_tx(&m_twi, dev_id, reg_data, len + 1, false);
	APP_ERROR_CHECK(rslt);
        return rslt;
  
}


int8_t Acc_i2c_Read(uint8_t dev_id, uint8_t reg_addr, uint8_t *reg_data, uint16_t len)
{
	int8_t rslt = 0;
     //   NRF_LOG_INFO("READ: dev_id: %x reg_addr: %x len: %i\n", dev_id, reg_addr, len);
	rslt = nrf_drv_twi_tx(&m_twi, dev_id, ®_addr, 1, false);
        APP_ERROR_CHECK(rslt);

	if (rslt == 0)
	{
		rslt = nrf_drv_twi_rx(&m_twi, dev_id, reg_data, len);
	}
     //   NRF_LOG_INFO("READ: %x",*reg_data);
	return rslt;
}

void Acc_delay_ms(uint32_t period)
{ 
	
/*if (period==NULL){
period = 1;
}// delay time*/
	
  nrf_delay_ms( period ) ;
}


void BMI160_init (void)
{
    sensor.id = 0x69,//BMI160_I2C_ADDR;         //0x69
    sensor.interface = BMI160_I2C_INTF;  //0x00
    sensor.read = &Acc_i2c_Read;
    sensor.write = &Acc_i2c_Write;
    sensor.delay_ms = &Acc_delay_ms;

    rslt = bmi160_init(&sensor);
    APP_ERROR_CHECK(rslt);

    if(rslt == BMI160_OK){
    NRF_LOG_INFO("BMI160 Initialized...");
    } else {
    NRF_LOG_INFO("BMI160 not Initialized...");
    }
    //NRF_LOG_FLUSH();

    sensor.accel_cfg.odr = BMI160_ACCEL_ODR_100HZ;//BMI160_ACCEL_ODR_1600HZ;
    sensor.accel_cfg.range = BMI160_ACCEL_RANGE_2G;
    sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
    sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;

    rslt = bmi160_set_sens_conf(&sensor);
    APP_ERROR_CHECK(rslt);

     if(rslt == BMI160_OK){
    NRF_LOG_INFO("sensor Configured...");
    } else {
    NRF_LOG_INFO("sensor not Configured...");
    }

/* Select the power mode */
sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;


/*  Set the Power mode  */
rslt = bmi160_set_power_mode(&sensor);
     if(rslt == BMI160_OK){
    NRF_LOG_INFO("normal Configured...");
    } else {
    NRF_LOG_INFO("normal not Configured...");
    }
    //NRF_LOG_FLUSH();
}

static void read_sensor_data()
{
      m_xfer_done = false;
      accel.x = 1;
      bmi160_get_sensor_data(BMI160_ACCEL_SEL, &accel, NULL, &sensor);
      NRF_LOG_INFO("DataX:%d", accel.x);
      NRF_LOG_INFO("DataY:%d", accel.y);
      NRF_LOG_INFO("DataZ:%d", accel.z);

}


#if 1
int main(void)
{

    APP_ERROR_CHECK(NRF_LOG_INIT(NULL));
    NRF_LOG_DEFAULT_BACKENDS_INIT();

    NRF_LOG_INFO("\r\nBMI160 get started.\n");
    NRF_LOG_FLUSH();
    Acc_delay_ms(100);

    twi_init();
    Acc_delay_ms(50);
   //LM75B_set_mode();
    BMI160_init();
    Acc_delay_ms(1000);


    uint8_t reg_addr = BMI160_CHIP_ID_ADDR;
    uint8_t data;
    uint16_t len = 1;
    rslt = bmi160_get_regs(reg_addr, &data, len, &sensor);
    NRF_LOG_INFO("chip addr: %x \r\n",data);

/* Select the power mode */
sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;


/*  Set the Power mode  */
rslt = bmi160_set_power_mode(&sensor);
     if(rslt == BMI160_OK){
    NRF_LOG_INFO("normal Configured...");
    } else {
    NRF_LOG_INFO("normal not Configured...");
    }
    Acc_delay_ms(1000);
    uint8_t reg_addr1 = BMI160_PMU_STATUS_ADDR;
    data = 0xff;
    len = 1;
    rslt = bmi160_get_regs(reg_addr1, &data, len, &sensor);
    NRF_LOG_INFO("mode: %x \r\n",data);
    uint8_t cmd =0x11;
    uint8_t *cmd1=&cmd;

    while(true)
    {
    /*  Set the Power mode  */

        Acc_delay_ms(1000);
  
        read_sensor_data();
        NRF_LOG_FLUSH();
    }
}