/**
 * Copyright (C) 2018 Bosch Sensortec GmbH
 *
 * SPDX-License-Identifier: BSD-3-Clause
 *
 * @file    bmi160_read_sensor_data.c
 * @brief   Sample file to read BMI160 sensor data using COINES library
 *
 */

/*********************************************************************/
/* system header files */
/*********************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>

/*********************************************************************/
/* own header files */
/*********************************************************************/
#include "coines.h"
#include "bmi160.h"
//BMX160 
#include "bmm150.h"

/*********************************************************************/
/* local macro definitions */
/*! i2c interface communication, 1 - Enable; 0- Disable */
#define BMI160_INTERFACE_I2C             0
/*! spi interface communication, 1 - Enable; 0- Disable */
#define BMI160_INTERFACE_SPI             1

#if (!((BMI160_INTERFACE_I2C==1) && (BMI160_INTERFACE_SPI==0)) && \
	(!((BMI160_INTERFACE_I2C==0) && (BMI160_INTERFACE_SPI==1))))
#error "Invalid value given for the macros BMI160_INTERFACE_I2C / BMI160_INTERFACE_SPI"
#endif

/*! bmi160 shuttle id */
#define BMI160_SHUTTLE_ID         0x38

/*! bmi160 Device address */
#define BMI160_DEV_ADDR BMI160_I2C_ADDR

//BMX160
#define ACC_FRAMES	10 /* 10 Frames are available every 100ms @ 100Hz */
#define GYR_FRAMES	10
#define MAG_FRAMES	10
#define FIFO_SIZE	250 

/*********************************************************************/
/* global variables */
/*********************************************************************/

/*! @brief This structure containing relevant bmi160 info */
struct bmi160_dev bmi160dev;
/*! @brief variable to hold the bmi160 accel data */
struct bmi160_sensor_data bmi160_accel;
/*! @brief variable to hold the bmi160 gyro data */
struct bmi160_sensor_data bmi160_gyro;

//BMX160 
struct bmm150_dev bmm150dev;
static uint8_t i2c_bus;
struct bmm150_settings bmm150dev_settings;


uint8_t fifo_buff[FIFO_SIZE];
struct bmi160_fifo_frame fifo_frame;
struct bmi160_aux_data aux_data[MAG_FRAMES];
struct bmm150_mag_data mag_data[MAG_FRAMES];
struct bmi160_sensor_data gyro_data[GYR_FRAMES], accel_data[ACC_FRAMES];

/*********************************************************************/
/* static function declarations */
/*********************************************************************/

/*!
 * @brief	internal API is used to initialize the sensor interface
 */
static void init_sensor_interface(void);

/*!
 * @brief	This internal API is used to initialize the bmi160 sensor with default
 */
static void init_bmi160(void);

/*!
 * @brief	This internal API is used to initialize the sensor driver interface
 */
static void init_bmi160_sensor_driver_interface(void);

//BMX160
static void init_bmm150_sensor_driver_interface(void);
void bmm150_delay_usec(uint32_t period, void *intf_ptr);
int8_t bmm150_aux_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr);
int8_t bmm150_aux_write(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr);

/*********************************************************************/
/* functions */
/*********************************************************************/
/*!
 *  @brief This internal API is used to initialize the sensor interface depending
 *   on selection either SPI or I2C.
 *
 *  @param[in] void
 *
 *  @return void
 *
 */
static void init_sensor_interface(void)
{
    /* Switch VDD for sensor off */
    coines_set_shuttleboard_vdd_vddio_config(0, 0);
    /* wait until the sensor goes off */
    coines_delay_msec(10);
#if BMI160_INTERFACE_I2C==1
    /* SDO pin is made low for selecting I2C address 0x68 */
    coines_set_pin_config(COINES_SHUTTLE_PIN_15, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_LOW);
    /* set the sensor interface as I2C */
    coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE);
    coines_delay_msec(10);
    /* CSB pin is made high for selecting I2C protocol*/
    coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH);
#endif
#if BMI160_INTERFACE_SPI==1
    /* CSB pin is made low for selecting SPI protocol*/
    coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_LOW);

    coines_delay_msec(10);
    coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3);
#endif
    coines_delay_msec(10);
    /* Switch VDD for sensor on */
    coines_set_shuttleboard_vdd_vddio_config(3300, 3300);

