/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file           : main.c
* @brief          : Main program body
******************************************************************************
* @attention
*
* <h2><center>&copy; Copyright (c) 2020 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"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include <pb_encode.h>
#include <pb_decode.h>
#include "handshake.pb.h"
#include "devices.h"
#include "config.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 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 0x30F

/* Macro to toggle between master and slave firmware */
#define MASTER
/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
I2C_HandleTypeDef hi2c1;

UART_HandleTypeDef huart1;

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private 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);
/* USER CODE BEGIN PFP */
bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
/* USER CODE END PFP */

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

/* USER CODE END 0 */

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void)
{
    /* USER CODE BEGIN 1 */

    /* 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_I2C1_Init();
    MX_USART1_UART_Init();
    /* USER CODE BEGIN 2 */

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

#ifdef MASTER
    /* Handshake variables */
    uint8_t hs_sts = IDLE;
    uint8_t debug_buf[128], term[] = "\r\n";
    uint8_t MDR_req_buf[8];
    size_t MDR_req_size;

    
    while (hs_sts != HS_FAILED && hs_sts != HS_REGISTERED) {
	switch (hs_sts) {
	case (IDLE):
	{
	    /* MDR_req_buf = malloc(8); */
	    m2s_MDR_request MDR_req_message;
	    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)) {
		// TODO Error handling for encoding fail
		
	    }
	    else {
		MDR_req_size = MDR_req_stream.bytes_written;
#ifdef TESTING_ENABLE
		goto __HS_IDLE_TESTING;
	    __HS_IDLE_TESTING_END:
		__asm__("nop");
#endif		
	    }
	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)I2C_ADDRESS, (uint8_t*)MDR_req_buf,
					MDR_req_buf_len, 10000) != HAL_OK) {
		// Error handling for transmit fail
#ifdef DEBUG_ENABLE
		uint8_t err_buf[] = "I2C error\r\n";
		HAL_UART_Transmit(&huart1, err_buf, sizeof(err_buf), 100);
#endif /* DEBUG_ENABLE */
		hs_sts = HS_FAILED;
	    }
	    else {
#ifdef DEBUG_ENABLE
		uint8_t err_buf[] = "I2C success??\r\n";
		HAL_UART_Transmit(&huart1, err_buf, sizeof(err_buf), 100);
#endif /* DEBUG_ENABLE */		
		hs_sts = HS_REGISTERED; // Placeholder for testing, change this to next step
	    }
	}
	}
    }

#ifdef TESTING_ENABLE
    {
	goto __TESTING_BLOCK_END;
    __HS_IDLE_TESTING:
	sprintf((char*)debug_buf, "Encoded 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;
    }
__TESTING_BLOCK_END:
    __asm__("nop");
#endif
    
    
#else /* MASTER */

#endif /* MASTER */
    
    /* USER CODE END 2 */
    /* Infinite loop */
    /* USER CODE BEGIN WHILE */
    while (1)
    {
	/* USER CODE END WHILE */
	/* USER CODE BEGIN 3 */
    }
    /* 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 
     */
    __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)
{

    /* USER CODE BEGIN I2C1_Init 0 */

    /* USER CODE END I2C1_Init 0 */

    /* USER CODE BEGIN I2C1_Init 1 */

    /* USER CODE END I2C1_Init 1 */
    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();
    }
    /* USER CODE BEGIN I2C1_Init 2 */

    /* USER CODE END I2C1_Init 2 */

}

/**
 * @brief USART1 Initialization Function
 * @param None
 * @retval None
 */
static void MX_USART1_UART_Init(void)
{

    /* USER CODE BEGIN USART1_Init 0 */

    /* USER CODE END USART1_Init 0 */

    /* USER CODE BEGIN USART1_Init 1 */

    /* USER CODE END USART1_Init 1 */
    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();
    }
    /* USER CODE BEGIN USART1_Init 2 */

    /* USER CODE END USART1_Init 2 */

}

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

}

/* USER CODE BEGIN 4 */
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_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;
}
/* 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 */
    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 */

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