/****************************************************************************/
/* 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 Octans device driver

  AUTHOR: Hans Thomas 

  $Id: _octansTest.cc,v 1.4 2001/07/19 06:02:25 hthomas Exp $
 *-----------------------------------------------------------------------*/

#include <signal.h>
#include <i86.h>
#include "MathP.h"
#include "OctansIF.h"
#include "Task.h"
#include "System.h"
#include "SerialParameters.h"

class OctansTest : public Task {

public:

  OctansTest(SerialParameters *serialParams);
  ~OctansTest();

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


OctansTest::OctansTest(SerialParameters *serialParams) 
  : Task("octansTest")
{
  char argString[100];
  sprintf(argString, "-serial %s", serialParams->string());

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

  ahrs = new OctansIF("Octans", OctansIFServerName);
  printf("ahrs is 0x%x\n", ahrs);
}


void OctansTest::run() {

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

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

  while (True) {

    TimeIF::TimeSpec sampleTime;
    AhrsIF::Attitude attitude;
    double accX, accY, accZ;
    long status;
    DeviceIF::Status octansStatus;

    octansStatus = ahrs->all(&attitude, 
			     &accX, &accY, &accZ, &status,
			     &sampleTime);

    if (octansStatus == 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, "status= 0x%x\n", status);
    } else if (octansStatus == DeviceIF::Initializing) {
      printf("octans is initializing\n");
    } else if (octansStatus == DeviceIF::Error) {
      fprintf(stderr, "Got device error from octans\n");
    }

    delay(250);
  }
}


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



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

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

  delete test;

  return 0;
}