#if BMI160_INTERFACE_SPI==1
    coines_delay_msec(10);
    /* CSB pin is made high for selecting SPI protocol
     * Note: CSB has to see rising after power up, to switch to SPI protocol */
    coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH);
#endif
}

/*!
 *  @brief This internal API is used to initializes the bmi160 sensor
 *  settings like power mode and OSRS settings.
 *
 *  @param[in] void
 *
 *  @return void
 *
 */
static void init_bmi160(void)
{
    int8_t rslt;
    rslt = bmi160_init(&bmi160dev);

    if (rslt == BMI160_OK)
    {
        printf("BMI160 initialization success !\n");
        printf("Chip ID 0x%X\n", bmi160dev.chip_id);
    }
    else
    {
        printf("BMI160 initialization failure !\n");
        exit(COINES_E_FAILURE);
    }

    /* Select the Output data rate, range of accelerometer sensor */
    /* Configure the accelerometer */
    bmi160dev.accel_cfg.odr = BMI160_ACCEL_ODR_100HZ;
    bmi160dev.accel_cfg.range = BMI160_ACCEL_RANGE_2G;
    bmi160dev.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
    bmi160dev.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;

    /* Configure the gyroscope */
    bmi160dev.gyro_cfg.odr = BMI160_GYRO_ODR_100HZ;
    bmi160dev.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS;
    bmi160dev.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;
    bmi160dev.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;

    /* Set the sensor configuration */
    rslt = bmi160_set_sens_conf(&bmi160dev);
}

/*!
 *  @brief This internal API is used to set the sensor driver interface to
 *  read/write the data.
 *
 *  @param[in] void
 *
 *  @return void
 *
 */
static void init_bmi160_sensor_driver_interface(void)
{
#if BMI160_INTERFACE_I2C==1
    /* I2C setup */
    /* link read/write/delay function of host system to appropriate
     * bmi160 function call prototypes */
    bmi160dev.write = coines_write_i2c;
    bmi160dev.read = coines_read_i2c;
    bmi160dev.delay_ms = coines_delay_msec;
    /* set correct i2c address */
    bmi160dev.id = BMI160_DEV_ADDR;
    bmi160dev.interface = BMI160_I2C_INTF;
#endif
#if BMI160_INTERFACE_SPI==1
    /* SPI setup */
    /* link read/write/delay function of host system to appropriate
     *  bmi160 function call prototypes */
    bmi160dev.write = coines_write_spi;
    bmi160dev.read = coines_read_spi;
    bmi160dev.delay_ms = coines_delay_msec;
    bmi160dev.id = COINES_SHUTTLE_PIN_7;
    bmi160dev.interface = BMI160_SPI_INTF;
#endif
}

//BMM150
static void init_bmm150_sensor_driver_interface(void)
{
    /* I2C setup */
    /* link read/write/delay function of host system to appropriate
     * bmi160 function call prototypes */
    i2c_bus = BMM150_DEFAULT_I2C_ADDRESS;
	bmm150dev.intf_ptr = &i2c_bus;
    bmm150dev.intf = BMM150_I2C_INTF;
    bmm150dev.read = bmm150_aux_read;
    bmm150dev.write = bmm150_aux_write;
    bmm150dev.delay_us = bmm150_delay_usec;
}

/* Auxiliary function definitions */
int8_t bmm150_aux_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr) 
{
    return bmi160_aux_read(reg_addr, reg_data, length, &bmi160dev);
}

int8_t bmm150_aux_write(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr)
{
    return bmi160_aux_write(reg_addr, reg_data, length, &bmi160dev);
}

void bmm150_delay_usec(uint32_t period, void *intf_ptr)
{
	coines_delay_usec(period);
}
/*!
 *  @brief Main Function where the execution getting started to test the code.
 *
 *  @param[in] argc
 *  @param[in] argv
 *
 *  @return status
 *
 */
