static char PanTiltAttitude_id[] = "$Header: PanTiltAttitude.cc,v 1.5 97/06/04 08:41:42 oreilly Exp $";

/*
$Log:	PanTiltAttitude.cc,v $
Revision 1.5  97/06/04  08:41:42  08:41:42  oreilly (Thomas C. O'Reilly)
*** empty log message ***

Revision 1.4  97/04/30  19:39:14  19:39:14  oreilly (Thomas C. O'Reilly)
Pan/tilt displays are now separate

Revision 1.3  97/03/20  12:30:18  12:30:18  oreilly (Thomas C. O'Reilly)
..

Revision 1.2  96/10/28  09:12:29  09:12:29  oreilly (Thomas C. O'Reilly)
*** empty log message ***

*/
#include <math.h>
#include <Xm/Form.h>
#include <Xm/Label.h>
#include <Xm/Separator.h>
#include "PanTiltAttitude.h"
#include "Iview.h"
#include "Math.h"
#include "DmErrno.h"
#include "State.h"

#define dprintf if (debug) fprintf

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

  _graphHalfWidth = 10.;
  _yOffset = 100.;

  Widget label;
  
  label = XtVaCreateManagedWidget("portLight", xmLabelWidgetClass,
				  _baseWidget, 
				  XmNleftAttachment, XmATTACH_FORM,
				  XmNtopAttachment, XmATTACH_FORM,
				  NULL);

  _pan[PortLight] = new DynamicLabel("portLightPan", _baseWidget,
				     XmALIGNMENT_BEGINNING);

  XtVaSetValues(_pan[PortLight]->baseWidget(),
		XmNleftAttachment, XmATTACH_FORM,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, label,
		NULL);

  _pan[PortLight]->show();
  
  _tilt[PortLight] = new DynamicLabel("portLightTilt", _baseWidget,
				     XmALIGNMENT_BEGINNING);

  XtVaSetValues(_tilt[PortLight]->baseWidget(),
		XmNleftAttachment, XmATTACH_FORM,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, _pan[PortLight]->baseWidget(),
		NULL);

  _tilt[PortLight]->show();

  label = XtVaCreateManagedWidget("stbdLight", xmLabelWidgetClass,
				  _baseWidget, 
				  XmNleftAttachment, XmATTACH_POSITION,
				  XmNleftPosition, 50,
				  XmNtopAttachment, XmATTACH_FORM,
				  NULL);

  _pan[StbdLight] = new DynamicLabel("stbdLightPan", _baseWidget,
				     XmALIGNMENT_BEGINNING);

  XtVaSetValues(_pan[StbdLight]->baseWidget(),
		XmNleftAttachment, XmATTACH_POSITION,
		XmNleftPosition, 50,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, label,
		NULL);

  _pan[StbdLight]->show();
  
  _tilt[StbdLight] = new DynamicLabel("stbdLightTilt", _baseWidget,
				      XmALIGNMENT_BEGINNING);

  XtVaSetValues(_tilt[StbdLight]->baseWidget(),
		XmNleftAttachment, XmATTACH_POSITION,
		XmNleftPosition, 50,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, _pan[StbdLight]->baseWidget(),
		NULL);

  _tilt[StbdLight]->show();

  
  _graph = new PanTiltAttitudeGraph("graph", _baseWidget, 
				    20.*Math::RadsPerDeg, 15.*Math::RadsPerDeg,
				    DefaultPanRange, DefaultTiltRange);

  XtVaSetValues(_graph->baseWidget(),
		XmNleftAttachment, XmATTACH_FORM,
		XmNrightAttachment, XmATTACH_FORM,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, _tilt[PortLight]->baseWidget(),
		NULL);
  
  _graph->show();
  
  label = XtVaCreateManagedWidget("portCamera", xmLabelWidgetClass,
				  _baseWidget, 
				  XmNleftAttachment, XmATTACH_FORM,
				  XmNtopAttachment, XmATTACH_WIDGET,
				  XmNtopWidget, _graph->baseWidget(),
				  NULL);

  _pan[PortCamera] = new DynamicLabel("portCamera", _baseWidget,
				     XmALIGNMENT_BEGINNING);

  XtVaSetValues(_pan[PortCamera]->baseWidget(),
		XmNleftAttachment, XmATTACH_FORM,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, label,
		NULL);

  _pan[PortCamera]->show();
  
  _tilt[PortCamera] = new DynamicLabel("portCameraTilt", _baseWidget,
				       XmALIGNMENT_BEGINNING);

  XtVaSetValues(_tilt[PortCamera]->baseWidget(),
		XmNleftAttachment, XmATTACH_FORM,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, _pan[PortCamera]->baseWidget(),
		NULL);

  _tilt[PortCamera]->show();

  
  label = XtVaCreateManagedWidget("stbdCamera", xmLabelWidgetClass,
				  _baseWidget, 
				  XmNleftAttachment, XmATTACH_POSITION,
				  XmNleftPosition, 50,
				  XmNtopAttachment, XmATTACH_WIDGET,
				  XmNtopWidget, _graph->baseWidget(),
				  NULL);

  _pan[StbdCamera] = new DynamicLabel("stbdCameraPan", _baseWidget,
				      XmALIGNMENT_BEGINNING);

  XtVaSetValues(_pan[StbdCamera]->baseWidget(),
		XmNleftAttachment, XmATTACH_POSITION,
		XmNleftPosition, 50,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, label,
		NULL);

  _pan[StbdCamera]->show();
  
  _tilt[StbdCamera] = new DynamicLabel("stbdCameraTilt", _baseWidget,
				       XmALIGNMENT_BEGINNING);

  XtVaSetValues(_tilt[StbdCamera]->baseWidget(),
		XmNleftAttachment, XmATTACH_POSITION,
		XmNleftPosition, 50,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, _pan[StbdCamera]->baseWidget(),
		NULL);

  _tilt[StbdCamera]->show();

  
  Widget separator = 
    XtVaCreateManagedWidget("", xmSeparatorWidgetClass, _baseWidget,
			    XmNleftAttachment, XmATTACH_FORM,
			    XmNrightAttachment, XmATTACH_FORM,
			    XmNtopAttachment, XmATTACH_WIDGET,
			    XmNtopWidget, _tilt[PortCamera]->baseWidget(),
			    NULL);
  
  _pitch = new DynamicLabel("pitch", _baseWidget, XmALIGNMENT_BEGINNING);

  XtVaSetValues(_pitch->baseWidget(),
		XmNleftAttachment, XmATTACH_FORM,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, separator,
		XmNrightAttachment, XmATTACH_POSITION,
		XmNrightPosition, 50,
		XmNbottomAttachment, XmATTACH_FORM,
		NULL);

  _pitch->show();

  _roll = new DynamicLabel("roll", _baseWidget, XmALIGNMENT_BEGINNING);

  XtVaSetValues(_roll->baseWidget(),
		XmNleftAttachment, XmATTACH_POSITION,
		XmNleftPosition, 50,
		XmNtopAttachment, XmATTACH_WIDGET,
		XmNtopWidget, separator,
		XmNrightAttachment, XmATTACH_FORM,
		XmNbottomAttachment, XmATTACH_FORM,
		NULL);

  _roll->show();
  
  char itemName[MaxDmNameLen];


  MBool readRaw = FALSE;
  _dmAttitude = new DmAttitudeObject(readRaw);
  if (_dmAttitude->error())
  {
    _dmAttitude->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  if (addDmObject(_dmAttitude, DmConsumeFlag) == -1)
    return;
  
  _prevPitch = _prevRoll = MAXFLOAT;

  // Pan/tilt values are read aperiodically
  for (int i = 0; i < 4; i++)
  {
    char panItemName[MaxDmNameLen];
    char tiltItemName[MaxDmNameLen];    

    switch (i)
    {
      case PortLight:
      strcpy(panItemName, IVIEW_PORT_LIGHT_PAN_DM);
      strcpy(tiltItemName, IVIEW_PORT_LIGHT_TILT_DM);
      break;
      
      case StbdLight:
      strcpy(panItemName, IVIEW_STBD_LIGHT_PAN_DM);
      strcpy(tiltItemName, IVIEW_STBD_LIGHT_TILT_DM);
      break;
      
      case PortCamera:
      strcpy(panItemName, IVIEW_PORT_CAMERA_PAN_DM);
      strcpy(tiltItemName, IVIEW_PORT_CAMERA_TILT_DM);
      break;
      
      case StbdCamera:
      strcpy(panItemName, IVIEW_STBD_CAMERA_PAN_DM);
      strcpy(tiltItemName, IVIEW_STBD_CAMERA_TILT_DM);
      break;

    }
    _dmPan[i] = new DmFlt32Object(panItemName);
    if (_dmPan[i]->error())
    {
      _dmPan[i]->errorMsg(errorBuf);
      setError(errorBuf);
      return;
    }
    
    if (addDmObject(_dmPan[i], DmConsumeFlag) == -1)
      return;

    _dmTilt[i] = new DmFlt32Object(tiltItemName);
    if (_dmTilt[i]->error())
    {
      _dmTilt[i]->errorMsg(errorBuf);
      setError(errorBuf);
      return;
    }    

    if (addDmObject(_dmTilt[i], DmConsumeFlag) == -1)
      return;
  }
  
  if (setConsumePeriod(period) == -1)
    return;

  for (i = 0; i < 4; i++)
    _prevPan[i] = _prevTilt[i] = MAXFLOAT;
  

  updateGui();
  updateFromPeer(this, NULL);
}


