/* 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"
#include "usart.h"
#include "spi.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <assert.h>

#include "math.h"
#include "stdio.h"

#include "dispcolor.h"
#include "constants.h"
#include "font.h"
/* USER CODE END Includes */

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

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define UART_RX_TIMEOUT 120
#define VL53L1X_UART_BUFFER_LENGTH 16 // constant for VL53L1X laser distance sensor
#define LCD_REFRESH_PERIOD 250  // ms
/* USER CODE END PD */

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

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
HAL_StatusTypeDef RxStatus;

// https://www.waveshare.com/wiki/TOF_Laser_Range_Sensor
typedef enum vl53l1xDistanceStatus // How Big Is An Enum? https://embedded.fm/blog/2016/6/28/how-big-is-an-enum
{
	MEASURING_DISTANCE_IS_VALID = 0,
	STANDARD_DEVIATION_IS_MORE_THAN_15MM = 1,
	SIGNAL_STRENGTH_IS_LOWER_THAN_1MCPS = 2,
	PHASE_EXCEEDS_BOUNDARY = 4,
	HW_OR_VCSEL_HAS_FAULT = 5,
	PHASE_IS_NOT_MATCHED = 7,
	INTERNAL_ALGORITHM_UNDERFLOW = 8,
	MEASURING_DISTANCE_IS_INVALID = 14
} vl53l1xDistanceStatus_t; // The GCC C compiler will make it uint8_t. Probably :) If not, the protocol decoder will fail.
static_assert( sizeof(vl53l1xDistanceStatus_t) == 1,
		"The vl53l1xDistanceStatus enum have to be of uint8_t to make the decoder work!" );

// https://www.waveshare.com/wiki/TOF_Laser_Range_Sensor
typedef struct vl53l1xProtocol
{
	uint8_t FrameHeader; // should be 0x57
	uint8_t FunctionMark;
	uint8_t Reserved1;
	uint8_t ID;
	uint32_t SystemTime;
	uint8_t Distance1000[3];
	vl53l1xDistanceStatus_t DistanceStatus; // have to be of uint8_t type
	uint16_t SignalStrength;
	uint8_t Reserved2;
	uint8_t CheckSum;
} vl53l1xProtocol_t; // 16 bytes

union vl53l1xProtocolDecoder
{
	uint8_t RawData[VL53L1X_UART_BUFFER_LENGTH];
	vl53l1xProtocol_t DataDecoded;
} SensorData;

char lcd_line[16];

uint32_t SoftTimerLCD;
/* USER CODE END PV */

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

uint8_t VerifyCheckSum(uint8_t *data)
{
	uint8_t TOF_check = 0;

	for (uint8_t k = 0; k < 15; k++)
	{
		TOF_check += data[k];
	}

	return !(TOF_check == (data[15]));
}

float CalculateDistance(uint8_t *RawBytes)
{
	return 0.001
			* ((((uint32_t) RawBytes[2]) << 16)
					| (((uint32_t) RawBytes[1]) << 8)
					| (((uint32_t) RawBytes[0]) << 0));
}
/* USER CODE END PFP */

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

/* 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_LPUART1_UART_Init();
	MX_USART1_UART_Init();
	MX_SPI3_Init();
	/* USER CODE BEGIN 2 */
	dispcolor_Init(240, 240);

	// HAL_UART_Receive_DMA(&huart1, SensorData.RawData, VL53L1X_UART_BUFFER_LENGTH);

	SoftTimerLCD = HAL_GetTick();
	/* USER CODE END 2 */

	/* Infinite loop */
	/* USER CODE BEGIN WHILE */
	while (1)
	{
		if (HAL_GetTick() - SoftTimerLCD > LCD_REFRESH_PERIOD)
		{
			SoftTimerLCD = HAL_GetTick();
			HAL_UART_Init(&huart1); // to be sorted out
			RxStatus = HAL_UART_Receive(&huart1, SensorData.RawData,
			VL53L1X_UART_BUFFER_LENGTH, UART_RX_TIMEOUT);
			HAL_UART_DeInit(&huart1); // to be sorted out
			if ((VerifyCheckSum(SensorData.RawData) == 0)
					&& (SensorData.DataDecoded.FrameHeader == 0x57)
					&& (SensorData.DataDecoded.DistanceStatus
							== MEASURING_DISTANCE_IS_VALID))
			{
				sprintf(lcd_line, "%.3f   ",
						CalculateDistance(SensorData.DataDecoded.Distance1000));
			}
			else
			{
				sprintf(lcd_line, "...      ");
			}
			dispcolor_DrawString(80, 100, FONTID_32F, lcd_line, WHITE);
		}
		/* USER CODE END WHILE */

		/* USER CODE BEGIN 3 */
	}
	/* USER CODE END 3 */
}

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

	/** Configure the main internal regulator output voltage
	 */
	HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);

	/** 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.PLLM = RCC_PLLM_DIV4;
	RCC_OscInitStruct.PLL.PLLN = 85;
	RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
	RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
	RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
	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_4) != HAL_OK)
	{
		Error_Handler();
	}
}

/* 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 */
