#include "stdio.h"
#include "usbstk5505.h"
#include "math.h"
#include "Dsplib.h"
extern Int16 AIC3204_rset( Uint16 regnum, Uint16 regval);
#define XmitL 0x10
#define XmitR 0x20
#define RcvR 0x08
#define RcvL 0x04
#define pi 3.1415926535897932384626433832795028841971693993751
#define nfilt 40

void main( void )
{
	void *za_obradu, *obrada, *pom;
    Int16 i, j, p, whichfilt, freq;
    Int16 sample, data4;
    float novi_desni[1200], za_obraditi_desni[1200][100], pomocna[1200], mfcc[100][13];		//wlen=0.0256, upperf i lowerf 3 x veci
    float ncep=13,lowerf=400, upperf=20566.4928, samprate=48000, nfft=256, win[1200], dfreq;
    float filters[1200][40], melmax, melmin, dmelbw, arange[42], filt_edge[42], leftfr, centerfr, rightfr, fwidth, height, leftslope, rightslope;
    int S, time= 1, wlen=1200;
    
    /* Initialize BSL */
    USBSTK5505_init( );
	printf("EXBUSSEL = %02x\n", SYS_EXBUSSEL);
    
    /* izvodenje*/
    printf("izvodenje je pocelo");
    
    /* program MFCC */
  
    /* Configure Serial Bus */
    SYS_EXBUSSEL |= 0x0100;  // Configure Serial bus 0 for I2S0
 
     
    /* Configure AIC3204 */
    AIC3204_rset( 0, 0 );      // Select page 0
    AIC3204_rset( 1, 1 );      // Reset codec
    AIC3204_rset( 0, 1 );      // Select page 1
    AIC3204_rset( 1, 8 );      // Disable crude AVDD generation from DVDD
    AIC3204_rset( 2, 1 );      // Enable Analog Blocks, use LDO power
    AIC3204_rset( 0, 0 );
    /* PLL and Clocks config and Power Up  */
    AIC3204_rset( 27, 0x0d );  // BCLK and WCLK are set as o/p; AIC3204(Master)
    AIC3204_rset( 28, 0x00 );  // Data ofset = 0
    AIC3204_rset( 4, 3 );      // PLL setting: PLLCLK <- MCLK, CODEC_CLKIN <-PLL CLK
    AIC3204_rset( 6, 7 );      // PLL setting: J=7
    AIC3204_rset( 7, 0x06 );   // PLL setting: HI_BYTE(D=1680)
    AIC3204_rset( 8, 0x90 );   // PLL setting: LO_BYTE(D=1680)
    AIC3204_rset( 30, 0x88 );  // For 32 bit clocks per frame in Master mode ONLY
                               // BCLK=DAC_CLK/N =(12288000/8) = 1.536MHz = 32*fs
    AIC3204_rset( 5, 0x91 );   // PLL setting: Power up PLL, P=1 and R=1
    AIC3204_rset( 13, 0 );     // Hi_Byte(DOSR) for DOSR = 128 decimal or 0x0080 DAC oversamppling
    AIC3204_rset( 14, 0x80 );  // Lo_Byte(DOSR) for DOSR = 128 decimal or 0x0080
    AIC3204_rset( 20, 0x80 );  // AOSR for AOSR = 128 decimal or 0x0080 for decimation filters 1 to 6
    AIC3204_rset( 11, 0x82 );  // Power up NDAC and set NDAC value to 2
    AIC3204_rset( 12, 0x87 );  // Power up MDAC and set MDAC value to 7
    AIC3204_rset( 18, 0x87 );  // Power up NADC and set NADC value to 7
    AIC3204_rset( 19, 0x82 );  // Power up MADC and set MADC value to 2
    /* DAC ROUTING and Power Up */
    AIC3204_rset( 0, 1 );      // Select page 1
    AIC3204_rset( 0x0c, 8 );   // LDAC AFIR routed to HPL
    AIC3204_rset( 0x0d, 8 );   // RDAC AFIR routed to HPR
    AIC3204_rset( 0, 0 );      // Select page 0
    AIC3204_rset( 64, 2 );     // Left vol=right vol
    AIC3204_rset( 65, 0 );     // Left DAC gain to 0dB VOL; Right tracks Left
    AIC3204_rset( 63, 0xd4 );  // Power up left,right data paths and set channel
    AIC3204_rset( 0, 1 );      // Select page 1
    AIC3204_rset( 0x10, 00 );  // Unmute HPL , 0dB gain
    AIC3204_rset( 0x11, 00 );  // Unmute HPR , 0dB gain
    AIC3204_rset( 9, 0x30 );   // Power up HPL,HPR
    AIC3204_rset( 0, 0 );      // Select page 0
    USBSTK5505_wait( 100 );    // wait
    /* ADC ROUTING and Power Up */
    AIC3204_rset( 0, 1 );      // Select page 1
    AIC3204_rset( 0x34, 0x30 );// STEREO 1 Jack
		                       // IN2_L to LADC_P through 40 kohm
    AIC3204_rset( 0x37, 0x30 );// IN2_R to RADC_P through 40 kohmm
    AIC3204_rset( 0x36, 3 );   // CM_1 (common mode) to LADC_M through 40 kohm
    AIC3204_rset( 0x39, 0xc0 );// CM_1 (common mode) to RADC_M through 40 kohm
    AIC3204_rset( 0x3b, 0 );   // MIC_PGA_L unmute
    AIC3204_rset( 0x3c, 0 );   // MIC_PGA_R unmute
    AIC3204_rset( 0, 0 );      // Select page 0
    AIC3204_rset( 0x51, 0xc0 );// Powerup Left and Right ADC
    AIC3204_rset( 0x52, 0 );   // Unmute Left and Right ADC
    
    
    AIC3204_rset( 0, 0 );    
    USBSTK5505_wait( 100 );  // Wait
    /* I2S settings */
    I2S0_SRGR = 0x0;     
    I2S0_CR = 0x8010;    // 16-bit word, slave, enable I2C
    I2S0_ICMR = 0x3f;    // Enable interrupts
    /* Play Tone */
	
	//Postavljanje elemenata matrica na 0
	for ( sample = 0 ; sample < 1200 ; sample++ ){
            	novi_desni[sample]=0;
            	pomocna[sample]=0;
    	for ( i = 0 ; i < 100 ; i++ ){
            za_obraditi_desni[sample][i]=0;
    	}
    }	
            
//Ono sa neta
           //Matrica Mel filtara
           for(i=0 ; i < 1200 ; i++){
           		for(j=0 ; j < 40 ; j++){
           		filters[i][j]=0;      //inicijalizacija na nule
           		}
           }
           
           dfreq = samprate / nfft;	 // nfft je sto je bio N u MATLAB-u															//Ovo sam kopirao samo reda radi,
/*           if (upperf > samprate/2){	// upperf i lowerf se moraju modificirati pošto je sad samprate 3 x veci pa moraju i oni biti														//jer ce uvjet nece nikada biti ispunjen
           	printf("\nGornja frekvencija %f je veca od Nyquistove %f" , upperf, samprate/2);	//
           }*/   //nemoeš printat nesto na screen ako nema screena
	
			melmax = 2595 * log10(1 + upperf / 700);
			melmin = 2595 * log10(1 + lowerf / 700);
			dmelbw = (melmax - melmin) / (nfilt + 1);
			
			//Rubovi filtara u Hz
			for(i=0 ; i < 42 ; i++){
				arange[i]=i;
			}
			for(i=0 ; i < 42 ; i++){
			filt_edge[i]=700. * (pow(10., (melmin+dmelbw*arange[i]) / 2595.) - 1.);
			}
			for(whichfilt=0 ; whichfilt < nfilt ; whichfilt++){
				//Filter trokuti u DFT tockama
				leftfr = filt_edge[whichfilt]/dfreq;
				centerfr = _nround(filt_edge[whichfilt + 1] / dfreq);
				rightfr = _nround(filt_edge[whichfilt + 2] / dfreq);
				fwidth = (rightfr - leftfr) * dfreq;
				height = 2. / fwidth;
				if (centerfr != leftfr) 
                  leftslope = height / (centerfr - leftfr); 
              	else
                  leftslope = 0;
				freq = leftfr + 1;
				while (freq < centerfr){ 
                   filters[freq][whichfilt] = (freq - leftfr) * leftslope; 
                   freq = freq + 1;
				}
				if (freq == centerfr){ // This is always true 
                   filters[freq][whichfilt] = height; 
                   freq = freq + 1;
				}
				if (centerfr != rightfr){ 
                   rightslope = height / (centerfr - rightfr);
				}
				while (freq < rightfr){ 
                   filters[freq][whichfilt] = (freq - rightfr) * rightslope; 
                   freq = freq + 1;
				}				
			}

	//Snimanje i spremanje u matricu
	//for(i=0 ; i < 5 ; i++)
	while(1)
	{
		for(i=0 ; i < time ; i++) // snimanje naredbe 1 sec
		{
            for ( sample = 0 ; sample < samprate ; sample++ )
            {
				/* Read Digital audio input */
      	        while((RcvR & I2S0_IR) == 0);  // Wait for receive interrupt to be pending
//                data3 = I2S0_W0_MSW_R;  // 16 bit left channel received audio data
      	        data4 = I2S0_W1_MSW_R;  // 16 bit right channel received audio data

				novi_desni[sample]=data4;

				/* Write Digital audio input */
                while((XmitR & I2S0_IR) == 0); // Wait for transmit interrupt to be pending
//				I2S0_W0_MSW_W = data3;  // 16 bit left channel transmit audio data
      	        I2S0_W1_MSW_W = data4;  // 16 bit right channel transmit audio data

            }
		}
//ne posto se snima blok od 1 sec.        
/*            //stavljanje zadnjih 10 ms sa prethodnog
            for ( sample = 720 ; sample < 1200 ; sample++ )
            {
            	za_obraditi_desni[sample-720]=stari_desni[sample];
            }
           
            //stavljanje prvih 15 ms sa novog
            for ( sample = 0 ; sample < 720 ; sample++ )
            {
            	za_obraditi_desni[sample+720]=novi_desni[sample];
            }
            
            //kopiranje novi_desni-->stari_desni
            for ( sample = 0 ; sample < 1200 ; sample++ )
            {
            	stari_desni[sample]=novi_desni[sample];
            }*/
            
           
           	S=0; //brojac dijelova audio signala
   			for (j=0; j <(samprate*time) ; j=j+480)
    		{
    			for(i=0; i<1200 ; i++){
    				za_obraditi_desni[i][S]=novi_desni[j+i];
    			}
        		S=S++;
            }
			S=S--;
			za_obradu = &za_obraditi_desni[0][0];
			
		
           //Izgradnja Hammingovog prozora
 //          wlen = wlen * samprate; //fali (int)
           
           for (i=0 ; i < wlen ; i++){ // wlen =1200
           		win[i]=0.54-0.46*pi*cos(2*(i-1)/(wlen-1));
           }
           
           //Množenje uzoraka sa Hammingovim prozorom
           for (i = 0 ; i < S ; i++){
           	for (j = 0 ; j < wlen ; j++){
           		za_obraditi_desni[j][i]=za_obraditi_desni[j][i]*win[i];
           	}
           }
// ovdje bio komad koda za filtere racunat sad je prije while petlje
			
			//Izrada DCT matrice
			//možda najbolje iz onog naseg iz MATLAB-a pod pretpostavkom da je matrica filtera ovdje filters
			
			// FFT
			rfft(za_obradu, nfft, NOSCALE);

			//apsolutna vrijednost i kvadrat svakog elementa matrice
			for(i=0; i < S; i++){
				for(j=0; j < wlen; j++){
					 za_obraditi_desni[i][j] = pow(fabs(za_obraditi_desni[i][j]),2);
				} // koristim iste 2 matrice za cijelu obradu zbog ogranicenja memorije na MCU
			}
			// mnozenje spektra snage sa svakim trokutastim filtrom zasebno
			for(j=0; j < S; j++){
    			for(i=0; i < nfilt; i++){
    				for(p=0; p < wlen; p++){
    					//ove dve matrice monozit u petlji
        				pomocna[p] = pomocna[p]+(za_obraditi_desni[j][p]*filters[i][p]);
    				}
    			}
   // vidi dal ces tu koristiti pointere ili bas te matrice - MATRICE??!!
    			for(p=0; p < wlen; p++){
 	  				za_obraditi_desni[j][p]= pomocna[p];
    				pomocna[p] = 0;
 	  			}
			}
			
			// Diskretna kosinusna transformacija DCT
			for(i=0; i < S; i++){
				for(j=0; j < ncep; j++){
					mfcc[i][j] = 0;
				}
			}
			pom=&pomocna[0];
			for(j=0; j < S; j++){
				obrada=&za_obraditi_desni[j][0];
				logn(obrada,pom,wlen);
    			for(p=0; p < ncep; p++){
        			for(i=0; i < wlen; i++){
            			if(za_obraditi_desni[i][j] != 0){
            				mfcc[p][j]=mfcc[p][j] +pomocna[i] * cos(pi*(p-1)/2/wlen*(2*(i-1)+1));
            			}
    			    }
   			 	}
			}
	}
	  
}