PanTiltAttitude::~PanTiltAttitude()
{
}


void PanTiltAttitude::updateGui(DM_Item handle)
{
  Boolean debug = False;
  
  Flt32 pitch, roll;
  char errorBuf[errorMsgSize];
  DM_Time t;
  StateValue state[NStateElements];
  Errno err;
  
  if ((err = _dmAttitude->read(state, &t)) != SUCCESS)
  {
    sprintDmError(err, "PanTiltAttitude::updateGui()", errorBuf);
    setError(errorBuf);
    return;
  }

  pitch = state[PitchIndex];
  roll = state[RollIndex];
  
  _graph->setPitch(pitch);
  _graph->setRoll(roll);
  _graph->update();
  
  char buf[100];
  char prevBuf[100];
  double dPitch = pitch;
  double dRoll = roll;

  Math::minusPiToPi(&dPitch);
  Math::minusPiToPi(&dRoll);

  const double minChange = 0.01 * Math::RadsPerDeg;
  
  dprintf(stderr, 
	  "roll: %.1f, dRoll: %.1f, prevRoll: %.1f, diff=%.1f  (Degrees)\n", 
	  roll / Math::RadsPerDeg,
	  dRoll / Math::RadsPerDeg,
	  _prevRoll / Math::RadsPerDeg,
	  fabs(dRoll - _prevRoll) / Math::RadsPerDeg
	  );

  dprintf(stderr, "roll: %.5f, dRoll: %.5f, prevRoll: %.5f  (Radians)\n", 
	  roll, dRoll, _prevRoll,
	  fabs(roll - _prevRoll)
	  );
  
  
  sprintf(buf, "%.1f", pitch / Math::RadsPerDeg);
  sprintf(prevBuf, "%.1f", _prevPitch / Math::RadsPerDeg);
    
  if (strcmp(buf, prevBuf))
  {
    if (pitch == BadStateValue)
      sprintf(buf, "No Pitch Data");
    else
      sprintf(buf, "Pitch: %6.1f", dPitch / Math::RadsPerDeg);

    _pitch->setText(buf);
  }

  sprintf(buf, "%.1f", roll / Math::RadsPerDeg);
  sprintf(prevBuf, "%.1f", _prevRoll / Math::RadsPerDeg);

  if (strcmp(buf, prevBuf))
  {
    if (roll == BadStateValue)
      sprintf(buf, "No Roll Data");
    else
      sprintf(buf, "Roll: %6.1f", dRoll / Math::RadsPerDeg);

    _roll->setText((const char *)buf);
    
  } 

  _prevPitch = pitch;
  _prevRoll = roll;

  Flt32 pan, tilt;

  for (int i = 0; i < 4; i++)
  {
    // Read pan and tilt
    dprintf(stderr, "Reading %s\n", _dmPan[i]->getName());
    if ((err = _dmPan[i]->read(&pan, &t)) != SUCCESS)
    {
      sprintDmError(err, "PanTiltAttitude::updateFromPeer()", errorBuf);
      setError(errorBuf);
      return;
    }
      
    dprintf(stderr, "Reading %s\n", _dmTilt[i]->getName());
    if ((err = _dmTilt[i]->read(&tilt, &t)) != SUCCESS)
    {
      sprintDmError(err, "PanTiltAttitude::updateFromPeer()", errorBuf);
      setError(errorBuf);
      return;
    }
    if (pan == _prevPan[i] && tilt == _prevTilt[i])
      continue;
      
    _prevPan[i] = pan;
    _prevTilt[i] = tilt;
      
    sprintf(buf, "P: %+5.1f", pan);
    _pan[i]->setText(buf);
    sprintf(buf, "T: %+4.1f", tilt);
    _tilt[i]->setText(buf);

    // DM values are in degrees
    pan *= Math::RadsPerDeg;
    tilt *= Math::RadsPerDeg;
      
    _graph->setPanTilt(i, pan, tilt);
    _graph->update();
  }
}