/**************************************************************************
Copyright 1998 MBARI                                                    
***************************************************************************
Summary  : Integrated viewing routines                                  
Filename : iview.C                                                      
Author   : Michael B. Matthews                                          
Project  : Tiburon ROV                                                  
Version  : 4.0                                                          
Created  : 8.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"				


// Global iView pointer
IView* iView;
SetPoint* setPointRegister[MAX_NUMBER_SETPOINTS];


/*****************************************************************************
Function : iview                                                           
Purpose  : iview main initialization and service loop
Inputs   : None               		                                      
Outputs  : None
******************************************************************************/

STATUS iview(void)
{
  logMsg("\n\n");
  logMsg("iView - Integrated Viewing System\n");
  logMsg("V4.0\n");
  logMsg("M.B. Matthews\n");
  logMsg("May 8, 1998\n\n");

  taskPrioritySet(taskIdSelf(),IVIEW_PRIORITY);

  // construct iView object
  iView = new IView();

  // start processing loop
  iView->Run();

  delete iView;
  logMsg("exiting iView\n");
  return OK;
}



/******************************************************************************
Function : Anciliary shell functions                                     
Purpose  : For VxWorks command control for testing 	
Inputs   : None               		                                      
Outputs  : None		                                                      
*******************************************************************************/

Void logCtlrs()     { iView->Log(); }
Void exitIView()    { iView->Exit(); }
Void resumeCtlrs()  { iView->Resume(); }
Void enableCtlrs()  { iView->Enable(); }
Void standbyCtlrs() { iView->Standby(); }
Void disableCtlrs() { iView->Disable(); }
Void enablePrint()  { iView->print = TRUE; }	
Void disablePrint() { iView->print = FALSE; }



/*****************************************************************************
Function : IView::IView                                                      
Purpose  : IView object constructor				              
Inputs   : None                                      
Outputs  : None		                                                      
Notes    :     
******************************************************************************/

IView::IView()
{
  Int i;
  logging = FALSE;
  loop = TRUE;
  print = FALSE;

  // default config parameters in case config file fails to open
  DeviceConfig portCameraConfig;
  DeviceConfig stbdCameraConfig;
  DeviceConfig portLightConfig;
  DeviceConfig stbdLightConfig;

  // open config file and read in parameters
  if ( ConfigFile(&portCameraConfig, 
		  &stbdCameraConfig, 
		  &portLightConfig, 
		  &stbdLightConfig, 
		  IVIEW_CONFIG_FILE) != OK )
  {
    logMsg("iview configuration failure\n");
    exit(1);
  }

  wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY);
  dmGroup = new DmGroup();

  for (i = 0; i < MAX_NUMBER_CONTROL_SOURCES; i++)
  {
    controlSource[i] = new ControlSourceInterface(i,wakeupSem);
    dmGroup->addItem(controlSource[i]->aperiodicRequestItem());  
  }

  // register all setpoints
  for (i = 0; i < MAX_NUMBER_SETPOINTS; i++)
    setPointRegister[i] = NULL;


  // initialize port pan/tilt controllers
  portCtlr = new Controller("PORT",
			    IviewPortCamera,
			    &portCameraConfig,
			    IviewPortLight,
			    &portLightConfig);
  

  // initialize stbd pan/tilt controllers
  stbdCtlr = new Controller("STBD",
			    IviewStbdCamera,
			    &stbdCameraConfig,
			    IviewStbdLight,
			    &stbdLightConfig);
}



/*****************************************************************************
Function : IView::~IView                                                      
Purpose  : IView object destructor				              
Inputs   : None                                      
Outputs  : None		                                                      
Notes    :     
******************************************************************************/

IView::~IView()
{
  Int i;

  delete dmGroup;

  delete portCtlr;
  delete stbdCtlr;

  // delete all control sources
  for (i = 0; i < MAX_NUMBER_CONTROL_SOURCES; i++)
    delete controlSource[i];

  for (i = 0; i < MAX_NUMBER_SETPOINTS; i++)
    if (setPointRegister[i]) 
      delete setPointRegister[i]; 
}



/*****************************************************************************
Function : IView::Run                                                      
Purpose  : Processing loop			              
Inputs   : None                                      
Outputs  : None		                                                      
Notes    :     
******************************************************************************/

