/** 
**************************************************************************
* @file 	modbus.c
* @brief 	Модуль для реализации MODBUS.
**************************************************************************	
* @details Файл содержит реализацию функций работы с Modbus, включая:
*          - доступ к coils и registers;
*          - обработку команд протокола;
*          - взаимодействие с RS (UART);
*          - инициализацию.
*
* @section Функции и макросы
*
* ### Доступ к coils:
* - MB_Set_Coil_Local()        — Установить coil по локальному адресу.
* - MB_Reset_Coil_Local()      — Сбросить coil по локальному адресу.
* - MB_Toogle_Coil_Local()     — Инвертировать coil по локальному адресу.
* - MB_Read_Coil_Local()       — Прочитать coil по локальному адресу.
* - MB_Write_Coil_Global()     — Установить/сбросить coil по глобальному адресу.
* - MB_Read_Coil_Global()      — Прочитать coil по глобальному адресу.
*
* ### Обработка команд Modbus:
* - MB_DefineRegistersAddress() — Определить начальный адрес регистра.
* - MB_DefineCoilsAddress()     — Определить начальный адрес coils.
* - MB_Check_Address_For_Arr()  — Проверить, принадлежит ли адрес массиву.
* - Основные команды Modbus:
*    - MB_Read_Coils()
*    - MB_Read_Hold_Regs()
*    - MB_Write_Single_Coil()
*    - MB_Write_Miltuple_Coils()
*    - MB_Write_Miltuple_Regs()
*
* ### Функции для работы с RS (UART):
* - RS_Parse_Message() / RS_Collect_Message() — Парсинг и сборка сообщения.
* - RS_Response()                             — Отправка ответа.
* - RS_Define_Size_of_RX_Message()            — Определение размера принимаемого сообщения.
* - RS_Init()                                 — Инициализация UART.
*
* ### Инициализация:
* - MODBUS_FirstInit()                        — Инициализация модуля Modbus.
*
* @section Структура данных Modbus
*
* #### Holding/Input Registers:
* - Регистры — 16-битные слова. Доступ к регистрам осуществляется через указатель.	
* Таким образом, сами регистры могут представлять собой как массив так и структуру.
*
* #### Coils:
* - Coils — это биты, упакованные в 16-битные слова. Доступ к коилам осуществляется через указатель.	
* Таким образом, сами коилы могут представлять собой как массив так и структуру.
*
* @section Инструкция по подключению
* Для корректной работы надо подключить обработчики RS_UART_Handler(), RS_TIM_Handler(),
* в соответствубщие низкоуровневые прерывания UART_IRQHandler, TIM_IRQHandler. После HAL'овского обработчика
* 
* Также необходимо в modbus_config.h настроить дефайны для нужной работы UART
* После для запуска Modbus:
* @verbatim
  //----------------Прием модбас----------------//
  #include "rs_message.h"
#include "../../src/modbus_data.c"
  MODBUS_FirstInit();
  RS_Receive_IT(&hmodbus1, &MODBUS_MSG);
* @endverbatim
*
******************************************************************************/

#include "rs_message.h"
#include "../../src/modbus_data.c"
uint32_t dbg_temp, dbg_temp2, dbg_temp3; // for debug
/* MODBUS HANDLES */
extern UART_HandleTypeDef rs_huart;
extern TIM_HandleTypeDef rs_htim;
RS_HandleTypeDef hmodbus1;

/* DEFINE REGISTERS/COILS */
MB_DeviceIdentificationTypeDef MB_INFO;
MB_DataStructureTypeDef MB_DATA;
RS_MsgTypeDef MODBUS_MSG;

//-------------------------------------------------------------------
//-----------------------------FOR USER------------------------------
/** 
	* @brief 		First set up of MODBUS.
	* @details 	Первый инит модбас. Заполняет структуры и инициализирует таймер и юарт для общения по модбас.
	* @note			This called from main
	*/
void MODBUS_FirstInit(void)
{
  MB_DevoceInentificationInit();
	//-----------SETUP MODBUS-------------
	// set up modbus: MB_RX_Size_NotConst and Timeout enable
	hmodbus1.ID = MODBUS_DEVICE_ID;
  hmodbus1.sRS_Timeout = MODBUS_TIMEOUT;
	hmodbus1.sRS_Mode = SLAVE_ALWAYS_WAIT;
	hmodbus1.sRS_RX_Size_Mode = RS_RX_Size_NotConst;
	
	// INIT
	hmodbus1.RS_STATUS = RS_Init(&hmodbus1, &rs_huart, &rs_htim, 0);	

	RS_EnableReceive();
}
/** 
	* @brief 		Set or Reset Coil at its global address.
	* @param 		Addr 					- адрес коила.
	* @param 		WriteVal 			- Что записать в коил: 0 или 1.
	* @return 	ExceptionCode	- Код исключения если коила по адресу не существует, и NO_ERRORS если все ок.
	*
  * @details 	Позволяет обратиться к любому коилу по его глобальному адрессу.
							Вне зависимости от того как коилы размещены в памяти.
	*/
