/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * File Name          : freertos.c
 * Description        : Code for freertos applications
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2024 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 "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include "usart.h"
#include "Eeprom.h"
#include "i2c.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 */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
uint8_t gl_Digit1, gl_Digit2, gl_Digit3;
uint8_t gl_Point1, gl_Point2, gl_Point3;
enum mode gl_Mode = GPS;

GPS_Data gl_GPSData[10];
float gl_temp;

uint16_t gl_addParcours, gl_addEnregistre;
float gl_VitesseMax, gl_AltMax, gl_Denive, gl_TempMax, gl_TempMin;

float gl_ProchaineLatitude, gl_ProchaineLongitude;

float gl_Ancienne_Alt;

float gl_courant;

/* USER CODE END Variables */
osThreadId AffichageHandle;
osThreadId MAJHandle;
osThreadId ReadGPSHandle;
osThreadId MAJEepromHandle;

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartAffichageTask(void const *argument);
void StartMAJ(void const *argument);
void StartReadGPS(void const *argument);
void StartMAJEeprom(void const *argument);

extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/* GetIdleTaskMemory prototype (linked to static allocation support) */
void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer,
		StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize);

/* USER CODE BEGIN GET_IDLE_TASK_MEMORY */
static StaticTask_t xIdleTaskTCBBuffer;
static StackType_t xIdleStack[configMINIMAL_STACK_SIZE];

void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer,
		StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize) {
	*ppxIdleTaskTCBBuffer = &xIdleTaskTCBBuffer;
	*ppxIdleTaskStackBuffer = &xIdleStack[0];
	*pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
	/* place for user code */
}
/* USER CODE END GET_IDLE_TASK_MEMORY */

/**
 * @brief  FreeRTOS initialization
 * @param  None
 * @retval None
 */
void MX_FREERTOS_Init(void) {
	/* USER CODE BEGIN Init */
	EEPROM_ReadUInt16(ADD_Parcours, &gl_addParcours);
	EEPROM_ReadFloat(gl_addParcours, &gl_ProchaineLatitude);
	EEPROM_ReadFloat(gl_addParcours + sizeof(float), &gl_ProchaineLongitude);
	EEPROM_ReadUInt16(ADD_ENREGISTRE, &gl_addParcours);
	GPS_Read(gl_GPSData);
	gl_Ancienne_Alt = gl_GPSData[0].altitude;

	/* USER CODE END Init */

	/* USER CODE BEGIN RTOS_MUTEX */
	/* add mutexes, ... */
	/* USER CODE END RTOS_MUTEX */

	/* USER CODE BEGIN RTOS_SEMAPHORES */
	/* add semaphores, ... */
	/* USER CODE END RTOS_SEMAPHORES */

	/* USER CODE BEGIN RTOS_TIMERS */
	/* start timers, add new ones, ... */
	/* USER CODE END RTOS_TIMERS */

	/* USER CODE BEGIN RTOS_QUEUES */
	/* add queues, ... */
	/* USER CODE END RTOS_QUEUES */

	/* Create the thread(s) */
	/* definition and creation of Affichage */
	osThreadDef(Affichage, StartAffichageTask, osPriorityNormal, 0, 128);
	AffichageHandle = osThreadCreate(osThread(Affichage), NULL);

	/* definition and creation of MAJ */
	osThreadDef(MAJ, StartMAJ, osPriorityIdle, 0, 128);
	MAJHandle = osThreadCreate(osThread(MAJ), NULL);

	/* definition and creation of ReadGPS */
	osThreadDef(ReadGPS, StartReadGPS, osPriorityIdle, 0, 128);
	ReadGPSHandle = osThreadCreate(osThread(ReadGPS), NULL);

	/* definition and creation of MAJEeprom */
	osThreadDef(MAJEeprom, StartMAJEeprom, osPriorityIdle, 0, 128);
	MAJEepromHandle = osThreadCreate(osThread(MAJEeprom), NULL);

	/* USER CODE BEGIN RTOS_THREADS */
	/* add threads, ... */
	/* USER CODE END RTOS_THREADS */

}

/* USER CODE BEGIN Header_StartAffichageTask */
/**
 * @brief  Function implementing the Affichage thread.
 * @param  argument: Not used
 * @retval None
 */
