
/*****************************************************************************/
/*                                                                           */
/* PROJECT                                                                   */
/*   Low Power Operation                                                     */
/*                                                                           */
/* FILENAME                                                                  */
/* 	 main.c                                                                  */
/*                                                                           */
/* DESCRIPTION                                                               */
/*   TMS320C5515 USB Stick Application 20 Low Power Operation                */  
/*   Alter the power taken by the TMS320C5515 USB stick by changing the      */
/*   frequency of the phase lock loop (PLL).                                 */
/*   Brightness of XF reflects the power taken by the processor.             */
/*                                                                           */
/* REVISION                                                                  */
/*   Revision: 1.00	                                                         */
/*   Author  : Richard Sikora                                                */
/*---------------------------------------------------------------------------*/
/*                                                                           */
/* HISTORY                                                                   */
/*                                                                           */
/*   Revision 1.00                                                           */
/*   5th March 2010. Original template code from Spectrum Digital.           */
/*                                                                           */
/*****************************************************************************/
/*
 * Copyright (C) 2010 Texas Instruments Incorporated - http://www.ti.com/ 
 * 
 * 
 *  Redistribution and use in source and binary forms, with or without 
 *  modification, are permitted provided that the following conditions 
 *  are met:
 *
 *    Redistributions of source code must retain the above copyright 
 *    notice, this list of conditions and the following disclaimer.
 *
 *    Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the 
 *    documentation and/or other materials provided with the   
 *    distribution.
 *
 *    Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 *  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 
 *  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 
 *  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 *  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 *  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/


#include "stdio.h"
#include "usbstk5515.h"
#include "usbstk5515_led.h"
#include "aic3204.h"
#include "PLL.h"
#include "stereo.h"
#include "oled.h"
#include "pushbuttons.h"

#include "Dsplib.h"
#include "IIR.h"

signed int wspLP_lib[6] = {0, 0, 0, 0, 0, 0 };
                                       
signed int x1[3] = { 0, 0, 0 };  // próbki wejściowe
signed int y1[3] = { 0, 0, 0 };  // próbki wyjściowe   


#define N_MAX 300
#define SAMPLES_PER_SECOND 48000
#define GAIN_IN_dB         0
#define MAX_COUNT 64

extern Int16 counter1; /* Counter from AIC3204 initialization */

/* ------------------------------------------------------------------------ *
 *                                                                          *
 *  main( )                                                                 *
 *                                                                          *
 * ------------------------------------------------------------------------ */
