static char Attitude_id[] = "$Header: Attitude.cc,v 1.1 96/10/28 09:11:48 oreilly Exp $";

/*
$Log:	Attitude.cc,v $
Revision 1.1  96/10/28  09:11:48  09:11:48  oreilly (Thomas C. O'Reilly)
Initial revision

*/
#include <math.h>
#include <Xm/Form.h>
#include <Xm/Label.h>
#include "Attitude.h"
#include "kalmanDM.h"
#include "Math.h"

Attitude::Attitude(const char *name, Widget parent, int period)
  : DmGuiObject(name, parent)
{
  _baseWidget = XtVaCreateWidget(name, xmFormWidgetClass, parent, NULL);
  installDestroyHandler();
  
  char errorBuf[errorMsgSize];

  _graphHalfWidth = 10.;
  _yOffset = 100.;

  _graph = new AttitudeGraph("graph", _baseWidget, 
			     20.*Math::RadsPerDeg, 20.*Math::RadsPerDeg);

  XtVaSetValues(_graph->baseWidget(),
		XmNleftAttachment, XmATTACH_FORM,
		XmNrightAttachment, XmATTACH_FORM,
		XmNtopAttachment, XmATTACH_FORM,
		NULL);
  
  _graph->show();
  
  _pitch = XtVaCreateManagedWidget("pitch", xmLabelWidgetClass,
				   _baseWidget, 
				   XmNleftAttachment, XmATTACH_FORM,
				   XmNtopAttachment, XmATTACH_WIDGET,
				   XmNtopWidget, _graph->baseWidget(),
				   XmNbottomAttachment, XmATTACH_FORM,
				   NULL);

  _roll = XtVaCreateManagedWidget("roll", xmLabelWidgetClass,
				  _baseWidget, 
				  XmNrightAttachment, XmATTACH_FORM,
				  XmNtopAttachment, XmATTACH_WIDGET,
				  XmNtopWidget, _graph->baseWidget(),
				  XmNbottomAttachment, XmATTACH_FORM,
				  NULL);
  
  char itemName[MaxDmNameLen];

/* ***********************************
  Kalman stuff is not yet ready!!!
*/
  degrees = True;
  
  strcpy(itemName, PITCH_DM);
  strcpy(itemName, "TIBURON.SENSOR.PITCH.DEGREES");
  
  _dmPitch = new DmFlt32Object(itemName);
  if (_dmPitch->error())
  {
    _dmPitch->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  if (addDmObject(_dmPitch, DmConsumeFlag) == -1)
    return;

  strcpy(itemName, ROLL_DM);
  strcpy(itemName, "TIBURON.SENSOR.ROLL.DEGREES");
  _dmRoll = new DmFlt32Object(itemName);
  if (_dmRoll->error())
  {
    _dmRoll->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  if (addDmObject(_dmRoll, DmConsumeFlag) == -1)
    return;
  
  if (setConsumePeriod(period) == -1)
    return;

  _prevPitch = _prevRoll = MAXFLOAT;
  
  updateGui();
}


Attitude::~Attitude()
{
}


void Attitude::updateGui(DM_Item handle)
{
  Flt32 pitch, roll;
  char errorBuf[errorMsgSize];
  DM_Time t;
  
  if (_dmPitch->read(&pitch, &t) != SUCCESS)
  {
    _dmPitch->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  if (_dmRoll->read(&roll, &t) != SUCCESS)
  {
    _dmRoll->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  if (degrees)
  {
    // Convert to radians
    pitch *= Math::RadsPerDeg;
    roll *= Math::RadsPerDeg;
  }
  
  _graph->setPitch(pitch);
  _graph->setRoll(roll);
  _graph->update();
  
  if (roll == _prevRoll && pitch == _prevPitch)
    return;

  char buf[100];
  double dPitch = pitch;
  double dRoll = roll;

  Math::minusPiToPi(&dPitch);
  Math::minusPiToPi(&dRoll);
  
  XmString xmstr;
  
  if (pitch != _prevPitch)
  {
    sprintf(buf, "P: %6.1f", dPitch / Math::RadsPerDeg);
    xmstr = XmStringCreateSimple(buf);
    XtVaSetValues(_pitch, XmNlabelString, xmstr, NULL);
    XmStringFree(xmstr);
  }
  if (roll != _prevRoll)
  {
    sprintf(buf, "R: %6.1f", dRoll / Math::RadsPerDeg);
    xmstr = XmStringCreateSimple(buf);
    XtVaSetValues(_roll, XmNlabelString, xmstr, NULL);
    XmStringFree(xmstr);
  } 

  _prevPitch = pitch;
  _prevRoll = roll;
}