/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "fatfs.h"
#include "i2c.h"
#include "usart.h"
#include "spi.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "math.h"
#include "capturow_defs.h"
#include "ili9341_font.h"
#include "ili9341_gfx.h"
#include "ili9341.h"
#include "capturow_gfx.h"
#include "gps.h"
#include "mma8452q_stm32.h"
#include "fatfs_sd.h"


/* USER CODE END Includes */

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

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

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

#define TXT_BUFF_SZ 32
#define SD_BUFF_SZ 1024
#define ACCEL_DMA_BUFF_SZ 8
#define COUNT_STR_SZ 16
#define GPS_UPDATE_INTERVAL 1000
#define GPS_BAUDRATE 9600
#define MINS_PER_HOUR 60
#define SECS_PER_MIN 60

#define NUM_ACCEL_DATA_BYTES 6
#define ACCEL_ODR ODR_100
#define ACCEL_SAMP_PER_SEC 100

#define GPS_DEBUG 1

#if (GPS_DEBUG == 1)
#define GPS_DEBUG_BUFF_SZ 128
#endif


//Debug defs
#define DRAW_COORDS 0

/* USER CODE END PM */

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

/* USER CODE BEGIN PV */
ili9341_t *lcd;

uint8_t nav_button_status;
uint8_t enter_button_status;

enum display_states{main_menu, session, sys_info};
enum display_states current_disp_state = main_menu;

GPS_t prev_gps_data;
GPS_t current_gps_data;
GPS_t dummy_gps_data;
GPS_string_t current_gps_string_struct;
uint8_t gps_rx_complete = 0;
uint32_t prev_gps_update_time;
uint32_t current_gps_update_time;

char *gps_debug_string;
#if (GPS_DEBUG == 1)
	char gps_debug_filename[] = "gps_log.txt";
#endif

char datalog_filename[] = "capturow_datalog.txt";
uint32_t log_line_cnt = 0;
uint8_t int_src_status[2];
uint8_t dbg_uart_write_gps = 1;
uint8_t dbg_uart_write_accel = 1;

uint8_t accel_data_buff[NUM_ACCEL_DATA_BYTES * ACCEL_SAMP_PER_SEC];
uint8_t accel_dummy_buff[NUM_ACCEL_DATA_BYTES];

MMA8452Q_DATA_STATUS mma8452q_stat;
SD_WRITE_STATUS sd_write_status;

perf_data_t perf_data;
perf_data_string_t perf_data_string;

accel_data_t accel_data[ACCEL_SAMP_PER_SEC];

uint16_t accel_data_idx;
accel_data_string_t accel_data_string;

uint32_t accel_process_start, accel_process_end, accel_process_delta;


/* USER CODE END PV */

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

/* USER CODE END PFP */

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

// USART DMA complete callback function
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	if(huart == &huart3){
		GPS_UART_CallBack(gps_debug_string, &gps_rx_complete);
	}
}


// EXTI Line8 External Interrupt ISR Handler CallBackFun (I2C Accelerometer data ready)
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	switch (GPIO_Pin){
	// If The INT Source Is EXTI Line8 (B8 Pin)
	case (GPIO_PIN_8):
		// Check interrupt source reg to find out if interrupt was caused by Data ready.
		if(HAL_I2C_Mem_Read(&hi2c1, (MMA8452Q_DEV_ADDR<<1), OUT_X_MSB, I2C_MEMADD_SIZE_8BIT, &(accel_data_buff[accel_data_idx * NUM_ACCEL_DATA_BYTES]), 6, I2C_TIMEOUT) == HAL_OK){
			if (accel_data_idx < (ACCEL_SAMP_PER_SEC)){
				if (accel_data_idx == 0){
					accel_process_start = HAL_GetTick();
				}else if (accel_data_idx == (ACCEL_SAMP_PER_SEC - 1)){
					accel_process_end = HAL_GetTick();
				}
				accel_data_idx += 1;
			}
		}
		break;
	// If The INT Source Is EXTI Line11 (B11 Pin)
	case (GPIO_PIN_11):
		enter_button_status = 1;
		break;

	default:
		break;
	}
}


