/*****************************************************************************
Copyright 1998 MBARI                                                      
******************************************************************************
Summary  : Miscellaneous functions
Filename : Misc.C
Author   : Michael B. Matthews                                                
Project  : iView                                                       
Version  : 3.8                                                         
Created  : 23.3.98  
Modified :  
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 <mbariConst.h>  	            
#include "usrTime.h"
};


#include "DataManager.h"
#include "Misc.h"


/*****************************************************************************
Function : sign                                                            
Purpose  : returns 1.0 * sign(x)             		              
Inputs   :                		                                      
Outputs  : 		                                                      
******************************************************************************/

FltN sign(FltN x)
{
  if (x >= 0) return 1.0;
  else return -1.0;
}



/*****************************************************************************
Function : Fabs                                         
Purpose  : Computes absolute value - replaces WRS version which doesn't work
Inputs   : z - input value
Outputs  : absolute value of z                                           
******************************************************************************/

Flt32 Fabs(Flt32 z)
{
  if (z < 0) return -z;
  else return z;
}



/****************************************************************************
Function : getNumber                                                   
Purpose  : Reads in configuration file			              
Inputs   :                		                                      
Outputs  : 		                                                      
*****************************************************************************/

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



/****************************************************************************
Function : getParameter                                                   
Purpose  : Reads in configuration file			              
Inputs   :                		                                      
Outputs  : 		                                                      
*****************************************************************************/

Flt32 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 (Flt32) atof(s1);
}



/****************************************************************************
Function : modulo2Pi                                                       
Purpose  : Returns the value modulo 2*pi                                   
Inputs   :                		                                      
Outputs  : 		                                                      
*****************************************************************************/

Flt32 modulo2Pi(Flt32 g)
{
  Flt32 x;
  x = (g - ((Int) (g/PI2)) * PI2);
  if (x < 0) x += PI2;
  return(x);
}



/*****************************************************************************
Function : Get_Time	                                                      
Purpose  : 							              
Inputs   :                		                                      
Outputs  : 		                                                      
Notes    : This routine has about 16uS of overhead			      
******************************************************************************/

Void Get_Time(Flt64* t)
{
  DM_Time sampleTime;			// update time                
  gettimeofday(&sampleTime, (struct timezone *) NULL);
  *t = (Flt64) sampleTime.tv_sec + (Flt64) sampleTime.tv_usec / 1000000.0;
}



/*****************************************************************************
Function : Get_Time	                                                      
Purpose  : 							              
Inputs   :                		                                      
Outputs  : 		                                                      
Notes    : This routine has about 16uS of overhead			      
******************************************************************************/

Void Get_Time(Flt64* t, Flt64* dt)
{
  static MBool init = TRUE;		// initialization flag        
  DM_Time sampleTime;			// update time                
  Flt64 time = 0.0;			// absolute time              
  static Flt64 time_last = 0.0;		// absolute time              
  static Flt64 timeOffset;		// time offset                

  gettimeofday(&sampleTime, (struct timezone *) NULL);
  time = (Flt64) sampleTime.tv_sec +
         (Flt64) sampleTime.tv_usec / 1000000.0;
  if (init){
    timeOffset = time;
    init = FALSE;
  }
  time -= timeOffset;
  *dt = time - time_last;
  *t = time;
  time_last = time;
}



/*****************************************************************************
Function : Get_Absolute_Time                                               
Purpose  : 							              
Inputs   :                		                                      
Outputs  : 		                                                      
Notes    : This routine has about 16uS of overhead			      
******************************************************************************/

Flt64 Get_Absolute_Time(void)
{
  DM_Time sampleTime;
  Flt64 t;
  gettimeofday(&sampleTime, (struct timezone *) NULL);
  t = (Flt64) sampleTime.tv_sec + (Flt64) sampleTime.tv_usec / 1000000.0;
  return t;
}