#include <errno.h>
#include <stdlib.h>
#include <stdio.h>
#include "sock.h"
#include "server.h"
#define type server_connection
#include "list.h"
#include "co.h"
#include "watch.h"
#include "lock.h"
#include "file.h"
#include "opt.h"

#define Assert(expr,str,...) if (!(expr)) { fprintf(stderr,"assertion failed in file %s, function %s, line %d:\n" str "\n",__FILE__,__FUNCTION__,__LINE__,__VA_ARGS__); exit(3); }

#define NOSIG(expression)			\
  (__extension__				\
   ({ long int __result;			\
     do __result = (long int) (expression);	\
     while (__result == -1L && errno == EINTR);	\
     __result; }))

typedef unsigned char uint8;
typedef unsigned uint32;

// arch dependant
uint8 TotalDioCount;
uint8 DioCapabilities[256];

void DioInit();
void SetDioDir(int dionum,int asOutput);
void SetDio(int dionum,int asHigh);
int GetDioDir(int dionum);
int GetDio(int dionum);

#ifdef CLIENT_ONLY
// simulation
int outval[16],outdir[16];

void DioInit() {
  int i;

  TotalDioCount = 16;
  // simulate 4 ins, 4 outs, and 8 I/Os 
  for (i=0;i<4;i++) {
    DioCapabilities[i] = 1;
  }
  for (i=4;i<8;i++) {
    DioCapabilities[i] = 6;
  }
  for (i=8;i<16;i++) {
    DioCapabilities[i] = 7;
  }
  outval[4] = 1;
  outval[5] = 1;
  outval[6] = 1;
  outval[7] = 1;
}

// emulate pull-ups on 4 output onlys, and pull-downs on 8 I/Os
void SetDioDir(int dionum,int asOutput) {
  if (dionum >= 4 && dionum < 16) {
    outdir[dionum] = asOutput;
    if (!asOutput) outval[dionum] = (dionum < 8); 
  }
}

void SetDio(int dionum,int asHigh) {
  if (dionum < 4) return;
  if (dionum < 16) {
    if (outdir[dionum]) {
      outval[dionum] = asHigh;
    }
  }
}

int GetDioDir(int dionum) {  // 1 = is output
  if (dionum >= 4 && dionum < 16) {
    return outdir[dionum];
  } else {
    return 0;
  }
}

// input0 <- output 4
// input1 <- output 5
// input2 <- output 6
// input3 <- output 7
int GetDio(int dionum) {    // 1 = reading high
  if (dionum < 0 && dionum >= 16) {
    return 0;
  }
  if (dionum < 4) {
    return outval[dionum+4];
  }
  return outval[dionum];
}
#endif
 

typedef struct server_data server_data;
struct server_data {
  server_connection *head;
};

typedef struct instance_data instance_data;

struct instance_data {
  void *context;
  int maplen[16];
  int *diomap[16];
};

instance_data *new_instance_data() {
  instance_data *inst_data = malloc(sizeof(instance_data));
  int i;

  inst_data->context = 0;
  for (i=0;i<16;i++) {
    inst_data->maplen[i] = 0;
    inst_data->diomap[i] = 0;
  }
  return inst_data;
}

void delete_instance_data(instance_data *inst) {
  if (inst) {
    if (inst->context) {
      free(inst->context);
    }
    free(inst);
  }
}

#define WAIT(x) while (!(x)) { coReturn(0); }
#define LOCK(id,res) while (!lock_request(id,res)) { coReturn(0); }
#define MULTILOCK(id,len,map) while (!lock_request_multiple(id,len,map)) { coReturn(0); }
#define UNLOCK(id,res) lock_release(id,res);

uint8 DioValue[256];

