#include <pal_master.hch>
#include <stdlib.hch>
#include "fft.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.
mpram
{
  ram signed 32 rwrite[256];
  rom signed 32 read[256];
} real with {block = "BlockRAM" /*block=4, 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" /*block=6, 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((adjs(x,40))*(adjs(y,40)));
}


/************************************************************************
* 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, k, f, j;
	unsigned 9 e, i;
	signed 16 weight1,weight2;
	signed 32 p,q,r,t;
	signed 40 a,b;
		
//	float u,v,z,c,s,p,q,r,t,w,a;

 //	n=1<<NUMBER_OF_COLUMNS;//n=256 order=8


 	for(level=1;level<=NUMBER_OF_COLUMNS;level++)
	{
		e=1<<(NUMBER_OF_COLUMNS-level+1);
		f=(e>>1)<-8;

		for(j=1;j<=f;j++)
		{
			weight1 = weight_re[((1<<(level-1))*(j-1))<-7];
			weight2 = (select_inverse) ? -weight_im[((1<<(level-1))*(j-1))<-7] : weight_im[((1<<(level-1))*(j-1))<-7];

  			for(i=0@j;i<=NUMBER_OF_POINTS;i+=e)
			{
				point1 = (i<-8)-1;
				point2 = ((i<-8)+f)-1;

				p = real.read[point1] + real.read[point2];
				r = real.read[point1] - real.read[point2];
				q = imaginary.read[point1] + imaginary.read[point2];
   				t = imaginary.read[point1] - imaginary.read[point2];
				
				real.rwrite[point1] = p;
        			imaginary.rwrite[point1] = q;

				a = multiply(adjs(r,40),adjs(weight1,40));
				b = multiply(adjs(t,40),adjs(weight2,40));
				
				real.rwrite[point2]=((a-b)>>FRACBITS)<-32;
				
				a = multiply(adjs(t,40),adjs(weight1,40));
				b = multiply(adjs(r,40),adjs(weight2,40));

				imaginary.rwrite[point2]=((a-b)>>FRACBITS)<-32;
   			}
		}
	}

	j=1;// Bit reversing start
	for(i=1;i<NUMBER_OF_POINTS;i++)
	{
		if(i < (0@j))
		{
			point1 = j-1;
   			point2 = (i-1)<-8;

			p = real.read[point1];
   			real.rwrite[point1] = real.read[point2];
   			real.rwrite[point2] = p;

   			q = imaginary.read[point1];
   			imaginary.rwrite[point1] = imaginary.read[point2];
   			imaginary.rwrite[point2] = q;
		}

		k=NUMBER_OF_POINTS>>1;

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

/*	if(select_inverse)
	{
		a=(1<<14)>>NUMBER_OF_COLUMNS;
		for(k=0;k<(adju(n,8));k++)
		{
			real.rwrite[k]*=(0@a);
			imaginary.rwrite[k]*=(0@a);
		}
	}*/
}


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

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);

	select_inverse=0;
	calculate_fft(select_inverse);
  
}

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

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("Real[");
	//	print_hex_value(adju(k,16));
		print_string("]: ");
		print_hex_value((unsigned)real.read[k]);
		print_eol();

	  	k++;
    	} while(k<64);
}

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

/*void equalize_audio(unsigned 4 *eq_level, unsigned 7 *fft_info)
{
  signed 24 p,q;
  signed 16 a;
  unsigned 8 i, mirror_i, bit, m, n;
  unsigned 7 old_value;
  unsigned 9 tmp;

  macro expr equalize_bar = multiply(q,a)[29:6];

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

  
  for(i=0;i<=NUMBER_OF_FREQUENCIES;i++)	  
  {  
	// set multiplication factor (0..64) for current frequency bar
	a = adjs(eq_settings[eq_level[i<-7]],16);

	// multiply frequency with this factor and divide by 64 (drop 6 LSB's)
	q = real.read[i];
	real.rwrite[i] = equalize_bar;

    q=imaginary.read[i];
	imaginary.rwrite[i] = equalize_bar;

	// the upper part(128..255) of the spectrum is mirrored to the lower part; 
	// these values need to be adjusted too
	if ((i<-7)!=0) // if not in DC component bar
	{
	  mirror_i = (NUMBER_OF_POINTS-1)-i+1;
	  q = real.read[mirror_i];
	  real.rwrite[mirror_i] = equalize_bar;

      q = imaginary.read[mirror_i];
	  imaginary.rwrite[mirror_i] = equalize_bar;
	};
  }
  
  //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[23]==1) p=-p; else delay;
    if (q[23]==1) q=-q; else delay;
    p = (p<q) ? q : p; // This is done to get the best visual frequency result
	 
    if (display_log)
	{

     bit=126;
	 while ((p[21]==0) && (bit!=0))
	 par{
	  p = p<<1;
	  bit = bit - 18;
	 }
	 old_value = fft_info[i<-7];
	 tmp=((0@old_value) + (0@bit))>>1;
	 fft_info[i<-7] = (old_value<=(tmp<-7)) ? (tmp<-7) : old_value-1;
    } else {
	 old_value = fft_info[i<-7];
	 fft_info[i<-7] = (old_value<=(unsigned)(p[21:15])) ? (unsigned)(p[21:15]) : old_value-1;
	}
  }

  // add DC component again before inverse FFT calculation is performed
  //real.rwrite[0] = real.read[0] + DC_COMPONENT; 
}
*/