// =================================================================================================
// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
//               MIT license - see license.md for details
// =================================================================================================
#include "ModbusMessage.h"
#undef LOCAL_LOG_LEVEL
// #define LOCAL_LOG_LEVEL LOG_LEVEL_ERROR
#include "Logging.h"

// Default Constructor - takes optional size of MM_data to allocate memory
ModbusMessage::ModbusMessage(uint16_t dataLen) {
  if (dataLen) MM_data.reserve(dataLen);
}

// Special message Constructor - takes a std::vector<uint8_t>
ModbusMessage::ModbusMessage(std::vector<uint8_t> s) :
MM_data(s) { }

// Destructor
ModbusMessage::~ModbusMessage() { 
  // If paranoid, one can use the below :D
  // std::vector<uint8_t>().swap(MM_data);
}

// Assignment operator
ModbusMessage& ModbusMessage::operator=(const ModbusMessage& m) {
  // Do anything only if not self-assigning
  if (this != &m) {
    // Copy data from source to target
    MM_data = m.MM_data;
  }
  return *this;
}

#ifndef NO_MOVE
  // Move constructor
ModbusMessage::ModbusMessage(ModbusMessage&& m) {
  MM_data = std::move(m.MM_data);
}
  
	// Move assignment
ModbusMessage& ModbusMessage::operator=(ModbusMessage&& m) {
  MM_data = std::move(m.MM_data);
  return *this;
}
#endif

// Copy constructor
ModbusMessage::ModbusMessage(const ModbusMessage& m) :
  MM_data(m.MM_data) { }

// Equality comparison
bool ModbusMessage::operator==(const ModbusMessage& m) {
  // Prevent self-compare
  if (this == &m) return true;
  // If size is different, we assume inequality
  if (MM_data.size() != m.MM_data.size()) return false;
  // We will compare bytes manually - for uint8_t it should work out-of-the-box,
  // but the data type might be changed later.
  // If we find a difference byte, we found inequality
  for (uint16_t i = 0; i < MM_data.size(); ++i) {
    if (MM_data[i] != m.MM_data[i]) return false;
  }
  // Both tests passed ==> equality
  return true;
}

// Inequality comparison
bool ModbusMessage::operator!=(const ModbusMessage& m) {
  return (!(*this == m));
}

// Conversion to bool
ModbusMessage::operator bool() {
  if (MM_data.size() >= 2) return true;
  return false;
}

// Exposed methods of std::vector
const uint8_t *ModbusMessage::data() { return MM_data.data(); }
uint16_t       ModbusMessage::size() { return MM_data.size(); }
void           ModbusMessage::push_back(const uint8_t& val) { MM_data.push_back(val); }
void           ModbusMessage::clear() { MM_data.clear(); }
// provide restricted operator[] interface
uint8_t  ModbusMessage::operator[](uint16_t index) const {
  if (index < MM_data.size()) {
    return MM_data[index];
  }
  LOG_W("Index %d out of bounds (>=%d).\n", index, MM_data.size());
  return 0;
}
// Resize internal MM_data
uint16_t ModbusMessage::resize(uint16_t newSize) { 
  MM_data.resize(newSize); 
  return MM_data.size(); 
}

// Add append() for two ModbusMessages or a std::vector<uint8_t> to be appended
void ModbusMessage::append(ModbusMessage& m) { 
  MM_data.reserve(size() + m.size()); 
  MM_data.insert(MM_data.end(), m.begin(), m.end()); 
}

void ModbusMessage::append(std::vector<uint8_t>& m) { 
  MM_data.reserve(size() + m.size()); 
  MM_data.insert(MM_data.end(), m.begin(), m.end()); 
}

uint8_t ModbusMessage::getServerID() const {
  // Only if we have data and it is at least as long to fit serverID and function code, return serverID
  if (MM_data.size() >= 2) { return MM_data[0]; }
  // Else return 0 - normally the Broadcast serverID, but we will not support that. Full stop. :-D
  return 0;
}

// Get MM_data[0] (server ID) and MM_data[1] (function code)
uint8_t ModbusMessage::getFunctionCode() const {
  // Only if we have data and it is at least as long to fit serverID and function code, return FC
  if (MM_data.size() >= 2) { return MM_data[1]; }
  // Else return 0 - which is no valid Modbus FC.
  return 0;
}

