#include <aspoutil.h>
#include "poll_modbus.h"

LOG_BUFFER(200)

#define LISTEN_BACKLOG 10
int fd;
#define FSZ 3
int fa[FSZ];
uint32_t addr[FSZ];
int finuse = 0;

int fa_close(int fdx);
int fa_find(int fdx);
int test_accept(void);

/******************/

int fa_find(int fdx) {
  int ix;
  for (ix = 0; ix < finuse; ++ix) {
    if (fa[ix] == fdx) return ix;
  }
  return -1;
}

int fa_close(int fdx) {
  int ix;
  LOG_IFERRNO( close(fdx) == -1, LOG_ERR) return -1;

  for (ix = 0; ix < finuse; ++ix) {
    if (fa[ix] == fdx) break;
  }
  if (ix == finuse) return -1;
  finuse--;
  for (; ix < finuse; ++ix) {
    fa[ix] = fa[ix+1];
    addr[ix] = addr[ix+1];
  }
  return 0;
}

int test_accept(void) {
  struct sockaddr_in sanxt;
  socklen_t len = sizeof(sanxt);
  int fdn;
  LOG_IFERRNO( (fdn = accept(fd, (struct sockaddr *) &sanxt, &len)) == -1, LOG_ERR) {
    return -1;
  }
  if (finuse < FSZ) {
    fa[finuse] = fdn;
    addr[finuse] = ntohl(sanxt.sin_addr.s_addr);
    finuse++;
  } else {
    NOTICE(" incoming call rejected due to lack of space, finuse (%d)", finuse);
    LOG_IFERRNO( close(fdn) == -1, LOG_ERR) return -1;
  }
  return 0;
}

/******************/

int proc_init(const char *arg) {
  struct sockaddr_in sain;

  (void) arg;
  LOG_IFERRNO( (fd = socket(PF_INET, SOCK_STREAM, 0)) == -1, LOG_ERR) {
    return -1;
  }

  {
    int opt = 1;
    LOG_IFERRNO( setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) == -1, LOG_ERR) {
      return -1;
    }
  }

  sain.sin_family = AF_INET;
  sain.sin_port = htons(1938);
  sain.sin_addr.s_addr = INADDR_ANY;
  LOG_IFERRNO( bind(fd, (struct sockaddr *) &sain, sizeof(sain)) == -1, LOG_ERR) {
    return -1;
  }

  LOG_IFERRNO(listen(fd, LISTEN_BACKLOG), LOG_ERR) {
    return -1;
  }

  return 0;
}

int nfds = 0;
fd_set set_read;
fd_set set_write;
fd_set set_wflag; /* have we written to a fd? */

int check_select(struct timespec *wait_time);
int check_write(int sz, const void *buf);

int check_select(struct timespec *wait_time) {
  int ix;

  FD_ZERO(&set_read);
  FD_ZERO(&set_write);
  FD_SET(fd, &set_read);
  nfds = fd;

  for (ix = 0; ix < finuse; ++ix) {
    int fdx = fa[ix];
    FD_SET(fdx, &set_read);
    if (!FD_ISSET(fdx, &set_wflag)) FD_SET(fdx, &set_write);
    if (nfds < fdx) nfds = fdx;
  }
  nfds++; /* i.e. convert from highest valid to count of entries from 0..nfds */

  LOG_VIFERRNO( (ix = pselect(nfds, &set_read, &set_write, NULL, wait_time, NULL)) == -1, LOG_ERR, " fd %d", fd ) {
    return -1;
  }
  if (ix == 0) {
    return 1;
  }
  return 0;
}

int check_write(int sz, const void *buf) {
  int fdx;
  for (fdx = 0; fdx < nfds; ++fdx) { /* writers */
    if (FD_ISSET(fdx, &set_write)) {
      int err;
      LOG_IFERRNO( (err = write(fdx, buf, sz)) == -1, LOG_ERR) {
	if (err == ECONNRESET) {
	  fa_close(fdx);
	} else {
	  return -1;
	}
      }
      FD_SET(fdx, &set_wflag);
    }
  }
  return 0;
}

int check_read(int fdx, int sz, char *buf);
int check_read(int fdx, int sz, char *buf) {
  /*int fdx; */
  int len = 0;

  /*LOG_IF( sz < 12 || buf == NULL, LOG_ERR) return -1; */

  if (fdx == fd) {
    if (test_accept() < 0) return -1;
  } else {
    int err;
    LOG_IFERRNO( (err = read(fdx, buf, sz-1)) == -1, LOG_ERR) {
      if (errno == ECONNRESET) {
	err = 0; /* similate eof */
      } else {
	return -1;
      }
    }
    if (err == 0) {
      fa_close(fdx);
    } else {
      len = err;
    }
  }
  return len;
}

int proc_run (size_t sz, struct proc_data_t *proc_data, struct timespec *timeout) {
#define BUFSZ 100
  char buf[BUFSZ];

  if (sz > 0) {
    snprintf(buf, BUFSZ, "0x%04x hej\n", proc_data->data[0]);
    FD_ZERO(&set_wflag);
  } else {
    buf[0] = '\0';
  }

  while(1) {
    int val;
    int fdx;
    struct timespec diff;

    timespec_update();
    diff = timespec_sub(*timeout, timespec_current);
    if (timespec_cmp(*timeout, timespec_current) <= 0) return 1;

    if ( (val = check_select(&diff)) != 0) return val;
    if ( (val = check_write(strlen(buf), buf)) == -1) return -1;
    for (fdx = 0; fdx < nfds; ++fdx) { /* readers */
      if (FD_ISSET(fdx, &set_read)) {
	if ( (val = check_read(fdx, BUFSZ-11, buf+11)) == -1 ) return -1;
	if (val > 0) {
	  int ix = fa_find(fdx);
	  buf[val+11] = '\0';
	  sprintf(buf, "0x%08x", addr[ix]);
	  buf[10] = ' ';
	  FD_ZERO(&set_wflag);
	  if (strncmp("exit", buf, 4) == 0) {
	    fa_close(fdx);
	  }
	  fputs(buf, stdout);
	}
      }
    }
  }
  return 1;
}

int proc_fini(void) {
  LOG_IFERRNO( close(fd) == -1, LOG_ERR) return 1;
  return 0;
}

/******************/

struct options_t g_opt;

int main(int argc, char *argv[]) {
  struct timespec nxt;
  uint16_t data[1] = { 0 };
  struct proc_data_t pd = { "localhost", 0, 0, 0, 1, data };

  /*struct timespec interval = { 0, 100*1000*1000 }; */
  g_opt.interval.tv_sec  = 1;
  g_opt.interval.tv_nsec = 0;

  (void) argc;
  (void) argv;
  (void) cmdline_extrpgm(argv);
  (void) log_open(NULL, LOG_PRIO | LOG_SIGHUP | 2, 0);

  { int err;
    if ((err = proc_init(NULL))) return err;
  }

  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++;
  LOG_IF(timespec_sleep_to(nxt), LOG_ERR) { };

  while (1) {
    int err;
    nxt = timespec_add(nxt, g_opt.interval);
    LOG_MARK();
    pd.tid++;
    pd.data[0]++;
    LOG_IF( (err = proc_run(1, &pd, &nxt)) == -1, LOG_ERR) { }
    if (err == 0) LOG_IF(timespec_sleep_to(nxt), LOG_ERR) { };
  }

  return proc_fini();
}