#include <stdlib.h>
#include <unistd.h>
#include "PilotTrayApp.h"
#include "PilotTrayInTask.h"
#include "Iview.h"
#include "IviewControl.h"

#define dprintf if (debug) fprintf

PilotTrayInTask::PilotTrayInTask(PilotTrayTaskControl *taskParams,
                           PilotTrayMicro *micro,
                           IviewControl *iviewControlPT, 
                           IviewControl *iviewControlZFI)
{

  MBool debug = TRUE;
  dprintf(stderr, "PilotTrayInTask constructor - taskParams->name()=\"%s\"\n",
          taskParams->name());

  _micro = micro;

  _taskParams = taskParams;

  _iviewControlPT = iviewControlPT;
  //Don't need This yet
  //_iviewControlZFI = iviewControlZFI;

                                  /* Start Consumer on Data               */
  _micro->startConsumeData();

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

PilotTrayInTask::~PilotTrayInTask()
{
  semDelete(_wakeupSem);
  delete _micro;
}


int PilotTrayInTask::run()
{
  MBool debug = FALSE;

  Int16 pan, oldPan = 0, tilt, oldTilt = 0;
  Int16 focus, oldFocus = 0, zoom, oldZoom = 0;
  Flt32 fltPan, fltTilt, fltZoom, fltFocus;

  _iviewControlPT->startPeriodic();
  //Don't need This Yet
  //_iviewControlZFI->startPeriodic();

  FOREVER
  {
    taskSafe();

      // Sample joystick, focus, zoom
      _micro->readData(&pan, &tilt, &zoom, &focus);
      // If joystick changed, send request to Iview
      if (pan != oldPan || tilt != oldTilt)
      {
        //Normalize axis setpoint to axis rate
        //Normalize axis setpoint to axis rate
        fltPan = ((pan * PAN_TILT_GAIN) / FULL_SCALE);
        fltTilt = ((tilt * PAN_TILT_GAIN) / FULL_SCALE);

        // Clip Float Values greater than Max or less than min after Scaling
        if (fltPan > FLT_FULL_SCALE)
            fltPan = FLT_FULL_SCALE;
        if (fltPan < -FLT_FULL_SCALE)
            fltPan = -FLT_FULL_SCALE;

        if (fltTilt > FLT_FULL_SCALE)
            fltTilt = FLT_FULL_SCALE;
        if (fltTilt < -FLT_FULL_SCALE)
            fltTilt = -FLT_FULL_SCALE;
        //dprintf(stderr,
        //    "Setpoint Received, panValue=%d, tiltValue=%d\n", pan, tilt);
        _iviewControlPT->setPanTilt(fltPan, fltTilt);
        oldPan = pan;
        oldTilt = tilt;
       }
#if 0 
       //No focus or zoom  devices supported yet
       // If focus changed, send request to Iview
       if (focus != oldFocus)
       {
         if (camera != IviewNoDevice)
         {
         //Normalize axis setpoint to axis rate
           fltFocus = (focus / FULL_SCALE);
           //dprintf(stderr,"Setpoint Received, focusValue=%d\n", focus);
           //dprintf(stderr,"Camera Selected, camera=%d\n", camera);
           _iviewControlZFI->setFocusRate(fltFocus);
         }
         //else
         //{
         // User tried to focus without selecting a camera
         //  _micro->writeSettings(PORT_CAMERA_SELECT_BUTTON,
         //                      PilotTrayButtonError, FALSE);

         //  _micro->writeSettings(STBD_CAMERA_SELECT_BUTTON,
         //                      PilotTrayButtonError, FALSE);
         //}
         oldFocus = focus;
       }

       // 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->setZoomRate(fltZoom);

        oldZoom = zoom;
       }

#endif

    taskUnsafe();                  /* pend until sem timeout  wakes us up   */
    semTake(_wakeupSem, sysClkRateGet() /
                   (USECS_PER_SEC / PILOT_TRAY_DM_PERIOD));

  }//FOREVER
  return 0;
}