#include "myModbus.h"
#include <cstdio>
#include <cstring>
#include <thread>
#include <chrono>
#include <atomic>

static int P = 0, F = 0;
#define T(n) do{printf("  %s ... ",n);}while(0)
#define OK() do{printf("OK\n");P++;}while(0)
#define FL(m) do{printf("FAIL: %s\n",m);F++;}while(0)

struct Srv { int port; std::atomic<bool> rdy, stop; std::thread t; };
static void srv_th(Srv *s) {
  modbus_t *m = modbus_new_tcp("127.0.0.1", s->port);
  if (!m) { s->rdy = true; return; } modbus_set_debug(m, 0);
  modbus_mapping_t *mp = modbus_mapping_new(500, 500, 500, 500);
  if (!mp) { modbus_free(m); s->rdy = true; return; }
  for (int i = 0; i < 500; i++) { mp->tab_bits[i] = i % 2; mp->tab_input_bits[i] = (i+1) % 2; mp->tab_registers[i] = i * 10; mp->tab_input_registers[i] = i * 100; }
  modbus_tcp_listen(m, 1); s->rdy = true;
  int cs = -1;
  if (modbus_tcp_accept(m, &cs) == -1) { modbus_mapping_free(mp); modbus_close(m); modbus_free(m); return; }
  uint8_t q[260]; while (!s->stop) { int rc = modbus_receive(m, q); if (rc > 0) modbus_reply(m, q, rc, mp); else if (rc == -1) break; }
  modbus_mapping_free(mp); modbus_close(m); modbus_free(m);
}
static void srv_start(Srv *s) { s->rdy = false; s->stop = false; s->t = std::thread(srv_th, s); while (!s->rdy) std::this_thread::sleep_for(std::chrono::milliseconds(2)); std::this_thread::sleep_for(std::chrono::milliseconds(200)); }
static void srv_stop(Srv *s) { s->stop = true; modbus_t *w = modbus_new_tcp("127.0.0.1", s->port); if (w) { modbus_set_debug(w, 0); modbus_set_response_timeout(w, 0, 100000); modbus_connect(w); modbus_close(w); modbus_free(w); } if (s->t.joinable()) s->t.join(); }
static modbus_t *cli(int port) { modbus_t *c = modbus_new_tcp("127.0.0.1", port); if (c) { modbus_set_debug(c, 0); modbus_set_response_timeout(c, 2, 0); } return c; }
static void with_srv(const char *name, int port, void (*fn)(modbus_t*)) { T(name); Srv s; s.port = port; srv_start(&s); modbus_t *c = cli(port); if (!c) { srv_stop(&s); FL("cli_new"); return; } if (modbus_connect(c)) { modbus_free(c); srv_stop(&s); FL("connect"); return; } fn(c); modbus_close(c); modbus_free(c); srv_stop(&s); std::this_thread::sleep_for(std::chrono::milliseconds(200)); }

