/****************************************************************************/
/* Copyright (c) 2008 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : xportTest.cc                                                  */
/* Author   : K Headley                                                     */
/* Project  : Benthic Imaging AUV                                           */
/* Created  : 04/01/2008                                                    */
/****************************************************************************/
/* Modification History:                                                    */
/*  $Id: xportTest.cc,v 1.3 2012/04/05 22:43:38 headley Exp $    */
/*  $Name: FastSim $  */
/****************************************************************************/

#include "XPort.h"
#include "BicamConsts.h"
#include "qnx_port.h"
#include <signal.h>

#ifdef QNX
using namespace std;
#endif

/* local function defs */
int testConnectXPort(XPort xport);
int testWriteMessage(XPort xport);
int testWriteCommand(XPort xport);
int testReadResponse(XPort xport);
void startMockXPort(char *host);

pid_t mockXPortPID;
pid_t gpioPID;
pid_t serialPID;
#define NUM_THREADS 2

int testConnectXPort(XPort *xport){
  int test;
  test=xport->connectxp(XPCON_ALL);
  if(test==OK){
    printdbg("testConnectXPort: host:%s gpioPort:%d serialPort:%d \n",
	  xport->getConnection()->getHost(), 
	  xport->getConnection()->getpGPIO(),
	  xport->getConnection()->getpSerial());
    return TEST_OK;
  }
  return TEST_FAILED;
}
int testDisconnectXPort(XPort xport){
  int test;
  test=xport.disconnectxp(XPCON_ALL);
  if(test==OK){
    return TEST_OK;
  }
  return TEST_FAILED;
}
int testWriteSerial(XPort xport){
  int test=0;
  char *msg="this is a test message\n";
  int len=(strlen(msg)+1);

  test=xport.writeSerial(msg,len,0);
  if(test!=len){
    cout << "testWriteSerial: failed; returned "<<test<<endl;
    return TEST_FAILED;
  }
  return TEST_OK;
}
int testWriteCommand(XPort xport){
  int test=0;
  int result=0;
  XPortResponse xpResponse;
  XPortCommand xpCommand(XP_CMD_GET_FUNCTIONS,0x7,0x7);

  //printdbg("testSendCommand: sending command\n");
  test=xport.writeCommand(&xpCommand);
  if(test!=XPORT_COMMAND_LEN){
    result++;
  }
  //printdbg("testSendCommand: reading response\n");
  xport.readResponse(&xpResponse);
  //printdbg("testSendCommand: response received:\n");
  //showResponse(&xpResponse);

  if(result>0)
    return TEST_FAILED;
  else
    return TEST_OK;
}

int testReadResponse(XPort xport){
  int test=0;
  int result=0;
  unsigned char testCmd=XP_CMD_GET_FUNCTIONS;
  XPortResponse xpResponse;
  XPortCommand xpCommand;

  xpCommand.set(testCmd,0xFF,0xFF);
  //printdbg("testReadResponse sending:\n");
  //showCommand(xpCommand);

  test=xport.writeCommand(&xpCommand);
  if(test!=XPORT_COMMAND_LEN){
    result++;
  }
  //printdbg("testReadResponse: sendCommand returned %d\n",test);
  test=xport.readResponse(&xpResponse);
  if(xpResponse.getCommand() != testCmd ||
     xpResponse.getP1()  != 0xff){
    result++;
  }

  if(result>0)
    return TEST_FAILED;
  else
    return TEST_OK;
}


int getListenerSocket(int *sd, char *hostname, unsigned short port, struct sockaddr_in *servAddr){
  struct hostent *h;

  printdbg("getListenerSocket: setting up listener on %s port %d (%hu)\n",hostname,port,port);
  /* create socket */
  *sd = socket(AF_INET, SOCK_STREAM, 0);
   if(*sd<0) {
    perror("getListenerSocket: cannot open socket ");
    return ERROR;
  }
   printdbg("getListenerSocket: got socket (%d)\n",*sd);

  /* bind server port */
   printdbg("getListenerSocket: calling gethostbyname %s\n",hostname);
   h = gethostbyname(hostname);
  if(h==NULL) {
    printdbg("getListenerSocket: unknown host '%s' (%d)\n",hostname,h_errno);
    return ERROR;
  }
   printdbg("getListenerSocket: gethostbyname complete\n");

  servAddr->sin_family = AF_INET;
  //memcpy((char *) &servAddr->sin_addr.s_addr, h->h_addr_list[0], h->h_length);
  /* not sure why, but it won't bind using the hostname: 
     memcpy((char *) &servAddr->sin_addr.s_addr, h->h_addr_list[0], h->h_length);
     must use INADDR_ANY
  */
  servAddr->sin_addr.s_addr = htonl(INADDR_ANY);
  servAddr->sin_port = htons(port);

  errno=0;
  if(bind(*sd, (struct sockaddr *) servAddr, sizeof(*servAddr))<0) {
    printdbg("getListenerSocket: cannot bind port (%d)\n",errno);
    return ERROR;
  }

  listen(*sd,2);    

  return *sd;
}

