/****************************************************************************/
/* 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: _dvlSideTest.cc,v 1.1 2012/10/16 16:19:22 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 = False;

  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, "-v -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");
  }
  //
  // Give the various Dvl tasks time to initialize before opening the 
  // interface.  It would be better to do this with a semTake & semPut, 
  // but I don't see how to do  here in Qnx.
  //
  // Also, there may be a way to check using something like dvlIF->connected(),
  // but I'll be danged if I can figure it out right now.
  //
  sleep(5);

  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 ( !kbhit() )   //This leaves worksite, dvlServer, and dvl still 
                         //running.

  while (True) 
  {
    TimeIF::TimeSpec sampleTime;

    if (dvlIF->get( &data, &newData ) != OK)
      Syslog::write(" dvlTest:: Invalid Dvl Data.\n");
    //
    // Clear the screen:
    //
    // printf("\033[2J");

    printf(" newData = %d\n"
	   " baddComms = %d\n"
	   " bottomTrackVelocity = [%8.3f %8.3f %8.3f %8.3f]'\n"
	   " bottomStatus = %0x\n"
	   " waterMassVelocity   = [%8.3f %8.3f %8.3f %8.3f]'\n"
	   " waterStatus = %0x\n"
	   " dataStatus = %0x\n"
	   " Range  = %f\n"
	   " Pitch  = %f\n"
	   " Roll   = %f\n"
	   " Heading= %f\n"
	   " Temp   = %f\n",
	   newData,
	   data.badComms,
	   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;
}