/** \file XPortSim.cc */
/****************************************************************************/
/* Copyright (c) 2008 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : XPortSim.cc                                                   */
/* Author   : K Headley                                                     */
/* Project  : Benthic Imaging AUV                                           */
/* Created  : 04/01/2008                                                    */
/****************************************************************************/
/* Modification History:                                                    */
/*  $Id: XPortSim.cc,v 1.5 2012/04/06 01:13:06 headley Exp $    */
/*  $Name: FastSim $  */
/****************************************************************************/

#include <stdio.h>
#include <unistd.h> /* close */
#include <stdlib.h>
#include <stdarg.h>
#include <ctype.h>
#include <string.h>
#include <signal.h>
#include <fcntl.h>
#include "XPort.h"
#include "Utils.h"
#include "qnx_port.h"
#include "BicamConsts.h"

/** shutdown if TRUE */
boolean shutdownFlag=FALSE;
/** quiet (not verbose) output */
boolean verbose=FALSE;

/** Signal handler (SIGINT, SIGTERM) for XPortSim process.
    @param sig signal received
*/
void sig_handler(int sig){
  switch(sig){
  case SIGINT:
    cout<<"got SIGINT"<<endl;
    shutdownFlag=TRUE;
    break;
  case SIGTERM:
    cout<<"got SIGTERM"<<endl;
    shutdownFlag=TRUE;
    break;
  default:
    cout<<"unhandled signal"<<endl;
  }

  return;
}

/** send bytes to specified socket.
    @param sd socket descriptor
    @param buf byte buffer
    @param len number of bytes to send
*/
int sendBytes(int sd, char *buf, int len){
  int rc;

    rc = send(sd, buf, len, 0);
    
    if(rc<0) {
      perror("sendBytes cannot send data");
      return -1;
    }
    return rc;
}
/** Send XPortResponse to specified socket.
    @param sd socket descriptor
    @param xpResponse pointer to XPortResponse 
 */
int sendResponse(int sd, XPortResponse *xpResponse){

  char bytes[XPORT_RESPONSE_LEN];
  xpResponse->toBytes(bytes);
  return sendBytes(sd,bytes,XPORT_RESPONSE_LEN);
}
/** Send bytes to specified socket.
    @param sd socket descriptor
    @param msg bytes to send.

*/
int sendMessage(int sd, char *msg){
  int rc;
    rc = send(sd, msg, strlen(msg), 0);
    if(rc<0) {
      perror("cannot send message ");
      close(sd);
      exit(1);
    }
    return rc;
}
/** Start XPortSim server 
    @param host hostname 
*/
void startMockXPort(char *host);

/** Process ID of XPortSim server */
pid_t mockXPortPID;
/** Process ID of GPIO TCP/IP process */
pid_t gpioPID;
/** Process ID of Serial IO TCP/IP process */
pid_t serialPID;

