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

#include "XPort.h"
#include "qnx_port.h"
#include "Utils.h"
#include "BicamConsts.h"

/** Default no argument constructor. */
XPortCommand::XPortCommand(){}
/** Initializing constructor.
    Sets XPort GPIO command parameters.
    @param cmd Command byte
    @param p1 control mask
    @param p2 value mask
*/
XPortCommand::XPortCommand(unsigned char cmd,unsigned int p1,unsigned int p2){
  set(cmd,p1,p2);
}
/** Set XPort GPIO command parameters.
    \note Does not perform range checking
    @param cmd Command byte
    @param p1 control mask
    @param p2 value mask
*/
void XPortCommand::set(unsigned char cmd,unsigned int p1,unsigned int p2){
  
  command=0;
  param1=0;
  param2=0;
  /* range checking here...*/
  command=cmd;
  param1=(p1&0xffff);
  param2=(p2&0xffff);
  return;
}
/** @return command member variable */
unsigned char XPortCommand::getCommand(){
  return command;
}
/** @return param1 member variable (control mask) */
unsigned int XPortCommand::getP1(){
  return param1;
}
/** @return param2 member variable (value mask) */
unsigned int XPortCommand::getP2(){
  return param2;
}
/** Validate command parameters. 
    Performs range checking on all parameters.
    @return OK if all parameters are valid, error code otherwise:
    - ERR_BADCMD: command parameter is invalid
    - ERR_BADPAR1: mask out of range
    - ERR_BADPAR2: mask out of range

    @see XPort.h
*/
int XPortCommand::validate(){
  if(command != XP_CMD_GET_FUNCTIONS &&
     command != XP_CMD_GET_DIRECTIONS &&
     command != XP_CMD_GET_ACTIVE_LEVELS &&
     command != XP_CMD_GET_CURRENT_STATES &&
     command != XP_CMD_SET_DIRECTIONS &&
     command != XP_CMD_SET_ACTIVE_LEVELS &&
     command != XP_CMD_SET_CURRENT_STATES &&
     command != XP_CMD_SEND_MESSAGE){
    return ERR_BADCMD;
  }
  if( param1>(unsigned)0xffff ){
    return ERR_BADPAR1;
  }
  if( param2>(unsigned)0xffff ){
    return ERR_BADPAR2;
  }

  return IS_VALID;

}
/** Populate this XPCommand from a (binary) byte array. 
    @param cmd Byte buffer containing command bytes
*/
int XPortCommand::parse(const char *cmd){

  memcpy(&command,&cmd[0],XP_CMD_LEN);
  memcpy(&param1,&cmd[1],XP_PARAM_LEN);
  memcpy(&param2,&cmd[5],XP_PARAM_LEN);

  return validate();
}
/** Format XPort command and print to console.
    \note Requires that DEBUG be enabled
*/
void XPortCommand::show(){
#ifdef DEBUG
  printdbg(" cmd: %02x %02x %02x\n",command,param1,param2);
#endif
  return ;
}
/** Writes XPort GPIO command bytes to specified buffer.
    \note Buffer should have length >= (XP_CMD_LEN+2*XP_PARAM_LEN)
    @param buf Byte buffer 
    @return pointer to buffer
*/
char *XPortCommand::toBytes(char *buf){
  memcpy(&buf[0],&command,XP_CMD_LEN);
  memcpy(&buf[1],&param1,XP_PARAM_LEN);
  memcpy(&buf[5],&param2,XP_PARAM_LEN);
  return buf;
}
/** output stream operator.
    Formats object for output stream insertion
    @param output output stream handle
    @param n XPortCommand object pointer
*/
ostream& operator<<(ostream& output, const XPortCommand& n){
  cout << "{0x"<<hex<<setiosflags(ios::fixed)<<setfill('0')<<setw(2)<<(unsigned int)n.command<<",0x"<<setw(2)<<n.param1<<",0x"<<setw(2)<<n.param2<<"}";
  return cout;
}

