/*****************************************************************************
Copyright 1998 MBARI                                                      
******************************************************************************
Summary  : Integrated viewing routines
Filename : Linear_System.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 "String.h"
#include "Misc.h"		// misc functions
#include "Buffer.h"		// homebrew I/O buffer class
#include "Linear_System.h"	


/*******************************************************************************
Function : Linear_System::Init                                      
Purpose  : Initializes the linear system matrices and vectors                      
Inputs   :                                                                 
         : Fi - state transition matrix                     
         : ni - system order  
	 : Di - input matrix
	 : mi - input dimension
	 : Hi - output matrix
	 : ki - output dimension
	 : x0 - initial state
Outputs  : None                                   
*******************************************************************************/

Void Linear_System::Init(FltN** Fi, Int ni,
			 FltN** Di, Int mi,
			 FltN** Hi, Int ki,
			 FltN* x0)
{
  Reg Int i,j;
  print = FALSE;

  // system dimensions
  n = ni;
  m = mi;
  k = ki;


  // initialize the system vectors
  x = new FltN[n]; bcopy((char*) x0, (char*) x, n * sizeof(FltN));
  s = new FltN[n]; bzero((char*) s, n * sizeof(FltN));
  v = new FltN[n]; bzero((char*) v, n * sizeof(FltN));
  y = new FltN[k]; bzero((char*) y, k * sizeof(FltN));
  u = new FltN[m]; bzero((char*) u, m * sizeof(FltN));


  // initialize the (n x n) state transition matrix
  F = new FltN*[n];
  for (i = 0; i < n; i++){
    F[i] = new FltN[n];
    for (j = 0; j < n; j++){
      F[i][j] = Fi[i][j];
    }
  }

  // initialize the (n x m) input matrix
  D = new FltN*[n];
  for (i = 0; i < n; i++){
    D[i] = new FltN[m];
    for (j = 0; j < m; j++)
      D[i][j] = Di[i][j];
  }

  // initialize the (k x n) output matrix
  H = new FltN*[k];
  for (i = 0; i < k; i++){
    H[i] = new FltN[n];
    for (j = 0; j < n; j++)
      H[i][j] = Hi[i][j];
  }

  // compute initial output
  Output();
}



/*****************************************************************************
Function : Linear_System::~Linear_System                                   
Purpose  : Linear system class destructor  		              
Inputs   : None                                                                
Outputs  : None 		                                                      
******************************************************************************/

Linear_System::~Linear_System()
{
  Reg Int i;

  delete[] x;
  delete[] s;
  delete[] v;
  delete[] y;
  delete[] u;

  for (i = 0; i < n; i++){
    delete[] F[i];
    delete[] D[i];
  }
  for (i = 0; i < k; i++)
    delete[] H[i];

  delete[] F;
  delete[] D;
  delete[] H;
}



/*****************************************************************************
Function : Linear_System::InputClip                                   
Purpose  : Clip input values 		              
Inputs   : u - state variable                                                   
Outputs  : returns clipped value
******************************************************************************/

FltN Linear_System::InputClip(FltN r)
{
  if (r > LINEAR_SYSTEM_MAX_INPUT)
    return LINEAR_SYSTEM_MAX_INPUT;
 
  if (r < -LINEAR_SYSTEM_MAX_INPUT)
    return -LINEAR_SYSTEM_MAX_INPUT;

  return r + SMALL_NUMBER;
}



/*****************************************************************************
Function : Linear_System::StateClip                                   
Purpose  : Clip input values 		              
Inputs   : r - input value                                                       
Outputs  : returns clipped value
Notes    : Because of a zero trap in the 68k FPU, which causes the processor 
         : to trap for 100mS, we never let the value of a state variable get
         : below the value SMALL_NUMBER.
******************************************************************************/

FltN Linear_System::StateClip(FltN r)
{
  if (r > LINEAR_SYSTEM_MAX_STATE)
    return LINEAR_SYSTEM_MAX_STATE;
 
  if (r < -LINEAR_SYSTEM_MAX_STATE)
    return -LINEAR_SYSTEM_MAX_STATE;

  return r + SMALL_NUMBER;
}



/*****************************************************************************
Function : Linear_System::Propagate                                           
Purpose  : Computes one-step system propagation                            
Inputs   : u - control input value
Outputs  : None                                           
******************************************************************************/