void *gpioListener(void *threadid){
  int tid;
  int sd, newSd;
  SOCKLEN svrLen;
  struct sockaddr_in svrAddr;
  int MSG_BUFFER_SIZE=32;
  char msgBuffer[32];
  int readLen=0;
  XPortCommand xpCommand;
  XPortResponse xpResponse;
  int test=0,disconnect=0;
  char hostName[MAXHOSTNAMELEN];

	//tid=(int)threadid;
	tid=(long)threadid;
  printdbg("GPIO listener: starting on port %ld thread tid=%d\n",GPIO_IP_PORT,tid);
  test=gethostname(hostName,MAXHOSTNAMELEN);


  /* create socket */
  sd = getListenerSocket(&sd,hostName,GPIO_IP_PORT,&svrAddr);
  if(sd==ERROR){
    perror("gpioListener: GPIO listener: cannot open socket\n");
    return NULL;//pthread_exit((void *)err);
  }

  while(1) {
    printdbg("GPIO listener: waiting on port %d sd=%d\n",GPIO_IP_PORT,sd);
    listen(sd,2);    

    svrLen = sizeof(svrAddr);
    newSd = accept(sd, (struct sockaddr *) &svrAddr, &svrLen);
    if(newSd<0) {
      perror("GPIO listener: cannot accept connection ");
      //pthread_exit((void *)&err);
      return NULL;
    }
    printdbg("GPIO listener: got GPIO connection from %s\n",inet_ntoa(svrAddr.sin_addr));

    /* wait for data */
    disconnect=0;
    while(!disconnect){
      errno=0;
      readLen=recv(newSd, msgBuffer,MSG_BUFFER_SIZE,0);
      if(readLen<0){
	printdbg("GPIO listener: recv returned %d (errno=%d)\n",readLen,errno);
	switch(errno){
	case ENOTCONN:
	  disconnect=1;
	  break;
	default:
	  break;
	}
      }else if(readLen==0){
	printdbg("GPIO listener: connection closed by client\n");
	disconnect=1;
      }else{
	if(readLen!=XPORT_COMMAND_LEN){
	  printdbg("GPIO listener: warning - invalid message length (%d!=%d)\n",readLen,XPORT_COMMAND_LEN);
	}
	test=xpCommand.parse(msgBuffer);
	if(test!=IS_VALID){
	  printdbg("GPIO listener: invalid command returned (%d)\n",test);
	}else{
	  xpResponse.set(xpCommand.getCommand(),xpCommand.getP1());
	  //printdbg("GPIO listener: sending response:\n");
	  //showResponse(&xpResponse);
	  send(newSd,xpResponse.toBytes(msgBuffer),XPORT_RESPONSE_LEN,0);
	}
      }
    } 
  }

  return NULL;//pthread_exit( (void *) err);

}