/** Default no argument constructor. */
XPortResponse::XPortResponse(){}
/** Initializing constructor.
    Sets XPort GPIO response parameters.
    @param cmd Command byte
    @param p1 control mask
*/
XPortResponse::XPortResponse(unsigned char cmd,unsigned int p1){
  set(cmd,p1);
}
/** Set XPort GPIO response parameters.
    \note Does not perform range checking
    @param cmd Command byte
    @param p1 control mask
*/
void XPortResponse::set(unsigned char cmd,unsigned int p1){
  command=0;
  param1=0;
  /* range checking here...*/
  command=cmd;
  param1=(p1&0xffff);
  return;
}
/** Validate response parameters. 
    Performs range checking on all parameters.
    @return OK if all parameters are valid, error code otherwise:
    - ERR_BADCMD: command parameter is invalid
    - ERR_BADPAR1: mask out of range

    @see XPort.h
*/
int XPortResponse::validate(){

  if(command != XP_CMD_GET_FUNCTIONS &&
     command != XP_CMD_GET_DIRECTIONS &&
     command != XP_CMD_GET_ACTIVE_LEVELS &&
     command != XP_CMD_GET_CURRENT_STATES &&
     command != XP_CMD_SET_DIRECTIONS &&
     command != XP_CMD_SET_ACTIVE_LEVELS &&
     command != XP_CMD_SET_CURRENT_STATES &&
     command != XP_CMD_SEND_MESSAGE){
    return ERR_BADCMD;
  }

  if( param1>(unsigned)0xffff ){
    return ERR_BADPAR1;
  }

  return IS_VALID;
}

/** Populate this XPResponse from a (binary) byte array. 
    @param response Byte buffer containing response bytes
*/
int XPortResponse::parse(char *response){
  memcpy(&command,&response[0],XP_CMD_LEN);
  memcpy(&param1,&response[1],XP_PARAM_LEN);
  return validate();
}
/** Format XPort response and print to console.
    \note Requires that DEBUG be enabled
*/
void XPortResponse::show(){
#ifdef DEBUG
  printdbg(" res: %02x %02x\n",command,param1);
#endif
  return;
}
/** @return command member variable */
unsigned char XPortResponse::getCommand(){
  return command;
}
/** @return param1 member variable (control mask) */
unsigned int XPortResponse::getP1(){
  return param1;
}
/** Writes XPort GPIO response bytes to specified buffer.
    \note Buffer should have length >= (XP_CMD_LEN+XP_PARAM_LEN)
    @param buf Byte buffer 
    @return pointer to buffer
*/
char *XPortResponse::toBytes(char *buf){
  memcpy(&buf[0],&command,XP_CMD_LEN);
  memcpy(&buf[1],&param1,XP_PARAM_LEN);
  return buf;
}

/** output stream operator.
    Formats object for output stream insertion
    @param output output stream handle
    @param n XPortResponse object pointer
*/
ostream& operator<<(ostream& output, const XPortResponse& n){
  cout << "{"<<hex<<setiosflags(ios::fixed)<<setfill('0')<<setw(2)<<(unsigned int)n.command<<",0x"<<setw(2)<<n.param1<<"}";
  return cout;
}