int server_processing(coParm,server_connection *conn) {
  coBeginContext;
  uint8 *dir,*val;
  unsigned char cmd,byte,dionum,mapnum,numdios;
  int i;
  coEndContext;

  instance_data *inst_data = conn->inst_data;

  coBegin {
    WAIT(ring_read_byte(conn->rx,&co(cmd)));
    switch(co(cmd)) {
    case 0: // Get DIO Capabilities
      WAIT(ring_write_byte(conn->tx,TotalDioCount));
      for (co(i)=0;co(i)<TotalDioCount;co(i)++) {
	WAIT(ring_write_byte(conn->tx,DioCapabilities[co(i)]));
      }
      break;
    case 1: // Set Single DIO State to Input
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(dionum) = co(byte);
      LOCK(conn->s,co(dionum));
      SetDioDir(co(dionum),0);
      UNLOCK(conn->s,co(dionum));
      break;
    case 2: // Set Single DIO State to Output High
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(dionum) = co(byte);
      LOCK(conn->s,co(dionum));
      SetDioDir(co(dionum),1);
      SetDio(co(dionum),1);
      UNLOCK(conn->s,co(dionum));
      break;
    case 3: // Set Single DIO State to Output Low
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(dionum) = co(byte);
      LOCK(conn->s,co(dionum));
      SetDioDir(co(dionum),1);
      SetDio(co(dionum),0);
      UNLOCK(conn->s,co(dionum));
      break;
    case 4: // Get Single DIO State
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(dionum) = co(byte);
      co(byte) = (GetDioDir(co(dionum)) << 7) + (GetDio(co(dionum)) << 6);
      WAIT(ring_write_byte(conn->tx,co(byte)));
      break;
    case 5: // Define DIO Group Mapping
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(mapnum) = co(byte) >> 4;
      co(numdios) = 1 + (co(byte) & 0xF);
      if (inst_data->diomap[co(mapnum)]) {
	free(inst_data->diomap[co(mapnum)]);
      }
      inst_data->maplen[co(mapnum)] = co(numdios);
      inst_data->diomap[co(mapnum)] = malloc(co(numdios) * sizeof(int));
      for (co(i)=0;co(i)<co(numdios);co(i)++) {
	WAIT(ring_read_byte(conn->rx,&co(byte)));
	inst_data->diomap[co(mapnum)][co(i)] = co(byte);
      }
      break;
    case 6: // Set DIO Group to Input
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(mapnum) = co(byte) >> 4;
      MULTILOCK(conn->s,inst_data->maplen[co(mapnum)],inst_data->diomap[co(mapnum)]);
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	SetDioDir(inst_data->diomap[co(mapnum)][co(i)],0);
      }
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	UNLOCK(conn->s,inst_data->diomap[co(mapnum)][co(i)]);
      }
      break;
    case 7: // Set DIO Group to Output High
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(mapnum) = co(byte) >> 4;
      MULTILOCK(conn->s,inst_data->maplen[co(mapnum)],inst_data->diomap[co(mapnum)]);
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	SetDioDir(inst_data->diomap[co(mapnum)][co(i)],1);
	SetDio(inst_data->diomap[co(mapnum)][co(i)],1);
      }
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	UNLOCK(conn->s,inst_data->diomap[co(mapnum)][co(i)]);
      }
      break;
    case 8: // Set DIO Group to Output Low
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(mapnum) = co(byte) >> 4;
      MULTILOCK(conn->s,inst_data->maplen[co(mapnum)],inst_data->diomap[co(mapnum)]);
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	SetDioDir(inst_data->diomap[co(mapnum)][co(i)],1);
	SetDio(inst_data->diomap[co(mapnum)][co(i)],0);
      }
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	UNLOCK(conn->s,inst_data->diomap[co(mapnum)][co(i)]);
      }
      break;
    case 9: // Set DIO Group States
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(mapnum) = co(byte) >> 4;
      // get all the states first before setting anything
      // in case we don't have enough data to do it all
      co(dir) = malloc(inst_data->maplen[co(mapnum)]);
      co(val) = malloc(inst_data->maplen[co(mapnum)]);
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	if (co(i) % 4 == 0) WAIT(ring_read_byte(conn->rx,&co(byte)));
	co(dir)[co(i)] = (co(byte) >> 7) & 1;
	co(val)[co(i)] = (co(byte) >> 6) & 1;
	co(byte) <<= 2;
      }
      MULTILOCK(conn->s,inst_data->maplen[co(mapnum)],inst_data->diomap[co(mapnum)]);
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	SetDioDir(inst_data->diomap[co(mapnum)][co(i)],co(dir[co(i)]));
	SetDio(inst_data->diomap[co(mapnum)][co(i)],co(val[co(i)]));
      }
      free(co(dir));
      free(co(val));
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	UNLOCK(conn->s,inst_data->diomap[co(mapnum)][co(i)]);
      }
      break;
    case 10: // Get DIO Group States
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(mapnum) = co(byte) >> 4;
      co(byte) = 0;
      // first, write the number of states we are going to send back
      WAIT(ring_write_byte(conn->tx,inst_data->maplen[co(mapnum)]));
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	co(byte) |= ((!!GetDioDir(inst_data->diomap[co(mapnum)][co(i)])) << (7-2*(co(i)%4)));
	co(byte) |= !!GetDio(inst_data->diomap[co(mapnum)][co(i)]) << (6-2*(co(i)%4));
	// might it be better to read everything to a buffer first?
	// that way, if ring_write_byte blocks, we at least have the state
	// of the DIOs all at the same time.
	if ((co(i) % 4 == 3) || (co(i)+1==inst_data->maplen[co(mapnum)])) {
	  WAIT(ring_write_byte(conn->tx,co(byte)));
	  co(byte) = 0;
	}
      }
      break;
    case 11: // Aquire DIO Group Lock (Blocking)
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(mapnum) = co(byte) >> 4;
      MULTILOCK(conn->s,inst_data->maplen[co(mapnum)],inst_data->diomap[co(mapnum)]);
      break;
    case 12: // Release DIO Group Lock
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(mapnum) = co(byte) >> 4;
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	UNLOCK(conn->s,inst_data->diomap[co(mapnum)][co(i)]);
      }
      break;
    case 13: // Aquire DIO Group Lock or abort request if unavailable
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(mapnum) = co(byte) >> 4;
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	if (lockedQ(inst_data->diomap[co(mapnum)][co(i)])) {
	  break;
	}
      }
      if (co(i) < inst_data->maplen[co(mapnum)]) {
	WAIT(ring_write_byte(conn->tx,0x5A));
      } else {
	MULTILOCK(conn->s,inst_data->maplen[co(mapnum)],inst_data->diomap[co(mapnum)]);
	WAIT(ring_write_byte(conn->tx,0x77));
      }
      break;
    case 14: // block until any DIO in Group is value specified for it
      WAIT(ring_read_byte(conn->rx,&co(byte)));
      co(mapnum) = co(byte) >> 4;
      // get all the states first before setting anything
      // in case we don't have enough data to do it all yet
      co(val) = malloc(inst_data->maplen[co(mapnum)]);

      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	if (co(i) % 8 == 0) WAIT(ring_read_byte(conn->rx,&co(byte)));
	co(val)[co(i)] = (co(byte) >> 7) & 1;
	co(byte) <<= 1;
      }
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	request_watch(conn->s,inst_data->diomap[co(mapnum)][co(i)],co(val)[co(i)]);
      }
      // now wait
      do {
	co(byte) = 0;
	for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	  if (DioValue[inst_data->diomap[co(mapnum)][co(i)]] == co(val)[co(i)]) {
	    co(byte)++;
	  }
	}
	if (co(byte) == 0) coReturn(0);
      } while (co(byte) == 0);
      // now get all the values in the group
      // FIX ME: we have no way on the receive side to know how much data
      // is being sent back!
      co(dir) = malloc(co(byte));
      co(byte) = 0;
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	if (DioValue[inst_data->diomap[co(mapnum)][co(i)]] == co(val)[co(i)]) {
	  co(dir)[co(byte)++] = co(i);
	}
      }
      WAIT(ring_write_byte(conn->tx,co(byte)));
      for (co(i)=0;co(i) < co(byte);co(i)++) {
	WAIT(ring_write_byte(conn->tx,co(i)));
      }
      // release values and watches
      free(co(dir));
      free(co(val));
      for (co(i)=0;co(i)<inst_data->maplen[co(mapnum)];co(i)++) {
	release_watch(conn->s);
      }
      break;
    case 15: // block until specified DIO is the specified value
      // TO DO
      break;
    case 16: // lock single DIO (blocking)
      // TO DO
      break;
    case 17: // unlock single DIO
      // TO DO
      break;
    case 18: // lock single DIO (non-blocking)
      // TO DO
      break;
    default:
      // to do: release all locks and close the connection
      break;
    }
  } coEnd;
  return 1;
}

