/** 
*******************************************************************************
* @file modbus_master.c
* @brief Модуль для реализации мастера MODBUS.
*******************************************************************************
* @details 
Файл содержит реализацию функций для работы Modbus в режиме мастера.

@section mast Функции и макросы
- MB_RespGet_RegisterAll()      — Считать все регистра из ответа
- MB_RespGet_RegisterValue()    — Считать один регистр из ответа
- MB_RespGet_CoilAll()          — Считать все коилы из ответа
- MB_RespGet_CoilState()        — Считать один коил из ответа
- MB_RespGet_NumberOfObjects()  — Считать количество принятых объектов идентификатора
- MB_RespGet_ObjectById()       — Считать объект идентификатора по 
                                  его ID
- MB_RespGet_ObjectByIndex()    — Считать объект идентификатора по 
                                  порядковому номеру в сообщении
- MB_RespGet_Diagnostic()       — Считать запрошенный диагностический счетчик

- MB_Master_Collect_Message() — Сбор сообщения в режиме мастера
- MB_Master_Parse_Message()   — Парс сообщения в режиме мастера
******************************************************************************/
#include "modbus.h"

#ifdef MODBUS_ENABLE_MASTER


//-------------------------------------------------------------------
//-----------------------------FOR USER------------------------------
/**
  * @brief Получить значение ВСЕХ регистров в ответе
  * @param modbus_msg Указатель на структуру сообщения
  * @param reg_addr Адрес регистра, значение которого нужно получить
  * @param reg_arr  Указатель для массив для сохранения значений регистров
  * @return количество считанных регистров, 0 - ошибка
  */
int MB_RespGet_RegisterAll(RS_MsgTypeDef *modbus_msg, uint16_t *reg_arr)
{
  if(modbus_msg == NULL || reg_arr == NULL)
    return 0;
  int read_cnt = 0;
  int i = 0;
  for(int addr = modbus_msg->Addr; addr < modbus_msg->Addr + modbus_msg->Qnt; addr++)
  {
    if(MB_RespGet_RegisterValue(modbus_msg, addr, &reg_arr[i]))
    {
      read_cnt++;
    }
    i++;
  }
  
  return read_cnt;
}

/**
  * @brief Получить значение регистра в ответе по его адресу
  * @param modbus_msg Указатель на структуру сообщения
  * @param reg_addr Адрес регистра, значение которого нужно получить
  * @param reg_value Указатель для значения регистра
  * @return 1 - успех, 0 - ошибка или reg_addr вне диапазона запроса
  */
int MB_RespGet_RegisterValue(RS_MsgTypeDef *modbus_msg, uint16_t reg_addr, uint16_t *reg_value)
{
  if(modbus_msg == NULL || reg_value == NULL)
    return 0;
  
  // Проверяем что ответ связан с регистрами
  if((modbus_msg->FuncCode != FC_R_DISC_IN) &&
     (modbus_msg->FuncCode != FC_R_HOLD_REGS) &&
     (modbus_msg->FuncCode != FC_R_IN_REGS))
  {
    return 0;
  }
  
  // Проверяем что reg_addr в пределах запрошенного диапазона
  if(reg_addr < modbus_msg->Addr || reg_addr >= modbus_msg->Addr + modbus_msg->Qnt)
    return 0;
  
  // Вычисляем индекс регистра в полученных данных
  uint16_t reg_index = reg_addr - modbus_msg->Addr;
  
  // Проверяем что регистр существует в данных
  if(reg_index >= modbus_msg->ByteCnt / 2)
    return 0;
  
  // Получаем значение регистра
  *reg_value = modbus_msg->MbData[reg_index];
  
  return 1;
}



/**
  * @brief Получить состояние ВСЕХ coil в ответе
  * @param modbus_msg Указатель на структуру сообщения
  * @param coil_arr   Указатель для массив доя сохранения состояний coil (1 - ON, 0 - OFF)
  * @return 1 - успех, 0 - ошибка или coil_addr вне диапазона запроса
  */
