/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file           : main.c
* @brief          : Main program body
******************************************************************************
* @attention
*
* 
******************************************************************************
*/

/* Standard library includes */
#include <stdio.h>

/* Library includes */
#include <pb_encode.h>
#include <pb_decode.h>
#include "handshake.pb.h"
#include "FreeRTOS.h"
#include "task.h"

/* Project includes */
#include "devices.h"
#include "config.h"
#include "main.h"
#include "semphr.h"

/* Private Macros */
#define device_MDR s2m_MDR_response
#define GET_IDX_FROM_ADDR(i2c_addr) i2c_addr-1
#define GET_BIT_FROM_IDX(a, b) a[b>>5]&(1<<(b%32))
#define SET_BIT_FROM_IDX(a, b) a[b>>5]|=(1<<(b%32))
#define COUNTOF(__BUFFER__)   (sizeof(__BUFFER__) / sizeof(*(__BUFFER__)))

#define I2C_ADDRESS 0x05
#define BUS_DEVICE_LIMIT 128

/* Macro to toggle between master and slave firmware */
#define MASTER

/* Private globals */
I2C_HandleTypeDef hi2c1;
UART_HandleTypeDef huart1;

device_info_t *device_info[BUS_DEVICE_LIMIT] = {NULL};
subscription_info_t* subs_info[BUS_DEVICE_LIMIT];
uint32_t allocated[4]={0};
uint8_t dev_sts[BUS_DEVICE_LIMIT] = {OFFLINE};

static QueueHandle_t xQueue = NULL;
#define mainQUEUE_RECEIVE_TASK_PRIORITY		(tskIDLE_PRIORITY + 2)
#define	mainQUEUE_SEND_TASK_PRIORITY		(tskIDLE_PRIORITY + 1)
#define mainHS_FREQUENCY_MS			(2000 / portTICK_PERIOD_MS)
#define mainQUEUE_LENGTH	                (1)

/* Function prototypes */
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_I2C1_Init(void);
static void MX_USART1_UART_Init(void);

bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
hs_status_t handshake(uint32_t i2c_addr);
bool todo_hs_or_not_todo_hs(uint8_t i2c_addr);
state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status);
bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);


static void prvHandshakeTask(void *pvParameters)
{
    TickType_t xNextWakeTime;
    (void) pvParameters;
    xNextWakeTime = xTaskGetTickCount();
    for( ;; )
    {
	/* Place this task in the blocked state until it is time to run again. */
	vTaskDelayUntil( &xNextWakeTime, mainHS_FREQUENCY_MS );
	hs_status_t hs_status;
	for (int curr_addr=0; curr_addr < 10; curr_addr++) {
	    if (todo_hs_or_not_todo_hs(curr_addr)) {
		hs_status = handshake(curr_addr);
		dev_sts[GET_IDX_FROM_ADDR(curr_addr)] = get_state_from_hs_status(curr_addr, hs_status);
	    }
	}	
    }
}

static void prvQueueSendTask(void *prvParam)
{
    
}

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void)
{ 
    /* MCU Configuration */

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

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

    /* Initialize all configured peripherals */
    MX_GPIO_Init();
    MX_I2C1_Init();
    MX_USART1_UART_Init();

#ifdef TESTING_ENABLE
#ifdef MASTER
    uint8_t reset_string[] = "\r\n\n==========MASTER RESET=========\r\n\n";
    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
#else
    uint8_t reset_string[] = "\r\n\n==========SLAVE RESET=========\r\n\n";
    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);
#endif /* MASTER */
#endif /* TESTING_ENABLE */
    
    xQueue = xQueueCreate( mainQUEUE_LENGTH, sizeof( uint32_t ) );
    if( xQueue != NULL )
    {
	/* Start the two tasks as described in the comments at the top of this
	   file. */
	xTaskCreate(prvHandshakeTask, "handshake_rx", configMINIMAL_STACK_SIZE, NULL,
		    mainQUEUE_RECEIVE_TASK_PRIORITY,NULL);
	
	xTaskCreate(prvQueueSendTask, "TX", configMINIMAL_STACK_SIZE, NULL,
		    mainQUEUE_SEND_TASK_PRIORITY, NULL);

	/* Start the tasks and timer running. */
	vTaskStartScheduler();
    }
    for( ;; );
    