MB_ExceptionTypeDef MB_Write_Coil_Global(uint16_t Addr, MB_CoilsOpTypeDef WriteVal)
{
	//---------CHECK FOR ERRORS----------
	MB_ExceptionTypeDef Exception = NO_ERRORS;
	uint16_t *coils;
	uint16_t start_shift = 0; // shift in coils register
	
	//------------WRITE COIL-------------
	Exception = MB_DefineCoilsAddress(&coils, Addr, 1, &start_shift, 1);
	if(Exception == NO_ERRORS)
	{
		switch(WriteVal)
		{
			case SET_COIL:
				*coils |= (1<<start_shift);				
				break;
				
			case RESET_COIL:
				*coils &= ~(1<<start_shift);					
				break;
				
			case TOOGLE_COIL:
				*coils ^= (1<<start_shift);						
				break;
			
		}		
	}
	return Exception;
}


/** 
	* @brief 		Read Coil at its global address.
	* @param 		Addr 					- адрес коила.
	* @param 		Exception 		- Указатель на переменную для кода исключения, в случа неудачи при чтении.
	* @return 	uint16_t			- Возвращает весь регистр с маской на запрошенном коиле.
	*
  * @details 	Позволяет обратиться к любому коилу по его глобальному адрессу.
							Вне зависимости от того как коилы размещены в памяти.
	*/
uint16_t MB_Read_Coil_Global(uint16_t Addr, MB_ExceptionTypeDef *Exception)
{
	//---------CHECK FOR ERRORS----------
	MB_ExceptionTypeDef Exception_tmp;
	if(Exception == NULL)							// if exception is not given to func fill it
		Exception = &Exception_tmp;
		
	uint16_t *coils;
	uint16_t start_shift = 0; // shift in coils register
	
	//------------READ COIL--------------
	*Exception = MB_DefineCoilsAddress(&coils, Addr, 1, &start_shift, 0);	
	if(*Exception == NO_ERRORS)
	{
		return ((*coils)&(1<<start_shift));
	}
	else
	{
		return 0;
	}
}

//-------------------------------------------------------------------
//----------------FUNCTIONS FOR PROCESSING MESSAGE-------------------
/** 
	* @brief 		Check is address valid for certain array.
	* @param 		Addr 					- начальный адресс.
	* @param 		Qnt 					- количество запрашиваемых элементов.
	* @param 		R_ARR_ADDR 		- начальный адресс массива R_ARR.
	* @param 		R_ARR_NUMB 		- количество элементов в массиве R_ARR.
	* @return 	ExceptionCode	- ILLEGAL DATA ADRESS если адресс недействителен, и NO_ERRORS если все ок.
	*
	* @details 	Позволяет определить, принадлежит ли адресс Addr массиву R_ARR:
	*						Если адресс Addr находится в диапазоне адрессов массива R_ARR, то возвращаем NO_ERROR.
	*						Если адресс Addr находится за пределами адрессов массива R_ARR - ILLEGAL_DATA_ADDRESSю.
	*/
MB_ExceptionTypeDef MB_Check_Address_For_Arr(uint16_t Addr, uint16_t Qnt, uint16_t R_ARR_ADDR, uint16_t R_ARR_NUMB)
{
	// if address from this array
	if(Addr >= R_ARR_ADDR) 
	{		
		// if quantity too big return error
		if ((Addr - R_ARR_ADDR) + Qnt > R_ARR_NUMB) 
		{
			return ILLEGAL_DATA_ADDRESS;	// return exception code
		}
		// if all ok - return no errors
		return NO_ERRORS;
	}
	// if address isnt from this array return error
	else 
		return ILLEGAL_DATA_ADDRESS;	// return exception code		
}
/** 
	* @brief 		Define Address Origin for Input/Holding Registers
	* @param 		pRegs 				- указатель на указатель регистров.
	* @param 		Addr 					- адрес начального регистра.
	* @param 		Qnt 					- количество запрашиваемых регистров.
	* @param 		WriteFlag 		- флаг регистр нужны для чтения или записи.
	* @return 	ExceptionCode	- Код исключения если есть, и NO_ERRORS если нет.
	*
  * @details 	Определение адреса начального регистра.
	* @note 		WriteFlag пока не используется.
	*/
