static char DepthAltitude_id[] = "$Header: DepthAltitude.cc,v 1.6 97/10/03 09:28:47 oreilly Exp $";

/*
$Log:	DepthAltitude.cc,v $
Revision 1.6  97/10/03  09:28:47  09:28:47  oreilly (Thomas C. O'Reilly)
*** empty log message ***

Revision 1.5  97/09/10  08:47:19  08:47:19  oreilly (Thomas C. O'Reilly)
Load and save preferences

Revision 1.4  97/08/12  14:36:31  14:36:31  oreilly (Thomas C. O'Reilly)
Added depth rate

Revision 1.3  97/03/20  12:29:01  12:29:01  oreilly (Thomas C. O'Reilly)
*** empty log message ***

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

*/
#include <stdlib.h>
#include <Xm/Form.h>
#include <Xm/Label.h>
#include <Xm/Separator.h>
#include <Xm/ToggleB.h>
#include <Xm/PushB.h>
#include <Xm/RowColumn.h>
#include "sensorsDm.h"
#include "controlDM.h"
#include "DepthAltitude.h"
#include "DmErrno.h"
#include <sys/param.h>

#define dprintf if (debug) fprintf

VkMenuDesc DepthAltitude::_menuPane[] = 
{
  {RADIOSUBMENU, "Scale", NULL, DepthAltitude::_scaleMenuPane},
  {RADIOSUBMENU, "Mode", NULL, DepthAltitude::_modeMenuPane},
  {END}
};

VkMenuDesc DepthAltitude::_scaleMenuPane[] = 
{
  {LABEL, "Scale"},
  {SEPARATOR},
  {TOGGLE, "Coarse Y scale", &DepthAltitude::coarseScaleCallback},
  {TOGGLE, "Medium Y scale", &DepthAltitude::mediumScaleCallback},
  {TOGGLE, "Fine Y scale",  &DepthAltitude::fineScaleCallback},
  {END}
};

VkMenuDesc DepthAltitude::_modeMenuPane[] = 
{
  {LABEL, "Mode"},
  {SEPARATOR},
  {TOGGLE, "Depth", &DepthAltitude::depthModeCallback},
  {TOGGLE, "Altimeter", &DepthAltitude::altimeterModeCallback},
  {END}
};


XtResource DepthAltitude::_resources[] = 
{
  {
    "vehicleDataColor",
    "VehicleDataColor",
    XmRString,
    sizeof(XmRString),
    XtOffset(DepthAltitude *, _vehicleDataColor),
    XmRString, 
    "green"
  },
  {
    "bottomDataColor",
    "BottomDataColor",
    XmRString,
    sizeof(XmRString),
    XtOffset(DepthAltitude *, _bottomDataColor),
    XmRString, 
    "red"
  }
};

const char *const DepthAltitude::modeMnem = "DepthAltitudeMode";
const char *const DepthAltitude::scaleMnem = "DepthAltitudeScale";

