static char RovDiagram_id[] = "$Header: /u/oreilly/rov/tmacs/RCS/RovDiagram.cc,v 1.5 1998/12/18 21:18:26 oreilly Exp $";

/*
$Log: RovDiagram.cc,v $
Revision 1.5  1998/12/18 21:18:26  oreilly
Remove event handler in destructor

Revision 1.4  1997/08/12 14:56:58  oreilly
Popup menu to plot DM items

Revision 1.3  97/05/07  17:41:37  17:41:37  oreilly (Thomas C. O'Reilly)
Delete contained DmInputDetector in destructor.

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

Revision 1.1  96/10/28  09:12:39  09:12:39  oreilly (Thomas C. O'Reilly)
Initial revision

*/
#include "RovDiagram.h"
#include "lightingDcon.h"
#include "RovDrawing.h"
#include "DmErrno.h"
#include "thrusterDM.h"
#include "powerTypes.h"
#include <values.h>
#include <sys/param.h>

Thruster::Thruster(Display *display,
		   GC lineGC, GC forwardGC, GC reverseGC,
		   Pixel enabledColor, Pixel disabledColor)
{
  _display = display;
  _originX = _originY = 0;
  _lineGC = lineGC;
  _forwardGC = forwardGC;
  _reverseGC = reverseGC;
  _enabledColor = enabledColor;
  _disabledColor = disabledColor;
  _status = Unknown;
  enable();
  
  setRPM(0);
}


Thruster::~Thruster()
{
}

void Thruster::setRPM(Int16 rpm)
{
  _rpm = rpm;
}


int Thruster::centerX()
{
  return _originX + _width / 2;
}


int Thruster::centerY()
{
  return _originY + _height / 2;
}


void Thruster::enable()
{
  _status = Enabled;
  XSetForeground(_display, _lineGC, _enabledColor);
}
  

void Thruster::disable()
{
  _status = Disabled;
  XSetForeground(_display, _lineGC, _disabledColor);
}


VerticalThruster::VerticalThruster(Display *display, 
				   GC lineGC, GC forwardGC, GC reverseGC,
				   Pixel enabledColor, Pixel disabledColor,
				   int centerX, int centerY, 
				   int radius)

  : Thruster(display, lineGC, forwardGC, reverseGC, 
	     enabledColor, disabledColor)
{
  _originX = centerX - radius;
  _originY = centerY - radius;
  _diameter = 2 * radius;
  _width = _height = _diameter;
  double d = (M_SQRT2 * _diameter - _diameter) / 2.;
  _strutOffset = (int )(d / M_SQRT2 + 0.5);
  _thrustScaling = _diameter / MaxRpm;
}


VerticalThruster::~VerticalThruster()
{
}


void VerticalThruster::draw(Drawable canvas)
{
  setStatusStyle();
  
  _prevRpm = _rpm;
  _prevStatus = _status;
  
  XDrawArc(_display, canvas, _lineGC, _originX, _originY, 
	   _diameter, _diameter, 0, 360 * 64);
  
  if (_rpm >= 0)
  {
    XSetLineAttributes(_display, _lineGC, StrutLineThickness, LineSolid, 
		       CapButt, JoinBevel);
    
    /* Thrusting downward, so draw motor struts first; this way the struts
       will appear to be blocked by the downward jet */
    XDrawLine(_display, canvas, _lineGC, 
	      _originX + _strutOffset, 
	      _originY + _strutOffset,
	      _originX + _diameter - _strutOffset, 
	      _originY + _diameter - _strutOffset);

    XDrawLine(_display, canvas, _lineGC, 
	      _originX + _diameter - _strutOffset, 
	      _originY + _strutOffset,
	      _originX + _strutOffset, 
	      _originY + _diameter - _strutOffset);

    XSetLineAttributes(_display, _lineGC, 
		       ThrusterLineThickness, LineSolid, CapButt, JoinBevel);
  }

  if (_rpm == 0)
    return;
  
  int thrustDiameter = abs((int )(_rpm * _thrustScaling + 0.5));

  GC *gc;
  if (_rpm >= 0)
    gc = &_forwardGC;
  else
    gc = &_reverseGC;

  
  int originX, originY;
  originX = _originX + _diameter / 2 - thrustDiameter / 2;
  originY = _originY + _diameter / 2 - thrustDiameter / 2;

  // Draw jet
  XFillArc(_display, canvas, *gc, originX, originY, 
	   thrustDiameter, thrustDiameter, 0, 360 * 64);

  if (_rpm < 0)
  {
    XSetLineAttributes(_display, _lineGC, StrutLineThickness, LineSolid, 
		       CapButt, JoinBevel);

    /* Thrusting up; draw struts now, so jet will appear to be behind struts
    */
    XDrawLine(_display, canvas, _lineGC, 
	      _originX + _strutOffset, 
	      _originY + _strutOffset,
	      _originX + _diameter - _strutOffset, 
	      _originY + _diameter - _strutOffset);

    XDrawLine(_display, canvas, _lineGC, 
	      _originX + _diameter - _strutOffset, 
	      _originY + _strutOffset,
	      _originX + _strutOffset, 
	      _originY + _diameter - _strutOffset);

    XSetLineAttributes(_display, _lineGC, 
		       ThrusterLineThickness, LineSolid, CapButt, JoinBevel);
  }
}