MB_ExceptionTypeDef MB_DefineRegistersAddress(uint16_t **pRegs, uint16_t Addr, uint16_t Qnt, uint8_t RegisterType)
{	
	/* check quantity error */
	if (Qnt > 125)	
	{		
			return ILLEGAL_DATA_VALUE;	// return exception code
	}
	
	if(RegisterType == RegisterType_Holding)
	{
		// Default holding registers
		if(MB_Check_Address_For_Arr(Addr, Qnt, R_HOLDING_ADDR, R_HOLDING_QNT) == NO_ERRORS)
		{
			*pRegs = MB_Set_Register_Ptr(&MB_DATA.HoldRegs, Addr);	// указатель на выбранный по Addr регистр
		}
		// if address doesnt match any array - return illegal data address response
		else	
		{
			return ILLEGAL_DATA_ADDRESS;
		}
	}
	else if(RegisterType == RegisterType_Input)
	{
		// Default input registers
		if(MB_Check_Address_For_Arr(Addr, Qnt, R_INPUT_ADDR, R_INPUT_QNT) == NO_ERRORS)
		{
			*pRegs = MB_Set_Register_Ptr(&MB_DATA.InRegs, Addr);	// указатель на выбранный по Addr регистр
		}
		// if address doesnt match any array - return illegal data address response
		else	
		{
			return ILLEGAL_DATA_ADDRESS;
		}
	}
	else
	{
			return ILLEGAL_FUNCTION;
	}
	// if found requeried array return no err
	return NO_ERRORS; // return no errors
}
/** 
	* @brief 		Define Address Origin for coils
	* @param 		pCoils 				- указатель на указатель коилов.
	* @param 		Addr 					- адресс начального коила.
	* @param 		Qnt 					- количество запрашиваемых коилов.
	* @param 		start_shift 	- указатель на переменную содержащую сдвиг внутри регистра для начального коила.
	* @param 		WriteFlag 		- флаг коилы нужны для чтения или записи.
	* @return 	ExceptionCode	- Код исключения если есть, и NO_ERRORS если нет.
	*
  * @details	Определение адреса начального регистра запрашиваемых коилов.
	* @note 		WriteFlag используется для определния регистров GPIO: ODR или IDR.
	*/
MB_ExceptionTypeDef MB_DefineCoilsAddress(uint16_t **pCoils, uint16_t Addr, uint16_t Qnt, uint16_t *start_shift, uint8_t WriteFlag)
{	
	/* check quantity error */
	if (Qnt > 2000)	
	{
		return ILLEGAL_DATA_VALUE;	// return exception code
	}
		
	// Default coils
	if(MB_Check_Address_For_Arr(Addr, Qnt, C_CONTROL_ADDR, C_CONTROL_QNT) == NO_ERRORS)
	{
		*pCoils = MB_Set_Coil_Reg_Ptr(&MB_DATA.Coils, Addr);	// указатель на выбранный по Addr массив коилов
	}
	// if address doesnt match any array - return illegal data address response
	else	
	{
		return ILLEGAL_DATA_ADDRESS;
	}
	
	*start_shift = Addr % 16;	// set shift to requested coil
	// if found requeried array return no err
	return NO_ERRORS;	// return no errors
}


/** 
	* @brief 		Proccess command Read Coils (01 - 0x01).
	* @param 		modbus_msg 				- указатель на структуру собщения modbus.
	* @return 	fMessageHandled		- статус о результате обработки комманды.
	* @details 	Обработка команды Read Coils.
	*/
uint8_t MB_Read_Coils(RS_MsgTypeDef *modbus_msg)
{
    uint16_t *bank, shift = 0, response_bytes = modbus_msg->ByteCnt;
    uint8_t status = mb_data_validate(1, modbus_msg->Qnt, modbus_msg->ByteCnt,
                                      sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]));
    if (!status) status = (uint8_t)MB_DefineCoilsAddress(&bank, modbus_msg->Addr, modbus_msg->Qnt, &shift, 0);
    if (!status) status = mb_data_transfer(1, modbus_msg->Qnt, modbus_msg->ByteCnt,
        bank, shift, modbus_msg->DATA, sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]), &response_bytes);
    modbus_msg->Except_Code = (MB_ExceptionTypeDef)status;
    if (status) return 0;
    modbus_msg->ByteCnt = response_bytes;
    return 1;
}

/**
	* @brief 		Proccess command Read Holding Registers (03 - 0x03).
	* @param 		modbus_msg 				- указатель на структуру собщения modbus.
	* @return 	fMessageHandled		- статус о результате обработки комманды.
	* @details 	Обработка команды Read Holding Registers.
	*/
