/*****************************************************************************
Copyright 1998 MBARI                                                      
******************************************************************************
Summary  : Integrated viewing routines
Filename : Linear_System.h
Author   : Michael B. Matthews                                                
Project  : New ROV                                                        
Version  : 3.8                                                         
Created  : 11.2.98  
Modified : 23.3.98  
Notes    :                                                   
****************************************************************************/

#ifndef linear_system_h
#define linear_system_h


class IIR_Filter;
class Linear_System;
class Kalman_Filter;



/******************************************************************************
Class   : L I N E A R  S Y S T E M                                              
Purpose : Provides general linear discrete-time state space system functions
Notes   : 
******************************************************************************/

#define LINEAR_SYSTEM_MAX_INPUT (FltN) 100.0
#define LINEAR_SYSTEM_MAX_STATE (FltN) 100000.0
#define SMALL_NUMBER 1.0e-35

class Linear_System 
{
protected:
  MBool print;		// diagnostic printing flag
  String name;		// system name
  Int n;		// system order
  Int m;		// input dimension
  Int k;		// output dimension
  FltN** F;		// (n x n) state transition matrix
  FltN** D;		// (n x m) input matrix
  FltN** H;		// (k x n) output matrix
  FltN* x;		// (n x 1) state vector
  FltN* y;		// (k x 1) output vector
  FltN* u;		// (m x 1) input vector
  FltN* s;		// (n x 1) scratch vector
  FltN* v;		// (n x 1) scratch vector

public:
  Linear_System(FltN** F, Int n,
		FltN** D, Int m,
		FltN** H, Int k,
		FltN* x0)
    : name(NULL) { Init(F, n, D, m, H, k, x0); }

  Linear_System(FltN** F, Int n,
		FltN** D, Int m,
		FltN** H, Int k,
		FltN* x0,
		String string) 
    : name(string) { Init(F, n, D, m, H, k, x0); }

  Linear_System(FltN** F, Int n,
		FltN** D, Int m,
		FltN** H, Int k,
		FltN* x0,
		char* string) 
    : name(string) { Init(F, n, D, m, H, k, x0); }

  Linear_System(FILE* fp) { }

  ~Linear_System();

  Void Init(FltN** F, Int n,
	    FltN** D, Int m,
	    FltN** H, Int k,
	    FltN* x0);

  STATUS Propagate(FltN*);

  inline STATUS Output();
  inline FltN InputClip(FltN);
  inline FltN StateClip(FltN);
  inline STATUS Init(FltN* x0) { for (Int i = 0; i < n; i++) x[i] = x0[i]; } 
  inline STATUS Init() { for (Int i = 0; i < n; i++) x[i] = 0; }

  virtual inline FltN State(Int i) { return x[i-1]; }
  virtual inline Int Order() { return n; }
  virtual Print();
};



/******************************************************************************
Class   : K A L M A N  F I L T E R                                               
Purpose : Kalman filter object class 
Notes   : 
******************************************************************************/

#define KALMAN_FILTER_MAX_INNOVATIONS (FltN) 100.0

class Kalman_Filter : public Linear_System 
{
private:
  DWord mask;		// mask for input complement
  FltN* z;		// (k x 1) measurement vector
  FltN* e;		// (k x 1) innovations vector
  FltN* xb;		// (n x 1) state estimate vector
  FltN** K;		// (n x k) Kalman gain matrix

public:
  Kalman_Filter(FltN** F, Int n,
		FltN** D, Int m,
		FltN** H, Int k,
		FltN** K,
		FltN* x0)
  : Linear_System(F, n, D, m, H, k, x0) { Init(K); }

  Kalman_Filter(FltN** F, Int n,
		FltN** D, Int m,
		FltN** H, Int k,
		FltN** K,
		FltN* x0,
		String string)
  : Linear_System(F, n, D, m, H, k, x0, string) { Init(K); }

  Kalman_Filter(FltN** F, Int n,
		FltN** D, Int m,
		FltN** H, Int k,
		FltN** K,
		FltN* x0,
		char* string)
  : Linear_System(F, n, D, m, H, k, x0, string) { Init(K); }

  ~Kalman_Filter();
  Void Init(FltN** K);

  STATUS Update();
  STATUS Update(FltN* z); 
  STATUS Update(DWord s) { mask = s; Update(); } 
  STATUS Update(FltN* z, DWord s) { mask = s; Update(z); } 
  Void  Mask(DWord s) { mask = s; }
  DWord Mask() { return mask; }

  inline FltN  InnovationsClip(FltN u);
  inline FltN  State(Int i = 1) { return x[i-1]; }
  inline FltN  Innovation(Int i = 1) { return e[i-1]; }
};



/******************************************************************************
Class   : F I R  F I L T E R                                              
Purpose : Provides FIR filter function of arbitrary order and response
Notes   : If constructor is called without existing input and feedback buffers,
        : it will create its own internal buffers.
******************************************************************************/

class FIR_Filter 
{
public:
  MBool localBuffers;
  Int n;		// filter dimension
  Flt32* b;		// feedforward coefficients
  IO_Buffer* x;		// input buffer

  FIR_Filter();
  FIR_Filter(char* filename);
  FIR_Filter(Flt32* b, Int n);
  FIR_Filter(Flt32* b, Int n, IO_Buffer* x);
  ~FIR_Filter() { delete[] b; if (localBuffers) delete x; }

  virtual Flt32 Compute(Flt32 s) { x->Update(s); return Compute(); }
  virtual Flt32 Compute();
  virtual Flt32 Compute(IO_Buffer* x);
  virtual Void Init() { if (localBuffers) x->Init(); } 
};



/******************************************************************************
Class   : I I R  F I L T E R                                              
Purpose : Provides IIR filter function of arbitrary order and response
Notes   : If constructor is called without existing input and feedback buffers,
        : it will create its own internal buffers.
******************************************************************************/

class IIR_Filter : public FIR_Filter
{
  Int m;		// filter dimension
  Flt32* a;		// feedback coefficients
  IO_Buffer* y;		// output buffer

public:
  IIR_Filter();
  IIR_Filter(Flt32* b, Int n, Flt32* a, Int m);
  IIR_Filter(Flt32* b, Int n, Flt32* a, Int m, IO_Buffer* x, IO_Buffer* y);
  ~IIR_Filter(){ delete[] a; if (localBuffers) delete y; }

  Flt32 Compute(Flt32 s) 
  { x->Update(s); y->Update(Compute()); return y->Output(); }
  Flt32 Compute();
  Flt32 Compute(IO_Buffer* x, IO_Buffer* y);
  Void Init() { if (localBuffers) { y->Init(); x->Init(); } } 
};


#endif