/*****************************************************************************
Copyright 1998 MBARI                                                      
******************************************************************************
Summary  : Integrated viewing routines
Filename : SetPoint.C
Author   : Michael B. Matthews                                                
Project  : New ROV                                                        
Version  : 3.8                                                          
Created  : 11.2.98
Modified : 23.3.98
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 SetPoint* setPointRegister[MAX_NUMBER_SETPOINTS];



/******************************************************************************
Function : SetPoint::SetPoint                                              
Purpose  : SetPoint class object constructor       	              
Inputs   : xi, yi, zi - initial setpoint coordinates
         : coord - coordinate frame of initial setpoint data
Outputs  : None		                                                      
*******************************************************************************/

Void SetPoint::Init(Flt32 xi, Flt32 yi, Flt32 zi, Int coord)
{
  // register this setpoint
  Register();

  // initialize variables
  deviceMap = 0;	
  Reference((Device*) NULL);
  xOrigin = yOrigin = zOrigin = 0.0;
  dx = dy = dz = 0;
  dPan = dTilt = dfd = 0;

  // initialize setpoint limits
  maxSetPointPan   = MAX_SETPOINT_PAN;	
  minSetPointPan   = MIN_SETPOINT_PAN;			
  maxSetPointTilt  = MAX_SETPOINT_TILT;		
  minSetPointTilt  = MIN_SETPOINT_TILT;		
  maxSetPointFocus = MAX_SETPOINT_FOCUS;		
  minSetPointFocus = MIN_SETPOINT_FOCUS;

  if (coord == RECTANGULAR)
  {
    x = xi;
    y = yi;
    z = zi;
    Rect2Polar();
    Limit();
  }
  else
  {
    fd   = xi;
    pan  = yi;
    tilt = zi;
    Limit();
    Polar2Rect();
  }

  // no initial control source
  controlSource = NULL;
}



/******************************************************************************
Function : SetPoint::SetPoint                                              
Purpose  : Replicates a setpoint and inserts in linked list 
Inputs   : sp - setpoint to be duplicated 
Outputs  : None		
Notes    : If the setpoint pointer is zero. then a default setpoint is created
*******************************************************************************/

SetPoint::SetPoint(SetPoint* setPoint)
{
  if (!setPoint)
    Init();
  else
  {
    bcopy((char*) setPoint, (char*) this, sizeof(SetPoint));
    Register();
  }
}



/******************************************************************************
Function: SetPoint::~SetPoint                                    
Purpose : SetPoint class destructor
Inputs  : None                                                                
Outputs : None	
Notes	: If there are control sources still assigned to this setpoint, which
        : would happen, for example, if a control source received a zero device
	: mask - the selected setpoint would be replicated with a zero device mask
	: and the control source would be attached to it - then the setpoint
	: pointers for those attached control sources are set to null and the
	: control source is flagged for updating. The update message then contains
	: a zero device mask.
*******************************************************************************/

SetPoint::~SetPoint()
{
  if (iView->print) fprintf(stderr, "Deleting setpoint[%d]  0x%x\n", index, this);
  if (controlSource) controlSource->setPoint = NULL;
  Unregister();
}



/******************************************************************************
Function : SetPoint assignment operator                                    
Purpose  :                                             	              
Inputs   :                                                                 
Outputs  : 		                                                      
*******************************************************************************/

SetPoint& SetPoint::operator=(const SetPoint& setPoint)
{
  bcopy((char*) &setPoint, (char*) this, sizeof(setPoint));
  Register();
  return *this;
}



/******************************************************************************
Function : SetPoint::Register                                              
Purpose  : Registers a setpoint address in global register
Inputs   : None
Outputs  : None		                                                      
*******************************************************************************/

Void SetPoint::Register()
{
  index = 0;
  while (setPointRegister[index])
  {
    index++;
    index = index % MAX_NUMBER_SETPOINTS;
    if (!index)
    {
      logMsg("Register overflow on setpoint 0x%x\n", this);
      exit(0);
    }
  }
  setPointRegister[index] = this;  
}