uint8_t MB_Read_Hold_Regs(RS_MsgTypeDef *modbus_msg)
{
    uint16_t *bank, shift = 0, response_bytes = modbus_msg->ByteCnt;
    uint8_t status = mb_data_validate(3, modbus_msg->Qnt, modbus_msg->ByteCnt,
                                      sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]));
    if (!status) status = (uint8_t)MB_DefineRegistersAddress(&bank, modbus_msg->Addr, modbus_msg->Qnt, RegisterType_Holding);
    if (!status) status = mb_data_transfer(3, modbus_msg->Qnt, modbus_msg->ByteCnt,
        bank, shift, modbus_msg->DATA, sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]), &response_bytes);
    modbus_msg->Except_Code = (MB_ExceptionTypeDef)status;
    if (status) return 0;
    modbus_msg->ByteCnt = response_bytes;
    return 1;
}

/**
	* @brief 		Proccess command Read Input Registers (04 - 0x04).
	* @param 		modbus_msg 				- указатель на структуру собщения modbus.
	* @return 	fMessageHandled		- статус о результате обработки комманды.
	* @details 	Обработка команды Read Input Registers.
	*/
uint8_t MB_Read_Input_Regs(RS_MsgTypeDef *modbus_msg)
{
    uint16_t *bank, shift = 0, response_bytes = modbus_msg->ByteCnt;
    uint8_t status = mb_data_validate(4, modbus_msg->Qnt, modbus_msg->ByteCnt,
                                      sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]));
    if (!status) status = (uint8_t)MB_DefineRegistersAddress(&bank, modbus_msg->Addr, modbus_msg->Qnt, RegisterType_Input);
    if (!status) status = mb_data_transfer(4, modbus_msg->Qnt, modbus_msg->ByteCnt,
        bank, shift, modbus_msg->DATA, sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]), &response_bytes);
    modbus_msg->Except_Code = (MB_ExceptionTypeDef)status;
    if (status) return 0;
    modbus_msg->ByteCnt = response_bytes;
    return 1;
}
/**
	* @brief 		Proccess command Write Single Coils (05 - 0x05).
	* @param 		modbus_msg 				- указатель на структуру собщения modbus.
	* @return 	fMessageHandled		- статус о результате обработки комманды.
	* @details 	Обработка команды Write Single Coils.
	*/
uint8_t MB_Write_Single_Coil(RS_MsgTypeDef *modbus_msg)
{
    uint16_t *bank, shift = 0, response_bytes = modbus_msg->ByteCnt;
    uint8_t status = mb_data_validate(5, modbus_msg->Qnt, modbus_msg->ByteCnt,
                                      sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]));
    if (!status) status = (uint8_t)MB_DefineCoilsAddress(&bank, modbus_msg->Addr, 1, &shift, 1);
    if (!status) status = mb_data_transfer(5, modbus_msg->Qnt, modbus_msg->ByteCnt,
        bank, shift, modbus_msg->DATA, sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]), &response_bytes);
    modbus_msg->Except_Code = (MB_ExceptionTypeDef)status;
    if (status) return 0;
    modbus_msg->ByteCnt = response_bytes;
    return 1;
}

/**
	* @brief 		Proccess command Write Single Register (06 - 0x06).
	* @param 		modbus_msg 				- указатель на структуру собщения modbus.
	* @return 	fMessageHandled		- статус о результате обработки комманды.
	* @details 	Обработка команды Write Single Register.
	*/
uint8_t MB_Write_Single_Reg(RS_MsgTypeDef *modbus_msg)
{
    uint16_t *bank, shift = 0, response_bytes = modbus_msg->ByteCnt;
    uint8_t status = mb_data_validate(6, modbus_msg->Qnt, modbus_msg->ByteCnt,
                                      sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]));
    if (!status) status = (uint8_t)MB_DefineRegistersAddress(&bank, modbus_msg->Addr, 1, RegisterType_Holding);
    if (!status) status = mb_data_transfer(6, modbus_msg->Qnt, modbus_msg->ByteCnt,
        bank, shift, modbus_msg->DATA, sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]), &response_bytes);
    modbus_msg->Except_Code = (MB_ExceptionTypeDef)status;
    if (status) return 0;
    modbus_msg->ByteCnt = response_bytes;
    return 1;
}

/**
	* @brief 		Proccess command Write Multiple Coils (15 - 0x0F).
	* @param 		modbus_msg 			- указатель на структуру собщения modbus.
	* @return 	fMessageHandled	- статус о результате обработки комманды.
	* @details 	Обработка команды Write Multiple Coils.
	*/