/** Default no argument constructor. */
XPortConnection::XPortConnection(){}
/** Initializing constructor.
    @param host XPort host name
    @param gpioIPPort TCP/IP port to use for GPIO
    @param serialIPPort TCP/IP port to use for serial IO
    @param dirMask GPIO direction pin-select mask
    @param dirValues GPIO initial direction values mask
    @param levelMask GPIO level pin-select mask
    @param levelValues GPIO initial level (state) values mask
*/
XPortConnection::XPortConnection(char *host, 
				 unsigned int gpioIPPort, 
				 unsigned int serialIPPort,
				 unsigned int dirMask,
				 unsigned int dirValues,
				 unsigned int levelMask,
				 unsigned int levelValues){
  configHost(host);
  configGPIO(gpioPort,dirMask,dirValues,levelMask,levelValues);
  configSerial(serialPort);
}
/** Configure all connection parameters
    @param host XPort host name
    @param gpioIPPort TCP/IP port to use for GPIO
    @param serialIPPort TCP/IP port to use for serial IO
    @param dirMask GPIO direction pin-select mask
    @param dirValues GPIO initial direction values mask
    @param levelMask GPIO level pin-select mask
    @param levelValues GPIO initial level (state) values mask
    @return OK;
*/
int XPortConnection::configAll( char *host, 
			   unsigned int gpioIPPort, 
			   unsigned int serialIPPort,
			   unsigned int dirMask,
			   unsigned int dirValues,
			   unsigned int levelMask,
			   unsigned int levelValues){
  configHost(host);
  configGPIO(gpioIPPort,dirMask,dirValues,levelMask,levelValues);
  configSerial(serialIPPort);

  //cout<<"configAll: {"<<host<<","<<gpioPort<<","<<serialPort<<",";
  //cout<<dirMask<<","<<dirValues<<",";
  //cout<<levelMask<<","<<levelValues<<"}"<<endl;
  return OK;
}
/** Configure host TCP/IP address
    @param hostName XPort host name
    @return OK;
*/
int XPortConnection::configHost( char *hostName){
  host=hostName;
  return OK;
}
/** Configure GPIO connection parameters
    @param gPort TCP/IP port to use for GPIO
    @param dMask GPIO direction pin-select mask
    @param dValues GPIO initial direction values mask
    @param lMask GPIO level pin-select mask
    @param lValues GPIO initial level (state) values mask
    @return OK;
*/
int XPortConnection::configGPIO( int gPort, 
			   unsigned int dMask,
			   unsigned int dValues,
			   unsigned int lMask,
			   unsigned int lValues){
  gpioPort=gPort;
  dirMask=dMask;
  levelMask=lMask;
  dirValues=dValues;
  levelValues=lValues;
  return OK;
}
/** Configure serial TCP/IP port parameters
    @param sPort TCP/IP port to use for serial IO
    @return OK;
*/
int XPortConnection::configSerial( int sPort){
  serialPort=sPort;
  return OK;
}
/** Set GPIO socket descriptor. 
    @param sd socket descriptor
*/
void XPortConnection::setsdGPIO(int sd){
  sdGPIO=sd;
}
/** Set serial socket descriptor. 
    @param sd socket descriptor
*/
void XPortConnection::setsdSerial(int sd){
  sdSERIAL=sd;
}
/** @return XPort host TCP/IP address/name */
 char *XPortConnection::getHost(){
  return host;
}
/** @return GPIO TCP/IP port */
unsigned int XPortConnection::getpGPIO(){
  return gpioPort;
}
/** @return Serial TCP/IP port */
unsigned int XPortConnection::getpSerial(){
  return serialPort;
}
/** @return GPIO TCP/IP socket descriptor */
int XPortConnection::getsdGPIO(){
  return sdGPIO;
}
/** @return Serial TCP/IP socket descriptor */
int XPortConnection::getsdSerial(){
  return sdSERIAL;
}
/** @return GPIO pin direction control mask */
unsigned int XPortConnection::getDirMask(){return dirMask;}
/** @return GPIO pin direction values mask */
unsigned int XPortConnection::getDirValues(){return dirValues;}
/** @return GPIO pin level control mask */
unsigned int XPortConnection::getLevelMask(){return levelMask;}
/** @return GPIO pin level values mask */
unsigned int XPortConnection::getLevelValues(){return levelValues;}


/** output stream operator.
    Formats object for output stream insertion
    @param output output stream handle
    @param n Field object pointer
*/
ostream& operator<<(ostream& output, const XPortConnection& n){

  cout << "{"<<endl<<" host:" << n.host <<endl;
  cout << " gpioPort:0x" << hex << n.gpioPort << " serialPort: "<<dec<<n.serialPort <<endl;
  cout << " dirMask:" << n.dirMask << " dirVal: "<<n.dirValues<<endl;
  cout << " levelMask:" << n.levelMask << " levelVal: "<<n.levelValues<<endl;
  cout << " sdSerial:" << n.sdSERIAL << " sdGPIO: "<<n.sdGPIO<<endl<<"}";
  return cout;
}

/** Default no argument constructor.
 */
XPort::XPort(){}

/** Initializing constructor. Configures
    XPortConnection member variable.

    @param host XPort host name
    @param gpioIPPort TCP/IP port to use for GPIO
    @param serialIPPort TCP/IP port to use for serial IO
    @param dMask GPIO direction pin-select mask
    @param dValues GPIO initial direction values mask
    @param lMask GPIO level pin-select mask
    @param lValues GPIO initial level (state) values mask
 */
