view sv/filter/DSP.cpp @ 229:7d5d51145b81

support stereo in MultiRealTimeFilter and integrate Equalizer filter

/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */

/*	Sound Access	
		EASAIER client application.	
		Dublin Institute of Technology - Audio Research Group 2007
		www.audioresearchgroup.com
		Author: Dan Barry

	This program is free software; you can redistribute it and/or    
	modify it under the terms of the GNU General Public License as    
	published by the Free Software Foundation; either version 2 of the    
	License, or (at your option) any later version.  See the file    
	COPYING included with this distribution for more information.
*/

//#include  "stdafx.h"
#include  "DSP.h"
#include  "math.h"
#include	<cassert>
#include	<cmath>
extern int currentposition;
extern float lastfactor;
extern float hopfactor;
//These variables are only required for find peaks...the same variables are passed in to other functions
extern int numpeaks;
extern float *peak_locations; 
//extern float *p_mags; 
//extern float *c_mags;

extern float *L_mags;		///CURRENT FRAME MAGNITUDES
extern float *R_mags;		///CURRENT FRAME MAGNITUDES
extern float *pL_mags;		///PREVIOUS FRAME MAGNITUDES
extern float *pR_mags;		///PREVIOUS FRAME MAGNITUDES

extern float *band1;
extern float *band2;
extern float *band3;
extern float *band4;
extern float *band5;

void intobyte(int num, char* pbyte1 ,char* pbyte2)
{

		
		char firstByte = (num & 0xff);
		int secondByteInt = (num & 0xff00);
		secondByteInt = secondByteInt>>8;
		char secondByte = secondByteInt;

		/*int thirdByteInt = (array[i] & 0xff0000);
		thirdByteInt = thirdByteInt>>16;
		char thirdByte = thirdByteInt;

		int fourthByteInt = (array[i] & 0xff000000);
		fourthByteInt = fourthByteInt>>24;
		char fourthByte = fourthByteInt;*/


		*pbyte1 = firstByte;
		*pbyte2 = secondByte;
}


void cart2pol(float* cart, float* mags, float* phases, int framesize)
{
	int length=framesize/2;
	for (int f = 0; f<length; f++)
	{
	
	mags[f]=sqrt((cart[f]*cart[f])+(cart[length+f]*cart[length+f]));
	

	phases[f]=atan2((float)(-1*cart[f+length]),(float)(cart[f]));
	
	

	}


}

void pol2cart(float* cart, float* mags, float* phases, int framesize)
{
	int length=framesize/2;
	for (int f = 0; f<length; f++)
	{
		
		cart[f]=mags[f]*cos(phases[f]);
		cart[length+f]=-(mags[f]*sin(phases[f]));
	
	
	}


}

void hanning(float* window, int framesize)
{
	
	for (int f = 0; f<framesize; f++)
	{
		
		window[f]= (float) (0.5*(1-cos(2*PI*(f+1)/(framesize+1))));
	
	}


}