int server_frontend(server_connection *conn,void *data) {
  server_data *svr_data = data;
  int i,val;
  
  if (!conn) { // timeout
#if 0
    for (i=0;i<TotalDioCount;i++) {
      val = GetDio(i);
      if (val != DioValue[i]) {
	DioValue[i] = val;
	watch_update_resource(i,val);
      }
    }
#endif
  } else if (!data) { // exception
  } else { // normal service
    if (conn == svr_data->head) { // new connection request
      struct sockaddr_in csa;
      unsigned size_csa = sizeof(csa);

      i = NOSIG(accept(conn->s, (struct sockaddr *)&csa, &size_csa));
      fileSetBlocking(i,0);
      if (i >= 0) {
	instance_data *inst_data = new_instance_data();
	server_connection *newconn;
	newconn = new_server_connection(i,inst_data,256,256);
	server_connection_Queue(&svr_data->head,newconn);
      }
    } else { // client request
      instance_data *inst_data = conn->inst_data;
      while(server_processing(&inst_data->context,conn));
    }
  }
  return 1;
}


void run_dioctl_server(int port) {
  server_data svr_data;
  int s;
  instance_data *inst_data = new_instance_data();

  s = create_server_socket(port);
  svr_data.head = new_server_connection(s,inst_data,1,0);

  server(server_frontend,&svr_data,&svr_data.head,10,delete_instance_data);
}