XPort::XPort(char *host, 
	     unsigned int gpioIPPort, 
	     unsigned int serialIPPort,
	     unsigned int dMask,
	     unsigned int dValues,
	     unsigned int lMask,
	     unsigned int lValues){

  xpConnection.configAll(host,
			 gpioIPPort,serialIPPort,
			 dMask,dValues,
			 lMask,lValues);

}
/** Configures XPortConnection member variable.
    @param host XPort host name
    @param gpioIPPort TCP/IP port to use for GPIO
    @param serialIPPort TCP/IP port to use for serial IO
    @param dMask GPIO direction pin-select mask
    @param dValues GPIO initial direction values mask
    @param lMask GPIO level pin-select mask
    @param lValues GPIO initial level (state) values mask
 */
int XPort::configure(char *host, 
		     unsigned int gpioIPPort, 
		     unsigned int serialIPPort,
		     unsigned int dMask,
		     unsigned int dValues,
		     unsigned int lMask,
		     unsigned int lValues){
  xpConnection.configAll(host,
			 gpioIPPort,serialIPPort,
			 dMask,dValues,
			 lMask,lValues);
  return OK;
}
/** @return pointer to XPortConnection member variable */
XPortConnection *XPort::getConnection(){
  return &xpConnection;
}

/** Connect to specified XPort TCP/IP socket. 
    @param host XPort TCP/IP host name or address
    @param port TCP/IP port to connect to
    @return Socket file handle
*/
int XPort::connectSocket(char *host, int port){
  int rc,sd;
  int opt=1;
  struct sockaddr_in localAddr, servAddr;
  struct hostent *h;

  h = gethostbyname(host);
  if(h==NULL) {
    printf("connectSocket: unknown host '%s' (%d)\n",host,h_errno);
    return ERROR;
  }

  servAddr.sin_family = h->h_addrtype;
  memcpy((char *) &servAddr.sin_addr.s_addr, h->h_addr_list[0], h->h_length);
  servAddr.sin_port = htons(port);
  
  /* create socket */
  sd = socket(AF_INET, SOCK_STREAM, 0);
  if(sd<0) {
    perror("connectSocket: cannot open socket ");
    return ERROR;
  }

  /* bind any port number */
  localAddr.sin_family = AF_INET;
  localAddr.sin_addr.s_addr = htonl(INADDR_ANY);
  localAddr.sin_port = htons(0);

  /* set socket options */
  setsockopt(sd,SOL_SOCKET,SO_REUSEADDR,(char *)&opt,sizeof(opt));

  rc = bind(sd, (struct sockaddr *) &localAddr, sizeof(localAddr));
  if(rc<0) {
    printf("connectSocket: cannot bind port TCP %u\n",port);
    perror("error ");
    return ERROR;
  }
				
  /* connect to server */
  rc = connect(sd, (struct sockaddr *) &servAddr, sizeof(servAddr));
  if(rc<0) {
    perror("connectSocket: cannot connect ");
    return ERROR;
  }
  //printf("connectSocket: got socket host:%s port:%d sd:%d\n",host,port,sd);
  return sd;
}
/** Establish socket connections to one or more XPort sockets.
    @param select Mnemonic indicating which connection to establish
    @return OK on success, ERROR otherwise
    @see XPort.h
    @see BicamConsts.h
*/
int XPort::connectxp(ConnectionID select){
  return connectxp(select,TRUE);
}

/** Establish socket connections to one or more XPort sockets.
    @param select Mnemonic indicating which connection to establish
    @param doConfig Enable XPort configuration when connecting if TRUE
    @return OK on success, ERROR otherwise
    @see XPort.h
    @see BicamConsts.h
*/
int XPort::connectxp(ConnectionID select,boolean doConfig){
  int test;

  if(select==XPCON_ALL || select==XPCON_GPIO){
    test=connectSocket(xpConnection.getHost(),xpConnection.getpGPIO());
    if(test==ERROR){
      perror("connectxp: connect GPIO failed");
      return ERROR;
    }
    xpConnection.setsdGPIO(test);
  }
  if(select==XPCON_ALL || select==XPCON_SERIAL){
    test=connectSocket(xpConnection.getHost(),xpConnection.getpSerial());
    if(test==ERROR){
      perror("connectxp: connect SERIAL failed");
      return ERROR;
    }
    xpConnection.setsdSerial(test);    
  }

  if(doConfig==TRUE){
    XPortResponse xpr;
    XPortCommand xpc(XP_CMD_SET_DIRECTIONS,getConnection()->getDirMask(),getConnection()->getDirValues());
    writeRead(&xpc,&xpr);
    
    xpc.set(XP_CMD_SET_ACTIVE_LEVELS,getConnection()->getLevelMask(),getConnection()->getLevelValues()); 
    writeRead(&xpc,&xpr);
  }
  return OK;
}