uint8_t MB_Write_Miltuple_Coils(RS_MsgTypeDef *modbus_msg)
{
    uint16_t *bank, shift = 0, response_bytes = modbus_msg->ByteCnt;
    uint8_t status = mb_data_validate(15, modbus_msg->Qnt, modbus_msg->ByteCnt,
                                      sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]));
    if (!status) status = (uint8_t)MB_DefineCoilsAddress(&bank, modbus_msg->Addr, modbus_msg->Qnt, &shift, 1);
    if (!status) status = mb_data_transfer(15, modbus_msg->Qnt, modbus_msg->ByteCnt,
        bank, shift, modbus_msg->DATA, sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]), &response_bytes);
    modbus_msg->Except_Code = (MB_ExceptionTypeDef)status;
    if (status) return 0;
    modbus_msg->ByteCnt = response_bytes;
    return 1;
}

/**
	* @brief 		Proccess command Write Multiple Registers (16 - 0x10).
	* @param 		modbus_msg 				- указатель на структуру собщения modbus.
	* @return 	fMessageHandled		- статус о результате обработки комманды.
	* @details 	Обработка команды Write Multiple Registers.
	*/
uint8_t MB_Write_Miltuple_Regs(RS_MsgTypeDef *modbus_msg)
{
    uint16_t *bank, shift = 0, response_bytes = modbus_msg->ByteCnt;
    uint8_t status = mb_data_validate(16, modbus_msg->Qnt, modbus_msg->ByteCnt,
                                      sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]));
    if (!status) status = (uint8_t)MB_DefineRegistersAddress(&bank, modbus_msg->Addr, modbus_msg->Qnt, RegisterType_Holding);
    if (!status) status = mb_data_transfer(16, modbus_msg->Qnt, modbus_msg->ByteCnt,
        bank, shift, modbus_msg->DATA, sizeof(modbus_msg->DATA) / sizeof(modbus_msg->DATA[0]), &response_bytes);
    modbus_msg->Except_Code = (MB_ExceptionTypeDef)status;
    if (status) return 0;
    modbus_msg->ByteCnt = response_bytes;
    return 1;
}

void MB_WriteObjectToMessage(char *mbdata, unsigned *ind, MB_DeviceObjectTypeDef *obj)
{
  mbdata[(*ind)++] = obj->length;
  for (int i = 0; i < obj->length; i++)
  {
    mbdata[(*ind)++] = obj->name[i];
  }
}
/**
	* @brief 		Proccess command Read Device Identification (43/14 - 0x2B/0E).
	* @param 		modbus_msg 				- указатель на структуру собщения modbus.
	* @return 	fMessageHandled		- статус о результате обработки комманды.
	* @details 	Обработка команды Write Single Register.
	*/
uint8_t MB_Read_Device_Identification(RS_MsgTypeDef *modbus_msg)
{
  char *mbdata = (char *)modbus_msg->DATA;
  unsigned ind = 0;
  switch(modbus_msg->DevId.ReadDevId)
  { 
    case MB_BASIC_IDENTIFICATION:
      mbdata[ind++] = 0x00;
      MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.VendorName);
      mbdata[ind++] = 0x01;
      MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.ProductCode);
      mbdata[ind++] = 0x02;
      MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.Revision);
      modbus_msg->DevId.NumbOfObj = 3;
      break;
    case MB_REGULAR_IDENTIFICATION:
      mbdata[ind++] = 0x03;
      MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.VendorUrl);
      mbdata[ind++] = 0x04;
      MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.ProductName);
      mbdata[ind++] = 0x05;
      MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.ModelName);
      mbdata[ind++] = 0x06;
      MB_WriteObjectToMessage(mbdata, &ind, &MB_INFO.UserApplicationName);
      modbus_msg->DevId.NumbOfObj = 4;
      break;
    default:
      return 0;
  }  
  
  modbus_msg->ByteCnt = ind;
  return 1;
}


/** 
	* @brief 		Respond accord to received message.
	* @param 		hRS 		- указатель на хендлер RS.
	* @param 		RS_msg 	- указатель на структуру сообщения.
	* @return 	RS_RES	- статус о результате ответа на комманду.
	* @details		Обработка принятой комманды и ответ на неё.
	*/