HorizontalThruster::HorizontalThruster(Display *display, 
				       GC lineGC, 
				       GC forwardGC, 
				       GC reverseGC,
				       Pixel enabledColor, 
				       Pixel disabledColor,
				       int centerX, int centerY, 
				       int height)

  : Thruster(display, lineGC, forwardGC, reverseGC, 
	     enabledColor, disabledColor)
{
  _height = height;
  _width = (int )(_height * 0.3886);

  _height = 103;
  _width = 40;
  
  _thrustScaling = _height / 2. / MaxRpm;
  _originX = centerX - _width / 2;
  _originY = centerY - _height / 2;
}



HorizontalThruster::~HorizontalThruster()
{
}


void HorizontalThruster::draw(Drawable canvas)
{
  setStatusStyle();

  XDrawRectangle(_display, canvas, _lineGC, _originX, _originY, 
		 _width, _height);

  XSetLineAttributes(_display, _lineGC, 
		     1, LineSolid, CapButt, JoinBevel);

  XDrawLine(_display, canvas, _lineGC, 
	    _originX, _originY + _height / 2,
	    _originX + _width, _originY + _height / 2);
	    
  XSetLineAttributes(_display, _lineGC, 
		     ThrusterLineThickness, LineSolid, CapButt, JoinBevel);

  if (_rpm == 0)
    return;
  
  int thrustHeight = MAX(abs((int )(_rpm * _thrustScaling + 0.5)), 1);

  GC *gc;
  int originX, originY;
  if (_rpm >= 0)
  {
    originY = _originY + _height / 2 + 1;
    gc = &_forwardGC;
  }
  else
  {
    originY = _originY + _height / 2 - thrustHeight - 1;
    gc = &_reverseGC;  
  }
  originX = _originX + 1;
  
  // Draw jet
  XFillRectangle(_display, canvas, *gc, originX, originY, 
		 _width - 1, thrustHeight);
}



LateralThruster::LateralThruster(Display *display, 
				 GC lineGC, 
				 GC forwardGC, 
				 GC reverseGC,
				 Pixel enabledColor,
				 Pixel disabledColor,
				 int centerX, int centerY, 
				 int width,
				 Boolean positiveToRight
				 )

  : Thruster(display, lineGC, forwardGC, reverseGC, 
	     enabledColor, disabledColor)
{
  _width = width;
  _height = (int )(_width * 0.3886);

  _width = 103;
  _height = 41;

  _thrustScaling = _width / 2. / MaxRpm;
  _originX = centerX - _width / 2;
  _originY = centerY - _height / 2;
  _positiveToRight = positiveToRight;
}



LateralThruster::~LateralThruster()
{
}


void LateralThruster::draw(Drawable canvas)
{
  setStatusStyle();

  XDrawRectangle(_display, canvas, _lineGC, _originX, _originY, 
		 _width, _height);

    XSetLineAttributes(_display, _lineGC, 
		       1, LineSolid, CapButt, JoinBevel);

  XDrawLine(_display, canvas, _lineGC, 
	    _originX + _width / 2, _originY,
	    _originX + _width / 2, _originY + _height);
	    
  XSetLineAttributes(_display, _lineGC, 
		     ThrusterLineThickness, LineSolid, CapButt, JoinBevel);

  if (_rpm == 0)
    return;
  
  int thrustWidth = MAX(abs((int )(_rpm * _thrustScaling + 0.5)), 1);

  GC *gc;
  int originX, originY;
  Boolean toRight;
  
  if (_rpm >= 0)
  { 
    if (_positiveToRight)
      toRight = True;
    else
      toRight = False;
  }
  else
  {
    if (_positiveToRight)
      toRight = False;
    else
      toRight = True;
  }
  
  if (toRight)
  {
    originX = _originX + _width / 2 + 1;
    gc = &_forwardGC;
  }
  else
  {
    originX = _originX + _width / 2 - thrustWidth - 1;
    gc = &_reverseGC;  
  }
  originY = _originY + 1;
  
  // Draw jet
  XFillRectangle(_display, canvas, *gc, originX, originY, 
		 thrustWidth, _height - 1);
}


