/**************************************************************************
 Copyright 1998 MBARI                                                    
***************************************************************************
Summary  : Integrated viewing routines                                  
Filename : ControlSource.C                                              
Author   : Michael B. Matthews                                          
Project  : Tiburon ROV                                                  
Version  : 4.0                                                          
Created  : 12.5.98                                                      
Modified :                                                            
Archived :    
Notes    :                                                         
***************************************************************************/

#define __PROTOTYPE_5_0			// logMsg variable arg list

#include <vxWorks.h>            	      
#include <sysLib.h>   	         	      
#include <stdioLib.h>           	     
#include <stdlib.h>  	         	      
#include <math.h>              		
#include <semLib.h>            		         
#include <logLib.h>            		         
#include <wdLib.h>             		    
#include <systime.h>           		   
#include <strLib.h>			             
#include <stdlib.h>			
#include <fppLib.h>             	   
#include <errno.h>        	
#include <tickLib.h>        	

extern "C" 
{
#include <trig.h>      		           
#include <mbariTypes.h>  	            
#include <sensorDcon.h>		
#include <vmeIbc.h>		      
#include <sensorsDm.h>	                                    
#include <datamgr.h>   		    
#include <dm_errno.h>   	   
#include <usrTime.h>
#include <tiburon.h>			
#include <camera.h>			
#include <panTilt.h>			
#include <pantilt.h>	
#include <rovPriority.h>	
};

#include "Iview.h"
#include "Misc.h"		// misc functions
#include "String.h"		// homebrew string classes
#include "DataManager.h"	// data manager classes
#include "DmGroup.h"		// data manager classes
#include "uFilter.h"		// filter and model parameters		
#include "Buffer.h"		// homebrew buffer class
#include "Linear_System.h"	// linear system and Kalman filter classes
#include "Calibration.h"	// calibration table class
#include "Axis.h"		// device axis class			
#include "Device.h"		// device classes		
#include "ControlSource.h"	// control source classes
#include "SetPoint.h"				
#include "iview.h"				


Extern IView* iView;
Extern Void Get_Time(Flt64*, Flt64*);


/******************************************************************************
Function : ControlSourceInterface::ControlSourceInterface            
Purpose  : Reads and processes ServerControlSource requests
Inputs   : None                                                                
Outputs  : None                                                                
Notes    : 								      
******************************************************************************/

ControlSourceInterface::ControlSourceInterface(Int i, SEM_ID sem) 
: ServerControlSource(i,sem)
{
  controlSource = NULL; 

  // Fill in IviewMessage to be sent to all clients
  IviewMessage msg;
  msg.id = 0;
  msg.panoramic = FALSE;
  msg.focusCaptured = FALSE;
  msg.serverStarting = TRUE;
  msg.deviceMask = IviewNoDevice;
  writeMessage(&msg);
}



/******************************************************************************
Function : ControlSourceInterface::deactivate     
Purpose  :
Inputs   : None                                                                
Outputs  : None                                                                
Notes    : 								      
******************************************************************************/

Void ControlSourceInterface::deactivate()
{
  ServerControlSource::deactivate();
  Remove();
}



/******************************************************************************
Function : ControlSourceInterface::activate     
Purpose  :
Inputs   : None                                                                
Outputs  : None                                                                
Notes    : 								      
******************************************************************************/

Void ControlSourceInterface::activate(AperiodicRequest* request)
{
  ServerControlSource::activate(request);

  controlName = request->controlName;
 
  // test for camera control source
  if ( (request->axisMask & IviewFocus)  ||
       (request->axisMask & IviewIris)   ||
       (request->axisMask & IviewZoom)   &&
      !(request->axisMask & IviewPan)    && 
      !(request->axisMask & IviewTilt) ) 
    controlSource = new CameraControlSource(this, request);
 
  // test for PT control source start request
  if (!(request->axisMask & IviewFocus)  &&
      !(request->axisMask & IviewIris)   &&
      !(request->axisMask & IviewZoom)   ||
       (request->axisMask & IviewPan)    || 
       (request->axisMask & IviewTilt) ) 
    controlSource = new PTControlSource(this, request);
}