#ifdef MASTER
    hs_status_t hs_status;
    for (int curr_addr=0; curr_addr < 10; curr_addr++) {
	if (todo_hs_or_not_todo_hs(curr_addr)) {
	    hs_status = handshake(curr_addr);
	    dev_sts[GET_IDX_FROM_ADDR(curr_addr)] = get_state_from_hs_status(curr_addr, hs_status);
	}
    }

#else /* Slave code*/
    {
	uint8_t MDR_buf[128], debug_buf[128], term[]="\r\n";
	size_t MDR_enc_size;
	s2m_MDR_response res;
	res.MDR_version=1.1;
	res.module_id = 1;
	res.module_class=1;
	res.entity_id=32;

	res.subscriptions.funcs.encode=encode_subscription_callback;
	pb_ostream_t ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf));
	if(!pb_encode(&ostream, s2m_MDR_response_fields, &res)){
#ifdef DEBUG_ENABLE
	    uint8_t err_buf[] = "MDR encoding error\r\n";
	    HAL_UART_Transmit(&huart1, err_buf, sizeof(err_buf), 100);
#endif
	    while(1) {
		HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
		HAL_Delay(500);
	    }
	}
	MDR_enc_size = ostream.bytes_written;
#ifdef TESTING_ENABLE	    
	sprintf((char*)debug_buf, "MDR Encoded size: %d\r\n", MDR_enc_size);
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	
	uint8_t bufbuf[] = "MDR Buffer: ";	  
	HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);
	for(int x=0; x<MDR_enc_size; x++) {
	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);
	}
	HAL_UART_Transmit(&huart1, debug_buf, MDR_enc_size, 100);  
	HAL_UART_Transmit(&huart1, term, 2, 100);
	memset(debug_buf, 0, 128);
#endif
	    
	uint8_t MDR_ACK_buf[8] = {0};
	s2m_MDR_req_ACK ack;
	ack.MDR_res_length = MDR_enc_size;
	pb_ostream_t MDR_ack_ostream = pb_ostream_from_buffer(MDR_ACK_buf,
							      sizeof(MDR_ACK_buf));
	if(!pb_encode(&MDR_ack_ostream, s2m_MDR_req_ACK_fields, &ack)) {
#ifdef DEBUG_ENABLE
	    uint8_t errbuf[] = "MDR ACK encoding error\r\n";
	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);
#endif /* DEBUG_ENABLE */
	    while(1) {
		HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
		HAL_Delay(500);
	    }
	}
	size_t MDR_ack_size = MDR_ack_ostream.bytes_written;
#ifdef TESTING_ENABLE
	sprintf((char*)debug_buf, "MDR ACK Encoded size: %d\r\n", MDR_ack_size);
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
#endif /* TESTING_ENABLE */

	uint8_t MDR_req_buf[8]; 
	m2s_MDR_request MDR_req;
	
	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)MDR_req_buf, 2, 10000) != HAL_OK) {
#ifdef DEBUG_ENABLE
	    uint8_t debug_buf[] = "Failed to get MDR req\r\n";
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
#endif /* DEBUG_ENABLE */
	}

#ifdef TESTING_ENABLE
	uint8_t bufbuf2[] = "MDR ACK buffer: ";
	HAL_UART_Transmit(&huart1, bufbuf2, sizeof(bufbuf2), 100);
	for(int x=0; x<2; x++) {
	    sprintf((char*)debug_buf+x, "%x", MDR_ACK_buf[x]);
	}
	HAL_UART_Transmit(&huart1, debug_buf, MDR_enc_size, 100);  
	HAL_UART_Transmit(&huart1, term, 2, 100);
	memset(debug_buf, 0, 128);
#endif
	
	pb_istream_t MDR_req_istream = pb_istream_from_buffer(MDR_req_buf, 2);
	if(!pb_decode(&MDR_req_istream, m2s_MDR_request_fields, &MDR_req)) {
#ifdef DEBUG_ENABLE
	    uint8_t errbuf[] = "MDR request decoding error\r\n";
	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);
#endif /* DEBUG_ENABLE */
	}
	