void Thruster::setStatusStyle()
{
  if (_status == Disabled)
  {
    XSetForeground(_display, _lineGC, _disabledColor);
  }
  else
  {
    XSetForeground(_display, _lineGC, _enabledColor);
  }
}


Light::Light(Display *display, GC lineGC, GC lightGC,
	     int centerX, int centerY, int radius, 
	     Pixel onColor, Pixel offColor)
{
  _originX = centerX - radius;
  _originY = centerY - radius;
  _diameter = 2 * radius;
  _display = display;
  _lineGC = lineGC;
  _lightGC = lightGC;
  _onColor = onColor;
  _offColor = offColor;

  set(LightOff);
}


void Light::set(Boolean on)
{
  _on = on;
}


void Light::draw(Drawable canvas)
{
//  XDrawArc(_display, canvas, _lineGC, _originX, _originY,
//	   _diameter, _diameter, 0, 360 * 64);
  
  if (_on)
    XSetForeground(_display, _lightGC, _onColor);
  else
    XSetForeground(_display, _lightGC, _offColor);

  int thickness = 0;
  
  XFillArc(_display, canvas, _lightGC, _originX + 2, _originY + 2,
	   _diameter, _diameter, 0, 360 * 64);  
}


int Light::centerX()
{
  return _originX + _diameter / 2;
}


int Light::centerY()
{
  return _originY + _diameter / 2;
}


int Light::width()
{
  return _diameter;
}


int Light::height()
{
  return _diameter;
}



XtResource RovDiagram::_resources[] = 
{
  {
    "fground",
    "Fground",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(RovDiagram *, _foreground),
    XmRString,
    "black"
  },

  {
    "bground",
    "Bground",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(RovDiagram *, _background),
    XmRString,
    "white"
  },

  {
    "disabledColor",
    "DisabledColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(RovDiagram *, _disabledColor),
    XmRString,
    "red"
  },

  {
    "forwardColor",
    "ForwardColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(RovDiagram *, _forwardColor),
    XmRString,
    "orange"
  },

  {
    "reverseColor",
    "ReverseColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(RovDiagram *, _reverseColor),
    XmRString,
    "purple"
  },

  {
    "lightOnColor",
    "LightOnColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(RovDiagram *, _lightOnColor),
    XmRString,
    "yellow"
  },

  {
    "lightOffColor",
    "LightOffColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(RovDiagram *, _lightOffColor),
    XmRString,
    "gray"
  }

};