/** Disconnect socket connections to one or more XPort sockets.
    @param select Mnemonic indicating which connection to disconnect
    @return OK on success, ERROR otherwise
    @see XPort.h
    @see BicamConsts.h
*/
int XPort::disconnectxp(ConnectionID select){
  int test=ERROR;
  int rtn=OK;

  if(select==XPCON_ALL || select==XPCON_GPIO){
    test=shutdown(xpConnection.getsdGPIO(),SHUT_RDWR);
    if(test<0){
      perror("disconnectxp: GPIO socket shutdown failed\n");
      rtn=ERROR;
    }
    close(xpConnection.getsdGPIO());
    if(test<0){
      perror("disconnectxp: GPIO socket close failed\n");
      rtn=ERROR;
    }
  }
  if(select==XPCON_ALL || select==XPCON_SERIAL){
    shutdown(xpConnection.getsdSerial(),SHUT_RDWR);
    if(test<0){
      perror("disconnectxp: SERIAL socket shutdown failed\n");
      rtn=ERROR;
    }
    close(xpConnection.getsdSerial());
    if(test<0){
      perror("disconnectxp: SERIAL socket close failed\n");
      rtn=ERROR;
    }
  }
  return rtn;
}
/** Write bytes to the specified socket descriptor.
    @param sd Socket descriptor
    @param buf pointer to character buffer
    @param len number of bytes to write
    @return Number of bytes written if successful, -1 otherwise
*/
int XPort::writeBytes(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;
}

/** Resd bytes from the specified socket descriptor into 
    a buffer.
    @param sd Socket descriptor
    @param buf pointer to character buffer
    @param len number of bytes to write
    @return Number of bytes read if successful, ERROR otherwise
*/
int XPort::readBytes(int sd, char *buf, int len){
  int n;

  n = recv(sd, &buf[0], len, 0);
  if (n<0) {
    perror("readResponse: cannot receive data ");
    return ERROR;
  } else if (n==0) {
    printf("readResponse: connection closed by client\n");
    close(sd);
    return ERROR;
  }

  return n;

}

/** Send XPortCommand to XPort GPIO port.
    @param xpCommand Command to send
    @return number of bytes written, -1 otherwise
 */
int XPort::writeCommand( XPortCommand *xpCommand){

  char bytes[XPORT_COMMAND_LEN];
  int sd=xpConnection.getsdGPIO();

  return writeBytes(sd,xpCommand->toBytes(bytes),XPORT_COMMAND_LEN);
}

/** Read response to XPortCommand into XPortResponse.
    @param xpResponse pointer to XPResponse to store results in
    @return number of bytes read, ERROR otherwise
 */
int XPort::readResponse( XPortResponse *xpResponse){
  char buf[XPORT_RESPONSE_LEN];
  int sd=xpConnection.getsdGPIO();

  readBytes(sd,buf,XPORT_RESPONSE_LEN);

  if(xpResponse==NULL){
    return ERR_NULL;
  }else{
    return xpResponse->parse(buf);
  }
}
/** Write XPCommand and get response in single operation.
    @param xpCommand Command to send
    @param xpResponse pointer to XPResponse to store results in
    @return number of bytes read, ERROR otherwise
*/
int XPort::writeRead( XPortCommand *xpCommand, XPortResponse *xpResponse){
  writeCommand(xpCommand);
  return readResponse(xpResponse);
}
/** Write bytes to XPort serial port.
    @param msg pointer to character buffer
    @param len number of bytes to send
    @param offset offset from start of buffer
    @return number of bytes sent, value <0 otherwise
*/
int XPort::writeSerial(const char *msg, unsigned int len, unsigned int offset){
  int rc;
  int sd=xpConnection.getsdSerial();

    rc = send(sd, msg, len, offset);
    if(rc<0) {
      perror("cannot send message ");
      //close(sd);
      //exit(1);
    }
    return rc;
}
/** @return XPConnection host TCP/IP address/name. */
char *XPort::getHost(){
  return xpConnection.getHost();
}
/** @return XPConnection GPIO TCP/IP port. */
unsigned int XPort::getpGPIO(){
  return xpConnection.getpGPIO();
}
/** @return XPConnection serial port TCP/IP port. */
unsigned int XPort::getpSerial(){
  return xpConnection.getpSerial();
}
/** @return XPConnection GPIO socket descriptor. */
int XPort::getsdGPIO(){
  return xpConnection.getsdGPIO();
}
/** @return XPConnection serial port socket descriptor. */
int XPort::getsdSerial(){
  return xpConnection.getsdSerial();
}