DepthAltitude::DepthAltitude(const char *name, Widget parent, 
			     unsigned maxAltitude,
			     unsigned historyMinutes,
			     unsigned period)
  : DmGuiObject(name, parent)
{
  _points = 0;
  
  _baseWidget = XtVaCreateWidget(name, xmFormWidgetClass, parent, 
				 NULL);
  
  installDestroyHandler();
  getResources(_resources, XtNumber(_resources));
  
  char errorBuf[errorMsgSize];

  if (period < 1000)
  {
    setError("Minimum allowed period is 1000 msec");
    return;
  }

  int totalPoints = historyMinutes * 60 * 1000 / period;
  if (!totalPoints)
  {
    sprintf(errorBuf, "Bad historyMinutes (%d) and/or period (%d)",
	    historyMinutes, period);

    setError(errorBuf);
    return;
  }
  int nbytes = totalPoints * sizeof(DepthAltitudePoint);

  _points = new CircularQueue<DepthAltitudePoint>(totalPoints);
  if (_points->error())
  {
    _points->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  
  if ((_xrtData = XrtMakeData(XRT_DATA_ARRAY, 2, totalPoints, True)) == NULL)
  {
    sprintf(errorBuf, 
	    "XrtMakeData() failed for _xrtData (%d points)", totalPoints);

    setError(errorBuf);
    return;
  }
  _maxAltitude = maxAltitude;
  float xmax = historyMinutes;

  XrtDataStyle dataStyles[2];
  for (int i = 0; i < 2; i++)
  {
    dataStyles[i].lpat = XRT_LPAT_SOLID;
    dataStyles[i].fpat = XRT_FPAT_SOLID;
    dataStyles[i].psize = 3;
    dataStyles[i].point = XRT_POINT_NONE;
    
    if (i == VehicleDepth)
    {
      dataStyles[i].color = _vehicleDataColor;
      dataStyles[i].pcolor = _vehicleDataColor;
      dataStyles[i].width = 2;
    }
    else
    {
      dataStyles[i].color = _bottomDataColor;
      dataStyles[i].pcolor = _bottomDataColor;
      dataStyles[i].width = 3;
    }
  }
    
  XrtDataStyle *dataStylesPtr[3];
  dataStylesPtr[0] = &dataStyles[0];
  dataStylesPtr[1] = &dataStyles[1];
  dataStylesPtr[2] = NULL;
  
  _graph = 
    XtVaCreateManagedWidget("graph", xtXrtGraphWidgetClass, _baseWidget,
			    XtNxrtType, XRT_TYPE_PLOT,
			    XtNxrtXAxisMin, XrtFloatToArgVal(0.),
			    XtNxrtXAxisMax, XrtFloatToArgVal(xmax),
			    XtNxrtXAxisReversed, True,
			    XtNxrtXNumMethod, XRT_NUM_PRECISION,
			    XtNxrtXPrecision, 0,
			    XtNxrtXNum, XrtFloatToArgVal(1.0),
			    XtNxrtXGrid, XrtFloatToArgVal(1.0),
			    XtNxrtXTick, XrtFloatToArgVal(0.5),
			    XtNxrtYPrecision, 0,
			    XtNxrtDataStyles, dataStylesPtr,
			    NULL);

  XtVaSetValues(_graph, 
		XmNleftAttachment, XmATTACH_FORM,
		XmNrightAttachment, XmATTACH_FORM,
		XmNtopAttachment, XmATTACH_FORM,
		XmNbottomAttachment, XmATTACH_FORM,
		NULL);

  new VkPopupMenu(_graph, "depthAltitudeMenu", _menuPane, (XtPointer )this);

  char itemName[MaxDmNameLen];

  strcpy(itemName, VEHICLE_ALTITUDE_DM);

  _dmAltitude = new DmFlt32Object(itemName);
  if (_dmAltitude->error())
  {
    _dmAltitude->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  if (addDmObject(_dmAltitude, DmConsumeFlag) == -1)
    return;

  strcpy(itemName, VEHICLE_DEPTH_DM);

  _dmDepth = new DmFlt64Object(itemName);
  if (_dmDepth->error())
  {
    _dmDepth->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  if (addDmObject(_dmDepth, DmConsumeFlag) == -1)
    return;

  strcpy(itemName, SENSOR_RATE_DEPTH_DM);
  
  _dmDepthRate = new DmFlt32Object(itemName);
  if (_dmDepthRate->error())
  {
    _dmDepthRate->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }

  if (addDmObject(_dmDepthRate, DmConsumeFlag) == -1)
    return;

#ifdef ZILCH
  /* FUTURE - USE WHEN KALMAN IS FIXED */
  MBool readRaw = FALSE;
  _dmPosition = new DmPositionObject(readRaw);
  if (_dmPosition->error())
  {
    _dmPosition->errorMsg(errorBuf);
    setError(errorBuf);
    return;
  }
  if (addDmObject(_dmPosition, DmConsumeFlag) == -1)
    return;
#endif
  
  if (setConsumePeriod((int )period) == -1)
    return;

  rescaleYAxis(mediumScaleRange);
  setMode(DepthMode);
  
  updateGui();
}


DepthAltitude::~DepthAltitude()
{
  delete _points;
  XrtDestroyData(_xrtData, True);
}


void DepthAltitude::updateGui(DM_Item handle)
{
  Boolean debug = False;
  DM_Time t;
  DepthAltitudePoint point;
  char errorBuf[errorMsgSize];

  /* Note that "time" is time right now, rather than time at which Dm items
     were written; this insures that depth and altitude use same time axis */
  time_t now = point.time = time(NULL);
  Errno errno;

  if ((errno = _dmDepth->read(&point.depth, &t)) != SUCCESS)
  {
    sprintDmError(errno, 
		  "DepthAltitude::updateGui(), error reading depth", 
		  errorBuf);

    setError(errorBuf);
    point.depth = MissingDepthAltitude;
  }
  
#ifdef ZILCH
  
  /* FUTURE - USE WHEN KALMAN IS FIXED! */
  StateValue state[NStateElements];
  if ((errno = _dmPosition->read(state, &t)) != SUCCESS)
  { 
    sprintDmError(errno, 
		  "DepthAltitude::updateGui(), error reading state", 
		  errorBuf);

    setError(errorBuf);
    point.depth = MissingDepthAltitude;
  }
  point.depth = state[2];
#endif
  

  if ((errno = _dmAltitude->read(&point.altitude, &t)) != SUCCESS)
  {
    sprintDmError(errno, 
		  "DepthAltitude::updateGui(), error reading altitude", 
		  errorBuf);

    setError(errorBuf);
    point.altitude = MissingDepthAltitude;
  }

  _points->add(&point);

  Flt32 depthRate;
  
  if ((errno = _dmDepthRate->read(&depthRate, &t)) != SUCCESS)
  {
    sprintDmError(errno, 
		  "DepthAltitude::updateGui(), error reading depthRate", 
		  errorBuf);

    setError(errorBuf);
  }

  // Update xrtData structure
  _points->startIter();
  DepthAltitudePoint *pnt;

  for (int i = 0; i < _points->maxQueueSize(); i++)
  {
    if (pnt = _points->next())
    {
      _xrtData->arr_xel(i) = (now - pnt->time) / 60.;
      if (_mode == DepthMode)
      {
	_xrtData->arr_yel(VehicleDepth, i) = pnt->depth;

	if (pnt->altitude <= _maxAltitude)
	  _xrtData->arr_yel(BottomDepth, i) = pnt->depth + pnt->altitude; 
	else
	  _xrtData->arr_yel(BottomDepth, i) = MissingDepthAltitude;
      }
      else if (_mode == AltimeterMode)
      {
	if (pnt->altitude <= _maxAltitude)
	  _xrtData->arr_yel(0, i) = pnt->altitude;
	else
	  _xrtData->arr_yel(0, i) = MissingDepthAltitude;

	_xrtData->arr_yel(1, i) = MissingDepthAltitude;
      }
    }
    else
    {
      _xrtData->arr_yel(VehicleDepth, i) = MissingDepthAltitude;
      _xrtData->arr_yel(BottomDepth, i) = MissingDepthAltitude;
    }
  }

  shiftYAxis(&point, &_depthAxisMin, &_depthAxisMax, _depthAxisRange);

  char modeBuf[100];

  Boolean reversed;
  if (_mode == DepthMode)
  {
    reversed = True;
    strcpy(modeBuf, "Depth");
  }
  else if (_mode == AltimeterMode)
  {
    reversed = False;
    strcpy(modeBuf, "Alt");
  }  
  
  char footerBuf1[100];
  char footerBuf2[100];  

  sprintf(footerBuf1, "%-5s %-5s %-5s %-5s %-5s", 
	  "Depth", "Alt", "Bot", "m/min", "Mode");
  
  char *depthFormat;
  char *bottomFormat;
  
  if (point.depth < 1000.)
    depthFormat = "%-5.1f";
  else
    depthFormat = "%-5.0f";

  char format[100];
  
  if (point.altitude <= _maxAltitude)
  {
    sprintf(format, "%s %%-5.1f %s %%-5.1f %%-5s", depthFormat, depthFormat);
    
    sprintf(footerBuf2, format,
	    point.depth, point.altitude, point.depth + point.altitude, 
	    depthRate * 60.,
	    modeBuf);
  }
  else
  {
    sprintf(format, "%s >%%-4d >%%-4.0f %%-5.1f %%-5s", depthFormat);

    sprintf(footerBuf2, format,
	    point.depth, _maxAltitude, "", point.depth + _maxAltitude,
	    depthRate * 60., modeBuf);
  }

  char *footerStrings[3];
  footerStrings[0] = footerBuf1;
  footerStrings[1] = footerBuf2;
  footerStrings[2] = NULL;
  
  // Update graph Widget
  XtVaSetValues(_graph,
		XtNxrtData, _xrtData,
		XtNxrtYAxisMin, XrtFloatToArgVal(_depthAxisMin),
		XtNxrtYAxisMax, XrtFloatToArgVal(_depthAxisMax),  
		XtNxrtFooterStrings, footerStrings,
		XtNxrtFooterAdjust, XRT_ADJUST_LEFT,
		XtNxrtYAxisReversed, reversed,
		NULL);
}



void DepthAltitude::shiftYAxis(DepthAltitudePoint *point,
			       float *axisMin, float *axisMax, 
			       float axisRange)
{
  _yAxisShiftFactor = 0.5;
  float bottomDepth;
  
  switch (_mode)
  {
    case DepthMode:
    if (point->depth < *axisMin)
    {
      *axisMin = MAX(point->depth - axisRange * _yAxisShiftFactor, 0.);
      *axisMax = *axisMin + axisRange;
    }
    else if (point->depth > *axisMax)
    {
      *axisMax = point->depth + axisRange * _yAxisShiftFactor;
      *axisMin = *axisMax - axisRange;
    }

    bottomDepth = point->altitude + point->depth;
    if (point->altitude < _maxAltitude && 
	bottomDepth > *axisMax && (bottomDepth - point->depth) < axisRange)
    { 
      // Shift scale to bring bottom into view
      *axisMax = bottomDepth;
      *axisMin = *axisMax - axisRange;  
    }
  
    break;

    case AltimeterMode:
    if (point->altitude < *axisMin)
    {
      *axisMin = MAX(point->altitude - axisRange * _yAxisShiftFactor, 0.);
      *axisMax = *axisMin + axisRange;
    }
    else if (point->altitude > *axisMax)
    {
      *axisMax = point->altitude + axisRange * _yAxisShiftFactor;
      *axisMin = *axisMax - axisRange;
    }    
    break;
  }
}


void DepthAltitude::rescaleYAxis(unsigned newRange)
{
  _depthAxisRange = newRange;
  XtVaSetValues(_graph, 
		XtNxrtYNum, XrtFloatToArgVal(newRange / 5.),
		XtNxrtYTick, XrtFloatToArgVal(newRange / 10.),
		NULL);
  
  _depthAxisMin = MAXFLOAT;
  _depthAxisMax = -MAXFLOAT;
  updateGui();
}


void DepthAltitude::showControls()
{
}


void DepthAltitude::setMode(DepthAltimeterMode mode)
{
  _mode = mode;
}


void DepthAltitude::buttonEventHandler(Widget w, 
				       XtPointer clientData, 
				       XEvent *event)
{
  Boolean debug = False;
  dprintf(stderr, "DepthAltitude::buttonEventCallback()\n");
  DepthAltitude *obj = (DepthAltitude *)clientData;
  obj->showControls();
}


void DepthAltitude::coarseScaleCallback(Widget w, XtPointer clientData,
					XtPointer callData)
{
  Boolean debug = False;
  DepthAltitude *obj = (DepthAltitude *)clientData;
  obj->rescaleYAxis(coarseScaleRange);  
}


void DepthAltitude::mediumScaleCallback(Widget w, XtPointer clientData,
					XtPointer callData)
{
  Boolean debug = False;
  DepthAltitude *obj = (DepthAltitude *)clientData;
  obj->rescaleYAxis(mediumScaleRange);
}


void DepthAltitude::fineScaleCallback(Widget w, XtPointer clientData,
				      XtPointer callData)
{
  Boolean debug = False;
  DepthAltitude *obj = (DepthAltitude *)clientData;
  obj->rescaleYAxis(fineScaleRange);  
}

void DepthAltitude::depthModeCallback(Widget w, XtPointer clientData,
				      XtPointer callData)
{
  Boolean debug = False;
  DepthAltitude *obj = (DepthAltitude *)clientData;
  obj->setMode(DepthMode);
}


void DepthAltitude::altimeterModeCallback(Widget w, XtPointer clientData,
					  XtPointer callData)
{
  Boolean debug = False;
  DepthAltitude *obj = (DepthAltitude *)clientData;
  obj->setMode(AltimeterMode);
}


int DepthAltitude::loadPreferences(DynArray<Preference *> *preferences, 
				   char *errorBuf)
{
  MBool error = FALSE;
  for (int i = 0; i < preferences->size(); i++)
  {
    Preference *preference;
    preferences->get(i, &preference);
    
    if (!strcmp(preference->keyword(), modeMnem))
    {
      DepthAltimeterMode mode = (DepthAltimeterMode )atoi(preference->value());
      setMode(mode);
    }
    
    else if (!strcmp(preference->keyword(), scaleMnem))
      rescaleYAxis(atoi(preference->value()));
  }
  
  return 0;
}


int DepthAltitude::savePreferences(DynArray<Preference *> *preferences, 
				   char *errorBuf)
{
  char buf[100];

  Preference *preference;

  sprintf(buf, "%d", _mode);
  preference = new Preference(modeMnem, buf);
  preferences->add(&preference);

  sprintf(buf, "%d", _depthAxisRange);
  preference = new Preference(scaleMnem, buf);
  preferences->add(&preference);

  return 0;
}