// getError() - returns error code
Error ModbusMessage::getError() const {
  // Do we have data long enough?
  if (MM_data.size() > 2) {
    // Yes. Does it indicate an error?
    if (MM_data[1] & 0x80)
    {
      // Yes. Get it.
      return static_cast<Modbus::Error>(MM_data[2]);
    }
  }
  // Default: everything OK - SUCCESS
  return SUCCESS;
}

// Modbus data manipulation
void    ModbusMessage::setServerID(uint8_t serverID) {
  // We accept here that [0] may allocate a byte!
  if (MM_data.empty()) {
    MM_data.reserve(3);  // At least an error message should fit
  }
  MM_data[0] = serverID;
}

void    ModbusMessage::setFunctionCode(uint8_t FC) {
  // We accept here that [0], [1] may allocate bytes!
  if (MM_data.empty()) {
    MM_data.reserve(3);  // At least an error message should fit
  }
  // No serverID set yet? use a 0 to initialize it to an error-generating value
  if (MM_data.size() < 2) MM_data[0] = 0; // intentional invalid server ID!
  MM_data[1] = FC;
}

// add() variant to copy a buffer into MM_data. Returns updated size
uint16_t ModbusMessage::add(const uint8_t *arrayOfBytes, uint16_t count) {
  // Copy it
  while (count--) {
    MM_data.push_back(*arrayOfBytes++);
  }
  // Return updated size (logical length of message so far)
  return MM_data.size();
}

// determineFloatOrder: calculate the sequence of bytes in a float value
uint8_t ModbusMessage::determineFloatOrder() {
  constexpr uint8_t floatSize = sizeof(float);
  // Only do it if not done yet
  if (floatOrder[0] == 0xFF) {
    // We need to calculate it.
    // This will only work for 32bit floats, so check that
    if (floatSize != 4) {
      // OOPS! we cannot proceed.
      LOG_E("Oops. float seems to be %d bytes wide instead of 4.\n", floatSize);
      return 0;
    }

    uint32_t i = 77230;                             // int value to go into a float without rounding error
    float f = i;                                    // assign it
    uint8_t *b = (uint8_t *)&f;                     // Pointer to bytes of f
    uint8_t expect[floatSize] = { 0x47, 0x96, 0xd7, 0x00 }; // IEEE754 representation 
    uint8_t matches = 0;                            // number of bytes successfully matched
     
    // Loop over the bytes of the expected sequence
    for (uint8_t inx = 0; inx < floatSize; ++inx) {
      // Loop over the real bytes of f
      for (uint8_t trg = 0; trg < floatSize; ++trg) {
        if (expect[inx] == b[trg]) {
          floatOrder[inx] = trg;
          matches++;
          break;
        }
      }
    }

    // All bytes found?
    if (matches != floatSize) {
      // No! There is something fishy...
      LOG_E("Unable to determine float byte order (matched=%d of %d)\n", matches, floatSize);
      floatOrder[0] = 0xFF;
      return 0;
    } else {
      HEXDUMP_V("floatOrder", floatOrder, floatSize);
    }
  }
  return floatSize;
}

// determineDoubleOrder: calculate the sequence of bytes in a double value
uint8_t ModbusMessage::determineDoubleOrder() {
  constexpr uint8_t doubleSize = sizeof(double);
  // Only do it if not done yet
  if (doubleOrder[0] == 0xFF) {
    // We need to calculate it.
    // This will only work for 64bit doubles, so check that
    if (doubleSize != 8) {
      // OOPS! we cannot proceed.
      LOG_E("Oops. double seems to be %d bytes wide instead of 8.\n", doubleSize);
      return 0;
    }

    uint64_t i = 5791007487489389;                  // int64 value to go into a double without rounding error
    double f = i;                                   // assign it
    uint8_t *b = (uint8_t *)&f;                     // Pointer to bytes of f
    uint8_t expect[doubleSize] = { 0x43, 0x34, 0x92, 0xE4, 0x00, 0x2E, 0xF5, 0x6D }; // IEEE754 representation 
    uint8_t matches = 0;                            // number of bytes successfully matched
     
    // Loop over the bytes of the expected sequence
    for (uint8_t inx = 0; inx < doubleSize; ++inx) {
      // Loop over the real bytes of f
      for (uint8_t trg = 0; trg < doubleSize; ++trg) {
        if (expect[inx] == b[trg]) {
          doubleOrder[inx] = trg;
          matches++;
          break;
        }
      }
    }

    // All bytes found?
    if (matches != doubleSize) {
      // No! There is something fishy...
      LOG_E("Unable to determine double byte order (matched=%d of %d)\n", matches, doubleSize);
      doubleOrder[0] = 0xFF;
      return 0;
    } else {
      HEXDUMP_V("doubleOrder", doubleOrder, doubleSize);
    }
  }
  return doubleSize;
}