void *serialListener(void *threadid){
  int tid;
  int sd, newSd;
  SOCKLEN svrLen;
  struct sockaddr_in svrAddr;
  int MSG_BUFFER_SIZE=256;
  char msgBuffer[256];
  int readLen=0;
  int disconnect=0;
  char hostName[MAXHOSTNAMELEN];
	//tid=(int)threadid;
	tid=(long)threadid;
  printdbg("SERIAL listener: starting on port %d thread tid=%ld\n",SERIAL_IP_PORT,tid);

  gethostname(hostName,MAXHOSTNAMELEN);

  /* create socket */
  sd = getListenerSocket(&sd,hostName,SERIAL_IP_PORT,&svrAddr);
  if(sd==ERROR){
    perror("SERIAL listener: cannot open socket");
    return NULL;//pthread_exit((void *)err);
  }

  while(1) {
    printdbg("SERIAL listener: waiting on port %d sd=%d\n",SERIAL_IP_PORT,sd);
    listen(sd,2);    

    svrLen = sizeof(svrAddr);
    newSd = accept(sd, (struct sockaddr *) &svrAddr, &svrLen);
    if(newSd<0) {
      perror("SERIAL listener: cannot accept connection ");
       return NULL;//pthread_exit((void *)err);
    }
    printdbg("SERIAL listener: got SERIAL connection from %s\n",inet_ntoa(svrAddr.sin_addr));

    /* wait for data */
    disconnect=0;
    while(!disconnect){
      errno=0;
      readLen=recv(newSd, msgBuffer,MSG_BUFFER_SIZE,0);
      if(readLen<0){
	printdbg("SERIAL listener: recv returned %d (errno=%d)\n",readLen,errno);
	switch(errno){
	case ENOTCONN:
	  disconnect=1;
	  break;
	default:
	  break;
	}
      }else if(readLen==0){
	printdbg("SERIAL listener: connection closed by client\n");
	disconnect=1;
      }else{
	msgBuffer[MSG_BUFFER_SIZE]='\0';
	printdbg("Serial listener received message:[%s]\n",msgBuffer);
      }
      memset(&msgBuffer[0],0x00,MSG_BUFFER_SIZE);
    } 
  }

  return NULL;//pthread_exit( (void *) err);

}
/*
void startMockXPortThreads(char *host){
  pthread_t threads[NUM_THREADS];
  pthread_attr_t pthread_custom_attr;
  int rc,i=0,j=1;

  /// fork a process to act as an XPort stand-in 
  mockXPortPID=fork();

  if( mockXPortPID == -1){
    perror("fork");
    exit(EXIT_FAILURE);
  }
  if( mockXPortPID == 0){
    printdbg("mock xport PID %ld on host %s\n",(long)getpid(),host);

    // spin threads to listen on different ports 
    pthread_attr_init(&pthread_custom_attr);

    rc=pthread_create(&threads[0],&pthread_custom_attr,gpioListener,(void *)i);
    if(rc!=0)
      printdbg("error creating gpio listener: %d\n",rc);

    rc=pthread_create(&threads[1],&pthread_custom_attr,serialListener,(void *)j);
    if(rc!=0)
      printdbg("error creating serial listener: %d\n",rc);

    printdbg("mock XPort listening...\n");
    pthread_join(threads[0],NULL);
    pthread_join(threads[1],NULL);
    printdbg("mock XPort exiting\n");
    return;//_exit(-1);
  }
  printdbg("sleeping 2 sec\n");
  sleep(2);

  return;  
}
*/
void startMockXPort(char *host){
  unsigned foo;
  /* fork processes to act as an XPort stand-in
     One listens for GPIO and the other for serial
     connections.
  */
  gpioPID=fork();

  if( gpioPID == -1){
    perror("startMockXPort: gpio fork");
    exit(EXIT_FAILURE);
  }
  if( gpioPID == 0){
    printdbg("startMockXPort: gpio PID %ld on host %s\n",(long)getpid(),host);
    gpioListener(NULL);
    _exit(-1);
  }

  errno=0;
  foo=sleep(2);

  serialPID=fork();

  if( serialPID == -1){
    perror("startMockXPort: serial fork");
    exit(EXIT_FAILURE);
  }
  if( serialPID == 0){
    printdbg("startMockXPort: serial PID %ld on host %s\n",(long)getpid(),host);
    serialListener(NULL);
    _exit(-1);
  }

  return;  
}

/* Main Entry Point */
int main (int argc, char *argv[]){
  char xportHost[MAXHOSTNAMELEN+1];
  int test,passed=0,failed=0;
  XPort xport(xportHost,
	      GPIO_IP_PORT,SERIAL_IP_PORT,
	      CFG_GPIO_ALL,CFG_GPIO_OUT,
	      CFG_GPIO_ALL,CFG_GPIO_HI);

  errno=0;
  test=gethostname(xportHost,MAXHOSTNAMELEN);
  if(test<0)
    cout<<"gethostname failed errno "<<errno<<endl;
  cout<<"starting xport simulated server on "<< xportHost <<"..."<<endl;
  startMockXPort(xportHost);
  sleep(2);

  cout<<"connecting to server..."<<endl;
  xport.getConnection()->configAll(xportHost,
				   GPIO_IP_PORT,SERIAL_IP_PORT,
				   CFG_GPIO_ALL,CFG_GPIO_OUT,
				   CFG_GPIO_ALL,CFG_GPIO_HI);

  
  test=testConnectXPort(&xport);
  (test==TEST_OK?passed++:failed++);
  cout << "main: test connectXport: "<<(test==TEST_OK?"OK":"ERR");
  cout << "(sdGPIO="<<xport.getConnection()->getsdGPIO()<<") ";
  cout << "(sdSerial="<<xport.getConnection()->getsdSerial()<<")"<<endl;
 
  test=testWriteCommand(xport);
  (test==TEST_OK?passed++:failed++);
  cout << "main: test sendCommand: "<<(test==TEST_OK?"OK":"ERR")<<endl;

  test=testReadResponse(xport);
  (test==TEST_OK?passed++:failed++);
  cout << "main: test readResponse: "<<(test==TEST_OK?"OK":"ERR")<<endl;

  test=testWriteSerial(xport);
  (test==TEST_OK?passed++:failed++);
  cout << "main: test writeSerial: "<<(test==TEST_OK?"OK":"ERR")<<endl;

  cout<<"main: disconnecting XPort"<<endl;
  test=testDisconnectXPort(xport);
  (test==TEST_OK?passed++:failed++);
  cout << "main: test disconnectXPort: "<<(test==TEST_OK?"OK":"ERR")<<endl;

  cout<<"main: killing GPIO server"<<endl;
  kill(gpioPID,SIGKILL);
  cout<<"main: killing SERIAL server"<<endl;
  kill(serialPID,SIGKILL);
  cout<<"main: done - "<<passed<<" tests passed "<<failed<<" tests failed"<<endl;

  exit(EXIT_SUCCESS);

  return 0;
}