FATFS fs;
FIL fil;
FRESULT fresult;
char sd_buffer[SD_BUFF_SZ];
UINT br, bw;
FATFS *pfs;
DWORD fre_clust;
uint32_t total, free_space;

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
	char txt_buff[TXT_BUFF_SZ];
	uint8_t pause_resume;
 	enter_button_status = 0;
 	nav_button_status = 0;
	pause_resume = 0;
	accel_data_idx = 0;
	mma8452q_stat = DATA_WAITING;
	sd_write_status = SD_WRITE_NOT_READY;

	accel_process_start = 0;
	accel_process_end = 0;
	accel_process_delta = 0;



  /* 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_DMA_Init();
  MX_SPI1_Init();
  MX_USART3_UART_Init();
  MX_I2C1_Init();
  MX_FATFS_Init();
  MX_LPUART1_UART_Init();
  /* USER CODE BEGIN 2 */

  // Init SD...
  sd_init(txt_buff, datalog_filename);


  // Init LCD...
  lcd = ili9341_new(
  		 &hspi1,
  		 TFT_RESET_GPIO_Port, TFT_RESET_Pin,
  		 TFT_CS_GPIO_Port,    TFT_CS_Pin,
  		 TFT_DC_GPIO_Port,    TFT_DC_Pin,
  		 isoLeft,
  		 TOUCH_CS_GPIO_Port,  TOUCH_CS_Pin,
  		 TOUCH_IRQ_GPIO_Port, TOUCH_IRQ_Pin,
  		itsNONE,
  		itnNONE);


   ili9341_text_attr_t stroke_count_txt_1 = {
		.bg_color = ILI9341_WHITE,
		.fg_color = ILI9341_BLACK,
		//.font = &ili9341_font_7x10,
		.font = &ili9341_font_16x26,
		.origin_x = 0,
		.origin_y = 0
   };

   ili9341_text_attr_t stroke_count_txt_2 = {
			.bg_color = ILI9341_WHITE,
			.fg_color = ILI9341_BLACK,
			//.font = &ili9341_font_7x10,
		.font = &ili9341_font_16x26,
			.origin_x = 0,
			.origin_y = 50
   };

   ili9341_text_attr_t stroke_count_txt_3 = {
  			.bg_color = ILI9341_WHITE,
  			.fg_color = ILI9341_BLACK,
  			//.font = &ili9341_font_7x10,
  		.font = &ili9341_font_16x26,
  			.origin_x = 0,
  			.origin_y = 100
     };

   ili9341_text_attr_t stroke_count_txt_4 = {
  			.bg_color = ILI9341_WHITE,
  			.fg_color = ILI9341_BLACK,
  			//.font = &ili9341_font_7x10,
  		.font = &ili9341_font_16x26,
  			.origin_x = 0,
  			.origin_y = 150
     };

   ili9341_text_attr_t stroke_count_txt_5 = {
  			.bg_color = ILI9341_WHITE,
  			.fg_color = ILI9341_BLACK,
  			//.font = &ili9341_font_7x10,
  		.font = &ili9341_font_16x26,
  			.origin_x = 0,
  			.origin_y = 200
     };

   MMA8452Q_init(&hi2c1, SCALE_2G, ACCEL_ODR);
   draw_startup_screen(lcd);
   // Wait 5 seconds and open main menu...
   HAL_Delay(1000);
   printf("Init Done!\n");
   ili9341_fill_screen(lcd, ILI9341_PINK);
   // Init GPS Struct...
   populate_gps_dummy(&current_gps_data);
   GPS_Init();
	if (GPS_DEBUG){
   	   sd_init_gps_log(gps_debug_filename);
   	   gps_debug_string = (char *) malloc(GPS_DEBUG_BUFF_SZ);
	}else{
		gps_debug_string = "";
	}


   current_disp_state = session;

  /* USER CODE END 2 */

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

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */

	// Update accelerometer data...
	// Dummy read to keep interrupt firing - We will ignore this data...
	//HAL_I2C_Mem_Read(&hi2c1, (MMA8452Q_DEV_ADDR<<1), OUT_X_MSB, I2C_MEMADD_SIZE_8BIT, accel_dummy_buff, 6, I2C_TIMEOUT);

	// Read good data...