//==================================================================
// CLIENT FUNCTIONS
struct diocaps {
  unsigned char numdios;
  unsigned char caps[];
};

#define TxByte(val) fputc(val,f)
#define RxByte(ch) { fflush(f); int ch2 = fgetc(f); if (ch2 < 0) return FAILURE; ch=ch2; }
#define RxData(buf,len,dest) { fflush(f); if (fread(buf,len,1,f) != 1) { dest; return FAILURE; } }

#define FAILURE 0
struct diocaps *dioctl_GetDIOCapabilities(FILE *f) {
  int numdios;
  struct diocaps *caps;

  TxByte(0); // Get DIO Capabilities
  RxByte(numdios);
  caps = malloc(1+numdios);
  caps->numdios = numdios;
  RxData(&caps->caps,numdios,free(caps));
  return caps;
}

#undef FAILURE
void dioctl_SetDIO(FILE *f,int num,int asOutput,int drive) {
  TxByte(asOutput ? (drive ? 2 : 3) : 1);  // Set Single DIO State
  TxByte(num);
}

// return:
// 0 - DIO is an input, reads LOW
// 1 - DIO is an input, reads HIGH
// 2 - DIO is an output, reads LOW
// 3 - DIO is an output, reads HIGH
#define FAILURE -1
int dioctl_GetDIO(FILE *f,int num) {
  int ch;

  TxByte(4); // Get Single DIO State
  TxByte(num);
  RxByte(ch);
  return ch >> 6;
}

#undef FAILURE
void dioctl_DefineGroup(FILE *f,int groupNum,int numDios,int *dios) {
  int i;

  if (groupNum < 0 || groupNum > 15) return;
  if (numDios < 0 || numDios > 16) return;

  TxByte(5); // Define DIO Group Mapping
  TxByte((groupNum << 4) + (numDios-1));
  for (i=0;i<numDios;i++) {
    TxByte(dios[i]);
  }
}

void dioctl_SetGroup(FILE *f,int groupNum,int asOutput,int drive) {
  TxByte(asOutput ? (drive ? 7 : 8) : 6);
  TxByte(groupNum << 4);
}

void dioctl_SetDIOStates(FILE *f,int groupNum,int numDios,int *dir,int *val) {
  int i;
  char byte=0;

  TxByte(9); // Set DIO Group States
  TxByte(groupNum << 4);
  for (i=0;i<numDios;i++) {
    byte |= ((dir[i]?1:0) << (7-2*(i%4)));
    byte |= ((val[i]?1:0) << (6-2*(i%4)));
    if (i % 4 == 3 || i+1 == numDios) {
      TxByte(byte);
      byte = 0;
    }
  }
}

// Idea: 
// for the callback we could pass a closure. create the closure by
// defining a function inside the function, let it declare a static
// (or malloced on demand?) variable, then pass certain values to
// initialize and clear it, and dump out its contents. kinda like
// a mini-server.
#define FAILURE
void dioctl_GetDIOGroup(FILE *f,int num,void (*callback)(int,int)) {
  int i,numdios,len,byte;

  TxByte(10); // Get DIO Group States
  TxByte(num << 4);
  RxByte(numdios);
  len = 1 + (numdios / 4) + (numdios % 4) ? 1 : 0;
  for (i=0;i<numdios;i++) {
    if (i % 4 == 0) {
      RxByte(byte);
    }
    callback(i,byte >> 6);
    byte = (byte << 2) & 0xFF;
  }
}

#undef FAILURE
void dioctl_LockGroup(FILE *f,int groupNum) {
  TxByte(11);
  TxByte(groupNum << 4);
}

void dioctl_UnlockGroup(FILE *f,int groupNum) {
  TxByte(12); // Release DIO Group Lock
  TxByte(groupNum << 4);
}

