#include "ADDS_21161_EzKit.h"
#include <def21161.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <trans.h> 

//wybór sposobu liczenia FFT:
#define refft	//FFT sygnału rzeczywistego
//#define cefft	//FFT sygnału zespolonego
#define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr	//zamiana miejscami próbek

#ifdef refft	//jeżeli FFT rzeczywista to wejciem jest 1024 próbek syg. rzeczywistego
float real_input[1024]; 
int N=1024;				
#endif

#ifdef cefft	//jeżeli FFT zespolona to wejciem jest 1024 próbek, z których każda posiada częć rzeczywistą i urojoną
float real_input[2048];
int N=2048;				
#endif

int m=0;
int a=0;
int i=0;
int j=0;
int probka=0;
int sinal =1;
int index  = 0;
int Index1 = 0;

float pi = 180;


float wtemp,wr,wpr,wpi,wi,theta,tempr,tempi;
int mmax,m,j,istep,i;

float real_2,imag_2;
float wart_max=0;

float XRp=0,XRm=0,XIp=0,XIm=0;

float phase = 0;		// faza generatora sygnalowego
float freq_ctrl = 0.05;	// czestotliwosc generatora sygnalowego
float freq_max = 0;	// czestotliwosc generatora sygnalowego


int count_start();	//procedury liczące czas wykonania programu
int count_end(int);
int time_start, time_elapsed;

void	main()		//początek programu głównego
{ 

 while(1)		//generacja sygnału testowego oraz jego próbkowania
 {    
 	if (index < N)
 	{ 
		Index1++;
        phase = phase + freq_ctrl;		// poprawka fazy
			
		if (phase > 3.141592654) 		//generator działa na zakresie (-PI:PI)
			phase = phase - 2*3.141592654;
		   	
//			printf ("%f\n",sinf(phase));
        
    	if (Index1==8)		//próbkujemy co ósmą próbkę sygnału testowego
			{	
        	Index1=0;
			real_input[index] = sinf(phase);
//		   	printf ("%d , %f\n",index,real_input[index]);
			index++;
			#ifdef cefft	//w przypadku sygnału zespolonego, częsć urojoną uzupełniamy zerami
				
				real_input[index] = 0;
				index++;
			#endif	
		   	}
        
    }
	else
	 {

	time_start=count_start(); //początek zliczania ilosci cykli potrzebnych do wykonania algorytmu	
    for (i=0;i<N/2;i+=2) // częć algorytmu wykonująca inwersję próbek wejciowych do FFT
    { 
        if (j > i) {
            //zamienia czesc rzeczywista
            SWAP(real_input[j],real_input[i]); 
            //zamienia czesc urojoną
            SWAP(real_input[j+1],real_input[i+1]);

			if((j/2)<(N/4)){
				SWAP(real_input[(N-(i+2))],real_input[(N-(j+2))]);
				SWAP(real_input[(N-(i+2))+1],real_input[(N-(j+2))+1]);
			}
			                    
        }
        m=N/2;
        while (m >= 2 && j >= m) 
        {
            j -= m;
            m = m/2;
        }
        
        j += m;		
    }

 //algorytm Danielson'a-Lanzczos'a liczący FFT
    mmax=2;
    //pętla zewnętrzna algorytmu
    while (N > mmax)
    {
        istep = mmax << 1;
        theta=sinal*(2*pi/mmax);
        theta=(theta/180)*3.14159;
        wtemp=sinf(0.5*theta);
        wpr = -2.0*wtemp*wtemp;
        wpi=sinf(theta);
        wr=1.0;
        wi=0.0;
        //pętle wewnętrzne algorytmu
        for (m=1;m<mmax;m+=2) {
            for (i= m;i<=N;i+=istep) {
                j=i+mmax;
                tempr=wr*real_input[j-1]-wi*real_input[j];
                tempi=wr*real_input[j]+wi*real_input[j-1];
                real_input[j-1]=real_input[i-1]-tempr;
                real_input[j]=real_input[i]-tempi;
                real_input[i-1] += tempr;
                real_input[i] += tempi;
            }
            wr=(wtemp=wr)*wpr-wi*wpi+wr;
            wi=wi*wpr+wtemp*wpi+wi;
        }
        mmax=istep;
    }	

	for (i=0;i<N/2;i+=2) //obliczenia 4zmiennych pomocniczych służących do obliczenia końcowego wyniku rzeczywistej FFT
	{
		XRp=((real_input[i]+real_input[N-i])/2);
		XRm=((real_input[i]-real_input[N-i])/2);
		XIp=((real_input[i+1]+real_input[N-(i+1)])/2);
		XIm=((real_input[i+1]-real_input[N-(i+1)])/2);
		//obliczenie końcowego wyniku FFT na podstawie powyższych zmiennych pomocniczych
		real_input[i]=XRp+cosf((pi*i)/N)*XIp-sinf((pi*i)/N)*XRm;
		real_input[i+1]=XRm-sinf((pi*(i+1))/N)*XIp-cosf((pi*(i+1))/N)*XRm;
	}   
	        

	wart_max=0;
	probka=0;
	
	//obliczenie modułu z otrzymanych wartości
	for (i=0;i<N/2;i+=2)
	{
		real_2=real_input[i]*real_input[i];
		imag_2=real_input[i+1]*real_input[i+1];
	    real_input[i]=sqrt(real_2+imag_2);
	}
	
	//wyszukanie wartości maksymalnej i nr próbki jej odpowiadającej
	for (i=0;i<N/2;i+=2)
	{
		if (real_input[i]>wart_max)
		{
			wart_max=real_input[i];
			probka=i;
		}
					
	}
    time_elapsed=count_end(time_start); //koniec zliczania ilosci cykli potrzebnych do wykonania algorytmu

// obliczenie rzeczywistej wartosci czestotliwosci dominujacej w sygnale wejsciowym 
	#ifdef refft
	freq_max = (6000*probka)/(2*N);
	printf (" RealFFT ilosc cykli = %d, Czestotliowsc dominujaca = %f, Numer prazka FFT: %i\n",time_elapsed,freq_max,probka);    	
   	#endif

 	#ifdef cefft
	freq_max = (6000*probka)/(N);
	printf (" ComplexFFT ilosc cykli = %d, Czestotliowsc dominujaca = %f, Numer prazka FFT: %i\n",time_elapsed,freq_max,probka);    	
   	#endif

   	
	index=0;
	Index1=0;
   	}
}
/*    for (i=0;i<N;i+=1) 
    {
    	printf("Lp: %i , Po: %i \n",i,real_input[i]);
    }	
*/		
}