void main( void )
{
    ushort  N = 200;

	DATA bufor1[N_MAX + 2];
	DATA bufor_FIR[N_MAX + 2];
	DATA H_1[N_MAX];
	
	FILE   *plik;
	
	int para = 0;
	
	
	DATA x[2];
	DATA r[2];
	DATA des[2];
	
	DATA krok = 300;
	
	Int16 left_input;
	Int16 right_input;
	Int16 left_output;
	Int16 right_output;
	
	Int16 old_input;
		
	unsigned long int i = 0;
	
	short i0;
		
	unsigned int step = 0;
	unsigned int last_step = 99; /* Some out of range value */
      
    
    LDATA energia;
    DATA moc;
    
    
    /* Initialize BSL */
    USBSTK5515_init( );
    
    pll_frequency_setup(100); // 100
    
    /* Initialise hardware interface and I2C for code */
    aic3204_hardware_init();
    
    /* Initialise the AIC3204 codec */
	aic3204_init(); 
	
	/* Turn off the 4 coloured LEDs */
	USBSTK5515_ULED_init();
	
	/* Initialise the OLED LCD display */
	oled_init();
	SAR_init();
	/* Flush display buffer */
    oled_display_message("                 ", "                   ");


    printf("Filtry adaptacyjne\n");

	/* Set sampling frequency in Hz and ADC gain in dB */
    set_sampling_frequency_and_gain(SAMPLES_PER_SECOND, GAIN_IN_dB);

    oled_display_message("Laboratorium FCiPS ", "Filtry adaptacyjne ");

	puts("1: Transmisja sygnału z wejścia na wyjście");
	puts("2: Predykcja liniowa");
	puts("3: Zapis w m-pliku współczynników filtru");
	puts("4: Predykcja liniowa NLMS z pomiarem mocy");
	puts("5: Predykcja liniowa NLMS z filtrem LP");
	puts("6: Zapis w m-pliku współczynników filtru");
	puts("6: Maksymalny rząd filtru = 300");

    asm(" bclr XF"); 
   	
 	for(;;)
 	{
     aic3204_codec_read(&left_input, &right_input); // Configured for one interrupt per two channels.
     step = pushbuttons_read(6); // 4 Different PLL settings

     switch (step)
      {      
       case 1:
		 /* Zwarcie */ 
		 
		  if ( step != last_step)
          {
            oled_display_message("1: zwarcie         ", " do zerowania      ");
            puts("1: - > Transmisja sygnału z wejścia na wyjście");           
          }
		 
		 right_output = right_input;		 		 
		 left_output =  left_input;

       break; 
       
       case 2:
      	if ( step != last_step)
         {
	            oled_display_message("L: FIR 1            ", "P: FIR 2            ");
	            puts("2: Predykcja liniowa z regulowanym krokiem");
	            puts("   Maksymalny rząd filtru = 300");
	            puts("   x -> left input");
	            puts("   left output -> y,  right output -> x \n");
	            
	            para = 0;            
	            for (i0 = 0; i0 < N+2; i0++) bufor1[i0] = 0; // clear delay buffer (a must)
	            for (i0 = 0; i0 < N; i0++) H_1[i0] = 0;
	            
	            printf("Krok korekcji = ");
	    		scanf("%d", &krok);
	    		
	    		printf("Rząd filtru N = ");
	    		scanf("%d", &N);
	    		printf("----------> Parametry przyjęte i filtr jest uruchomiony\n");
	          }	
	           					
			if(para == 0)
			{
				x[0]   = old_input;
				des[0] = left_input;
				right_output = des[1];
				left_output = r[1];
				para = 1;				
			}
			else
			{
				x[1]   = old_input;
				des[1] = left_input;
				right_output = des[0];				
				dlms(x,  H_1, r, des,  bufor1, krok, N, 2 );										
				left_output = r[0];
				para = 0;					
			}
			
			old_input = left_input;   
                                                   
       break;
       
        case 3:
       if ( step != last_step )
          {
           oled_display_message("Zwarcie              ", "FP = 48 KHZ           ");
           printf("\n3. Zapisanie w m-pliku chwilowej odpowiedzi imulusowej filtru\n");
           printf("   M-plik  chwilowej odpowiedzi imulusowej filtru filtr.m jest w katalogu mpiki\n");
           printf("   Filtr o tych współczynnikach w kanale prawym\n");
           printf("   Kanał lewy transmisja sygnału\n");
           printf("\nKrok korekcji poprzedniego filtru = %d\n", krok);          
           
			if ((plik = fopen("..\\mpliki\\filtr.m", "w"))== NULL)
			{
			    printf("Nie można otworzyć pliku\n");
			    exit(0);
			}
			fprintf(plik, "b = ["); 
			
			for (i=0; i<N; i++)
			    fprintf(plik, "%d\n", H_1[i]);          // zapis odpowiedzi impulsowej w pliku  			    

			fprintf(plik, "];\n b=b./2^15;\nfvtool(b)"); 			
			fclose(plik);

           //last_step = step;
          }	
         left_output =  left_input;
	     fir(&right_input, H_1, &right_output, bufor_FIR, 1, N);
				                                                  
       break;  
              
       case 4:
       if ( step != last_step)
         {
            oled_display_message("L: FIR 1            ", "P: FIR 2            ");
            puts("4: Predykcja liniowa znormalizowany");
            puts("   Maksymalny rząd filtru = 300");
            puts("   x -> left input");
            puts("   left output -> y,  right output -> x \n");
            
            para = 0;            
            for (i0 = 0; i0 < N+2; i0++) bufor1[i0] = 0; // clear delay buffer (a must)
            for (i0 = 0; i0 < N; i0++) H_1[i0] = 0;
            
            printf("Krok korekcji poprzedniego filtru = %d\n", krok);
    		//scanf("%d", &krok);
    		
    		printf("Rząd filtru N = ");
    		scanf("%d", &N);
    		printf("----------> Parametry przyjęte i filtr jest uruchomiony\n");
    		
    		//N = 50;
    		energia = 0L;
    		old_input = 0;

          }	
           				
		if(para == 0)
		{
			x[0]   = old_input;
			des[0] = left_input;
			right_output = des[1];
			left_output = r[1];
			para = 1;
			
			//power(&bufor1[1], &energia, N);
			//moc = energia / N;
			
			//krok = (5L << 16) / (5 + moc);
			
			energia += _smpy(old_input, old_input);;
		
			if( bufor1[0] == 0 )
			{ 
				energia -= _smpy(bufor1[N], bufor1[N]);
				energia -= _smpy(bufor1[N + 1], bufor1[N + 1]);
			}
				else
				{
				
				if( bufor1[0] == 1 )
				{ 
					energia -= _smpy(bufor1[1], bufor1[1]);
					energia -= _smpy(bufor1[N + 1], bufor1[N + 1]);
				}
				
					else
					{ 
						energia -= _smpy(bufor1[bufor1[0] - 1], bufor1[bufor1[0] - 1 ]) ;
						energia -= _smpy(bufor1[bufor1[0]], bufor1[bufor1[0]]);
					}
				}
		}
		else
		//if(para == 1)
		{
			x[1]   = old_input;
			des[1] = left_input;
			right_output = des[0];
			
			energia += _smpy(old_input, old_input);			
			moc = energia / N;
			
			//krok = (1L << 16) / (5 + moc);
			krok = 0x02ff / (1 + moc);
			if (krok > 6000) krok = 6000;
			if (krok < 2) krok = 2;
			
			//if(krok < 5) krok = 5;
			
			dlms(x,  H_1, r, des,  bufor1, krok, N, 2 );
									
			left_output = r[0];
			para = 0;
				
		}
		
		old_input = left_input; 
				                                
       break;  
       
       case 5:
		 if ( step != last_step)
         {
            oled_display_message("L: FIR 1            ", "P: FIR 2            ");
            puts("5: Predykcja liniowa z regulowanym krokiem LP");
            puts("   Maksymalny rząd filtru = 300"); 
            puts("   x -> left input");
            puts("   left output -> y,  right output -> x \n");
            
            para = 0;            
            for (i0 = 0; i0 < N+2; i0++) bufor1[i0] = 0; // clear delay buffer (a must)
            for (i0 = 0; i0 < N; i0++) H_1[i0] = 0;
            
             printf("Krok korekcji poprzedniego filtru = %d\n", krok);
    		//scanf("%d", &krok);
    		
    		printf("Rząd filtru N = ");
    		scanf("%d", &N);
    		printf("----------> Parametry przyjęte i filtr jest uruchomiony\n");
    		
    		krok = 5;
    		moc = 0;
    		
    		wsp_LP(50, SAMPLES_PER_SECOND, wspLP_lib);

          }	
           		
		
		moc  = IIR_I_order(x1, y1, wspLP_lib, _smpy(left_input, left_input));
		
		if(moc == 0) krok = 30000; 
			else     krok = 0x7fff / moc;
		
		
		//moc  = IIR_I_order(x1, y1, wspLP_lib, _abss(left_input));
		//krok = 300000L / (1 + moc);
		
		
		if (krok < 3) krok = 3;
		
		if(para == 0)
		{
			x[0]   = old_input;
			des[0] = left_input;
			right_output = des[1];
			left_output = r[1];
			para = 1;
			
			
		}
		else
		//if(para == 1)
		{
			x[1]   = old_input;
			des[1] = left_input;
			right_output = des[0];
			
			dlms(x,  H_1, r, des,  bufor1, krok, N, 2 );
						
			
			left_output = r[0];
			para = 0;
				
		}
		
		old_input = left_input;   
         
       break; 
       
       case 6:
       if ( step != last_step )
          {
           oled_display_message("Zwarcie              ", "FP = 48 KHZ           ");
           printf("\n6. Zapisanie w m-pliku chwilowej odpowiedzi imulusowej filtru\n");
           printf("   M-plik  chwilowej odpowiedzi imulusowej filtru filtr.m jest w katalogu mpiki\n");
           printf("   Filtr o tych współczynnikach w kanale prawym\n");
           printf("   Kanał lewy transmisja sygnału\n");
           printf("\nKrok korekcji poprzedniego filtru = %d\n", krok);          
           
			if ((plik = fopen("..\\mpliki\\filtr.m", "w"))== NULL)
			{
			    printf("Nie można otworzyć pliku\n");
			    exit(0);
			}
			fprintf(plik, "b = ["); 
			
			for (i=0; i<N; i++)
			    fprintf(plik, "%d\n", H_1[i]);          // zapis odpowiedzi impulsowej w pliku  			    

			fprintf(plik, "];\n b=b./2^15;\nfvtool(b)"); 			
			fclose(plik);

           //last_step = step;
          }	
         left_output =  left_input;
	     fir(&right_input, H_1, &right_output, bufor_FIR, 1, N);
				                                                  
       break;  
       
      }
     
   //  LED_PWM(); /* LED on bright for 120 MHz PLL. Dull for 1 MHz PLL. */

     last_step = step; 
      
     aic3204_codec_write(left_output, right_output);
 	}

   /* Disable I2S and put codec into reset */ 
    aic3204_disable();

    printf( "\n***Program has Terminated***\n" );
    SW_BREAKPOINT;
}

/* ------------------------------------------------------------------------ *
 *                                                                          *
 *  End of main1.c                                                          *
 *                                                                          *
 * ------------------------------------------------------------------------ */