/******************************************************************************
Function : ControlSourceInterface::processAperiodicRequest     
Purpose  : Reads and processes aperiodic requests
Inputs   : None                                                                
Outputs  : None                                                                
Notes    : 								      
******************************************************************************/

Void ControlSourceInterface::processAperiodicRequest(AperiodicRequest* request, 
						     RequestStatus* status)
{
  // if control source is undefined then create new one
  controlSource->deviceRequest(request);

  // update status
  status->requestNo = request->requestNo;
  status->code = IviewSuccess;
  status->deviceMask = request->deviceMask;
  status->refDevice = request->refDevice;
}



/******************************************************************************
Function : ControlSourceInterface::processPeriodicRequest                        
Purpose  : Processes periodic requests
Inputs   : None                                                                
Outputs  : None                                                                
Notes    : 								      
******************************************************************************/

Void ControlSourceInterface::processPeriodicRequest(PeriodicRequest* request, 
						    RequestStatus* status) 
{
  if (isNewRequest(request))
  {
    status->requestNo = request->requestNo;
    status->code = IviewSuccess;
    status->deviceMask = controlSource->deviceMap;		
    status->refDevice = controlSource->refDevice;	
  }

  controlSource->periodicUpdate(request);
}



/*****************************************************************************
Function : ControlSourceInterface::Remove                            
Purpose  : Remove the control source
Inputs   : None
Outputs  : Returns iViewError if problem with removal.
Notes    : Removes the control source from the attached setpoint before
         : deleting the control source and setting the pointer to NULL.
******************************************************************************/

Void ControlSourceInterface::Remove()
{
  if (controlSource)
  {
    delete controlSource;
    controlSource = NULL;
  }
}



/*****************************************************************************
Function : ControlSource::ControlSource                                   
Purpose  : 
Inputs   : request - request struct from Dm control source
Outputs  : Error status                                                      
Notes    : 								      
******************************************************************************/

ControlSource::ControlSource(ControlSourceInterface* cs)
{
  controlSourceInterface = cs;
  deviceMap = 0;
  refDevice = IviewNoDevice;
  msg.id = 0;
}
  


/******************************************************************************
Function : PTControlSource::PTControlSource                                    
Purpose  : PTControlSource derived class contructor                            
Inputs   : None                                                                
Outputs  : None                                                                 
Notes    : Constructs PT control source and processes device request.
******************************************************************************/

PTControlSource::PTControlSource(ControlSourceInterface* cs, 
				 AperiodicRequest* request)
: ControlSource(cs)
{
  if ( (request->axisMask & IviewPanRate) || 
       (request->axisMask & IviewTiltRate) )
    mode = RATE_MODE;
  else
    mode = POSITION_MODE;

  panoramicMode = FALSE;
  uPan = uTilt = 0;
  setPoint = NULL;
}



/******************************************************************************
Function : PTControlSource::~PTControlSource                                    
Purpose  :                           
Inputs   : None                                                                
Outputs  : None                                                                 
Notes    : 
******************************************************************************/

PTControlSource::~PTControlSource()
{
  Remove(); 
}



/*****************************************************************************
Function : PTControlSource::deviceSetPoint                                     
Purpose  : Returns pointer to device's setpoint
Inputs   : device - device of interest
Outputs  : pointer to setpoint                                            
Notes    : 								      
******************************************************************************/

SetPoint* PTControlSource::deviceSetPoint(IviewDevice device)
{
  switch (device)
  {
  case IviewPortCamera:
    return iView->portCtlr->camera->setPoint;
    break;
  case IviewStbdCamera:
    return iView->stbdCtlr->camera->setPoint;
    break;
  case IviewPortLight:
    return iView->portCtlr->light->setPoint;
    break;
  case IviewStbdLight:
    return iView->stbdCtlr->light->setPoint;
    break;
  default:
    return (SetPoint*) NULL;
    break;
  }
}



/*****************************************************************************
Function : PTControlSource::deviceRequest                                     
Purpose  : Process device request to change device mapping
Inputs   : request - request struct from Dm control source
Outputs  : Error status                                              
Notes    : 								      
******************************************************************************/