int MB_RespGet_CoilAll(RS_MsgTypeDef *modbus_msg, int *coil_arr)
{
  if(modbus_msg == NULL || coil_arr == NULL)
    return 0;

  int read_cnt = 0;
  int i = 0;
  for(int addr = modbus_msg->Addr; addr < modbus_msg->Addr + modbus_msg->Qnt; addr++)
  {
    if(MB_RespGet_CoilState(modbus_msg, addr, &coil_arr[i]))
    {
      read_cnt++;
    }
    i++;
  }
  
  return 1;
}

/**
  * @brief Получить состояние coil в ответе по его адресу
  * @param modbus_msg Указатель на структуру сообщения
  * @param coil_addr Адрес coil, состояние которого нужно получить
  * @param coil_state Указатель для состояния coil (1 - ON, 0 - OFF)
  * @return 1 - успех, 0 - ошибка или coil_addr вне диапазона запроса
  */
int MB_RespGet_CoilState(RS_MsgTypeDef *modbus_msg, uint16_t coil_addr, int *coil_state)
{
  if(modbus_msg == NULL || coil_state == NULL)
    return 0;
  
  // Проверяем что ответ связан с коилами
  if(modbus_msg->FuncCode != FC_R_COILS)
  {
    return 0;
  }
  
  // Проверяем что coil_addr в пределах запрошенного диапазона
  if(coil_addr < modbus_msg->Addr || coil_addr >= modbus_msg->Addr + modbus_msg->Qnt)
    return 0;
  
  // Вычисляем индекс coil в полученных данных
  uint16_t coil_index = coil_addr - modbus_msg->Addr;
  
  // Вычисляем байт и бит
  uint8_t byte_index = coil_index / 8;
  uint8_t data_index = coil_index / 16;
  uint8_t bit_index = coil_index % 16;
  
  // Проверяем что байт существует в данных
  if(byte_index >= modbus_msg->ByteCnt)
    return 0;
  
  // Получаем байт и проверяем бит  
  if(bit_index < 8)
    *coil_state = (modbus_msg->MbData[data_index] >> (bit_index+8)) & 0x01;
  else
    *coil_state = ((modbus_msg->MbData[data_index]&0xFF) >> (bit_index-8)) & 0x01;
    
  
  return 1;
}


/**
  * @brief    Получить количество объектов в сообщении
  * @param    modbus_msg  Указатель на структуру сообщения
  * @return   int         Количество объектов
  */
int MB_RespGet_NumberOfObjects(RS_MsgTypeDef *modbus_msg)
{
  if(modbus_msg == NULL)
  {
    return 0;
  }
  // Проверяем что ответ связан с диагностикой
  if(modbus_msg->FuncCode != FC_R_DEVICE_ID)
  {
    return 0;
  }
  
  return modbus_msg->DevId.NumbOfObj;
}

/**
  * @brief    Найти объект по ID в сообщении
  * @param    modbus_msg  Указатель на структуру сообщения
  * @param    obj_id      ID искомого объекта
  * @param    obj_data    Буфер для данных объекта (может быть NULL)
  * @param    obj_length  Указатель для длины объекта
  * @return   int         Найден ли объект (1 - да, 0 - нет)
  */
int MB_RespGet_ObjectById(RS_MsgTypeDef *modbus_msg, uint8_t obj_id, char *obj_data, uint8_t *obj_length)
{
  if((modbus_msg == NULL) || (obj_data == NULL))
    return 0;
  
  // Проверяем что ответ связан с диагностикой
  if(modbus_msg->FuncCode != FC_R_DEVICE_ID)
  {
    return 0;
  }
  
  uint8_t *data = (uint8_t*)modbus_msg->MbData;
  unsigned ind = 0;
  
  for(int i = 0; i < modbus_msg->DevId.NumbOfObj; i++)
  {
    uint8_t current_id = data[ind++];
    uint8_t current_length = data[ind++];
    
    if(current_id == obj_id)
    {
      if(obj_length) 
        *obj_length = current_length;

      for(int j = 0; j < current_length; j++)
      {
        obj_data[j] = data[ind++];
      }
      obj_data[current_length] = '\0'; // добавляем \0

      return 1;
    }
    else
    {
      // Пропускаем данные этого объекта
      ind += current_length;
    }
  }
  
  return 0;
}