RovDiagram::RovDiagram(const char *name, Widget parent, int period)
  : DmGuiObject(name, parent), VkDoubleBuffer(name, parent)
{
  DmGuiObject::_baseWidget = VkDoubleBuffer::_baseWidget;
  
  DmGuiObject::getResources(_resources, XtNumber(_resources));

  XtAddEventHandler(DmGuiObject::_baseWidget,
		    ButtonPressMask,
		    False, 
		    (XtEventHandler )&DmGuiObject::eventHandler, 
		    (XtPointer )this);
  int i;

  for (i = 0; i < NThrusters; i++)
    _enableDetector[i] = NULL;
  
  _display = theApplication->display();
  _screenNum = DefaultScreen(_display);
  
  XGCValues gcValues;
  gcValues.foreground = _foreground;
  gcValues.background = _background;
  gcValues.line_width = 3;
  
  _lineGC = XCreateGC(_display, RootWindow(_display, _screenNum),
		      (GCForeground | GCBackground | GCLineWidth), &gcValues);

  gcValues.foreground = _forwardColor;
  _forwardGC = XCreateGC(_display, RootWindow(_display, _screenNum),
			 GCForeground, &gcValues);

  gcValues.foreground = _reverseColor;
  _reverseGC = XCreateGC(_display, RootWindow(_display, _screenNum),
			 GCForeground, &gcValues);

  gcValues.foreground = _lightOnColor;
  _lightGC = XCreateGC(_display, RootWindow(_display, _screenNum),
		       GCForeground, &gcValues);
  

  if ((_vehicleBitmap = 
       XCreateBitmapFromData(_display, RootWindow(_display, _screenNum),
			     RovDrawing_bits, 
			     RovDrawing_width, RovDrawing_height))
      == False)
  {
    setError("XCreateBitmapFromData() failed");
    return;
  }
  
  _vehicleBitmapOffset.x = 0;
  _vehicleBitmapOffset.y = 40;

  char errorBuf[errorMsgSize];
  char itemName[MaxDmNameLen];

  static char *lightDesignator[NLights] = 
  {
    "TIBURON.POWER.LIGHTS.LIGHT1", "TIBURON.POWER.LIGHTS.LIGHT0", 
    "TIBURON.POWER.LIGHTS.LIGHT5", "TIBURON.POWER.LIGHTS.LIGHT2",
    "TIBURON.POWER.LIGHTS.LIGHT4", "TIBURON.POWER.LIGHTS.LIGHT3"
  };
  
  strcpy(itemName, "");
  char *namePtr = itemName + strlen(itemName);

  int x, y;
  y = 0;
  
  for (i = 0; i < NLights; i++)
  {
    if (!(i % 2))
      y += 20;

    switch (i)
    {
      case PortAuxLight:
      case PortFixedLight:
      x = 53;
      break;

      case StbdAuxLight:
      case StbdFixedLight:
      x = 163;
      break;
      
      case PortDirectionalLight:
      x = 33;
      break;
      
      case StbdDirectionalLight:
      x = 183;
    }
    
    _light[i].diagram = new Light(_display, _lineGC, _lightGC,
				  x, y, 10, 
				  _lightOnColor, _lightOffColor);
 
    sprintf(namePtr, "%s.%s", lightDesignator[i], "SWITCH_STATUS");

    _light[i].dm = new DmEnumObject(itemName);
    if (_light[i].dm->error())
    {
      _light[i].dm->errorMsg(errorBuf);
      setError(errorBuf);
      return;
    }

    if (addDmObject(_light[i].dm, DmConsumeFlag) == -1)
      return;
  }

  int thrusterStartY = 120;
  
  for (i = 0; i < NThrusters; i++)
  {
    char *thrusterDmName;
    char *enableDmName;
    
    switch (i)
    {
      case PortVertThruster:
      thrusterDmName = PORT_VERT_RPM_DM;
      enableDmName = PORT_VERT_ENABLE_DM;
      
      _thruster[i].diagram = 
	new VerticalThruster(_display, 
			     _lineGC, _forwardGC, _reverseGC,
			     _foreground, _disabledColor,
			     31, 213, 26);
      break;
      
      case StbdVertThruster:
      thrusterDmName = STBD_VERT_RPM_DM;
      enableDmName = STBD_VERT_ENABLE_DM;
      
      _thruster[i].diagram = 
	new VerticalThruster(_display, 
			     _lineGC, _forwardGC, _reverseGC,
			     _foreground, _disabledColor,
			     188, 212, 26);
      break;
      

      case PortHorizThruster:
      thrusterDmName = PORT_HORIZ_RPM_DM;
      enableDmName = PORT_HORIZ_ENABLE_DM;
      
      _thruster[i].diagram = 
	new HorizontalThruster(_display, 
			       _lineGC, _forwardGC, _reverseGC,
			       _foreground, _disabledColor,
			       30, 294, 103);
      break;

      case StbdHorizThruster:
      thrusterDmName = STBD_HORIZ_RPM_DM;
      enableDmName = STBD_HORIZ_ENABLE_DM;
      
      _thruster[i].diagram = 
	new HorizontalThruster(_display, 
			       _lineGC, _forwardGC, _reverseGC,
			       _foreground, _disabledColor,
			       190, 294, 103);
      break;


      case FwdLatThruster:
      thrusterDmName = FWD_LATERAL_RPM_DM;
      enableDmName = FWD_LATERAL_ENABLE_DM;
      _thruster[i].diagram =
	new LateralThruster(_display,
			    _lineGC, _forwardGC, _reverseGC,
			     _foreground, _disabledColor,
			    110, 171, 103, False); 

      break;
      

      case AftLatThruster:
      thrusterDmName = AFT_LATERAL_RPM_DM;
      enableDmName = AFT_LATERAL_ENABLE_DM;

      _thruster[i].diagram =
	new LateralThruster(_display,
			    _lineGC, _forwardGC, _reverseGC,
			    _foreground, _disabledColor,
			    109, 257, 103, True);       
    }
    
    _thruster[i].dmRpm = new DmInt16Object(thrusterDmName);
    if (_thruster[i].dmRpm->error())
    {
      _thruster[i].dmRpm->errorMsg(errorBuf);
      setError(errorBuf);
      return;
    }

    if (addDmObject(_thruster[i].dmRpm, DmConsumeFlag) == -1)
      return;

    _thruster[i].dmEnable = new DmEnumObject(enableDmName);
    if (_thruster[i].dmEnable->error())
    {
      _thruster[i].dmEnable->errorMsg(errorBuf);
      setError(errorBuf);
      return;
    }
    char enableName[100];
    sprintf(enableName, "enable%d", i);
    
    _enableDetector[i] = 
      new DmInputDetector(enableName, parent, _thruster[i].dmEnable, this);
    
    _thruster[i].prevValue = MAXSHORT;
  }
  
  if (setConsumePeriod(period) == -1)
    return;

  updateGui();
}


