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

@section Функции и макросы

### Инициализация:
- MODBUS_SetupHardware()    — Инициализация модуля Modbus.
- MODBUS_Config()           — Инициализация модуля Modbus.

### Функции для Modbus
- MB_Slave_Response()
- MB_Slave_Collect_Message()
- MB_Slave_Parse_Message()
- MB_Master_Collect_Message()
- MB_Master_Parse_Message()

### Функции для работы с RS (UART):
- RS_Parse_Message() / RS_Collect_Message() — Парсинг и сборка сообщения.
- RS_Response()                             — Отправка ответа.

******************************************************************************/

#include "modbus.h"

/* MODBUS HANDLES */
RS_HandleTypeDef hmodbus1;
RS_MsgTypeDef MODBUS_MSG;

/* DEFINE REGISTERS/COILS */
MB_DeviceIdentificationTypeDef MB_DEVID;
MB_DataStructureTypeDef MB_DATA = {0};;

//-------------------------------------------------------------------
//-----------------------------FOR USER------------------------------
/** 
  * @brief    Инициализация периферии модбас.
  * @param    hmodbus Указатель на хендлер RS
  * @details  Подключает хендлы периферии к hmodbus
  *           Конфигурация выставляется по умолчанию из modbus_config.h
  */
void MODBUS_SetupHardware(RS_HandleTypeDef *hmodbus, UART_HandleTypeDef *huart, TIM_HandleTypeDef *htim)
{
  if((hmodbus == NULL) || (huart == NULL))
  {
    return;
  }
  MB_DeviceInentificationInit();
  //-----------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 = RS_SLAVE_ALWAYS_WAIT;
  hmodbus1.sRS_RX_Size_Mode = RS_RX_Size_NotConst;
  
  // INIT
  hmodbus1.RS_STATUS = RS_Init(hmodbus, huart, htim, 0);  

  RS_EnableReceive();
}
/** 
  * @brief    Программная конфигурация модбас.
  * @param    hmodbus указатель на хендлер RS
  * @param    Timeout Время тишины между двумя байтами после которых перезапускается прием
  * @param    master  Режим мастер (пока не сделан)
  * @details  Конфигурирует ID, таймаут и режим hmodbus
  */
void MODBUS_Config(RS_HandleTypeDef *hmodbus, uint8_t ID, uint16_t Timeout, uint8_t master)
{
  if(hmodbus == NULL)
  {
    return;
  }
  //-----------SETUP MODBUS-------------
  // set up modbus: MB_RX_Size_NotConst and Timeout enable
  hmodbus->ID = ID;
  hmodbus->sRS_Timeout = Timeout;
  if(master)
    hmodbus->sRS_Mode = RS_SLAVE_ALWAYS_WAIT;
  else
    hmodbus->sRS_Mode = RS_SLAVE_ALWAYS_WAIT;
  hmodbus->sRS_RX_Size_Mode = RS_RX_Size_NotConst;
}

/** 
  * @brief    Запуск слейв модбас.
  * @param    hmodbus     Указатель на хендлер RS.
  * @param    modbus_msg  Указатель на структуру сообщения.
  * @details  Конфигурирует ID, таймаут и режим hmodbus
  */
void MODBUS_SlaveStart(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg)
{
  if(hmodbus == NULL)
  {
    return;
  }
  
  if(modbus_msg)
    RS_Receive_IT(hmodbus, modbus_msg);
  else
    RS_Receive_IT(hmodbus, &MODBUS_MSG);
}




/** 
  * @brief    Ответ на сообщение в режиме слейва.
  * @param    hmodbus     Указатель на хендлер RS.
  * @param    modbus_msg  Указатель на структуру сообщения.
  * @return   RS_RES  Статус о результате ответа на комманду.
  */
