/*****************************************************************************
Copyright 1998 MBARI                                                      
******************************************************************************
Summary  : Calibration table class member functions
Filename : Calibration.C
Author   : Michael B. Matthews                                                
Project  : iView                                                       
Version  : 3.8                                                         
Created  : 23.3.98  
Modified :  
Notes    : Used to be part of device.h in versions before V3.8
****************************************************************************/

#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 "Misc.h"		// misc functions
#include "String.h"		// homebrew string class
#include "Buffer.h"		// homebrew buffer class
#include "Linear_System.h"	// linear system and Kalman filter classes
#include "Calibration.h"	



/******************************************************************************
Function : CalibrationTable::CalibrationTable                              
Purpose  : Class constructor	              
Inputs   : file - filename of table                                              
Outputs  : None		                                                      
Notes    : 
*******************************************************************************/

CalibrationTable::CalibrationTable(String& file)
{
  Int i;
  FILE* fp;
  size = 0;
  mode = CALIBRATION_TABLE_BYPASS_MODE;

  logMsg("opening file       %s\n", file.str);
  if ((fp = fopen(file.str, "r")) == NULL)
  {
    logMsg("error opening file %s\n", file.str);
    mode = CALIBRATION_TABLE_BYPASS_MODE;
  }
  else
  {
    // read in size of table
    fscanf(fp, "%hd\n", &size);

    if ( (size > 2) && (size < 500) )
    {
      // create arrays
      x = new Flt32[size];
      y = new Flt32[size];
      z = new Flt32[size];
    
      // read in calibration table
      for (i = 0; i < size; i++)
	fscanf(fp, "%f %f %f\n", &x[i], &y[i], &z[i]);

      mode = CALIBRATION_TABLE_NORMAL_MODE;
    }
    else
    {
      logMsg("error opening file %s\n", file.str);
      mode = CALIBRATION_TABLE_BYPASS_MODE;
    }

    fclose(fp);
  }
}



/******************************************************************************
Function : CalibrationTable::LinearPiecewiseInterpolationX                       
Purpose  : Computes linear interpolation from table	              
Inputs   : s - input measurement                                              
Outputs  : returns interpolated value x --> y  
*******************************************************************************/

Flt32 CalibrationTable::LinearPiecewiseInterpolationX(Flt32 s)
{
  if (mode == CALIBRATION_TABLE_BYPASS_MODE)
    return s;

  Int16 i = FindIndexX(s);
  if (i == -1)
  {
    if (s < x[0])
      return y[0];
    else
      return y[size-1];
  }
  else
    return y[i] + (y[i+1] - y[i]) / (x[i+1] - x[i]) * (s - x[i]);
}



/******************************************************************************
Function : CalibrationTable::LinearPiecewiseInterpolationY                       
Purpose  : Computes linear interpolation from table	              
Inputs   : s - input measurement                                              
Outputs  : returns interpolated value: y --> x 
*******************************************************************************/

Flt32 CalibrationTable::LinearPiecewiseInterpolationY(Flt32 s)
{
  if (mode == CALIBRATION_TABLE_BYPASS_MODE)
    return s;

  Int16 i = FindIndexY(s);
  if (i == -1)
  {
    if (s < y[0])
      return x[0];
    else
      return x[size-1];
  }
  else
    return x[i] + (x[i+1] - x[i]) / (y[i+1] - y[i]) * (s - y[i]);
}



/******************************************************************************
Function : CalibrationTable::FindIndexX                                     
Purpose  : Find index i such that  x[i] <= x < x[i+1]
Inputs   : s - input measurement                                             
Outputs  : returns index; returns -1 if input is outside calibration table   
*******************************************************************************/

Int16 CalibrationTable::FindIndexX(Flt32 s)
{
  Int16 i = 0;
  for (i = 0; i < size-2; i++)
  {
    if ( (s >= x[i]) && (s < x[i+1]) )
      return i;
    if ( (s >= x[size-i-2]) && (s < x[size-i-1]) )
      return size-i-2;
  }
  return -1;
}



/******************************************************************************
Function : CalibrationTable::FindIndexY                                     
Purpose  : Find index i such that  y[i] <= x < y[i+1]
Inputs   : s - input measurement                                              
Outputs  : returns index; returns -1 if input is outside calibration table   
*******************************************************************************/

Int16 CalibrationTable::FindIndexY(Flt32 s)
{
  Int16 i = 0;
  for (i = 0; i < size-2; i++)
  {
    if ( (s >= y[i]) && (s < y[i+1]) )
      return i;
    if ( (s >= y[size-i-2]) && (s < y[size-i-1]) )
      return size-i-2;
  }
  return -1;
}