int main(int argc, char *argv[])
{
    struct coines_board_info board_info;
    int16_t rslt;
    int times_to_read = 0;

    init_bmi160_sensor_driver_interface();

//BMX160
    init_bmm150_sensor_driver_interface();

    rslt = coines_open_comm_intf(COINES_COMM_INTF_USB);

    if (rslt < 0)
    {
        printf("\n Unable to connect with Application Board ! \n"
               " 1. Check if the board is connected and powered on. \n"
               " 2. Check if Application Board USB driver is installed. \n"
               " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n");
        exit(rslt);
    }

    rslt = coines_get_board_info(&board_info);

    init_sensor_interface();
    /* after sensor init introduce 200 msec sleep */
    coines_delay_msec(200);
    init_bmi160();

    /* Configure the BMI160's auxiliary interface for the BMM150 */
    bmi160dev.aux_cfg.aux_sensor_enable = BMI160_ENABLE;
    bmi160dev.aux_cfg.aux_i2c_addr = BMM150_DEFAULT_I2C_ADDRESS;
    bmi160dev.aux_cfg.manual_enable = BMI160_ENABLE; /* Manual mode */
    bmi160dev.aux_cfg.aux_rd_burst_len = BMI160_AUX_READ_LEN_3; /* 8 bytes */

	rslt = bmi160_aux_init(&bmi160dev);

//BMX160
	rslt = bmm150_init(&bmm150dev);

    /* Configure the magnetometer. The regular preset supports up to 100Hz in Forced mode */
    bmm150dev_settings.preset_mode = BMM150_PRESETMODE_REGULAR;
    rslt = bmm150_set_presetmode(&bmm150dev_settings, &bmm150dev);
    /* Check rslt for any error codes */

    bmm150dev_settings.pwr_mode = BMM150_POWERMODE_FORCED;
    bmm150_set_op_mode(&bmm150dev_settings, &bmm150dev);

    uint8_t bmm150_data_start = BMM150_REG_DATA_X_LSB;
    bmi160dev.aux_cfg.aux_odr = BMI160_AUX_ODR_100HZ;
    rslt = bmi160_set_aux_auto_mode(&bmm150_data_start, &bmi160dev);
    if (rslt < 0)
    {exit(rslt);
    }

    fifo_frame.data = fifo_buff;
    fifo_frame.length = FIFO_SIZE;
    bmi160dev.fifo = &fifo_frame;

    /* Clear all existing FIFO configurations */
    rslt = bmi160_set_fifo_config(BMI160_FIFO_CONFIG_1_MASK , BMI160_DISABLE, &bmi160dev);
    /* Check rslt for any error codes */

    uint8_t fifo_config = BMI160_FIFO_HEADER | BMI160_FIFO_AUX |  BMI160_FIFO_ACCEL | BMI160_FIFO_GYRO;
    rslt = bmi160_set_fifo_config(fifo_config, BMI160_ENABLE, &bmi160dev);

    while (times_to_read < 100)
    {
        /* It is VERY important to reload the length of the FIFO memory as after the 
         * call to bmi160_get_fifo_data(), the bmi.fifo->length contains the
         * number of bytes read from the FIFO */
		uint8_t i; 
        bmi160dev.fifo->length = FIFO_SIZE;
        rslt = bmi160_get_fifo_data(&bmi160dev);

        uint8_t aux_inst = MAG_FRAMES, gyr_inst = GYR_FRAMES, acc_inst = ACC_FRAMES;
        rslt = bmi160_extract_aux(aux_data, &aux_inst, &bmi160dev);
        /* Check rslt for any error codes */
        rslt = bmi160_extract_gyro(gyro_data, &gyr_inst, &bmi160dev);
        /* Check rslt for any error codes */
        rslt = bmi160_extract_accel(accel_data, &acc_inst, &bmi160dev);
        /* Check rslt for any error codes */
        for ( i = 0; i < acc_inst; i++) 
		{
        	printf("Accel data gx:%d\tgy:%d\tgz:%d\n", accel_data[i].x, accel_data[i].y, accel_data[i].z);
        	printf("Gyro data gx:%d\tgy:%d\tgz:%d\n", gyro_data[i].x, gyro_data[i].y, gyro_data[i].z);
            rslt = bmm150_aux_mag_data(&aux_data[i].data[0], &mag_data[i], &bmm150dev);

        	printf("Mag data gx:%d\tgy:%d\tgz:%d\n", mag_data[i].x, mag_data[i].y, mag_data[i].z);
        }

		
		fflush(stdout);	
        coines_delay_msec(100);
        times_to_read = times_to_read + 1;
    }

    coines_close_comm_intf(COINES_COMM_INTF_USB);
    return EXIT_SUCCESS;
}