#define FAILURE 0
int dioctl_TryLockGroup(FILE *f,int groupNum) {
  int result;

  TxByte(13); // Try to Aquire DIO Group Lock (Non-Blocking)
  TxByte(groupNum << 4);
  RxByte(result);
  return result == 0x77;
}

#undef FAILURE
#define FAILURE
// callback(int groupIndex,int value)
void dioctl_WaitDIO(FILE *f,int groupNum,int numdios,int *val,void (*callback)(int,int)) {
  int i,n,byte;
  TxByte(14); // block until any DIO in Group is value specified for it
  TxByte(groupNum << 4);
  for (i=0;i<numdios;i++) {
    byte |= ((val[i]?1:0) << 7-(i%8));
    if (i%8 == 7 || i+1==numdios) TxByte(byte);
  }
  RxByte(n);
  for (i=0;i<n;i++) {
    RxByte(byte);
    callback(byte,val[byte]);
  }
}



// TO CONSIDER:
// map groups and DIOs into the same address space. for example,
// let DIOs 240-255 represent a whole group.  then we can uniformly
// operate on both groups and individual dios. the only problem
// with this is the expectation of how much data gets returned!
// OLD FUNCTIONS:

int opt_int(char *arg,int *target,int opt) {
  if (!arg) {
    *target = 4500;
  } else {
    *target = atoi(arg);
  }
  return 1;
}

char *parse_dec_octets(char *buf,int *n) {
  char *ret,*s;
  int i,len = strlen(buf),count=0;
  *n = 0;
  for (i=0;i<len;i++) {
    if (buf[i] == ':') {
      count++;
    }
  }
  ret = malloc(2+count);
  while (buf) {
    s = strchr(buf,':');
    if (s) {
      *s++ = 0;
    }
    ret[n[0]] = strtoul(buf,0,10);
    n[0]++;
    buf = s;
  }
  return ret;
}

// must call this first:
//   FILE *f = 
// must call this as needed. auto-called before Rx
//   fflush(f); // optional

int opt_client = -1;
FILE *client;
int opt_dioseq(char *arg,unsigned *target,int opt) {
  int i,j;
  char *buf;
  struct diocaps *caps;
  void printDIOvalue(int index,int value) {
    printf("diodirG%d=%d\n",index,(value&2)?1:0);
    printf("diovalG%d=%d\n",index,(value&1)?1:0);
  }
  void printDIOvalue1(int index,int value) {
    printf("diovalG%d=%d\n",index,(value&1)?1:0);
  }

  if (opt_client < 0) {
    fprintf(stderr,"must specify port first!\n");
    return 0;
  }
  if (!client) {
    client = fdopen(opt_client,"a+");
    if (!client) {
      fprintf(stderr,"fdopen:%m");
      exit(1);
    }
  }
  switch (opt) {
  case 'g':
    caps = dioctl_GetDIOCapabilities(client);
    if (!caps) {
      printf("server=0\n");
    } else {
      printf("server=1\n");
      printf("numdios=%d\n",caps->numdios);
      for (i=0;i<caps->numdios;i++) {
	printf("dioCanInput%d=%d\n",i,(caps->caps[i] & 1) != 0);
	printf("dioCanDriveHigh%d=%d\n",i,(caps->caps[i] & 2) == 2);
	printf("dioCanDriveLow%d=%d\n",i,(caps->caps[i] & 4) == 4);
      }
      free(caps);
    }
    break;
  case 'r':
    j = atoi(arg);
    if (j >= 1000) {
      dioctl_GetDIOGroup(client,j-1000,printDIOvalue);
    } else {
      i = dioctl_GetDIO(client,j);
      if (i < 0) {
	printf("server=0\n");
      } else {
	printf("server=1\n");
	printf("diodir%d=%d\n",j,(i & 2)?1:0);
	printf("dioval%d=%d\n",j,(i & 1)?1:0);
      }
    }
    break;
  case '1':
    j = atoi(arg);
    if (j >= 1000) {
      dioctl_SetGroup(client,j-1000,1,1);
    } else {
      dioctl_SetDIO(client,j,1,1);
    }
    break;
  case '0':
    j = atoi(arg);
    if (j >= 1000) {
      dioctl_SetGroup(client,j-1000,1,0);
    } else {
      dioctl_SetDIO(client,j,1,0);
    }
    break;
  case 'i':
    j = atoi(arg);
    if (j >= 1000) {
      dioctl_SetGroup(client,j-1000,0,0);
    } else {
      dioctl_SetDIO(client,j,0,0);
    }
    break;
  case 'G': {
    int *dios;
    buf = parse_dec_octets(arg,&j);
    dios = malloc(sizeof(int) * j);
    for (i=0;i<j-1;i++) {
      dios[i] = buf[i+1];
    }
    dioctl_DefineGroup(client,buf[0],j-1,dios);
    break;
  }
  case 'w': {
    int *vals;
    buf = parse_dec_octets(arg,&j);
    vals = malloc(sizeof(int) * j);
    for (i=0;i<j-1;i++) {
      vals[i] = buf[i+1];
    }
    dioctl_WaitDIO(client,buf[0],j-1,vals,printDIOvalue1);
    break;
  }

  }
  return 1;
}