STATUS Linear_System::Propagate(FltN* ui)
{
  Reg Int i;
  Reg Int j;


  // copy input vector 
  for (i = 0; i < m; i++)
    u[i] = ui[i];


  // input propagation
  for (i = 0; i < n; i++)
  {
    s[i] = 0.0;
    for (j = 0; j < m; j++)
      s[i] += D[i][j] * InputClip(u[j]);
  }


  // state propagation
  for (i = 0; i < n; i++)
  {
    v[i] = 0.0;
    for (j = 0; j < n; j++)
      v[i] += F[i][j] * x[j];
  }


  // add input and state
  for (i = 0; i < n; i++)
    x[i] = StateClip(v[i]) + s[i];


  // compute output
  Output();

  return OK;
}



/*****************************************************************************
Function : Linear_System::Output                                           
Purpose  : Computes system output from current state y = H * x                    
Inputs   : None
Outputs  : None                                           
******************************************************************************/

STATUS Linear_System::Output()
{
  Reg Int i;
  Reg Int j;

  for (i = 0; i < k; i++)
  {
    y[i] = 0;
    for (j = 0; j < n; j++)
      y[i] += H[i][j] * x[j];
  }

  return OK;
}



/*****************************************************************************
Function : Linear_System::Print                                           
Purpose  : Prints out system parameters                    
Inputs   : None
Outputs  : None                                           
******************************************************************************/

STATUS Linear_System::Print()
{
  Reg Int i;
  Reg Int j;

  if (print)
  {
    printf("\nx - vector\n");
    for (i = 0; i < n; i++)
      printf("%7.2f   ", x[i]);
    printf("\n");

    printf("\ny - vector\n");
    for (i = 0; i < k; i++)
      printf("%7.2f   ", y[i]);
    printf("\n");

    printf("\nF - matrix\n");
    for (i = 0; i < n; i++)
    {
      for (j = 0; j < n; j++)
	printf("%7.2f   ", F[i][j]);
      printf("\n");
    }

    printf("\nH - matrix\n");
    for (i = 0; i < k; i++)
    {
      for (j = 0; j < n; j++)
	printf("%7.2f   ", H[i][j]);
      printf("\n");
    }

    printf("\nD - matrix\n");
    for (i = 0; i < n; i++)
    {
      for (j = 0; j < m; j++)
	printf("%7.2f   ", D[i][j]);
      printf("\n");
    }
  }
}



/*******************************************************************************
Function : KalmanFilter::Init                                        
Purpose  : Kalman filter object initialization                            
Inputs   : Ki - Kalman gain                           
Outputs  : None                                   
*******************************************************************************/

Void Kalman_Filter::Init(FltN** Ki)
{
  // enable all inputs
  mask = 0xffffffff;

  xb = new FltN[n]; bzero((char*) xb, n * sizeof(FltN));
  e  = new FltN[k]; bzero((char*) e,  k * sizeof(FltN));
  z  = new FltN[k]; bzero((char*) z,  k * sizeof(FltN));

  // initialize (n x k) Kalman gain matrix
  K = new FltN*[n];
  for (Int i = 0; i < n; i++)
  {
    K[i] = new FltN[k];
    for (Int j = 0; j < k; j++)
      K[i][j] = Ki[i][j];
  }
}



/*****************************************************************************
Function : Kalman_Filter::~Kalman_Filter                                         
Purpose  : Kalman filter class destructor  		              
Inputs   : None                                                                
Outputs  : None		                                                      
******************************************************************************/

Kalman_Filter::~Kalman_Filter()
{
  delete[] xb;
  delete[] e;
  for (Int i = 0; i < n; i++)
    delete[] K[i];
  delete[] K;
}



/*****************************************************************************
Function : Kalman_Filter::Update                                             
Purpose  : Computes Kalman filter measurement update                           
Inputs   : None
Outputs  : None
Notes    : Elements of the innovations vector corresponding to a zero mask 
         : element are zeroed.                                       
******************************************************************************/

STATUS Kalman_Filter::Update()
{
  Reg Int i;
  Reg Int j;

  // compute innovations and multiply by Kalman gain matrix K
  for (i = 0; i < n; i++)
  {
    xb[i] = 0;
    for (j = 0; j < k; j++)
    {
      e[j] = InnovationsClip(z[j] - y[j]);
      if (mask & (1 << j)) 
	xb[i] += K[i][j] * e[j];
    }
  }

  // add innovations vector to propagated state vector
  for (i = 0; i < n; i++)
    x[i] += xb[i];

  return OK;
}



/*****************************************************************************
Function : Kalman_Filter::Update                                             
Purpose  : Computes Kalman filter measurement update                           
Inputs   : z - input vector
Outputs  : None     
Notes    : Elements of the innovations vector corresponding to a zero mask 
         : element are zeroed.                                       
******************************************************************************/

STATUS Kalman_Filter::Update(FltN* zi)
{
  bcopy((char*) zi, (char*) z, k * sizeof(FltN));
  Update();
  return OK;
}