// swapFloat() and swapDouble() will re-order the bytes of a float or double value
// according a user-given pattern
float ModbusMessage::swapFloat(float& f, int swapRule) {
  LOG_V("swap float, swapRule=%02X\n", swapRule);
  // Make a byte pointer to the given float
  uint8_t *src = (uint8_t *)&f;
  // Define a "work bench" float and byte pointer to it
  float interim;
  uint8_t *dst = (uint8_t *)&interim;
  // Loop over all bytes of a float
  for (uint8_t i = 0; i < sizeof(float); ++i) {
    // Get i-th byte from the spot the swap table tells
    // (only the first 4 tables are valid for floats)
    LOG_V("dst[%d] = src[%d]\n", i, swapTables[swapRule & 0x03][i]);
    dst[i] = src[swapTables[swapRule & 0x03][i]];
    // Does the swar rule require nibble swaps?
    if (swapRule & 0x08) {
      // Yes, it does. 
      uint8_t nib = ((dst[i] & 0x0f) << 4) | ((dst[i] >> 4) & 0x0F);
      dst[i] = nib;
    }
  }
  // Save and return result
  f = interim;
  return interim;
}

double ModbusMessage::swapDouble(double& f, int swapRule) {
  LOG_V("swap double, swapRule=%02X\n", swapRule);
  // Make a byte pointer to the given double
  uint8_t *src = (uint8_t *)&f;
  // Define a "work bench" double and byte pointer to it
  double interim;
  uint8_t *dst = (uint8_t *)&interim;
  // Loop over all bytes of a double
  for (uint8_t i = 0; i < sizeof(double); ++i) {
    // Get i-th byte from the spot the swap table tells
    LOG_V("dst[%d] = src[%d]\n", i, swapTables[swapRule & 0x07][i]);
    dst[i] = src[swapTables[swapRule & 0x07][i]];
    // Does the swar rule require nibble swaps?
    if (swapRule & 0x08) {
      // Yes, it does. 
      uint8_t nib = ((dst[i] & 0x0f) << 4) | ((dst[i] >> 4) & 0x0F);
      dst[i] = nib;
    }
  }
  // Save and return result
  f = interim;
  return interim;
}

// add() variant for a vector of uint8_t
uint16_t ModbusMessage::add(vector<uint8_t> v) {
  for (auto& b: v) {
    MM_data.push_back(b);
  }
  return MM_data.size();
}

// add() variants for float and double values
// values will be added in IEEE754 byte sequence (MSB first)
uint16_t ModbusMessage::add(float v, int swapRule) {
  // First check if we need to determine byte order
  LOG_V("add float, swapRule=%02X\n", swapRule);
  HEXDUMP_V("float", (uint8_t *)&v, sizeof(float));
  if (determineFloatOrder()) {
    // If we get here, the floatOrder is known
    float interim = 0;
    uint8_t *dst = (uint8_t *)&interim;
    uint8_t *src = (uint8_t *)&v;
    // Put out the bytes of v in normalized sequence
    for (uint8_t i = 0; i < sizeof(float); ++i) {
      dst[i] = src[floatOrder[i]];
    }
    HEXDUMP_V("normalized float", (uint8_t *)&interim, sizeof(float));
    // Do we need to apply a swap rule?
    if (swapRule & 0x0B) {
      // Yes, so do it.
      swapFloat(interim, swapRule & 0x0B);
    }
    HEXDUMP_V("swapped float", (uint8_t *)&interim, sizeof(float));
    // Put out the bytes of v in normalized (and swapped) sequence
    for (uint8_t i = 0; i < sizeof(float); ++i) {
      MM_data.push_back(dst[i]);
    }
  }

  return MM_data.size();
}