#ifdef TESTING_ENABLE
	sprintf((char*)debug_buf, "Got requested record type: %ld\r\n", MDR_req.record_type);
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
#endif /* TESTING_ENABLE */

	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);

	if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_ACK_buf, s2m_MDR_req_ACK_size, 10000) != HAL_OK) {
#ifdef DEBUG_ENABLE
	    sprintf((char*)debug_buf, "Unable to send MDR ACK. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
#endif /* DEBUG_ENABLE */
	}

	uint8_t MDR_CTS_buf[8];
	m2s_MDR_res_CTS MDR_CTS;
	
	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {
#ifdef DEBUG_ENABLE
	    sprintf((char*)debug_buf, "Failed to get MDR CTS\r\n");
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
#endif /* DEBUG_ENABLE */
	}
#ifdef TESTING_ENABLE
	uint8_t ctsbuf[] = "Got CTS buffer: ";
	for(int x=0; x<2; x++) {
	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);
	}
	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 100);
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	HAL_UART_Transmit(&huart1, term, 2, 100);
	memset(debug_buf, 0, 128);
#endif /* TESTING_ENABLE */
	
	pb_istream_t MDR_CTS_istream = pb_istream_from_buffer(MDR_CTS_buf, 2);
	if (!pb_decode(&MDR_CTS_istream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {
#ifdef DEBUG_ENABLE
	    sprintf((char*)debug_buf, "Failed to decode MDR CTS\r\n");
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
#endif /* DEBUG_ENABLE */
	    Error_Handler();
	}

	uint32_t MDR_timeout = MDR_CTS.timeout;
	if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_buf, MDR_enc_size, 10000) !=
	    HAL_OK) {
#ifdef DEBUG_ENABLE
	    sprintf((char*)debug_buf, "Unable to send MDR. I2C error: %ld\r\n",
		    HAL_I2C_GetError(&hi2c1));
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
#endif /* DEBUG_ENABLE */
	}
	else {
	    sprintf((char*)debug_buf, "Successfully sent MDR\r\n");
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
	}
    }	
#endif /* MASTER */
    
    while (1)
    {

    }
}

hs_status_t handshake(uint32_t i2c_addr)
{

    /* Handshake variables */

    uint8_t hs_sts = IDLE;
    uint8_t *MDR_req_buf, *MDR_ACK_buf, *MDR_CTS_buf, *MDR_buf;
    uint32_t AF_error_counter = 0;
    uint32_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);
    uint32_t MDR_len = 0;
    
    m2s_MDR_request MDR_req_message;
    s2m_MDR_req_ACK MDR_ACK;
    m2s_MDR_res_CTS MDR_CTS;
    s2m_MDR_response MDR_res_message;

#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
    uint8_t debug_buf[128];
#endif
#ifdef TESTING_ENABLE
    uint8_t term[] = "\r\n";
    size_t MDR_req_size, MDR_CTS_size;