Int PTControlSource::deviceRequest(AperiodicRequest* request)
{
  if (iView->print)
  {
      fprintf(stderr, "%s\n", controlSourceInterface->controlName.str);
      fprintf(stderr, "device map     0x%x\n", request->deviceMask);
      fprintf(stderr, "panoramic mode %d\n", request->panoramic);
  }

  // update panoramic mode
  if (panoramicMode != request->panoramic)
  {
    panoramicMode = request->panoramic;
    if (panoramicMode)
      setPanoramic(IviewPortCamera | IviewStbdCamera);
    else
      resetPanoramic(IviewPortCamera | IviewStbdCamera);

    Update();
    return IviewSuccess;
  }

  // update sensitivity 
  sensitivity = request->sensitivity;

  // remove this control source from old setpoint
  Remove();

  // check for singly mapped lights and no cameras
  if ( (request->deviceMask & IviewPortLight)	 &&
      !(request->deviceMask & IviewStbdLight)	 &&
      !(request->deviceMask & IviewPortCamera)	 &&
      !(request->deviceMask & IviewStbdCamera))
  {
    setPoint = deviceSetPoint(IviewPortLight);
    setPoint->Reference(IviewPortLight); 
  }

  if (!(request->deviceMask & IviewPortLight)	 &&
       (request->deviceMask & IviewStbdLight)	 &&
      !(request->deviceMask & IviewPortCamera)	 &&
      !(request->deviceMask & IviewStbdCamera))
  {
    setPoint = deviceSetPoint(IviewStbdLight);
    setPoint->Reference(IviewStbdLight); 
  }

 
  // check for singly mapped camera regardless of lights
  if ( (request->deviceMask & IviewPortCamera)	 &&
      !(request->deviceMask & IviewStbdCamera) )
  {
    setPoint = deviceSetPoint(IviewPortCamera);
    setPoint->Reference(IviewPortCamera); 
  }

  if (!(request->deviceMask & IviewPortCamera)	 &&
       (request->deviceMask & IviewStbdCamera) )
  {
    setPoint = deviceSetPoint(IviewStbdCamera);
    setPoint->Reference(IviewStbdCamera); 
  }


  // check for light pair only and set as reference the first light
  if ( (request->deviceMask & IviewPortLight)	 &&
       (request->deviceMask & IviewStbdLight)	 &&
      !(request->deviceMask & IviewPortCamera)	 &&
      !(request->deviceMask & IviewStbdCamera) )
{
    if ( (deviceMap & IviewPortLight)	&&
	!(deviceMap & IviewStbdLight) )
    {
      setPoint = deviceSetPoint(IviewPortLight);
      setPoint->Reference(IviewPortLight); 
    }

    if (!(deviceMap & IviewPortLight)	&&
	 (deviceMap & IviewStbdLight) )
    {
      setPoint = deviceSetPoint(IviewStbdLight);
      setPoint->Reference(IviewStbdLight); 
    }

    if ( (deviceMap & IviewPortLight)	&&
	 (deviceMap & IviewStbdLight) )
    {
      setPoint = deviceSetPoint(IviewPortLight);
      setPoint->Reference(IviewPortLight); 
    }
  }


  // check for camera pair and set as reference the first camera
  if ( (request->deviceMask & IviewPortCamera)	 &&
       (request->deviceMask & IviewStbdCamera) )
  {
    setPoint = deviceSetPoint(IviewPortCamera);
 
    if ( (deviceMap & IviewPortCamera)	&&
	!(deviceMap & IviewStbdCamera) )
      setPoint = deviceSetPoint(IviewPortCamera);

    if (!(deviceMap & IviewPortCamera)	&&
	 (deviceMap & IviewStbdCamera) )
      setPoint = deviceSetPoint(IviewStbdCamera);
  }

  // update device map
  deviceMap = request->deviceMask;
 
  if (setPoint)
  {
    // replicate the setpoint
    SetPoint* setPointNew = new SetPoint(setPoint);

    // remap this control source to new setpoint
    setPointNew->Remap(this);

    // connect this control source to new setpoint
    setPoint = setPointNew;
  }

  // inform control source of change of state
  Update();

  return IviewSuccess;
}



/*****************************************************************************
Function : PTControlSource::periodicUpdate                                       
Purpose  : Update pan-tilt control source                             
Inputs   : None                                                                
Outputs  : None                                                                
Notes    : Updates associated setpoint.
******************************************************************************/

