#include <pal_master.hch>
#include <stdlib.hch>
#include "weights256.hch"
#include "config.hch"
#include "debug.hch"

/* Define two multi-port RAMs for FFT calculation; one for real and one for imaginary values
 * Extra block RAM settings are defined to make sure read and write actions can be performed
 * within one clock-cycle.
 * Left out extra settings on new board the clock changes TODO !!!!
 */
mpram
{
  ram signed 32 rwrite[256];
  rom signed 32 read[256];
} real with {block = "BlockRAM"/*, westart=2.5, welength=1, rclkpos={1.5}, wclkpos={3}, clkpulselen=0.5*/};

mpram 
{
  ram signed 32 rwrite[256];
  rom signed 32 read[256];
} imaginary with {block = "BlockRAM"/*, westart=2.5, welength=1, rclkpos={1.5}, wclkpos={3}, clkpulselen=0.5*/};

// multiplication factors for equalizer function
ram signed 7 eq_settings[16] = {0,2,4,7,10,13,16,19,22,26,30,35,41,48,55,63};


/****************************************************************
* Function:    multiply                                         *
*                                                               *
* Arguments                                                     *
*   x,y        signed variables                                 *
*                                                               *
* Description                                                   *
*   Just a multiplier. But by doing this in a function the      *
*   FPGA space needed is reduced.                               *
*                                                               *
* Return Values                                                 *
*   The result after multiplication                             *
****************************************************************/
signed multiply(signed x,signed y) 
{
  return(x*y);
}


/*******************************************************************
* Function:    calculate_fft                                       *
*                                                                  *
* Arguments                                                        *
*   select_inverse	Boolean that indicates FFT or iFFT calculation *
*                                                                  *
* Description                                                      *
*   This routine performs the Fast Fourier Transform for		   *
*   calculation of the frequency spectrum					       *
*                                                                  *
*******************************************************************/
void calculate_fft(unsigned 1 *select_inverse)
{
  	unsigned 4 level;
  	unsigned 8 point1,point2,j,f,k;
  	unsigned 9 e,i;
  	signed 16 weight1,weight2;
  	signed 32 p,q,r,t;
  	signed 38 a,b;

  	for(level=1;level<=NUMBER_OF_COLUMNS;level++) // count all the columns
  	{
    		e=1<<(NUMBER_OF_COLUMNS-level+1); // number of points in each block in this column
    		f=(e>>1)<-8; 					  // number of butterflies in each block in this column

    		for(j=1;j<=f;j++)
		// count all the butterflies in each block
    		//par
		{
			// Weight factors for real (the same for FFT and iFFT)
		  	weight1 = weight_re[((j-1)<<(level-1))<-7]; 
	      	
			// Weight factors for imaginary (opposite for FFT and iFFT)
			weight2 = (*select_inverse==0) ? -(weight_im[((j-1)<<(level-1))<-7]) : weight_im[((j-1)<<(level-1))<-7]; 

	      		for(i=0@j;i<=NUMBER_OF_POINTS;i+=e)   // count all the blocks in this column
			{	// Butterfly calculation
		        	//par{
					point1 = ((i<-8)-1);
	      				point2 = (((i<-8)+f)-1);
	    			//}
	    			//par{
					
						p = real.read[point1];
						p += real.read[point2];

			      		r = real.read[point1];
						r -= real.read[point2];

          				q = imaginary.read[point1];
						q += imaginary.read[point2];

          				t = imaginary.read[point1];
						t -= imaginary.read[point2];
        			//}
					a = multiply(adjs(r,38),adjs(weight1,38));
					b = multiply(adjs(t,38),adjs(weight2,38));
        			//par{
          				real.rwrite[point2] = ((a-b)>>FRACBITS)<-32;
          				imaginary.rwrite[point1] = q;
		        	//}
					a = multiply(adjs(t,38),adjs(weight1,38));
					b = multiply(adjs(r,38),adjs(weight2,38));
        			//par{
			  		real.rwrite[point1] = p;
		  			imaginary.rwrite[point2] = ((a+b)>>FRACBITS)<-32;
        			//}
	  		}
    		}
	  }
	k=0;
	do
	{
		print_string("FFT[");
		print_hex_value(adju(k,32));
		print_string("]: ");
		print_hex_value((unsigned)(real.read[k]));
		print_eol();
		k++;
	}
	while (k!=0);
	 // Start bit-reversing samples
 	j = 0; 
 	for(i = 0;i < NUMBER_OF_POINTS;i++)
 	{
	   	if(i < (0@j))
   		{
	     	//	point1 = j-1;
     		//	point2 = (i-1)<-8;
     			//par
	     		//{
       				//seq
       				//{
        	 			p = real.read[j];
	         			real.rwrite[j] = real.read[i<-8];
         				real.rwrite[i<-8] = p;
       				//}
       				//seq
       				//{
	         			q = imaginary.read[j];
         				imaginary.rwrite[j] = imaginary.read[i<-8];
         				imaginary.rwrite[i<-8] = q;
       				//}
     			//}   
	   	}

    		k = NUMBER_OF_POINTS>>1;

    		while((j>=k)&&(k>=2))
    		{
      			j -= k;
      			k = k>>1;
    		}
		j += k;
  	} // Bit reversing end
}