#endif
    
    while (hs_sts != HS_FAILED && hs_sts != HS_REGISTERED) {
	switch (hs_sts) {
	case (IDLE):
	{
	    MDR_req_buf = malloc(8);	    
	    pb_ostream_t MDR_req_stream = pb_ostream_from_buffer(MDR_req_buf, 2);
	    MDR_req_message.record_type = 7; /* Placeholder for default record type */
	    if(!pb_encode(&MDR_req_stream, m2s_MDR_request_fields, &MDR_req_message)) {
		hs_sts = HS_FAILED;
#ifdef DEBUG_ENABLE
		goto __MDR_REQ_ENC_FAIL;
	    __MDR_REQ_ENC_FAIL_END:
		__asm__("nop");
#endif
	    }
	    else {
#ifdef TESTING_ENABLE
		MDR_req_size = MDR_req_stream.bytes_written;
		goto __HS_IDLE_TESTING;
	    __HS_IDLE_TESTING_END:
		__asm__("nop");
#endif	    
		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)MDR_req_buf,
					    MDR_req_buf_len, 10000) != HAL_OK) {
		    hs_sts = HS_FAILED;
#ifdef DEBUG_ENABLE
		    goto __HS_MDR_REQ_I2C_ERROR;
		__HS_MDR_REQ_I2C_ERROR_END:
		    __asm__("nop");
#endif
		}
		else {
		    hs_sts = HS_MDR_ACK;
		}
		free(MDR_req_buf);
		break;
	    }
	}
	case (HS_MDR_ACK):
	{
	    MDR_ACK_buf = malloc(8);
	    AF_error_counter = 0;
	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, (uint8_t*)MDR_ACK_buf,
					  s2m_MDR_req_ACK_size, 100) != HAL_OK) {
		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
		    hs_sts = HS_FAILED;
		}
		if (++AF_error_counter > 1500) {
		    hs_sts = HS_FAILED;
		}
		if (hs_sts == HS_FAILED) {
#ifdef DEBUG_ENABLE
		    goto __HS_MDR_ACK_I2C_ERROR;
		__HS_MDR_ACK_I2C_ERROR_END:
		    __asm__("nop");
#endif
		    break;
		}
	    }
	    if (hs_sts != HS_FAILED) {
		pb_istream_t MDR_ACK_istream = pb_istream_from_buffer(MDR_ACK_buf, 2);
		if (!pb_decode(&MDR_ACK_istream, s2m_MDR_req_ACK_fields, &MDR_ACK)) {
		    hs_sts = HS_FAILED;
#ifdef DEBUG_ENABLE
		    goto __MDR_ACK_DEC_ERROR;
		__MDR_ACK_DEC_ERROR_END:
		    __asm__("nop");
#endif
		}
		else {
		    MDR_len = MDR_ACK.MDR_res_length;
		    hs_sts = HS_MDR_CTS;
#ifdef TESTING_ENABLE
		    goto __HS_MDR_ACK_TESTING;
		__HS_MDR_ACK_TESTING_END:
		    __asm__("nop");
#endif
		}
		free(MDR_ACK_buf);
	    }
	    break;
	}
	case (HS_MDR_CTS):
	{
	    MDR_CTS_buf = (uint8_t*)malloc(8);
	    pb_ostream_t MDR_CTS_ostream = pb_ostream_from_buffer(MDR_CTS_buf, sizeof(MDR_CTS_buf));
	    MDR_CTS.timeout = 100;
	    if (!pb_encode(&MDR_CTS_ostream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {
		hs_sts = HS_FAILED;
#ifdef DEBUG_ENABLE
		goto __MDR_CTS_ENC_ERROR;
	    __MDR_CTS_ENC_ERROR_END:
		__asm__("nop");
#endif
	    }
	    else {
#ifdef TESTING_ENABLE
		MDR_CTS_size = MDR_CTS_ostream.bytes_written;
		goto __HS_MDR_CTS_TESTING;
	    __HS_MDR_CTS_TESTING_END:
		__asm__("nop");
#endif
		if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr,
					    (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {
		    hs_sts = HS_FAILED;
#ifdef DEBUG_ENABLE
		    goto __HS_CTS_I2C_ERROR;
		__HS_CTS_I2C_ERROR_END:
		    __asm__("nop");
#endif
		}
		else {
		    hs_sts = HS_MDR_MDR;
		    free(MDR_CTS_buf);
		}
	    }
	    break;
	}
	case (HS_MDR_MDR):
	{
	    MDR_buf = (uint8_t*)malloc(MDR_len);
	    AF_error_counter = 0;
	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
					  (uint8_t*)MDR_buf, MDR_len, 1000) != HAL_OK) {
		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
		    hs_sts = HS_FAILED;
#ifdef DEBUG_ENABLE
		    goto __HS_MDR_I2C_ERROR;
		__HS_MDR_I2C_ERROR_END:
		    __asm__("nop");
#endif
		    break;
		}
		else if (++AF_error_counter > 1500) {
		    hs_sts = HS_FAILED;
		    break;
		}
	    }
	    if (hs_sts != HS_FAILED) {
#ifdef TESTING_ENABLE
		goto __HS_MDR_MDR_TESTING;
	    __HS_MDR_MDR_TESTING_END:
		__asm__("nop");
#endif
		MDR_res_message.subscriptions.funcs.decode = decode_subscriptions_callback;
		MDR_res_message.subscriptions.arg = (void*)dev_idx;
		pb_istream_t MDR_res_stream = pb_istream_from_buffer(MDR_buf, MDR_len);
		if (!pb_decode(&MDR_res_stream, s2m_MDR_response_fields, &MDR_res_message)) {
#ifdef DEBUG_ENABLE
		    goto __HS_MDR_DEC_ERROR;
		__HS_MDR_DEC_ERROR_END:
		    __asm__("nop");
#endif
		}
		else {
#ifdef TESTING_ENABLE
		    goto __MDR_DEC_TESTING;
		__MDR_DEC_TESTING_END:
		    __asm__("nop");
#endif
		    hs_sts = HS_REGISTERED;
		}	       		
	    }
	    break;
	}
	}

    }