/*****************************************************************************
Function : Kalman_Filter::InnovationsClip                                   
Purpose  : Clip innovations values 		              
Inputs   : u - innovations value                                                 
Outputs  : returns clipped value
******************************************************************************/

FltN Kalman_Filter::InnovationsClip(FltN r)
{
  if (r > KALMAN_FILTER_MAX_INNOVATIONS)
    return KALMAN_FILTER_MAX_INNOVATIONS;
 
  if (r < -KALMAN_FILTER_MAX_INNOVATIONS)
    return -KALMAN_FILTER_MAX_INNOVATIONS;

  return r + SMALL_NUMBER;
}



/*******************************************************************************
Function : FIR_Filter::FIR_Filter                                          
Purpose  : Initializes the FIR filter structure                            
Inputs   :
Outputs  : 		                                                      
Notes    : Creates a one-dimensional unity-gain filter		      
*******************************************************************************/

FIR_Filter::FIR_Filter()
{
  localBuffers = TRUE;
  n = 1;
  b = new Flt32[n];
  b[0] = 1;
  x = new IO_Buffer(n);
}



/*******************************************************************************
Function : FIR_Filter::FIR_Filter                                          
Purpose  : Initializes the FIR filter structure                            
Inputs   : b - array of numerator filter coefficients                      
Outputs  : None		                                                      
Notes    : The sign of denominator coefficients corresponds to the         
         : transfer function format in the sense that they are subtracted  
         : from the numerator inner-product; this is the output convention 
         : of the MATLAB filter design routine.			      
*******************************************************************************/

FIR_Filter::FIR_Filter(char* filename)
{
  FILE* fp;
  char s[100];
  localBuffers = FALSE;

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

  // read in filter dimension
  n  = (Int) getNumber(fp,s);
  b = new Flt32[n];

  Int i;
  for (i = 0; i < n; i++)
  {
    b[i] = getNumber(fp,s);
//    printf("%e\n", b[i]); 
  }

  fclose(fp);
}



/*******************************************************************************
Function : FIR_Filter::FIR_Filter                                          
Purpose  : Initializes the FIR filter structure                            
Inputs   : b - array of numerator filter coefficients                      
         : n - length of numerator   
Outputs  : 		                                                      
Notes    : The sign of denominator coefficients corresponds to the         
         : transfer function format in the sense that they are subtracted  
         : from the numerator inner-product; this is the output convention 
         : of the MATLAB filter design routine.			      
*******************************************************************************/

FIR_Filter::FIR_Filter(Flt32* bCoeff, Int nLength)
{
  localBuffers = TRUE;
  n = nLength;
  b = new Flt32[n];
  bcopy((char*) bCoeff, (char*) b, n * sizeof(Flt32));
  x = new IO_Buffer(n);
}



/*******************************************************************************
Function : FIR_Filter::FIR_Filter                                          
Purpose  : Initializes the FIR filter structure                            
Inputs   :                                                                 
         : b - array of numerator filter coefficients                      
         : n - length of numerator   
         : a - array of denominator coefficients                           
         : m - length of denominator
	 : x - pointer to input IO_Buffer
	 : y - pointer to output IO_Buffer
Outputs  : 		                                                      
Notes    : The sign of denominator coefficients corresponds to the         
         : transfer function format in the sense that they are subtracted  
         : from the numerator inner-product; this is the output convention 
         : of the MATLAB filter design routine.			      
*******************************************************************************/

FIR_Filter::FIR_Filter(Flt32* bCoeff, Int nLength, IO_Buffer* xBuff)
{
  localBuffers = FALSE;
  n = nLength;
  b = new Flt32[n];
  bcopy((char*) bCoeff, (char*) b, n * sizeof(Flt32));
  x = xBuff;
}



/*****************************************************************************
Function : FIR_Filter::Compute                                             
Purpose  : Computes FIR filter on given buffers                            
Inputs   : u - input value
Outputs  : Current filter output                                           
******************************************************************************/

Flt32 FIR_Filter::Compute(IO_Buffer* x)
{
  Reg Int16 i;
  Reg Int16 j;
  Reg Int16 k;
  Flt32 u = 0;

  // numerator terms b[0],b[1],...,b[n] 
  j = x->index;
  for (i = 0; i < n; i++){
    k = j - i;
    if (k < 0) k += x->size;
    u += b[i] * x->u[k];
  }
 
  return u;
}



/*****************************************************************************
Function : FIR_Filter::Compute                                             
Purpose  : Computes FIR filter on given buffers                            
Inputs   : u - input value
Outputs  : Current filter output                                           
******************************************************************************/