/* USER CODE END Header_StartAffichageTask */
void StartAffichageTask(void const *argument) {
	/* init code for USB_DEVICE */
	MX_USB_DEVICE_Init();
	/* USER CODE BEGIN StartAffichageTask */
	/* Infinite loop */
	for (;;) {
		CD74HC4511_DisplayDigit(gl_Digit1, 1, gl_Point1);
		osDelay(10);
		CD74HC4511_DisplayDigit(gl_Digit2, 2, gl_Point2);
		osDelay(10);
		CD74HC4511_DisplayDigit(gl_Digit3, 3, gl_Point3);
		osDelay(10);
	}
	/* USER CODE END StartAffichageTask */
}

/* USER CODE BEGIN Header_StartMAJ */
/**
 * @brief Function implementing the MAJ thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_StartMAJ */
void StartMAJ(void const *argument) {
	/* USER CODE BEGIN StartMAJ */
	/* Infinite loop */
	for (;;) {
		float alt = Altitude();
		float tmp = Temperature();
		float vit = Vitesse();
		float dis = Distance();
		switch (gl_Mode) {
		case Alt:
			decode(alt);
			break;
		case temp:
			decode(tmp);
			break;
		case Vit:
			decode(vit);
			break;
		case GPS:
			decode(dis);
			break;
		case Cali:
			decode(0);
			break;
		}

		osDelay(200);
	}
	/* USER CODE END StartMAJ */
}

/* USER CODE BEGIN Header_StartReadGPS */
/**
 * @brief Function implementing the ReadGPS thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_StartReadGPS */
void StartReadGPS(void const *argument) {
	/* USER CODE BEGIN StartReadGPS */
	/* Infinite loop */
	for (;;) {
		Shift_GPS_Data(gl_GPSData); // Décaler les anciennes données
		GPS_Read(gl_GPSData);
		ReadTemp();
		osDelay(1000);
	}
	/* USER CODE END StartReadGPS */
}

/* USER CODE BEGIN Header_StartMAJEeprom */
/**
 * @brief Function implementing the MAJEeprom thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_StartMAJEeprom */