#ifdef TESTING_ENABLE
    {
	goto __TESTING_BLOCK_END;
    __HS_IDLE_TESTING:
	sprintf((char*)debug_buf, "MDR req length: %d\r\n", MDR_req_size);
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	uint8_t bufbuf[] = "MDR req buffer: ";
	HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);
	for(int x=0; x<MDR_req_size; x++) {
	    sprintf((char*)debug_buf+x, "%x", MDR_req_buf[x]);
	}
	HAL_UART_Transmit(&huart1, debug_buf, MDR_req_size, 100);
	HAL_UART_Transmit(&huart1, term, 2, 100);
	memset(debug_buf, 0, 128);
	goto __HS_IDLE_TESTING_END;
    __HS_MDR_ACK_TESTING:
	sprintf((char*)debug_buf, "Got MDR message length: %ld\r\n", MDR_ACK.MDR_res_length);
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __HS_MDR_ACK_TESTING_END;
    __HS_MDR_CTS_TESTING:
	sprintf((char*)debug_buf, "CTS size: %d\r\n", MDR_CTS_size);
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	uint8_t ctsbuf[] = "\tCTS buffer: ";
	for(int x=0; x<2; x++) {
	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);
	}
	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 100);
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	HAL_UART_Transmit(&huart1, term, 2, 100);
	memset(debug_buf, 0, 128);
	goto __HS_MDR_CTS_TESTING_END;
    __HS_MDR_MDR_TESTING:
	for (int x=0; x<MDR_len; x++) {
	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);
	}
	uint8_t mdrbuf[] = "Got MDR: ";
	HAL_UART_Transmit(&huart1, mdrbuf, sizeof(mdrbuf), 100);
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);  
	HAL_UART_Transmit(&huart1, term, 2, 100);
	memset(debug_buf, 0, 128);
	goto __HS_MDR_MDR_TESTING_END;
    __MDR_DEC_TESTING:
	sprintf((char*)debug_buf, "MDR Decode success\r\n\tFirst subscibed module: %d\r\n",
		subs_info[dev_idx]->module_ids[1]);
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __MDR_DEC_TESTING_END;
    }
__TESTING_BLOCK_END:
    __asm__("nop");
#endif