/******************************************************************************
Function : SetPoint::Unregister                                              
Purpose  : Unregisters a setpoint address in global register
Inputs   : None
Outputs  : None		                                                      
*******************************************************************************/

Void SetPoint::Unregister()
{
  setPointRegister[index] = NULL; 
}



/******************************************************************************
Function : SetPoint::Remap                                    
Purpose  : Remap control source and devices to a setpoint                  
Inputs   : controlSource - pointer to control source to be added
Outputs  : None
Notes    : Adds selected devices to the device map and remaps them to this
         : setpoint. Adds the new control source to this setpoint. 
*******************************************************************************/

STATUS SetPoint::Remap(PTControlSource* controlSrc)
{
  // update device map of this setpoint
  deviceMap = controlSrc->deviceMap;
  
  // add new control sources to this setpoint
  controlSource = controlSrc; 

  // remap all selected devices
  if (deviceMap & IviewStbdLight)
    iView->stbdCtlr->light->Remap(this);

  if (deviceMap & IviewPortLight)
    iView->portCtlr->light->Remap(this);

  if (deviceMap & IviewStbdCamera)
    iView->stbdCtlr->camera->Remap(this);

  if (deviceMap & IviewPortCamera)
    iView->portCtlr->camera->Remap(this);

  return OK;
}



/******************************************************************************
Function : SetPoint::Add                                    
Purpose  : Add a device to a setpoint                         	              
Inputs   : device - devices to be added
Outputs  : None
Notes    : Adds the selected devices to the device map and informs the 
         : associated control source of a change.
*******************************************************************************/

STATUS SetPoint::Add(Word device)
{
  deviceMap |= device; 
  return OK; 
}



/******************************************************************************
Function : SetPoint::Remove                                    
Purpose  : Remove a device from setpoint                       	              
Inputs   : device - devices to be removed
Outputs  : None
Notes    : Removes the selected devices from device map and informs control 
         : sources of a change.
*******************************************************************************/

STATUS SetPoint::Remove(Word device)
{
  // mask off selected devices from this setpoint
  deviceMap &= ~device; 

  // inform control source that its device map has changed
  if (controlSource) controlSource->Update();

  // if reference device removed, then determine out new reference
  if ( ((device == IviewPortLight)  && (refDevice != iView->portCtlr->light))	||
       ((device == IviewPortCamera) && (refDevice != iView->portCtlr->camera))	||	
       ((device == IviewStbdLight)  && (refDevice != iView->stbdCtlr->light))	||
       ((device == IviewStbdCamera) && (refDevice != iView->stbdCtlr->camera)) ) 
    return OK;
   

  // check for singly mapped lights and no cameras
  if ( (deviceMap & IviewPortLight)	 &&
      !(deviceMap & IviewStbdLight)	 &&
      !(deviceMap & IviewPortCamera)	 &&
      !(deviceMap & IviewStbdCamera))
    Reference(IviewPortLight); 

  if (!(deviceMap & IviewPortLight)	 &&
       (deviceMap & IviewStbdLight)	 &&
      !(deviceMap & IviewPortCamera)	 &&
      !(deviceMap & IviewStbdCamera))
    Reference(IviewStbdLight); 

 
  // check for singly mapped camera regardless of lights
  if ( (deviceMap & IviewPortCamera)	 &&
      !(deviceMap & IviewStbdCamera) )
    Reference(IviewPortCamera); 

  if (!(deviceMap & IviewPortCamera)	 &&
       (deviceMap & IviewStbdCamera) )
    Reference(IviewStbdCamera); 


  // check for light pair only
  if ( (deviceMap & IviewPortLight)	 &&
       (deviceMap & IviewStbdLight)	 &&
      !(deviceMap & IviewPortCamera)	 &&
      !(deviceMap & IviewStbdCamera) )
    Reference(IviewPortLight); 

  return OK; 
}



/******************************************************************************
Function : SetPoint::Reference                                              
Purpose  : Assign new reference to setpoint and recompute reference coordinates
Inputs   : reference - device flag 
Outputs  : None
Notes    : 
*******************************************************************************/