void testpins(int asHigh,int n,int *p1,int *p2,int *p3) {
  int i,j,err=0;

  // set all primary DIOs to !asHigh, all secondary and tertiary to inputs
  for (i=1;i<n;i++) {
    if (p1[i] != -1) {
      dioctl_SetDIO(client,p1[i],1,!asHigh);
      if (p2[i] != -1) {
	dioctl_SetDIO(client,p2[i],0,0);
	if (p3[i] != -1) {
	  dioctl_SetDIO(client,p3[i],0,0);
	}
      }
    } else { // no primary so set any remaining to !asHigh
      if (p2[i] != -1) dioctl_SetDIO(client,p2[i],1,!asHigh);
      if (p3[i] != -1) dioctl_SetDIO(client,p3[i],0,!asHigh);
    }
  }

  for (i=1;i<n;i++) {
    if (p1[i] != -1) {
      dioctl_SetDIO(client,p1[i],1,asHigh);
      if ((dioctl_GetDIO(client,p1[i])&1) != asHigh) {
	fprintf(stderr,"DIO %d %s failed\n",p1[i],asHigh?"high":"low");
      }
      //Assert(dioctl_GetDIO(client,p1[i])&1,"DIO %d %s failed\n",p1[i],asHigh?"high":"low");
      err=0;
      for (j=1;j<n;j++) {
	if (i != j && p1[j] > -1) {
	  if((dioctl_GetDIO(client,p1[j])&1) != !asHigh) {
	    if (p1[j] != 15 && p1[j] != 16 && p1[j] != 37) {
	      // ignore CAN_TX, CAN_RX, RXD_485
	      fprintf(stderr,"{%d,%d},",p1[j],p1[i],asHigh?"high":"low",i,j);
	      err++;
	    }
	    //fprintf(stderr,"DIO %d failed on DIO %d %s (i=%d,j=%d)\n",p1[j],p1[i],asHigh?"high":"low",i,j);
	    /*
	    for (i=0;i<n;i++) {
	      fprintf(stderr,"%d:\t%d\t%d\t%d\n",i,p1[i],p2[i],p3[i]);
	    }
	    */
	    //exit(3);
	  }
	  //Assert(!(dioctl_GetDIO(client,p1[j])&1),"DIO %d failed on DIO %d %s\n",p1[j],p1[i],asHigh?"high":"low");
	} else {
	  if (p2[j] != -1) {
	    if (asHigh != dioctl_GetDIO(client,p2[i])&1) {
	      fprintf(stderr,"DIO %d loopback failed on DIO %d %s\n",p2[j],p1[i],asHigh?"high":"low");
	    }
	    //Assert(asHigh == dioctl_GetDIO(client,p2[i])&1,"DIO %d loopback failed on DIO %d %s\n",p2[j],p1[i],asHigh?"high":"low");
	    if (p3[j] != -1) {
	      if (asHigh != dioctl_GetDIO(client,p2[i])&1) {
		fprintf(stderr,"DIO %d loopback failed on DIO %d %s\n",p2[j],p1[i],asHigh?"high":"low");
	      }
	      //Assert(asHigh == dioctl_GetDIO(client,p2[i])&1,"DIO %d loopback failed on DIO %d %s\n",p2[j],p1[i],asHigh?"high":"low");
	    }
	  }
	}
      }
      if (err) fprintf(stderr,"\n");
      dioctl_SetDIO(client,p1[i],1,!asHigh);
    }
  }

}