/**
  * @brief    Получить объект по индексу в сообщении
  * @param    modbus_msg  Указатель на структуру сообщения
  * @param    index       Индекс объекта (0..N-1)
  * @param    obj_id      Указатель для ID объекта
  * @param    obj_data    Буфер для данных объекта
  * @param    obj_length  Указатель для длины объекта
  * @return   int         Успешность получения (1 - получен, 0 - не найден)
  */
int MB_RespGet_ObjectByIndex(RS_MsgTypeDef *modbus_msg, int index, uint8_t *obj_id, char *obj_data, uint8_t *obj_length)
{
  if((modbus_msg == NULL) || (obj_data == NULL))
    return 0;
  
  // Проверяем что ответ связан с диагностикой
  if(modbus_msg->FuncCode != FC_R_DEVICE_ID)
  {
    return 0;
  }
  
  if(index >= modbus_msg->DevId.NumbOfObj) 
    return 0;
  
  uint8_t *data = (uint8_t*)modbus_msg->MbData;
  unsigned ind = 0;
  
  for(int i = 0; i <= index; i++)
  {
    uint8_t current_id = data[ind++];
    uint8_t current_length = data[ind++];
    
    if(obj_id)
      *obj_id = current_id;
    if(obj_length)
      *obj_length = current_length;
    
    if(i == index)
    {
      for(int j = 0; j < current_length; j++)
      {
        obj_data[j] = data[ind++];
      }
      obj_data[current_length] = '\0'; // добавляем \0
      return 1;
    }
    else
    {
      // Пропускаем данные этого объекта
      ind += current_length;
    }
  }
  
  return 0;
}


/**
  * @brief Получить данные диагностики из сообщения (MbData[1])
  * @param modbus_msg Указатель на структуру сообщения
  * @param data Указатель куда положить данные
  * @return 1 - успех, 0 - ошибка
  */
int MB_RespGet_Diagnostic(RS_MsgTypeDef *modbus_msg, uint16_t *data)
{
  if(modbus_msg == NULL || data == NULL)
    return 0;
  
  // Проверяем что ответ связан с диагностикой
  if(modbus_msg->FuncCode != FC_R_DIAGNOSTICS)
  {
    return 0;
  }
  
  
  
  *data = modbus_msg->MbData[1];
  return 1;
}




//-------------------------------------------------------------------
//-----------------------------INTERNAL------------------------------
/** 
  * @brief    Определить размер модбас запроса (МАСТЕР версия).
  * @param    hRS           Указатель на хендлер RS.
  * @param    rx_data_size  Указатель на переменную для записи кол-ва байт для принятия.
  * @return   RS_RES        Статус о корректности рассчета кол-ва байт для принятия.
  * @details  Определение сколько байтов надо принять по протоколу.
  */
static int MB_Define_Size_of_Function(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg)
{   
  RS_StatusTypeDef MB_RES = 0;
  int mb_func_size = 0;
  

  // Master mode - calculating response size from slave
  if (modbus_msg->FuncCode & FC_ERR_VALUES_START) 
  {
    // Error response: [Addr][Func|0x80][ExceptCode][CRC]
    mb_func_size = -1; // Only Exception Code
  }
  else if (modbus_msg->FuncCode == FC_R_DIAGNOSTICS)
  {
    // Diagnostics response: [SubFunc_HI][SubFunc_LO][Data_HI][Data_LO]
    mb_func_size = 1;
  }
  else if (modbus_msg->FuncCode == FC_R_DEVICE_ID)
  {
    // Device identifications: variable size, need to read first to determine
    mb_func_size = 0; // Will be determined after reading header
  }
  else 
  {
    switch (modbus_msg->FuncCode & ~FC_ERR_VALUES_START)
    {
      case 0x01: // Read Coils
      case 0x02: // Read Discrete Inputs
      case 0x03: // Read Holding Registers  
      case 0x04: // Read Input Registers
        // Response: [ByteCount][Data...]
        mb_func_size = modbus_msg->ByteCnt + 2; // ByteCount + variable data
        break;
        
      case 0x05: // Write Single Coil
      case 0x06: // Write Single Register
        // Echo response: [Addr][Value][CRC]
        mb_func_size = 4; // Address(2) + Value(2)
        break;
          
      case 0x0F: // Write Multiple Coils
      case 0x10: // Write Multiple Registers
        // Echo response: [Addr][Qty][CRC]  
        mb_func_size = 4; // Address(2) + Quantity(2)
        break;
          
      default:
        mb_func_size = 0;
    }
  }
  
  mb_func_size = RS_RX_FIRST_PART_SIZE + mb_func_size; // size of whole message
  return mb_func_size;
}