Void SetPoint::Reference(Word reference)
{
  if (reference == IviewNoDevice)
    Reference((Device*) NULL);
  if (reference & IviewPortCamera)
    Reference(iView->portCtlr->camera);
  if (reference & IviewPortLight)
    Reference(iView->portCtlr->light);
  if (reference & IviewStbdCamera)
    Reference(iView->stbdCtlr->camera);
  if (reference & IviewStbdLight)
    Reference(iView->stbdCtlr->light);
}



/******************************************************************************
Function : SetPoint::Reference                                              
Purpose  : Assign new reference to setpoint and recompute reference coordinates
Inputs   : reference - device pointer
Outputs  : None
Notes    : 
*******************************************************************************/

Void SetPoint::Reference(Device* reference)
{
  refDevice = reference;
  Rect2Polar();
}



/******************************************************************************
Function : SetPoint::Set                                              
Purpose  : Initializes setpoint location        	              
Inputs   : (xi,yi,zi) - coordinates (recatangular or polar depending on coord)
Outputs  : None
Notes    : Recursively sets all setpoint in linked list if setAll is true
*******************************************************************************/

Void SetPoint::Set(Flt32 xi, Flt32 yi, Flt32 zi, Int coord)
{
  if (coord == RECTANGULAR)
  {
    x = xi; dx = 0;
    y = yi; dy = 0;
    z = zi; dz = 0;
    Rect2Polar();
    Limit();
  }
  else
  {
    fd   = xi; dfd   = 0; 
    pan  = yi; dPan  = 0;
    tilt = zi; dTilt = 0; 
    Limit();
    Polar2Rect();
  }
}



/******************************************************************************
Function : SetPoint::Update                                                
Purpose  : Moves setpoint in reference spherical coordinate frame with control 
         : inputs according to linear 1st-order model. Converts back to fiducial
         : rectangular coordinate frame for global use. If the linked setpoint
	 : has no mapped devices, i.e., device map is zero, then it is deleted
	 : and the link is bridged to next setpoint.
Inputs   : None                                                                
Outputs  : None
Notes	 : The control inputs from the control sources are normalized to [-1,1].
         : Focus control input is in the range [0,1].
*******************************************************************************/

STATUS SetPoint::Update(Flt32 uPan, Flt32 uTilt, Int mode, IviewSensitivity s)
{
  Flt32 u = 0;
  Flt32 scale = 1.0;

  // if both controllers are down, don't change setpoints
  if (iView->CtlrStatus())
  {
    if (mode == RATE_MODE)
    {
      // select scaling
      switch (s)
      {
      case IviewDefaultSensitivity:
	scale = SETPOINT_DEFAULT_SENSITIVITY;
	break;
      case IviewLowSensitivity:
	scale = SETPOINT_LOW_SENSITIVITY;
	break;
      case IviewMediumSensitivity:
	scale = SETPOINT_MEDIUM_SENSITIVITY;
	break;
      case IviewHighSensitivity:
	scale = SETPOINT_HIGH_SENSITIVITY;
	break;
      case IviewAutoSensitivity:
	scale = SETPOINT_DEFAULT_SENSITIVITY;
	if (refDevice == iView->stbdCtlr->camera)
	  scale = iView->stbdCtlr->camera->zoom->Position();
	if (refDevice == iView->portCtlr->camera)
	  scale = iView->portCtlr->camera->zoom->Position();
	scale = SETPOINT_LOW_SENSITIVITY 
	  - (SETPOINT_LOW_SENSITIVITY - SETPOINT_HIGH_SENSITIVITY) * scale;
	break;
      }

      // check for limit switches and update setpoint pan direction
      if ( (!refDevice->pan->positiveLimitSwitch() && (uPan >= 0))	||
	   (!refDevice->pan->negativeLimitSwitch() && (uPan < 0)) )
	u = MAX_SETPOINT_RATE_PAN * scale * uPan;

      pan  = A11 * pan + A12 * dPan + B1 * u;
      dPan = A21 * pan + A22 * dPan + B2 * u;

      if ( (!refDevice->tilt->positiveLimitSwitch() && (uTilt >= 0))	||
	   (!refDevice->tilt->negativeLimitSwitch() && (uTilt < 0)) )
	u = MAX_SETPOINT_RATE_TILT * scale * uTilt;

      tilt  = A11 * tilt + A12 * dTilt + B1 * u;
      dTilt = A21 * tilt + A22 * dTilt + B2 * u;
    }
    else
    {
      pan  = MAX_SETPOINT_PAN * uPan;
      tilt = MAX_SETPOINT_TILT * uTilt;
    }
  }


  // update focal distance with open-loop focus value
  if (refDevice == iView->stbdCtlr->camera)
    fd = iView->stbdCtlr->camera->focus->focalDistance();
  if (refDevice == iView->portCtlr->camera)
    fd = iView->portCtlr->camera->focus->focalDistance();


  // clip setpoint and convert to fiducial rectangular coordinate frame
  Limit();
  Polar2Rect();

  return OK;
}



