#include "stdio.h"
#include "usbstk5505.h"
#include "aic3204.h"
#include "PLL.h"
#include "stereo.h"
#include "Dsplib.h"
#include "math.h"

Int16 left_input;
Int16 right_input;
Int16 left_output;
Int16 right_output;
Int16 mono_input;


#define SAMPLES_PER_SECOND 16000
#define pi 3.1415926535897932384626433832795028841971693993751
#define nfilt 40

void *za_obradu, *obrada, *pom;
Uint16 i, j, p, whichfilt, freq,sample, S;
float ham1=0.54, ham2=0.46;
float novi_desni[4000], pomocna[400], mfcc[23][13],za_obraditi_desni[400][23];
float ncep=13,lowerf=133.3333, upperf=6855.4976, wlen=400, melmax, melmin, dmelbw, samprate=16000, nfft=256, dfreq, leftfr, centerfr, rightfr, fwidth, height, leftslope, rightslope;
float filters[400][40], win[400],  arange[42], filt_edge[42];
double time= 1;


/* ------------------------------------------------------------------------ *
 *                                                                          *
 *  main( )                                                                 *
 *                                                                          *
 * ------------------------------------------------------------------------ */
void main( void ) 
{
    /* Initialize BSL */
    USBSTK5505_init( );
	
	/* Initialize PLL */
	pll_frequency_setup(100);

    /* Initialise hardware interface and I2C for code */
    aic3204_hardware_init();
    
    /* Initialise the AIC3204 codec */
	aic3204_init(); 

    /* izvodenje*/
    printf("izvodenje je pocelo \n");
	
	/* Setup sampling frequency and 0dB gain for microphone */
    set_sampling_frequency_and_gain(SAMPLES_PER_SECOND, 0);	
  
    //asm(" bclr XF");
    
   //Postavljanje elemenata matrica na 0
	for ( sample = 0 ; sample < wlen ; sample++ ){
            	pomocna[sample]=0;
    	for ( i = 0 ; i < S ; i++ ){
            za_obraditi_desni[sample][i]=0;
    	}
    }
    for(sample = 0 ; sample < 8000 ; sample++){
    	novi_desni[sample]=0;}
   
//Ono sa neta
           //Matrica Mel filtara
           for(i=0 ; i < wlen ; i++){
           		for(j=0 ; j < nfilt ; 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)
	{
		printf("petlja radi\n");
			//Snima se 0.5 sekunda -> 0.5L
    	for ( i = 0  ; i < SAMPLES_PER_SECOND * 0.25L  ;i++  )
 		{

     	aic3204_codec_read(&left_input, &right_input); // Configured for one interrupt per two channels.
   
     	mono_input = stereo_to_mono(left_input, right_input);
     	
     	novi_desni[i]= mono_input;
 		}

          	S=0; //brojac dijelova audio signala
   			for (j=0; j <(8000-320) ; j=j+160)
    		{
    			for(i=0; i<wlen ; i++){
    				za_obraditi_desni[i][S]=novi_desni[j+i];
    			}
        		S++;
            }
			S--;
			za_obradu = &za_obraditi_desni[0][0];
			
		
           //Izgradnja Hammingovog prozora
 //          wlen = wlen * samprate; //fali (int)
           
           for (i=0 ; i < wlen ; i++){
           		win[i]=ham1-ham2*cos((double)2*(i-1)/(wlen-1));
           }
           time=cos((double)(2*(250-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, SCALE);
			printf("petlja POSLJE FFT\n");

			//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;
				}
			}

			for(j=0; j < S; j++){
				obrada=&za_obraditi_desni[j][0];
				pom=&pomocna[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));
            			}
    			    }
   			 	}
			}
	}

}

/* ------------------------------------------------------------------------ *
 *                                                                          *
 *  End of main.c                                                           *
 *                                                                          *
 * ------------------------------------------------------------------------ */