static char PanTiltAttitudeGraph_id[] = "$Header: PanTiltAttitudeGraph.cc,v 1.2 97/03/20 12:30:20 oreilly Exp $";

/*
$Log:	PanTiltAttitudeGraph.cc,v $
Revision 1.2  97/03/20  12:30:20  12:30:20  oreilly (Thomas C. O'Reilly)
..

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

*/
#include <math.h>
#include <Vk/VkApp.h>
#include "PanTiltAttitudeGraph.h"

#define dprintf if (debug) fprintf

XtResource PanTiltAttitudeGraph::_panTiltResources[] = 
{
  {
    "panTiltAxisColor",
    "PanTiltAxisColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(PanTiltAttitudeGraph *, _panTiltAxisColor),
    XmRString,
    "black"
  },

  {
    "lightColor",
    "LightColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(PanTiltAttitudeGraph *, _lightColor),
    XmRString,
    "white"
  },

  {
    "cameraColor",
    "CameraColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(PanTiltAttitudeGraph *, _cameraColor),
    XmRString,
    "black"
  }
};


PanTiltAttitudeGraph::PanTiltAttitudeGraph(const char *name, Widget parent,
					   Radians headingRange, 
					   Radians pitchRange,
					   Radians panRange,
					   Radians tiltRange)
  : AttitudeGraph(name, parent, headingRange, pitchRange)
{
  getResources(_panTiltResources, XtNumber(_panTiltResources));
  
  XGCValues gcValues;
  gcValues.foreground = _cameraColor;
  gcValues.background = BlackPixel(_display, _screenNum);

  _panTiltPointGC = XCreateGC(_display, RootWindow(_display, _screenNum), 
			      (GCForeground | GCBackground), &gcValues);

  _panTilt[PortLight] = new PanTiltGraph(_display, _axisGC, _panTiltPointGC,
					 panRange, tiltRange,
					 PanTiltGraph::Circle);

  _panTilt[StbdLight] = new PanTiltGraph(_display, _axisGC, _panTiltPointGC,
					 panRange, tiltRange,
					 PanTiltGraph::Circle);

  _panTilt[PortCamera] = new PanTiltGraph(_display, _axisGC, _panTiltPointGC,
					  panRange, tiltRange,
					  PanTiltGraph::Rectangle);

  _panTilt[StbdCamera] = new PanTiltGraph(_display, _axisGC, _panTiltPointGC,
					  panRange, tiltRange,
					  PanTiltGraph::Rectangle);
}


PanTiltAttitudeGraph::~PanTiltAttitudeGraph()
{
}


void PanTiltAttitudeGraph::drawAxes()
{
  int x1, y1, x2, y2;

  XSetForeground(_display, _axisGC, _axisColor);

  // X axis
  x1 = 0;
  x2 = _width;
  y1 = y2 = _height / 2;
  XDrawLine(_display, _canvas, _axisGC, x1, y1, x2, y2);
  
  // Y axis
  x1 = x2 = _width / 2;
  y1 = 0;
  y2 = _height;
  XDrawLine(_display, _canvas, _axisGC, x1, y1, x2, y2);
}


void PanTiltAttitudeGraph::draw()
{
  AttitudeGraph::draw();

  // Compute new PanTilt graph dimensions
  int originX, originY, width, height;
  width = _width / 2;
  height = _height / 2;
  
  originX = 0;
  originY = 0;
  _panTilt[PortLight]->setDimensions(originX, originY, width, height);
  originX = _width / 2;
  _panTilt[StbdLight]->setDimensions(originX, originY, width, height);  
  originX = 0;
  originY = _height / 2;
  _panTilt[PortCamera]->setDimensions(originX, originY, width, height);
  originX = _width / 2;
  _panTilt[StbdCamera]->setDimensions(originX, originY, width, height);  

  XSetForeground(_display, _axisGC, _panTiltAxisColor);
  XSetForeground(_display, _panTiltPointGC, _lightColor);
  _panTilt[PortLight]->draw(_canvas);
  _panTilt[StbdLight]->draw(_canvas);  
  XSetForeground(_display, _panTiltPointGC, _cameraColor);
  _panTilt[PortCamera]->draw(_canvas);
  _panTilt[StbdCamera]->draw(_canvas);  
}


void PanTiltAttitudeGraph::setPanTilt(int index, Radians pan, Radians tilt)
{
  _panTilt[index]->setPan(pan);

  Math::minusPiToPi(&pan);
  if (pan >= -M_PI_2 && pan <= M_PI_2)
  {
    _panTilt[index]->setCenterPan(0.);
    _panTilt[index]->setLabel("");
  }
  else
  {
    _panTilt[index]->setCenterPan(M_PI);
    _panTilt[index]->setLabel("Aft");
  }
  
  _panTilt[index]->setTilt(tilt);
}