#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>

#include "PilotTrayApp.h"
#include "PilotTrayInTask.h"
#include "PilotTrayOutTask.h"
#include "pilotTray.h"

#include "vxUtils.h"


#define dprintf if (debug) fprintf

PilotTrayApp::PilotTrayApp(const char *name)
{
  MBool debug = TRUE;

  _name = strdup(name);

  _uiObject = new PilotTrayMicro(name);

  // Create PilotTrayParams object
  _params = new PilotTrayTaskControl(name);

  // Get Task Id
  _params->SRQTaskPid = taskIdSelf();

  // Create IviewControl Objects
  _iviewControlPT = new IviewControl((IviewPanRate | IviewTiltRate), 
                                      FALSE, "PilotTrayPanTilt");

  _iviewControlZFI = new IviewControl(IviewFocus | IviewZoomRate | IviewIrisRate, 
	                              FALSE, "PilotTrayFocusZoomIris"); 

  dprintf(stderr,"PilotTrayApp constructor - params->name()=\"%s\"\n",
          _params->name());
  dprintf(stderr, "PilotTrayApp constructor; created params \"%s\"\n", name);

  _inTask = NULL;
  _outTask = NULL;
  _deviceMask = 0;
}


PilotTrayApp::~PilotTrayApp()
{
  MBool debug = TRUE;
  free(_name);

  delete _uiObject;
  delete _params;
  delete _iviewControlPT;
  delete _iviewControlZFI;

  if (_inTask)
  {
    dprintf(stderr, "delete _inTask\n");
    delete _inTask;
  }

  if (_outTask)
  {
    dprintf(stderr, "delete _outTask\n");
    delete _outTask;
  }
}



int PilotTrayApp::run()
{
  MBool reset;

  if (spawnTasks() == -1)
  {
    setError("spawnTasks() failed\n");
    prtErrorMsg();
    return(-1);
  }

  // Initialize button lights
  unsigned devices = _params->getPanTilts();
  IviewDevice camera = _params->getCamera();
  PilotTrayButtonState portCameraState = PilotTrayButtonOff;
  PilotTrayButtonState stbdCameraState = PilotTrayButtonOff;

  if (camera == IviewPortCamera)
  {
    portCameraState = PilotTrayButtonOn;
  }
  else if (camera == IviewStbdCamera)
  {
    stbdCameraState = PilotTrayButtonOn;
  }

  // Process Events
  FOREVER
  {
    PilotTrayButton button;

    _uiObject->readEvent(&button);

    // Process button press
    processButton(button);

  }//FOREVER

  delete _params;
  return(OK);
}

void PilotTrayApp::processButton(PilotTrayButton button)
{
  MBool debug = FALSE;
  const int SetDeviceTimeout = 2;

  //dprintf(stderr,"Event Received, button=%d\n", button);

  _iviewControlPT->getDevices(&_deviceMask, &_camera);
  _iviewControlZFI->getDevices(&_deviceMaskZFI, &_cameraZFI);

  switch (button)
  {
    case PORT_LIGHT_BUTTON:
    if (_deviceMask & IviewPortLight)
      _deviceMask &= ~IviewPortLight;
    else
      _deviceMask |= IviewPortLight;

    _iviewControlPT->setDevices(_deviceMask, _camera, SetDeviceTimeout);

    break;

    case PORT_CAMERA_BUTTON:
    if (_deviceMask & IviewPortCamera)
      _deviceMask &= ~IviewPortCamera;
    else
      _deviceMask |= IviewPortCamera;

    _iviewControlPT->setDevices(_deviceMask, _camera, SetDeviceTimeout);

    break;

    case STBD_LIGHT_BUTTON:
    if (_deviceMask & IviewStbdLight)
      _deviceMask &= ~IviewStbdLight;
    else
      _deviceMask |= IviewStbdLight;

    _iviewControlPT->setDevices(_deviceMask, _camera, SetDeviceTimeout);

    break;

    case STBD_CAMERA_BUTTON:
    if (_deviceMask & IviewStbdCamera)
      _deviceMask &= ~IviewStbdCamera;
    else
      _deviceMask |= IviewStbdCamera;

    _iviewControlPT->setDevices(_deviceMask, _camera, SetDeviceTimeout);

    break;

#if 0 // No Camera Select implemented Yet
    case PORT_CAMERA_SELECT_BUTTON:
    if (_deviceMaskZFI == IviewPortCamera)
    { 
      // Already selected: de-select camera
      _iviewControlZFI->setDevices(IviewNoDevice, _cameraZFI, SetDeviceTimeout);
    }
    else
    {
       _iviewControlZFI->setDevices(IviewPortCamera, _cameraZFI, SetDeviceTimeout);
    }
    break;

    case STBD_CAMERA_SELECT_BUTTON:
    if (_deviceMaskZFI == IviewStbdCamera)
    {
      // Already selected: de-select camera
      _iviewControlZFI->setDevices(IviewNoDevice, _cameraZFI, SetDeviceTimeout);
    }
    else
    {
       _iviewControlZFI->setDevices(IviewStbdCamera, _cameraZFI, SetDeviceTimeout);
    }
    break;
#endif

    default:
    break;
  }
}

void PilotTrayApp::runInputTask(PilotTrayApp *app)
{
  app->_inTask = new PilotTrayInTask(app->_params, app->_uiObject,
                                     app->_iviewControlPT, 
                                     app->_iviewControlZFI);

  if (app->_inTask->error())
  {
    app->setError("PilotTrayInTask constructor error");
    return;
  }

  app->_inTask->run();
}


void PilotTrayApp::runOutputTask(PilotTrayApp *app)
{
  app->_outTask = new PilotTrayOutTask(app->_params, app->_uiObject,
                                       app->_iviewControlPT, 
                                       app->_iviewControlZFI);

  if (app->_outTask->error())
  {
    app->setError("PilotTrayOutTask constructor error");
    return;
  }

  app->_outTask->run();
}

int PilotTrayApp::spawnTasks()
{
    Nat32 inTaskNo, outTaskNo;

                                     /* Initialize lap Box  Data Out Task    */
    if (outTaskNo = taskSpawn("pilotTrayOut", BELLY_PACK_OUT_PRIORITY,
                               VX_FP_TASK, PILOT_TRAY_STACKSIZE,
                               (FUNCPTR) &PilotTrayApp::runOutputTask,
                               (int)this, 0, 0, 0, 0, 0, 0, 0, 0, 0) == ERROR)
    {
        taskDelete(outTaskNo);
        delete _params;
        delete _uiObject;
        fprintf(stderr,"Pilot Tray: Error spawning data output task\n");
        return(ERROR);
    } /* if */

                                    /* Initialize Pilot Tray Data In Task    */
    if (inTaskNo = taskSpawn( "pilotTrayIn", BELLY_PACK_IN_PRIORITY, VX_FP_TASK,
                   PILOT_TRAY_STACKSIZE, (FUNCPTR) &PilotTrayApp::runInputTask,
                   (int)this, 0, 0, 0, 0, 0, 0, 0, 0, 0) == ERROR)
    {
                            /* Delete Pilot Tray Out Task                */
        taskDelete(outTaskNo);
        taskDelete(inTaskNo);
        delete _params;
        delete _uiObject;
        fprintf(stderr, "Pilot Tray: Error spawning data input task\n");
        return(ERROR);
    } /* if */

  return 0;
}