#include "zmodbus_slave.h"

#define UINT16ToUINT8(u16d, u8addbegin) \
  {                                     \
    (u8addbegin)[0] = u16d >> 8;        \
    (u8addbegin)[1] = (uint8_t)u16d;    \
  }

void ModbusCreateTx_setReg(uint8_t *txbuffer, uint8_t length, uint16_t off, uint16_t regdata) {
  if (3 + off * 2 > length - 2) {
    return;
  }
  txbuffer[3 + off * 2] = regdata >> 8;
  txbuffer[3 + off * 2 + 1] = (uint8_t)regdata;
}

void ModbusCreateTxData(uint8_t *txbuffer, uint8_t length, uint16_t *sendlength, ModbusSlaveData_t *txdata) {
  /**
   * @brief TODO:添加txbuffer长度计算
   */
  if (length < 4) return;
  txbuffer[0] = txdata->deviceId;
  txbuffer[1] = (uint8_t)txdata->functionCode;
  *sendlength = 0;
  volatile uint16_t _sendlength = 0;
  volatile uint16_t i = 0;

  if (txdata->functionCode == ModbusOrder01 || txdata->functionCode == ModbusOrder02 ||
      txdata->functionCode == ModbusOrder03 || txdata->functionCode == ModbusOrder04) {
    /**
     * @brief 01,02 指令操作bit
     */

    //读取多个输出状态
    // sendLen =
    //   add(1byte)+functionCode(1byte)+addNum(1)+crc(2) = 5+numbyte
    volatile uint16_t numbyte = txdata->d.O01.bytenum;
    _sendlength = 5 + numbyte;
    if (length < _sendlength) return;
    txbuffer[2] = txdata->d.O01.bytenum;
    if (txdata->d.O01.byte_table != NULL) {
      for (i = 0; i < numbyte; i++) {
        txbuffer[3 + i] = txdata->d.O01.byte_table[i];
      }
    }
  } else if (txdata->functionCode == ModbusOrder0F || txdata->functionCode == ModbusOrder10) {
    //写入多个输出
    // sendLen = add(1byte)+functionCode(1byte)+startAdd(2)+addNum(2)+crc(2) = 8
    _sendlength = 8;
    if (length < _sendlength) return;

    UINT16ToUINT8(txdata->d.O0F.startbit, &txbuffer[2]);
    UINT16ToUINT8(txdata->d.O0F.bitnum, &txbuffer[4]);
  } else {
    /**
     * @brief Not support now
     */
  }
  // calculate  crc
  if (_sendlength > 2) {
    ZGenerateCRC16CheckCodeToPacket(txbuffer, _sendlength);
  }
  *sendlength = _sendlength;
}

void ModbusCreateExceptionData(uint8_t *txbuffer, uint8_t length, uint16_t *sendlength, uint8_t deviceid,
                               uint8_t functioncode, ModbusStatus status) {
  /**
   * @brief TODO:添加txbuffer长度计算
   */
  volatile uint16_t _sendlength = 0;
  volatile uint16_t i = 0;

  if (length < 4) return;
  txbuffer[0] = deviceid;
  txbuffer[1] = functioncode | 0x80;
  txbuffer[2] = (uint8_t)status;
  *sendlength = 0;
  // sendLen = add(1byte)+functionCode(1byte)+exception(1)+crc(2) = 4
  _sendlength = 4;
  ZGenerateCRC16CheckCodeToPacket(txbuffer, _sendlength);
  *sendlength = _sendlength;
  return;
}