Flt32 FIR_Filter::Compute()
{
  Reg Int16 i;
  Reg Int16 j;
  Reg Int16 k;
  Flt32 u = 0;

  // numerator terms b[0],b[1],...,b[n] 
  j = x->index;
  for (i = 0; i < n; i++){
    k = j - i;
    if (k < 0) k += x->size;
    u += b[i] * x->u[k];
  }
 
  return u;
}



/*******************************************************************************
Function : IIR_Filter::IIR_Filter                                          
Purpose  : Initializes the IIR filter structure                            
Inputs   :
Outputs  : 		                                                      
Notes    : Creates a one-dimensional unity-gain filter		      
*******************************************************************************/

IIR_Filter::IIR_Filter() : FIR_Filter()
{
  m = 1;
  a = new Flt32[m];
  a[0] = 0;
  y = new IO_Buffer(m);
}



/*******************************************************************************
Function : IIR_Filter::IIR_Filter                                          
Purpose  : Initializes the IIR filter structure                            
Inputs   :                                                                 
         : b - array of numerator filter coefficients                      
         : n - length of numerator   
         : a - array of denominator coefficients                           
         : m - length of denominator 
Outputs  : 		                                                      
Notes    : The sign of denominator coefficients corresponds to the         
         : transfer function format in the sense that they are subtracted  
         : from the numerator inner-product; this is the output convention 
         : of the MATLAB filter design routine.			      
*******************************************************************************/

IIR_Filter::IIR_Filter(Flt32* bCoeff, Int nLength, Flt32* aCoeff, Int mLength)
: FIR_Filter(bCoeff, nLength)
{
  m = mLength;
  a = new Flt32[m];
  bcopy((char*) aCoeff, (char*) a, m * sizeof(Flt32));
  y = new IO_Buffer(m);
}



/*******************************************************************************
Function : IIR_Filter::IIR_Filter                                          
Purpose  : Initializes the IIR filter structure                            
Inputs   :                                                                 
         : b - array of numerator filter coefficients                      
         : n - length of numerator   
         : a - array of denominator coefficients                           
         : m - length of denominator
	 : x - pointer to input IO_Buffer
	 : y - pointer to output IO_Buffer
Outputs  : 		                                                      
Notes    : The sign of denominator coefficients corresponds to the         
         : transfer function format in the sense that they are subtracted  
         : from the numerator inner-product; this is the output convention 
         : of the MATLAB filter design routine.			      
*******************************************************************************/

IIR_Filter::IIR_Filter(Flt32* bCoeff, Int nLength, Flt32* aCoeff, Int mLength,
		       IO_Buffer* xBuff, IO_Buffer* yBuff)
: FIR_Filter(bCoeff, nLength, xBuff)
{
  m = mLength;
  a = new Flt32[m];
  bcopy((char*) aCoeff, (char*) a, m * sizeof(Flt32));
  y = yBuff;
}



/*****************************************************************************
Function : IIR_Filter::Compute                                             
Purpose  : Computes IIR filter on given buffers                            
Inputs   : u - input value
Outputs  : Current filter output                                           
******************************************************************************/

Flt32 IIR_Filter::Compute()
{
  Reg Int16 i;
  Reg Int16 j;
  Reg Int16 k;
  Flt32 u = 0;

  // numerator terms b[0],b[1],...,b[n] 
  j = x->index;
  for (i = 0; i < n; i++){
    k = j - i;
    if (k < 0) k += x->size;
    u += b[i] * x->u[k];
  }

  // denominator terms a[1],...,a[m] 
  j = y->index;
  for (i = 1; i < m; i++){
    k = j - i + 1;
    if (k < 0) k += y->size;
    u -= a[i] * y->u[k];
  }
 
  return u;
}



/*****************************************************************************
Function : IIR_Filter::Compute                                             
Purpose  : Computes IIR filter on given buffers                            
Inputs   : u - input value
Outputs  : Current filter output                                           
******************************************************************************/

Flt32 IIR_Filter::Compute(IO_Buffer* x, IO_Buffer* y)
{
  Reg Int16 i;
  Reg Int16 j;
  Reg Int16 k;
  Flt32 u = 0;

  // numerator terms b[0],b[1],...,b[n] 
  j = x->index;
  for (i = 0; i < n; i++){
    k = j - i;
    if (k < 0) k += x->size;
    u += b[i] * x->u[k];
  }

  // denominator terms a[1],...,a[m] 
  j = y->index;
  for (i = 1; i < m; i++){
    k = j - i + 1;
    if (k < 0) k += y->size;
    u -= a[i] * y->u[k];
  }
 
  return u;
}