#include <stdlib.h>
#include <unistd.h>
#include <mbariTypes.h>
#include <usrTime.h>
#include "LapBoxApp.h"
#include "LapBoxInTask.h"
#include "Iview.h"
#include "IviewControl.h"

#define dprintf if (debug) fprintf


LapBoxInTask::LapBoxInTask(LapBoxTaskControl *taskParams, LapBoxMicro *micro,
                           IviewControl *iviewControlPT, 
                           IviewControl *iviewControlZFI)
{
  MBool debug = TRUE;
  dprintf(stderr, "LapBoxInTask constructor - taskParams->name()=\"%s\"\n",
	  taskParams->name());

  _micro = micro;
  
  _taskParams = taskParams;

  _iviewControlPT = iviewControlPT;
  _iviewControlZFI = iviewControlZFI;

                                   /* Create wakeup semaphore and watch dog */
                                  /* timer for provider rate control       */
  if ((_wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL )
  {
    fprintf(stderr, "LapBoxInTask::LapBoxInTask() - semBCreate() failed\n");
    exit(1);
  }
}


LapBoxInTask::~LapBoxInTask()
{ 
  MBool debug = TRUE;

  semDelete(_wakeupSem);
  delete _micro;
}


int LapBoxInTask::run()
{
  MBool debug = TRUE;
  Int16 joystickInit = 1;	// MBM 26.7.01, reinit JS filter
  Int16 pan, oldPan = 0, tilt, oldTilt = 0;
  Int16 focus, oldFocus = 0, zoom, oldZoom = 0, iris, oldIris = 0;
  Int16 medianFocus, inputFocus = 0, focusLast = 0, focusLastLast = 0;
  Flt32 focusFilter[FOCUS_FILTER_SIZE], focusAcc, focusAvg;
  Flt32 fltPan, fltTilt, fltZoom, fltFocus, fltIris;
  Flt32 avgPan = 0, avgTilt = 0;
  Flt32	inputPanLast = 0, inputTiltLast = 0;
  Flt32	inputPan = 0, inputTilt = 0;

  _iviewControlPT->startPeriodic();
  _iviewControlZFI->startPeriodic();

  FOREVER
  { 
    _taskParams->wait(IN_TASK, WAIT_FOREVER);
    
    while(_taskParams->microLinkState() == LINK_UP)
    {
      taskSafe();

      // check for lapbox enable 
      if (_taskParams->getEnabled())
      {
	// Sample joystick, focus, zoom, iris
	_micro->readData(&pan, &tilt, &zoom, &focus, &iris);
	LapBoxApp::updateAxis(&pan, &tilt, &zoom, &focus, &iris);         

	// normalize axis setpoint to axis rate (MBM, 5.2.01)
	inputPan = (pan / FULL_SCALE);
	inputTilt = (tilt / FULL_SCALE);

	// initialize joystick running sum or continue running average
	if (joystickInit)
	{
	  // when input value exists within deadband, include in running sum 
	  if ( (inputPan > 0 && inputPan < JOYSTICK_INIT_DEADBAND) ||
	       (inputPan < 0 && inputPan > -JOYSTICK_INIT_DEADBAND) )
	    if ( (inputTilt > 0 && inputTilt < JOYSTICK_INIT_DEADBAND) ||
		 (inputTilt < 0 && inputTilt > -JOYSTICK_INIT_DEADBAND) )
	    {
	      avgPan += inputPan;
	      avgTilt += inputTilt;
	      joystickInit++;
	    }

	  // stop pan-tilt movement during initialization
	  fltPan = 0.0;
	  fltTilt = 0.0;

	  // compute averages when running sum is complete
	  if (joystickInit == JOYSTICK_INIT_COUNT+1)
	  {
	    avgPan /= (Flt32) JOYSTICK_INIT_COUNT;
	    avgTilt /= (Flt32) JOYSTICK_INIT_COUNT;
	    fltPan = inputPan - avgPan;
	    fltTilt = inputTilt - avgTilt;
	    joystickInit = 0;
	  }	     

	  // printf("%9.6f %9.6f %9.6f %9.6f %9.6f %9.6f (initialize)\n", 
	  // inputPan, inputTilt, 
	  // avgPan, avgTilt,
	  // fltPan, fltTilt); 
	}
	else
	{
	  // compute filtered joystick values
	  // clip filter pan-axis input values and compare to deadband
	  if (inputPan > 0.0 && inputPan < AVG_INPUT_CLIP ||
	      inputPan < 0.0 && inputPan > -AVG_INPUT_CLIP)
	  {
	    avgPan = A1 * avgPan + B0 * inputPan + B1 * inputPanLast;
	    inputPanLast = inputPan;
	  }
	  fltPan = inputPan - avgPan;
	  if ( (fltPan > 0 && fltPan < JOYSTICK_DEADBAND) ||
	       (fltPan < 0 && fltPan > -JOYSTICK_DEADBAND) )
	    fltPan = 0.0;

	  // clip filter tilt-axis input values and compare to deadband
	  if (inputTilt > 0.0 && inputTilt < AVG_INPUT_CLIP ||
	      inputTilt < 0.0 && inputTilt > -AVG_INPUT_CLIP)
	  {
	    avgTilt  = A1 * avgTilt + B0 * inputTilt + B1 * inputTiltLast;
	    inputTiltLast = inputTilt;
	  }
	  fltTilt = inputTilt - avgTilt;
	  if ( (fltTilt > 0 && fltTilt < JOYSTICK_DEADBAND) ||
	       (fltTilt < 0 && fltTilt > -JOYSTICK_DEADBAND) )
	    fltTilt = 0.0;

	  // printf("%9.6f %9.6f %9.6f %9.6f %9.6f %9.6f\n", 
	  // inputPan, inputTilt, 
	  // avgPan, avgTilt,
	  // fltPan, fltTilt); 
	}

        // if joystick changed, send request to Iview
        if (pan != oldPan || tilt != oldTilt) 
        {
	  _iviewControlPT->setPanTilt(fltPan, fltTilt);
	  oldPan = pan;
	  oldTilt = tilt;
        }
    
#if 0	
	// Median filter focus input and check against deadband (MBM 24.7.01)
	// This code solves a problem with the deadband check. It uses
	// the median value of the last three focus values to establish a
	// current focus value; further focus input values are checked against
	// a deadband around the current value. Only values outside the
	// deadband are sent to iView.
	if ( (focus >= focusLast) && (focus <= focusLastLast) 	||
	     (focus >= focusLastLast) && (focus <= focusLast) )
	  medianFocus = focus;
	else
	  if ( (focusLast >= focus) && (focusLast <= focusLastLast) ||
	       (focusLast >= focusLastLast) && (focusLast <= focus) )
	    medianFocus = focusLast;
	  else
	    if ( (focusLastLast >= focus) && (focusLastLast <= focusLast) ||
		 (focusLastLast >= focusLast) && (focusLastLast <= focus) )
	      medianFocus = focusLastLast;

	if ( (medianFocus > inputFocus + FOCUS_DEADBAND) ||
	     (medianFocus < inputFocus - FOCUS_DEADBAND) )
	{
	  inputFocus = medianFocus;
	  fltFocus = inputFocus/FULL_SCALE;
	  _iviewControlZFI->setFocus(fltFocus);

	  // printf("%4d %4d %4d %4d %4d %f\n", 
	  // focus, focusLast, focusLastLast, medianFocus, inputFocus, fltFocus); 
	}
	focusLastLast = focusLast;
	focusLast = focus;
#else
	// Running filter focus input and check against deadband (MBM 27.7.01)
	// This code solves a problem with the deadband check. It uses
	// a running average value of the last several focus values to establish an
	// average focus value; further focus input values are checked against
	// a deadband around the average value. Only values outside the
	// deadband are sent to iView.
	focusAcc -= focusFilter[FOCUS_FILTER_SIZE-1];
	for (int i = 1; i < FOCUS_FILTER_SIZE; i++)
	  focusFilter[FOCUS_FILTER_SIZE-i] = focusFilter[FOCUS_FILTER_SIZE-i-1];
	focusFilter[0] = (Flt32) focus/FULL_SCALE;
	focusAcc += focusFilter[0];
	focusAvg = focusAcc/FOCUS_FILTER_SIZE;

	if ( (focusFilter[0] > focusAvg + FOCUS_DEADBAND) ||
	     (focusFilter[0] < focusAvg - FOCUS_DEADBAND) )
	{
	  fltFocus = focusFilter[0];
	  _iviewControlZFI->setFocus(fltFocus);
	}
#endif
    
	// If Zoom changed, send request to Iview
	if (zoom != oldZoom)
        {
	  //Normalize axis setpoint to axis rate
	  fltZoom = (zoom / FULL_SCALE);
	  //dprintf(stderr,"Setpoint Received, zoomValue=%d\n", zoom);
	  _iviewControlZFI->setZoom(fltZoom);
	  oldZoom = zoom;
	}

        // If Iris changed, send request to Iview
	if (iris != oldIris)
        {
	  //Normalize axis setpoint to axis rate
	  fltIris = (iris / FULL_SCALE);
	  //dprintf(stderr,"Setpoint Received, irisValue=%d\n", iris);
	  _iviewControlZFI->setIris(-fltIris);
	  oldIris = iris;
	}
      }  // enable check
      else
      {
	// if lapbox not enabled, restart joystick initialization
	joystickInit = 1;
	avgPan = 0;
	avgTilt = 0;
      }
       
      taskUnsafe();	// pend until sem timeout wakes us up   

      semTake(_wakeupSem, 
	      sysClkRateGet() / (USECS_PER_SEC / LAP_BOX_DM_PERIOD));

    }	// while LINK_UP  
  }	// FOREVER

  return 0;
}