#include "poll_modbus.h"

#include <math.h>
#define RESSZMAX (MODBUS_HDRSZ_MBAP+PDUSZ_MAX)

struct per_dev_t {
  uint8_t request[REQSZ];
  int subreq; /* number of subrequsts needed */
  uint8_t *response;
  struct timespec to;
  int flag;
  int cntpkg;
};

size_t summing(int sz, uint16_t *arr) {
  int ix;
  size_t sum = 0;
  for (ix = 0; ix < sz; ++ix) sum += arr[ix];
  return sum;
}

int run(int devsz, struct net_state *netstate, uint16_t *starting_address, uint16_t *quantity) {
  int ix;
  unsigned count;
  struct timespec tfail = { 10, 0}; /* when failure, wait this time */
  struct timespec topen = {  1, 0}; /* when open new, wait this time before */
  struct timespec nxt;
  struct per_dev_t dev[devsz];
  struct proc_data_t proc_data[devsz];
  int submax = 0;

  const size_t datasz = summing(devsz, quantity);
  uint16_t data[datasz];

  int (*process_data)(size_t sz, struct proc_data_t *proc_data, struct timespec *timeout) = process_data_print;

  if (g_opt.dl_proc) process_data = g_opt.dl_proc;

  /* init dev[] */
  memset(dev, 0, sizeof(struct per_dev_t)*devsz);
  memset(proc_data, 0, sizeof(struct proc_data_t)*devsz);
  {
    size_t offs = 0;
    for (ix = 0; ix < devsz; ++ix) {
      proc_data[ix].fd = -1;
      proc_data[ix].host = netstate[ix].host;
      proc_data[ix].starting_address = starting_address[ix];
      proc_data[ix].quantity = quantity[ix];
      /*proc_data[ix].tid = 0 by memset above */
      proc_data[ix].data = data + offs;
      offs += quantity[ix];
      if (netstate[ix].hints.ai_socktype == SOCK_DGRAM && starting_address[ix] == 0 && quantity[ix] == 8) {
	/* unfortunately I have to do 8 single register reads for the Raditex ipio-module v.2 */
	uint8_t reqt[] = {
	  MODBUS_INITMBAP( proc_data[ix].tid, 6, 0xff),
	  MODBUS_INITREQREAD( 0x03, 0, 1),
	};
	memcpy(dev[ix].request, reqt, sizeof(reqt));
	dev[ix].subreq = 8;
      } else {
	uint8_t reqt[] = {
	  MODBUS_INITMBAP( proc_data[ix].tid, 6, 0xff),
	  MODBUS_INITREQREAD( 0x03, starting_address[ix], quantity[ix]),
	};
	memcpy(dev[ix].request, reqt, sizeof(reqt));
	dev[ix].subreq = 1;
      }
      LOG_IFERRNO( (dev[ix].response = malloc(sizeof(uint8_t) * RESSZ(quantity[ix]))) == NULL, LOG_ERR) return -1;
      /* .to = { 0, 0 } by memset */
      /* .flag = 0 by memset */
    }
  }
  submax = 8;

  LOG_IF( timespec_align_to(1), LOG_ERR) { };
  nxt.tv_nsec = 0;
  nxt.tv_sec = timespec_current.tv_sec;
  if (timespec_current.tv_nsec > 800*1000*1000) nxt.tv_sec++;

  for (count = 0; !g_opt.count || count < g_opt.count ; ) {
    nxt = timespec_add(nxt, g_opt.interval);
    uint16_t reqnum;
    int cont = 1;

    for (ix = 0; ix < devsz; ++ix) {
      int kx;
      dev[ix].cntpkg = 0;
      dev[ix].flag = 1;
      for (kx = 0; kx < quantity[ix]; ++kx) proc_data[ix].data[kx] = 0xffff;

      if (timespec_cmp(timespec_current, dev[ix].to) < 0) continue;
      if (proc_data[ix].fd < 0) {
	LOG_IF( (proc_data[ix].fd = netclient_nxt(netstate+ix)) < 0, LOG_NOTICE) {
	  dev[ix].to = timespec_add(nxt, tfail);
	  proc_data[ix].fd = -1;
	  continue;
	}
	dev[ix].to = timespec_add(nxt, topen);
	DEBUG("host %s, ix %d, fd %d", netstate[ix].host, ix, proc_data[ix].fd);
	continue;
      }
      dev[ix].flag = 1;
    }

    for (reqnum = 0; reqnum < submax+2 && cont; ++reqnum) {
      cont = 0;
      for (ix = 0; ix < devsz; ++ix) {
	int cntio;

	if (reqnum >= dev[ix].subreq) continue;
	if (dev[ix].flag == 0) continue;

	dev[ix].request[0] = (proc_data[ix].tid+reqnum) >> 8;
	dev[ix].request[1] = (proc_data[ix].tid+reqnum) & 0xff;
	if (dev[ix].subreq != 1) dev[ix].request[MODBUS_HDRSZ_MBAP+2] = reqnum;
	LOG_IF( (cntio = write( proc_data[ix].fd, dev[ix].request, REQSZ)) < 0 || (size_t) cntio != REQSZ, LOG_ERR) {
	  /* TODO, but no problem yet */
	}
      }
      for (ix = 0; ix < devsz; ++ix) {
	int to;
	int cntio;
	uint8_t response[RESSZMAX];
      read_redo:

	if (dev[ix].cntpkg >= dev[ix].subreq) continue;
	if (dev[ix].flag == 0) continue;

	if (reqnum >= dev[ix].subreq) to = 50;
	else to = 20;
	LOG_VIF( (cntio = run_read(proc_data[ix].fd, RESSZMAX, response, to)) < 0, LOG_ERR, " ix (%d)", ix) {
	  if (netstate[ix].hints.ai_socktype == SOCK_STREAM) {
	    LOG_IFERRNO( close(proc_data[ix].fd) < 0, LOG_ERR) {
	    }
	    proc_data[ix].fd = -1;
	  } else {
	    /* TODO, what to do about UDP in this case, nothing? */
	  }
	} else if (cntio == 0) {
	  /* ignore timeouts, we'll try to catch up later */
	} else {
	  int itid;
	  uint8_t *pdu = response + MODBUS_HDRSZ_MBAP;
	  int offs;
	  int kx;
	  uint16_t tid;
	  int wanted = quantity[ix]/dev[ix].subreq;
	  uint16_t len = response[4]<<8 | response[5];

	  LOG_VIF( (itid = modbus_mbapcheck(cntio, response, 0xff)) == -1, LOG_ERR, " cntio (%d), len (%hd)", cntio, len) {
	    char buf[100];
	    char *bufp = buf;
	    size_t blen = 100;
	    const int blanks[] = { 7, 0 };
	    byte2hex(&bufp, &blen, response, cntio, blanks, 0);
	    NOTICE("%s", buf);
	    continue;
	  }
	  LOG_IF(itid < 0 || itid > UINT16_MAX, LOG_ERR) continue;
	  tid = itid;
	  LOG_VIF(tid - proc_data[ix].tid >= dev[ix].subreq, LOG_NOTICE, " got stray data, ix (%d) tid (%hu) .tid (%hu) .subreq (%hu)", ix, tid, proc_data[ix].tid, dev[ix].subreq) continue;
	  if (pdu[0] == 0x06) continue; /* TODO some other part might send a write_single_register */
	  LOG_VIF(pdu[0] != 0x03, LOG_ERR, " got wrong function code 0x%02hhx", pdu[0]) continue;
	  LOG_VIF(pdu[1] != 2*wanted, LOG_ERR, " got wrong number of data, ix (%d) byte count (%hhu) wanted (%d)", ix, pdu[1], wanted) continue;

	  offs = tid - proc_data[ix].tid;
	  LOG_VIF( offs < 0, LOG_ERR, " dropping old packet, ix (%d) tid (%hu) .tid (%hu)", ix, tid, proc_data[ix].tid) goto read_redo;
	  LOG_VIF( tid - proc_data[ix].tid >= (uint16_t) dev[ix].subreq, LOG_ERR, " ix (%d) tid (%hu) .tid (%hu)", ix, tid, proc_data[ix].tid) continue;
	  memcpy(dev[ix].response, response, cntio); /* response ok, copy it for backup */
	  for (kx = 0; kx < wanted; ++kx) {
	    proc_data[ix].data[kx + offs] = ((uint16_t)pdu[2*kx+2]) << 8 | pdu[2*kx+3];
	  }
	  dev[ix].cntpkg++;
	}
	if (dev[ix].cntpkg < dev[ix].subreq) cont = 1;
      }
    }
    for (ix = 0; ix < devsz; ++ix) {
      proc_data[ix].tid += dev[ix].subreq;
      LOG_VIF(dev[ix].cntpkg != dev[ix].subreq, LOG_ERR, " ix (%d) .cntpkg (%d)", ix, dev[ix].cntpkg) { }
    }

    LOG_IF( (ix = (*process_data)(devsz, proc_data, &nxt)) < 0, LOG_ERR) { }
    if (ix == 0) LOG_IF(timespec_sleep_to(nxt), LOG_ERR) { };
    ++count;
  }

  return 0;
}