/*
    Modbus.cpp - Source for Modbus Base Library
    Copyright (C) 2014 André Sarmento Barbosa
    Copyright (C) 2023 Pascal JEAN aka epsilonrt
*/
#include "Modbus.h"

//-------------------------------------------------------------------------------
Modbus::Modbus()  :
  _regs_head (0),
  _regs_last (0),
  _additional_data (0),
  _frame (nullptr),
  _len (0),
  _reply (0),
  _debug (nullptr) {
}

//-------------------------------------------------------------------------------
// private
TRegister *Modbus::searchRegister (word address) {
  TRegister *reg = _regs_head;
  //if there is no register configured, bail
  if (reg == 0) {
    return (0);
  }
  //scan through the linked list until the end of the list or the register is found.
  //return the pointer.
  do {
    if (reg->address == address) {
      return (reg);
    }
    reg = reg->next;
  }
  while (reg);
  return (0);
}

//-------------------------------------------------------------------------------
// private
void Modbus::addReg (word address, word value) {
  TRegister *newreg;

  newreg = (TRegister *) malloc (sizeof (TRegister));
  newreg->address = address;
  newreg->value   = value;
  newreg->edata   = 0;
  newreg->next    = 0;

  if (_regs_head == 0) {
    _regs_head = newreg;
    _regs_last = _regs_head;
  }
  else {
    //Assign the last register's next pointer to newreg.
    _regs_last->next = newreg;
    //then make temp the last register in the list.
    _regs_last = newreg;
  }
}

//-------------------------------------------------------------------------------
// private
bool Modbus::setReg (word address, word value) {
  TRegister *reg;
  //search for the register address
  reg = searchRegister (address);
  //if found then assign the register value to the new value.
  if (reg) {
    reg->value = value;
    return true;
  }
  else {
    return false;
  }
}

//-------------------------------------------------------------------------------
// private
word Modbus::reg (word address) {
  TRegister *reg;
  reg = searchRegister (address);
  if (reg) {
    return (reg->value);
  }
  else {
    return (0);
  }
}

//-------------------------------------------------------------------------------
// private
bool  Modbus::setRegBounds (word address, word min, word max) {
  TRegister *reg;

  reg = searchRegister (address);

  if (reg) {

    if (!reg->edata) {

      reg->edata = new TExtData;
    }
    if (reg->edata) {

      reg->edata->min = min;
      reg->edata->max = max;
      return true;
    }
  }
  return false;
}

//-------------------------------------------------------------------------------
// private
bool  Modbus::regOutOfBounds (word address, word value) {
  TRegister *reg;

  reg = searchRegister (address);

  if (reg) {

    if (reg->edata) {

      return (value < reg->edata->min) || (value > reg->edata->max);
    }
  }
  return false;
}

//-------------------------------------------------------------------------------
// protected
void Modbus::receivePDU (byte *frame) {
  byte fcode  = frame[0];
  word field1 = (word) frame[1] << 8 | (word) frame[2];
  word field2 = (word) frame[3] << 8 | (word) frame[4];

  switch (fcode) {

    case MB_FC_WRITE_REG:
      //field1 = reg, field2 = value
      writeSingleRegister (field1, field2);
      break;

    case MB_FC_WRITE_REGS:
      //field1 = startreg, field2 = status
      writeMultipleRegisters (frame, field1, field2, frame[5]);
      break;

    case MB_FC_READ_REGS:
      //field1 = startreg, field2 = numregs
      readRegisters (fcode, field1, field2);
      break;

      #ifndef USE_HOLDING_REGISTERS_ONLY
    case MB_FC_READ_INPUT_REGS:
      //field1 = startreg, field2 = numregs
      readRegisters (fcode, field1, field2);
      break;

    case MB_FC_READ_INPUT_STAT:
    case MB_FC_READ_COILS:
      //field1 = startreg, field2 = numregs
      readBits (fcode, field1, field2);
      break;

    case MB_FC_WRITE_COIL:
      //field1 = reg, field2 = status
      writeSingleCoil (field1, field2);
      break;

    case MB_FC_WRITE_COILS:
      //field1 = startreg, field2 = numoutputs
      writeMultipleCoils (frame, field1, field2, frame[5]);
      break;

    case MB_FC_REPORT_SERVER_ID:
      reportServerId();
      break;

      #endif

    default:
      exceptionResponse (fcode, MB_EX_ILLEGAL_FUNCTION);
  }
}

//-------------------------------------------------------------------------------
// private
void Modbus::exceptionResponse (const byte fcode, const byte excode) {

  _len = 2;
  _frame = (byte *) realloc (_frame, _len);
  _frame[0] = fcode + 0x80;
  _frame[1] = excode;

  _reply = MB_REPLY_NORMAL;
}

