// sosfilt.cc -- Second order IIR filtering

// This file may be used under the terms defined by the GNU 
// General Public License [write to the Free Software Foundation, 
// 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA]
//
// WITHOUT ANY WARRANTY; without even the implied warranty 
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
//
// Copyright (C) 2008 Eric Chassande-Mottin, CNRS (France)

//

#include <octave/config.h>

#include <iostream>
#include <sstream>
using namespace std;
#include <string>

#include <octave/defun-dld.h>
#include <octave/error.h>
#include <octave/oct-obj.h>
#include <octave/pager.h>
#include <octave/symtab.h>
#include <octave/variables.h>

#include <vector>
using namespace std;


DEFUN_DLD (sosfilt, args,,
  "Second order IIR filtering\n")
{
  octave_value_list retval;
  
  int nargin = args.length ();
  
  if (nargin < 2)
    {
      error("Usage:  y = sosfilt(sos,x)");
      return retval;
    }

  Matrix sos( args(0).matrix_value() );

  ColumnVector x( args(1).vector_value() );
  int n=x.length();

  if (sos.columns()!=6)
    {
      error("Second-order matrix must be a non-empty Lx6 matrix");
      return retval;
    }

  ColumnVector y(n,0.0);
  
  for (int j=0; j<sos.rows(); j++)
    {

      double v0=0.0, v1=0.0, v2=0.0;

      double b0=sos(j,0);
      double b1=sos(j,1);
      double b2=sos(j,2);
      double a1=sos(j,4);
      double a2=sos(j,5);

      for (int i=0; i<n; i++)
	{
	  v0=x(i)-a1*v1-a2*v2;
	  y(i)=b0*v0+b1*v1+b2*v2;
	  v2=v1;
	  v1=v0;
	}

      x=y;

    }

  retval(0)=y;
  
  return retval;
}