/******************************************************************************
Function : SetPoint::Focus                          
Purpose  : Computes focus setting from setpoint distance to reference origin
Inputs   : None                                            
Outputs  : Returns focus value
Notes	 :   
******************************************************************************/

Flt32 SetPoint::Focus()
{ 
  if (refDevice == iView->stbdCtlr->camera)
    return iView->stbdCtlr->camera->focus->Focal_Distance_to_Focus(fd);
  else
    return iView->portCtlr->camera->focus->Focal_Distance_to_Focus(fd);
}



/******************************************************************************
Function : SetPoint::Distance                         
Purpose  : Computes setpoint distance from reference origin using focus setting   
Inputs   : focus - focus setting                                            
Outputs  : returns focal length
Notes	 : Uses camera focal length if reference device is camera, else 100.0.
******************************************************************************/

Flt32 SetPoint::Distance(Flt32 focus)
{ 
  if (refDevice == iView->stbdCtlr->camera)
    return iView->stbdCtlr->camera->focus->Focus_to_Focal_Distance(focus);
  else
    return iView->portCtlr->camera->focus->Focus_to_Focal_Distance(focus);

}



/******************************************************************************
Function : SetPoint::Limit                                       
Purpose  : Clip polar coordinates
Inputs   : None                                                                
Outputs  : None		                                                      
*******************************************************************************/

Void SetPoint::Limit()
{
  if (fd   > maxSetPointFocus) fd   = maxSetPointFocus;
  if (fd   < minSetPointFocus) fd   = minSetPointFocus;
  if (pan  > maxSetPointPan)   pan  = maxSetPointPan;
  if (pan  < minSetPointPan)   pan  = minSetPointPan;
  if (tilt > maxSetPointTilt)  tilt = maxSetPointTilt;
  if (tilt < minSetPointTilt)  tilt = minSetPointTilt;
}



/******************************************************************************
Function : SetPoint::Polar2Rect                                       
Purpose  : Converts polar coordinates in reference frame to rectangular
         : coordinates in fiducial frame.                    	              
Inputs   : None                                                                
Outputs  : None	
Notes    : Reference frame is that corresponding to reference device.             
*******************************************************************************/