STATUS IView::Run() 
{
  Nat32 ticks = tickGet();	// for subtacting loop time from timer
  Int32 delay;              	// calculated loop delay  

  // enable controllers
  Enable();
  Start();

  while (loop) 
  {
    // [Write heartbeat]

    // compute delay time for iview loop execution time
    delay = sysClkRateGet()/20 - min(tickGet() - ticks, sysClkRateGet()/20);
    if (delay > 0) 
    {
      if (semTake(wakeupSem, delay) == OK)
      {
	aperiodicRequestCheck();
	continue;
      }
    }
    else
      aperiodicRequestCheck();
    
    ticks = tickGet();


    Int i;
    for (i = 0; i < MAX_NUMBER_CONTROL_SOURCES; i++) 
    {
      if (!controlSource[i]->isActive()) 
	continue;

      if (!controlSource[i]->hasClient()) 
      {
        controlSource[i]->deactivate();
        continue;
      }

      // Process periodic request
      controlSource[i]->periodicUpdate();
    }

    // update all devices
    portCtlr->Update();
    stbdCtlr->Update();      
  }

  return OK;
}



/*****************************************************************************
Function : IView::aperiodicRequestCheck                                                 
Purpose  :               
Inputs   : 
Outputs  : None		                                                      
******************************************************************************/

MBool IView::aperiodicRequestCheck() 
{
  if (!dmGroup->getChanges()) return FALSE;

  // Determine which client(s) issued request
  for (Int i = 0; i < MaxControlSources; i++) 
  {
    if ( dmGroup->itemChanged(controlSource[i]->aperiodicRequestItem()) )
      controlSource[i]->aperiodicUpdate();
  }

  return TRUE;
}



/*****************************************************************************
Function : IView::getParameter                                                    
Purpose  : Reads a floating point line in configuration file	           
Inputs   : fp - file pointer           		                                      
Outputs  : Load s with description string and return floating point number.
******************************************************************************/

FltN IView::getParameter(FILE* fp, char* s)
{
  char* s1;
  do {
    fgets(s,100,fp);
  } while (s[0] == '%' || strtok(s, " \t\n") == NULL);
  s1 = strtok(NULL, " \t\n");
  return (FltN) atof(s1);
}



/*****************************************************************************
Function : IView::ConfigFile                                                 
Purpose  : Reads in configuration file			              
Inputs   : Pointers to device config struct for each device; cfg filename
Outputs  : Returns ERROR status if error on cfg file open                             
******************************************************************************/

