/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : _dvlTest.   cc                                                */
/* Author   : Rob McEwen, Tom O'Reilly                                      */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 23 Mar 01                                                     */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*-----------------------------------------------------------------------*

  $Id: _dvlTest.cc,v 1.6 2001/04/02 20:23:56 rob Exp $
  *-----------------------------------------------------------------------*/

#include <stdlib.h>
#include <stdio.h>
#include <conio.h>
#include <signal.h>
#include <process.h>
#include <time.h>

#include "DvlIF.h"
#include "Syslog.h"
#include "System.h"
#include "SerialParameters.h"

int main( int argc, char **argv ) 
{
  DvlIF *dvlIF;
  DvlIF::Data data;
  Boolean newData;

  double depth, temp, pressure;

  pid_t _serverPid;
  pid_t workSiteServer;

  const int sleepMillisecs = 500;
  struct timespec sleepTime;

  sleepTime.tv_sec = 0;
  sleepTime.tv_nsec = sleepMillisecs * 1000000;

  if ((workSiteServer = System::spawn("workSite",  "")) == -1) 
    throw Exception("System::spawn(workSite Server) failed");

  printf(" dvlTest: Started WorkSite Server.\n");

  try 
  { 
    SerialParameters serialParams(&argc, argv);
    char argString[100];
    sprintf(argString, "-serial %s", serialParams.string());
    if ((_serverPid = System::spawn("dvlServer", argString)) == -1)
      {
	Syslog::write("System::spawn(dvlServer) failed\n");
      }
  }
  catch ( ... )
  {
    Syslog::write("dvlTest -- "
		  "caught exception on spawning of Dvl Server\n");
  }

  try 
  {
    dvlIF = new DvlIF( "Dvl", 1000 );
  }
  catch (...)
  {
    Syslog::write("dvlTest -- Caught exception on IF creation\n");
    exit( -1 );
  }

  printf("Press any key to exit\n");

  while (True) 
  {
    TimeIF::TimeSpec sampleTime;

    if (dvlIF->get( &data, &newData ) != OK)
      fprintf(stderr, "dvlTest:: Invalid DVL Velocity\n");

    printf("\n\n\n"
	   " newData = %d\n"
	   " bottomTrackVelocity = [%f %f %f %f]'\n"
	   " bottomStatus = %0x\n"
	   " waterMassVelocity   = [%f %f %f %f]'\n"
	   " waterStatus = %0x\n"
	   " dataStatus = %0x\n"
	   " Range  = %f\n"
	   " Pitch  = %f\n"
	   " Roll   = %f\n"
	   " Heading= %f\n"
	   " Temp   = %f\n",
	   newData,
	   data.bottomTrackVelocity[0],
	   data.bottomTrackVelocity[1],
	   data.bottomTrackVelocity[2],
	   data.bottomTrackVelocity[3],
	   data.bottomStatus,
	   data.waterMassVelocity[0],
	   data.waterMassVelocity[1],
	   data.waterMassVelocity[2],
	   data.waterMassVelocity[3],
	   data.waterStatus,
	   data.dataStatus,
	   data.range,
	   data.pitch,
	   data.roll,
	   data.heading,
	   data.temp);

    //nanosleep(&sleepTime, 0);
    sleep( 1 );
  }

  delete dvlIF;
  kill(0, SIGTERM);
  return 0;
}