uint16_t ModbusMessage::add(double v, int swapRule) {
  // First check if we need to determine byte order
  LOG_V("add double, swapRule=%02X\n", swapRule);
  HEXDUMP_V("double", (uint8_t *)&v, sizeof(double));
  if (determineDoubleOrder()) {
    // If we get here, the doubleOrder is known
    double interim = 0;
    uint8_t *dst = (uint8_t *)&interim;
    uint8_t *src = (uint8_t *)&v;
    // Put out the bytes of v in normalized sequence
    for (uint8_t i = 0; i < sizeof(double); ++i) {
      dst[i] = src[doubleOrder[i]];
    }
    HEXDUMP_V("normalized double", (uint8_t *)&interim, sizeof(double));
    // Do we need to apply a swap rule?
    if (swapRule & 0x0F) {
      // Yes, so do it.
      swapDouble(interim, swapRule & 0x0F);
    }
    HEXDUMP_V("swapped double", (uint8_t *)&interim, sizeof(double));
    // Put out the bytes of v in normalized (and swapped) sequence
    for (uint8_t i = 0; i < sizeof(double); ++i) {
      MM_data.push_back(dst[i]);
    }
  }

  return MM_data.size();
}

// get() variants for float and double values
// values will be read in IEEE754 byte sequence (MSB first)
uint16_t ModbusMessage::get(uint16_t index, float& v, int swapRule) const {
  // First check if we need to determine byte order
  if (determineFloatOrder()) {
    // If we get here, the floatOrder is known
    // Will it fit?
    if (index <= MM_data.size() - sizeof(float)) {
      // Yes. Get the bytes of v in normalized sequence
      uint8_t *bytes = (uint8_t *)&v;
      for (uint8_t i = 0; i < sizeof(float); ++i) {
        bytes[i] = MM_data[index + floatOrder[i]];
      }
      HEXDUMP_V("got float", (uint8_t *)&v, sizeof(float));
      // Do we need to apply a swap rule?
      if (swapRule & 0x0B) {
        // Yes, so do it.
        swapFloat(v, swapRule & 0x0B);
      }
      HEXDUMP_V("got float swapped", (uint8_t *)&v, sizeof(float));
      index += sizeof(float);
    }
  }

  return index;
}

uint16_t ModbusMessage::get(uint16_t index, double& v, int swapRule) const {
  // First check if we need to determine byte order
  if (determineDoubleOrder()) {
    // If we get here, the doubleOrder is known
    // Will it fit?
    if (index <= MM_data.size() - sizeof(double)) {
      // Yes. Get the bytes of v in normalized sequence
      uint8_t *bytes = (uint8_t *)&v;
      for (uint8_t i = 0; i < sizeof(double); ++i) {
        bytes[i] = MM_data[index + doubleOrder[i]];
      }
      HEXDUMP_V("got double", (uint8_t *)&v, sizeof(double));
      // Do we need to apply a swap rule?
      if (swapRule & 0x0F) {
        // Yes, so do it.
        swapDouble(v, swapRule & 0x0F);
      }
      HEXDUMP_V("got double swapped", (uint8_t *)&v, sizeof(double));
      index += sizeof(double);
    }
  }

  return index;
}

// get() - read a byte array of a given size into a vector<uint8_t>. Returns updated index
uint16_t ModbusMessage::get(uint16_t index, vector<uint8_t>& v, uint8_t count) const {
  // Clean target vector
  v.clear();
  // Loop until required count is complete or the source is exhausted
  while (index < MM_data.size() && count--) {
    v.push_back(MM_data[index++]);
  }
  return index;
}

// Data validation methods for the different factory calls
// 0. serverID and function code - used by all of the below
Error ModbusMessage::checkServerFC(uint8_t serverID, uint8_t functionCode) {
  if (serverID == 0)      return INVALID_SERVER;   // Broadcast - not supported here
  if (serverID > 247)     return INVALID_SERVER;   // Reserved server addresses
  if (FCT::getType(functionCode) == FCILLEGAL)  return ILLEGAL_FUNCTION; // FC 0 does not exist
  return SUCCESS;
}

// 1. no additional parameter (FCs 0x07, 0x0b, 0x0c, 0x11)
Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode) {
  LOG_V("Check data #1\n");
  Error returnCode = checkServerFC(serverID, functionCode);
  if (returnCode == SUCCESS)
  {
    FCType ft = FCT::getType(functionCode);
    if (ft != FC07_TYPE && ft != FCUSER && ft != FCGENERIC) {
      returnCode = PARAMETER_COUNT_ERROR;
    }
  }
  return returnCode;
}

// 2. one uint16_t parameter (FC 0x18)
Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1) {
  LOG_V("Check data #2\n");
  Error returnCode = checkServerFC(serverID, functionCode);
  if (returnCode == SUCCESS)
  {
    FCType ft = FCT::getType(functionCode);
    if (ft != FC18_TYPE && ft != FCUSER && ft != FCGENERIC) {
      returnCode = PARAMETER_COUNT_ERROR;
    }
  }
  return returnCode;
}