STATUS IView::ConfigFile(DeviceConfig* portCamera, 
			 DeviceConfig* stbdCamera, 
			 DeviceConfig* portLight, 
			 DeviceConfig* stbdLight, 
			 char* file)
{
  FILE* fp;
  char* s1;
  char s[100];

  logMsg("opening file       %s\n", file);
  if ( (fp = fopen(file, "r")) == NULL ){
    logMsg("error opening file %s\n", file);
    return ERROR;
  }

  logging = (MBool) getParameter(fp,s);		// logging switch      

  // port camera PID gains 
  portCamera->pGainPan  = getParameter(fp,s);
  portCamera->dGainPan  = getParameter(fp,s);
  portCamera->iGainPan  = getParameter(fp,s);
  portCamera->fGainPan  = getParameter(fp,s);

  portCamera->pGainTilt = getParameter(fp,s);
  portCamera->dGainTilt = getParameter(fp,s);
  portCamera->iGainTilt = getParameter(fp,s);
  portCamera->fGainTilt = getParameter(fp,s);

  // stbd camera PID gains 
  stbdCamera->pGainPan  = getParameter(fp,s);
  stbdCamera->dGainPan  = getParameter(fp,s);
  stbdCamera->iGainPan  = getParameter(fp,s);
  stbdCamera->fGainPan  = getParameter(fp,s);

  stbdCamera->pGainTilt = getParameter(fp,s);
  stbdCamera->dGainTilt = getParameter(fp,s);
  stbdCamera->iGainTilt = getParameter(fp,s);
  stbdCamera->fGainTilt = getParameter(fp,s);

  // port light PID gains 
  portLight->pGainPan  = getParameter(fp,s);
  portLight->dGainPan  = getParameter(fp,s);
  portLight->iGainPan  = getParameter(fp,s);
  portLight->fGainPan  = getParameter(fp,s);

  portLight->pGainTilt = getParameter(fp,s);
  portLight->dGainTilt = getParameter(fp,s);
  portLight->iGainTilt = getParameter(fp,s);
  portLight->fGainTilt = getParameter(fp,s);

  // stbd light PID gains 
  stbdLight->pGainPan  = getParameter(fp,s);
  stbdLight->dGainPan  = getParameter(fp,s);
  stbdLight->iGainPan  = getParameter(fp,s);
  stbdLight->fGainPan  = getParameter(fp,s);

  stbdLight->pGainTilt = getParameter(fp,s);
  stbdLight->dGainTilt = getParameter(fp,s);
  stbdLight->iGainTilt = getParameter(fp,s);
  stbdLight->fGainTilt = getParameter(fp,s);

  // camera velocity limits 
  portCamera->maxVelocityPan  = DEGS_TO_RADS(getParameter(fp,s));
  portCamera->minVelocityPan  = DEGS_TO_RADS(getParameter(fp,s));
  portCamera->maxVelocityTilt = DEGS_TO_RADS(getParameter(fp,s));
  portCamera->minVelocityTilt = DEGS_TO_RADS(getParameter(fp,s));

  stbdCamera->maxVelocityPan  = DEGS_TO_RADS(getParameter(fp,s));
  stbdCamera->minVelocityPan  = DEGS_TO_RADS(getParameter(fp,s));
  stbdCamera->maxVelocityTilt = DEGS_TO_RADS(getParameter(fp,s));
  stbdCamera->minVelocityTilt = DEGS_TO_RADS(getParameter(fp,s));

  // light velocity limits 
  portLight->maxVelocityPan  = DEGS_TO_RADS(getParameter(fp,s));
  portLight->minVelocityPan  = DEGS_TO_RADS(getParameter(fp,s));
  portLight->maxVelocityTilt = DEGS_TO_RADS(getParameter(fp,s));
  portLight->minVelocityTilt = DEGS_TO_RADS(getParameter(fp,s));

  stbdLight->maxVelocityPan  = DEGS_TO_RADS(getParameter(fp,s));
  stbdLight->minVelocityPan  = DEGS_TO_RADS(getParameter(fp,s));
  stbdLight->maxVelocityTilt = DEGS_TO_RADS(getParameter(fp,s));
  stbdLight->minVelocityTilt = DEGS_TO_RADS(getParameter(fp,s));

  // camera velocity to pan-tilt command conversion
  portCamera->velocityConversion = RADS_TO_DEGS(getParameter(fp,s));
  stbdCamera->velocityConversion = RADS_TO_DEGS(getParameter(fp,s));

  // light velocity to pan-tilt command conversion
  portLight->velocityConversion = RADS_TO_DEGS(getParameter(fp,s));
  stbdLight->velocityConversion = RADS_TO_DEGS(getParameter(fp,s));

  // camera offsets from front center
  portCamera->offsetPan  = DEGS_TO_RADS(getParameter(fp,s));	             
  portCamera->offsetTilt = DEGS_TO_RADS(getParameter(fp,s));	             
  stbdCamera->offsetPan  = DEGS_TO_RADS(getParameter(fp,s));	             
  stbdCamera->offsetTilt = DEGS_TO_RADS(getParameter(fp,s));	             

  // light offsets from front center
  portLight->offsetPan  = DEGS_TO_RADS(getParameter(fp,s));	             
  portLight->offsetTilt = DEGS_TO_RADS(getParameter(fp,s));	             
  stbdLight->offsetPan  = DEGS_TO_RADS(getParameter(fp,s));	             
  stbdLight->offsetTilt = DEGS_TO_RADS(getParameter(fp,s));	             

  // port camera origin
  portCamera->xOrigin = getParameter(fp,s);	             
  portCamera->yOrigin = getParameter(fp,s);	             
  portCamera->zOrigin = getParameter(fp,s);	             

  // stbd camera origin
  stbdCamera->xOrigin = getParameter(fp,s);	             
  stbdCamera->yOrigin = getParameter(fp,s);	             
  stbdCamera->zOrigin = getParameter(fp,s);	             

  // port light origin
  portLight->xOrigin = getParameter(fp,s);	             
  portLight->yOrigin = getParameter(fp,s);	             
  portLight->zOrigin = getParameter(fp,s);	             

  // stbd light origin
  stbdLight->xOrigin = getParameter(fp,s);	             
  stbdLight->yOrigin = getParameter(fp,s);	             
  stbdLight->zOrigin = getParameter(fp,s);	             

  // port camera setpoint
  portCamera->xSetPoint = getParameter(fp,s);	             
  portCamera->ySetPoint = getParameter(fp,s);	             
  portCamera->zSetPoint = getParameter(fp,s);	             

  // stbd camera setpoint
  stbdCamera->xSetPoint = getParameter(fp,s);	             
  stbdCamera->ySetPoint = getParameter(fp,s);	             
  stbdCamera->zSetPoint = getParameter(fp,s);	             

  // port light setpoint
  portLight->xSetPoint = getParameter(fp,s);	             
  portLight->ySetPoint = getParameter(fp,s);	             
  portLight->zSetPoint = getParameter(fp,s);	             

  // stbd light setpoint
  stbdLight->xSetPoint = getParameter(fp,s);	             
  stbdLight->ySetPoint = getParameter(fp,s);	             
  stbdLight->zSetPoint = getParameter(fp,s);	             

  // port camera pan/tilt ranges
  portCamera->posLimitPan  = DEGS_TO_RADS(getParameter(fp,s));	             
  portCamera->negLimitPan  = DEGS_TO_RADS(getParameter(fp,s));             
  portCamera->posLimitTilt = DEGS_TO_RADS(getParameter(fp,s));	             
  portCamera->negLimitTilt = DEGS_TO_RADS(getParameter(fp,s));	             

  // stbd camera pan/tilt ranges
  stbdCamera->posLimitPan  = DEGS_TO_RADS(getParameter(fp,s));	             
  stbdCamera->negLimitPan  = DEGS_TO_RADS(getParameter(fp,s));	             
  stbdCamera->posLimitTilt = DEGS_TO_RADS(getParameter(fp,s));	             
  stbdCamera->negLimitTilt = DEGS_TO_RADS(getParameter(fp,s));	             

  // port light pan/tilt ranges
  portLight->posLimitPan  = DEGS_TO_RADS(getParameter(fp,s));	             
  portLight->negLimitPan  = DEGS_TO_RADS(getParameter(fp,s));	             
  portLight->posLimitTilt = DEGS_TO_RADS(getParameter(fp,s));	             
  portLight->negLimitTilt = DEGS_TO_RADS(getParameter(fp,s));	             

  // stbd light pan/tilt ranges
  stbdLight->posLimitPan  = DEGS_TO_RADS(getParameter(fp,s));	             
  stbdLight->negLimitPan  = DEGS_TO_RADS(getParameter(fp,s));	             
  stbdLight->posLimitTilt = DEGS_TO_RADS(getParameter(fp,s));	             
  stbdLight->negLimitTilt = DEGS_TO_RADS(getParameter(fp,s));	             

  // initial focal lengths
  portCamera->initialFocalLength = getParameter(fp,s);
  stbdCamera->initialFocalLength = getParameter(fp,s);	

  // camera pan/tilt mounting bracket offsets (inches)
  portCamera->bracketOffsetPan  = getParameter(fp,s);		             
  portCamera->bracketOffsetTilt = getParameter(fp,s);		             
  stbdCamera->bracketOffsetPan  = getParameter(fp,s);		             
  stbdCamera->bracketOffsetTilt = getParameter(fp,s);		             

  // light pan/tilt mounting bracket offsets (inches)
  portLight->bracketOffsetPan  = getParameter(fp,s);		             
  portLight->bracketOffsetTilt = getParameter(fp,s);		             
  stbdLight->bracketOffsetPan  = getParameter(fp,s);		             
  stbdLight->bracketOffsetTilt = getParameter(fp,s);		             

  // camera panoramic mode offsets
  portCamera->panoramicOffsetPan  = DEGS_TO_RADS(getParameter(fp,s));             
  portCamera->panoramicOffsetTilt = DEGS_TO_RADS(getParameter(fp,s));             
  stbdCamera->panoramicOffsetPan  = DEGS_TO_RADS(getParameter(fp,s));             
  stbdCamera->panoramicOffsetTilt = DEGS_TO_RADS(getParameter(fp,s));             

  // light panoramic mode offsets
  portLight->panoramicOffsetPan  = DEGS_TO_RADS(getParameter(fp,s));	          
  portLight->panoramicOffsetTilt = DEGS_TO_RADS(getParameter(fp,s));             
  stbdLight->panoramicOffsetPan  = DEGS_TO_RADS(getParameter(fp,s));	          
  stbdLight->panoramicOffsetTilt = DEGS_TO_RADS(getParameter(fp,s));   

  // device logging buffer lengths
  portCamera->bufferLength = (Int16) getParameter(fp,s);	
  portLight->bufferLength  = (Int16) getParameter(fp,s);	
  stbdCamera->bufferLength = (Int16) getParameter(fp,s);	
  stbdLight->bufferLength  = (Int16) getParameter(fp,s);	

  fclose(fp);
  return(OK);
}