/** \file bicamTest.cc
    Unit tests for Bicam classes.
*/
/****************************************************************************/
/* Copyright (c) 2008 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Test the BIAUV Camera interface elements                      */
/* Filename : bicamTest                                                     */
/* Author   : K Headley                                                     */
/* Project  : Benthic Imaging AUV                                           */
/* Created  : 04/01/2008                                                    */
/****************************************************************************/
/* Modification History:                                                    */
/*  $Id: bicamTest.cc,v 1.8 2012/04/06 01:09:13 headley Exp $   */
/*  $Name: FastSim $ */
/****************************************************************************/

#include <stdarg.h>
#include <signal.h>
#include "XPortGPIO.h"
#include "Bicam.h"
#include "Utils.h"


int iterations=5;
double triggerDelaySec=1.5;
double updateDelaySec=0.5;
double strobeDelaySec=1.5;

/** Signal handler (SIGTERM) for BicamGPS process.
    @param sig signal
 */
void sig_handler(int sig){
  switch(sig){
  case SIGTERM:
    cout<<"GPS got SIGTERM"<<endl;
    exit(0);
    break;
  default:
    cout<<"unhandled signal"<<endl;
  }
}


/** Print use message.
    @param prog program name
 */
void printUsage(char *prog){
  cout<<endl;
  cout<<"usage: "<<prog<<" [<options>...] <XPort host addresss>"<<endl<<endl;
  cout<<"Options:"<<endl;
  cout<<"  -h                  help (returns usage message)"<<endl;
  cout<<"  -H <host>           XPort host address (may also specify as last argument)"<<endl;
  cout<<"  -g <gpio IP port>   GPIO TCP/IP port                      ["<<GPIO_IP_PORT<<"]"<<endl;
  cout<<"  -s <serial IP port> Serial TCP/IP port                    ["<<SERIAL_IP_PORT<<"]"<<endl;
  cout<<"  -n <iterations>     Trigger Iterations (don't stop if <0) ["<<iterations<<"]"<<endl;
  cout<<"  -t <delaySec>       Delay between triggers (decimal sec)  ["<<triggerDelaySec<<"]"<<endl;
  cout<<"  -u <delaySec>       Delay between GPS value updates       ["<<updateDelaySec<<"]"<<endl;
  cout<<"  -S <delaySec>       Strobe warm-up delay                  ["<<strobeDelaySec<<"]"<<endl;

  cout<<endl;
}


