/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2022 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdint.h>
#include <string.h>
#include <stdarg.h>
#include <math.h>

//#include "asf.h"
//#include "task.h"
//#include "arm_math.h"
//#include "demo-tasks.h"

#include "bhy_support.h"
#include "bhy_uc_driver.h"
#include "Bosch_PCB_7183_di03_BMI160_BMM150-7183_di03.2.1.11696_170103.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* should be greater or equal to 69 bytes, page size (50) + maximum packet size(18) + 1 */
#define FIFO_SIZE                      300
#define ROTATION_VECTOR_SAMPLE_RATE    100
#define MAX_PACKET_LENGTH              18
#define OUT_BUFFER_SIZE                60

#define BHI160_INT_GPIO_Port			GPIOB
#define BHI160_INT_Pin					GPIO_PIN_5
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
extern I2C_HandleTypeDef hi2c1;

/* USER CODE BEGIN PV */
char out_buffer[OUT_BUFFER_SIZE] =
		" W: 0.999  X: 0.999  Y: 0.999  Z: 0.999   \r";

uint8_t fifo[FIFO_SIZE];

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_I2C1_Init(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/*!
 * @brief This function is  callback function for acquring sensor datas
 *
 * @param[in]   sensor_data
 * @param[in]   sensor_id
 */
static void sensors_callback_rotation_vector(bhy_data_generic_t *sensor_data,
		bhy_virtual_sensor_t sensor_id) {
	float temp;
	uint8_t index;

	temp = sensor_data->data_quaternion.w / 16384.0f; /* change the data unit by dividing 16384 */
	out_buffer[3] = temp < 0 ? '-' : ' ';
	temp = temp < 0 ? -temp : temp;
	out_buffer[4] = floor(temp) + '0';

	for (index = 6; index <= 8; index++) {
		temp = (temp - floor(temp)) * 10;
		out_buffer[index] = floor(temp) + '0';
	}

	temp = sensor_data->data_quaternion.x / 16384.0f;
	out_buffer[13] = temp < 0 ? '-' : ' ';
	temp = temp < 0 ? -temp : temp;
	out_buffer[14] = floor(temp) + '0';

	for (index = 16; index <= 18; index++) {
		temp = (temp - floor(temp)) * 10;
		out_buffer[index] = floor(temp) + '0';
	}

	temp = sensor_data->data_quaternion.y / 16384.0f;
	out_buffer[23] = temp < 0 ? '-' : ' ';
	temp = temp < 0 ? -temp : temp;
	out_buffer[24] = floor(temp) + '0';

	for (index = 26; index <= 28; index++) {
		temp = (temp - floor(temp)) * 10;
		out_buffer[index] = floor(temp) + '0';
	}

	temp = sensor_data->data_quaternion.z / 16384.0f;
	out_buffer[33] = temp < 0 ? '-' : ' ';
	temp = temp < 0 ? -temp : temp;
	out_buffer[34] = floor(temp) + '0';

	for (index = 36; index <= 38; index++) {
		temp = (temp - floor(temp)) * 10;
		out_buffer[index] = floor(temp) + '0';
	}

	//DEBUG("x=%d, y=%d, z=%d, w=%d\n", sensor_data->data_quaternion.x, sensor_data->data_quaternion.y, sensor_data->data_quaternion.z, sensor_data->data_quaternion.w);
}
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_I2C1_Init();
  /* USER CODE BEGIN 2 */

	bhy_initialize_support();

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
	while (1) {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */

		//int8_t ret;
		/* BHY Variable*/
		uint8_t *fifoptr = NULL;
		uint8_t bytes_left_in_fifo = 0;
		uint16_t bytes_remaining = 0;
		uint16_t bytes_read = 0;
		bhy_data_generic_t fifo_packet;
		bhy_data_type_t packet_type;
		BHY_RETURN_FUNCTION_TYPE result;
		/* the remapping matrix for BHA or BHI here should be configured according to its placement on customer's PCB. */
		/* for details, please check 'Application Notes Axes remapping of BHA250(B)/BHI160(B)' document. */
		int8_t bhy_mapping_matrix_config[3 * 3] = { 0, 1, 0, -1, 0, 0, 0, 0, 1 };
		/* the remapping matrix for Magnetometer should be configured according to its placement on customer's PCB.  */
		/* for details, please check 'Application Notes Axes remapping of BHA250(B)/BHI160(B)' document. */
		int8_t mag_mapping_matrix_config[3 * 3] = { 0, 1, 0, 1, 0, 0, 0, 0, -1 };
		/* the sic matrix should be calculated for customer platform by logging uncalibrated magnetometer data. */
		/* the sic matrix here is only an example array (identity matrix). Customer should generate their own matrix. */
		/* This affects magnetometer fusion performance. */
		float sic_array[9] = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };

		/* To get the customized version number in firmware, it is necessary to read Parameter Page 2, index 125 */
		/* to get this information. This feature is only supported for customized firmware. To get this customized */
		/* firmware, you need to contact your local FAE of Bosch Sensortec. */
		//struct cus_version_t       bhy_cus_version;
		/* To get the customized version number in firmware, it is necessary to read Parameter Page 2, index 125 */
		/* to get this information. This feature is only supported for customized firmware. To get this customized */
		/* firmware, you need to contact your local FAE of Bosch Sensortec. */
		//struct cus_version_t      bhy_cus_version;
		while (HAL_GPIO_ReadPin(BHI160_INT_GPIO_Port, BHI160_INT_Pin)
				== GPIO_PIN_RESET)
			;
		/* init the bhy chip */
		if (bhy_driver_init((uint8_t*) &bhy1_fw)) {
			//DEBUG("Fail to init bhy\n");
		}

		/* wait for the bhy trigger the interrupt pin go down and up again */

		//while (HAL_GPIO_ReadPin(BHI160_INT_GPIO_Port, BHI160_INT_Pin) == GPIO_PIN_SET);
		HAL_Delay(100);

		while (HAL_GPIO_ReadPin(BHI160_INT_GPIO_Port, BHI160_INT_Pin)
				== GPIO_PIN_RESET)
			;

		/* To get the customized version number in firmware, it is necessary to read Parameter Page 2, index 125 */
		/* to get this information. This feature is only supported for customized firmware. To get this customized */
		/* firmware, you need to contact your local FAE of Bosch Sensortec. */
		//bhy_read_parameter_page(BHY_PAGE_2, PAGE2_CUS_FIRMWARE_VERSION, (uint8_t*)&bhy_cus_version, sizeof(struct cus_version_t));
		//DEBUG("cus version base:%d major:%d minor:%d\n", bhy_cus_version.base, bhy_cus_version.major, bhy_cus_version.minor);
		/* the remapping matrix for BHI and Magmetometer should be configured here to make sure rotation vector is */
		/* calculated in a correct coordinates system. */
		bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_ACC,
				bhy_mapping_matrix_config);
		bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_MAG,
				mag_mapping_matrix_config);
		bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_GYRO,
				bhy_mapping_matrix_config);
		/* This sic matrix setting affects magnetometer fusion performance. */
		bhy_set_sic_matrix(sic_array);

		/* install the callback function for parse fifo data
		 * SET VS_TYPE as Required */
		if (bhy_install_sensor_callback(VS_TYPE_ROTATION_VECTOR, VS_WAKEUP,
				sensors_callback_rotation_vector)) {
			//DEBUG("Fail to install sensor callback\n");
		}

		/* enables the virtual sensor */
		if (bhy_enable_virtual_sensor(VS_TYPE_ROTATION_VECTOR, VS_WAKEUP,
		ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0)) {
			//DEBUG("Fail to enable sensor id=%d\n", VS_TYPE_ROTATION_VECTOR);
		}

		/* Data Reading Loop */
		while (1) {

			/* wait until the interrupt fires */
			/* unless we already know there are bytes remaining in the fifo */
			while (!HAL_GPIO_ReadPin(BHI160_INT_GPIO_Port, BHI160_INT_Pin)
					&& !bytes_remaining) {
			}

			bhy_read_fifo(fifo + bytes_left_in_fifo,
			FIFO_SIZE - bytes_left_in_fifo, &bytes_read, &bytes_remaining);
			bytes_read += bytes_left_in_fifo;
			fifoptr = fifo;
			packet_type = BHY_DATA_TYPE_PADDING;

			do {
				/* this function will call callbacks that are registered */
				result = bhy_parse_next_fifo_packet(&fifoptr, &bytes_read,
						&fifo_packet, &packet_type);

				/* prints all the debug packets */
				if (packet_type == BHY_DATA_TYPE_DEBUG) {
					bhy_print_debug_packet(&fifo_packet.data_debug, bhy_printf);
				}

			/* the logic here is that if doing a partial parsing of the fifo, then we should not parse  */
			/* the last 18 bytes (max length of a packet) so that we don't try to parse an incomplete   */
			/* packet */
			} while ((result == BHY_SUCCESS)
					&& (bytes_read > (bytes_remaining ? MAX_PACKET_LENGTH : 0)));

			bytes_left_in_fifo = 0;

			if (bytes_remaining) {
				/* shifts the remaining bytes to the beginning of the buffer */
				while (bytes_left_in_fifo < bytes_read) {
					fifo[bytes_left_in_fifo++] = *(fifoptr++);
				}
			}
		}
	}
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLLMUL_4;
  RCC_OscInitStruct.PLL.PLLDIV = RCC_PLLDIV_2;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)
  {
    Error_Handler();
  }
  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C1;
  PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief I2C1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2C1_Init(void)
{

  /* USER CODE BEGIN I2C1_Init 0 */

  /* USER CODE END I2C1_Init 0 */

  /* USER CODE BEGIN I2C1_Init 1 */

  /* USER CODE END I2C1_Init 1 */
  hi2c1.Instance = I2C1;
  hi2c1.Init.Timing = 0x00707CBB;
  hi2c1.Init.OwnAddress1 = 0;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c1.Init.OwnAddress2 = 0;
  hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c1) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Analogue filter
  */
  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
  {
    Error_Handler();
  }

  /** Configure Digital filter
  */
  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN I2C1_Init 2 */

  /* USER CODE END I2C1_Init 2 */

}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOB_CLK_ENABLE();

  /*Configure GPIO pin : PB5 */
  GPIO_InitStruct.Pin = GPIO_PIN_5;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
	/* User can add his own implementation to report the HAL error return state */
	__disable_irq();
	while (1) {
	}
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */