#include <stdio.h>
#include <csl.h>
#include <csl_irq.h>


#define FPGA_BASE 	0xA0000000UL		// CE2
#define FPGA_RXSTAT	(FPGA_BASE+0x01)
#define FPGA_TXSTAT (FPGA_BASE+0x01)
#define FPGA_RXFIFO (FPGA_BASE+0x03)
#define FPGA_TXFIFO (FPGA_BASE+0x03)

#define INSFILTER_LEN 32


/*****************************************************************************************************/


static float sin_table[] = {
	0,				// 0
	0.70710678,		// 45
	1,				// 90
	0.70710678,		// 135
	0,				// 180
	-0.70710678,	// 225
	-1,				// 270
	-0.70710678		// 315
};

static float ins_xl[INSFILTER_LEN+1] = {0};
static float ins_xr[INSFILTER_LEN+1] = {0};
static float mbuf[4] = {0};
static float nbuf[4] = {0};

extern far void vectors();



/*****************************************************************************************************/



static short read_fifo()
{
	volatile short *rxstat = (short *)FPGA_RXSTAT;
	volatile short *rxfifo = (short *)FPGA_RXFIFO;
	volatile short *txfifo = (short *)FPGA_TXFIFO;

	while (1) {
		short a = *rxstat;
		printf("rxstat = %4x\n", *rxstat);
		*txfifo = 0x1234;
		printf("rxfifo = %4x\n", *rxfifo);
	}

	return *rxfifo;

}


static void write_fifo(float val)
{
}



/*****************************************************************************************************/



static float do_filter(const int nlen, const float *a, const float *y)
{
        float out = 0;
        int i = 0;

        for (i=0; i<nlen; i++) {
                out += a[i] * y[i];
        }

        return out;
}


static void do_shift(float *buf, const int nlen)
{
        int i = 0;

        for (i=0; i<nlen-1; i++) {
                buf[i] = buf[i+1];
        }
}



static float get_subcarrier()
{
	static int i = 0;
	float val;

	val = sin_table[i*2];

	i++;
	if (i>=4) {
		i = 0;
	}

	return val;
}


static float get_pilot()
{
	static int i = 0;
	float val;

	val = sin_table[i];

	i++;
	if (i>=8) {
		i = 0;
	}

	return val;
}



static short do_insert(float datal, float datar)
{
        static const float a[INSFILTER_LEN+1] = {
           0.0013,    0.0018,    0.0029,    0.0047,    0.0074,    0.0110,    0.0157,    0.0214,    0.0278,    0.0347,
           0.0418,    0.0487,    0.0550,    0.0604,    0.0645,    0.0671,    0.0680,    0.0671,    0.0645,    0.0604,
           0.0550,    0.0487,    0.0418,    0.0347,    0.0278,    0.0214,    0.0157,    0.0110,    0.0074,    0.0047,
           0.0029,    0.0018,    0.0013
        };

        float l;
        float r;
        int i = 0;

        for (i=0; i<4; i++) {
                do_shift(ins_xl, INSFILTER_LEN+1);
                if (i==0) ins_xl[INSFILTER_LEN] = datal;
                else ins_xl[INSFILTER_LEN] = 0;
                do_shift(ins_xr, INSFILTER_LEN+1);
                if (i==0) ins_xr[INSFILTER_LEN] = datar;
                else ins_xr[INSFILTER_LEN] = 0;

                l = do_filter(INSFILTER_LEN+1, a, ins_xl);
                r = do_filter(INSFILTER_LEN+1, a, ins_xr);

                mbuf[i] = r+l;
                nbuf[i] = r-l;
        }
}



void main() {

  /* Initialize the chip support library, must when using CSL */
  CSL_init(); 
  
  IRQ_setVecs(vectors);     /* point to the IRQ vector table	*/
  IRQ_globalEnable();       /* Globally enable interrupts       */ 
  IRQ_nmiEnable();          /* Enable NMI interrupt             */   

  IRQ_map(IRQ_EVT_EXTINT4, 14);
  IRQ_reset(IRQ_EVT_EXTINT4);
  IRQ_enable(IRQ_EVT_EXTINT4);
    

  while(1) {
	float l = 0;
	float r = 0;
	float m = 0;		// L+R
	float n = 0;		// L-R
	float sc = 0;		// subcarrier
	float pilot = 0;	// pilot
	float s = 0;		// N * subcarrier
	float bband = 0;	// base band
	int i = 0;

	l = read_fifo();
	r = read_fifo();
	do_insert(l, r);

	for (i=0; i<4; i++) {
		m = mbuf[i];
		n = nbuf[i];
		sc = get_subcarrier();
		pilot = get_pilot();
		s = n * sc;
		bband = m + s + pilot;
		write_fifo(bband);
	}
  }
  
}



void ExtEventHandler(void)
{
	printf("ExtEventHandler set\n");
}



interrupt void    
c_extint(void)    
{
    ExtEventHandler();
    return;
}