RS_StatusTypeDef RS_Response(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg)
{
	RS_StatusTypeDef MB_RES = 0;
	hmodbus->f.MessageHandled = 0;
	hmodbus->f.EchoResponse = 0;
	RS_Reset_TX_Flags(hmodbus);			// reset flag for correct transmit
	
	if(modbus_msg->Func_Code < ERR_VALUES_START)// if no errors after parsing
	{
		switch (modbus_msg->Func_Code)
		{
			// Read Coils
			case MB_R_COILS: 
				hmodbus->f.MessageHandled = 	MB_Read_Coils(hmodbus->pMessagePtr);
				break;
			
			// Read Hodling Registers
			case MB_R_HOLD_REGS:
				hmodbus->f.MessageHandled = 	MB_Read_Hold_Regs(hmodbus->pMessagePtr);
				break;
			case MB_R_IN_REGS: 
				hmodbus->f.MessageHandled = 	MB_Read_Input_Regs(hmodbus->pMessagePtr);
				break;
			
			
			// Write Single Coils
			case MB_W_COIL: 
				hmodbus->f.MessageHandled =	MB_Write_Single_Coil(hmodbus->pMessagePtr);	
				if(hmodbus->f.MessageHandled) 
				{
					hmodbus->f.EchoResponse = 1; 
					hmodbus->RS_Message_Size -= 2; // echo response if write ok (minus 2 cause of two CRC bytes)
				}
				break;
			
			case MB_W_HOLD_REG: 	
				hmodbus->f.MessageHandled =	MB_Write_Single_Reg(hmodbus->pMessagePtr);	
				if(hmodbus->f.MessageHandled) 
				{
					hmodbus->f.EchoResponse = 1;
					hmodbus->RS_Message_Size -= 2; // echo response if write ok (minus 2 cause of two CRC bytes)
				}
				break;
			
			// Write Multiple Coils
			case MB_W_COILS: 
				hmodbus->f.MessageHandled = 	MB_Write_Miltuple_Coils(hmodbus->pMessagePtr);
				if(hmodbus->f.MessageHandled) 
				{
					hmodbus->f.EchoResponse = 1; 
					hmodbus->RS_Message_Size = 6; // echo response if write ok (withous data bytes)
				}
				break;
			
			// Write Multiple Registers
			case MB_W_HOLD_REGS:
				hmodbus->f.MessageHandled = 	MB_Write_Miltuple_Regs(hmodbus->pMessagePtr);
				if(hmodbus->f.MessageHandled) 
				{
					hmodbus->f.EchoResponse = 1; 
					hmodbus->RS_Message_Size = 6; // echo response if write ok (withous data bytes)
				}
				break;
			
      case MB_R_DEVICE_INFO:
        hmodbus->f.MessageHandled =   MB_Read_Device_Identification(hmodbus->pMessagePtr);
        break;
        
       
			/* unknown func code */
			default: modbus_msg->Except_Code = 0x01; /* set exception code: illegal function */
		}		
				
		if(hmodbus->f.MessageHandled == 0)
    {
			modbus_msg->Func_Code += ERR_VALUES_START;
    }
    else
    {
    }
		
			
	}
	
	// if we need response - check that transmit isnt busy
	if( RS_Is_TX_Busy(hmodbus) ) 
		RS_Abort(hmodbus, ABORT_TX);		// if tx busy - set it free
	
	// Transmit right there, or sets (fDeferredResponse) to transmit response in main code	
	MB_RES = RS_Handle_Transmit_Start(hmodbus, modbus_msg);
	
	hmodbus->RS_STATUS = MB_RES;
	return MB_RES;
}

/** 
	* @brief 		Collect message in buffer to transmit it.
	* @param 		hRS 					- указатель на хендлер RS.
	* @param 		RS_msg 				- указатель на структуру сообщения.
	* @param 		msg_uart_buff	- указатель на буффер UART.
	* @return 	RS_RES				- статус о результате заполнения буфера.
	* @details		Заполнение буффера UART из структуры сообщения.
	*/