//	if (mma8452q_stat == DATA_READY){
//		mma8452q_stat = DATA_WAITING;
//		if(HAL_I2C_Mem_Read(&hi2c1, (MMA8452Q_DEV_ADDR<<1), OUT_X_MSB, I2C_MEMADD_SIZE_8BIT, &(accel_data_buff[accel_data_idx * NUM_ACCEL_DATA_BYTES]), 6, I2C_TIMEOUT) == HAL_OK){
//			if (accel_data_idx < (ACCEL_SAMP_PER_SEC)){
//				if (accel_data_idx == 0){
//					accel_process_start = HAL_GetTick();
//				}else if (accel_data_idx == (ACCEL_SAMP_PER_SEC - 1)){
//					accel_process_end = HAL_GetTick();
//				}
//				accel_data_idx += 1;
//			}
//		}
//	}


	// If accel buffer filled, process...
	if (accel_data_idx == ACCEL_SAMP_PER_SEC){
		//MMA8452Q_process(accel_data_buff_2, SCALE_2G, &(accel_data[accel_data_idx].accel_x), &(accel_data[accel_data_idx].accel_y), &(accel_data[accel_data_idx].accel_z));
		MMA8452Q_process_debug(accel_data_buff, ACCEL_SAMP_PER_SEC, SCALE_2G, accel_data);
		//update_accel_strings(accel_data);
		sd_write_status = SD_WRITE_READY;
		accel_data_idx = 0;
		accel_process_delta = accel_process_end - accel_process_start;
	}

	switch (current_disp_state){
		case (main_menu):
			draw_main_menu(lcd);
			printf("Longitude = %f\n", GPS.dec_longitude);

		break;

		case (session):
			if (enter_button_status){
				pause_resume = !pause_resume;
				// Clear screen text...
				ili9341_fill_screen(lcd, ILI9341_PINK);
				// clear button pressed flag...
				enter_button_status = 0;
			}
			if (!pause_resume){
				current_gps_update_time = HAL_GetTick();
				// Update GPS data at GPS_UPDATE_INTERVAL...
				if ((current_gps_update_time - prev_gps_update_time) >= GPS_UPDATE_INTERVAL){
					// Update gps struct every second
					prev_gps_data = current_gps_data;
					current_gps_data = GPS;
					// Get split from GPS speed
					convert_speed(current_gps_data.speed_km, &perf_data);
					update_perf_data_string(&perf_data, &perf_data_string);
					update_gps_strings(&current_gps_data);
					prev_gps_update_time = current_gps_update_time;
					//update_accel_strings(&(accel_data_debug[ACCEL_SAMP_PER_SEC - 1]));
					if (DRAW_COORDS){
						strcpy(txt_buff, "Lat = ");
						ili9341_draw_string(lcd, stroke_count_txt_1, strcat(txt_buff, current_gps_string_struct.dec_latitude));
						strcpy(txt_buff, "Long = ");
						ili9341_draw_string(lcd, stroke_count_txt_2, strcat(txt_buff, current_gps_string_struct.dec_longitude));
						strcpy(txt_buff, "KM/H (GPS) = ");
						ili9341_draw_string(lcd, stroke_count_txt_3, strcat(txt_buff, current_gps_string_struct.speed_km));
						strcpy(txt_buff, "Split = ");
						strcat(txt_buff, perf_data_string.split_mins);
						strcat(txt_buff, ":");
						strcat(txt_buff, perf_data_string.split_secs);
						ili9341_draw_string(lcd, stroke_count_txt_4, txt_buff);
					}else{
						// Dsiplay GPS KM/H Values...
						strcpy(txt_buff, "KM/H (GPS) = ");
						ili9341_draw_string(lcd, stroke_count_txt_1, strcat(txt_buff, current_gps_string_struct.speed_km));
						strcpy(txt_buff, "Split = ");
						strcat(txt_buff, perf_data_string.split_mins);
						strcat(txt_buff, ":");
						strcat(txt_buff, perf_data_string.split_secs);
						ili9341_draw_string(lcd, stroke_count_txt_2, txt_buff);

						// Dsiplay GPS Knots Values converted to KM/H...
						current_gps_data.speed_km =  current_gps_data.speed_k * 1.852;
						convert_speed(current_gps_data.speed_km, &perf_data);
						update_perf_data_string(&perf_data, &perf_data_string);
						update_gps_strings(&current_gps_data);
						strcpy(txt_buff, "Knot -> KM/H = ");
						ili9341_draw_string(lcd, stroke_count_txt_3, strcat(txt_buff, current_gps_string_struct.speed_k));
						strcpy(txt_buff, "Split = ");
						strcat(txt_buff, perf_data_string.split_mins);
						strcat(txt_buff, ":");
						strcat(txt_buff, perf_data_string.split_secs);
						ili9341_draw_string(lcd, stroke_count_txt_4, txt_buff);
					}
				}
				// Once Accelerometer buffer filled, write log to SD...
				if (sd_write_status == SD_WRITE_READY){
					sd_write_log(datalog_filename);
					sd_write_status = SD_WRITE_NOT_READY;
				}
				if ((GPS_DEBUG) && (gps_rx_complete)){
					write_GPS_sentences_to_SD(gps_debug_string, gps_debug_filename);
					gps_rx_complete = 0;
				}

			}else{
				ili9341_draw_string(lcd, stroke_count_txt_4, "PAUSED");
			}
			break;

		case (sys_info):
			break;

	}


  }

  /* 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
  */
  if (HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 1;
  RCC_OscInitStruct.PLL.PLLN = 20;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV7;
  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 */

void populate_gps_dummy(GPS_t *gps_data)
{
		gps_data->altitude_ft = 0;
		gps_data->course_d = 0;
		gps_data->course_m = 0;
		gps_data->course_m_unit = '\0';
		gps_data->course_t = 0;
		gps_data->course_t_unit = '\0';
		gps_data->date = 0;
		gps_data->dec_latitude = 0;
		gps_data->dec_longitude = 0;
		gps_data->ew = 'W';
		gps_data->gll_status = 'A';
		gps_data->hdop = 0;
		gps_data->lock = 0;
		gps_data->msl_altitude = 19.5;
		gps_data->msl_units = 'M';
		gps_data->nmea_latitude = 0;
		gps_data->nmea_longitude = 0;
		gps_data->ns = 'N';
		gps_data->rmc_status = '\0';
		gps_data->satelites = 0;
		gps_data->speed_k = 0;
		gps_data->speed_k_unit = '\0';
		gps_data->speed_km = 0;
		gps_data->speed_km_unit = '\0';
		gps_data->utc_time = 0;
}

void update_gps_strings(GPS_t *gps_data)
{

	snprintf(current_gps_string_struct.altitude_ft, 16, "%f", gps_data->altitude_ft);
	snprintf(current_gps_string_struct.course_d, 16, "%f", gps_data->course_d);
	snprintf(current_gps_string_struct.course_m, 16, "%f", gps_data->course_m);
	snprintf(current_gps_string_struct.course_m_unit, 16, "%d", gps_data->course_m_unit);
	snprintf(current_gps_string_struct.course_t, 16, "%f", gps_data->course_t);
	snprintf(current_gps_string_struct.course_t_unit, 16, "%d", gps_data->course_t_unit);
	snprintf(current_gps_string_struct.date, 16, "%d", gps_data->date);
	snprintf(current_gps_string_struct.dec_latitude, 16, "%f", gps_data->dec_latitude);
	snprintf(current_gps_string_struct.dec_longitude, 16, "%f", gps_data->dec_longitude);
	snprintf(current_gps_string_struct.ew, 16, "%d", gps_data->ew);
	snprintf(current_gps_string_struct.gll_status, 16, "%d", gps_data->gll_status);
	snprintf(current_gps_string_struct.hdop, 16, "%f", gps_data->hdop);
	snprintf(current_gps_string_struct.lock, 16, "%d", gps_data->lock);
	snprintf(current_gps_string_struct.msl_altitude, 16, "%f", gps_data->msl_altitude);
	snprintf(current_gps_string_struct.msl_units, 16, "%d", gps_data->msl_units);
	snprintf(current_gps_string_struct.nmea_latitude, 16, "%f", gps_data->nmea_latitude);
	snprintf(current_gps_string_struct.nmea_longitude, 16, "%f", gps_data->nmea_longitude);
	snprintf(current_gps_string_struct.ns, 16, "%d", gps_data->ns);
	snprintf(current_gps_string_struct.rmc_status, 16, "%d", gps_data->rmc_status);
	snprintf(current_gps_string_struct.satelites, 16, "%d", gps_data->satelites);
	snprintf(current_gps_string_struct.speed_k, 16, "%f", gps_data->speed_k);
	snprintf(current_gps_string_struct.speed_k_unit, 16, "%d", gps_data->speed_k_unit);
	snprintf(current_gps_string_struct.speed_km, 16, "%.3f", gps_data->speed_km);
	snprintf(current_gps_string_struct.speed_km_unit, 16, "%d", gps_data->speed_km_unit);
	snprintf(current_gps_string_struct.utc_time, 16, "%f", gps_data->utc_time);

}

void update_accel_strings(accel_data_t *accel_data)
{
	snprintf(accel_data_string.accel_x, 16, "%f", (accel_data)->accel_x);
	snprintf(accel_data_string.accel_y, 16, "%f", (accel_data)->accel_y);
	snprintf(accel_data_string.accel_z, 16, "%f", (accel_data)->accel_z);

}

void update_perf_data_string(perf_data_t *perf_data, perf_data_string_t *perf_data_string)
{

	snprintf(perf_data_string->split_mins, 16, "%.0lf", perf_data->split_mins);
	snprintf(perf_data_string->split_secs, 16, "%.1lf", perf_data->split_secs);

}

void convert_speed(float speed, perf_data_t *perf_data)
{
	double factor;
	double integral_part;
	double fract_part;

	// convert km/h to time/500m
	if (speed > 0){
		factor = speed / 0.5;
		perf_data->split_mins = MINS_PER_HOUR / factor;
		// Split up mins and secs...
		fract_part = modf(perf_data->split_mins, &integral_part);
		perf_data->split_mins = integral_part;
		perf_data->split_secs = fract_part * SECS_PER_MIN;
	}
}

uint16_t bufsize(char *buf)
{
	uint16_t i = 0;
	while (*buf++ != '\0'){
		i++;
	}
	return i;
}

void bufclear(char *buffer, uint16_t buff_sz)
{
	for (int i = 0; i < buff_sz; i++){
		*(buffer + i) = '\0';
	}
}

void sd_init(char *txt_buff, char *file_name)
{
	 // Init SD card...
	 fresult = f_mount(&fs, "", 0);
	 if (fresult != FR_OK){
		 printf("Error in mounting SD card...\n");
	 }else{
		 printf("SD Card Mounted Succesfully!...\n");
	 }
	 f_getfree("", &fre_clust, &pfs);
	 total = (uint32_t) ((pfs->n_fatent - 2) * pfs->csize * 0.5);
	 sprintf(txt_buff, "SD CARD Total Size: \t%lu\n", total);
	 printf("%s", txt_buff);
	 bufclear(txt_buff, TXT_BUFF_SZ);
	 free_space = (uint32_t) (fre_clust * pfs->csize * 0.5);
	 sprintf(txt_buff, "SD CARD Free Space: \t%lu\n", free_space);
	 printf("%s", txt_buff);

	 // Clear datalog...
	 f_unlink(file_name);
	 fresult = f_open(&fil, file_name, FA_CREATE_NEW | FA_READ | FA_WRITE);
	 fresult = f_puts ("START OF DATALOG\n", &fil);
	 fresult = f_close(&fil);

}

void sd_write_log(char *filename)
{
	uint16_t sd_buff_sz;
	UINT bytes_written;
	HAL_GPIO_WritePin(DEBUG_LED_2_Port, DEBUG_LED_2_Pin, GPIO_PIN_SET);
	bufclear(sd_buffer, SD_BUFF_SZ);
	fresult = f_open(&fil, filename, FA_OPEN_APPEND | FA_READ | FA_WRITE);
	sprintf(sd_buffer, "Time(mS): %lu\tLog #: %lu\tODR: %i\tLatitude: %s\tLongitude: %s\tGPS Speed (KM/H): %s\n", HAL_GetTick(), log_line_cnt, ACCEL_SAMP_PER_SEC, current_gps_string_struct.dec_latitude, current_gps_string_struct.dec_longitude, current_gps_string_struct.speed_km);
	sd_buff_sz = bufsize(sd_buffer);
	//fresult = f_printf (&fil, "Time(mS): %lu\tLog #: %lu\tODR: %d\tLatitude: %s\tLongitude: %s\tGPS Speed (KM/H): %s\n", HAL_GetTick(), log_line_cnt, ACCEL_SAMP_PER_SEC, current_gps_string_struct.dec_latitude, current_gps_string_struct.dec_longitude, current_gps_string_struct.speed_km);
	fresult = f_write(&fil, sd_buffer, sd_buff_sz, &bytes_written);
	if (dbg_uart_write_gps){
		HAL_UART_Transmit(&hlpuart1, (uint8_t*)sd_buffer, strlen(sd_buffer), 0xFFFF);
	}
	// Write Accelerometer data...
	for (int i = 0; i < ACCEL_SAMP_PER_SEC; i++){
		update_accel_strings(accel_data + i);
		if (i == (ACCEL_SAMP_PER_SEC - 1)){
			// Add newline if writing last accel data element
			sprintf(sd_buffer, "X: %s\tY: %s\tZ: %s\n", accel_data_string.accel_x, accel_data_string.accel_y, accel_data_string.accel_z);
			sd_buff_sz = bufsize(sd_buffer);
			fresult = f_write(&fil, sd_buffer, sd_buff_sz, &bytes_written);
			if (dbg_uart_write_accel){
					HAL_UART_Transmit(&hlpuart1, (uint8_t*)sd_buffer, strlen(sd_buffer), 0xFFFF);
			}
			//fresult = f_printf (&fil, "X: %f\tY: %f\tZ: %f\n", accel_data[i].accel_x, accel_data[i].accel_y, accel_data[i].accel_z);
		}else{
			sprintf(sd_buffer, "X: %s\tY: %s\tZ: %s\t", accel_data_string.accel_x, accel_data_string.accel_y, accel_data_string.accel_z);
			sd_buff_sz = bufsize(sd_buffer);
			fresult = f_write(&fil, sd_buffer, sd_buff_sz, &bytes_written);
			if (dbg_uart_write_accel){
				HAL_UART_Transmit(&hlpuart1, (uint8_t*)sd_buffer, strlen(sd_buffer), 0xFFFF);
			}
			//fresult = f_printf (&fil, "X: %f\tY: %f\tZ: %f\t", accel_data[i].accel_x, accel_data[i].accel_y, accel_data[i].accel_z);
		}
	}
	fresult = f_close(&fil);
	HAL_GPIO_WritePin(DEBUG_LED_2_Port, DEBUG_LED_2_Pin, GPIO_PIN_RESET);
	log_line_cnt += 1;
}


void sd_init_gps_log(char *filename)
{
	 // Clear datalog...
	 f_unlink(filename);
	 fresult = f_open(&fil, filename, FA_CREATE_NEW | FA_READ | FA_WRITE);
	 fresult = f_puts ("START OF GPS STRING LOG\n", &fil);
	 fresult = f_close(&fil);

}

void write_GPS_sentences_to_SD(char *gps_string, char *filename) {

	uint32_t sd_buff_sz;
	UINT bytes_written;
	bufclear(sd_buffer, SD_BUFF_SZ);
	fresult = f_open(&fil, filename, FA_OPEN_APPEND | FA_READ | FA_WRITE);
	sprintf(sd_buffer, "Time (ms): %lu \t %s", HAL_GetTick(), gps_string);
	sd_buff_sz = bufsize(sd_buffer);
	fresult = f_write(&fil, sd_buffer, sd_buff_sz, &bytes_written);
	fresult = f_close(&fil);

}


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

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
