/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : psa916Test.cc                                                 */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include <conio.h>
#include <process.h>
#include <signal.h>
#include <stdlib.h>
#include "RangeFinderIF.h"
#include "Exception.h"
#include "StringConverter.h"
#include "MathP.h"
#include "System.h"
#include "Task.h"
#include "Syslog.h"
#include "SerialParameters.h"

class PSA916Test : public Task {

public:

  PSA916Test(SerialParameters *serialParams);
  virtual ~PSA916Test();

  virtual void run();

  pid_t _serverPid;
protected:

  RangeFinderIF *_rangeFinder;

};


PSA916Test::PSA916Test(SerialParameters *serialParams) 
  : Task("psa916Test")
{

  char argString[100];
  sprintf(argString, "-serial %s", serialParams->string());

  // Start tailcone server
  if ((_serverPid = System::spawn("psa916Server", argString)) == -1) {
    throw Exception("System::spawn(psa916Server) failed");
  }

  _rangeFinder =  new RangeFinderIF("psa916");


  DeviceIF::Name deviceName;
  DeviceIF::Name serialNo;

  _rangeFinder->name(deviceName);
  _rangeFinder->serialNumber(serialNo);

  fprintf(stderr, "Device name: %s, serial#: %s\n", deviceName, serialNo);

}


PSA916Test::~PSA916Test()
{
  Boolean debug = True;
  dprintf("PSA916Test::PSA916Test() - delete _rangeFinder...\n");
  delete _rangeFinder;
  dprintf("PSA916Test::PSA916Test() - kill _serverPid %d...\n", _serverPid);
  //  kill(_serverPid, SIGTERM);
}


void PSA916Test::run()
{
  char buf[256];
  Boolean echoReceived;
  Boolean debug = True;
  double value;
  double range;
  long rangeTime_s;
  long rangeTime_ns;
  TimeIF::TimeSpec sampleTime;

  while (True) {

    if (_rangeFinder->range(&echoReceived, &range, 
			    &sampleTime) != DeviceIF::Ok)
      fprintf(stderr, "Error from psa916->range()\n");
    else 
      printf("Echo: %c, Range = %5.2lf,time = %ld, %ld\n", 
	     (echoReceived ? 'Y' : 'N'), range, 
	     sampleTime.seconds, sampleTime.nanoSeconds);

    sleep(1);
  }

  dprintf("Leaving PSA916Test::run()...\n");
}


int main(int argc, char **argv)
{
  Boolean debug = True;
  Boolean error = False;

  PSA916Test *test;

  try {
    SerialParameters serialParams(&argc, argv);
    test = new PSA916Test(&serialParams);
    test->run();
  }
  catch (Exception e) {
    fprintf(stderr, "Caught exception: %s\n", e.msg);
    error = True;
  }
  catch (...) {
    fprintf(stderr, "Caught some exception...");
    error = True;
  }

  dprintf("delete PSA916Test object!");
  delete test;

  // Signal all children
  kill(0, SIGTERM);
  Syslog::write("OK, LEAVING!!!\n");

  if (error)
    return 1;
  else
    return 0;
}