RS_StatusTypeDef RS_Collect_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
{
	int ind = 0; // ind for modbus-uart buffer
	
	if(hmodbus->f.EchoResponse && hmodbus->f.MessageHandled) 	// if echo response need
		ind = hmodbus->RS_Message_Size;
	else
	{
		//------INFO ABOUT DATA/MESSAGE------
		//-----------[first bytes]-----------
		// set ID of message/user
		modbus_uart_buff[ind++] = modbus_msg->MbAddr;	

		// set dat or err response
		modbus_uart_buff[ind++] = modbus_msg->Func_Code;	

		if (modbus_msg->Func_Code < ERR_VALUES_START) // if no error occur
		{
      // fill modbus header
      if(modbus_msg->Func_Code == MB_R_DEVICE_INFO) // devide identification header
      {
        modbus_uart_buff[ind++] = modbus_msg->DevId.MEI_Type;
        modbus_uart_buff[ind++] = modbus_msg->DevId.ReadDevId;
        modbus_uart_buff[ind++] = modbus_msg->DevId.Conformity;
        modbus_uart_buff[ind++] = modbus_msg->DevId.MoreFollows;
        modbus_uart_buff[ind++] = modbus_msg->DevId.NextObjId;
        modbus_uart_buff[ind++] = modbus_msg->DevId.NumbOfObj;
        
        if (modbus_msg->ByteCnt > DATA_SIZE*2) // if ByteCnt less than DATA_SIZE
        {
          return RS_COLLECT_MSG_ERR;
        }
        
        
        //---------------DATA----------------
        //-----------[data bytes]------------
        uint8_t *tmp_data_addr = (uint8_t *)modbus_msg->DATA;
        for(int i = 0; i < modbus_msg->ByteCnt; i++) // filling buffer with data
        {	// set data
          modbus_uart_buff[ind++] = *tmp_data_addr;
          tmp_data_addr++;
        }
        
      }
      else // modbus data header
      {
        // set size of received data
        if (modbus_msg->ByteCnt <= DATA_SIZE*2) // if ByteCnt less than DATA_SIZE
          modbus_uart_buff[ind++] = modbus_msg->ByteCnt;
        else																				// otherwise return data_size err
        {
          return RS_COLLECT_MSG_ERR;
        }        
        
        //---------------DATA----------------
        //-----------[data bytes]------------
        uint16_t *tmp_data_addr = (uint16_t *)modbus_msg->DATA;
        for(int i = 0; i < modbus_msg->ByteCnt; i++) // filling buffer with data
        {	// set data
          if (i%2 == 0)		// HI byte
            modbus_uart_buff[ind++] = (*tmp_data_addr)>>8;
          else						// LO byte
          {
            modbus_uart_buff[ind++] = *tmp_data_addr;
            tmp_data_addr++;
          }
        }

      }
      
		}
		else 	// if some error occur
		{			// send expection code
			modbus_uart_buff[ind++] = modbus_msg->Except_Code;
		}
	}
	//---------------CRC----------------
	//---------[last 16 bytes]----------
	// calc crc of received data
	uint16_t CRC_VALUE = crc16(modbus_uart_buff, ind);
	// write crc to message structure and modbus-uart buffer
	modbus_msg->MB_CRC = CRC_VALUE;
	modbus_uart_buff[ind++] = CRC_VALUE;
	modbus_uart_buff[ind++] = CRC_VALUE >> 8;
	
	hmodbus->RS_Message_Size = ind;
	
	return RS_OK; // returns ok
}

/** 
	* @brief 		Parse message from buffer to process it.
	* @param 		hRS 					- указатель на хендлер RS.
	* @param 		RS_msg 				- указатель на структуру сообщения.
	* @param 		msg_uart_buff	- указатель на буффер UART.
	* @return 	RS_RES				- статус о результате заполнения структуры.
	* @details		Заполнение структуры сообщения из буффера UART.
	*/
RS_StatusTypeDef RS_Parse_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
{
	uint32_t check_empty_buff;
	int ind = 0; // ind for modbus-uart buffer
	//-----INFO ABOUT DATA/MESSAGE-------
	//-----------[first bits]------------
	// get ID of message/user
	modbus_msg->MbAddr = modbus_uart_buff[ind++];
	if(modbus_msg->MbAddr != hmodbus->ID)
		return RS_SKIP;
	
	// get func code
	modbus_msg->Func_Code = modbus_uart_buff[ind++];
	if(modbus_msg->Func_Code == MB_R_DEVICE_INFO) // if it device identification request
  {
    modbus_msg->DevId.MEI_Type = modbus_uart_buff[ind++];
    modbus_msg->DevId.ReadDevId = modbus_uart_buff[ind++];
    modbus_msg->DevId.NextObjId = modbus_uart_buff[ind++];
    modbus_msg->ByteCnt = 0;
  }
  else // if its classic modbus request
  {
	// get address from CMD
	modbus_msg->Addr = modbus_uart_buff[ind++] << 8;
	modbus_msg->Addr |= modbus_uart_buff[ind++];
	
	// get address from CMD
	modbus_msg->Qnt = modbus_uart_buff[ind++] << 8;
	modbus_msg->Qnt |= modbus_uart_buff[ind++];
	}
	if(hmodbus->f.RX_Half == 0) // if all message received
	{
		//---------------DATA----------------
		// 					  (optional)
		if (modbus_msg->ByteCnt != 0)
		{
			ind++; // increment ind for data_size byte
			//check that data size is correct
			if (modbus_msg->ByteCnt > DATA_SIZE*2)
			{
				modbus_msg->Func_Code += ERR_VALUES_START;
				return RS_PARSE_MSG_ERR;
			}
			uint16_t *tmp_data_addr = (uint16_t *)modbus_msg->DATA;
			for(int i = 0; i < modbus_msg->ByteCnt; i++) // /2 because we transmit 8 bits, not 16 bits
			{	// set data
				if (i%2 == 0)
					*tmp_data_addr = ((uint16_t)modbus_uart_buff[ind++] << 8);
				else
				{
					*tmp_data_addr |= modbus_uart_buff[ind++];
					tmp_data_addr++;
				}
			}
		}
		
		//---------------CRC----------------
		//----------[last 16 bits]----------
		// calc crc of received data
		uint16_t CRC_VALUE = crc16(modbus_uart_buff, ind);
		// get crc of received data
		modbus_msg->MB_CRC = modbus_uart_buff[ind++];
		modbus_msg->MB_CRC |= modbus_uart_buff[ind++] << 8;
		// compare crc
		if (modbus_msg->MB_CRC != CRC_VALUE)
    {
			modbus_msg->Func_Code += ERR_VALUES_START;
    }
	//		hmodbus->MB_RESPONSE = MB_CRC_ERR; // set func code - error about wrong crc
		
		// check is buffer empty
		check_empty_buff = 0;
		for(int i=0; i<ind;i++)
			check_empty_buff += modbus_uart_buff[i];
	//	if(check_empty_buff == 0) 
	//		hmodbus->MB_RESPONSE = MB_EMPTY_MSG; // 
	}
	
	return RS_OK;
	
}