/**
  Demonstrates and tests use of XPort, XPortGPIO and BicamGPS in an 
  application. Requires XPort hardware or suitable proxy.
  @see XPortSim.cc
*/
int main (int argc, char *argv[]) {
  char *host=NULL;
  int serialIPPort=SERIAL_IP_PORT1;
  int gpioIPPort=GPIO_IP_PORT;
  int testScan;
  int c;
  int gpsPID;

   
  // Print use message if no args supplied 
  if(argc == 1 ) {
    printUsage(argv[0]);
    exit(1);
  }

   
  // parse command line arguments 
  opterr = 0;
  while((c= getopt(argc, argv, "g:hH:n:t:u:s:S:")) != -1 ){
    switch(c)
      {
      case 'g':
	testScan=sscanf(optarg,"%d",&gpioIPPort);
	if(testScan<1 || gpioIPPort<1000){
	  cerr<<"Invalid GPIO IP Port: ["<<optarg<<"] - expecting integer > 1000"<<endl;
	  exit(-1);
	}
	break;
      case 'h':
	printUsage(argv[0]);
	exit(0);
	break;
      case 'H':
	host=optarg;
	cout<<"XPort host name set to "<<host<<endl;
	break;
      case 'n':
	testScan=sscanf(optarg,"%d",&iterations);
	if(testScan<1){
	  cerr<<"Invalid Iteration value: ["<<optarg<<"] - expecting integer"<<endl;
	  exit(-1);
	}
	break;
      case 's':
	testScan=sscanf(optarg,"%d",&serialIPPort);
	if(testScan<1 || serialIPPort<1000){
	  cerr<<"Invalid Serial IP Port: ["<<optarg<<"] - expecting integer > 1000"<<endl;
	  exit(-1);
	}
	break; 
      case 'S':
	testScan=sscanf(optarg,"%lf",&strobeDelaySec);
	if(testScan<1 || strobeDelaySec<0.0){
	  cerr<<"Invalid Strobe Delay value: ["<<optarg<<"] - expecting decimal seconds >0"<<endl;
	  exit(-1);
	}
	break;
      case 't':
	testScan=sscanf(optarg,"%lf",&triggerDelaySec);
	if(testScan<1 || triggerDelaySec<0.0){
	  cerr<<"Invalid Trigger Delay value: ["<<optarg<<"] - expecting decimal seconds >0"<<endl;
	  exit(-1);
	}
	break;
      case 'u':
	testScan=sscanf(optarg,"%lf",&updateDelaySec);
	if(testScan<1 || updateDelaySec<0.0){
	  cerr<<"Invalid Update Delay value: ["<<optarg<<"] - expecting decimal seconds >0"<<endl;
	  exit(-1);
	}
	break;
     default:
	fprintf(stderr,"Unknown option: [%c]",optopt);
	exit(-1);
	break;
      }
  }
   
  // choose last arg as host if none chosen 
  if(argc>optind){
    host=argv[argc-1];
  }else
  if(host==NULL){
    printUsage(argv[0]);
    exit(1);
  }

   
  //  Create an XPort 
  //  Arguments (in order):
  //  XPort hostname or IP address
  //  TCP/IP port to use for GPIO
  //  TCP/IP port to use for Serial
  //  GPIO pin direction mask (which pins affect for input/output)
  //  GPIO pin direction levels (select input(0)/output(1))
  //  GPIO pin level mask (which pins affect for output level)
  //  GPIO pin level levels (select active LO(0)/active HI(1))
  //     
  //  here, we set all setting all GPIO pins as active HI outputs.
  
  XPort xport(host,
	      gpioIPPort,serialIPPort,
			  CFG_GPIO_DIR_MASK,
			  CFG_GPIO_DIR_LEVEL_MASK,
			  CFG_GPIO_LEVEL_MASK,
			  CFG_GPIO_ACTIVE_LEVEL_MASK);
  
  // Connect to the XPort 
  // (both serial and gpio sockets)
  
  xport.connectxp(XPCON_ALL);


   
  //  Create a XPortGPIO
  //  Constructor arguments:
  //  an XPort reference
  //  GPIO pin select mask (which pins to activate when triggering camera)
  //  GPIO pin direction mask (which pins to set as outputs(1)/inputs(0))
  //  GPIO pin active level mask (which pins to set active HI(0)/active LO(1))
  //  GPIO pin state mask (which pins to set active(1)/inactive(0)))
  XPortGPIO bicTrigger(&xport,
					   CAMTRIGGER_GPIO_SELECT_MASK,
					   CAMTRIGGER_GPIO_DIR_MASK,
					   CAMTRIGGER_GPIO_ACTIVE_LEVEL_MASK,
					   CAMTRIGGER_GPIO_STATE_MASK);
	
  cout<<"got gpio connection:"<<xport.getHost();
  cout<<" GPIO IP port:"<<xport.getpGPIO();
  cout<<" GPIO sd:"<<xport.getsdGPIO()<<endl;

  //  Create a XPortGPIO to control strobe enable
  //  Constructor arguments:
  //  an XPort reference
  //  GPIO pin select mask (which pins to activate when triggering camera)
  //  GPIO pin direction mask (which pins to set as outputs(1)/inputs(0))
  //  GPIO pin active level mask (which pins to set active HI(0)/active LO(1))
  //  GPIO pin state mask (which pins to set active(1)/inactive(0)))
  XPortGPIO bicStrobe(&xport,
					  STROBE_DISABLE_GPIO_SELECT_MASK,
					  STROBE_DISABLE_GPIO_DIR_MASK,
					  STROBE_DISABLE_GPIO_ACTIVE_LEVEL_MASK,
					  STROBE_DISABLE_GPIO_STATE_MASK);
   
  // Create a BicamGPS 
  // Constructor arguments:
  // an XPort reference
  
  BicamGPS bicGPS(&xport);
  cout<<"got serial connection:"<<xport.getHost();
  cout<<" Serial IP port:"<<xport.getpSerial();
  cout<<" Serial sd:"<<xport.getsdSerial()<<endl;

   
  // For this test, fork a separate process for the
  // GPS. Here, the GPS is updated every second. A camera plugged into
  // the XPort serial port should receive valid GPS signals (GPGGA, GPRMC).
  
  // In the main (trigger) process the camera is triggered every 3 seconds
  // via the XPort GPIO. The specified GPIO pin(s) should become active
  // when the camera is triggered.
  
  gpsPID=fork();
  if(gpsPID<0){
    perror("bicamTest: fork");
    exit(-1);
  }
  if(gpsPID==0){
    signal(SIGTERM,sig_handler);
     
    // GPS trigger loop
    // Sweep lat, lon, alt and time
    
    while(1){
      double lat=-45.0,lon=45.0,alt=0.0;
      int i;
      time_t now;
      struct timeval tvnow;
      for(i=0;i<5;i++){
	lat-=1.5;
	lon+=1.5;
	alt+=0.25;
	cout<<"writing GPS"<<endl;
	time(&now);
	gettimeofday(&tvnow,NULL);
	// Here are the main GPS calls:
	//
	// update sets the GPS location
	// (note that we only change four
	// parameters; the others are set 
	// in initialization)
	//
	// writeNMEA formats the NMEA strings
	// and sends them to the XPort
	bicGPS.update(tvnow,lat,lon,alt);
	bicGPS.writeNMEA();
	delay(updateDelaySec);
      }
    }
  }
  int i;

  cout<<"enabling strobe"<<endl;
  bicStrobe.setActive();

  for(i=0; (iterations<=0) || (i<iterations);i++){     
    cout<<iterations<<" "<<i<<endl;
    // pulse() activates the
    // XPort GPIO pin(s) selected in the 
    // XPort constructor by turning them on
    // and immediately off again.
    delay(strobeDelaySec);
    cout<<"triggering camera"<<endl;
    bicTrigger.pulse();
    delay(triggerDelaySec);
  }

  cout<<"disabling strobe"<<endl;
  bicStrobe.setInactive();
  
  // Issue SIGTERM to GPS process so 
  // it can clean up if necessary.
  
  cout<<"killing GPS..."<<endl;
  if(gpsPID>0){
    kill(gpsPID,SIGTERM);
  }

  cout<< "tcamif: done"<<endl;
  return 0;
}