void updatephases(float* c_phase,float* p_phase,float* c_synthphase,float* p_synthphase, int framesize,float hopfactor,float interpfactor)
{

	float synth_hopsize=((float) framesize) /4;
	float actual_anhop=floor(hopfactor*synth_hopsize);
	float anhop = floor((hopfactor*synth_hopsize)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;

	for (int f = 0; f<(framesize/2); f++)
	{
		
		//float bin = (f/(float)framesize);

		interpfactor;

		omega_k=(float) ((2*PI*f)/float(framesize));			//This is the predicted angular freqency rads/sec of the centre frequency
		
		delta_phi=(c_phase[f]-p_phase[f])-(anhop*omega_k);

		a = (float) (delta_phi/(2*PI));
		
		k =  (float) (floor(a+0.5));
		
		delta_phi = (float) (delta_phi-k*2*PI);
		
		inst_freq=omega_k+delta_phi/anhop; 
	
		
		
		c_synthphase[f] = p_synthphase[f]+(inst_freq*synth_hopsize); 

		if (currentposition<actual_anhop){c_synthphase[f] = c_phase[f];}
		
		p_synthphase[f] = c_synthphase[f];

		p_phase[f] = c_phase[f];

		
		
		

	
	}
}

void updatephases2(float* c_phase,float* p_phase,float* c_synthphase,float* p_synthphase, int framesize,float hopfactor,float interpfactor)
{

	float synth_hopsize=((float) framesize)/4;
	float actual_anhop=actual_anhop=floor(hopfactor*synth_hopsize);
	float anhop = floor((hopfactor*synth_hopsize)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;

	for (int f = 0; f<(framesize/2); f++)
	{
		
		//float bin = (f/(float)framesize);

		interpfactor;

		omega_k=(float) ((2*PI*f)/float(framesize));			//This is the predicted angular freqency rads/sec of the centre frequency
		
		delta_phi=(c_phase[f]-p_phase[f])-(anhop*omega_k);

		a = (float) (delta_phi/(2*PI));
		
		k = (float) (floor(a+0.5));
		
		delta_phi = (float) (delta_phi-k*2*PI);
		
		inst_freq=omega_k+delta_phi/anhop; 
	
		
		
		c_synthphase[f] = p_synthphase[f]+(inst_freq*synth_hopsize); 

		

		if (currentposition<actual_anhop){c_synthphase[f] = c_phase[f];}
		
		if (!(anhop==lastfactor)){c_synthphase[f]=c_phase[f];}
		
		
		
		p_synthphase[f] = c_synthphase[f];
		

		p_phase[f] = c_phase[f];

		

		

	
	}
	lastfactor=anhop;
}

void rotatephases(float* c_phase, float* p_phase, float* c_synthphase, float* p_synthphase, int framesize, float interpfactor)
{
	float phase_diff;
	int diffhop = framesize/4;

	float anhop = floor((diffhop)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;
	
	//findpeaks(c_mags, p_mags, framesize, peak_locations, numpeaks);
	
	for (int i=0; i<framesize/2; i++)
	{
		
		phase_diff=(c_phase[i]-p_phase[i]);
		
		omega_k=(float) ((2*PI*i)/float(framesize));

		delta_phi=(phase_diff)-(anhop*omega_k);

		a = (float) (delta_phi/(2*PI));
		
		k = (float) (floor(a+0.5));
		
		delta_phi = (float) (delta_phi-k*2*PI);
		
		inst_freq=omega_k+delta_phi/anhop; 
					
		c_synthphase[i]=p_synthphase[i]+(inst_freq*diffhop);

		//c_synthphase[i]=p_synthphase[i]+(phase_diff);

		//c_synthphase[i]=p_synthphase[i]+((phase_diff*diffhop)*interpfactor);
		
		//if (currentposition<framesize/2 || (!(lastfactor==hopfactor))){c_synthphase[i] = c_phase[i];}
		if (currentposition<framesize/2){c_synthphase[i] = c_phase[i];}
		
		p_synthphase[i]=c_synthphase[i];
	}

	lastfactor=hopfactor;
}

void rotatephases_peaklocked(float* c_phase, float* p_phase, float* c_synthphase, float* p_synthphase, float* c_mags, int framesize, float interpfactor)
{
	float phase_diff;
	int diffhop = framesize/4;

	float anhop = floor((diffhop)/interpfactor);
	float inst_freq;
	float omega_k;
	float delta_phi;
	float a, k;
	int start, end, i;
	float original_peak_phase;
	float cutoff;

	cutoff = 10000;

	cutoff = floor(cutoff/(44100/framesize));

	numpeaks = findpeaks(c_mags, pL_mags,(float) framesize, peak_locations);
	
	int p=1;
	//while(peak_locations[p]<cutoff)
		while(p<=numpeaks)
	{
		
		i=(int) peak_locations[p];

		original_peak_phase=c_phase[i];
		
		//start=i-2;
		//end=i+2;
		
		
		if (p==1){
			start=0;
			end=(int) floor(peak_locations[p]+((peak_locations[p+1]-peak_locations[p])/2));
		}

		else{
		if (p==numpeaks){
			start=(int) floor(peak_locations[p-1]+((peak_locations[p]-peak_locations[p-1])/2));
			end=(framesize/2);
		}
		else {
			start=(int) floor(peak_locations[p-1]+((peak_locations[p]-peak_locations[p-1])/2));
			end=(int) floor(peak_locations[p]+((peak_locations[p+1]-peak_locations[p])/2));
		}
		}
			
		phase_diff=(c_phase[i]-p_phase[i]);
		omega_k=(float) ((2*PI*i)/float(framesize));
		delta_phi=(phase_diff)-(anhop*omega_k);
		a = (float) (delta_phi/(2*PI));
		k = (float) (floor(a+0.5));
		delta_phi = (float) (delta_phi-k*2*PI);
		inst_freq=omega_k+delta_phi/anhop; 
		c_synthphase[i]=p_synthphase[i]+(inst_freq*diffhop);
		//c_synthphase[i]=p_synthphase[i]+(phase_diff);
		//c_synthphase[i]=p_synthphase[i]+((phase_diff*diffhop)*interpfactor);
		//if (currentposition<framesize/2 || (!(lastfactor==hopfactor))){c_synthphase[i] = c_phase[i];}
		if (currentposition<framesize/2){c_synthphase[i] = c_phase[i];}
		p_synthphase[i]=c_synthphase[i];

		int j;
		for (j=start; j<i; j++)
		{
			c_synthphase[j]=c_synthphase[i]-(original_peak_phase-c_phase[j]);
			p_synthphase[j]=c_synthphase[j];
		}

		for (j=i+1; j<end; j++)
		{
			c_synthphase[j]=c_synthphase[i]-(original_peak_phase-c_phase[j]);
			p_synthphase[j]=c_synthphase[j];
		}


		 p++;
	}

	/*for (i=peak_locations[p]+1; i<(framesize/2); i++)
	{
		phase_diff=(c_phase[i]-p_phase[i]);
		
		omega_k=(2*PI*i)/float(framesize);

		delta_phi=(phase_diff)-(anhop*omega_k);

		a = delta_phi/(2*PI);
		
		k = floor(a+0.5);
		
		delta_phi = delta_phi-k*2*PI;
		
		inst_freq=omega_k+delta_phi/anhop; 
					
		c_synthphase[i]=p_synthphase[i]+(inst_freq*diffhop);

		p_synthphase[i]=c_synthphase[i];
	}  */

	lastfactor=hopfactor;
}

void cur2last(float* c_phase, float* c_synthphase, float* p_synthphase, int framesize)
{

for (int i=0; i<framesize/2; i++)
	{
		
		c_synthphase[i]=c_phase[i];
						
		p_synthphase[i]=c_synthphase[i];
	}


}


int findpeaks(float* c_mags, float* p_mags, float framesize, float* peak_locations)

{
	numpeaks=0;
	peak_locations[0]=0;
	float peakthreshold=0;

	for (int i=2; i<(framesize/4)-10; i++)
	{
		
		if(c_mags[i]>c_mags[i-1] && c_mags[i]>c_mags[i+1] && c_mags[i]>c_mags[i-2] && c_mags[i]>c_mags[i+2] && c_mags[i]>peakthreshold)
		{
			numpeaks++;
			peak_locations[numpeaks]=(float) i;
			
		}
		
		
	}
	return numpeaks;
}

bool transient_detect(float* L_mags, float* R_mags, float* pL_mags, float* pR_mags, float drumthresh, float framesize)
{
	
	extern float *L_phase, *R_phase;
	int trans = 0;
	
	for (int f = 0; f<(framesize/2); f++)
	{
		
			if	((R_mags[f]-pR_mags[f])>0 ||
				(L_mags[f]-pL_mags[f])>0){trans++;}
				
				
				pR_mags[f]=R_mags[f];
				pL_mags[f]=L_mags[f];
				

	}
	drumthresh=(drumthresh/100)*(framesize/2);
	

	if (trans>=drumthresh)
	{

	return true;
	}
	else{return false;}
}





void log10plot(float* L_mags, float* plotFFTarray, int framesize, int plotsize)
{
	float interpfactor1 = 10/100;	//1 - 100hz		bin 1 - 10
	float interpfactor2 = 90/100;	//100 - 1000hz		bin 10 - 100
	float interpfactor3 = 900/100;	//1000 - 10000hz		bin 100 - 1000
	float interpfactor4 = 1048/100;	//10000 - 20000hz			bin 1000 - 2048
	int temp;
	int i;
	float p;
	float ratio,dec;
	
	for (i=0 ; i<plotsize/4; i++)
	{
		p=(float) i;
		
		temp=(int) floor((((1+p)/100)*9)+1);
		ratio=((((1+p)/100)*9)+1)-floor((float) temp);
		
		plotFFTarray[i]=log((((L_mags[temp+1]-L_mags[temp])*ratio)+L_mags[temp])/1000);
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log((((L_mags[temp+1]-L_mags[temp])*ratio)+L_mags[temp])/1000);
	}

	for (i=plotsize/4 ; i<(plotsize/4)*2; i++)
	{
		p=(float) i;
		temp= (int) floor((((1+p-plotsize/4)/100)*90)+10);
		plotFFTarray[i]=log(L_mags[temp]/1000);
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}

	for (i=(plotsize/4)*2 ; i<(plotsize/4)*3; i++)
	{
		p=(float) i;
		temp=(int) floor((((1+p-(plotsize/4)*2)/100)*900)+100);
		
		plotFFTarray[i]=log(L_mags[temp]/1000);
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}

	for (i=(plotsize/4)*3 ; i<(plotsize/4)*4; i++)
	{
		p=(float) i;
		temp=(int) floor((((1+p-(plotsize/4)*3)/100)*1048)+1000);
		
		plotFFTarray[i]=log(L_mags[temp]/1000);
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}
}


void log10plot2(float* L_mags, float* plotFFTarray, int framesize, int plotsize)
{
	float interpfactor1 = 10/100;	//1 - 100hz		bin 1 - 10
	float interpfactor2 = 90/100;	//100 - 1000hz		bin 10 - 100
	float interpfactor3 = 900/100;	//1000 - 10000hz		bin 100 - 1000
	float interpfactor4 = 1048/100;	//10000 - 20000hz			bin 1000 - 2048
	int temp;
	int i;
	float p;
	float ratio,dec;

	
		
	for (i=0 ; i<plotsize/4; i++)
	{
		p=(float) i;
		
		temp=(int) floor((((1+p)/100)*9)+1);
		ratio=((((1+p)/100)*9)+1)-floor((float) temp);
		
		
		plotFFTarray[i]=((((L_mags[temp+1]-L_mags[temp])*ratio)+L_mags[temp]));
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log((((L_mags[temp+1]-L_mags[temp])*ratio)+L_mags[temp])/1000);
	}

	for (i=plotsize/4 ; i<(plotsize/4)*2; i++)
	{
		p=i;
		temp=floor((((1+p-plotsize/4)/100)*90)+10);
		plotFFTarray[i]=(L_mags[temp]);
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}

	for (i=(plotsize/4)*2 ; i<(plotsize/4)*3; i++)
	{
		p=i;
		temp=floor((((1+p-(plotsize/4)*2)/100)*900)+100);
		
		plotFFTarray[i]=(L_mags[temp]);
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}

	for (i=(plotsize/4)*3 ; i<(plotsize/4)*4; i++)
	{
		p=i;
		temp=floor((((1+p-(plotsize/4)*3)/100)*1048)+1000);
		
		plotFFTarray[i]=(L_mags[temp]);
		//plotFFTarray[i]=dec*plotFFTarray[i]+0.5*log(L_mags[temp]/1000);
	}
}

void applyEQ(float* L_mags, float* R_mags, int framesize, int plotsize, QVector<int> eqcurve)
{
	//(80,85,400,120,0) Screen Coordinates
	float gain, interpfactor, p,result;
	int i, ind;

	for(i=0; i<10; i++)
	{
		gain=(110-(eqcurve.at(i*10)-85))/120;  // eqcurve is the array filled when the user draws on the canvas
		if (gain < 0){gain=0;}
		L_mags[i]*=gain;
		R_mags[i]*=gain;
			
	}

	for(i=10; i<100; i++)
	{
		
		p=i;
		interpfactor=1.1;
		result=100+((interpfactor*(p-10)));
		ind=int(result);
		gain=(110-(eqcurve.at(ind)-85))/120;
		//gain=(110-(eqcurve[i+90]-85))/120;
		
		if (gain < 0){gain=0;}
		L_mags[i]*=gain;
		R_mags[i]*=gain;
			
			

	}

	for(i=100; i<1000; i++)
	{
		p=i;
		interpfactor=0.11;
		result=200+((interpfactor*(p-100)));
		ind=int(result);
		gain=(110-(eqcurve.at(ind)-85))/120;
		if (gain < 0){gain=0;}
		L_mags[i]*=gain;
		R_mags[i]*=gain;
			
			

	}
	
	for(i=1000; i<2048; i++)
	{
		p=i;
		interpfactor=.09;
		result=300+((interpfactor*(p-1000)));
		ind=int(result);
		gain=(110-(eqcurve.at(ind)-85))/120;
		if (gain < 0){gain=0;}
		L_mags[i]*=gain;
		R_mags[i]*=gain;
			
			

	}
}



void stereo2ms(float* left, float* right, int framesize)
{
	float m, s;
	for (int i=0;i<framesize;i++)
	{
		m=(left[i]+right[i])/2;
		s=(left[i]-right[i])/2;
		left[i]=m;
		right[i]=s;
	}
}

void ms2stereo(float* left, float* right, int framesize)
{
	float m, s;
	for (int i=0;i<framesize;i++)
	{
		m=(left[i]+right[i]);
		s=(left[i]-right[i]);
		left[i]=m;
		right[i]=s;
	}
}


void bandeq(float* L_mags, float* R_mags, int gainband1, int gainband2, int gainband3, int gainband4, int gainband5, float* bandcurve, int framesize)
{
	float maxcut=190; //18db of attenuation
	float inc_cut=(1-(1/maxcut))/100;
	
	float maxboost=2; //6 db of gain
	float inc_boost=(maxboost-1)/100;
	float g1=1,g2=1,g3=1,g4=1,g5=1,instgain=1;

	if (gainband1 < 0){g1=(1/maxcut)+((100+ (float)gainband1)*inc_cut);} //not finished!!!!
	if (gainband1 == 0){g1=1;}
	if (gainband1 > 0){g1=1+(((float)gainband1)*inc_boost);}
	//cout(g1);
	if (gainband2 < 0){g2=(1/maxcut)+((100+ (float)gainband2)*inc_cut);} //not finished!!!!
	if (gainband2 == 0){g2=1;}
	if (gainband2 > 0){g2=1+(((float)gainband2)*inc_boost);}

	if (gainband3 < 0){g3=(1/maxcut)+((100+ (float)gainband3)*inc_cut);} //not finished!!!!
	if (gainband3 == 0){g3=1;}
	if (gainband3 > 0){g3=1+(((float)gainband3)*inc_boost);}

	if (gainband4 < 0){g4=(1/maxcut)+((100+ (float)gainband4)*inc_cut);} //not finished!!!!
	if (gainband4 == 0){g4=1;}
	if (gainband4 > 0){g4=1+(((float)gainband4)*inc_boost);}

	if (gainband5 < 0){g5=(1/maxcut)+((100+ (float)gainband5)*inc_cut);} //not finished!!!!
	if (gainband5 == 0){g5=1;}
	if (gainband5 > 0){g5=1+(((float)gainband5)*inc_boost);}


	for (int i=0;i<framesize/2;i++)
	{
		instgain=(g1*band1[i])+(g2*band2[i])+(g3*band3[i])+(g4*band4[i])+(g5*band5[i]);
		bandcurve[i]=instgain;
		L_mags[i]*=instgain;
		R_mags[i]*=instgain;	
	}




}


void create_filterbands(int framesize)

{
	float binwidth = 44100/4096;
	
	float xover1 = 80; //100
	float xover2 = 700; //700
	float xover3 = 3000; //3000
	float xover4 = 9000; //9000

	float width1 = 50; //50
	float width2 = 300; //100
	float width3 = 3000; //300
	float width4 = 1900; //900

	int w;

	xover1=(xover1/binwidth);
	xover2=(xover2/binwidth);
	xover3=(xover3/binwidth);
	xover4=(xover4/binwidth);

	width1=(width1/binwidth);
	width2=(width2/binwidth);
	width3=(width3/binwidth);
	width4=(width4/binwidth);

	int p1 = 0;
	int p2 = floor(xover1-(width1/2));
	int p3 = floor(xover1+(width1/2));
	int p4 = floor(xover2-(width2/2));
	int p5 = floor(xover2+(width2/2));
	int p6 = floor(xover3-(width3/2));
	int p7 = floor(xover3+(width3/2));
	int p8 = floor(xover4-(width4/2));
	int p9 = floor(xover4+(width4/2));
	int p10 = 2048;

	
	//cout(p1);
	
	for (int i=0;i<framesize/2;i++)
	{
		if (i < p2)
		{
			band1[i]=1;
			band2[i]=0;
			band3[i]=0;
			band4[i]=0;
			band5[i]=0;
		}

		if (i >= p2 && i <= p3)
		{	
			w = p3-p2+1;
			band1[i]= (0.5*(1-cos(2*PI*((i-p2+w)+1)/((2*w)))));
			band2[i]= (0.5*(1-cos(2*PI*((i-p2)+1)/((2*w)))));
			band3[i]=0;
			band4[i]=0;
			band5[i]=0;
		}


		if (i > p3 && i < p4)
		{
			band1[i]=0;
			band2[i]=1;
			band3[i]=0;
			band4[i]=0;
			band5[i]=0;
		}

		if (i >= p4 && i <= p5)
		{	
			w = p4-p5+1;
			band1[i]=0;
			band2[i]= (0.5*(1-cos(2*PI*((i-p4+w)+1)/((2*w)))));
			band3[i]= (0.5*(1-cos(2*PI*((i-p4)+1)/((2*w)))));
			band4[i]=0;
			band5[i]=0;
		}

		if (i > p5 && i < p6)
		{
			band1[i]=0;
			band2[i]=0;
			band3[i]=1;
			band4[i]=0;
			band5[i]=0;
		}

		if (i >= p6 && i <= p7)
		{	
			w = p6-p7+1;
			band1[i]=0;
			band2[i]=0;
			band3[i]= (0.5*(1-cos(2*PI*((i-p6+w)+1)/((2*w)))));
			band4[i]= (0.5*(1-cos(2*PI*((i-p6)+1)/((2*w)))));
			band5[i]=0;
		}

		if (i > p7 && i < p8)
		{
			band1[i]=0;
			band2[i]=0;
			band3[i]=0;
			band4[i]=1;
			band5[i]=0;
		}

		if (i >= p8 && i <= p9)
		{	
			w = p8-p9+1;
			band1[i]=0;
			band2[i]=0;
			band3[i]=0;
			band4[i]= (0.5*(1-cos(2*PI*((i-p8+w)+1)/((2*w)))));
			band5[i]= (0.5*(1-cos(2*PI*((i-p8)+1)/((2*w)))));
		
		}
	
		if (i > p9 && i < p10)
		{
			band1[i]=0;
			band2[i]=0;
			band3[i]=0;
			band4[i]=0;
			band5[i]=1;
		}

	}

	
	
	
}