#include <stdlib.h>
#include <Xm/Form.h>
#include <Xm/RowColumn.h>
#include <Xm/ToggleB.h>
#include <Xm/Label.h>
#include "JoystickSetup.h"
#include "tiburon.h"
#include "controlDM.h"
#include "DmErrno.h"

#define dprintf if (debug) fprintf

#define JOYSTICK_DEADBAND_DM "TIBURON.CONTROL.JOYSTICK_DEADBAND"

#define NDeadBands 4
#define SpeedModeMnem "speedMode"
#define ThrustModeMnem "thrustMode"

VkMenuDesc JoystickSetup::_modeMenuDesc[] = 
{
  {ACTION, SpeedModeMnem, &JoystickSetup::modeCallback},
  {ACTION, ThrustModeMnem, &JoystickSetup::modeCallback},
  {END}
};


#define RangeMnem "range"

VkMenuDesc JoystickSetup::_rangeMenuDesc[] = 
{
  {ACTION, RangeMnem "100", &JoystickSetup::rangeCallback},
  {ACTION, RangeMnem "80", &JoystickSetup::rangeCallback},
  {ACTION, RangeMnem "50", &JoystickSetup::rangeCallback},
  {ACTION, RangeMnem "20", &JoystickSetup::rangeCallback},
  {ACTION, RangeMnem "10", &JoystickSetup::rangeCallback},
  {END}
};


VkMenuDesc JoystickSetup::_deadBandMenuDesc[] = 
{
  {ACTION, "fwdRev", &JoystickSetup::deadBandCallback},
  {ACTION, "lateral", &JoystickSetup::deadBandCallback},
  {ACTION, "depth", &JoystickSetup::deadBandCallback},
  {ACTION, "heading", &JoystickSetup::deadBandCallback},
  {END}
};