// 3. two uint16_t parameters (FC 0x01, 0x02, 0x03, 0x04, 0x05, 0x06)
Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2) {
  LOG_V("Check data #3\n");
  Error returnCode = checkServerFC(serverID, functionCode);
  if (returnCode == SUCCESS)
  {
    FCType ft = FCT::getType(functionCode);
    if (ft != FC01_TYPE && ft != FCUSER && ft != FCGENERIC) {
      returnCode = PARAMETER_COUNT_ERROR;
    } else {
      switch (functionCode) {
      case 0x01:
      case 0x02:
        if ((p2 > 0x7d0) || (p2 == 0)) returnCode = PARAMETER_LIMIT_ERROR;
        break;
      case 0x03:
      case 0x04:
        if ((p2 > 0x7d) || (p2 == 0)) returnCode = PARAMETER_LIMIT_ERROR;
        break;
      case 0x05:
        if ((p2 != 0) && (p2 != 0xff00)) returnCode = PARAMETER_LIMIT_ERROR;
        break;
      }
    }
  }
  return returnCode;
}

// 4. three uint16_t parameters (FC 0x16)
Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint16_t p3) {
  LOG_V("Check data #4\n");
  Error returnCode = checkServerFC(serverID, functionCode);
  if (returnCode == SUCCESS)
  {
    FCType ft = FCT::getType(functionCode);
    if (ft != FC16_TYPE && ft != FCUSER && ft != FCGENERIC) {
      returnCode = PARAMETER_COUNT_ERROR;
    } 
  }
  return returnCode;
}

// 5. two uint16_t parameters, a uint8_t length byte and a uint16_t* pointer to array of words (FC 0x10)
Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint16_t *arrayOfWords) {
  LOG_V("Check data #5\n");
  Error returnCode = checkServerFC(serverID, functionCode);
  if (returnCode == SUCCESS)
  {
    FCType ft = FCT::getType(functionCode);
    if (ft != FC10_TYPE && ft != FCUSER && ft != FCGENERIC) {
      returnCode = PARAMETER_COUNT_ERROR;
    } else {
      if ((p2 == 0) || (p2 > 0x7b)) returnCode = PARAMETER_LIMIT_ERROR;
      else if (count != (p2 * 2)) returnCode = ILLEGAL_DATA_VALUE;
    }
  }
  return returnCode;
}

// 6. two uint16_t parameters, a uint8_t length byte and a uint16_t* pointer to array of bytes (FC 0x0f)
Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint8_t *arrayOfBytes) {
  LOG_V("Check data #6\n");
  Error returnCode = checkServerFC(serverID, functionCode);
  if (returnCode == SUCCESS)
  {
    FCType ft = FCT::getType(functionCode);
    if (ft != FC0F_TYPE && ft != FCUSER && ft != FCGENERIC) {
      returnCode = PARAMETER_COUNT_ERROR;
    } else {
      if ((p2 == 0) || (p2 > 0x7b0)) returnCode = PARAMETER_LIMIT_ERROR;
      else if (count != ((p2 / 8 + (p2 % 8 ? 1 : 0)))) returnCode = ILLEGAL_DATA_VALUE;
    }
  }
  return returnCode;
}

// 7. generic constructor for preformatted data ==> count is counting bytes!
Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t count, uint8_t *arrayOfBytes) {
  LOG_V("Check data #7\n");
  Error returnCode = checkServerFC(serverID, functionCode);
  if (returnCode == SUCCESS)
  {
    FCType ft = FCT::getType(functionCode);
    if (ft != FCUSER && ft != FCGENERIC) {
      returnCode = PARAMETER_COUNT_ERROR;
    } 
  }
  return returnCode;
}

// Factory methods to create valid Modbus messages from the parameters
// 1. no additional parameter (FCs 0x07, 0x0b, 0x0c, 0x11)
Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode) {
  // Check parameter for validity
  Error returnCode = checkData(serverID, functionCode);
  // No error? 
  if (returnCode == SUCCESS)
  {
    // Yes, all fine. Create new ModbusMessage
    MM_data.reserve(2);
    MM_data.shrink_to_fit();
    MM_data.clear();
    add(serverID, functionCode);
  }
  return returnCode;
}

