/****************************************************************************/
/* Copyright (c) 2008 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : operate XPort from command line                               */
/* Filename : xportIO.cc                                                  */
/* Author   : K Headley                                                     */
/* Project  : Benthic Imaging AUV                                           */
/* Created  : 04/01/2008                                                    */
/****************************************************************************/
/* Modification History:                                                    */
/*  $Id: xportIO.cc,v 1.3 2012/04/05 22:42:28 headley Exp $    */
/*  $Name: FastSim $  */
/****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <unistd.h>

#include "XPort.h"
#include "BicamConsts.h"

#ifdef QNX
using namespace std;
#endif


/* 
   local function definitions 
*/
void printUsage(char *prog){
  cout<<endl;
  cout<<"usage: "<<prog<<" [<options>...] <XPort host addresss>\n"<<endl;
  cout<<endl;
  cout<<"  -d         : get direction"<<endl;
  cout<<"  -D [p1,p2] : set direction"<<endl;
  cout<<"  -f         : get functions"<<endl;
  cout<<"  -h         : print usage message"<<endl;
  cout<<"  -H [host]  : xport host to use (use last argument by default)"<<endl;
  cout<<"  -l         : get active levels"<<endl;
  cout<<"  -L [p1,p2] : set active levels"<<endl;
  cout<<"  -m [msg]   : send message"<<endl;
  cout<<"  -s         : get current states"<<endl;
  cout<<"  -S [p1,p2] : set current states"<<endl;
  cout<<endl;
  cout<<"where"<<endl;
  cout<<"  p1 - gpio select mask"<<endl;
  cout<<"  p2 - gpio value mask"<<endl;
  cout<<"  msg - message string (enclosed in single quotes if it contains whitespace)"<<endl;
  cout<<endl;
  cout<<"  p1 and p2 are hex integers from 0x0-0xFFFF, e.g., 0x04,0x3A."<<endl;
  cout<<"  Leading '0x' and leading zeros are permitted but not required."<<endl;
  cout<<"  NOTE: XPort firmware supports up to 32 GPIO pins, but implements only 3 pins"<<endl;
  cout<<endl;
}

XPortCommand *makeCommand(unsigned char cmd,unsigned int p1,unsigned int p2){
  return new XPortCommand(cmd,p1,p2);
}

