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

#include "LapBoxApp.h"
#include "LapBoxInTask.h"
#include "LapBoxOutTask.h"
#include "lapBox.h"

#include "vxUtils.h"

#define dprintf if (debug) fprintf

LapBoxApp::LapBoxApp(Nat16 lapBoxSerialChan, const char *name, 
                     const char *dmPrefix)
{
  MBool debug = TRUE;

  _name = strdup(name);

  _uiObject = new LapBoxMicro(name,lapBoxSerialChan);

  // Create LapBoxParams object
  _params = new LapBoxTaskControl(name, 3, _uiObject, dmPrefix);  

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

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

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

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


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


LapBoxApp::~LapBoxApp()
{
  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 LapBoxApp::run()
{ 
  MBool reset;
  
  if (spawnTasks() == -1)
  {
    setError("spawnTasks() failed\n");
    prtErrorMsg();
    return(-1);
  }

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

  LapBoxButtonState portCameraState = LapBoxButtonOff;
  LapBoxButtonState stbdCameraState = LapBoxButtonOff;

  if (camera == IviewPortCamera)
  {
    portCameraState = LapBoxButtonOn;
  }
  else if (camera == IviewStbdCamera)
  {
    stbdCameraState = LapBoxButtonOn;
  }
  
  _uiObject->writeSettings(PORT_CAMERA_SELECT_BUTTON,
			   portCameraState, FALSE); 

  _uiObject->writeSettings(STBD_CAMERA_SELECT_BUTTON,
			   stbdCameraState, FALSE); 

  if (_params->getEnabled())     
    _uiObject->writeSettings(ENABLE_BUTTON, LapBoxButtonOn, FALSE); 
  else
    _uiObject->writeSettings(ENABLE_BUTTON, LapBoxButtonOff, FALSE); 

  // Process SRQs
  FOREVER
  {
    LapBoxButton button;

    if (_params->microResetCount() != 0)

    reset = _uiObject->readEvent(&button);
    
    if (!reset)
    {
      // Process button press
      processButton(button);
    }
    else
    {
      // Handle reset event
      _params->countReset();
      if(_params->microResetOccurred()==OK)
      {
	 //printf("SRQ task, before wakeup(): _params=X%x\n", _params);
         _params->wakeupTask(OUT_TASK);
	 //printf("SRQ task, after wakeup(): _params=X%x\n", _params);
     }
      else
         _params->setMicroLinkState(LINK_DOWN);
      
    }
  }//FOREVER
  taskDelete(_params->getTaskId(OUT_TASK));
  taskDelete(_params->getTaskId(IN_TASK));

  delete _params;
  return(OK);
}

void LapBoxApp::processButton(LapBoxButton button)
{
  MBool debug = TRUE;
  const int SetDeviceTimeout = 2;
  IviewSensitivity sensitivity;

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

  if (button == ENABLE_BUTTON)
  {
    //dprintf(stderr, "enabled status: %d\n", _params->getEnabled());
    _params->setEnabled(_params->getEnabled() ? FALSE : TRUE);     
                  
    _uiObject->writeSettings(ENABLE_BUTTON,
			     _params->getEnabled() ? LapBoxButtonOn 
			     : LapBoxButtonOff, FALSE);

    //dprintf(stderr, "enabled status now: %d\n", _params->getEnabled());
  }

  if (!_params->getEnabled())
    return;
  
  _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;

    case PANORAMIC_BUTTON:
         _iviewControlPT->setPanoramic(!_params->panoramic);
    break;

    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;

    case AUTO_IRIS_BUTTON:
    {
      if(_params->getCamera()!=IviewNoDevice)
         _params->setAutoIris(_params->getAutoIris() ? FALSE : TRUE);     
    }
    break;

    case SENSITIVITY_BUTTON:
    { 
      if(_params->getSensitivity() == IviewDefaultSensitivity)
         _params->setSensitivity(IviewHighSensitivity);
      
      else if(_params->getSensitivity() == IviewHighSensitivity)
         _params->setSensitivity(IviewAutoSensitivity);
      
      else
         _params->setSensitivity(IviewDefaultSensitivity);
     
      sensitivity = _params->getSensitivity();
      _iviewControlPT->setSensitivity(sensitivity);       
    }
    break;
    
    case MEMORY_BUTTON:
    {
      _params->setMemory(_params->getMemory() ? FALSE : TRUE);     
    }
    break;

    default:
    break;
  }
}


/*****************************************************************************/
/* Function    : scaleJoystick                                               */
/* Purpose     : Scale raw joystick values into required range & add deadband*/ 
/* Inputs      : Raw Joystick value, zero offset, desired full scale value,  */
/*               observed fullscale +ve and -ve values and deadband range    */
/* Outputs     : Returns scaled, adjusted joystick value                     */
/*****************************************************************************/
    MLocal Int16
scaleJoystick( Int16 joystickRaw, Int16 offset, Flt32 fullScaleActual,
	 Flt32 fullScalePosObs, Flt32 fullScaleNegObs, Flt32 deadband )
{
    Flt32 joystickScaled;		/* Adjusted joystick value to return */

    joystickRaw -= offset;		/* Subtract measured zero offset     */

    if (joystickRaw >= 0)		/* Check quadrant of raw value       */
    {					/* Scale raw value into desired range*/
        joystickScaled = joystickRaw * fullScaleActual / 
                               (fullScalePosObs - offset);
				  	/* Limit scaled value to max limit   */
        if (joystickScaled > fullScaleActual)
            joystickScaled = fullScaleActual;
					/* Apply deadband range to joystick  */
	if (joystickScaled < deadband)
	  joystickScaled = 0.0;

    } /* if */

    else
    {					/* Scale raw value into desired range*/
        joystickScaled = joystickRaw * -fullScaleActual / 
                                 (fullScaleNegObs - offset);
				  	/* Limit scaled value to max limit   */
        if (joystickScaled < -fullScaleActual)
            joystickScaled = -fullScaleActual;

					/* Apply deadband range to joystick  */
	if (joystickScaled > -deadband)
	  joystickScaled = 0.0;

    } /* else */
					/* Return scaled, adjusted value     */
    return( (Int16) joystickScaled);
} /* scaleJoystick() */


void LapBoxApp::updateAxis(Int16 *joystickX,
			  Int16 *joystickY,
			  Int16 *zoom,
			  Int16 *focus,
			  Int16 *iris)

{
    Int16 X, Y, Zoom, Focus, Iris;

    X = *joystickX;
    Y = *joystickY;
    Zoom = *zoom;
    Focus = *focus;
    Iris = *iris;

    *joystickX = scaleJoystick(X, JOYSTICK_X_OFFSET,
                              JOYSTICK_X_FULL_SCALE, JOYSTICK_X_POS_FSV,
                              JOYSTICK_X_NEG_FSV, JOYSTICK_X_DEADBAND);

    *joystickY = scaleJoystick(Y, JOYSTICK_Y_OFFSET,
                              JOYSTICK_Y_FULL_SCALE, JOYSTICK_Y_POS_FSV,
                              JOYSTICK_Y_NEG_FSV, JOYSTICK_Y_DEADBAND);

    *zoom = scaleJoystick(Zoom, ZOOM_OFFSET,
                              ZOOM_FULL_SCALE, ZOOM_POS_FSV,
                              ZOOM_NEG_FSV, ZOOM_DEADBAND);

    *focus = scaleJoystick(Focus, FOCUS_OFFSET,
                              FOCUS_FULL_SCALE, FOCUS_POS_FSV,
                              FOCUS_NEG_FSV, FOCUS_DEADBAND);

    *iris = scaleJoystick(Iris, IRIS_OFFSET,
                              IRIS_FULL_SCALE, IRIS_POS_FSV,
                              IRIS_NEG_FSV, IRIS_DEADBAND);
}
 


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

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


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

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


int LapBoxApp::spawnTasks()
{
   Nat32 taskNo;
                                     /* Initialize lap Box  Data Out Task    */
    if (taskNo = taskSpawn("lapBoxOut", BELLY_PACK_OUT_PRIORITY, VX_FP_TASK,
                   LAP_BOX_STACKSIZE,  (FUNCPTR) &LapBoxApp::runOutputTask, 
	    (int)this, 0, 0, 0, 0, 0, 0, 0, 0, 0) != ERROR)
   
    _params->setTaskId(OUT_TASK, taskNo);
    
    else
    {
                                    /* Close serial communication channel    */
        delete _params;
        delete _uiObject;
        fprintf(stderr,"Lap Box: Error spawning data output task\n");
        return(ERROR);
    } /* if */

                                    /* Initialize Lap Box Data In Task        */
    if (taskNo = taskSpawn( "lapBoxIn", BELLY_PACK_IN_PRIORITY, VX_FP_TASK, 
                   LAP_BOX_STACKSIZE, (FUNCPTR) &LapBoxApp::runInputTask, 
                   (int)this, 0, 0, 0, 0, 0, 0, 0, 0, 0) != ERROR)
    
    _params->setTaskId(IN_TASK, taskNo);
    
    else
    {				    /* Delete Lap Box Out Task                */
	delete _params;
        delete _uiObject;
        fprintf(stderr, "Lap Box: Error spawning data input task\n");
        return(ERROR);
    } /* if */

  return 0;
}

//Example of how to spawn task in unix
#if 0
int LapBoxApp::spawnTasks()
{
  pid_t pid;

  char *processName;

  for (int i = 0; i < 2; i++)
  {
    if (i == 0)
      processName = "LapBoxIn";
    else if (i == 1)
      processName = "LapBoxOut";

    char buf[100];
    
    switch (pid = fork())
    {
      case -1:
      sprintf(buf, "LapBoxApp::spawnTasks(), fork() #%d", i);
      perror(buf);
      return -1;
      break;
    
      case 0:
      // In child process
      execlp(processName, processName, _name, NULL);
    
      // If we get here, execlp() failed
      sprintf(buf, "LapBoxApp::spawnTasks(), execlp() of %s", processName);
      perror(buf);
      return -1;
    
      default:
      break;
    }
  }
  
  return 0;
}
#endif