#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <common.h>
#include <mcbspdrv.h>
#include <intr.h>
#include <board.h>
#include <codec.h>
#include <mcbsp.h>
#include <mathf.h>

#define PRINT_DBG 1 
#define FILTERLENGTH 100



short buffer[FILTERLENGTH]; 
short w[FILTERLENGTH];

/*--------------------------------------------------------------------*/
/*      All code, except contents of function serialPortRcvISR(), was     */
/*      provided by J.P. Slavinsky and his Elec 434 group                                 */
/*--------------------------------------------------------------------*/

/*--------------------------------------------------------------------*/
/* FILE LOCAL (STATIC) PROTOTYPES                                                                         */
/*--------------------------------------------------------------------*/

void hookint(void);
interrupt void serialPortRcvISR (void);

/*--------------------------------------------------------------------*/
/*       Main                                                                                             */      
/*--------------------------------------------------------------------*/

main()
{

        Mcbsp_dev dev;
        Mcbsp_config  mcbspConfig;
        int i;
        printf("Got to main!");


/*--------------------------------------------------------------------*/
/* Initialize EVM                                                                                                         */
/*--------------------------------------------------------------------*/


        for(i = 0; i < FILTERLENGTH; i++)
        {
                /* Initialize coefficients to 1's */
                w[i] = 1;
                /* Initialize buffer of previous inputs to 0's */
                buffer[i] = 0;
                /* printf("Wi: %d |",w[i]); */
        }

        evm_init();


/*--------------------------------------------------------------------*/
/* Open MCBSP for subsequent Examples                                                             */
/*--------------------------------------------------------------------*/

        mcbsp_drv_init();

        dev = mcbsp_open(0);

        if (dev == NULL)
        {
                printf("Error opening MCBSP 0    \n  ");
                return(ERROR);
        }


/*--------------------------------------------------------------------*/
/* configure McBSP                                                                                                        */
/*--------------------------------------------------------------------*/

        memset(&mcbspConfig,0,sizeof(mcbspConfig));

        mcbspConfig.loopback              = FALSE;

        mcbspConfig.tx.update             = TRUE;
        mcbspConfig.tx.clock_mode         = CLK_MODE_EXT;
        mcbspConfig.tx.frame_length1      = 0;
        mcbspConfig.tx.word_length1       = WORD_LENGTH_32;

        mcbspConfig.rx.update             = TRUE;
        mcbspConfig.rx.clock_mode         = CLK_MODE_EXT;
        mcbspConfig.rx.frame_length1      = 0;
        mcbspConfig.rx.word_length1       = WORD_LENGTH_32;

        mcbsp_config(dev,&mcbspConfig);

        MCBSP_ENABLE(0, MCBSP_BOTH);

/*--------------------------------------------------------------------*/
/* configure CODEC                                                                                                        */
/*--------------------------------------------------------------------*/

        codec_init();

        /* link codec channels to audio inputs         */
        codec_adc_control(RIGHT,0.0,FALSE,LINE_SEL);
        codec_adc_control(LEFT,0.0,TRUE,MIC_SEL);

        /* mute (L/R)LINE input to mixer              */
        codec_line_in_control(LEFT,MIN_AUX_LINE_GAIN,TRUE);
        codec_line_in_control(RIGHT,MIN_AUX_LINE_GAIN,TRUE);


        /* D/A 0.0 dB atten, do not mute DAC outputs  */
        codec_dac_control(LEFT, 0.0, FALSE);
        codec_dac_control(RIGHT, 0.0, FALSE);

        /*
        sampleRate = 11025;

        actualrate = codec_change_sample_rate(sampleRate, TRUE);
        */

        codec_interrupt_enable();

        hookint();


/*--------------------------------------------------------------------*/
/* Main Loop, wait for Interrupt                                              */
/*--------------------------------------------------------------------*/


        while (1)
        {
        }

        /* mute DAC outputs (unused, as best we can tell)*/

        codec_dac_control(LEFT, 0.0, TRUE);
        codec_dac_control(RIGHT, 0.0, TRUE);


/*--------------------------------------------------------------------*/
/* End Single Block Capture and Playback Example                      */
/*--------------------------------------------------------------------*/

        mcbsp_close(dev);

        return(OK);

}

/*--------------------------------------------------------------------*/
/* FUNCTIONS                                                                                                              */
/*--------------------------------------------------------------------*/

void hookint()
{
        intr_init();
        intr_map(CPU_INT15, ISN_RINT0);
        intr_hook(serialPortRcvISR, CPU_INT15);

        //INTR_ENABLE(1);
        INTR_ENABLE(15);
        INTR_GLOBAL_ENABLE();

        return;
}


/* Our Function - Where all the real work gets done */

interrupt void serialPortRcvISR (void)
{

        int i;
        int accum = 0;       
        int sample_data;

        short chan1;
        short chan2;
        short primary;
        short ref;
   
        short next_output;
        short accumShort;   

        /* read in left and right channels */

        sample_data = MCBSP_READ(0);
                        
        /* Separate samples from left and right channels */
        chan1 = sample_data & 0xffff;
        chan2 = (sample_data & 0xffff0000) >> 16;
    
        primary = chan2;
        ref = chan1;
    

        /* Shift old buffered inputs right one position in array */
        for(i = FILTERLENGTH; i > 0; i--) {
                buffer[i] = buffer[i - 1];
                /*printf("b%d: %d",i,buffer[i]); */
        }
  

        /* Append new input value to front of buffer */
        buffer[0] = sample_data & 0xffff;

   
        /* Take dot product of buffered inputs and filter coefficients */
        for(i=0; i<FILTERLENGTH; i++)
        {    
                accum += (buffer[i] * w[i]);
        }
    
        /* Right shift to convert to Q15 format (what the DSP wants) */
        accumShort = accum >> 16;
   
        /* Find "error" */
        next_output = primary - accumShort;
 

        /* Update coefficients using newest "error" calculation */
        for(i = 0; i < FILTERLENGTH; i++)
        {
                w[i] += (2 * buffer[i] * next_output)>>16;
        }
   
        /* Print to stdout if output sample gets too big */
        if(next_output > 32000)
                printf("\nAck! Out: %d",next_output);

        /* write out result to d/a converter*/
        /* multiply output by 2 to make it louder */
        MCBSP_WRITE(0, 2 * next_output);

        return;

}