#include <Xm/ArrowB.h>
#include <Xm/RowColumn.h>
#include <Vk/VkErrorDialog.h>
#include <Vk/VkApp.h>
#include "HeadingControl.h"
#include "DmErrno.h"
#include "Math.h"

#define dprintf if (debug) fprintf

#define HeadingIncrement (1. * Math::RadsPerDeg)

HeadingControlBase::HeadingControlBase(const char *name, Widget parent) 
  : DmGuiObject(name, parent)
{
  _baseWidget = XtVaCreateWidget(name, xmRowColumnWidgetClass, parent,
				 XmNorientation, XmHORIZONTAL,
				 NULL);
  
  installDestroyHandler();

  _ccwButton = 
    XtVaCreateManagedWidget("ccwButton", xmArrowButtonWidgetClass, 
			    _baseWidget,
			    XmNarrowDirection, XmARROW_LEFT,
			    NULL);
  
  XtAddCallback(_ccwButton, XmNarmCallback,
		&HeadingControlBase::callback,
		(XtPointer )this);

  XtAddCallback(_ccwButton, XmNdisarmCallback,
		&HeadingControlBase::callback,
		(XtPointer )this);
  
  _cwButton =
    XtVaCreateManagedWidget("cwButton", xmArrowButtonWidgetClass, 
			    _baseWidget,
			    XmNarrowDirection, XmARROW_RIGHT,
			    NULL);
  
  XtAddCallback(_cwButton, XmNarmCallback,
		&HeadingControlBase::callback,
		(XtPointer )this);

  XtAddCallback(_cwButton, XmNdisarmCallback,
		&HeadingControlBase::callback,
		(XtPointer )this);

  _timer = 0;
  
  updateGui();
}


HeadingControlBase::~HeadingControlBase()
{
}



void HeadingControlBase::callback(Widget w, 
			      XtPointer clientData, 
			      XtPointer callData)
{
  Boolean debug = True;
  
  HeadingControlBase *obj = (HeadingControlBase *)clientData;
  if (w == obj->_ccwButton)
    obj->_sense = -1;
  else
    obj->_sense = +1;
  
  XmArrowButtonCallbackStruct *cb = (XmArrowButtonCallbackStruct *)callData;

  if (cb->reason == XmCR_ARM)
  {
    if (obj->setHeading() != -1)
    {
      obj->_timer = XtAppAddTimeOut(theApplication->appContext(), 500, 
				    &HeadingControlBase::timerCallback, obj);
    }
  }
  else
  {
    dprintf(stderr, "Remove timer\n");
    
    if (obj->_timer)
      XtRemoveTimeOut(obj->_timer);
  }
}


void HeadingControlBase::timerCallback(XtPointer clientData, XtIntervalId *id)
{
  HeadingControlBase *obj = (HeadingControlBase *)clientData;
  if (obj->setHeading() != -1)
    obj->_timer = XtAppAddTimeOut(theApplication->appContext(), 100, 
				  &HeadingControlBase::timerCallback, obj);
}



HeadingControl::HeadingControl(const char *name, Widget parent, 
			       const char *dmItemName, Boolean inDegrees)
  : HeadingControlBase(name, parent)
{
  _inDegrees = inDegrees;
  
  _dmHeading = new DmFlt32Object(dmItemName);
  
  Errno errno;
  char errorBuf[errorMsgSize];
  
  if (addDmObject(_dmHeading, DmProvideFlag | DmConsumeFlag) == -1)
    return;

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

  updateGui();
}


HeadingControl::~HeadingControl()
{
}


void HeadingControl::updateGui(DM_Item handle)
{
  Errno errno;
  DM_Time t;
  char errorBuf[errorMsgSize];
  
  if ((errno = _dmHeading->read(&_heading, &t)) != SUCCESS)
  {
    sprintDmError(errno, "HeadingControlBase::updateGui()", errorBuf);
    fprintf(stderr, "%s\n", errorBuf);
  }
  else if (_inDegrees)
    _heading *= Math::RadsPerDeg;
}


int HeadingControl::setHeading()
{
  _heading = _heading + _sense * HeadingIncrement;
  double h = _heading;
  
  Math::zeroToTwoPi(&h);

  _heading = h;

  Flt32 heading;
  
  if (_inDegrees)
    heading = _heading / Math::RadsPerDeg;
  else
    heading = _heading;
  
  DM_Time t;
  t.tv_sec = time(NULL);
  t.tv_usec = 0;
  
  Errno errno;
  if ((errno = _dmHeading->write(heading, &t)) != SUCCESS)
  {
    char buf[100];
    sprintf(buf, "Writing heading to %s", _dmHeading->getName());
    char errorBuf[errorMsgSize];
    sprintDmError(errno, buf, errorBuf);
    theErrorDialog->postAndWait(errorBuf);
    return -1;
  }

  return 0;
}



// ------------------------------------
RovHeadingControl::RovHeadingControl(const char *name, Widget parent) 
  : HeadingControlBase(name, parent)
{
  
  _dmAttitude = new DmAttitudeObject();
  
  Errno errno;
  char errorBuf[errorMsgSize];
  
  if (addDmObject(_dmAttitude, DmProvideFlag | DmConsumeFlag) == -1)
    return;

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

  updateGui();
}


RovHeadingControl::~RovHeadingControl()
{
}


void RovHeadingControl::updateGui(DM_Item handle)
{
  Errno errno;
  DM_Time t;
  char errorBuf[errorMsgSize];
  
  if ((errno = _dmAttitude->read(_state, &t)) != SUCCESS)
  {
    sprintDmError(errno, "RovHeadingControl::updateGui()", errorBuf);
    fprintf(stderr, "%s\n", errorBuf);
  }
}


int RovHeadingControl::setHeading()
{
  _state[HeadingIndex] += _sense * HeadingIncrement;
  double h = _state[HeadingIndex];
  
  Math::zeroToTwoPi(&h);

  _state[HeadingIndex] = h;

  DM_Time t;
  t.tv_sec = time(NULL);
  t.tv_usec = 0;
  
  Errno errno;
  if ((errno = _dmAttitude->write(_state, &t)) != SUCCESS)
  {
    char buf[100];
    sprintf(buf, "Writing state to %s", _dmAttitude->getName());
    char errorBuf[errorMsgSize];
    sprintDmError(errno, buf, errorBuf);
    theErrorDialog->postAndWait(errorBuf);
    return -1;
  }
  return 0;
}