static char Compass_id[] = "$Header: Compass.cc,v 1.4 97/12/04 14:56:08 oreilly Exp $";

#define HeadingFormat "%.0f"

/*
$Log:	Compass.cc,v $
Revision 1.4  97/12/04  14:56:08  14:56:08  oreilly (Thomas C. O'Reilly)
Utilize floating face

Revision 1.1  97/11/20  08:21:31  08:21:31  oreilly (Thomas C. O'Reilly)
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 "Compass.h"
#include "DmErrno.h"
#include "DmInputDetector.h"
#include "controlDM.h"
#include "vehicleTurnsDM.h"
#include "parseUtils.h"

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

  char errorBuf[errorMsgSize];

  _dmInDegrees = inDegrees;
  
  _baseWidget = XtCreateWidget(_name, xmFormWidgetClass, parent, NULL, 0);

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

  // 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();
  
  dprintf(stderr, 
	  "Compass::Compass() - create DM item "
	  "\"%s\"\n", dmItemName);

  
  MBool readRaw = FALSE;
  _dmHeading = new DmFlt32Object(dmItemName);

  if (_dmHeading->error())
  {
    _dmHeading->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  if (addDmObject(_dmHeading, 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;

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

  // Force GUI update on first call to updateGui()
  _prevHeading = NoPrevHeading;
  
  updateGui();
}


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

  _face->setHeading(heading);

  if ((errno = _dmTurnsSetpoint->read(&_turnsSetpoint, &t)) != SUCCESS)
  { 
    sprintDmError(errno, 
		  "Compass::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;
  }
}