JoystickSetup::JoystickSetup(const char *name, Widget parent)
  : DmGuiObject(name, parent)
{
  _baseWidget = 
    XtVaCreateWidget(name, xmFormWidgetClass, parent, NULL);
  

  Widget label = 
    XtVaCreateManagedWidget("joystickSetupLabel", xmLabelWidgetClass,
			    _baseWidget,
			    XmNtopAttachment, XmATTACH_FORM,
			    XmNleftAttachment, XmATTACH_FORM,
			    NULL);
			    
  Widget mgr = 
    XtVaCreateWidget("mgr", xmFormWidgetClass, _baseWidget,
		     XmNtopAttachment, XmATTACH_WIDGET,
		     XmNtopWidget, label,
		     XmNleftAttachment, XmATTACH_FORM,
		     XmNrightAttachment, XmATTACH_FORM,
		     XmNbottomAttachment, XmATTACH_FORM,
		     NULL);
  
  _mode = new VkOptionMenu(mgr, "mode", _modeMenuDesc, this);

  XtVaSetValues(_mode->baseWidget(),
		XmNleftAttachment, XmATTACH_FORM,
		XmNtopAttachment, XmATTACH_FORM,
		NULL);
  
  _range = new VkOptionMenu(mgr, "range", _rangeMenuDesc, this);

  XtVaSetValues(_range->baseWidget(),
		XmNleftAttachment, XmATTACH_WIDGET,
		XmNleftWidget, _mode->baseWidget(),
		XmNtopAttachment, XmATTACH_FORM,
		NULL);

  _fullRange = 
    XtVaCreateManagedWidget("fullRange", xmToggleButtonWidgetClass, mgr,
			    XmNleftAttachment, XmATTACH_WIDGET,
			    XmNleftWidget, _range->baseWidget(),
			    XmNtopAttachment, XmATTACH_FORM,
			    NULL);
  
  XtAddCallback(_fullRange, XmNvalueChangedCallback,
		&JoystickSetup::fullRangeCallback,
		(XtPointer )this);
  
#ifdef SHOWDEADBAND
  _deadBand = new VkOptionMenu(mgr, "deadBand", _deadBandMenuDesc, this);

  XtVaSetValues(_deadBand->baseWidget(),
		XmNleftAttachment, XmATTACH_WIDGET,
		XmNleftWidget, _range->baseWidget(),
		XmNtopAttachment, XmATTACH_FORM,
		NULL);
  
  _deadBandValue = new TextField("deadBandValue", mgr);
  _deadBandValue->setColumns(5);

  XtVaSetValues(_deadBandValue->baseWidget(),
		XmNleftAttachment, XmATTACH_WIDGET,
		XmNleftWidget, _deadBand->baseWidget(),
		XmNtopAttachment, XmATTACH_FORM,
		NULL);
  
  _deadBandValue->show();
  
#endif

  XtManageChild(mgr);

  char errorBuf[errorMsgSize];
  
  _dmMode = new DmEnumObject(JOYSTICK_MODE_DM);
  if (_dmMode->error())
  {
    _dmMode->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  _dmRange = new DmNat16Object(JOYSTICK_GAIN_DM);
  if (_dmRange->error())
  {
    _dmRange->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  _dmDeadBand = new DmNat16Object(JOYSTICK_DEADBAND_DM, NDeadBands);
  if (_dmDeadBand->error())
  {
    _dmDeadBand->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  _dmSelectableRange = new DmBooleanObject(JOYSTICK_GAIN_SWITCH_DM);
  if (_dmSelectableRange->error())
  {
    _dmSelectableRange->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  if (addDmObject(_dmMode, DmConsumeFlag | DmProvideFlag) == -1)
    return;

  if (addDmObject(_dmRange, DmConsumeFlag | DmProvideFlag) == -1)
    return;

  if (addDmObject(_dmDeadBand, DmConsumeFlag | DmProvideFlag) == -1)
    return;

  if (addDmObject(_dmSelectableRange, DmConsumeFlag | DmProvideFlag) == -1)
    return;
  
  if (setConsumePeriod(Aperiodic) == -1)
    return;
  
  updateGui();
}


JoystickSetup::~JoystickSetup()
{
}


void JoystickSetup::updateGui(DM_Item handle)
{
  DM_Time t;
  Errno errno;
  char errorBuf[errorMsgSize];
  char buf[100];
  
  Nat16 range;
  Nat16 deadBand[NDeadBands];
  Int32 mode;
  
  if ((errno = _dmMode->read(&mode, &t)) != SUCCESS)
  {
    sprintDmError(errno, "JoystickSetup::updateGui(), reading mode", 
		  errorBuf);
    
    setError(errorBuf);
  }
  else
  {
    switch (mode)
    {
      case JoystickSpeedMode:
      _mode->set(SpeedModeMnem);
      break;
      
      case JoystickThrustMode:
      _mode->set(ThrustModeMnem);
      break;
    }
  }

  if ((errno = _dmRange->read(&range, &t)) != SUCCESS)
  {
    sprintDmError(errno, "JoystickSetup::updateGui(), reading range", 
		  errorBuf);
    
    setError(errorBuf);
  }
  else
  {
    sprintf(buf, "%s%d", RangeMnem, range);
    _range->set(buf);
  }

  MBool selectableRange;
  
  if ((errno = _dmSelectableRange->read(&selectableRange, &t)) != SUCCESS)
  {
    sprintDmError(errno, 
		  "JoystickSetup::updateGui(), reading selectable range", 
		  errorBuf);
    
    setError(errorBuf);
  }
  else
  {
    Boolean set;
    if (selectableRange)
    {
      set = False;
    }
    else
    {
      set = True;
    }
    
    XtVaSetValues(_fullRange, XmNset, set, NULL);

    // Set sensitivity of range control appropriately
    XtSetSensitive(_range->baseWidget(), !set);
  }

#ifdef SHOW_DEADBAND

  if ((errno = _dmDeadBand->read(deadBand, &t)) != SUCCESS)
  {
    sprintDmError(errno, "JoystickSetup::updateGui(), reading deadBand", 
		  errorBuf);
    
    setError(errorBuf);
  }
  else
  {
    int selectedIndex = _deadBand->getIndex();
    sprintf(buf, "%d", deadBand[selectedIndex]);
    _deadBandValue->set(buf);
  }

#endif
  
}


void JoystickSetup::modeCallback(Widget w,
				 XtPointer clientData,
				 XtPointer callData)
{
  JoystickSetup *obj = (JoystickSetup *)clientData;
  
  Nat32 value;
  
  if (!strcmp(XtName(w), SpeedModeMnem))
    value = JoystickSpeedMode;
  else
    value = JoystickThrustMode;

  DM_Time t;
  t.tv_sec = time(NULL);
  t.tv_usec = 0;
  
  obj->_dmMode->write(value, &t);
  
}


void JoystickSetup::rangeCallback(Widget w,
				  XtPointer clientData,
				  XtPointer callData)
{
  JoystickSetup *obj = (JoystickSetup *)clientData;

  char *wName = XtName(w);
  Nat16 value = atoi(wName + strlen(RangeMnem));

  DM_Time t;
  t.tv_sec = time(NULL);
  t.tv_usec = 0;
  
  obj->_dmRange->write(value, &t);
  
}


void JoystickSetup::deadBandCallback(Widget w,
				     XtPointer clientData,
				     XtPointer callData)
{
  JoystickSetup *obj = (JoystickSetup *)clientData;

  char errorBuf[errorMsgSize];
  DM_Time t;
  Errno errno;
  
  Nat16 deadBand[NDeadBands];
  
  if ((errno = obj->_dmDeadBand->read(deadBand, &t)) != SUCCESS)
  {
    sprintDmError(errno, "JoystickSetup::updateGui(), reading deadBand", 
		  errorBuf);
    
    obj->setError(errorBuf);
    return;
  }

  char buf[100];
  
}


void JoystickSetup::fullRangeCallback(Widget w,
				      XtPointer clientData,
				      XtPointer callData)
{
  JoystickSetup *obj = (JoystickSetup *)clientData;

  XmToggleButtonCallbackStruct *cb = 
    (XmToggleButtonCallbackStruct *)callData;
  
  obj->writeGainSwitch(cb->set);
}



void JoystickSetup::writeGainSwitch(Boolean fullRange)
{
  MBool selectableRange;
  
  if (fullRange)
  {
    selectableRange = FALSE;
    XtSetSensitive(_range->baseWidget(), False);
  }
  else
  {
    selectableRange = TRUE;
    XtSetSensitive(_range->baseWidget(), True);
  }

  DM_Time t;
  t.tv_sec = time(NULL);
  t.tv_usec = 0;
  
  _dmSelectableRange->write(selectableRange, &t);
}