static RS_StatusTypeDef MB_Slave_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(hmodbus->ID == 0)
  {
    hmodbus->RS_STATUS = RS_SKIP;
    return MB_RES;
  }

  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_Proccess_Read_Coils(hmodbus->pMessagePtr);
        break;
      
      // Read Hodling Registers
      case MB_R_HOLD_REGS:
        hmodbus->f.MessageHandled =   MB_Proccess_Read_Hold_Regs(hmodbus->pMessagePtr);
        break;
      case MB_R_IN_REGS: 
        hmodbus->f.MessageHandled =   MB_Proccess_Read_Input_Regs(hmodbus->pMessagePtr);
        break;
      
      
      // Write Single Coils
      case MB_W_COIL: 
        hmodbus->f.MessageHandled = MB_Proccess_Write_Single_Coil(hmodbus->pMessagePtr); 
        if(hmodbus->f.MessageHandled) 
        {
          hmodbus->f.DataUpdated = 1;
          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_Proccess_Write_Single_Reg(hmodbus->pMessagePtr);  
        if(hmodbus->f.MessageHandled) 
        {
          hmodbus->f.DataUpdated = 1;
          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.DataUpdated = 1;
          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_Proccess_Write_Miltuple_Regs(hmodbus->pMessagePtr);
        if(hmodbus->f.MessageHandled) 
        {
          hmodbus->f.DataUpdated = 1;
          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_Proccess_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)
    {
      TrackerCnt_Warn(hmodbus->rs_err);
			modbus_msg->Func_Code |= ERR_VALUES_START;
    }
    else
    {
      TrackerCnt_Ok(hmodbus->rs_err);
    }
    
      
  }
  
  // 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  
  if(hmodbus->f.DeferredResponse == 0)
  {
    MB_RES = RS_Handle_Transmit_Start(hmodbus, modbus_msg);
  }
  else
  {
    RS_Handle_Receive_Start(hmodbus, modbus_msg);
    hmodbus->f.DeferredResponse = 0;
  }
  
  hmodbus->RS_STATUS = MB_RES;
  return MB_RES;
}



/** 
  * @brief    Сбор сообщения в буфер UART в режиме слейв.
  * @param    hmodbus           Указатель на хендлер RS.
  * @param    modbus_msg        Указатель на структуру сообщения.
  * @param    modbus_uart_buff  Указатель на буффер UART.
  * @return   RS_RES        Статус о результате заполнения буфера.
  */
static RS_StatusTypeDef MB_Slave_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
        {
          TrackerCnt_Err(hmodbus->rs_err);
          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
        {
          TrackerCnt_Err(hmodbus->rs_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;
    }
  }
  if(ind < 0)
    return RS_COLLECT_MSG_ERR;
  
  //---------------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    Определить размер модбас запроса.
  * @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;
  
  if ((modbus_msg->Func_Code & ~ERR_VALUES_START) < 0x0F)
  {
    modbus_msg->ByteCnt = 0;
    mb_func_size = 1;
    
  }
  else
  {
    modbus_msg->ByteCnt = hmodbus->pBufferPtr[RX_FIRST_PART_SIZE-1]; // get numb of data in command
  // +1 because that defines is size, not ind.
    mb_func_size = modbus_msg->ByteCnt + 2;
  }
  
  
  if(modbus_msg->Func_Code == MB_R_DEVICE_INFO)
  {
    mb_func_size = 0;
  }
  
  mb_func_size = RX_FIRST_PART_SIZE + mb_func_size; // size of whole message
  return mb_func_size;
}


/** 
  * @brief    Парс сообщения в режиме слейв.
  * @param    hmodbus           Указатель на хендлер RS.
  * @param    modbus_msg        Указатель на структуру сообщения.
  * @param    modbus_uart_buff  Указатель на буффер UART.
  * @return   RS_RES        Статус о результате заполнения структуры.
  * @details    Заполнение структуры сообщения из буффера UART.
  */
