static char ArtificialHorizon_id[] = "$Header: /usr/tiburon/unix/gui/tmacs/RCS/ArtificialHorizon.cc,v 1.1 1996/10/28 09:11:46 oreilly Exp $";

/*
$Log: ArtificialHorizon.cc,v $
Revision 1.1  1996/10/28 09:11:46  oreilly
Initial revision

*/
#include <math.h>
#include <Xm/Form.h>
#include <Xm/Label.h>
#include "ArtificialHorizon.h"
#include "ArtificialHorizon_xrt.h"
#include "kalmanDM.h"

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

  if ((_xrtData = XrtMakeData(XRT_DATA_ARRAY, 1, 2, True)) == NULL)
  {
    sprintf(errorBuf, "XrtMakeData() failed for _xrtData (2 points)");
    setError(errorBuf);
    return;
  }

  if ((_xrtData2 = XrtMakeData(XRT_DATA_ARRAY, 2, 2, True)) == NULL)
  {
    sprintf(errorBuf, "XrtMakeData() failed for _xrtData2 (2 points)");
    setError(errorBuf);
    return;
  }

  _graphHalfWidth = 10.;
  _yOffset = 100.;

  // Second dataset just contains horizontal and vertical lines
  _xrtData2->arr_xel(0) = -_graphHalfWidth - 1.;
  _xrtData2->arr_xel(1) = _graphHalfWidth + 1.;  
  _xrtData2->arr_yel(0, 0) = _yOffset;
  _xrtData2->arr_yel(0, 1) = _yOffset;

  _xrtData2->arr_xel(0) = 0.;
  _xrtData2->arr_xel(1) = 0.;
  _xrtData2->arr_yel(1, 0) = _yOffset + _graphHalfWidth + 1;
  _xrtData2->arr_yel(1, 1) = _yOffset - _graphHalfWidth - 1;
  
  _graph = ArtificialHorizon_xrt_create(_baseWidget);

  XtVaSetValues(_graph,
		XtNxrtXAxisMin, 
		XrtFloatToArgVal(-_graphHalfWidth),
		XtNxrtXAxisMax, 
		XrtFloatToArgVal(_graphHalfWidth),
		XtNxrtYAxisMin, 
		XrtFloatToArgVal(-_graphHalfWidth + _yOffset),
		XtNxrtYAxisMax, 
		XrtFloatToArgVal(_graphHalfWidth + _yOffset),
		XtNxrtY2AxisMin, 
		XrtFloatToArgVal(-_graphHalfWidth + _yOffset),
		XtNxrtY2AxisMax, 
		XrtFloatToArgVal(_graphHalfWidth + _yOffset),
		XtNxrtFrontDataset, XRT_DATASET2,
		XtNxrtY2AxisShow, False,
		XtNxrtGraphMarginBottom, 0,
		XmNleftAttachment, XmATTACH_FORM,
		XmNrightAttachment, XmATTACH_FORM,
		XmNtopAttachment, XmATTACH_FORM,
		NULL);
  
  _pitchRoll = XtVaCreateManagedWidget("pitchRoll", xmLabelWidgetClass,
				       _baseWidget, 
				       XmNleftAttachment, XmATTACH_FORM,
				       XmNrightAttachment, XmATTACH_FORM,
				       XmNtopAttachment, XmATTACH_WIDGET,
				       XmNtopWidget, _graph,
				       XmNbottomAttachment, XmATTACH_FORM,
				       NULL);
  
  char itemName[MaxDmNameLen];

  strcpy(itemName, PITCH_DM);
  _dmPitch = new DmFlt32Object(itemName);
  if (_dmPitch->error())
  {
    _dmPitch->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  if (addDmObject(_dmPitch, DmConsumeFlag) == -1)
    return;

  strcpy(itemName, ROLL_DM);
  _dmRoll = new DmFlt32Object(itemName);
  if (_dmRoll->error())
  {
    _dmRoll->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  if (addDmObject(_dmRoll, DmConsumeFlag) == -1)
    return;
  
  if (setConsumePeriod(period) == -1)
    return;
  
  updateGui();
}


ArtificialHorizon::~ArtificialHorizon()
{
  XrtDestroyData(_xrtData, True);
  XrtDestroyData(_xrtData2, True);
}


void ArtificialHorizon::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 (roll == _prevRoll && pitch == _prevPitch)
    return;
  
  _prevRoll = roll;
  _prevPitch = pitch;
  
  double tanRoll = tan(roll);
  
  _xrtData->arr_xel(0) = _graphHalfWidth;
  _xrtData->arr_yel(0, 0) = (-_graphHalfWidth * tanRoll) + _yOffset;

  _xrtData->arr_xel(1) = -_graphHalfWidth;
  _xrtData->arr_yel(0, 1) = (_graphHalfWidth * tanRoll) + _yOffset;

  XtVaSetValues(_graph,
		XtNxrtData, _xrtData,
		XtNxrtData2, _xrtData2,
		XtNxrtYOrigin, XrtFloatToArgVal(0.),
		NULL);


  char buf[100];
  sprintf(buf, "Pitch: %+5.1f  Roll: %+5.1f", 
	  pitch / RADS_PER_DEG, roll / RADS_PER_DEG);

  XmString xmstr = XmStringCreateSimple(buf);
  XtVaSetValues(_pitchRoll, XmNlabelString, xmstr, NULL);
  XmStringFree(xmstr);
}