RovDiagram::~RovDiagram()
{
  XFreePixmap(_display, _vehicleBitmap);

  for (int i = 0; i < NThrusters; i++)
  {
    if (_enableDetector[i])
      delete _enableDetector[i];
  }

  XtRemoveEventHandler(DmGuiObject::_baseWidget,
		       ButtonPressMask,
		       False, 
		       (XtEventHandler )&DmGuiObject::eventHandler, 
		       (XtPointer )this);
}



void RovDiagram::updateGui(DM_Item handle)
{
  Int16 rpm;
  char errorBuf[errorMsgSize];
  DM_Time t;
  int i;
  Int32 status;
  
  for (i = 0; i < NLights; i++)
  {
    if (_light[i].dm->read(&status, &t) != SUCCESS)
    {
      _light[i].dm->errorMsg(errorBuf);
      setError(errorBuf);
      return;
    }
    if (status == SWITCH_ON)
      _light[i].diagram->set(True);
    else
      _light[i].diagram->set(False);
  }
  
  for (i = 0; i < NThrusters; i++)
  {
    if (_thruster[i].dmRpm->read(&rpm, &t) != SUCCESS)
    {
      _thruster[i].dmRpm->errorMsg(errorBuf);
      setError(errorBuf);
      return;
    }
    _thruster[i].diagram->setRPM(rpm);
  }

  update();
}


void RovDiagram::draw()
{
  XSetForeground(_display, _lineGC, _background);
  XFillRectangle(_display, _canvas, _lineGC, 0, 0, _width, _height);
  XSetForeground(_display, _lineGC, _foreground);

  XCopyPlane(_display, _vehicleBitmap, _canvas, _lineGC,
	     5, 0, 
	     RovDrawing_width, RovDrawing_height, 
	     _vehicleBitmapOffset.x, _vehicleBitmapOffset.y, 
	     01);
  
  int i;
  for (i = 0; i < NLights; i++)
    _light[i].diagram->draw(_canvas);
  
  for (i = 0; i < NThrusters; i++)
    _thruster[i].diagram->draw(_canvas);

  return;
}


void RovDiagram::updateFromPeer(DmGuiObject *peer, void *clientData)
{
  Boolean debug = False;
  DmObject *changedDmObject =  ((DmInputDetector *)peer)->dmObject();
  
  for (int i = 0; i < NThrusters; i++)
  {
    if (_thruster[i].dmEnable == changedDmObject)
    {
      // Read enable motor enable status
      Errno err;
      Int32 enableStatus;
      DM_Time t;
      char errorBuf[errorMsgSize];
      
      if ((err = _thruster[i].dmEnable->read(&enableStatus, &t)) != SUCCESS)
      {
	sprintDmError(err, "RovDiagram::updateFromPeer()", errorBuf);
	setError(errorBuf);
	return;
      }
      
      // Set status of motor diagram element accordingly
      if (enableStatus == ENABLE_MOTOR)
      {
	dprintf(stderr, "Enable thruster %d\n", i);  
	_thruster[i].diagram->enable();
      }
      else
      {
	dprintf(stderr, "Disable thruster %d\n", i);
	_thruster[i].diagram->disable();
      }
    }
  }
}