static RS_StatusTypeDef MB_Slave_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
  hmodbus->f.RX_Continue = 0;
  //-----INFO ABOUT DATA/MESSAGE-------
  //-----------[first bits]------------
  // get ID of message/user
  if(modbus_uart_buff[ind] != hmodbus->ID)
  {
    modbus_msg->MbAddr = 0;
    return RS_SKIP;
  }
	modbus_msg->MbAddr = modbus_uart_buff[ind++];
  
  // get func code
  modbus_msg->Func_Code = modbus_uart_buff[ind++];
  if(modbus_msg->Func_Code & ERR_VALUES_START) // явная херня
  {
    modbus_msg->MbAddr = 0;
    return RS_SKIP;
  }
  
  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->pMessagePtr->Func_Code == 0x0F) || (hmodbus->pMessagePtr->Func_Code == 0x10))
		hmodbus->pMessagePtr->ByteCnt = modbus_uart_buff[ind++];
	else
		hmodbus->pMessagePtr->ByteCnt = 0;
  
  //---------------DATA----------------
  //            (optional)
  if (modbus_msg->ByteCnt != 0)
  {
    //check that data size is correct
    if (modbus_msg->ByteCnt > DATA_SIZE*2)
    {
      TrackerCnt_Err(hmodbus->rs_err);
      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++)
    { // 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)
  {
    TrackerCnt_Err(hmodbus->rs_err);
    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; // 
  
  // если размер меньше ожидаемого - продолжаем принимать
  if(hmodbus->RS_Message_Size < MB_Define_Size_of_Function(hmodbus, modbus_msg))
  {
    hmodbus->f.RX_Continue = 1;
    return RS_SKIP;
  }
  // если больше Ошибка
  else if (hmodbus->RS_Message_Size > MB_Define_Size_of_Function(hmodbus, modbus_msg))
  {
    return RS_PARSE_MSG_ERR;
  }
  
  return RS_OK;
}








/** 
  * @brief    Сбор сообщения в буфер UART в режиме мастер.
  * @param    hmodbus           Указатель на хендлер RS.
  * @param    modbus_msg        Указатель на структуру сообщения.
  * @param    modbus_uart_buff  Указатель на буффер UART.
  * @return   RS_RES        Статус о результате заполнения буфера.
  */
static RS_StatusTypeDef MB_Master_Collect_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
{
  return RS_PARSE_MSG_ERR;
}


/** 
  * @brief    Парс сообщения в режиме мастер.
  * @param    hmodbus           Указатель на хендлер RS.
  * @param    modbus_msg        Указатель на структуру сообщения.
  * @param    modbus_uart_buff  Указатель на буффер UART.
  * @return   RS_RES        Статус о результате заполнения структуры.
  */
static RS_StatusTypeDef MB_Master_Parse_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
{
  return RS_PARSE_MSG_ERR;
}





/* Реализация функций из rs_message.c для протокола */
RS_StatusTypeDef RS_Response(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg)
{
  if(hmodbus->sRS_Mode >= RS_MASTER_START)
  {
    return RS_ERR;
  }
  return MB_Slave_Response(hmodbus, modbus_msg);
}

RS_StatusTypeDef RS_Collect_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
{
  if(hmodbus->sRS_Mode < RS_MASTER_START)
  {
    return MB_Slave_Collect_Message(hmodbus, modbus_msg, modbus_uart_buff);
  }
  else
  {
    return MB_Master_Collect_Message(hmodbus, modbus_msg, modbus_uart_buff);
  }
}

RS_StatusTypeDef RS_Parse_Message(RS_HandleTypeDef *hmodbus, RS_MsgTypeDef *modbus_msg, uint8_t *modbus_uart_buff)
{
  if(hmodbus->sRS_Mode < RS_MASTER_START)
  {
    return MB_Slave_Parse_Message(hmodbus, modbus_msg, modbus_uart_buff);
  }
  else
  {
    return MB_Master_Parse_Message(hmodbus, modbus_msg, modbus_uart_buff);
  }
}