/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : kvhTest.cc                                                    */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*-----------------------------------------------------------------------*
  PROGRAM: kvhTest

  DESCRIPTION: Binary wrapped for Kvh instruments through the AHRS IF

  $Id: kvhTest.cc,v 1.6 2000/05/12 15:29:58 oreilly Exp $
  *-----------------------------------------------------------------------*/

#include <stdio.h>
#include <string.h>
#include <signal.h>

#include "System.h"
#include "Syslog.h"
#include "Exception.h"
#include "AhrsIF.h"
#include "Task.h"
#include "Math.h"
#include "KvhServer.h"

class KvhTest : public Task {

public:

  KvhTest(char *incDevFilename, char *compDevFilename);
  ~KvhTest();

  virtual void run();
  AhrsIF *ahrs;
  pid_t serverPid;
};


KvhTest::KvhTest(char *incDevFilename, char *compDevFilename) 
  : Task("ahrsTest")
{
  char argString[100];
  sprintf(argString, "-inc %s -comp %s", incDevFilename, compDevFilename);

  if ((serverPid = System::spawn("kvhServer", argString)) == -1) {
    throw Exception("Couldn't spawn kvhServer");
  }

  ahrs = new AhrsIF("ahrs");
}


void KvhTest::run() {

  char name[256];
  char serialNo[256];
  
  ahrs->name(name);
  ahrs->serialNumber(serialNo);
  Boolean magneticCompass = ahrs->magneticCompass();

  printf("AHRS name: %s, serialNo: %s\n", name, serialNo);
  printf("Magnetic: %c\n", magneticCompass ? 'Y' : 'N');

  while (True) {

    TimeIF::TimeSpec sampleTime;
    AhrsIF::Attitude attitude;

    if (ahrs->getAttitude(&attitude, &sampleTime) != DeviceIF::Ok) {
      fprintf(stderr, "Got device error from ahrs\n");
    }
    else {

      printf("roll: %.4f  pitch: %.4f  heading: %.4f\n",
	     Math::DegsPerRad * attitude.roll, 
	     Math::DegsPerRad * attitude.pitch, 
	     Math::DegsPerRad * attitude.yaw);

      printf("rollRate: %.4f  pitchRate: %.4f  headingRate: %.4f\n",
	     Math::DegsPerRad * attitude.rollRate, 
	     Math::DegsPerRad * attitude.pitchRate, 
	     Math::DegsPerRad * attitude.yawRate);

    }

    sleep(1);
  }
}


KvhTest::~KvhTest()
{
  //  delete ahrs;
  kill(serverPid, SIGTERM);
}



int main(int argc, char **argv)
{
  Boolean error = False;
  KvhTest *test = 0;
  char *incDevFilename;
  char *compDevFilename;

  if (KvhServer::getOptions(argc, argv, 
			    &incDevFilename, 
			    &compDevFilename) == -1) {
    return 1;
  }

  try {
    test = new KvhTest(incDevFilename, compDevFilename);
    test->run();
  }
  catch (Exception e) {
    fprintf(stderr, "Caught exception: %s\n", e.msg);
    error = True;
  }
  catch (...) {
    fprintf(stderr, "Caught some exception...");
    error = True;
  }

  delete test;

  if (error)
    return 1;
  else
    return 0;

}