/** 
	* @brief 		Define size of RX Message that need to be received.
	* @param 		hRS 					- указатель на хендлер RS.
	* @param 		rx_data_size 	- указатель на переменную для записи кол-ва байт для принятия.
	* @return 	RS_RES				- статус о корректности рассчета кол-ва байт для принятия.
	* @details	Определение сколько байтов надо принять по протоколу.
	*/
RS_StatusTypeDef RS_Define_Size_of_RX_Message(RS_HandleTypeDef *hmodbus, uint32_t *rx_data_size)
{		
	RS_StatusTypeDef MB_RES = 0;
	
	MB_RES = RS_Parse_Message(hmodbus, hmodbus->pMessagePtr, hmodbus->pBufferPtr);
	if(MB_RES == RS_SKIP) // if message not for us
		return MB_RES;																// return 
	
  
	if ((hmodbus->pMessagePtr->Func_Code & ~ERR_VALUES_START) < 0x0F)
	{
		hmodbus->pMessagePtr->ByteCnt = 0;
		*rx_data_size = 1;
    
	}
	else
	{
		hmodbus->pMessagePtr->ByteCnt = hmodbus->pBufferPtr[RX_FIRST_PART_SIZE-1]; // get numb of data in command
	// +1 because that defines is size, not ind.
		*rx_data_size = hmodbus->pMessagePtr->ByteCnt + 2;
	}
  
  
  if(hmodbus->pMessagePtr->Func_Code == MB_R_DEVICE_INFO)
  {
		*rx_data_size = 0;
  }
  
	hmodbus->RS_Message_Size = RX_FIRST_PART_SIZE + *rx_data_size; // size of whole message
	return RS_OK;
}

//-----------------------------FOR USER------------------------------
//-------------------------------------------------------------------


void MB_DevoceInentificationInit(void)
{
  MB_INFO.VendorName.name = MODBUS_VENDOR_NAME;
  MB_INFO.ProductCode.name = MODBUS_PRODUCT_CODE;
  MB_INFO.Revision.name = MODBUS_REVISION;
  MB_INFO.VendorUrl.name = MODBUS_VENDOR_URL;
  MB_INFO.ProductName.name = MODBUS_PRODUCT_NAME;
  MB_INFO.ModelName.name = MODBUS_MODEL_NAME;
  MB_INFO.UserApplicationName.name = MODBUS_USER_APPLICATION_NAME;
  
  
  MB_INFO.VendorName.length = sizeof(MODBUS_VENDOR_NAME);
  MB_INFO.ProductCode.length = sizeof(MODBUS_PRODUCT_CODE);
  MB_INFO.Revision.length = sizeof(MODBUS_REVISION);
  MB_INFO.VendorUrl.length = sizeof(MODBUS_VENDOR_URL);
  MB_INFO.ProductName.length = sizeof(MODBUS_PRODUCT_NAME);
  MB_INFO.ModelName.length = sizeof(MODBUS_MODEL_NAME);
  MB_INFO.UserApplicationName.length = sizeof(MODBUS_USER_APPLICATION_NAME);
}