/* This Module performs a 4 Pole clutter filter on the Binet i, and q
   data buffers. The filter coefficients and gain can be modified to
   suit the application. The filter consists of two cascaded second order
   biquad IIR stages in Transpose form.
*/
   
#define EXTERN  extern
#include        "define.h"


c_filter()
   {
   register float               R0,R1,R2,R3,R4,R6;
   register int                 AR1;
   volatile register int        *AR0;
   register float               *AR2,*AR4,*AR5,*AR6;
   volatile register int        gatecount;

   AR6 = IQPTR;               /* size of array =  GATES * 2 */

   AR4 = DELAY_PTR;     /* size of array = GATES * 8 */
   AR5 = DELAY_PTR;

   AR0 = FIFO;
   AR1 = 16;

/*   
   for(gatecount=0; gatecount<GATES*2; gatecount++)
*/      
asm("         LDI      249,RC");       /* this line must match GATES * 2 - 1 */
asm("         RPTB    ENDOFLOOP"); 

      {
      AR2 = COEFF;
	      
      R6  = *AR6;

      R0 = *AR2++ * R6; /* n(0) * x(i) -> r0 */
      R1 = *AR2++ * R6; /* n(1) * x(i) -> r1 */
      R2 = R0 + *AR4++; /* n(0) * x(i) + s1(i-1) -> r2 */
      R0 = *AR2++ * R2; /* d(1) * y(i) -> r0 */
      R3 = R1 + *AR4++; /* n(1) * x(i) + s2(i-1) -> r3 */
      R4 = *AR2++ * R6; /* n(2) * x(i) -> r4 */
      R1 = *AR2++ * R2; /* d(2) * y(i) -> r1 */
      R0 = R3 - R0;     /* w(1) - d(1) * y(i) -> r0 */
      R1 = R4 - R1;     /* w(2) - d(2) * y(i) -> r1 */
      *AR5++ = R0;
      *AR5++ = R1;
      
      R6 = R2;

      R0 = *AR2++ * R6; /* n(0) * x(i) -> r0 */
      R1 = *AR2++ * R6; /* n(1) * x(i) -> r1 */
      R2 = R0 + *AR4++; /* n(0) * x(i) + s1(i-1) -> r2 */
      R0 = *AR2++ * R2; /* d(1) * y(i) -> r0 */
      R3 = R1 + *AR4++; /* n(1) * x(i) + s2(i-1) -> r3 */
      R4 = *AR2++ * R6; /* n(2) * x(i) -> r4 */
      R1 = *AR2++ * R2; /* d(2) * y(i) -> r1 */
      *AR6++ = R2;
      R0 = R3 - R0;     /* w(1) - d(1) * y(i) -> r0 */
      R1 = R4 - R1;     /* w(2) - d(2) * y(i) -> r1 */
      *AR5++ = R0;
      *AR5++ = R1;
      }
asm("ENDOFLOOP:");
   
   }