void test4500on8200() {
  int i,j,k;
  int p1[32] = {
    0, 1, 2, 4, 6, 7, 8, 9, 11,13,15,16,17,19,21,22,
    24,25,26,30,33,36,37,38,39,40,41,43,45,48,49,51
  };
  int p2[32] = {
    -1,23,3, 5, 35,-1,10,-1,12,14,-1,-1,18,20,-1,44,
    29,28,27,31,34,-1,-1,-1,-1,47,42,50,46,-1,-1,52
  };
  int p3[32] = {
    -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,
    -1,-1,-1,32,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1
  };
  int pull[53] = {
    1,1,1,1,1,1,1,0,1,1,
    1,0,0,1,1,1,1,1,1,0,
    0,0,1,1,1,1,1,1,1,1,
    1,1,1,1,1,1,1,1,1,0,
    1,1,1,0,1,1,1,1,1,1,
    0,0,0
  };

  // tri-state everything
  for (i=1;i<53;i++) {
    dioctl_SetDIO(client,i,0,0);
  }
  // verify pull-ups / pull-downs
  for (i=1;i<53;i++) {
    if (pull[i] != dioctl_GetDIO(client,i)) {
      fprintf(stderr,"DIO %d pull-%s fail\n",i,pull[i]?"up":"down");
    }
    //Assert(pull[i] == dioctl_GetDIO(client,i),"DIO %d pull-%s fail\n",i,pull[i]?"up":"down");
  }
  fprintf(stderr,"HIGH TESTS\n");
  testpins(1,32,p1,p2,p3); // Set 1 primary DIO at a time to 1
  testpins(1,32,p2,p1,p3); // Set 1 secondary DIO at a time to 1
  testpins(1,32,p3,p1,p2); // Set 1 tertiary DIO at a time to 1
  
  fprintf(stderr,"LOW TESTS\n");
  testpins(0,32,p1,p2,p3); // Set 1 primary DIO at a time to 0
  testpins(0,32,p2,p1,p3); // Set 1 secondary DIO at a time to 0
  testpins(0,32,p3,p1,p2); // Set 1 tertiary DIO at a time to 0
  
}

/*
./dioctl -p 4500 -g
./dioctl -p 4500 -r 0 -r 4 -r 8 # expect 0 1 0 1 0 0
./dioctl -p 4500 -0 4 -r 0 -r 4 # expect 0 0 1 0
./dioctl -p 4500 -1 4 -r 0 -r 4 # expect 0 1 1 1
./dioctl -p 4500 -G 1:4:5:8 -1 1001 -r 4 -r 5 -r 8 # expect 1 1 1 1 1 1
./dioctl -p 4500 -G 1:4:5:8 -r 1001 # expect 1 1 1 1 1 1

# client 1
./dioctl -p 4500 -G 1:4:5:8 -w 1:0:0:1
# client 2
./dioctl -p 4500 -0 4
*/