/********************************************************************/

void perform_fft(signed 16 *pcm_audio)
{
  	unsigned 1 select_inverse;
  	unsigned 8 k;

    // copy audio data to real-array before starting FFT calculation
	// and set imaginary values to zero
	k=0;
    	do
    	//par
    	{
      		real.rwrite[k] = (0@pcm_audio[k]);
      		imaginary.rwrite[k]=0;
      		k++;
    	} while (k!=0);

	do
	{
		print_string("IA[");
		print_hex_value(adju(k,32));
		print_string("]: ");
		print_hex_value((unsigned)(real.read[k]));
		print_eol();
		k++;
	}
	while (k!=0);
	select_inverse=0;
	calculate_fft(&select_inverse);
  	
    /*	do  
    	{
	//	print_signed_hex_value(real.read[k],32);
	//	print_eol();
		k++;
 	}while(k!=0);*/
}

/********************************************************************/

void perform_ifft(signed 16 *modified_audio/*, unsigned 6 *ifft_info*/)
{
  	unsigned 1 select_inverse;
  	unsigned 8 k;
  	signed 32 p;

  	select_inverse=1;
  	calculate_fft(&select_inverse);

    	k=0;
	do
	{
		// divide samples by number of points
      		p=(real.read[(k+95)]>>NUMBER_OF_COLUMNS);
	  
	  	// write data to output buffer & display buffer
      		modified_audio[k <- 6] = (p<-16);
		//ifft_info[k] = (unsigned)(p[15:10]); 
	//	print_string("MA[");
	//	print_hex_value(adju(k,16));
	//	print_string("]: ");
	//	print_hex_value((unsigned)(modified_audio[k]));
	//	print_eol();
		
		k++;
    	} while(k[5:0] != 0);
}

/********************************************************************/

void equalize_audio(unsigned 8 *fft_info)
{
  signed 32 p,q;
  signed 38 a;
  unsigned 8 i, mirror_i, old_value, m, n;
  unsigned 8 bit;
  unsigned 9 tmp;

  macro expr equalize_bar = multiply(adjs(q,38),a)[37:6];

  real.rwrite[0] = real.read[0] - 8470527; // remove DC component for calculations
//  imaginary.rwrite[0] = 0; 				   // remove DC component 

  //write data to fft_info for display purposes
  for(i=0;i<NUMBER_OF_FREQUENCIES;i++)
  {
    p=real.read[i];
    q=imaginary.read[i];

    if (p[31]==1) p=-p; else delay;
    if (q[31]==1) q=-q; else delay;
    p = (p<q) ? q : p; // This is done to get the best visual frequency result
	 
    // Logartimic display of the frequency spectrum
    bit=248;
	while ((p[21]==0) && (bit!=0))
	//par
	{
	 p = p<<1;
	 bit = bit - 31;
	}
	old_value = fft_info[i];
	tmp=((0@old_value) + (0@bit))>>1;
	fft_info[i] = (old_value<=(tmp<-8)) ? (tmp<-8) : old_value-1;
  }

  // add DC component again before IFFT calculation is performed
  real.rwrite[0] = real.read[0] + 8470527; 
}