Void PTControlSource::periodicUpdate(PeriodicRequest* request)
{
  if (request->pan != IviewNoChange)
    uPan = request->pan;
  if (request->tilt != IviewNoChange)
    uTilt = -request->tilt;
  if (setPoint) 
    setPoint->Update(uPan, uTilt, mode, sensitivity);
}



/*****************************************************************************
Function : PTControlSource::Remove                                       
Purpose  : Remove this control source from the connected setpoint
Inputs   : None                                                                
Outputs  : 
Notes    : 
******************************************************************************/

Void PTControlSource::Remove()
{
  if (setPoint)
  {
    setPoint->controlSource = NULL;
    setPoint = NULL;
  }
}



/*****************************************************************************
Function : PTControlSource::setPanoramic                                       
Purpose  : 
Inputs   : None                                                                
Outputs  : 
Notes    : 
******************************************************************************/

Void PTControlSource::setPanoramic(Word devices)
{
  if (devices & IviewStbdLight)
    iView->stbdCtlr->light->setPanoramic();

  if (devices & IviewPortLight)
    iView->portCtlr->light->setPanoramic();

  if (devices & IviewStbdCamera)
    iView->stbdCtlr->camera->setPanoramic();

  if (devices & IviewPortCamera)
    iView->portCtlr->camera->setPanoramic();
}



/*****************************************************************************
Function : PTControlSource::resetPanoramic                                       
Purpose  : 
Inputs   : None                                                                
Outputs  : 
Notes    : 
******************************************************************************/

Void PTControlSource::resetPanoramic(Word devices)
{
  if (devices & IviewStbdLight)
    iView->stbdCtlr->light->resetPanoramic();

  if (devices & IviewPortLight)
    iView->portCtlr->light->resetPanoramic();

  if (devices & IviewStbdCamera)
    iView->stbdCtlr->camera->resetPanoramic();

  if (devices & IviewPortCamera)
    iView->portCtlr->camera->resetPanoramic();
}



/*****************************************************************************
Function : PTControlSource::Update                                       
Purpose  : Send state message to client                                      
Inputs   : msg - reference to message struct
Outputs  : None                                                                
Notes    : 								      
******************************************************************************/

Void PTControlSource::Update()
{
  msg.id++;
  msg.panoramic = panoramicMode;
  msg.focusCaptured = FALSE;
  msg.serverStarting = FALSE;

  if (setPoint)
    msg.deviceMask = setPoint->deviceMap;
  else
    msg.deviceMask = 0;
  
  if (iView->print)
  {
      fprintf(stderr, "%s\n", controlSourceInterface->controlName.str);
      fprintf(stderr, "device map     0x%x\n", msg.deviceMask);
      fprintf(stderr, "panoramic mode %d\n", msg.panoramic);
  }

  controlSourceInterface->writeMessage(&msg);
}



/******************************************************************************
Function : CameraControlSource::CameraControlSource                           
Purpose  : CameraControlSource derived class constructor                    
Inputs   : None                                                                
Outputs  : None                                                                 
Notes    : 
******************************************************************************/

CameraControlSource::CameraControlSource(ControlSourceInterface* cs, 
					 AperiodicRequest* request)
: ControlSource(cs)
{
  axisMap = request->axisMask;
  uFocus = uIris = uZoom = 0;
  focusEnable = FALSE;
  camera = NULL;
}



/*****************************************************************************
Function : CameraControlSource::Remove                                       
Purpose  : Remove this control source from the attached camera
Inputs   : None                                                                
Outputs  : 
Notes    : 
******************************************************************************/

Void CameraControlSource::Remove()
{
  if (camera)
  {
    camera->focus->controlSource = NULL;
    camera = NULL;
  }
}



/*****************************************************************************
Function : CameraControlSource::deviceRequest                                  
Purpose  : Process device request to change device mapping
Inputs   : request - request struct from Dm control source
Outputs  : Error status                                                    
Notes    : 								      
******************************************************************************/