/** 
  * @brief    Сбор сообщения в буфер UART в режиме мастер (фрейм мастера из msg -> uart).
  * @param    hmodbus           Указатель на хендлер RS.
  * @param    modbus_msg        Указатель на структуру сообщения.
  * @param    modbus_uart_buff  Указатель на буффер UART.
  * @return   RS_RES        Статус о результате заполнения буфера.
  */
RS_StatusTypeDef MB_Master_Collect_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
{
  int ind = 0; // ind for modbus-uart buffer
  
  //------INFO ABOUT DATA/MESSAGE------
  //-----------[first bytes]-----------
  // set ID of slave device
  modbus_uart_buff[ind++] = modbus_msg->DeviceAddr; 
  
  // set function code
  modbus_uart_buff[ind++] = modbus_msg->FuncCode;  
  
  if(modbus_msg->FuncCode < FC_ERR_VALUES_START) // if no error occur
  {
    // fill modbus header
    if(0) {}
#ifdef MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
    else if(modbus_msg->FuncCode == FC_R_DEVICE_ID) // device identifications request
    {
      modbus_uart_buff[ind++] = modbus_msg->DevId.MEI_Type;
      modbus_uart_buff[ind++] = modbus_msg->DevId.ReadDevId;
      modbus_uart_buff[ind++] = modbus_msg->DevId.NextObjId;
    }
#endif //MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
#ifdef MODBUS_ENABLE_DIAGNOSTICS
    else if(modbus_msg->FuncCode == FC_R_DIAGNOSTICS)
    {
      // Diagnostics: [SubFunc_HI][SubFunc_LO][Data_HI][Data_LO]
      modbus_uart_buff[ind++] = modbus_msg->MbData[0] >> 8;   // Sub-function HI
      modbus_uart_buff[ind++] = modbus_msg->MbData[0] & 0xFF; // Sub-function LO
      modbus_uart_buff[ind++] = modbus_msg->MbData[1] >> 8;   // Data HI
      modbus_uart_buff[ind++] = modbus_msg->MbData[1] & 0xFF; // Data LO
    }
#endif //MODBUS_ENABLE_DIAGNOSTICS
    else // classic modbus request
    {
      // set address
      modbus_uart_buff[ind++] = modbus_msg->Addr >> 8;
      modbus_uart_buff[ind++] = modbus_msg->Addr & 0xFF;
      
      // set quantity
      modbus_uart_buff[ind++] = modbus_msg->Qnt >> 8;
      modbus_uart_buff[ind++] = modbus_msg->Qnt & 0xFF;
      
      // for write multiple functions
      if((modbus_msg->FuncCode == 0x0F) || (modbus_msg->FuncCode == 0x10))
      {
        modbus_uart_buff[ind++] = modbus_msg->ByteCnt;
        
        // write data bytes
        uint8_t *tmp_data_addr = (uint8_t *)modbus_msg->MbData;
        for(int i = 0; i < modbus_msg->ByteCnt; i++)
        {
          modbus_uart_buff[ind++] = tmp_data_addr[i];
        }
      }
    }
  }
  
  if(ind < 0)
    return RS_COLLECT_MSG_ERR;
  
  //---------------CRC----------------
  //---------[last 2 bytes]----------
  uint16_t CRC_VALUE = crc16(modbus_uart_buff, ind);
  modbus_msg->MbCRC = CRC_VALUE;
  modbus_uart_buff[ind++] = CRC_VALUE & 0xFF;
  modbus_uart_buff[ind++] = CRC_VALUE >> 8;
  
  hmodbus->RS_Message_Size = ind;
  
  return RS_OK;
}

/** 
  * @brief    Парс сообщения в режиме мастер (фрейм слейва из uart -> msg).
  * @param    hmodbus           Указатель на хендлер RS.
  * @param    modbus_msg        Указатель на структуру сообщения.
  * @param    modbus_uart_buff  Указатель на буффер UART.
  * @return   RS_RES        Статус о результате заполнения структуры.
  */
