/* Adaptive Filter Program
   Sridhar Arunachalam          10/26/95
*/

#include <21020.h>      /* for idle and timer           */
#include <signal.h>     /* for interrupt and set_flag   */
#include <macros.h>     /* for segment                  */

volatile int in_portl   segment (hip_reg0); /* input signal     */
volatile int in_portr   segment (hip_reg1); /* reference noise  */
volatile int out_portl  segment (hip_reg2); /* output of filter */
volatile int out_portr  segment (hip_reg3); /* not used         */

void process_input(int);
void timer_isr(int interrupt_number);
void irq2_isr(int sig_number);

int run_adapt=0;
int w_tap[12];
int z_delay[12];
int count;
float u=0.000001;

void main(void)
{
    for(count=0;count<12;count++)       /* init tap weights and delays */
        { w_tap[count]=0;
          z_delay[count]=0;
        }

    set_flag(SET_FLAG1, CLR_FLAG);      /* clear all LEDs       */
    set_flag(SET_FLAG2, CLR_FLAG);
    set_flag(SET_FLAG3, CLR_FLAG);
    interrupt(SIG_IRQ3, process_input); /* set interrupt vectors*/
    interrupt(SIG_TMZ0, timer_isr);
    interrupt(SIG_IRQ2, irq2_isr);
    timer_set(50000,50000);           /* set timer            */
    timer_on();

    do
    {
    idle();
    } while(1);
}

void timer_isr(int interrupt_number)
{
    set_flag(SET_FLAG1, TGL_FLAG);      /* toggle LED 1         */
}

void process_input(int sig_number)
{int sum;
 int count;

    if (run_adapt==0)
       {out_portl=in_portl;             /* send each input channel directly*/
        out_portr=in_portr;             /* to corresponding output         */
       }
    else
       {
       z_delay[0]=in_portr;             /* get noise ref                   */
       sum=in_portl;                    /* calculate output signal         */
       count=1;
       do
       {
        sum=sum-(z_delay[count]*w_tap[count]);
        count++;
       }while (count<12);

       out_portl=sum;                   /* output filtered signal          */
       
       out_portr=in_portl;  /*DEBUG*/
                          
       count=1;                         /* calculate new filter taps       */
       do                               /* using widrow-hoff algorithm     */
       {
        w_tap[count]=w_tap[count]+2*u*sum*z_delay[count];
        count++;
       } while(count<12);

       count=11;                        /* move memory window of noise ref */
       do
       {
        z_delay[count]=z_delay[count-1];
        count=count-1;
       }while(count>0);

       }
}

void irq2_isr(int sig_number)
{
    if (run_adapt==0)                   /* toggle adaptive algorithm */
       {run_adapt=1;
        set_flag(SET_FLAG2, SET_FLAG);
       }
    else 
       {run_adapt=0;
        set_flag(SET_FLAG2, SET_FLAG);
       }
}