/** Establish listener socket 
    @param sd socket descriptor to assign
    @param hostname XPortSim server hostname
    @param port XPortSim server TCP/IP port
    @param servAddr socket address
*/
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;
}
/** GPIO handler. 
    Receives incoming XPort GPIO traffic and returns appropriate messages.
*/
void *gpioListener(){
  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[256];

  gethostname(hostName,255);

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

  while(1) {
    if(verbose==TRUE)
      cout<<"GPIO listener: waiting on port "<<GPIO_IP_PORT<<" sd:"<<sd<<endl;
    listen(sd,2);    

    svrLen = sizeof(svrAddr);
    newSd = accept(sd, (struct sockaddr *) &svrAddr, &svrLen);
    if(newSd<0) {
      perror("GPIO listener: cannot accept connection ");
      return NULL;
    }
    if(verbose==TRUE)
      cout<<"GPIO listener: got GPIO connection from "<<inet_ntoa(svrAddr.sin_addr)<<endl;

    /* 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){
	  if(verbose==TRUE)
	    cout<<"GPIO listener: invalid command returned ["<<test<<"]"<<endl;
	}else{
	  xpResponse.set(xpCommand.getCommand(),xpCommand.getP1());
	  //printdbg("GPIO listener: sending response:\n");
	  if(verbose==TRUE)
	    cout<<"GPIO listener: command received - ["<<xpCommand<<"]"<<endl;
	  sendResponse(newSd,&xpResponse);
	}
      }
    } 
  }

  return NULL;

}

/** Serial handler. 
    Receives incoming XPort Serial traffic and returns appropriate messages.
    ToDo: use local serial port for full emulation.
*/
void *serialListener(){
  int sd, newSd=-1;
  SOCKLEN svrLen;
  struct sockaddr_in svrAddr;
  int MSG_BUFFER_SIZE=256;
  char msgBuffer[256];
  int readLen=0;
  int disconnect=0;
  char hostName[256];


  gethostname(hostName,255);

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

  while(shutdownFlag==FALSE) {
    if(verbose==TRUE)
      cout<<"SERIAL listener: waiting on port "<<SERIAL_IP_PORT1<<" sd:"<<sd<<endl;
    listen(sd,2);    

    svrLen = sizeof(svrAddr);
    newSd = accept(sd, (struct sockaddr *) &svrAddr, &svrLen);
    if(newSd<0) {
      perror("SERIAL listener: cannot accept connection ");
      return NULL;
    }
    
    if(verbose==TRUE)
      cout<<"SERIAL listener: got SERIAL connection from "<<inet_ntoa(svrAddr.sin_addr)<<endl;

    /* 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';
	if(verbose==TRUE)
	  cout<<"Serial listener received message:["<<msgBuffer<<"]"<<endl;
      }
      memset(&msgBuffer[0],0x00,MSG_BUFFER_SIZE);
    } 
  }
  return NULL;

}
/** start XPortSim server 
    @param host hostname
*/
void startMockXPort(const 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){
    if(verbose==TRUE)
      cout<<"startMockXPort: gpio PID "<< (long)getpid() <<" on host "<< host <<endl;
    gpioListener();    
    _exit(-1);
  }

  errno=0;
  foo=sleep(2);

  serialPID=fork();

  if( serialPID == -1){
    perror("startMockXPort: serial fork");
    exit(EXIT_FAILURE);
  }
  if( serialPID == 0){
    if(verbose==TRUE)
      cout<<"startMockXPort: serial PID "<< (long)getpid() <<" on host "<< host <<endl;
    serialListener();
    _exit(-1);
  }

  return;  
}


/** XPortSim Entry Point.
    Starts XPortSim server.
    @param argc number of command line arguments
    @param argv command line parameters
    @returns 0 if normal termination, error code otherwise
*/
int main (int argc, char *argv[]) {
  char xportHost[256];

  cout<<"XPort Simulator starting -- type CTRL-C to quit"<<endl<<endl;

  if(argc>1)
    if(strcmp(argv[1],"-v")==0)
      verbose=TRUE;

  gethostname(xportHost,255);

  startMockXPort((const char *)xportHost);
  signal(SIGINT, sig_handler);
  signal(SIGTERM,sig_handler);

  sleep(1);
  if(verbose==TRUE)
    cout<<"waiting for connection..."<<endl;

  // wait for SIGINT
  while(shutdownFlag==FALSE)
    sleep(30);

  printf("stopping GPIO server\n");
  kill(gpioPID,SIGKILL);
  printf("stopping SERIAL server\n");
  kill(serialPID,SIGKILL);

  exit(EXIT_SUCCESS);

  return 0;
}
/*  XPort xport(xportHost,
	      XP_GPIO_IP_PORT,XP_SERIAL_IP_PORT1,
	      CFG_MASK_GPIO_SEL_ALL,CFG_GPIO_HI,
	      CFG_MASK_GPIO_SEL_ALL,CFG_GPIO_HI);

  // Connect to the XPort 
  // (both serial and gpio sockets)  
  xport.connectxp(XPCON_ALL);*/