// 2. one uint16_t parameter (FC 0x18)
Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1) {
  // Check parameter for validity
  Error returnCode = checkData(serverID, functionCode, p1);
  // No error? 
  if (returnCode == SUCCESS)
  {
    // Yes, all fine. Create new ModbusMessage
    MM_data.reserve(4);
    MM_data.shrink_to_fit();
    MM_data.clear();
    add(serverID, functionCode, p1);
  }
  return returnCode;
}

// 3. two uint16_t parameters (FC 0x01, 0x02, 0x03, 0x04, 0x05, 0x06)
Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2) {
  // Check parameter for validity
  Error returnCode = checkData(serverID, functionCode, p1, p2);
  // No error? 
  if (returnCode == SUCCESS)
  {
    // Yes, all fine. Create new ModbusMessage
    MM_data.reserve(6);
    MM_data.shrink_to_fit();
    MM_data.clear();
    add(serverID, functionCode, p1, p2);
  }
  return returnCode;
}

// 4. three uint16_t parameters (FC 0x16)
Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint16_t p3) {
  // Check parameter for validity
  Error returnCode = checkData(serverID, functionCode, p1, p2, p3);
  // No error? 
  if (returnCode == SUCCESS)
  {
    // Yes, all fine. Create new ModbusMessage
    MM_data.reserve(8);
    MM_data.shrink_to_fit();
    MM_data.clear();
    add(serverID, functionCode, p1, p2, p3);
  }
  return returnCode;
}

// 5. two uint16_t parameters, a uint8_t length byte and a uint16_t* pointer to array of words (FC 0x10)
Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint16_t *arrayOfWords) {
  // Check parameter for validity
  Error returnCode = checkData(serverID, functionCode, p1, p2, count, arrayOfWords);
  // No error? 
  if (returnCode == SUCCESS)
  {
    // Yes, all fine. Create new ModbusMessage
    MM_data.reserve(7 + count * 2);
    MM_data.shrink_to_fit();
    MM_data.clear();
    add(serverID, functionCode, p1, p2);
    add(count);
    for (uint8_t i = 0; i < (count >> 1); ++i) {
      add(arrayOfWords[i]);
    }
  }
  return returnCode;
}

// 6. two uint16_t parameters, a uint8_t length byte and a uint8_t* pointer to array of bytes (FC 0x0f)
Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint8_t *arrayOfBytes) {
  // Check parameter for validity
  Error returnCode = checkData(serverID, functionCode, p1, p2, count, arrayOfBytes);
  // No error? 
  if (returnCode == SUCCESS)
  {
    // Yes, all fine. Create new ModbusMessage
    MM_data.reserve(7 + count);
    MM_data.shrink_to_fit();
    MM_data.clear();
    add(serverID, functionCode, p1, p2);
    add(count);
    for (uint8_t i = 0; i < count; ++i) {
      add(arrayOfBytes[i]);
    }
  }
  return returnCode;
}

// 7. generic constructor for preformatted data ==> count is counting bytes!
Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t count, uint8_t *arrayOfBytes) {
  // Check parameter for validity
  Error returnCode = checkData(serverID, functionCode, count, arrayOfBytes);
  // No error? 
  if (returnCode == SUCCESS)
  {
    // Yes, all fine. Create new ModbusMessage
    MM_data.reserve(2 + count);
    MM_data.shrink_to_fit();
    MM_data.clear();
    add(serverID, functionCode);
    for (uint8_t i = 0; i < count; ++i) {
      add(arrayOfBytes[i]);
    }
  }
  return returnCode;
}

// 8. Error response generator
Error ModbusMessage::setError(uint8_t serverID, uint8_t functionCode, Error errorCode) {
  // No error checking for server ID or function code here, as both may be the cause for the message!? 
  MM_data.reserve(3);
  MM_data.shrink_to_fit();
  MM_data.clear();
  add(serverID, static_cast<uint8_t>((functionCode | 0x80) & 0xFF), static_cast<uint8_t>(errorCode));
  return SUCCESS;
}

// Error output in case a message constructor will fail
void ModbusMessage::printError(const char *file, int lineNo, Error e, uint8_t serverID, uint8_t functionCode) {
  LOG_E("(%s, line %d) Error in constructor: %02X - %s (%02X/%02X)\n", file_name(file), lineNo, e, (const char *)(ModbusError(e)), serverID, functionCode);
}

uint8_t ModbusMessage::floatOrder[] = { 0xFF };
uint8_t ModbusMessage::doubleOrder[] = { 0xFF };