static char RovCompass_id[] = "$Header: /usr/tiburon/unix/gui/tmacs/RCS/RovCompass.cc,v 1.1 1997/11/20 08:21:31 oreilly Exp $";

#define HeadingFormat "%.0f"

/*
$Log: RovCompass.cc,v $
Revision 1.1  1997/11/20 08:21:31  oreilly
Initial revision

*/
#include <stdlib.h>
#include <values.h>
#include <Xm/Form.h>
#include <Xm/Label.h>
#include <Xm/PushB.h>
#include <Xm/TextF.h>
#include <Vk/VkErrorDialog.h>
#include "RovCompass.h"
#include "DmErrno.h"
#include "DmInputDetector.h"
#include "controlDM.h"
#include "vehicleTurnsDM.h"
#include "parseUtils.h"

VkMenuDesc RovCompass::_menuPane[] = 
{
  {ACTION, "resetTurns", &RovCompass::resetCallback, NULL},
  {END}
};

RovCompass::RovCompass(const char *name, Widget parent, 
				 int period, 
				 int historyInterval
				 ) : 
  DmGuiObject(name, parent)
{
  MBool debug = FALSE;

  char errorBuf[errorMsgSize];

  _autoOnOff = NULL;
  
  _dmInDegrees = FALSE;
  const char *dmItemName = "TIBURON.VEHICLE.ATTITUDE";
  
  _baseWidget = XtCreateWidget(_name, xmFormWidgetClass, parent, NULL, 0);

  installDestroyHandler();
  const int headingStartCol = 40;
  const int headingEndCol = 50;
  
  XmString xmstr;

  new VkPopupMenu(_baseWidget, RovCompass::_menuPane, (XtPointer )this);
  
  // Initialize heading text
  _headingText = new DynamicLabel("heading", _baseWidget, 
				  XmALIGNMENT_BEGINNING);

  XtVaSetValues(_headingText->baseWidget(),
		XmNlabelString, xmstr,
		XmNleftAttachment, XmATTACH_POSITION,
		XmNleftPosition, headingStartCol,
		XmNtopAttachment, XmATTACH_FORM,
		NULL);		

  _headingText->setText("No Data");
  _headingText->show();
  
  _sampleTime = 0;
  int nHistoryPoints;
  
  if (historyInterval > 0)
  {
    _historySampleInterval = 
      (int )((float )historyInterval / nHistoryIntervals + 0.5);

    _saveHistory = True;

    nHistoryPoints = historyInterval / _historySampleInterval;
  }
  else
  {
    nHistoryPoints = 0;
    _saveHistory = False;
  }
  
  _face = new CompassFace("face", _baseWidget, nHistoryPoints);

  XtVaSetValues(_face->baseWidget(),
		XmNleftAttachment, XmATTACH_FORM,
		XmNrightAttachment, XmATTACH_FORM,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, _headingText->baseWidget(),
		NULL);

  _face->show();
  
  _turnsText = XtVaCreateWidget("turns", xmLabelWidgetClass, 
				_baseWidget, 
				XmNrightAttachment, XmATTACH_FORM,
				XmNtopAttachment, XmATTACH_FORM,
				NULL);

  dprintf(stderr, 
	  "RovCompass::RovCompass() - create DM item "
	  "\"%s\"\n", dmItemName);

  MBool readRaw = FALSE;
  _dmAttitude = new DmAttitudeObject(readRaw);

  if (_dmAttitude->error())
  {
    _dmAttitude->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  if (addDmObject(_dmAttitude, DmConsumeFlag) == -1)
    return;

  _dmTurnsSetpoint = new DmFlt32Object(VEHICLE_TURNS_SETPOINT_DM);
  if (_dmTurnsSetpoint->error())
  {
    _dmTurnsSetpoint->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  if (addDmObject(_dmTurnsSetpoint, DmConsumeFlag) == -1)
    return;

  _dmAutoHeading = new DmFlt32Object(SETPOINT_KALMAN_HEADING_DM);

  if (_dmAutoHeading->error())
  {
    _dmAutoHeading->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  if (addDmObject(_dmAutoHeading, DmConsumeFlag) == -1)
    return;

  if (setConsumePeriod(period) == -1)
    return;

  _dmAutoOnOff = new DmBooleanObject(AUTO_HEADING_DM);
  if (_dmAutoOnOff->error())
  {
    _dmAutoOnOff->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
    
  _autoOnOff = 
    new DmInputDetector("autoOnOff", parent, _dmAutoOnOff, this);
  
  _dmTurns = new DmInt16Object(VEHICLE_TURNS_COUNT_DM);
  if (_dmTurns->error())
  {
    _dmTurns->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  new DmInputDetector("nTurns", parent, _dmTurns, this);
  
  // Force GUI update on first call to updateGui()
  _prevHeading = NoPrevHeading;
  _prevTurns = MAXINT;
  
  updateFromPeer(this, NULL);

  _turnsDialog = new TetherTurnsDialog("turnsDialog"); 
}


void RovCompass::updateFromPeer(DmGuiObject *peer, void *clientData)
{
  Boolean debug = False;
  
  dprintf(stderr, "RovCompass::updateFromPeer()\n");
  char errorBuf[errorMsgSize];
  Errno errno;
  Int16 nTurns;
  DM_Time t;

  // Read turns counter
  if ((errno = _dmTurns->read(&nTurns, &t)) != SUCCESS)
  {
    sprintDmError(errno, 
		  "RovCompass::updateFromPeer(), error reading turns", 
		  errorBuf);

    setError(errorBuf);
    return;
  }

  char buf[100];
  
  if (nTurns != _prevTurns)
  {
    // Update turns text
    if (nTurns > 0) 
      sprintf(buf, "%2d cw\nturn", nTurns);
    else
      sprintf(buf, "%2d ccw\nturn", abs(nTurns));

    if (abs(nTurns) > 1)
      strcat(buf, "s");
    
    XmString xmstr = XmStringCreateLtoR(buf, XmSTRING_DEFAULT_CHARSET);
    XtVaSetValues(_turnsText, XmNlabelString, xmstr, NULL);
    XmStringFree(xmstr);  

    if (nTurns)
    {
      XtManageChild(_turnsText);
    }
    else
    {
      XtUnmanageChild(_turnsText);
    }

    _prevTurns = nTurns;
  }
  
  // Read value of AutoOnOff
  if ((errno = _dmAutoOnOff->read(&_autoHeadingOn, &t)) != SUCCESS)
  {
    sprintDmError(errno, 
		  "RovCompass::updateFromPeer(), error reading autohead", 
		  errorBuf);

    setError(errorBuf);
    return;
  }

  if (!_autoHeadingOn)
    _face->clearAutoHeading();

  updateGui();
}


void RovCompass::updateGui(DM_Item handle)
{
  MBool debug = FALSE;
  
  dprintf(stderr, "Compass::updateGui()\n");
  char errorBuf[errorMsgSize];
  
  // Read heading value
  StateValue state[NStateElements];
  DM_Time t;
  Errno errno;
  Flt32 heading;
  
  if ((errno = _dmAttitude->read(state, &t)) != SUCCESS)
  {
    sprintDmError(errno, "RovCompass::updateGui()", errorBuf);
    setError(errorBuf);
    return;
  }
  heading = state[HeadingIndex];
  
  if (_dmInDegrees)
    // Convert to radians
    heading *= Math::RadsPerDeg;

  _face->setHeading(heading);

  if (_autoHeadingOn)
  {
    Flt32 autoHeading;
    // Read heading setpoint
    if ((errno = _dmAutoHeading->read(&autoHeading, &t)) != SUCCESS)
    {
      sprintDmError(errno, "RovCompass::updateGui()", errorBuf);
      setError(errorBuf);
      return;
    }
    _face->setAutoHeading(autoHeading);
  }

  if ((errno = _dmTurnsSetpoint->read(&_turnsSetpoint, &t)) != SUCCESS)
  { 
    sprintDmError(errno, 
		  "RovCompass::updateGui(), error reading setpoint", 
		  errorBuf);

    setError(errorBuf);
    return;
  }
  
  _face->setSetpoint(_turnsSetpoint);
  
  // Update compass display  
  if (heading != BadStateValue)   
    _face->update();

  time_t tm = time(NULL);
 
  if (_saveHistory && (tm - _sampleTime > _historySampleInterval))
  {
    _face->addHistory(heading);
    _sampleTime = tm;
  }
  
  
  XmString xmstr;
  char buf[100];
  
  if (heading != _prevHeading)
  {  
    // Update heading text
    if (heading == BadStateValue)
      sprintf(buf, "No Data");
    else
      sprintf(buf, HeadingFormat, heading / Math::RadsPerDeg);

    char prevBuf[100];
    sprintf(prevBuf, HeadingFormat, _prevHeading / Math::RadsPerDeg);
    if (strcmp(buf, prevBuf))
    {
      _headingText->setText((const char *)buf);
    }
    
    // Keep track of accumulated twist
    if (_prevHeading != NoPrevHeading && heading != BadStateValue &&
	_prevHeading != BadStateValue)
    {
      double angToPrev = Math::angularSeparation(heading, _prevHeading);
      float dir = -1.;       
      if (angToPrev > M_PI)
      {
	// Assume shortest route from previous heading to current
	angToPrev = Math::TwoPi - angToPrev;
	dir = 1.;
      }
    }
    
    _prevHeading = heading;
  }
}


void RovCompass::resetTurns()
{
}


void RovCompass::resetCallback(Widget w, XtPointer clientData,
				    XtPointer callData)
{
  RovCompass *obj = (RovCompass *)clientData;

  obj->_turnsDialog->setTitle("turnsResetTitle");
  
  obj->_turnsDialog->post(NULL, 
			  &TetherTurnsDialog::applyCallback,
			  NULL, 
			  &TetherTurnsDialog::applyCallback,
			  (XtPointer )obj->_turnsDialog);

}