/* Main Entry Point */
int main (int argc, char *argv[]){

  int c;
  int testScan;
  unsigned int param1=0,param2=0;
  //int test;
  XPortResponse xpResponse;
  char* xportHost=NULL;

  XPortCommand *xpCommands[MAX_XPORT_COMMANDS];
  char *messages[MAX_XPORT_COMMANDS];
  unsigned int i,commandCount=0;  
  unsigned int messageCount=0;
  
  if(argc < 2) {
    printUsage(argv[0]);
    exit(1);
  }


  /* parse command line arguments */
  opterr = 0;
  while((c= getopt(argc, argv, "hH:fdD:lL:m:sS:")) != -1 ){
    switch(c)
      {
      case 'd':
	/* get direction */
	xpCommands[commandCount++]=makeCommand(XP_CMD_GET_DIRECTIONS,CFG_MASK_GPIO_SEL_NONE,CFG_GPIO_NONE);
	break;
      case 'D':
	/* set direction */
	testScan=sscanf(optarg,"%x,%x",&param1,&param2);
	if(testScan<2){
	  cerr<<"Invalid direction specified: ["<<optarg<<"] - expecting comma-separated hex values"<<endl;
	  exit(-1);
	}
	xpCommands[commandCount++]=makeCommand(XP_CMD_SET_DIRECTIONS,param1,param2);
	break;
      case 'f':
	/* get functions */
	xpCommands[commandCount++]=makeCommand(XP_CMD_GET_FUNCTIONS,CFG_MASK_GPIO_SEL_NONE,CFG_GPIO_NONE);
	break;
      case 'h':
	printUsage(argv[0]);
	exit(0);
	break;
      case 'H':
	xportHost=optarg;
	if(strlen(xportHost)>MAXHOSTNAMELEN){
	  cerr<<"host name exceeds MAXHOSTNAMELEN ("<<MAXHOSTNAMELEN<<")"<<endl;
	  exit(1);
	}
	cout<<"host set to "<<xportHost<<endl;
	break;
      case 'l':
	/* get active levels */
	xpCommands[commandCount++]=makeCommand(XP_CMD_GET_ACTIVE_LEVELS,CFG_MASK_GPIO_SEL_NONE,CFG_GPIO_NONE);
	break;
      case 'L':
	/* set active levels */
	testScan=sscanf(optarg,"%x,%x",&param1,&param2);
	if(testScan<2){
	  cerr<<"Invalid level specified: ["<<optarg<<"] - expecting comma-separated hex values"<<endl;
	  exit(-1);
	}
	xpCommands[commandCount++]=makeCommand(XP_CMD_SET_ACTIVE_LEVELS,param1,param2);
	break;
      case 'm':
	messages[messageCount]=optarg;
	xpCommands[commandCount++]=makeCommand(XP_CMD_SEND_MESSAGE,messageCount++,0x00);
	break;
      case 's':
	/* get current states */
	xpCommands[commandCount++]=makeCommand(XP_CMD_GET_CURRENT_STATES,CFG_MASK_GPIO_SEL_NONE,CFG_GPIO_NONE);
	break;
      case 'S':
	/* set current states */
	testScan=sscanf(optarg,"%x,%x",&param1,&param2);
	if(testScan<2){
	  cerr<<"Invalid state specified: ["<<optarg<<"] - expecting comma-separated hex values"<<endl;
	  exit(-1);
	}
	xpCommands[commandCount++]=makeCommand(XP_CMD_SET_CURRENT_STATES,param1,param2);
	break;
      default:
	cerr<<"Unknown option: ["<<optopt<<"]"<<endl;
	exit(-1);
	break;
      }

    if(commandCount>=MAX_XPORT_COMMANDS){
      cerr<<"too many commands on command line ("<<MAX_XPORT_COMMANDS<<" max)"<<endl;
      exit(-1);
    }
  }

  if((argc-1)>=optind){
    xportHost=argv[argc-1];
  }else
  if(xportHost==NULL){
    cout<<"argc="<<argc<<" optind="<<optind<<" host=NULL"<<endl;
    cerr<<endl<<"No XPort host specified"<<endl;
    printUsage(argv[0]);
    exit(1);
  }

  cout<<"argc="<<argc<<" optind="<<optind<<" host="<<xportHost<<endl;
	
  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;
  */
  xport.getConnection()->configAll(xportHost,
				   GPIO_IP_PORT,SERIAL_IP_PORT,
				   CFG_GPIO_ALL,CFG_GPIO_OUT,
				   CFG_GPIO_ALL,CFG_GPIO_HI);

  xport.connectxp(XPCON_ALL,FALSE);

  for(i=0;i<commandCount;i++){
    cout<<i<<" :";
    if(xpCommands[i]!=NULL){
      if(xpCommands[i]->getCommand()==XP_CMD_SEND_MESSAGE){
	cout<<" >> "<<xpCommands[i];
	xport.writeSerial(messages[xpCommands[i]->getP1()],strlen(messages[xpCommands[i]->getP1()]),0);
	xport.writeSerial("\r\n",strlen("\r\n"),0);
	delete(xpCommands[i]);
      }else{
	cout<<" >> "<< *xpCommands[i];
	xport.writeRead(xpCommands[i],&xpResponse);
	cout<<" << "<<xpResponse<<endl;
	delete(xpCommands[i]);
      }
    }
  }
  cout<<endl;
  xport.disconnectxp(XPCON_ALL);

  return 0;
  
}