/* Func 06: Write Single Register
    Request
      Function code     1 Byte  0x06
      Register Address  2 Bytes 0x0000 to 0xFFFF
      Register Value    2 Bytes 0x0000 to 0xFFFF
    Response
      Function code     1 Byte  0x06
      Register Address  2 Bytes 0x0000 to 0xFFFF
      Register Value    2 Bytes 0x0000 to 0xFFFF
*/
//-------------------------------------------------------------------------------
// private
void Modbus::writeSingleRegister (const word reg, const word value) {

  if (hregOutOfBounds (reg, value)) {
    exceptionResponse (MB_FC_WRITE_REG, MB_EX_ILLEGAL_VALUE);
    return;
  }

  //Check Address and execute (reg exists?)
  if (!setHreg (reg, value)) {
    exceptionResponse (MB_FC_WRITE_REG, MB_EX_ILLEGAL_ADDRESS);
    return;
  }

  //Check for failure ? Why ???
  /*
    if (hreg (reg) != value) {
    exceptionResponse (MB_FC_WRITE_REG, MB_EX_SLAVE_FAILURE);
    return;
    }
  */
  _reply = MB_REPLY_ECHO; // reply with received frame
}

/* Func 16: Write Multiple registers
    Request
      Function code         1 Byte        0x10
      Starting Address      2 Bytes       0x0000 to 0xFFFF
      Quantity of Registers 2 Bytes       0x0001 to 0x007B
      Byte Count            1 Byte        2 x N*
      Registers Value       N* x 2 Bytes  value
       N = Quantity of Registers
    Response
      Function code         1 Byte        0x10
      Starting Address      2 Bytes       0x0000 to 0xFFFF
      Quantity of Registers 2 Bytes       1 to 123 (0x7B)
*/
//-------------------------------------------------------------------------------
// private
void Modbus::writeMultipleRegisters (const byte *frame, const word startreg, const word numoutputs, const byte bytecount) {
  //Check value
  if (numoutputs < 1 || numoutputs > 123 || bytecount != 2 * numoutputs) {
    exceptionResponse (MB_FC_WRITE_REGS, MB_EX_ILLEGAL_VALUE);
    return;
  }

  //Check Address (startreg...startreg + numregs - 1)
  for (word k = 0; k < numoutputs; k++) {
    if (!searchRegister (startreg + TRegister::HregOffset + k)) {
      exceptionResponse (MB_FC_WRITE_REGS, MB_EX_ILLEGAL_ADDRESS);
      return;
    }
  }

  for (word i = 0; i < numoutputs; i++) {

    word val = (word) frame[6 + i * 2] << 8 | (word) frame[7 + i * 2];
    if (hregOutOfBounds (startreg + i, val)) {

      exceptionResponse (MB_FC_WRITE_REG, MB_EX_ILLEGAL_VALUE);
      return;
    }
    setHreg (startreg + i, val);
  }

  //Clean frame buffer
  free (_frame);
  _len = 5;
  _frame = (byte *) malloc (_len);
  if (!_frame) {
    exceptionResponse (MB_FC_WRITE_REGS, MB_EX_SLAVE_FAILURE);
    return;
  }

  _frame[0] = MB_FC_WRITE_REGS;
  _frame[1] = startreg >> 8;
  _frame[2] = startreg & 0x00FF;
  _frame[3] = numoutputs >> 8;
  _frame[4] = numoutputs & 0x00FF;
  _reply = MB_REPLY_NORMAL;
}