/*
int main(){
  char buf[XPORT_COMMAND_LEN];

  cout << "so far so good\n" << endl;
  cout << ">>> test XPortCommand" << endl;
  cout << "setCommand 0x77 0x4 0x5 "<<endl;
  XPortCommand xpc;
  xpc.set(0x77,0x4,0x5);
  cout << hex << "0x" << (int)xpc.getCommand() << " " << xpc.getP1() << " " << xpc.getP2() << endl;
  cout << "is valid (should be NO) " << (xpc.validate()==IS_VALID?"YES":"NO") << endl;
  xpc.set(GET_DIRECTIONS,0x4,0x7);
  cout << "is valid (should be YES) " << (xpc.validate()==IS_VALID?"YES":"NO") << endl;
  
  unsigned char foo=SET_ACTIVE_LEVELS;
  unsigned int bar=0x8DEF,baz=0xBEAF;
  memcpy(&buf[0],&foo, XP_CMD_LEN);
  memcpy(&buf[1],&bar, XP_PARAM_LEN);
  memcpy(&buf[5],&baz, XP_PARAM_LEN);
  xpc.parse(buf);
  cout << "parse command 0x1a 0x8def 0xbeaf and do showCommand" << endl;
  cout << hex << "0x" << (int)xpc.getCommand() << " 0x" << xpc.getP1() << " 0x" << xpc.getP2() << endl;
  xpc.show();

  cout << ">>> test XPortResponse" << endl;
  cout << "setCommand 0x77 0x4 0x5 "<<endl;
 XPortResponse xpr;
  xpr.set(0x77,0x4);
  cout << hex << "0x" << (int)xpr.getCommand() << " " << xpr.getP1() << endl;
  cout << "is valid (should be NO) " << (xpr.validate()==IS_VALID?"YES":"NO") << endl;
  xpr.set(GET_DIRECTIONS,0x4);
  cout << "is valid (should be YES) " << (xpr.validate()==IS_VALID?"YES":"NO") << endl;

  XPort xport;
  (xport.getConnection())->setTCPIP("c80815e",GPIO_IP_PORT,SERIAL_IP_PORT1);
  (xport.getConnection())->setGPIO(CFG_GPIO_ALL,CFG_GPIO_OUT,CFG_GPIO_ALL,CFG_GPIO_HI);
  xport.connectxp(XPCON_ALL);
  cout << *(xport.getConnection()) <<endl;

  xpc.set(SET_DIRECTIONS,CFG_GPIO_ALL,CFG_GPIO_HI);
  xport.writeCommand(&xpc);
  xport.readResponse(&xpr);

  xpc.set(SET_ACTIVE_LEVELS,CFG_GPIO_ALL,CFG_GPIO_HI);
  xport.writeCommand(&xpc);
  xport.readResponse(&xpr);


  xpc.set(SET_CURRENT_STATES,CFG_GPIO_ALL,CFG_GPIO_HI);
  xport.writeRead(&xpc,&xpr);
  cout<<"cmd:"<<xpc<<endl;
  cout<<"res:"<<xpr<<endl;

  sleep(1);

  xpc.set(SET_CURRENT_STATES,CFG_GPIO_ALL,CFG_GPIO_LO);
  xport.writeRead(&xpc,&xpr);
  cout<<"cmd:"<<xpc<<endl;
  cout<<"res:"<<xpr<<endl;

  xport.writeSerial("This is a test message\r\n");

  xport.disconnectxp(XPCON_ALL);
  return 0;
}
*/