static char AttitudeGraph_id[] = "$Header: AttitudeGraph.cc,v 1.2 97/03/20 12:28:44 oreilly Exp $";

/*
$Log:	AttitudeGraph.cc,v $
Revision 1.2  97/03/20  12:28:44  12:28:44  oreilly (Thomas C. O'Reilly)
*** empty log message ***

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

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

#define dprintf if (debug) fprintf

XtResource AttitudeGraph::_resources[] = 
{
  {
    "skyColor",
    "SkyColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(AttitudeGraph *, _skyColor),
    XmRString,
    "lightblue"
  },

  {
    "horizonColor",
    "HorizonColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(AttitudeGraph *, _horizonColor),
    XmRString,
    "black"
  },

  {
    "axisColor",
    "AxisColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(AttitudeGraph *, _axisColor),
    XmRString,
    "black"
  },

  {
    "groundColor",
    "GroundColor",
    XtRPixel,
    sizeof(Pixel),
    XtOffset(AttitudeGraph *, _groundColor),
    XmRString,
    "brown"
  }
};


AttitudeGraph::AttitudeGraph(const char *name, Widget parent,
			     Radians headingRange, Radians pitchRange)
  : VkDoubleBuffer(name, parent)
{
  getResources(_resources, XtNumber(_resources));
  
  _display = theApplication->display();
  _screenNum = DefaultScreen(_display);
  
  XGCValues gcValues;
  gcValues.foreground = _axisColor;
  gcValues.background = BlackPixel(_display, _screenNum);

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

  gcValues.foreground = _horizonColor;
  gcValues.background = BlackPixel(_display, _screenNum);
  
  _horizonGC = XCreateGC(_display, RootWindow(_display, _screenNum), 
			 (GCForeground | GCBackground), &gcValues);

  XSetLineAttributes(_display, _horizonGC, 3, LineSolid, CapButt, JoinBevel);
  
  gcValues.foreground = _skyColor;
  gcValues.background = BlackPixel(_display, _screenNum);

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

  gcValues.foreground = _groundColor;
  gcValues.background = BlackPixel(_display, _screenNum);

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

  setRoll(0.);
  setPitch(0.);
  setHeading(0.);

  if (headingRange <= 0. || pitchRange <= 0. || 
      headingRange >= 2.*M_PI || pitchRange >= 2.*M_PI)
  {
    setError("Invalid headingRange or pitchRange");
    return;
  }
  
  _xRange = headingRange;
  _yRange = pitchRange;
}


AttitudeGraph::~AttitudeGraph()
{
}


void AttitudeGraph::setPitch(Radians pitch)
{
  _pitch = pitch;
  Math::minusPiToPi(&_pitch);  
}


void AttitudeGraph::setRoll(Radians roll)
{
  _roll = roll;
  Math::zeroToTwoPi(&_roll);
}


void AttitudeGraph::setHeading(Radians heading)
{
  _heading = heading;
  Math::zeroToTwoPi(&_heading);
}



void AttitudeGraph::draw()
{
  // Fill in sky
  XFillRectangle(_display, _canvas, _skyGC, 0, 0, _width, _height);

  drawHorizon();
  drawAxes();
}


void AttitudeGraph::drawHorizon()
{
  Boolean debug = False;

  /*
     Find equation of horizon line in vehicle coordinate system (after 
     applying pitch and roll) 
  */
  
  // Find origin after pitch
  double newX0 = 0.;
  double newY0 = _pitch;

  // Prior to pitch and roll, two points on horizon line are (xmin, 0) and
  // (xmax, 0).  Find coordinates of those points after pitch and roll.
  double xmin = -_xRange / 2.;
  double xmax = -xmin;
  double ymin = -_yRange / 2.;
  double ymax = -ymin;
  
  double roll = -_roll;
  
  double cosRoll = cos(roll);
  double sinRoll = sin(roll);
  
  double x1 = (xmax - newX0) * cosRoll - newY0 * sinRoll;
  double y1 = -newY0 * cosRoll - (xmax - newX0) * sinRoll;
  double x2 = (xmin - newX0) * cosRoll - newY0 * sinRoll;
  double y2 = -newY0 * cosRoll - (xmin - newX0) * sinRoll;

  int i1, j1, i2, j2;
  int minJ = 0;
  int maxJ = _height;

  Boolean upsideDown;
  double degrees = _roll / Math::RadsPerDeg;
  physicalCoords(x1, ymax, &i1, &j1);
  physicalCoords(x2, ymin, &i2, &j2);      
  if (i2 == i1)
  {
    // Vertical line
    int deg = (int )(degrees + 0.5);
    
    if (deg == 90)
    {
      upsideDown = False;
      j1 = minJ;
      j2 = maxJ;
    }
    else
    {
      upsideDown = True;
      j1 = maxJ;
      j2 = minJ;
    }  
  }
  else 
  {
    // Determine equation of horizon line after pitch and roll
    double slope = (y2 - y1) / (x2 - x1);
    double intercept = y1 - slope * x1;
  
    double y;

    if (degrees > 270. || degrees < 90.)
    {
      y = xmax * slope + intercept;
      physicalCoords(xmax, y, &i1, &j1);
      y = xmin * slope + intercept;
      physicalCoords(xmin, y, &i2, &j2);
      upsideDown = False;
    }
    else
    {
      y = xmin * slope + intercept;
      physicalCoords(xmin, y, &i1, &j1);
      y = xmax * slope + intercept;
      physicalCoords(xmax, y, &i2, &j2);
      upsideDown = True;
    }

    if (j1 < minJ || j1 > maxJ || j2 < minJ || j2 > maxJ)
    {
      double m = (double )(j2 - j1) / (double )(i2 - i1);
      double b = j1 - m * i1;

      if (j1 < minJ)
      {
	j1 = minJ;
	i1 = (int )((j1 - b) / m);
      }
      if (j1 > maxJ)
      {
	j1 = maxJ;
	i1 = (int )((j1 - b) / m);
      }
    
      if (j2 < minJ)
      {
	j2 = minJ;
	i2 = (int )((j2 - b) / m);
      }
      if (j2 > maxJ)
      {
	j2 = maxJ;
	i2 = (int )((j2 - b) / m);
      }
    }
  }

  XDrawLine(_display, _canvas, _horizonGC, i1, j1, i2, j2);  

  int nPoints = defineHorizonPolygon(i1, j1, i2, j2, upsideDown);
  
  XFillPolygon(_display, _canvas, _groundGC, _horizonPolygon, nPoints,
	       Nonconvex, CoordModeOrigin);

  return;
}


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

  // 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);

  unsigned height = _height / 2;
  unsigned width = _width / 2;
  int x = _width / 2 - width / 2;
  int y = _height / 2 - height / 2;
  XDrawRectangle(_display, _canvas, _axisGC, x, y, width, height); 
}


