/**
******************************************************************************
* @file           : main.c
* @brief          : Main program body
******************************************************************************
* @attention
*
* 
******************************************************************************
*/

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

/* Library includes */
#include <pb_encode.h>
#include <pb_decode.h>

/* Project includes */
#include "main.h"
#include "devices.h"
#include "config.h"
#include "dataflow.h"
#include "handshake.pb.h"
#include "data.pb.h"

/* Private Macros */
#define device_MDR s2m_MDR_response
#define GET_IDX_FROM_ADDR(i2c_addr) i2c_addr-1
#define GET_ADDR_FROM_IDX(idx)      idx+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 */

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

/* 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);
bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
bool encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
bool decode_data_callback(pb_istream_t *istream, const pb_field_t *field, void **args);

/**
 * @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();

    uint8_t reset_string[] = "\r\n\n==========SLAVE RESET=========\r\n\n";
    HAL_UART_Transmit(&huart1, reset_string, sizeof(reset_string), 100);

    {
	uint8_t MDR_buf[128], debug_buf[128], term[]="\r\n";
	size_t MDR_enc_size;
	s2m_MDR_response res;
	res.MDR_version = 0.1;
	res.module_id = 1;
	res.module_class = 2;
	res.entity_id=1;

	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);
	}
    }	

    
    while (1) {
	uint8_t SOR_buf[m2s_SOR_size] = {0}, debug_buf[128];
	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, m2s_SOR_size, 10000) != HAL_OK) {
	    sprintf((char*)debug_buf, "Failed to get SOR\r\n");
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
	}
	else {
	    sprintf((char*)debug_buf, "Got SOR\r\n");
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
	}
	m2s_SOR SOR_message;
	pb_istream_t SOR_istream = pb_istream_from_buffer(SOR_buf, 2);
	if (!pb_decode(&SOR_istream, m2s_SOR_fields, &SOR_message)) {
	    sprintf((char*)debug_buf, "SOR decode error\r\n");
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
	}
	else {
	    sprintf((char*)debug_buf, "SOR decoded; code: %ld\r\n", SOR_message.SOR_code);
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
	}

	if (SOR_message.SOR_code == 1) {
	    uint8_t data_buf[128];
	    size_t data_enc_size;
	    s2m_data data;
	    data.datapoints.funcs.encode = encode_datapoint_callback;
	    pb_ostream_t data_ostream = pb_ostream_from_buffer(data_buf, sizeof(data_buf));
	    if (!pb_encode(&data_ostream, s2m_data_fields, &data)) {
		sprintf((char*)debug_buf, "Data encoding error\r\n");
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
		Error_Handler();
	    }
	    data_enc_size = data_ostream.bytes_written;
	    
	    s2m_DOC doc = s2m_DOC_init_zero;
	    uint8_t doc_buf[s2m_DOC_size];
	    doc.DOC_code  = 5;
	    doc.tx_length = data_enc_size;
	    pb_ostream_t doc_ostream = pb_ostream_from_buffer(doc_buf, 4);

	    if (!pb_encode(&doc_ostream, s2m_DOC_fields, &doc)) {
		sprintf((char*)debug_buf, "DOC encoding error\r\n");
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
		Error_Handler();
	    }
	    
	    sprintf((char*)debug_buf, "s2m_DOC encoded length: %d\r\n", doc_ostream.bytes_written);
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);

	    if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)doc_buf, 4, 10000) != HAL_OK) {
		sprintf((char*)debug_buf, "DOC I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
		Error_Handler();
	    }
	    else {
		sprintf((char*)debug_buf, "SENT DOC\r\n");
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
	    }

	    uint8_t CTS_buf[8];
	    
	    if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)CTS_buf, 2, 10000) != HAL_OK) {
		sprintf((char*)debug_buf, "Failed to get CTS: %ld\r\n", HAL_I2C_GetError(&hi2c1));
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
		Error_Handler();
	    }
	    else {
		sprintf((char*)debug_buf, "Got CTS\r\n");
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
	    }

	    if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)data_buf, data_enc_size, 10000) != HAL_OK) {
		sprintf((char*)debug_buf, "Data I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
		Error_Handler();
	    }
	    else {
		sprintf((char*)debug_buf, "SENT DATA\r\n");
		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
		memset(debug_buf, 0, 128);
	    }
	}
	else if (SOR_message.SOR_code == 2) {
	    uint8_t CTS_buf[] = {0x0, 0x1};
	    uint8_t len_buf[4], *MDR_buf, *data_buf;
	    /* _datapoint datapoints[16]; */
	    
	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
	    sprintf((char*)debug_buf, "Sent CTS\r\n");
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
		
	    HAL_I2C_Slave_Receive(&hi2c1, len_buf, 4, 1000);
		
	    uint16_t MDR_len = len_buf[1]+(len_buf[0]<<8);
	    MDR_buf = malloc(MDR_len);
	    uint16_t data_len = len_buf[3]+(len_buf[2]<<8);
	    data_buf = malloc(data_len);

	    sprintf((char*)debug_buf, "Got lengths. MDR: %d, data: %d\r\n", MDR_len, data_len);
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
		
	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
	    HAL_I2C_Slave_Receive(&hi2c1, MDR_buf, MDR_len, 10000);
	    HAL_I2C_Slave_Receive(&hi2c1, data_buf, data_len, 10000);
		
	    _datapoint datapoint_message;
	    s2m_MDR_response MDR_message;
	    pb_istream_t MDR_istream  = pb_istream_from_buffer(MDR_buf, MDR_len);
	    pb_istream_t data_istream = pb_istream_from_buffer(data_buf, data_len);
		
	    pb_decode(&MDR_istream, s2m_MDR_response_fields, &MDR_message);
	    pb_decode(&data_istream, _datapoint_fields, &datapoint_message);

	    sprintf((char*)debug_buf, "Got data from %ld, running version %f\r\n\tdata 0: %f\r\n", MDR_message.module_id, MDR_message.MDR_version, datapoint_message.data);
	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
	    memset(debug_buf, 0, 128);
	}
    }   
}

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<20; x++) {
	    _subscriptions subs;
	    subs.module_id = x+1;
	    subs.has_module_id=true;
	    subs.has_entity_id=false;
	    subs.has_module_class=false;
	    subs.has_i2c_address=false;
	    if(!pb_encode_tag_for_field(ostream, field)) {
		return false;
	    }
	    if(!pb_encode_submessage(ostream, _subscriptions_fields, &subs)) {
		return false;
	    }
	}
    }
    else{
	return false;
    }
    return true;
}

bool encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
{
    if (ostream != NULL && field->tag == s2m_data_datapoints_tag) {
	for (int i = 0; i < 4; i++) {
	    _datapoint datapoint = _datapoint_init_zero;
	    datapoint.entity_id  = 1;
	    datapoint.data = 20.70+((float)i/100);
	    if (!pb_encode_tag_for_field(ostream, field))
		return false;
	    if (!pb_encode_submessage(ostream, _datapoint_fields, &datapoint))
		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 */