Int CameraControlSource::deviceRequest(AperiodicRequest* request)
{
  if (iView->print)
  {
      fprintf(stderr, "%s\n", controlSourceInterface->controlName.str);
      fprintf(stderr, "device map     0x%x\n", request->deviceMask);
  }

  deviceMap = request->deviceMask;

  if (!deviceMap) refDevice = IviewNoDevice;

  // remove this control source from assigned camera
  Remove();

  // assign new camera
  if (deviceMap & IviewStbdCamera)
  {
    camera = iView->stbdCtlr->camera;
    camera->focus->Remap(this);
    deviceMap = IviewStbdCamera;
    refDevice = IviewStbdCamera;
  }

  if (deviceMap & IviewPortCamera)
  {
    camera = iView->portCtlr->camera;
    camera->focus->Remap(this);
    deviceMap = IviewPortCamera;
    refDevice = IviewPortCamera;
  }

  focusEnable = FALSE;
  Update();

  return IviewSuccess;
}



/*****************************************************************************
Function : CameraControlSource::periodicUpdate                                       
Purpose  : Update camera control source                              
Inputs   : None                                                                
Outputs  : None                                                                
Notes    : Updates camera with control inputs.
******************************************************************************/

Void CameraControlSource::periodicUpdate(PeriodicRequest* request)
{
  if (!camera) return;

  // update focus rate control input
  if (request->focus != IviewNoChange)
  {
    uFocus = request->focus;
    if (axisMap & IviewFocusRate)
      camera->focus->Rate(uFocus);
    else
    {
      if (uFocus < 0) uFocus = 0;	
      if (updateFocusEnable()) Update();
      if (focusEnable) camera->focus->Position(uFocus);
    }    
  }

  // update zoom rate control input
  if (request->zoom != IviewNoChange)
  {
    uZoom = request->zoom;
    if (axisMap & IviewZoomRate)
      camera->zoom->Rate(uZoom);
    else
      camera->zoom->Position(uZoom);
  }
 
  // update iris rate control input
  if (request->iris != IviewNoChange)
  {
    uIris = request->iris;
    if (axisMap & IviewIrisRate)
      camera->iris->Rate(uIris);
    else
      camera->iris->Position(uIris);
  }
}



/*****************************************************************************
Function : CameraControlSource::Update                                      
Purpose  : Inform control source of change                               
Inputs   : None                                                                
Outputs  : None                                                                
Notes    : The device update flag is set; this will cause a message update
         : on the next message poll. The latest device map is also updated from
         : the assigned setpoint.
******************************************************************************/

Void CameraControlSource::Update()
{
  msg.id++;
  msg.panoramic = FALSE;
  msg.focusCaptured = focusEnable;
  msg.serverStarting = FALSE;
  msg.deviceMask = deviceMap & refDevice;
  
  if (iView->print)
  {
      fprintf(stderr, "%s\n", controlSourceInterface->controlName.str);
      fprintf(stderr, "device map     0x%x\n", msg.deviceMask);
      fprintf(stderr, "focus capture  %d\n", msg.focusCaptured);
  }

  controlSourceInterface->writeMessage(&msg);
}



/*****************************************************************************
Function : CameraControlSource::updateFocusEnable                           
Purpose  : Checks if focus is close as setpoint focus; enables accordingly   
Inputs   : camera - pointer to camera                                      
Outputs  : Focus enable status    
Notes	 : This routine is used when a new focus device request is received. 
         : The focus input value of this control source is compared with the 
         : focus value of the corresponding camera. If the two values are 
         : close, this control source is enabled and a focusCaptureOn message 
         : is sent. If the two values differ, then this control source is 
         : disabled and a focusCaptureOff message is sent. The user must move 
         : the focus slider on the lapbox to "capture" the correct focus value 
         : and enable this control source. 
******************************************************************************/

MBool CameraControlSource::updateFocusEnable()
{
  if (camera == NULL)
  {
    focusEnable = FALSE;
    return FALSE;
  }

  Flt32 d = Fabs(uFocus - camera->focus->focusPosition());

  if ( (d <= FOCUS_ENABLE_THRESHOLD) && !focusEnable )
  {	
    focusEnable = TRUE;
    return TRUE;
  }
 
  if ( (d > FOCUS_ENABLE_THRESHOLD + FOCUS_ENABLE_HYSTERESIS) && focusEnable )
  {
    focusEnable = FALSE;
    return TRUE;
  }

  return FALSE;
}