Void SetPoint::Polar2Rect()
{
  Flt32 cosPan  = cos(pan);
  Flt32 sinPan  = sin(pan);
  Flt32 cosTilt = cos(tilt);
  Flt32 sinTilt = sin(tilt);
  Flt32 cosPanxcosTilt = cosPan * cosTilt;
  Flt32 sinPanxsinTilt = sinPan * sinTilt;
  Flt32 cosPanxsinTilt = cosPan * sinTilt;
  Flt32 sinPanxcosTilt = sinPan * cosTilt;
  Flt32 l = 0;

  if (refDevice)
  {
    l =  refDevice->l;
    x =  fd * cosPanxcosTilt + l * cosPanxsinTilt + refDevice->xOrigin;
    y =  fd * sinPanxcosTilt + l * sinPanxsinTilt + refDevice->yOrigin;
    z = -fd * sinTilt        + l * cosTilt        + refDevice->zOrigin;
  }
  else 
  {
    x =  fd * cosPanxcosTilt + xOrigin;
    y =  fd * sinPanxcosTilt + yOrigin;
    z = -fd * sinTilt        + zOrigin;
  }
  
  Flt32 fdxdPan  = fd * dPan;
  Flt32 fdxdTilt = fd * dTilt;
  Flt32 lxdPan   =  l * dPan;
  Flt32 lxdTilt  =  l * dTilt;

  // compute time derivative of fiducial rectangular coordinate frame
  dx =   dfd      * cosPanxcosTilt 
       - fdxdPan  * sinPanxcosTilt
       - fdxdTilt * cosPanxsinTilt
       - lxdPan   * sinPanxsinTilt
       + lxdTilt  * cosPanxcosTilt; 

  dy =   dfd      * sinPanxcosTilt 
       + fdxdPan  * cosPanxcosTilt 
       - fdxdTilt * sinPanxsinTilt
       + lxdPan   * cosPanxsinTilt
       + lxdTilt  * sinPanxcosTilt; 
  
  dz =  -dfd      * sinTilt 
       - fdxdTilt * cosTilt
       - lxdTilt  * sinTilt; 
}



/******************************************************************************
Function : SetPoint::Rect2Polar                                       
Purpose  : Converts rectangular coodinates in fiducial frame to polar coordinates
         : in reference frame.                                	              
Inputs   : None                                                                
Outputs  : None	
Notes    : Reference frame is that corresponding to reference device.             
*******************************************************************************/

Void SetPoint::Rect2Polar()
{
  Flt32 xr, yr, zr;
  Flt32 l = 0;

  // if reference device defined then use it's origin; otherwise use fiducial pt
  if (refDevice)
  {
    xr = x - refDevice->xOrigin;
    yr = y - refDevice->yOrigin;
    zr = z - refDevice->zOrigin;
    l = refDevice->l;
  }
  else 
  {
    xr = x - xOrigin;
    yr = y - yOrigin;
    zr = z - zOrigin;
  }

  // compute polar coordinates
  f    = sqrt(xr*xr + yr*yr);	
  fd   = sqrt(f*f + zr*zr);	
  pan  = sign(yr) * acos(xr/f);	
  tilt = atan((l*f - fd*zr) / (fd*f + l*zr));	
}



/******************************************************************************
Function : SetPoint::Print                                                
Purpose  : Prints state of setpoint
Inputs   : None                                                                
Outputs  : None		                                                      
*******************************************************************************/

Void SetPoint::Print()
{
  if (iView->print)
  {
    fprintf(stderr,"Setpoint        0x%x\n", this);

//  fprintf(stderr,"Origin        (%6.2f,%6.2f,%6.2f)\n", 
//		  xOrigin, yOrigin, zOrigin);
//  fprintf(stderr,"Position (p)    (%6.2f,%6.2f,%6.2f)\n", 
//	    fd, RADS_TO_DEGS(pan), RADS_TO_DEGS(tilt) );
//  fprintf(stderr,"Position (r)    (%6.2f,%6.2f,%6.2f)\n", x, y, z);
//  fprintf(stderr,"Velocity        (%6.2f,%6.2f,%6.2f)\n", dx, dy, dz);
//  fprintf(stderr,"Device map      %d\n", deviceMap);

    fprintf(stderr,"Ref Device");
    if (refDevice == iView->portCtlr->camera)
      fprintf(stderr,"      port camera\n");
    if (refDevice == iView->portCtlr->light)
      fprintf(stderr,"      port light\n");
    if (refDevice == iView->stbdCtlr->camera)
      fprintf(stderr,"      stbd camera\n");
    if (refDevice == iView->stbdCtlr->light)
      fprintf(stderr,"      stbd light\n");

    if (controlSource)
      fprintf(stderr,"P/T Control source    0x%x\n", controlSource);
  }
}