static void t01() { T("new_tcp"); modbus_t *c = modbus_new_tcp("127.0.0.1", 502); if (!c) FL("NULL"); else { modbus_free(c); OK(); } }
static void t02() { T("new_rtu"); modbus_t *c = modbus_new_rtu("/dev/ttyS0", 9600, 0x4E, 8, 1); if (!c) FL("NULL"); else { modbus_free(c); OK(); } }
static void t03() { T("slave"); modbus_t *c = modbus_new_tcp("127.0.0.1", 502); if (modbus_set_slave(c, 5) || modbus_get_slave(c) != 5) FL("slv"); else OK(); modbus_free(c); }
static void t04() { T("timeout"); modbus_t *c = modbus_new_tcp("127.0.0.1", 502); modbus_set_response_timeout(c, 2, 500000); uint32_t s = 0, u = 0; modbus_get_response_timeout(c, &s, &u); if (s != 2 || u != 500000) FL("to"); else OK(); modbus_free(c); }
static void t05() { T("hdr_len"); modbus_t *c = modbus_new_tcp("127.0.0.1", 502); if (modbus_get_header_length(c) != 7) FL("hdr"); else OK(); modbus_free(c); }
static void t06() { T("strerror"); const char *es = modbus_strerror(EMBXILFUN); if (!es || !es[0]) FL("se"); else OK(); }
static void t07() { T("quirks"); modbus_t *c = modbus_new_tcp("127.0.0.1", 502); if (modbus_enable_quirks(c, MODBUS_QUIRK_MAX_SLAVE) || modbus_disable_quirks(c, MODBUS_QUIRK_MAX_SLAVE)) FL("qk"); else OK(); modbus_free(c); }
static void t08() { T("pre_sock"); modbus_t *c = modbus_new_tcp("127.0.0.1", 502); if (modbus_get_socket(c) != -1) FL("sock"); else OK(); modbus_free(c); }
static void t09() { T("err_rec"); modbus_t *c = modbus_new_tcp("127.0.0.1", 502); if (modbus_set_error_recovery(c, MODBUS_ERROR_RECOVERY_LINK)) FL("er"); else OK(); modbus_free(c); }
static void t10() { T("debug"); modbus_t *c = modbus_new_tcp("127.0.0.1", 502); if (modbus_set_debug(c, 1) || modbus_set_debug(c, 0)) FL("dbg"); else OK(); modbus_free(c); }
static void fn_connect(modbus_t *c) { (void)c; OK(); }
static void fn_bits(modbus_t *c) { uint8_t b[10] = {0}; if (modbus_read_bits(c, 0, 10, b) != 10) { FL("rb"); return; } if (b[0] != 0 || b[1] != 1) { FL("coil"); return; } uint8_t d[10] = {0}; if (modbus_read_input_bits(c, 0, 10, d) != 10) { FL("di"); return; } if (d[0] != 1 || d[1] != 0) { FL("div"); return; } OK(); }
static void fn_regs(modbus_t *c) { uint16_t r[5] = {0}; if (modbus_read_registers(c, 0, 5, r) != 5) { FL("hr"); return; } if (r[0] != 0 || r[1] != 10 || r[2] != 20) { FL("hrv"); return; } uint16_t i[5] = {0}; if (modbus_read_input_registers(c, 0, 5, i) != 5) { FL("ir"); return; } if (i[0] != 0 || i[1] != 100) { FL("irv"); return; } OK(); }
static void fn_write1(modbus_t *c) { if (modbus_write_bit(c, 5, 1) != 1) { FL("wb"); return; } uint8_t b[10] = {0}; modbus_read_bits(c, 0, 10, b); if (b[5] != 1) { FL("bv"); return; } modbus_write_bit(c, 5, 0); if (modbus_write_register(c, 3, 999) != 1) { FL("wr"); return; } uint16_t r[5] = {0}; modbus_read_registers(c, 0, 5, r); if (r[3] != 999) { FL("rv"); return; } OK(); }
static void fn_writen(modbus_t *c) { uint8_t wb[5] = {1, 0, 1, 0, 1}; if (modbus_write_bits(c, 10, 5, wb) != 5) { FL("wb"); return; } uint8_t rb[5] = {0}; modbus_read_bits(c, 10, 5, rb); if (memcmp(rb, wb, 5)) { FL("bm"); return; } uint16_t wr[5] = {111, 222, 333, 444, 555}; if (modbus_write_registers(c, 5, 5, wr) != 5) { FL("wr"); return; } uint16_t rr[5] = {0}; modbus_read_registers(c, 5, 5, rr); if (memcmp(rr, wr, 10)) { FL("rm"); return; } OK(); }
static void fn_mask(modbus_t *c) { modbus_write_register(c, 2, 0x55AA); modbus_mask_write_register(c, 2, 0xFF00, 0x00FF); uint16_t r[1] = {0}; modbus_read_registers(c, 2, 1, r); if (r[0] != 0x55FF) { printf("(0x%04X) ", r[0]); FL("mk"); return; } OK(); }
static void fn_bad(modbus_t *c) { uint8_t b[1]; uint16_t r[1]; if (modbus_read_bits(c, 999, 1, b) != -1) { FL("bits"); return; } if (modbus_read_registers(c, 999, 1, r) != -1) { FL("regs"); return; } OK(); }
static void t_zero() { T("zero_cnt"); modbus_t *c = modbus_new_tcp("127.0.0.1", 502); uint8_t b[1]; if (modbus_read_bits(c, 0, 0, b) != -1) FL("zero"); else OK(); modbus_free(c); }
static void t_null() { T("NULL_ctx"); if (modbus_set_slave(NULL, 1) != -1) FL("slv"); else if (modbus_read_registers(NULL, 0, 1, NULL) != -1) FL("read"); else OK(); }

int main(void) {
  printf("\n=== libmodbus test ===\n\n");
  t01(); t02(); t03(); t04(); t05(); t06(); t07(); t08(); t09(); t10();
  printf("\n--- loopback ---\n");
  with_srv("connect", 15025, fn_connect);
  with_srv("read_bits+di", 15026, fn_bits);
  with_srv("read_regs", 15027, fn_regs);
  with_srv("write_bit+reg", 15028, fn_write1);
  with_srv("write_multi", 15029, fn_writen);
  with_srv("mask_write", 15030, fn_mask);
  with_srv("bad_addr", 15031, fn_bad);
  printf("\n--- misc ---\n");
  t_zero(); t_null();
  printf("\n========================\n  %d pass, %d fail\n========================\n", P, F);
  return F > 0 ? 1 : 0;
}