/*
  Func 03: Read Holding Registers
    Request
      Function code         1 Byte  0x03
      Starting Address      2 Bytes 0x0000 to 0xFFFF
      Quantity of Registers 2 Bytes 1 to 125 (0x7D)
    Response
      Function code         1 Byte  0x03
      Byte count            1 Byte  2 x N*
      Register value        N* x 2 Bytes
     N = Quantity of Registers
  Func 04: Read Input Registers
    Request
      Function code               1 Byte        0x04
      Starting Address            2 Bytes       0x0000 to 0xFFFF
      Quantity of Input Registers 2 Bytes       0x0001 to 0x007D
    Response
      Function code               1 Byte        0x04
      Byte count                  1 Byte        2 x N*
      Input Registers             N* x 2 Bytes
     N = Quantity of Input Registers
*/
//-------------------------------------------------------------------------------
// private
void Modbus::readRegisters (const byte fcode, const word startreg, const word numregs) {

  // Check value (numregs)
  if (numregs < 1 || numregs > 0x7D) {

    exceptionResponse (fcode, MB_EX_ILLEGAL_VALUE);
    return;
  }

  // calculate the query reply message length
  // for each register queried add 2 bytes
  _len = 2 + numregs * 2;

  _frame = (byte *) realloc (_frame, _len);
  if (!_frame) {

    exceptionResponse (fcode, MB_EX_SLAVE_FAILURE);
    return;
  }

  _frame[0] = fcode;
  _frame[1] = _len - 2;   //byte count

  const word offset = (fcode == MB_FC_READ_REGS) ? TRegister::HregOffset : TRegister::IregOffset;
  for (word i = 0; i < numregs; i++) {

    const word address = startreg + offset + i;
    if (searchRegister (address)) {

      // retrieve the value from the register bank for the current register
      word val = reg (address);

      // write the high byte of the register value
      _frame[2 + i * 2]  = val >> 8;
      // write the low byte of the register value
      _frame[3 + i * 2] = val & 0xFF;
    }
    else {

      exceptionResponse (fcode, MB_EX_ILLEGAL_ADDRESS);
      return;
    }
  }

  _reply = MB_REPLY_NORMAL;
}

//-------------------------------------------------------------------------------
// protected
void Modbus::debugMessage (bool reply) {

  if (_len && isDebug()) {
    char str[3];  // buffer for sprintf
    for (uint8_t i = 0; i < _len; i++) {

      sprintf (str, "%02X", _frame[i]); // str stores the string representation of the byte, in hex form
      _debug->write (!reply ? '[' : '<');
      _debug->print (str);
      _debug->write (!reply ? ']' : '>');
    }
    _debug->println ("");
  }
}

#ifndef USE_HOLDING_REGISTERS_ONLY
//-------------------------------------------------------------------------------
/* Func 01:  Read Coils
    Request
      Function code     1 Byte  0x01
      Starting Address  2 Bytes 0x0000 to 0xFFFF
      Quantity of coils 2 Bytes 1 to 2000 (0x7D0)
    Response
      Function code     1 Byte  0x01
      Byte count        1 Byte  N*
      Coil Status       n Byte  n = N or N+1
     N = Quantity of Outputs / 8, if the remainder is different of 0 -> N = N+1
   Func 02: Read Discrete Inputs
    Request
      Function code       1 Byte      0x02
      Starting Address    2 Bytes     0x0000 to 0xFFFF
      Quantity of Inputs  2 Bytes     1 to 2000 (0x7D0)
    Response
      Function code       1 Byte      0x02
      Byte count          1 Byte      N*
      Input Status        N* x 1 Byte
     N = Quantity of Inputs / 8, if the remainder is different of 0 -> N = N+1
*/
//-------------------------------------------------------------------------------
// private
void Modbus::readBits (const byte fcode, const word startreg, const word numregs) {

  //Check value (numregs)
  if (numregs < 0x0001 || numregs > 0x07D0) {
    
    exceptionResponse (fcode, MB_EX_ILLEGAL_VALUE);
    return;
  }

  // Determine the message length = function type, byte count and
  // for each group of 8 registers the message length increases by 1
  _len = 2 + numregs / 8;
  if (numregs % 8) {
    _len++;  // Add 1 to the message length for the partial byte.
  }

  _frame = (byte *) realloc (_frame, _len);
  if (!_frame) {

    exceptionResponse (fcode, MB_EX_SLAVE_FAILURE);
    return;
  }

  _frame[0] = fcode;
  _frame[1] = _len - 2; // byte count (_len - function code and byte count)

  const word offset = (fcode == MB_FC_READ_COILS) ? TRegister::CoilOffset : TRegister::IstsOffset;
  for (word i = 0; i < numregs; i++) {

    const word address = startreg + offset + i;
    if (searchRegister (address)) {

      word byteIndex = (i / 8) + 2;
      byte bitIndex = i % 8;
      if (reg (address) == 0xFF00) {

        bitSet (_frame[byteIndex], bitIndex);
      }
      else {

        bitClear (_frame[byteIndex], bitIndex);
      }
    }
    else {

      exceptionResponse (fcode, MB_EX_ILLEGAL_ADDRESS);
      return;
    }
  }

  _reply = MB_REPLY_NORMAL;
}