RS_StatusTypeDef MB_Master_Parse_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
{
  int ind = 0; // ind for modbus-uart buffer
  int expected_size = 0;
    
  // get ID of slave device
  modbus_msg->DeviceAddr = modbus_uart_buff[ind++];
  
  // get function code (check if error response)
  modbus_msg->FuncCode = modbus_uart_buff[ind++];
  
  if(modbus_msg->FuncCode & FC_ERR_VALUES_START) // error response
  {
      modbus_msg->Except_Code = modbus_uart_buff[ind++];
  }
  else if(modbus_msg->FuncCode < FC_ERR_VALUES_START) // normal response
  {
    if(0) {}
#ifdef MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
    else if(modbus_msg->FuncCode == FC_R_DEVICE_ID) // device identifications response
    {
      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 = modbus_uart_buff[ind++];
      
      modbus_msg->ByteCnt = 0;
      
      // Парсинг объектов идентификации устройства
      uint8_t *tmp_data_addr = (uint8_t *)modbus_msg->MbData;
      int data_index = 0;
      
      for(int obj = 0; obj < modbus_msg->DevId.NumbOfObj; obj++)
      {
        // Читаем ID объекта
        uint8_t object_id = modbus_uart_buff[ind++];
        tmp_data_addr[data_index++] = object_id;
        
        // Читаем длину объекта
        uint8_t object_length = modbus_uart_buff[ind++];
        tmp_data_addr[data_index++] = object_length;
        
        // Читаем данные объекта
        for(int i = 0; i < object_length; i++)
        {
          tmp_data_addr[data_index++] = modbus_uart_buff[ind++];
        }
        
        modbus_msg->ByteCnt += (2 + object_length); // ID + длина + данные
      }
    }
#endif //MODBUS_ENABLE_DEVICE_IDENTIFICATIONS
#ifdef MODBUS_ENABLE_DIAGNOSTICS
    else if(modbus_msg->FuncCode == FC_R_DIAGNOSTICS)
    {
      // Diagnostics response: [SubFunc_HI][SubFunc_LO][Data_HI][Data_LO]
      modbus_msg->MbData[0] = modbus_uart_buff[ind++] << 8; 
      modbus_msg->MbData[0] |= modbus_uart_buff[ind++];
      modbus_msg->MbData[1] = modbus_uart_buff[ind++] << 8; 
      modbus_msg->MbData[1] |= modbus_uart_buff[ind++];
    }
#endif //MODBUS_ENABLE_DIAGNOSTICS
    else // classic modbus response
    {
      // get byte count for read functions
      if((modbus_msg->FuncCode == 0x01) || (modbus_msg->FuncCode == 0x02) || 
         (modbus_msg->FuncCode == 0x03) || (modbus_msg->FuncCode == 0x04))
      {
        modbus_msg->ByteCnt = modbus_uart_buff[ind++];
        
        // read data bytes
        uint16_t *tmp_data_addr = (uint16_t *)modbus_msg->MbData;
        for(int i = 0; i < modbus_msg->ByteCnt; i++)
        {
          if(i % 2 == 0)   // HI byte
            tmp_data_addr[i/2] = (uint16_t)modbus_uart_buff[ind++] << 8;
          else            // LO byte
            tmp_data_addr[i/2] |= modbus_uart_buff[ind++];
        }
      }
      // for write functions - echo address and quantity
      else if((modbus_msg->FuncCode == 0x05) || (modbus_msg->FuncCode == 0x06) ||
              (modbus_msg->FuncCode == 0x0F) || (modbus_msg->FuncCode == 0x10))
      {
        modbus_msg->Addr = modbus_uart_buff[ind++] << 8;
        modbus_msg->Addr |= modbus_uart_buff[ind++];
        modbus_msg->Qnt = modbus_uart_buff[ind++] << 8;
        modbus_msg->Qnt |= modbus_uart_buff[ind++];
      }
    }
  }
  
  
  //---------------CRC----------------
  //----------[last 2 bytes]----------
  uint16_t CRC_VALUE = crc16(modbus_uart_buff, ind);
  modbus_msg->MbCRC = modbus_uart_buff[ind++];
  modbus_msg->MbCRC |= modbus_uart_buff[ind++] << 8;
  
  if(modbus_msg->MbCRC != CRC_VALUE)
  {
    TrackerCnt_Err(hmodbus->rs_err);
    return RS_PARSE_MSG_ERR;
  }  
  
  return RS_OK;
}