int main(int argc,char *argv[]) {
  int i;
  int opt_server = 0, opt_doseq, opt_test=0;

  struct option2 opts[] = {
    { 2, (opt_func)opt_int   ,&opt_server ,"<s>erver", "<port>      Daemonize and run as server listening on port" },
    { 2, (opt_func)opt_cli   ,&opt_client ,"<p>ort", "<host><:port> Talk to dioctl server" },
    { 0, (opt_func)opt_dioseq,&opt_doseq  ,"<g>etcaps", "           Get DIO capabilities" },
    { 1, (opt_func)opt_dioseq,&opt_doseq  ,"<r>ead", "DIOnum        Read the state of the specified DIO" },
    { 1, (opt_func)opt_dioseq,&opt_doseq  ,"<1>", "DIOnum           set the specified DIO to output and drive high" },
    { 1, (opt_func)opt_dioseq,&opt_doseq  ,"<0>", "DIOnum           set the specified DIO to output and drive low" },
    { 1, (opt_func)opt_dioseq,&opt_doseq  ,"<i>nput", "DIOnum       set the specified DIO to an input" },
    { 1, (opt_func)opt_dioseq,&opt_doseq  ,"<G>roup", "num:val1...  set the specified DIO Group number to the list of DIOs" },
    { 1, (opt_func)opt_dioseq,&opt_doseq  ,"<w>ait", "num:val1...   wait for any DIO in the specified group number to be the given value" },
    { 0, (opt_func)opt_bool  ,&opt_test   ,"<t>est", 0 /*"              perform TS-7200/TS-4500 test" */ },
    { 0,0,0,"Technologic Systems DIO controller manipulation.\n\nGeneral options:\n",
      "To specify a DIO group where a DIO number is expected, add 1000 to the group number."
    }
  };

  opt_cli_defaultport = 4500;
  process_options(argc,argv,opts);
  if (opt_test) {
    if (opt_client < 0) {
      fprintf(stderr,"Must specify port to run test on\n");
      return 1;
    }
    if (!client) {
      client = fdopen(opt_client,"a+");
      if (!client) {
	fprintf(stderr,"fdopen:%m");
	exit(1);
      }
    }
    test4500on8200();
  }
  if (opt_server) {
    DioInit();
    for (i=0;i<256;i++) {
      DioValue[i] = 0xFF;
    }
    lock_init(256); // one write lock for each possible DIO
    init_watch(256);
    run_dioctl_server(opt_server);
  }
  return 0;
}
/*
to do:
go back and make sure the spec below matches the latest changes to
the code.

DIOCTL protocol
  The protocol for talking to the dioctl server is documented below.
  All data sent to the server is prefixed with "->"
  All data returned to the client is prefixed with "<-"
  After the prefix comes the data type and the name of the data.
  When the value of data determines what comes next, that is denoted
  by indention to a new level for each permissible value.
  Comments start with a semi-colon and go to the end of the line.

  Example: In the protocol spec below, the server expects a request
  to start with an 8-bit unsigned integer containing the command ("CMD").
  If that value is equal to 0, it is interpreted as a "Get DIO 
  Capabilities" command, and the server returns an 8-bit unsigned
  value containing the total number of DIOs on the board, followed by
  an 8-bit unsigned value for each DIO containing it's capabilities.

  The commands that change DIOs (direction or state) are:
    CMD = 1,2,3,6,7,8, or 9
  These commands will block if any of the DIOs specified are locked
  by another client.  When the lock is released, the DIO operation
  will be performed.   Multiple clients can be waiting on the same
  DIO(s) and will get access in the order that complete requests are 
  received.

  Any locks held by a client will be automatically released when the
  client connection is closed.

  -> uint8 CMD
     CMD=0 ; Get DIO Capabilities
       <- uint8 TotalDIOcount
       <- uint8 DIOCapabilities[TotalDIOcount]
          format of DIOCapabilities:
	    bit 0 = input type
	      0 = output only; cannot be tri-stated
	      1 = input capable; can be tri-stated
	    bits 2:1 = output type
	      0 = input only; cannot drive an output 
	      1 = can only drive output high
	      2 = can only drive output low
	      3 = can drive output high or low
            if bits 2:0 == 0, DIO is not present
     CMD=1 ; Set Single DIO State to Input
       -> uint4 DIOnumber
     CMD=2 ; Set Single DIO State to Output High
       -> uint4 DIOnumber
     CMD=3 ; Set Single DIO State to Output Low
       -> uint4 DIOnumber
     CMD=4 ; Get Single DIO State
       -> uint4 DIOnumber
       <- boolean isOutput
       <- boolean isReadingHigh
     CMD=5 ; Define DIO Group Mapping
           ; each client has its own private group mappings
	   ; up to 16 mappings can be defined
	   ; up to 16 DIO can be in each mapping
       -> uint4 mapNumber
       -> uint4 numberOfDIOs
       -> uint4 DIOnumber[numberOfDIOs]
     CMD=6 ; Set DIO Group to Input
       -> uint4 mapNumber
     CMD=7 ; Set DIO Group to Output High
       -> uint4 mapNumber
     CMD=8 ; Set DIO Group to Output Low
       -> uint4 mapNumber
     CMD=9 ; Set DIO Group States
       -> uint4 mapNumber
       -> uint2 DirAndState[numberOfDIOs[mapNumber]]
     CMD=10; Get DIO Group States
       -> uint4 mapNumber
       <- uint2 DirAndState[numberOfDIOs[mapNumber]]
     CMD=11; Aquire DIO Group Lock, Blocking
           ; lock is aquired on all DIOs in group
	   ; before next command is processed
       -> uint4 mapNumber
     CMD=12; Release DIO Group Lock
       -> uint4 mapNumber
     CMD=13; Aquire DIO Group Lock, Non-Blocking
       -> uint4 mapNumber
       <- uint8 result
         result=0x55 ; FAIL, one or more DIOs in group 
	             ; is already locked by another client
         result=0x77 ; SUCCEEDED, all DIOs in group locked ok
     CMD=14; wait for event of any DIO in group being the specified value
     CMD=15; wait until specified DIO is the specified value
     CMD=16; lock single DIO (blocking)
     CMD=17; unlock single DIO
     CMD=18; lock single DIO (non-blocking)

     CMD=19; wait for event of any DIO in group to be input Low
     CMD=20; wait for event of any DIO in group to be input High
     CMD=21; get timestamp of last event
*/