/*
 * Code generated from Atmel Start.
 *
 * This file will be overwritten when reconfiguring your Atmel Start project.
 * Please copy examples or other code you want to keep to a separate file or main.c
 * to avoid loosing it when reconfiguring.
 */
#include <atmel_start.h>
#include <bosch_sensor_main.h>
#include <hal_io.h>
#include <hal_delay.h>

#include <hal_i2c_m_sync.h>

#include <bno055_config.h>

#define I2C_BUFFER_LEN 8
#define BNO055_I2C_BUS_WRITE_ARRAY_INDEX ((u8)1)
/*----------------------------------------------------------------------------*
 *  struct bno055_t parameters can be accessed by using BNO055
 *	BNO055_t having the following parameters
 *	Bus write function pointer: BNO055_WR_FUNC_PTR
 *	Bus read function pointer: BNO055_RD_FUNC_PTR
 *	Burst read function pointer: BNO055_BRD_FUNC_PTR
 *	Delay function pointer: delay_msec
 *	I2C address: dev_addr
 *	Chip id of the sensor: chip_id
 *---------------------------------------------------------------------------*/
static struct bno055_t ORIENTATION_SENSOR_0_desc;

/*	\Brief: The function is used as I2C bus write
 *	\Return : Status of the I2C write
 *	\param dev_addr : The device address of the sensor
 *	\param reg_addr : Address of the first register, will data is going to be written
 *	\param reg_data : It is a value hold in the array,
 *		will be used for write the value into the register
 *	\param cnt : The no of byte of data to be write
 */
static s8 ORIENTATION_SENSOR_0_I2C_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
{
	int32_t rst;
	u8      array[I2C_BUFFER_LEN];
	u8      stringpos        = BNO055_INIT_VALUE;
	array[BNO055_INIT_VALUE] = reg_addr;
	for (stringpos = BNO055_INIT_VALUE; stringpos < cnt; stringpos++) {
		array[stringpos + BNO055_I2C_BUS_WRITE_ARRAY_INDEX] = *(reg_data + stringpos);
	}

	i2c_m_sync_set_slaveaddr(&COMMUNICATION_IO, dev_addr, I2C_M_SEVEN);

	rst = io_write(&COMMUNICATION_IO.io, array, cnt + 1);
	if (rst != cnt + 1) {
		return BNO055_ERROR;
	}

	return (s8)BNO055_SUCCESS;
}

/*	\Brief: The function is used as I2C bus read
 *	\Return : Status of the I2C read
 *	\param dev_addr : The device address of the sensor
 *	\param reg_addr : Address of the first register, will data is going to be read
 *	\param reg_data : This data read from the sensor, which is hold in an array
 *	\param cnt : The no of byte of data to be read
 */
static s8 ORIENTATION_SENSOR_0_I2C_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
{
	int32_t rst;
	u8      array[I2C_BUFFER_LEN] = {BNO055_INIT_VALUE};
	u8      stringpos             = BNO055_INIT_VALUE;
	array[BNO055_INIT_VALUE]      = reg_addr;

	/* Select BNO055 as I2C slave target */

	i2c_m_sync_set_slaveaddr(&COMMUNICATION_IO, dev_addr, I2C_M_SEVEN);

	/* Set slave internal register address to read from (I2C write) */
	rst = io_write(&COMMUNICATION_IO.io, (uint8_t *)&reg_addr, 1);
	if (rst != 1) {
		return BNO055_ERROR;
	}
	/* Read value from BNO055 (I2C read) */
	rst = io_read(&COMMUNICATION_IO.io, (uint8_t *)array, cnt);
	if (rst != cnt) {
		return BNO055_ERROR;
	}

	for (stringpos = BNO055_INIT_VALUE; stringpos < cnt; stringpos++) {
		*(reg_data + stringpos) = array[stringpos];
	}
	/* add delay to fit with BNO055 continuous communication timing
	 * requirement, refer to BNO055 datasheet 4.6 I2C Protocol */
	delay_ms(1);

	return (s8)BNO055_SUCCESS;
}

BNO055_RETURN_FUNCTION_TYPE ORIENTATION_SENSOR_0_init(void)
{
	ORIENTATION_SENSOR_0_desc.bus_write  = ORIENTATION_SENSOR_0_I2C_bus_write;
	ORIENTATION_SENSOR_0_desc.bus_read   = ORIENTATION_SENSOR_0_I2C_bus_read;
	ORIENTATION_SENSOR_0_desc.delay_msec = (void (*)(BNO055_MDELAY_DATA_TYPE))delay_ms;
	ORIENTATION_SENSOR_0_desc.dev_addr   = CONF_BNO055_I2C_ADDR;

	return bno055_init(&ORIENTATION_SENSOR_0_desc);
}

void ORIENTATION_SENSOR_0_example(void)
{
	/* variable used to read the accel x data */
	s16 accel_datax = BNO055_INIT_VALUE;
	/* variable used to read the accel y data */
	s16 accel_datay = BNO055_INIT_VALUE;
	/* variable used to read the accel z data */
	s16 accel_dataz = BNO055_INIT_VALUE;

	/* Raw accel X, Y and Z data can read from the register page - page 0
	register - 0x08 to 0x0D */
	bno055_read_accel_x(&accel_datax);
	bno055_read_accel_y(&accel_datay);
	bno055_read_accel_z(&accel_dataz);
}

void bosch_sensor_init(void)
{

	ORIENTATION_SENSOR_0_init();
}