/* Func 05: Write Single Coil
    Request
      Function code   1 Byte  0x05
      Output Address  2 Bytes 0x0000 to 0xFFFF
      Output Value    2 Bytes 0x0000 or 0xFF00
  Response
      Function code   1 Byte  0x05
      Output Address  2 Bytes 0x0000 to 0xFFFF
      Output Value    2 Bytes 0x0000 or 0xFF00
*/
//-------------------------------------------------------------------------------
// private
void Modbus::writeSingleCoil (const word reg, const word status) {
  //Check value (status)
  if (status != 0xFF00 && status != 0x0000) {
    exceptionResponse (MB_FC_WRITE_COIL, MB_EX_ILLEGAL_VALUE);
    return;
  }

  //Check Address and execute (reg exists?)
  if (!setCoil (reg, status != 0)) {
    exceptionResponse (MB_FC_WRITE_COIL, MB_EX_ILLEGAL_ADDRESS);
    return;
  }

  //Check for failure ?? Why ?
  /*
    if (coil (reg) != (bool) status) {
    exceptionResponse (MB_FC_WRITE_COIL, MB_EX_SLAVE_FAILURE);
    return;
    }
  */

  _reply = MB_REPLY_ECHO; // reply with received frame
}

/* Func 15: Write Multiple Coils
    Request
      Function code       1 Byte  0x0F
      Starting Address    2 Bytes 0x0000 to 0xFFFF
      Quantity of Outputs 2 Bytes 0x0001 to 0x07B0
      Byte Count          1 Byte  N* (Quantity of Outputs / 8, if the remainder is different of 0 -> N = N+1)
      Outputs Value       N* x 1 Byte
    Response
      Function code       1 Byte  0x0F
      Starting Address    2 Bytes 0x0000 to 0xFFFF
      Quantity of Outputs 2 Bytes 0x0001 to 0x07B0
*/
//-------------------------------------------------------------------------------
// private
void Modbus::writeMultipleCoils (const byte *frame, const word startreg, const word numoutputs, const byte bytecount) {

  // Check value
  word bytecount_calc = numoutputs / 8;
  if (numoutputs % 8) {

    bytecount_calc++;
  }

  if (numoutputs < 0x0001 || numoutputs > 0x07B0 || bytecount != bytecount_calc) {

    exceptionResponse (MB_FC_WRITE_COILS, MB_EX_ILLEGAL_VALUE);
    return;
  }

  for (word i = 0; i < numoutputs; i++) {

    const word address = startreg + i;
    if (searchRegister (address + TRegister::CoilOffset)) {

      word byteIndex = (i / 8) + 6;
      byte bitIndex = i % 8;
      setCoil (address, bitRead (frame[byteIndex], bitIndex));
    }
    else {

      exceptionResponse (MB_FC_WRITE_COILS, MB_EX_ILLEGAL_ADDRESS);
      return;
    }
  }

  _len = 5;
  _frame = (byte *) realloc (_frame, _len);
  if (!_frame) {

    exceptionResponse (MB_FC_WRITE_COILS, MB_EX_SLAVE_FAILURE);
    return;
  }

  _frame[0] = MB_FC_WRITE_COILS;
  _frame[1] = startreg >> 8;
  _frame[2] = startreg & 0x00FF;
  _frame[3] = numoutputs >> 8;
  _frame[4] = numoutputs & 0x00FF;

  _reply = MB_REPLY_NORMAL;
}

/*
  Func 17:  Report Server ID
    Request
      Function code         1 Byte            0x11
    Response
      Function code         1 Byte            0x11
      Byte Count            1 Byte
      Server ID             device specific
      Run Indicator Status  1 Byte            0x00 = OFF, 0xFF = ON
      Additional Data
*/
//-------------------------------------------------------------------------------
// private
void Modbus::reportServerId() {

  _len = 4;
  if (_additional_data) {

    _len += strlen (_additional_data);
  }

  _frame = (byte *) realloc (_frame, _len);
  if (!_frame) {

    exceptionResponse (MB_FC_REPORT_SERVER_ID, MB_EX_SLAVE_FAILURE);
    return;
  }

  _frame[0] = MB_FC_REPORT_SERVER_ID;
  _frame[1] = _len - 2;   //byte count
  _frame[2] = 0x00; // Server ID
  _frame[3] = 0xFF; // Run Indicator Status
  if (_additional_data) { // Additional Data

    strcpy ( (char *) &_frame[4], _additional_data);
  }
  _reply = MB_REPLY_NORMAL;
}

//-------------------------------------------------------------------------------
int Modbus::setAdditionalServerData (const char data[]) {

  free (_additional_data);
  if (data) {
    size_t l = strlen (data) + 1;

    _additional_data = (char *) malloc (l);
    if (_additional_data) {

      strcpy (_additional_data, data);
      return l;
    }
  }
  return 0;
}

//-------------------------------------------------------------------------------
#endif // USE_HOLDING_REGISTERS_ONLY not defined