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

  DESCRIPTION: Client for testing the Crossbow device driver

  AUTHOR: Aaron Marsh

  $Id: crossbowTest.cc,v 1.5 2001/06/02 21:33:40 hthomas Exp $
 *-----------------------------------------------------------------------*/

#include <signal.h>
#include "MathP.h"
#include "CrossbowIF.h"
#include "Task.h"
#include "System.h"

class CrossbowTest : public Task {

public:

  CrossbowTest();
  ~CrossbowTest();

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


CrossbowTest::CrossbowTest() 
  : Task("crossbowTest")
{
  if ((serverPid = System::spawn("crossbowServer", "")) == -1) {
    throw Exception("Couldn't spawn crossbowServer");
  }

  ahrs = new CrossbowIF("ahrs");
}


void CrossbowTest::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;
    double magX, magY, magZ;
    double accX, accY, accZ;
    double temperature;

    if (ahrs->all(&attitude, 
		  &accX, &accY, &accZ,
		  &magX, &magY, &magZ,
		  &temperature,
		  &sampleTime) == DeviceIF::Ok) {

      fprintf(stderr, "Roll = %7.2lf : Pitch = %7.2lf : Yaw = %7.2lf\n",
	      Math::radToDeg(attitude.roll), 
	      Math::radToDeg(attitude.pitch),
	      Math::radToDeg(attitude.yaw) );

      fprintf(stderr, 
	      "RollRate = %7.2lf : PitchRate = %7.2lf : YawRate = %7.2lf\n",
	      Math::radToDeg(attitude.rollRate), 
	      Math::radToDeg(attitude.pitchRate),
	      Math::radToDeg(attitude.yawRate) );

      fprintf(stderr, 
	      "accX = %7.2lf : accY = %7.2lf : accZ = %7.2lf\n",
	      accX, accY, accZ);

      fprintf(stderr, 
	      "magX = %7.2lf : magY = %7.2lf : magZ = %7.2lf\n",
	      magX, magY, magZ);

      fprintf(stderr, "temperature: %.2f\n", temperature);

    }
    else {
      fprintf(stderr, "Got device error from crossbow\n");
    }

    sleep(1);
  }
}


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



int main( int argc, char **argv )
{
  CrossbowTest *test = 0;

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

  delete test;

  return 0;
}