#include "Mezz_send_via_UART.h"

uint32_t validity_UART_send = 8;

extern UART_HandleTypeDef huart2;
extern TIM_HandleTypeDef htim6;

//------read CAN-------
extern int16_t duty_cycle_Left;
extern int16_t current_Left;
extern int32_t erpm_Left;
extern int16_t duty_cycle_Right;
extern int16_t current_Right;
extern int32_t erpm_Right;
//------read PWM-------
extern uint32_t recSteer;		// steer
extern uint32_t recThrottle;	// throttle
//------read PSI--------
extern int16_t roll;
extern int16_t pitch;
extern int16_t yaw;
extern int16_t lon;
extern int16_t lat;
extern int16_t ver;
//-----read UART-------
extern double latitude;
extern double longitude;
extern float velocity;
extern float course;
//----send CAN--------
extern int32_t throttle_sent_via_CAN_Left;
extern int32_t throttle_sent_via_CAN_Right; // new
//----Odometry--------
extern float velocityOdo;

uint8_t arr2send[80];
uint8_t indexArr2send 	= 0;
uint16_t delimiterStart = 10203;
uint16_t delimiterEnd 	= 61514;
uint16_t timeStamp 		= 0;

uint8_t initializatin_UART_send = 1;
uint8_t initTimer6 = 1;


uint8_t Mezz_send_via_UART(void)
{
#ifdef MACRON
	if(initTimer6 == 1)
	{
		HAL_TIM_Base_Start(&htim6);
		initTimer6 = 0;
	}
	// delimiter
		add_int16(delimiterStart);
	add_int16(duty_cycle_Left);
	add_int16(current_Left);
	add_int32(erpm_Left);
	add_int16(duty_cycle_Right);
	add_int16(current_Right);
	add_int32(erpm_Right);
	add_int32(recSteer);
	add_int32(recThrottle);
	add_int16(roll);
	add_int16(pitch);
	add_int16(yaw);
	add_int16(lon);
	add_int16(lat);
	add_int16(ver);
	add_double( latitude);
	add_double( longitude);
	add_float(velocity);
	add_float(course);
	add_int32(throttle_sent_via_CAN_Left);
	add_int32(throttle_sent_via_CAN_Right);
	add_float(velocityOdo);
	// sending time of the sequence is 6us max. Resolution in 1us
		timeStamp = __HAL_TIM_GET_COUNTER(&htim6);
		__HAL_TIM_SET_COUNTER(&htim6,0);
		add_int16(timeStamp);
	// delimiter
		add_int16(delimiterEnd);

	HAL_UART_Transmit_IT(&huart2,arr2send,indexArr2send);
	indexArr2send = 0;
#endif
	validity_UART_send = 0;

return validity_UART_send;
}
#ifdef MACRON

void add_int16(int16_t value)
{
		arr2send[indexArr2send]= (uint8_t) value;
		indexArr2send++;

		arr2send[indexArr2send]= (uint8_t) (value >> 8);
		indexArr2send++;
}

void add_int32(int32_t value)
{
		arr2send[indexArr2send]= (uint8_t) value;
		indexArr2send++;

		arr2send[indexArr2send]= (uint8_t) (value >> 8);
		indexArr2send++;

		arr2send[indexArr2send]= (uint8_t) (value >> 16);
		indexArr2send++;

		arr2send[indexArr2send]= (uint8_t) (value >> 24);
		indexArr2send++;
}

void add_int64(int64_t value)
{
		arr2send[indexArr2send]= (uint8_t) value;
		indexArr2send++;

		arr2send[indexArr2send]= (uint8_t) (value >> 8);
		indexArr2send++;

		arr2send[indexArr2send]= (uint8_t) (value >> 16);
		indexArr2send++;

		arr2send[indexArr2send]= (uint8_t) (value >> 24);
		indexArr2send++;

		arr2send[indexArr2send]= (uint8_t) (value >> 32);
		indexArr2send++;

		arr2send[indexArr2send]= (uint8_t) (value >> 40);
		indexArr2send++;

		arr2send[indexArr2send]= (uint8_t) (value >> 48);
		indexArr2send++;

		arr2send[indexArr2send]= (uint8_t) (value >> 56);
		indexArr2send++;
}

void add_double(double value)
{
	int64_t int_val;
	memcpy(&int_val,&value, sizeof value);
	add_int64(int_val);
}

void add_float(float value)
{
	//value = value * 1000;
	int32_t int_val;
	memcpy(&int_val,&value, sizeof value);
	add_int32(int_val);
}

/*
 * Bluetooth module initialization function.
 * Initialization of the bluetooth has to be done ones at the beginning.
 * Bluetoot store the setting even it lose power supply
 */
void initBluetoothModule_HC06(void)
{
		if(initializatin_UART_send == 0)
		{
		/*	AT+BAUD8 OK115200
			AT+BAUD9 OK230400
			AT+BAUDA OK460800
			AT+BAUDB OK921600
			AT+BAUDC OK1382400	*/

			uint8_t string1[2] = "AT"; // Confirm AT
			HAL_UART_Transmit(&huart2, string1, sizeof(string1), 300);
			HAL_Delay(1500); // 1.5 second wait is needed

			uint8_t string2[13] = "AT+NAMEPVT_TQ";
			HAL_UART_Transmit(&huart2, string2, sizeof(string2), 300);
			HAL_Delay(1500); // 1.5 second wait is needed

			uint8_t string3[8] = "AT+BAUDB";
			HAL_UART_Transmit(&huart2, string3, sizeof(string3), 300);
			HAL_Delay(1500); // 1.5 second wait is needed
			initializatin_UART_send = 1;

			huart2.Init.BaudRate = 921600;
			if (HAL_UART_Init(&huart2) != HAL_OK)
			{
				_Error_Handler(__FILE__, __LINE__);
			}
		}
}
#endif