/* Convert x and y to physical coordinates. Origin is always at center of
graph area, and x=0, y=0 */
int AttitudeGraph::physicalCoords(double x, double y,
				   int *i, int *j)
{
  double value;
  double xmin = -_xRange / 2.;
  value = ((x - xmin) * _width / _xRange); 
  if (fabs(value) > MAXINT)
    return -1;
  
  *i = (int )(value + 0.5);
  
  double ymin = -_yRange / 2.;
  value = _height - ((y - ymin) * _height / _yRange);
  if (fabs(value) > MAXINT)
    return -1;
  
  *j = (int )(value + 0.5);
  
  return 0;
}


int AttitudeGraph::defineHorizonPolygon(int x1, int y1, int x2, int y2, 
					 Boolean upsideDown)
{
  int xPad = _width;
  int yPad = _height;
  
  XPoint corner[4];
  corner[0].x = -2 * xPad;
  corner[0].y = -2 * yPad;
  corner[1].x = _width + 2 * xPad;
  corner[1].y = -2 * yPad;
  corner[2].x = _width + 2 * xPad;
  corner[2].y = _height + 2 * yPad;
  corner[3].x = -2 * xPad;
  corner[3].y = _height + 2 * yPad;
  
  _horizonPolygon[0].x = x1;
  _horizonPolygon[0].y = y1;

  int nPoints = 1;

  if (!upsideDown)
  {
    _horizonPolygon[1].x = x1 + xPad;
    _horizonPolygon[1].y = y1;
    _horizonPolygon[2].x = corner[1].x;
    _horizonPolygon[2].y = corner[1].y;
    _horizonPolygon[3].x = corner[2].x;
    _horizonPolygon[3].y = corner[2].y;
    _horizonPolygon[4].x = corner[3].x;
    _horizonPolygon[4].y = corner[3].y;
    _horizonPolygon[5].x = corner[0].x;
    _horizonPolygon[5].y = corner[0].y;
    _horizonPolygon[6].x = x2 - xPad;
    _horizonPolygon[6].y = y2;    
  }
  else
  {
    _horizonPolygon[1].x = x1 - xPad;
    _horizonPolygon[1].y = y1;
    _horizonPolygon[2].x = corner[3].x;
    _horizonPolygon[2].y = corner[3].y;
    _horizonPolygon[3].x = corner[0].x;
    _horizonPolygon[3].y = corner[0].y;
    _horizonPolygon[4].x = corner[1].x;
    _horizonPolygon[4].y = corner[1].y;
    _horizonPolygon[5].x = corner[2].x;
    _horizonPolygon[5].y = corner[2].y;
    _horizonPolygon[6].x = x2 + xPad;
    _horizonPolygon[6].y = y2;    
  }
  
  _horizonPolygon[7].x = x2;
  _horizonPolygon[7].y = y2;

  return 8;
}