/**********************************************
	 Digital EQ Program
	Coded for the ADSP21020 by 
 Eric Wigforss, Kevin Johns, John Siedlecki
   Digital Signal Processing Design Lab
	    7 April 1995
***********************************************/

#include <21020.h>
#include <signal.h>
#include <macros.h>
#include <math.h>

volatile int in_portl   segment (hip_reg0);
volatile int in_portr   segment (hip_reg1);
volatile int out_portl  segment (hip_reg2);
volatile int out_portr  segment (hip_reg3);
volatile int control_0  segment (hip_reg4);
volatile int control_1  segment (hip_reg5);
volatile int vin_a      segment (adc_a);
volatile int vin_b      segment (adc_b);

void process_input(int);
void timer_isr (int interrupt_number);
void irq2_interrupt(int sig_number);
void calc_coeff(void);

#define fir_size  51
#define fir_half  25
#define sampling_rate  8000

int     current_sample = 0;
float   input_fir[fir_size];
float   coeff[fir_size];
float   c[fir_size];

int     bw = 1000;
int     wn = 1000;

#include "cosine.h"     /* cosine terms in the polyval */
#include "sine.h"       /* sine terms in the polyval */
#include "hamming.h"    /* hamming window */
#include "filter.h"     /* bandpass from 500 to 1.5K Hz */

void main(void)
{
    int i;
    for (i = 0; i < fir_size; i++)    
	input_fir[i] = 0;

    timer_set(6250000, 6250000);
    interrupt(SIG_IRQ3, process_input);
    interrupt(SIG_TMZ0, timer_isr);
    interrupt(SIG_IRQ2, irq2_interrupt);
    timer_on();

    set_flag(SET_FLAG1, CLR_FLAG);  /* turn off led 1 */
    set_flag(SET_FLAG2, CLR_FLAG);

    for (i = 1; i < fir_half; i++)
	c[i-1] = 3.141592 * i; 

    calc_coeff();

    for(;;)                                     /* infinite loop */
	{
	}
}

void timer_isr(int interrupt_number)
{
    set_flag(SET_FLAG1, TGL_FLAG);
}

void process_input(int sig_number)
{
    int i, n;           /* counter variables */
    float out_l = 0.0;  /* output to the left channel */
    float out_r = 0.0;

    /* compute fir filter - starting at sample current_sample */
    input_fir[current_sample] = (float) in_portl;
    n = current_sample;
    for (i = 0; i < fir_size; i++)
	{
	    out_l = out_l + (coeff[n] * input_fir[n]);
	    out_r = out_r + (filtercoeff[n] * input_fir[n]); 
	    n++;
	    if (n == fir_size) 
		n = 0;
	}
    current_sample--;
    if (current_sample < 0)
	current_sample = (fir_size - 1);

    /* output to port */
    out_portl = (int) out_l;   /* left */
    out_portr = (int) out_r;   /* right */
}

void irq2_interrupt (int sig_number)
{
    control_0 = 0x2400; /* set to line-level input */
    /* control_1 = 0x000D;*/  /* set to 44.1 KHz sampling */
}

void calc_coeff (void) 
{
    float fl = ((wn - bw / 2) / sampling_rate) / 2;
    float fh = ((wn + bw / 2) / sampling_rate) / 2;
/*    float c0 = (fl * 2) + (fh * 2); */
    float c1 = fh - fl;
    float c2 = fh + fl;
    int i, i1, i2;
    float c3, c4;
    float ct = 0, st = 0;

    coeff[fir_half] = 2 * c1;

    for (i = 0; i < fir_half; i++)
	{
	    i1 = fir_half - i - 1;
	    i2 = fir_half + i + 1;

	    c3 = c[i] * c1;
	    c4 = c[i] * c2;
	    coeff[i1] = 2 * sinf(c3) * cosf(c4) / c[i];
	    coeff[i1] = coeff[i1] * hamming[i1];
	    coeff[i2] = coeff[i1];
	}

    /* calculate gain */
/*    for (i = 0; i < fir_size; i++)
	{
	    i1 = 50 - i;
	    ct = ct + (coeff[i] * cosine[i1]);
	    st = st + (coeff[i] * sine[i1]);
	}
    c1 = sqrtf((ct * ct) + (st * st));
*/    c1 = 0.9852;

    for (i = 0; i < fir_size; i++)
	coeff[i] = coeff[i] / c1;
}