#ifdef DEBUG_ENABLE
    {
	goto __DEBUG_BLOCK_END;
    __MDR_REQ_ENC_FAIL:
	sprintf((char*)debug_buf, "MDR reqest encoding error\r\n");
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __MDR_REQ_ENC_FAIL_END;
    __HS_MDR_REQ_I2C_ERROR:
	sprintf((char*)debug_buf, "Unable to send MDR request. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __HS_MDR_REQ_I2C_ERROR_END;
    __HS_MDR_ACK_I2C_ERROR:
	sprintf((char*)debug_buf, "Unable to get MDR ACK. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __HS_MDR_ACK_I2C_ERROR_END;
    __MDR_ACK_DEC_ERROR:
	sprintf((char*)debug_buf, "MDR ACK decoding error\r\n");
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __MDR_ACK_DEC_ERROR_END;
    __MDR_CTS_ENC_ERROR:	
	sprintf((char*)debug_buf, "MDR encoding error\r\n");
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __MDR_CTS_ENC_ERROR_END;
    __HS_CTS_I2C_ERROR:
	sprintf((char*)debug_buf, "Unable to send MDR CTS. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __HS_CTS_I2C_ERROR_END;
    __HS_MDR_I2C_ERROR:
	sprintf((char*)debug_buf, "Unable to get MDR. I2C error: %ld\n\tError counter: %ld\r\n", HAL_I2C_GetError(&hi2c1), AF_error_counter);
	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	memset(debug_buf, 0, 128);
	goto __HS_MDR_I2C_ERROR_END;	
    }
__HS_MDR_DEC_ERROR:
    sprintf((char*)debug_buf, "MDR decode error\r\n");
    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
    memset(debug_buf, 0, 128);
    goto __HS_MDR_DEC_ERROR_END;
__DEBUG_BLOCK_END:
    __asm__("nop");
#endif
    
    return hs_sts;
}

bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
{
    _subscriptions subs;
    int *subs_idx = (int*)args;

    /* Check is storage is allocated; if not, allocate it */
    if ((GET_BIT_FROM_IDX(allocated, *subs_idx)) == 0) {
	subs_info[*subs_idx] = (subscription_info_t*)malloc(sizeof(subscription_info_t));
	SET_BIT_FROM_IDX(allocated, *subs_idx);
	subs_info[*subs_idx]->mod_idx = subs_info[*subs_idx]->entity_idx =
	    subs_info[*subs_idx]->class_idx = subs_info[*subs_idx]->i2c_idx = 0;
    }
    
    if(!pb_decode(istream, _subscriptions_fields, &subs))
    	return false;

    /* Parse all fields if they're included */
    if (subs.has_module_id)
	subs_info[*subs_idx]->module_ids[subs_info[*subs_idx]->mod_idx++] =
	    subs.module_id;
    if (subs.has_entity_id)
	subs_info[*subs_idx]->entity_ids[subs_info[*subs_idx]->entity_idx++] =
	    subs.entity_id;
    if (subs.has_module_class)
	subs_info[*subs_idx]->module_class[subs_info[*subs_idx]->class_idx++] =
	    subs.module_class;
    if (subs.has_i2c_address)
	subs_info[*subs_idx]->i2c_address[subs_info[*subs_idx]->i2c_idx++] =
	    subs.i2c_address;
    return true;
}

bool todo_hs_or_not_todo_hs(uint8_t i2c_addr)
{
    uint8_t device_idx = GET_IDX_FROM_ADDR(i2c_addr);
    state_t device_curr_state = dev_sts[device_idx];
    bool do_hs = false;
    switch(device_curr_state) {
    case NO_HS:
    case CONNECTED:
    case FAILED:
    case OFFLINE:	
	do_hs = true;
	break;
    case REGISTERED:
    case NO_DATA:
	do_hs = false;
	break;	
    }
    return do_hs;
}

state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status)
{
    state_t device_state = OFFLINE;
    switch(hs_status) {
    case IDLE:
    case HS_FAILED:
	device_state = OFFLINE;
	break;
    case HS_MDR_ACK:
    case HS_MDR_CTS:
    case HS_MDR_MDR:
	device_state = FAILED;
	break;
    case HS_REGISTERED:
	device_state = REGISTERED;
	break;    
    }
    return device_state;
}

bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
{
    if(ostream!=NULL && field->tag == s2m_MDR_response_subscriptions_tag) {
	for (int x=0; x<2; x++) {
	    _subscriptions subs;
	    subs.module_id = x+10*x;
	    subs.i2c_address = x+1;
	    subs.has_entity_id=false;
	    subs.has_module_class=false;
	    subs.has_module_id=true;
	    subs.has_i2c_address=true;
	    if(!pb_encode_tag_for_field(ostream, field)){
		printf("ERR1\n");
		return false;
	    }
	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)){
		printf("ERR2\n");
		return false;
	    }
	}
    }
    else{
	return false;
    }
    return true;
}

/**
 * @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_RCC_PWR_CLK_ENABLE();
    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);
    /** Initializes the CPU, AHB and APB busses clocks 
     */
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    {
	Error_Handler();
    }
    /** Initializes the CPU, AHB and APB busses clocks 
     */
    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
	|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
    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_0) != HAL_OK)
    {
	Error_Handler();
    }
}

/**
 * @brief I2C1 Initialization Function
 * @param None
 * @retval None
 */
static void MX_I2C1_Init(void)
{
    hi2c1.Instance = I2C1;
    hi2c1.Init.ClockSpeed = 100000;
    hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
    hi2c1.Init.OwnAddress1 = I2C_ADDRESS;
    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
    hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
    hi2c1.Init.OwnAddress2 = 0xFF;
    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
    if (HAL_I2C_Init(&hi2c1) != HAL_OK)
    {
	Error_Handler();
    }

}

/**
 * @brief USART1 Initialization Function
 * @param None
 * @retval None
 */
static void MX_USART1_UART_Init(void)
{
    huart1.Instance = USART1;
    huart1.Init.BaudRate = 9600;
    huart1.Init.WordLength = UART_WORDLENGTH_8B;
    huart1.Init.StopBits = UART_STOPBITS_1;
    huart1.Init.Parity = UART_PARITY_NONE;
    huart1.Init.Mode = UART_MODE_TX_RX;
    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    huart1.Init.OverSampling = UART_OVERSAMPLING_16;
    if (HAL_UART_Init(&huart1) != HAL_OK)
    {
	Error_Handler();
    }

}

/**
 * @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_GPIOC_CLK_ENABLE();
    __HAL_RCC_GPIOH_CLK_ENABLE();
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();

    /*Configure GPIO pin Output Level */
    HAL_GPIO_WritePin(led_GPIO_Port, led_Pin, GPIO_PIN_RESET);

    /*Configure GPIO pin : led_Pin */
    GPIO_InitStruct.Pin = led_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(led_GPIO_Port, &GPIO_InitStruct);

}

