// Modbus bridge include
#include "ModbusBridgeWiFi.h"
// Modbus RTU client include
#include "ModbusClientRTU.h"
// Modbus server include
//#include "ModbusServerRTU.h"
#include "ModbusServerWiFi.h"

const uint8_t MY_SERVER(5);
uint16_t memo[128];  
// Worker function for serverID=1, function code 0x03 or 0x04
ModbusMessage FC03(ModbusMessage request) {
  uint16_t addr = 0;        // Start address to read
  uint16_t wrds = 0;        // Number of words to read
  ModbusMessage response;

  // Get addr and words from data array. Values are MSB-first, get() will convert to binary
  request.get(2, addr);
  request.get(4, wrds);
  
  // address valid?
  if (!addr || addr > 128) {
    // No. Return error response
    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_ADDRESS);
    return response;
  }

  // Modbus address is 1..n, memory address 0..n-1
  addr--;

  // Number of words valid?
  if (!wrds || (addr + wrds) > 127) {
    // No. Return error response
    response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_DATA_ADDRESS);
    return response;
  }

  // Prepare response
  response.add(request.getServerID(), request.getFunctionCode(), (uint8_t)(wrds * 2));

  // Loop over all words to be sent
  for (uint16_t i = 0; i < wrds; i++) {
    // Add word MSB-first to response buffer
    response.add(memo[addr + i]);
  }

  // Return the data response
  return response;
}


// Create a ModbusRTU client instance
ModbusClientRTU MB(Serial2, 4);
// Create server
ModbusBridgeWiFi MBbridge;
// Set up a Modbus server
ModbusServerWiFi MBserver;

  // Set RTU Modbus message timeout to 2000ms
  MB.setTimeout(2000);
  MB.onDataHandler(&handleData);

  // Start ModbusRTU background task on core 1
  MB.begin(1);
  //////////////////////////////
  ///////////////////// MBSERVER
  // Register the worker function with the Modbus server
  MBserver.registerWorker(MY_SERVER, READ_HOLD_REGISTER, &FC03);
  
  // Register the worker function again for another FC
  MBserver.registerWorker(MY_SERVER, READ_INPUT_REGISTER, &FC03);
  // Initialize server memory with consecutive values

  // Start the Modbus TCP server:
  // Port number 503, maximum of 4 clients in parallel, 10 seconds timeout
  MBserver.start(503, 4, 5000);

  //////////////////////////////
  ///////////////////// MBSERVER  

  MBbridge.attachServer(1, 1, ANY_FUNCTION_CODE, &MB);
  // Check: print out all combinations served to Serial
  MBbridge.listServer();
  // Start the bridge. Port 502, 4 simultaneous clients allowed, 600ms inactivity to disconnect client
  MBbridge.start(port, 4, 5000);

    for (uint16_t i = 0; i < 128; ++i) {
      memo[i] = SDM630[i];
    }

  for (uint16_t i = 0; i < wrds; i++) {
    // Add word MSB-first to response buffer
    response.add(memo[addr + i]);
  }
  response = MB.syncRequest(0, 1, READ_INPUT_REGISTER, 0, 80);
  //MB.addRequest(0, 1, READ_INPUT_REGISTER, 0, 80);
  // Return the data response
  return response;
}