/** @brief Сформировать запрос на чтение коилов */
RS_MsgTypeDef MB_REQUEST_READ_COILS(uint8_t slave_addr, uint16_t start_addr, uint16_t quantity)
{
  RS_MsgTypeDef msg = {slave_addr, FC_R_COILS, {0}, start_addr, quantity, 0, {0}, 0, 0};
  return msg;
}

/** @brief Сформировать запрос на чтение дискретных регистров */
RS_MsgTypeDef MB_REQUEST_READ_DISCRETE_INPUTS(uint8_t slave_addr, uint16_t start_addr, uint16_t quantity)
{
  RS_MsgTypeDef msg = {slave_addr, FC_R_DISC_IN, {0}, start_addr, quantity, 0, {0}, 0, 0};
  return msg;
}

/** @brief Сформировать запрос на чтение холдинг регистров */
RS_MsgTypeDef MB_REQUEST_READ_HOLDING_REGS(uint8_t slave_addr, uint16_t start_addr, uint16_t quantity)
{
  RS_MsgTypeDef msg = {slave_addr, FC_R_HOLD_REGS, {0}, start_addr, quantity, 0, {0}, 0, 0};
  return msg;
}

/** @brief Сформировать запрос на чтение инпут регистров */
RS_MsgTypeDef MB_REQUEST_READ_INPUT_REGS(uint8_t slave_addr, uint16_t start_addr, uint16_t quantity)
{
  RS_MsgTypeDef msg = {slave_addr, FC_R_IN_REGS, {0}, start_addr, quantity, 0, {0}, 0, 0};
  return msg;
}

/** @brief Сформировать запрос на запись одного коила */
RS_MsgTypeDef MB_REQUEST_WRITE_SINGLE_COIL(uint8_t slave_addr, uint16_t coil_addr, uint8_t value)
{
  RS_MsgTypeDef msg = {slave_addr, FC_W_COIL, {0}, coil_addr, (value ? 0xFF00 : 0x0000), 0, {0}, 0, 0};
  return msg;
}

/** @brief Сформировать запрос на запись одного регистра */
RS_MsgTypeDef MB_REQUEST_WRITE_SINGLE_REG(uint8_t slave_addr, uint16_t reg_addr, uint16_t value)
{
  RS_MsgTypeDef msg = {slave_addr, FC_W_HOLD_REG, {0}, reg_addr, value, 0, {0}, 0, 0};
  return msg;
}

/** @brief Сформировать запрос на запись нескольких регистров */
RS_MsgTypeDef MB_REQUEST_WRITE_MULTIPLE_COILS(uint8_t slave_addr, uint16_t start_addr, uint16_t quantity, uint8_t *coils_data)
{
  RS_MsgTypeDef msg = {slave_addr, FC_W_COILS, {0}, start_addr, quantity, 0, {0}, 0, 0};
  
  // Calculate byte count and prepare data
  uint8_t byte_count = (quantity + 7) / 8;
  msg.ByteCnt = byte_count;
  
  // Copy coil data to message MbData array
  for(int i = 0; i < byte_count; i++) {
    if(i < MbData_size) {
      msg.MbData[i] = coils_data[i];
    }
  }
  
  return msg;
}

/** @brief Сформировать запрос на запись нескольких коилов */
RS_MsgTypeDef MB_REQUEST_WRITE_MULTIPLE_REGS(uint8_t slave_addr, uint16_t start_addr, uint16_t quantity, uint16_t *regs_data)
{
  RS_MsgTypeDef msg = {slave_addr, FC_W_HOLD_REGS, {0}, start_addr, quantity, 0, {0}, 0, 0};
  
  msg.ByteCnt = quantity * 2; // Each register is 2 bytes
  
  // Copy register data to message MbData array
  for(int i = 0; i < quantity && i < MbData_size; i++) {
    msg.MbData[i] = regs_data[i];
  }
  
  return msg;
}