/**
 * @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 */
    while (1) {
	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
	HAL_Delay(1000);
    }
    /* 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,
       tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
    /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

// Macro to use CCM (Core Coupled Memory) in STM32F4
#define CCM_RAM __attribute__((section(".ccmram")))

#define FPU_TASK_STACK_SIZE 256

StackType_t fpuTaskStack[FPU_TASK_STACK_SIZE] CCM_RAM;  // Put task stack in CCM
StaticTask_t fpuTaskBuffer CCM_RAM;  // Put TCB in CCM

void vApplicationTickHook(void) {
}

/* vApplicationMallocFailedHook() will only be called if
   configUSE_MALLOC_FAILED_HOOK is set to 1 in FreeRTOSConfig.h.  It is a hook
   function that will get called if a call to pvPortMalloc() fails.
   pvPortMalloc() is called internally by the kernel whenever a task, queue,
   timer or semaphore is created.  It is also called by various parts of the
   demo application.  If heap_1.c or heap_2.c are used, then the size of the
   heap available to pvPortMalloc() is defined by configTOTAL_HEAP_SIZE in
   FreeRTOSConfig.h, and the xPortGetFreeHeapSize() API function can be used
   to query the size of free heap space that remains (although it does not
   provide information on how the remaining heap might be fragmented). */
void vApplicationMallocFailedHook(void) {
  taskDISABLE_INTERRUPTS();
  for(;;);
}

/* vApplicationIdleHook() will only be called if configUSE_IDLE_HOOK is set
   to 1 in FreeRTOSConfig.h.  It will be called on each iteration of the idle
   task.  It is essential that code added to this hook function never attempts
   to block in any way (for example, call xQueueReceive() with a block time
   specified, or call vTaskDelay()).  If the application makes use of the
   vTaskDelete() API function (as this demo application does) then it is also
   important that vApplicationIdleHook() is permitted to return to its calling
   function, because it is the responsibility of the idle task to clean up
   memory allocated by the kernel to any task that has since been deleted. */
void vApplicationIdleHook(void) {
}

void vApplicationStackOverflowHook(xTaskHandle pxTask, signed char *pcTaskName) {
  (void) pcTaskName;
  (void) pxTask;
  /* Run time stack overflow checking is performed if
     configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2.  This hook
     function is called if a stack overflow is detected. */
  taskDISABLE_INTERRUPTS();
  for(;;);
}

StaticTask_t xIdleTaskTCB CCM_RAM;
StackType_t uxIdleTaskStack[configMINIMAL_STACK_SIZE] CCM_RAM;

/* configUSE_STATIC_ALLOCATION is set to 1, so the application must provide an
implementation of vApplicationGetIdleTaskMemory() to provide the memory that is
used by the Idle task. */
void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize) {
  /* Pass out a pointer to the StaticTask_t structure in which the Idle task's
  state will be stored. */
  *ppxIdleTaskTCBBuffer = &xIdleTaskTCB;

  /* Pass out the array that will be used as the Idle task's stack. */
  *ppxIdleTaskStackBuffer = uxIdleTaskStack;

  /* Pass out the size of the array pointed to by *ppxIdleTaskStackBuffer.
  Note that, as the array is necessarily of type StackType_t,
  configMINIMAL_STACK_SIZE is specified in words, not bytes. */
  *pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
}

static StaticTask_t xTimerTaskTCB CCM_RAM;
static StackType_t uxTimerTaskStack[configTIMER_TASK_STACK_DEPTH] CCM_RAM;

/* configUSE_STATIC_ALLOCATION and configUSE_TIMERS are both set to 1, so the
application must provide an implementation of vApplicationGetTimerTaskMemory()
to provide the memory that is used by the Timer service task. */
void vApplicationGetTimerTaskMemory(StaticTask_t **ppxTimerTaskTCBBuffer, StackType_t **ppxTimerTaskStackBuffer, uint32_t *pulTimerTaskStackSize) {
  *ppxTimerTaskTCBBuffer = &xTimerTaskTCB;
  *ppxTimerTaskStackBuffer = uxTimerTaskStack;
  *pulTimerTaskStackSize = configTIMER_TASK_STACK_DEPTH;
}