void StartMAJEeprom(void const *argument) {
	/* USER CODE BEGIN StartMAJEeprom */
	/* Infinite loop */
	for (;;) {
		EEPROM_WriteFloat(ADD_AltMax, gl_AltMax);
		EEPROM_WriteFloat(ADD_Denivle, gl_Denive);
		EEPROM_WriteFloat(ADD_TempMax, gl_TempMin);
		EEPROM_WriteFloat(ADD_TempMin, gl_TempMax);
		EEPROM_WriteFloat(ADD_VitessMax, gl_VitesseMax);
		osDelay(1000 * 60);
	}
	/* USER CODE END StartMAJEeprom */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void decode(float val) {
	if (val >= 100) {
		gl_Point1 = 0;
		gl_Point2 = 0;
		gl_Point3 = 1;
		gl_Digit1 = (uint8_t) (val / 100);
		gl_Digit2 = (uint8_t) ((fmod(val, 100)) / 10);
		gl_Digit3 = (uint8_t) (fmod(val, 10));
	} else if (val >= 10) {
		gl_Point1 = 0;
		gl_Point2 = 1;
		gl_Point3 = 0;
		gl_Digit1 = (uint8_t) (val / 10);
		gl_Digit2 = (uint8_t) fmod(val, 10);
		gl_Digit3 = (uint8_t) (fmod(val * 10, 10)); // Chiffre après la virgule
	} else {
		gl_Point1 = 1;
		gl_Point2 = 0;
		gl_Point3 = 0;
		gl_Digit1 = (uint8_t) val;
		gl_Digit2 = (uint8_t) (fmod(val * 10, 10));
		gl_Digit3 = (uint8_t) (fmod(val * 100, 10)); // Troisième chiffre significatif
	}
}

void CD74HC4511_DisplayDigit(uint8_t digit, uint8_t position, uint8_t point) {
// Assurez-vous que digit est entre 0 et 9 et position entre 1 et 3
	if (digit > 9 || position < 1 || position > 3)
		return;

// Convertir le chiffre en une représentation binaire sur 4 bits
	uint8_t d0 = digit & 0x01;
	uint8_t d1 = (digit >> 1) & 0x01;
	uint8_t d2 = (digit >> 2) & 0x01;
	uint8_t d3 = (digit >> 3) & 0x01;

// Écrire les valeurs sur les broches D0 à D3
	HAL_GPIO_WritePin(D0_GPIO_Port, D0_Pin, d0 ? GPIO_PIN_SET : GPIO_PIN_RESET);
	HAL_GPIO_WritePin(D1_GPIO_Port, D1_Pin, d1 ? GPIO_PIN_SET : GPIO_PIN_RESET);
	HAL_GPIO_WritePin(D2_GPIO_Port, D2_Pin, d2 ? GPIO_PIN_SET : GPIO_PIN_RESET);
	HAL_GPIO_WritePin(D3_GPIO_Port, D3_Pin, d3 ? GPIO_PIN_SET : GPIO_PIN_RESET);

// Latch les valeurs
	HAL_GPIO_WritePin(Latch_GPIO_Port, Latch_Pin, GPIO_PIN_SET);
	osDelay(1);
	HAL_GPIO_WritePin(Latch_GPIO_Port, Latch_Pin, GPIO_PIN_RESET);

// Activer ou désactiver le point
	HAL_GPIO_WritePin(Point_GPIO_Port, Point_Pin,
			point ? GPIO_PIN_SET : GPIO_PIN_RESET);

// Activer l'affichage pour le chiffre sélectionné
	HAL_GPIO_WritePin(Di1_GPIO_Port, Di1_Pin,
			(position == 1) ? GPIO_PIN_SET : GPIO_PIN_RESET);
	HAL_GPIO_WritePin(Di2_GPIO_Port, Di2_Pin,
			(position == 2) ? GPIO_PIN_SET : GPIO_PIN_RESET);
	HAL_GPIO_WritePin(Di3_GPIO_Port, Di3_Pin,
			(position == 3) ? GPIO_PIN_SET : GPIO_PIN_RESET);

// Activer l'afficheur
	HAL_GPIO_WritePin(EN_GPIO_Port, EN_Pin, GPIO_PIN_SET);
	osDelay(1);
	HAL_GPIO_WritePin(EN_GPIO_Port, EN_Pin, GPIO_PIN_RESET);

// Désactiver tous les chiffres après affichage pour éviter les chevauchements
	HAL_GPIO_WritePin(Di1_GPIO_Port, Di1_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(Di2_GPIO_Port, Di2_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(Di3_GPIO_Port, Di3_Pin, GPIO_PIN_RESET);
}

float Altitude() {
	if (gl_GPSData[0].altitude > gl_AltMax) {
		gl_AltMax = gl_GPSData[0].altitude;
	}
	if (gl_Ancienne_Alt < gl_GPSData[0].altitude) {
		gl_Denive += gl_GPSData[0].altitude - gl_Ancienne_Alt;
	}
	gl_Ancienne_Alt = gl_GPSData[0].altitude;

	return gl_GPSData[0].altitude;
}

float Temperature() {
	return gl_temp;
}

float Vitesse() {
	if (gl_GPSData[0].speed > gl_VitesseMax) {
		gl_VitesseMax = gl_GPSData[0].speed;
	}
	return gl_GPSData[0].speed;
}

float Distance() {
	float dist = haversine_distance(gl_ProchaineLatitude, gl_ProchaineLongitude,
			gl_GPSData[0].latitude, gl_GPSData[0].longitude);
	if (dist < 0.200)
		Suivant();
	return dist;
}

uint16_t Angle() {
	float AzimutCourant = Courant();
	float AzimutCible = calculate_bearing(gl_ProchaineLatitude,
			gl_ProchaineLongitude, gl_GPSData[0].latitude,
			gl_GPSData[0].longitude);
	float angle = calculate_relative_direction(AzimutCourant, AzimutCible);
	return angle;
}

void GPS_Read(GPS_Data *data) {
	char buffer[128];
	char *token;
	const char *delimiter = ",";

	// Lire les données NMEA de l'UART
	HAL_UART_Receive(&huart3, (uint8_t*) buffer, sizeof(buffer), HAL_MAX_DELAY);

	// Rechercher le début de la trame GPGGA (trame qui contient l'altitude)
	if (strstr(buffer, "$GPGGA")) {
		// Extraire les données GPGGA
		token = strtok(buffer, delimiter); // "$GPGGA"
		for (int i = 0; i < 10; i++) {
			token = strtok(NULL, delimiter);
			if (i == 1) { // Latitude
				data[0].latitude = atof(token);
			} else if (i == 2) { // N/S indicator
				if (token[0] == 'S') {
					data[0].latitude = -data[0].latitude;
				}
			} else if (i == 3) { // Longitude
				data[0].longitude = atof(token);
			} else if (i == 4) { // E/W indicator
				if (token[0] == 'W') {
					data[0].longitude = -data[0].longitude;
				}
			} else if (i == 9) { // Altitude
				data[0].altitude = atof(token);
			}
		}
	}

	// Rechercher le début de la trame GPVTG (trame qui contient la vitesse)
	if (strstr(buffer, "$GPVTG")) {
		// Extraire les données GPVTG
		token = strtok(buffer, delimiter); // "$GPVTG"
		for (int i = 0; i < 7; i++) {
			token = strtok(NULL, delimiter);
			if (i == 6) { // Speed in km/h
				data[0].speed = atof(token);
			}
		}
	}
}

void Shift_GPS_Data(GPS_Data *data) {
	// Décaler les anciennes données vers l'index suivant
	for (int i = 9; i > 0; i--) {
		data[i] = data[i - 1];
	}
}

void DoubleBoutton() {
	/*
	 if(gl_Mode!=Cali)
	 gl_Mode = Cali;
	 else
	 gl_Mode = GPS;*/

	calibre();
}



void BoutonDroitCours() {
	changeMode();
}

void BoutonDroitLong() {
	Suivant();
}

void BoutonGaucheCours() {
	Enregistre();
}

void BoutonGaucheLong() {
	Precedent();
}

void changeMode() {
	switch (gl_Mode) {
	case Alt:
		gl_Mode = temp;
		break;
	case temp:
		gl_Mode = Vit;
		break;
	case Vit:
		gl_Mode = GPS;
		break;
	case GPS:
		gl_Mode = Alt;
		break;
	case Cali:
		break;
	}

}

void Suivant() {
	gl_addParcours += (sizeof(float) * 2);
	if (gl_addParcours > STARTADD_ENR)
		gl_addParcours = STARTADD_ENR - sizeof(float) * 2;
	EEPROM_WriteUInt16(ADD_Parcours, gl_addParcours);
	EEPROM_ReadFloat(gl_addParcours, &gl_ProchaineLatitude);
	EEPROM_ReadFloat(gl_addParcours + sizeof(float), &gl_ProchaineLongitude);

}

void Precedent() {
	gl_addParcours -= (sizeof(float) * 2);
	if (gl_addParcours < STARTADD_PAR)
		gl_addParcours = STARTADD_PAR;
	EEPROM_WriteUInt16(ADD_Parcours, gl_addParcours);
	EEPROM_ReadFloat(gl_addParcours, &gl_ProchaineLatitude);
	EEPROM_ReadFloat(gl_addParcours + sizeof(float), &gl_ProchaineLongitude);
}

void Enregistre() {
	EEPROM_WriteFloat(gl_addEnregistre, gl_GPSData[0].latitude);
	gl_addEnregistre += sizeof(float);
	EEPROM_WriteFloat(gl_addEnregistre, gl_GPSData[0].longitude);
	gl_addEnregistre += sizeof(float);
	EEPROM_WriteFloat(gl_addEnregistre, gl_GPSData[0].altitude);
	gl_addEnregistre += sizeof(float);
	EEPROM_WriteFloat(gl_addEnregistre, gl_GPSData[0].speed);
	gl_addEnregistre += sizeof(float);
	EEPROM_WriteFloat(gl_addEnregistre, gl_temp);
	gl_addEnregistre += sizeof(float);
	EEPROM_WriteUInt16(ADD_ENREGISTRE, gl_addEnregistre);
}


void calibre(){

}


void ReadTemp() {
	uint8_t tempReg[2] = { 0 };
	uint16_t tempData;
	float temperature;

	if (HAL_I2C_Master_Receive(&hi2c1, TMP102_I2C_ADDRESS, tempReg, 2, 1000) == HAL_OK){
		tempData = (tempReg[0] << 4) | (tempReg[1] >> 4);

		if (tempData & 0x800){
			tempData |= 0xF000;  // Complément à deux pour les nombres négatifs
			temperature = tempData * 0.0625; // Multiplicateur pour obtenir la température en degrés Celsius
		} else {
			temperature = tempData * 0.0625; // Multiplicateur pour obtenir la température en degrés Celsius
		}
	}
	if(gl_TempMax>temperature)
		gl_TempMax=temperature;
	if(gl_TempMin<temperature)
		gl_TempMin=temperature;
	gl_temp=temperature;

}

float haversine_distance(float lat1, float lon1, float lat2, float lon2) {
	float dlat = (lat2 - lat1) * DEG_TO_RAD;
	float dlon = (lon2 - lon1) * DEG_TO_RAD;
	lat1 *= DEG_TO_RAD;
	lat2 *= DEG_TO_RAD;

	float a = sin(dlat / 2) * sin(dlat / 2)
			+ cos(lat1) * cos(lat2) * sin(dlon / 2) * sin(dlon / 2);
	float c = 2 * atan2(sqrt(a), sqrt(1 - a));

	return EARTH_RADIUS_KM * c;
}

/*
 * lat 1 lon1 c'est les suivante
 * lat 2 lon2 c'est celle de ce jour
 */
float calculate_bearing(float lat1, float lon1, float lat2, float lon2) {
	lat1 *= DEG_TO_RAD;
	lon1 *= DEG_TO_RAD;
	lat2 *= DEG_TO_RAD;
	lon2 *= DEG_TO_RAD;

	float dlon = lon2 - lon1;

	float x = sin(dlon) * cos(lat2);
	float y = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dlon);

	float initial_bearing = atan2(x, y);
	// Convertir de radians à degrés
	initial_bearing *= RAD_TO_DEG;

	// Normaliser l'angle pour qu'il soit compris entre 0° et 360°
	initial_bearing = fmod((initial_bearing + 360.0), 360.0);

	return initial_bearing;
}

// Fonction pour calculer la direction relative en fonction de l'azimut actuel
float calculate_relative_direction(float current_bearing, float target_bearing) {
	float relative_direction = target_bearing - current_bearing;
	// Normaliser l'angle pour qu'il soit compris entre 0° et 360°
	relative_direction = fmod((relative_direction + 360.0), 360.0);

	return relative_direction;
}

void Led() {
	switch (gl_Mode) {
	case Alt:
		HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_RESET);

		break;
	case temp:
		HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_SET);
		break;
	case Vit:
		HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_SET);
		break;
	case GPS:
		uint16_t angle = Angle();
		if (angle > 332.5 || angle <= 22.5) {
			HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_RESET);
		} else if (angle > 22.5 && angle <= 67.5) {
			HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_RESET);
		} else if (angle > 67.5 && angle <= 112.5) {
			HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_RESET);
		} else if (angle > 112.5 && angle <= 157.5) {
			HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_RESET);
		} else if (angle > 157.5 && angle <= 202.5) {
			HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_RESET);
		} else if (angle > 202.5 && angle <= 247.5) {
			HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_RESET);
		} else if (angle > 247.5 && angle <= 292.5) {
			HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_RESET);
		} else if (angle > 292.5 && angle <= 332.5) {
			HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_SET);
		} else {
			HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_RESET);
		}

		break;
	case Cali:
		HAL_GPIO_WritePin(J1_GPIO_Port, J1_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J2_GPIO_Port, J2_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J3_GPIO_Port, J3_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J4_GPIO_Port, J4_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J5_GPIO_Port, J5_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J6_GPIO_Port, J6_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J7_GPIO_Port, J7_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(J8_GPIO_Port, J8_Pin, GPIO_PIN_SET);
		break;
	}
}

float Courant() {
	int i;
	if (gl_GPSData[0].speed > 120)
		i = 2;
	else if (gl_GPSData[0].speed > 90)
		i = 3;
	else if (gl_GPSData[0].speed > 70)
		i = 4;
	else if (gl_GPSData[0].speed > 60)
		i = 5;
	else if (gl_GPSData[0].speed > 50)
		i = 7;
	else if (gl_GPSData[0].speed > 5)
		i = 9;
	else
		return gl_courant; // si on va a moins de 5 kM/h ont garde l'ancien (Pour un qui fait la marche il fuadra une boussole)
	gl_courant = calculate_bearing(gl_GPSData[0].latitude,
			gl_GPSData[0].longitude, gl_GPSData[i].latitude,
			gl_GPSData[i].longitude);
	return gl_courant;

}
/* USER CODE END Application */