//---------ДИАГНОСТИЧЕСКИЕ ДАННЫЕ-----------
RS_MsgTypeDef MB_REQUEST_DIAGNOSTIC_QUERY(uint8_t slave_addr, uint16_t sub_function, uint16_t data)
{
  RS_MsgTypeDef msg = {slave_addr, FC_R_DIAGNOSTICS, {0}, 0, 0, 0, {sub_function, data}, 0, 0};
  return msg;
}
RS_MsgTypeDef MB_REQUEST_RETURN_QUERY_DATA(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x0000, 0x0000);
}

RS_MsgTypeDef MB_REQUEST_RESTART_COMMUNICATIONS(uint8_t slave_addr, uint16_t data)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x0001, data);
}

RS_MsgTypeDef MB_REQUEST_RETURN_DIAGNOSTIC_REGISTER(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x0002, 0x0000);
}

RS_MsgTypeDef MB_REQUEST_FORCE_LISTEN_ONLY_MODE(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x0004, 0x0000);
}

RS_MsgTypeDef MB_REQUEST_CLEAR_COUNTERS_AND_DIAGNOSTIC_REGISTER(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x000A, 0x0000);
}

RS_MsgTypeDef MB_REQUEST_RETURN_BUS_MESSAGE_COUNT(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x000B, 0x0000);
}

RS_MsgTypeDef MB_REQUEST_RETURN_BUS_COMMUNICATION_ERROR_COUNT(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x000C, 0x0000);
}

RS_MsgTypeDef MB_REQUEST_RETURN_SLAVE_EXCEPTION_ERROR_COUNT(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x000D, 0x0000);
}

RS_MsgTypeDef MB_REQUEST_RETURN_SLAVE_MESSAGE_COUNT(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x000E, 0x0000);
}

RS_MsgTypeDef MB_REQUEST_RETURN_SLAVE_NO_RESPONSE_COUNT(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x000F, 0x0000);
}

RS_MsgTypeDef MB_REQUEST_RETURN_SLAVE_NAK_COUNT(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x0010, 0x0000);
}

RS_MsgTypeDef MB_REQUEST_RETURN_SLAVE_BUSY_COUNT(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x0011, 0x0000);
}

RS_MsgTypeDef MB_REQUEST_RETURN_BUS_CHARACTER_OVERRUN_COUNT(uint8_t slave_addr)
{
  return MB_REQUEST_DIAGNOSTIC_QUERY(slave_addr, 0x0012, 0x0000);
}

//---------ИДЕНТИФИКАТОРЫ МОДБАС-----------
RS_MsgTypeDef MB_REQUEST_READ_DEVICE_ID_BASIC(uint8_t slave_addr)
{
  RS_MsgTypeDef msg = {slave_addr, FC_R_DEVICE_ID, {0x0E, 0x01, 0x00, 0, 0, 0}, 0, 0, 0, {0}, 0, 0};
  return msg;
}

RS_MsgTypeDef MB_REQUEST_READ_DEVICE_ID_REGULAR(uint8_t slave_addr)
{
  RS_MsgTypeDef msg = {slave_addr, FC_R_DEVICE_ID, {0x0E, 0x02, 0x00, 0, 0, 0}, 0, 0, 0, {0}, 0, 0};
  return msg;
}

RS_MsgTypeDef MB_REQUEST_READ_DEVICE_ID_EXTENDED(uint8_t slave_addr)
{
  RS_MsgTypeDef msg = {slave_addr, FC_R_DEVICE_ID, {0x0E, 0x03, 0x00, 0, 0, 0}, 0, 0, 0, {0}, 0, 0};
  return msg;
}

RS_MsgTypeDef MB_REQUEST_READ_DEVICE_ID_SPECIFIC(uint8_t slave_addr, uint8_t object_id)
{
  RS_MsgTypeDef msg = {slave_addr, FC_R_DEVICE_ID, {0x0E, 0x04, object_id, 0, 0, 0}, 0, 0, 0, {0}, 0, 0};
  return msg;
}

#endif //MODBUS_ENABLE_MASTER
