Index: src/effects/Equalization.cpp
===================================================================
--- src/effects/Equalization.cpp	(revision 12939)
+++ src/effects/Equalization.cpp	(working copy)
@@ -56,6 +56,25 @@
 
 *//*******************************************************************/
 
+// added by Andrew Hallendorff intrinsics processing
+// note this does not yet check the processor
+#define USEINTRINSICSACH
+#define SSEOPTIONLEVEL 8
+// options are 
+// 0 original path
+// 1 SSE no BitReverse Table
+// 2 SSE no SinCos Table
+// 3 SSE no SinCos and no BitReverse buffer
+// 4 SSE with SinCos and BitReverse buffer
+// 8 SSE threaded no SinCos and no BitReverse buffer
+// 9 SSE threaded no BitReverse Table
+// 10 SSE threaded no SinCos Table
+// 11 SSE threaded with SinCos and BitReverse buffer
+
+// !!! note tracks less than 256 blocks will not thread
+
+// need to put options in a hidden menu for testing
+
 #include "../Audacity.h"
 #include "Equalization.h"
 #include "../AColor.h"
@@ -76,6 +95,9 @@
 #include "float_cast.h"
 #include <vector>
 
+// added by Andrew Hallendorff intrinsics processing
+#include <xmmintrin.h>
+
 #include <wx/bitmap.h>
 #include <wx/button.h>
 #include <wx/msgdlg.h>
@@ -191,7 +213,7 @@
 #define NUM_INTERP_CHOICES 3
 static wxString interpChoiceStrings[NUM_INTERP_CHOICES];
 
-void EffectEqualization::ReadPrefs()
+void EffectEqualization::ReadPrefs() 
 {
 
    gPrefs->Read(wxT("/Effects/Equalization/FilterLength"),
@@ -222,6 +244,7 @@
 EffectEqualization::EffectEqualization()
 {
    hFFT = InitializeFFT(windowSize);
+
    mFFTBuffer = new float[windowSize];
    mFilterFuncR = new float[windowSize];
    mFilterFuncI = new float[windowSize];
@@ -356,11 +379,17 @@
    return true;
 }
 
+
+
 bool EffectEqualization::Process()
 {
    this->CopyInputTracks(); // Set up mOutputTracks.
    bool bGoodResult = true;
 
+   TableUsage(SSEOPTIONLEVEL);
+   if(SSEOPTIONLEVEL)  // !!! Filter MUST BE QUAD WORD ALIGNED !!!!
+	   mM=(mM&(~15))+1;
+
    SelectedTrackListOfKindIterator iter(Track::Wave, mOutputTracks);
    WaveTrack *track = (WaveTrack *) iter.First();
    int count = 0;
@@ -375,11 +404,29 @@
          sampleCount end = track->TimeToLongSamples(t1);
          sampleCount len = (sampleCount)(end - start);
 
-         if (!ProcessOne(count, track, start, len))
-         {
-            bGoodResult = false;
-            break;
-         }
+		 if(!SSEOPTIONLEVEL) {
+			 if (!ProcessOne(count, track, start, len))
+			 {
+				bGoodResult = false;
+				break;
+			 }
+		 } else {
+			 if(SSEOPTIONLEVEL<8) {
+				 if (!ProcessOne4x(count, track, start, len))
+				 {
+					bGoodResult = false;
+					break;
+				 }
+			 } else {
+				 if (!ProcessOne4xThreaded(count, track, start, len))
+				 {
+					bGoodResult = false;
+					break;
+				 }
+			 }
+		 }
+
+
       }
 
       track = (WaveTrack *) iter.Next();
@@ -391,6 +438,7 @@
 }
 
 
+
 bool EffectEqualization::ProcessOne(int count, WaveTrack * t,
                                  sampleCount start, sampleCount len)
 {
@@ -422,6 +470,9 @@
    int wcopy = 0;
    int offset = (mM - 1)/2;
 
+	int totalProcessed=0;
+
+
    while(len)
    {
       sampleCount block = idealBlockLen;
@@ -454,6 +505,7 @@
       }  //next i, lump of this block
 
       output->Append((samplePtr)buffer, floatSample, block);
+	  totalProcessed+=block;
       len -= block;
       s += block;
 
@@ -584,7 +636,591 @@
    ReorderToTime(hFFT, mFFTBuffer, buffer);
 }
 
+bool EffectEqualization::ProcessBuffer(fft_type *sourceBuffer, fft_type *destBuffer, sampleCount bufferLength, sampleCount windowSize, sampleCount filterSize)
 
+{
+	sampleCount blockSize=windowSize-filterSize; // 12,384 = 16,384 - 4000
+	// length must be a factor of window size for 4x processing. 
+	
+	sampleCount blockCount=bufferLength/blockSize;
+	sampleCount lastBlockSize=bufferLength%blockSize;
+	if(lastBlockSize)
+		blockCount++;
+
+	float *workBuffer=new float[windowSize*2];
+
+	for(int runx=0;runx<blockCount;runx++) 
+	{
+		float *currentBuffer=&workBuffer[windowSize*(runx&1)]; 
+		memcpy(currentBuffer, sourceBuffer, sizeof(float)*blockSize);
+		sourceBuffer+=blockSize;
+		ZeroMemory(&currentBuffer[blockSize], sizeof(float)*filterSize);
+		Filter(windowSize, currentBuffer);
+		float *writeEnd=currentBuffer+blockSize;
+		if(runx+1==blockCount) 
+			writeEnd=currentBuffer+(lastBlockSize+filterSize);
+		if(runx) {
+			float *lastOverrun=&workBuffer[windowSize*((runx+1)&1)+blockSize]; 
+			for(int j=0;j<filterSize;j++)
+				*destBuffer++= *currentBuffer++ + *lastOverrun++;
+		} else 
+			currentBuffer+=filterSize>>1; // this will skip the first filterSize on the first run
+		while(currentBuffer<writeEnd)
+			*destBuffer++ = *currentBuffer++;
+	}
+	delete workBuffer;
+	return true;
+}
+
+
+bool EffectEqualization::ProcessBuffer4x(BufferInfo *bufferInfo)
+{
+	sampleCount windowSize=bufferInfo->mFftWindowSize; // 16,384
+	sampleCount filterSize=bufferInfo->mFftFilterSize; // 4000
+	sampleCount blockSize=windowSize-filterSize; // 12,384
+	// length must be a factor of window size for 4x processing. 
+	if(bufferInfo->mBufferLength%blockSize)
+		return false;
+	// All buffers must be quadword aligned
+	for(int i=0;i<4;i++) {
+		if((int)bufferInfo->mBufferSouce[i] & ((int)0x0f)) 
+			return false;
+		if((int)bufferInfo->mBufferDest[i] & ((int)0x0f)) 
+			return false;
+	} 
+	
+	sampleCount blockCount=bufferInfo->mBufferLength/blockSize;
+
+	__m128 *readBlocks[4]; // some temps so we dont destroy the vars in the struct
+	__m128 *writeBlocks[4];
+	for(int i=0;i<4;i++) {
+		readBlocks[i]=(__m128 *)bufferInfo->mBufferSouce[i];
+		writeBlocks[i]=(__m128 *)bufferInfo->mBufferDest[i];
+	}
+
+	__m128 *swizzledBuffer128=(__m128 *) _aligned_malloc(sizeof(__m128)*windowSize*3,16); // 2 windows + a scratch buffer
+	__m128 *scratchBuffer=&swizzledBuffer128[windowSize*2];
+
+	for(int run4x=0;run4x<blockCount;run4x++) 
+	{
+		// swizzle the data to the swizzle buffer
+		__m128 *currentSwizzledBlock=&swizzledBuffer128[windowSize*(run4x&1)]; // <- this lookx h4x0r
+		for(int i=0,j=0;j<blockSize;i++,j+=4) {
+			__m128 tmp0   = _mm_shuffle_ps(readBlocks[1][i], readBlocks[0][i], 0xee);
+			__m128 tmp1   = _mm_shuffle_ps(readBlocks[1][i], readBlocks[0][i], 0x44);
+			__m128 tmp2   = _mm_shuffle_ps(readBlocks[3][i], readBlocks[2][i], 0xee);
+			__m128 tmp3   = _mm_shuffle_ps(readBlocks[3][i], readBlocks[2][i], 0x44);
+			currentSwizzledBlock[j]   = _mm_shuffle_ps(tmp2, tmp0, 0xdd);
+			currentSwizzledBlock[j+1] = _mm_shuffle_ps(tmp2, tmp0, 0x88);
+			currentSwizzledBlock[j+2] = _mm_shuffle_ps(tmp3, tmp1, 0xdd);
+			currentSwizzledBlock[j+3] = _mm_shuffle_ps(tmp3, tmp1, 0x88);
+		}
+		ZeroMemory(&currentSwizzledBlock[blockSize], filterSize*sizeof(__m128));
+		Filter4x(windowSize, (float *)currentSwizzledBlock, (float *)scratchBuffer);
+		int writeStart=0, writeToStart=0; // note readStart is where the read data is written
+		if(run4x) {
+			// maybe later swizzle add and write in one
+			__m128 *lastOverrun128=&swizzledBuffer128[windowSize*((run4x+1)&1)+blockSize]; 
+//			for(int j=0;j<filterSize;j++)
+//				currentSwizzledBlock[j]=_mm_add_ps(currentSwizzledBlock[j], lastOverrun128[j]);
+			// add and swizzle data + filter
+			for(int i=0,j=0;j<filterSize;i++,j+=4) {
+				__m128 tmps0 = _mm_add_ps(currentSwizzledBlock[j], lastOverrun128[j]);
+				__m128 tmps1 = _mm_add_ps(currentSwizzledBlock[j+1], lastOverrun128[j+1]);
+				__m128 tmps2 = _mm_add_ps(currentSwizzledBlock[j+2], lastOverrun128[j+2]);
+				__m128 tmps3 = _mm_add_ps(currentSwizzledBlock[j+3], lastOverrun128[j+3]);
+				__m128 tmp0   = _mm_shuffle_ps(tmps1, tmps0, 0xee);
+				__m128 tmp1   = _mm_shuffle_ps(tmps1, tmps0, 0x44);
+				__m128 tmp2   = _mm_shuffle_ps(tmps3, tmps2, 0xee);
+				__m128 tmp3   = _mm_shuffle_ps(tmps3, tmps2, 0x44);
+				writeBlocks[0][i] = _mm_shuffle_ps(tmp2, tmp0, 0xdd);
+				writeBlocks[1][i] = _mm_shuffle_ps(tmp2, tmp0, 0x88);
+				writeBlocks[2][i] = _mm_shuffle_ps(tmp3, tmp1, 0xdd);
+				writeBlocks[3][i] = _mm_shuffle_ps(tmp3, tmp1, 0x88);
+			}
+			writeStart=filterSize;
+			writeToStart=filterSize>>2;
+		} else writeStart=filterSize>>1; // this will skip the first filterSize on the first run
+		int writeEnd=blockSize;
+		if(run4x+1==blockCount)
+			writeEnd+=filterSize>>1; // if last run include the filter data
+		// swizzle it back
+		for(int i=writeToStart,j=writeStart;j<writeEnd;i++,j+=4) {
+			__m128 tmp0   = _mm_shuffle_ps(currentSwizzledBlock[j+1], currentSwizzledBlock[j], 0xee);
+			__m128 tmp1   = _mm_shuffle_ps(currentSwizzledBlock[j+1], currentSwizzledBlock[j], 0x44);
+			__m128 tmp2   = _mm_shuffle_ps(currentSwizzledBlock[j+3], currentSwizzledBlock[j+2], 0xee);
+			__m128 tmp3   = _mm_shuffle_ps(currentSwizzledBlock[j+3], currentSwizzledBlock[j+2], 0x44);
+			writeBlocks[0][i] = _mm_shuffle_ps(tmp2, tmp0, 0xdd);
+			writeBlocks[1][i] = _mm_shuffle_ps(tmp2, tmp0, 0x88);
+			writeBlocks[2][i] = _mm_shuffle_ps(tmp3, tmp1, 0xdd);
+			writeBlocks[3][i] = _mm_shuffle_ps(tmp3, tmp1, 0x88);
+		}
+		for(int i=0;i<4;i++) { // shift each block
+			readBlocks[i]+=blockSize>>2; // these are 128b pointers, each window is 1/4 blockSize for those
+			writeBlocks[i]+=(blockSize-(run4x?0:(filterSize>>1)))>>2; // the first run is truncated by the filterSize
+		}
+	}
+	_aligned_free(swizzledBuffer128);
+	return true;
+}
+
+static int compairWaves = 0;
+	
+bool EffectEqualization::ProcessOne4x(int count, WaveTrack * t,
+                                 sampleCount start, sampleCount len)
+{
+//	sampleCount windowSize=windowSize; // 16,384
+	sampleCount filterSize=(mM-1)&(~15); // 4000 !!! Filter MUST BE QUAD WORD ALIGNED !!!!
+	sampleCount blockSize=windowSize-filterSize; // 12,384
+	sampleCount blockCount=len/blockSize;
+
+	if(blockCount<16) // it's not worth 4x processing do a regular process
+		return ProcessOne(count, t, start, len);
+
+	if(compairWaves++&1)
+		return ProcessOne(count, t, start, len);
+
+	sampleCount trackBlockSize = t->GetMaxBlockSize();
+
+	// we're going to do 4x processing of 80 windows at a time
+	// this equates to approx 6 seconds of audio and ~4 blocks short of a mb
+	// 176,400b = 1,058,400b
+
+	int blocksPerBuffer=64;
+	int bigBufferSize=blockSize*blocksPerBuffer;
+	float *bigBuffer=(float *)_aligned_malloc(sizeof(float)*bigBufferSize,16); 
+
+	AudacityProject *p = GetActiveProject();
+	WaveTrack *output=p->GetTrackFactory()->NewWaveTrack(floatSample, t->GetRate());
+
+	TrackProgress(count, 0.0);
+	int bigRuns=len/bigBufferSize;
+	int trackBlocksPerBig=bigBufferSize/trackBlockSize;
+	int trackLeftovers=bigBufferSize-trackBlocksPerBig*trackBlockSize;
+	int singleProcessLength=len%bigBufferSize;
+	sampleCount currentSample=start;
+
+	// fill the bufferInfo
+	BufferInfo bufferInfo;
+	bufferInfo.mFftWindowSize=windowSize;
+	bufferInfo.mFftFilterSize=filterSize;
+	bufferInfo.mBufferLength=bigBufferSize>>2;
+
+	for(int i=0;i<4;i++)
+		bufferInfo.mBufferDest[i]=bufferInfo.mBufferSouce[i]=&bigBuffer[i*bufferInfo.mBufferLength];
+
+	for(int bigRun=0;bigRun<bigRuns;bigRun++)
+	{
+		// fill the buffer
+		for(int i=0;i<trackBlocksPerBig;i++) {
+			t->Get((samplePtr)&bigBuffer[i*trackBlockSize], floatSample, currentSample, trackBlockSize);
+			currentSample+=trackBlockSize;
+		}
+		if(trackLeftovers) {
+			t->Get((samplePtr)&bigBuffer[trackBlocksPerBig*trackBlockSize], floatSample, currentSample, trackLeftovers);
+			currentSample+=trackLeftovers;
+		}
+		ProcessBuffer4x(&bufferInfo);
+		if (TrackProgress(count, (double)(bigRun)/(double)bigRuns))
+		{
+			 break;
+		}
+		for(int i=0;i<trackBlocksPerBig;i++) 
+			output->Append((samplePtr)&bigBuffer[i*trackBlockSize], floatSample, trackBlockSize);
+		if(trackLeftovers) 
+			output->Append((samplePtr)&bigBuffer[trackBlocksPerBig*trackBlockSize], floatSample, trackLeftovers);
+	}
+	if(singleProcessLength) {
+		t->Get((samplePtr)bigBuffer, floatSample, currentSample, singleProcessLength);
+		ProcessBuffer(bigBuffer, bigBuffer, singleProcessLength, windowSize, filterSize);
+		output->Append((samplePtr)bigBuffer, floatSample, singleProcessLength);
+	}
+
+	output->Flush();
+
+	if(true)
+	{
+      // now move the appropriate bit of the output back to the track
+      // (this could be enhanced in the future to use the tails)
+	  int offset = (mM - 1)/2;
+	  // my data is 1 off because I have to maintain quadword alignment
+
+//	  double offsetT0 = t->LongSamplesToTime((sampleCount)offset);
+      double lenT = t->LongSamplesToTime(len);
+      // 'start' is the sample offset in 't', the passed in track
+      // 'startT' is the equivalent time value
+      // 'output' starts at zero
+      double startT = t->LongSamplesToTime(start);
+      
+      //output has one waveclip for the total length, even though 
+      //t might have whitespace seperating multiple clips
+      //we want to maintain the original clip structure, so
+      //only paste the intersections of the new clip.
+      
+      //Find the bits of clips that need replacing
+      std::vector<std::pair<double, double> > clipStartEndTimes;
+      std::vector<std::pair<double, double> > clipRealStartEndTimes; //the above may be truncated due to a clip being partially selected
+      for (WaveClipList::compatibility_iterator it=t->GetClipIterator(); it; it=it->GetNext())
+      {
+         WaveClip *clip;
+         double clipStartT;
+         double clipEndT;
+         
+         clip = it->GetData();
+         clipStartT = clip->GetStartTime();
+         clipEndT = clip->GetEndTime();
+         if( clipEndT <= startT )
+            continue;   // clip is not within selection
+         if( clipStartT >= startT + lenT )
+            continue;   // clip is not within selection
+            
+         //save the actual clip start/end so that we can rejoin them after we paste.
+         clipRealStartEndTimes.push_back(std::pair<double,double>(clipStartT,clipEndT));            
+            
+         if( clipStartT < startT )  // does selection cover the whole clip?
+            clipStartT = startT; // don't copy all the new clip
+         if( clipEndT > startT + lenT )  // does selection cover the whole clip?
+            clipEndT = startT + lenT; // don't copy all the new clip
+         
+         //save them
+         clipStartEndTimes.push_back(std::pair<double,double>(clipStartT,clipEndT));
+      }
+      //now go thru and replace the old clips with new
+      for(unsigned int i=0;i<clipStartEndTimes.size();i++)
+      {
+         Track *toClipOutput;
+         //remove the old audio and get the new
+         t->Clear(clipStartEndTimes[i].first,clipStartEndTimes[i].second);
+//         output->Copy(clipStartEndTimes[i].first-startT+offsetT0,clipStartEndTimes[i].second-startT+offsetT0, &toClipOutput);   
+         output->Copy(clipStartEndTimes[i].first-startT,clipStartEndTimes[i].second-startT, &toClipOutput);   
+         if(toClipOutput)
+         {
+            //put the processed audio in
+            bool bResult = t->Paste(clipStartEndTimes[i].first, toClipOutput);
+            wxASSERT(bResult); // TO DO: Actually handle this.
+            //if the clip was only partially selected, the Paste will have created a split line.  Join is needed to take care of this
+            //This is not true when the selection is fully contained within one clip (second half of conditional)
+            if( (clipRealStartEndTimes[i].first  != clipStartEndTimes[i].first || 
+                 clipRealStartEndTimes[i].second != clipStartEndTimes[i].second) &&
+                 !(clipRealStartEndTimes[i].first <= startT &&  
+                 clipRealStartEndTimes[i].second >= startT+lenT) )
+               t->Join(clipRealStartEndTimes[i].first,clipRealStartEndTimes[i].second);
+            delete toClipOutput;
+         }
+      }
+   } 
+	delete output;
+	_aligned_free(bigBuffer);
+
+	return true;
+}
+
+//#define DEBUGEQTHREADS
+
+
+void *EQWorker::Entry()
+{
+   int currentIndex=0;
+   while(!mExitLoop) {
+#ifdef DEBUGEQTHREADS
+		   OutputDebugString(wxString::Format(wxT("Thread %d arrived list mutex\n"), mThreadID));
+#endif
+//      Sleep(1000);
+      mMutex->Lock();
+#ifdef DEBUGEQTHREADS
+		  OutputDebugString(wxString::Format(wxT("Thread %d aquired list mutex\n"), mThreadID));
+#endif
+	  int checkCount=0;
+	  while(checkCount++<mBufferInfoCount && !mExitLoop) {// check through all the buffers
+		 if(mBufferInfoList[currentIndex].mBufferStatus==BufferReady) // we found an unlocked ready buffer
+			 if(!mBufferInfoList[currentIndex].TryLock()) // buffer is ours for the taking
+				 break; 
+	     currentIndex=(currentIndex+1)%mBufferInfoCount;
+	  }
+	  if(mExitLoop || checkCount>=mBufferInfoCount) {
+	     mMutex->Unlock();
+	  } else {
+#ifdef DEBUGEQTHREADS
+		 OutputDebugString(wxString::Format(wxT("Thread %d released list mutex No data\n"), mThreadID));
+#endif
+#ifdef DEBUGEQTHREADS
+	     OutputDebugString(wxString::Format(wxT("Thread %d aquired data at index %d Locked\n"), mThreadID, currentIndex));
+#endif
+		 mBufferInfoList[currentIndex].mBufferStatus=BufferBusy; // we own it now
+		 mMutex->Unlock();
+#ifdef DEBUGEQTHREADS
+		 OutputDebugString(wxString::Format(wxT("Thread %d released list mutex with data. Started processing\n"), mThreadID));
+#endif
+		 mEffectEqualization->ProcessBuffer4x(&mBufferInfoList[currentIndex]);
+		 mBufferInfoList[currentIndex].mBufferStatus=BufferDone; // we're done
+#ifdef DEBUGEQTHREADS
+		 OutputDebugString(wxString::Format(wxT("Thread %d released data mutex index %d\n"), mThreadID, currentIndex));
+#endif
+		 mBufferInfoList[currentIndex].Unlock();
+	   }
+	}
+	return NULL;
+}
+
+bool EffectEqualization::ProcessOne4xThreaded(int count, WaveTrack * t,
+                                 sampleCount start, sampleCount len)
+{
+//	sampleCount windowSize=windowSize; // 16,384
+	sampleCount filterSize=(mM-1)&(~15); // 4000 !!! Filter MUST BE QUAD WORD ALIGNED !!!!
+	sampleCount blockSize=windowSize-filterSize; // 12,384
+	sampleCount blockCount=len/blockSize;
+
+	if(blockCount<16) // it's not worth 4x processing do a regular process
+		return ProcessOne4x(count, t, start, len);
+
+	sampleCount trackBlockSize = t->GetMaxBlockSize();
+
+	int threadCount=wxThread::GetCPUCount();
+	if(threadCount<=0 || blockCount<256) // dont do it without cores or big data
+		return ProcessOne4x(count, t, start, len);
+	int blocksPerBuffer=64;
+	int bigBufferSize=blockSize*blocksPerBuffer;
+	int workerDataCount=threadCount+2; // 2 extra slots
+	float *bigBuffer=(float *)_aligned_malloc(sizeof(float)*bigBufferSize*workerDataCount,16); 
+
+	TrackProgress(count, 0.0);
+	int bigRuns=len/bigBufferSize;
+	int trackBlocksPerBig=bigBufferSize/trackBlockSize;
+	int trackLeftovers=bigBufferSize-trackBlocksPerBig*trackBlockSize;
+	int singleProcessLength=len%bigBufferSize;
+	sampleCount currentSample=start;
+
+	// fill the bufferInfo
+	BufferInfo* bufferInfo = new BufferInfo[workerDataCount];
+	for(int i=0;i<workerDataCount;i++)
+	{
+		bufferInfo[i].mFftWindowSize=windowSize;
+		bufferInfo[i].mFftFilterSize=filterSize;
+		bufferInfo[i].mBufferLength=bigBufferSize>>2;
+		for(int j=0;j<4;j++)
+			bufferInfo[i].mBufferDest[j]=bufferInfo[i].mBufferSouce[j]=&bigBuffer[j*bufferInfo[i].mBufferLength+bigBufferSize*i];
+	}
+
+	AudacityProject *p = GetActiveProject();
+	WaveTrack *output=p->GetTrackFactory()->NewWaveTrack(floatSample, t->GetRate());
+
+	// start the workers
+	wxMutex dataMutex;
+	dataMutex.IsOk();
+	EQWorker* eQWorkers=new EQWorker[threadCount];
+	for(int i=0;i<threadCount;i++) {
+		eQWorkers[i].SetData( bufferInfo, workerDataCount, &dataMutex, this);
+		eQWorkers[i].Create();
+		eQWorkers[i].Run();
+#ifdef DEBUGEQTHREADS
+		OutputDebugString(wxString::Format(wxT("Thread %d created and running\n"), eQWorkers[i].mThreadID));
+#endif
+	}
+	int bigBlocksRead=workerDataCount, bigBlocksWritten=0;
+
+	// fill the first workerDataCount buffers we checked above and there is at least this data
+	for(int i=0;i<workerDataCount;i++)
+	{
+		// fill the buffer
+		for(int j=0;j<trackBlocksPerBig;j++) {
+			t->Get((samplePtr)&bufferInfo[i].mBufferSouce[0][j*trackBlockSize], floatSample, currentSample, trackBlockSize);
+			currentSample+=trackBlockSize;
+		}
+		if(trackLeftovers) {
+			t->Get((samplePtr)&bufferInfo[i].mBufferSouce[0][trackBlocksPerBig*trackBlockSize], floatSample, currentSample, trackLeftovers);
+			currentSample+=trackLeftovers;
+		}
+		bufferInfo[i].mBufferStatus=BufferReady; // free for grabbin
+	}
+#ifdef DEBUGEQTHREADS
+    OutputDebugString(wxString::Format(wxT("Main loop added %d buffers\n"), workerDataCount));
+#endif
+	int currentIndex=0;
+	while(bigBlocksWritten<bigRuns) {
+		while(bufferInfo[currentIndex].mBufferStatus!=BufferDone) Sleep(1); // make sure the data was done
+//		dataMutex.Lock(); // Get in line for data
+//		if(!bufferInfo[currentIndex].TryLock()) { // data is ours
+				if (TrackProgress(count, (double)(bigBlocksWritten)/(double)bigRuns))
+				{
+					 break;
+				}
+				for(int i=0;i<trackBlocksPerBig;i++) 
+					output->Append((samplePtr)&bufferInfo[currentIndex].mBufferDest[0][i*trackBlockSize], floatSample, trackBlockSize);
+				if(trackLeftovers) 
+					output->Append((samplePtr)&bufferInfo[currentIndex].mBufferDest[0][trackBlocksPerBig*trackBlockSize], floatSample, trackLeftovers);
+				bigBlocksWritten++;
+#ifdef DEBUGEQTHREADS
+	OutputDebugString(wxString::Format(wxT("Main loop retired block %d \n"), currentIndex));
+#endif
+				if(bigBlocksRead<bigRuns) {
+					// fill the buffer
+					for(int j=0;j<trackBlocksPerBig;j++) {
+						t->Get((samplePtr)&bufferInfo[currentIndex].mBufferSouce[0][j*trackBlockSize], floatSample, currentSample, trackBlockSize);
+						currentSample+=trackBlockSize;
+					}
+					if(trackLeftovers) {
+						t->Get((samplePtr)&bufferInfo[currentIndex].mBufferSouce[0][trackBlocksPerBig*trackBlockSize], floatSample, currentSample, trackLeftovers);
+						currentSample+=trackLeftovers;
+					}
+					bufferInfo[currentIndex].mBufferStatus=BufferReady; // free for grabbin
+					bigBlocksRead++;
+#ifdef DEBUGEQTHREADS
+		OutputDebugString(wxString::Format(wxT("Main loop filled block %d \n"), currentIndex));
+#endif
+				} else bufferInfo[currentIndex].mBufferStatus=BufferEmpty; // this is completely unecessary
+//				bufferInfo[currentIndex].Unlock(); // let the dogs out
+				currentIndex=(currentIndex+1)%workerDataCount;
+//			} else {
+//				bufferInfo[currentIndex].Unlock(); // we got the lock but the data wasn't ready
+//			}
+		} 
+//		dataMutex.Unlock(); // Get back in line for data
+//	}
+	if(singleProcessLength) {
+		t->Get((samplePtr)bigBuffer, floatSample, currentSample, singleProcessLength);
+		ProcessBuffer(bigBuffer, bigBuffer, singleProcessLength, windowSize, filterSize);
+		output->Append((samplePtr)bigBuffer, floatSample, singleProcessLength);
+	}
+	for(int i=0;i<threadCount;i++) { // tell all the workers to exit
+		eQWorkers[i].ExitLoop();
+#ifdef DEBUGEQTHREADS
+    OutputDebugString(wxString::Format(wxT("Worker %d told to exit\n"), eQWorkers[i].mThreadID));
+#endif
+	}
+	for(int i=0;i<threadCount;i++) { // tell all the workers to exit
+		eQWorkers[i].Wait();
+#ifdef DEBUGEQTHREADS
+		OutputDebugString(wxString::Format(wxT("Worker %d exited\n"), eQWorkers[i].mThreadID));
+#endif
+	}
+	delete[] eQWorkers; // kill the workers ( go directly to jail)
+	delete [] bufferInfo;
+	_aligned_free(bigBuffer);
+	output->Flush();
+
+	if(true)
+	{
+      // now move the appropriate bit of the output back to the track
+      // (this could be enhanced in the future to use the tails)
+	  int offset = (mM - 1)/2;
+	  // my data is 1 off because I have to maintain quadword alignment
+
+//	  double offsetT0 = t->LongSamplesToTime((sampleCount)offset);
+      double lenT = t->LongSamplesToTime(len);
+      // 'start' is the sample offset in 't', the passed in track
+      // 'startT' is the equivalent time value
+      // 'output' starts at zero
+      double startT = t->LongSamplesToTime(start);
+      
+      //output has one waveclip for the total length, even though 
+      //t might have whitespace seperating multiple clips
+      //we want to maintain the original clip structure, so
+      //only paste the intersections of the new clip.
+      
+      //Find the bits of clips that need replacing
+      std::vector<std::pair<double, double> > clipStartEndTimes;
+      std::vector<std::pair<double, double> > clipRealStartEndTimes; //the above may be truncated due to a clip being partially selected
+      for (WaveClipList::compatibility_iterator it=t->GetClipIterator(); it; it=it->GetNext())
+      {
+         WaveClip *clip;
+         double clipStartT;
+         double clipEndT;
+         
+         clip = it->GetData();
+         clipStartT = clip->GetStartTime();
+         clipEndT = clip->GetEndTime();
+         if( clipEndT <= startT )
+            continue;   // clip is not within selection
+         if( clipStartT >= startT + lenT )
+            continue;   // clip is not within selection
+            
+         //save the actual clip start/end so that we can rejoin them after we paste.
+         clipRealStartEndTimes.push_back(std::pair<double,double>(clipStartT,clipEndT));            
+            
+         if( clipStartT < startT )  // does selection cover the whole clip?
+            clipStartT = startT; // don't copy all the new clip
+         if( clipEndT > startT + lenT )  // does selection cover the whole clip?
+            clipEndT = startT + lenT; // don't copy all the new clip
+         
+         //save them
+         clipStartEndTimes.push_back(std::pair<double,double>(clipStartT,clipEndT));
+      }
+      //now go thru and replace the old clips with new
+      for(unsigned int i=0;i<clipStartEndTimes.size();i++)
+      {
+         Track *toClipOutput;
+         //remove the old audio and get the new
+         t->Clear(clipStartEndTimes[i].first,clipStartEndTimes[i].second);
+//         output->Copy(clipStartEndTimes[i].first-startT+offsetT0,clipStartEndTimes[i].second-startT+offsetT0, &toClipOutput);   
+         output->Copy(clipStartEndTimes[i].first-startT,clipStartEndTimes[i].second-startT, &toClipOutput);   
+         if(toClipOutput)
+         {
+            //put the processed audio in
+            bool bResult = t->Paste(clipStartEndTimes[i].first, toClipOutput);
+            wxASSERT(bResult); // TO DO: Actually handle this.
+            //if the clip was only partially selected, the Paste will have created a split line.  Join is needed to take care of this
+            //This is not true when the selection is fully contained within one clip (second half of conditional)
+            if( (clipRealStartEndTimes[i].first  != clipStartEndTimes[i].first || 
+                 clipRealStartEndTimes[i].second != clipStartEndTimes[i].second) &&
+                 !(clipRealStartEndTimes[i].first <= startT &&  
+                 clipRealStartEndTimes[i].second >= startT+lenT) )
+               t->Join(clipRealStartEndTimes[i].first,clipRealStartEndTimes[i].second);
+            delete toClipOutput;
+         }
+      }
+   } 
+	delete output;
+
+	return true;
+}
+
+
+
+void EffectEqualization::Filter4x(sampleCount len,
+         float *buffer, float *scratchBuffer)
+{
+   int i;
+   __m128 real128, imag128;
+   // Apply FFT
+   RealFFTf4x(buffer, hFFT);
+
+   // Apply filter
+   // DC component is purely real
+   __m128 *localFFTBuffer=(__m128 *)scratchBuffer;
+   __m128 *localBuffer=(__m128 *)buffer;
+
+   __m128 filterFuncR, filterFuncI;
+   filterFuncR=_mm_set1_ps(mFilterFuncR[0]);
+   localFFTBuffer[0]=_mm_mul_ps(localBuffer[0], filterFuncR); 
+   int halfLength=(len/2);
+
+   bool useBitReverseTable=SSEOPTIONLEVEL&1;
+
+   for(i=1; i<halfLength; i++)
+   {
+	   if(useBitReverseTable) {
+		  real128=localBuffer[hFFT->BitReversed[i]  ];
+		  imag128=localBuffer[hFFT->BitReversed[i]+1];
+	   } else {
+		  real128=localBuffer[SmallReverseBits(i,14)  ];
+		  imag128=localBuffer[SmallReverseBits(i,14)+1];
+	   }
+      filterFuncR=_mm_set1_ps(mFilterFuncR[i]);
+      filterFuncI=_mm_set1_ps(mFilterFuncI[i]);
+      localFFTBuffer[2*i  ] = _mm_sub_ps( _mm_mul_ps(real128, filterFuncR), _mm_mul_ps(imag128, filterFuncI));
+      localFFTBuffer[2*i+1] = _mm_add_ps( _mm_mul_ps(real128, filterFuncI), _mm_mul_ps(imag128, filterFuncR));
+   }
+   // Fs/2 component is purely real
+   filterFuncR=_mm_set1_ps(mFilterFuncR[halfLength]);
+   localFFTBuffer[1] = _mm_mul_ps(localBuffer[1], filterFuncR);
+
+   // Inverse FFT and normalization
+   InverseRealFFTf4x(scratchBuffer, hFFT);
+   ReorderToTime4x(hFFT, scratchBuffer, buffer);
+}
+
+
+
 //----------------------------------------------------------------------------
 // EqualizationPanel
 //----------------------------------------------------------------------------
@@ -1302,12 +1938,7 @@
    szrC->Add( btn, 0, wxALIGN_CENTRE | wxALL, 4 );
    mGridOnOff = new wxCheckBox(this, GridOnOffID, _("G&rids"),
                             wxDefaultPosition, wxDefaultSize,
-#if defined(__WXGTK__)
-// Fixes bug #662
-                            wxALIGN_LEFT);
-#else
                             wxALIGN_RIGHT);
-#endif
    mGridOnOff->SetName(_("Grids"));
    szrC->Add( mGridOnOff, 0, wxALIGN_CENTRE | wxALL, 4 );
 
@@ -3475,3 +4106,4 @@
 }
 
 #endif
+
Index: src/effects/Equalization.h
===================================================================
--- src/effects/Equalization.h	(revision 12939)
+++ src/effects/Equalization.h	(working copy)
@@ -42,6 +42,60 @@
 #include "../widgets/Ruler.h"
 #include "../RealFFTf.h"
 
+// added by Andrew Hallendorff intrinsics processing
+   enum EQBufferStatus
+   {
+      BufferEmpty=0,
+	  BufferReady,
+	  BufferBusy,
+	  BufferDone
+   };
+
+
+class BufferInfo : public wxMutex {
+public:
+	BufferInfo() { mBufferLength=0; mBufferStatus=BufferEmpty; IsOk();};
+	float* mBufferSouce[4];
+	float* mBufferDest[4];
+	int mBufferLength;
+	sampleCount mFftWindowSize;
+	sampleCount mFftFilterSize;
+	EQBufferStatus mBufferStatus;
+};
+
+class EffectEqualization;
+
+static int EQWorkerCounter=0;
+
+class EQWorker : public wxThread {
+ public:
+   EQWorker():wxThread(wxTHREAD_JOINABLE) {   
+	  mBufferInfoList=NULL;
+      mBufferInfoCount=0;
+	  mMutex=NULL;
+	  mEffectEqualization=NULL;
+	  mExitLoop=false;
+	  mThreadID=EQWorkerCounter++;
+}
+   void SetData( BufferInfo* bufferInfo, int bufferInfoCount, wxMutex *mutex, EffectEqualization *effectEqualization) {
+	  mBufferInfoList=bufferInfo;
+      mBufferInfoCount=bufferInfoCount;
+	  mMutex=mutex;
+	  mEffectEqualization=effectEqualization;
+   }
+   void ExitLoop() { // this will cause the thread to drop from the loops
+      mExitLoop=true;
+   }
+   virtual void* Entry();
+   BufferInfo* mBufferInfoList;
+   int mBufferInfoCount, mThreadID;
+   wxMutex *mMutex;
+   EffectEqualization *mEffectEqualization;
+   bool mExitLoop;
+};
+
+// endach
+
 class EqualizationDialog;
 
 //
@@ -113,6 +167,13 @@
    // low range of human hearing
    enum {loFreqI=20};
 
+// added by Andrew Hallendorff intrinsics processing
+// these are called by the threading class
+   bool ProcessBuffer(fft_type *sourceBuffer, fft_type *destBuffer, sampleCount bufferLength, sampleCount windowSize, sampleCount filterSize);
+   bool ProcessBuffer4x(BufferInfo *bufferInfo);
+// endach
+
+
 private:
    bool ProcessOne(int count, WaveTrack * t,
                    sampleCount start, sampleCount len);
@@ -119,6 +180,13 @@
 
    void Filter(sampleCount len,
                float *buffer);
+
+// added by Andrew Hallendorff intrinsics processing
+   bool ProcessOne4x(int count, WaveTrack * t, sampleCount start, sampleCount len);
+   bool ProcessOne4xThreaded(int count, WaveTrack * t, sampleCount start, sampleCount len);
+   void Filter4x(sampleCount len, float *buffer, float *scratchBuffer);
+// endach
+
    void ReadPrefs();
 
    HFFT hFFT;
Index: src/include/sse_mathfun.h
===================================================================
--- src/include/sse_mathfun.h	(revision 0)
+++ src/include/sse_mathfun.h	(working copy)
@@ -0,0 +1,711 @@
+/* SIMD (SSE1+MMX or SSE2) implementation of sin, cos, exp and log
+
+   Inspired by Intel Approximate Math library, and based on the
+   corresponding algorithms of the cephes math library
+
+   The default is to use the SSE1 version. If you define USE_SSE2 the
+   the SSE2 intrinsics will be used in place of the MMX intrinsics. Do
+   not expect any significant performance improvement with SSE2.
+*/
+
+/* Copyright (C) 2007  Julien Pommier
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+
+  (this is the zlib license)
+*/
+
+#include <xmmintrin.h>
+
+/* yes I know, the top of this file is quite ugly */
+
+#ifdef _MSC_VER /* visual c++ */
+# define ALIGN16_BEG __declspec(align(16))
+# define ALIGN16_END 
+#else /* gcc or icc */
+# define ALIGN16_BEG
+# define ALIGN16_END __attribute__((aligned(16)))
+#endif
+
+/* __m128 is ugly to write */
+typedef __m128 v4sf;  // vector of 4 float (sse1)
+
+#ifdef USE_SSE2
+# include <emmintrin.h>
+typedef __m128i v4si; // vector of 4 int (sse2)
+#else
+typedef __m64 v2si;   // vector of 2 int (mmx)
+#endif
+
+/* declare some SSE constants -- why can't I figure a better way to do that? */
+#define _PS_CONST(Name, Val)                                            \
+  static const ALIGN16_BEG float _ps_##Name[4] ALIGN16_END = { Val, Val, Val, Val }
+#define _PI32_CONST(Name, Val)                                            \
+  static const ALIGN16_BEG int _pi32_##Name[4] ALIGN16_END = { Val, Val, Val, Val }
+#define _PS_CONST_TYPE(Name, Type, Val)                                 \
+  static const ALIGN16_BEG Type _ps_##Name[4] ALIGN16_END = { Val, Val, Val, Val }
+
+_PS_CONST(1  , 1.0f);
+_PS_CONST(0p5, 0.5f);
+/* the smallest non denormalized float number */
+_PS_CONST_TYPE(min_norm_pos, int, 0x00800000);
+_PS_CONST_TYPE(mant_mask, int, 0x7f800000);
+_PS_CONST_TYPE(inv_mant_mask, int, ~0x7f800000);
+
+_PS_CONST_TYPE(sign_mask, int, (int)0x80000000);
+_PS_CONST_TYPE(inv_sign_mask, int, ~0x80000000);
+
+_PI32_CONST(1, 1);
+_PI32_CONST(inv1, ~1);
+_PI32_CONST(2, 2);
+_PI32_CONST(4, 4);
+_PI32_CONST(0x7f, 0x7f);
+
+_PS_CONST(cephes_SQRTHF, 0.707106781186547524);
+_PS_CONST(cephes_log_p0, 7.0376836292E-2);
+_PS_CONST(cephes_log_p1, - 1.1514610310E-1);
+_PS_CONST(cephes_log_p2, 1.1676998740E-1);
+_PS_CONST(cephes_log_p3, - 1.2420140846E-1);
+_PS_CONST(cephes_log_p4, + 1.4249322787E-1);
+_PS_CONST(cephes_log_p5, - 1.6668057665E-1);
+_PS_CONST(cephes_log_p6, + 2.0000714765E-1);
+_PS_CONST(cephes_log_p7, - 2.4999993993E-1);
+_PS_CONST(cephes_log_p8, + 3.3333331174E-1);
+_PS_CONST(cephes_log_q1, -2.12194440e-4);
+_PS_CONST(cephes_log_q2, 0.693359375);
+
+#ifndef USE_SSE2
+typedef union xmm_mm_union {
+  __m128 xmm;
+  __m64 mm[2];
+} xmm_mm_union;
+
+#define COPY_XMM_TO_MM(xmm_, mm0_, mm1_) {          \
+    xmm_mm_union u; u.xmm = xmm_;                   \
+    mm0_ = u.mm[0];                                 \
+    mm1_ = u.mm[1];                                 \
+}
+
+#define COPY_MM_TO_XMM(mm0_, mm1_, xmm_) {                         \
+    xmm_mm_union u; u.mm[0]=mm0_; u.mm[1]=mm1_; xmm_ = u.xmm;      \
+  }
+
+#endif // USE_SSE2
+
+/* natural logarithm computed for 4 simultaneous float 
+   return NaN for x <= 0
+*/
+v4sf log_ps(v4sf x) {
+#ifdef USE_SSE2
+  v4si emm0;
+#else
+  v2si mm0, mm1;
+#endif
+  v4sf one = *(v4sf*)_ps_1;
+
+  v4sf invalid_mask = _mm_cmple_ps(x, _mm_setzero_ps());
+
+  x = _mm_max_ps(x, *(v4sf*)_ps_min_norm_pos);  /* cut off denormalized stuff */
+
+#ifndef USE_SSE2
+  /* part 1: x = frexpf(x, &e); */
+  COPY_XMM_TO_MM(x, mm0, mm1);
+  mm0 = _mm_srli_pi32(mm0, 23);
+  mm1 = _mm_srli_pi32(mm1, 23);
+#else
+  emm0 = _mm_srli_epi32(_mm_castps_si128(x), 23);
+#endif
+  /* keep only the fractional part */
+  x = _mm_and_ps(x, *(v4sf*)_ps_inv_mant_mask);
+  x = _mm_or_ps(x, *(v4sf*)_ps_0p5);
+
+#ifndef USE_SSE2
+  /* now e=mm0:mm1 contain the really base-2 exponent */
+  mm0 = _mm_sub_pi32(mm0, *(v2si*)_pi32_0x7f);
+  mm1 = _mm_sub_pi32(mm1, *(v2si*)_pi32_0x7f);
+  v4sf e = _mm_cvtpi32x2_ps(mm0, mm1);
+  _mm_empty(); /* bye bye mmx */
+#else
+  emm0 = _mm_sub_epi32(emm0, *(v4si*)_pi32_0x7f);
+  v4sf e = _mm_cvtepi32_ps(emm0);
+#endif
+
+  e = _mm_add_ps(e, one);
+
+  /* part2: 
+     if( x < SQRTHF ) {
+       e -= 1;
+       x = x + x - 1.0;
+     } else { x = x - 1.0; }
+  */
+  v4sf mask = _mm_cmplt_ps(x, *(v4sf*)_ps_cephes_SQRTHF);
+  v4sf tmp = _mm_and_ps(x, mask);
+  x = _mm_sub_ps(x, one);
+  e = _mm_sub_ps(e, _mm_and_ps(one, mask));
+  x = _mm_add_ps(x, tmp);
+
+
+  v4sf z = _mm_mul_ps(x,x);
+
+  v4sf y = *(v4sf*)_ps_cephes_log_p0;
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p1);
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p2);
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p3);
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p4);
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p5);
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p6);
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p7);
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_log_p8);
+  y = _mm_mul_ps(y, x);
+
+  y = _mm_mul_ps(y, z);
+  
+
+  tmp = _mm_mul_ps(e, *(v4sf*)_ps_cephes_log_q1);
+  y = _mm_add_ps(y, tmp);
+
+
+  tmp = _mm_mul_ps(z, *(v4sf*)_ps_0p5);
+  y = _mm_sub_ps(y, tmp);
+
+  tmp = _mm_mul_ps(e, *(v4sf*)_ps_cephes_log_q2);
+  x = _mm_add_ps(x, y);
+  x = _mm_add_ps(x, tmp);
+  x = _mm_or_ps(x, invalid_mask); // negative arg will be NAN
+  return x;
+}
+
+_PS_CONST(exp_hi,	88.3762626647949f);
+_PS_CONST(exp_lo,	-88.3762626647949f);
+
+_PS_CONST(cephes_LOG2EF, 1.44269504088896341);
+_PS_CONST(cephes_exp_C1, 0.693359375);
+_PS_CONST(cephes_exp_C2, -2.12194440e-4);
+
+_PS_CONST(cephes_exp_p0, 1.9875691500E-4);
+_PS_CONST(cephes_exp_p1, 1.3981999507E-3);
+_PS_CONST(cephes_exp_p2, 8.3334519073E-3);
+_PS_CONST(cephes_exp_p3, 4.1665795894E-2);
+_PS_CONST(cephes_exp_p4, 1.6666665459E-1);
+_PS_CONST(cephes_exp_p5, 5.0000001201E-1);
+
+v4sf exp_ps(v4sf x) {
+  v4sf tmp = _mm_setzero_ps(), fx;
+#ifdef USE_SSE2
+  v4si emm0;
+#else
+  v2si mm0, mm1;
+#endif
+  v4sf one = *(v4sf*)_ps_1;
+
+  x = _mm_min_ps(x, *(v4sf*)_ps_exp_hi);
+  x = _mm_max_ps(x, *(v4sf*)_ps_exp_lo);
+
+  /* express exp(x) as exp(g + n*log(2)) */
+  fx = _mm_mul_ps(x, *(v4sf*)_ps_cephes_LOG2EF);
+  fx = _mm_add_ps(fx, *(v4sf*)_ps_0p5);
+
+  /* how to perform a floorf with SSE: just below */
+#ifndef USE_SSE2
+  /* step 1 : cast to int */
+  tmp = _mm_movehl_ps(tmp, fx);
+  mm0 = _mm_cvttps_pi32(fx);
+  mm1 = _mm_cvttps_pi32(tmp);
+  /* step 2 : cast back to float */
+  tmp = _mm_cvtpi32x2_ps(mm0, mm1);
+#else
+  emm0 = _mm_cvttps_epi32(fx);
+  tmp  = _mm_cvtepi32_ps(emm0);
+#endif
+  /* if greater, substract 1 */
+  v4sf mask = _mm_cmpgt_ps(tmp, fx);    
+  mask = _mm_and_ps(mask, one);
+  fx = _mm_sub_ps(tmp, mask);
+
+  tmp = _mm_mul_ps(fx, *(v4sf*)_ps_cephes_exp_C1);
+  v4sf z = _mm_mul_ps(fx, *(v4sf*)_ps_cephes_exp_C2);
+  x = _mm_sub_ps(x, tmp);
+  x = _mm_sub_ps(x, z);
+
+  z = _mm_mul_ps(x,x);
+  
+  v4sf y = *(v4sf*)_ps_cephes_exp_p0;
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_exp_p1);
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_exp_p2);
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_exp_p3);
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_exp_p4);
+  y = _mm_mul_ps(y, x);
+  y = _mm_add_ps(y, *(v4sf*)_ps_cephes_exp_p5);
+  y = _mm_mul_ps(y, z);
+  y = _mm_add_ps(y, x);
+  y = _mm_add_ps(y, one);
+
+  /* build 2^n */
+#ifndef USE_SSE2
+  z = _mm_movehl_ps(z, fx);
+  mm0 = _mm_cvttps_pi32(fx);
+  mm1 = _mm_cvttps_pi32(z);
+  mm0 = _mm_add_pi32(mm0, *(v2si*)_pi32_0x7f);
+  mm1 = _mm_add_pi32(mm1, *(v2si*)_pi32_0x7f);
+  mm0 = _mm_slli_pi32(mm0, 23); 
+  mm1 = _mm_slli_pi32(mm1, 23);
+  
+  v4sf pow2n; 
+  COPY_MM_TO_XMM(mm0, mm1, pow2n);
+  _mm_empty();
+#else
+  emm0 = _mm_cvttps_epi32(fx);
+  emm0 = _mm_add_epi32(emm0, *(v4si*)_pi32_0x7f);
+  emm0 = _mm_slli_epi32(emm0, 23);
+  v4sf pow2n = _mm_castsi128_ps(emm0);
+#endif
+  y = _mm_mul_ps(y, pow2n);
+  return y;
+}
+
+_PS_CONST(minus_cephes_DP1, -0.78515625);
+_PS_CONST(minus_cephes_DP2, -2.4187564849853515625e-4);
+_PS_CONST(minus_cephes_DP3, -3.77489497744594108e-8);
+_PS_CONST(sincof_p0, -1.9515295891E-4);
+_PS_CONST(sincof_p1,  8.3321608736E-3);
+_PS_CONST(sincof_p2, -1.6666654611E-1);
+_PS_CONST(coscof_p0,  2.443315711809948E-005);
+_PS_CONST(coscof_p1, -1.388731625493765E-003);
+_PS_CONST(coscof_p2,  4.166664568298827E-002);
+_PS_CONST(cephes_FOPI, 1.27323954473516); // 4 / M_PI
+
+
+/* evaluation of 4 sines at onces, using only SSE1+MMX intrinsics so
+   it runs also on old athlons XPs and the pentium III of your grand
+   mother.
+
+   The code is the exact rewriting of the cephes sinf function.
+   Precision is excellent as long as x < 8192 (I did not bother to
+   take into account the special handling they have for greater values
+   -- it does not return garbage for arguments over 8192, though, but
+   the extra precision is missing).
+
+   Note that it is such that sinf((float)M_PI) = 8.74e-8, which is the
+   surprising but correct result.
+
+   Performance is also surprisingly good, 1.33 times faster than the
+   macos vsinf SSE2 function, and 1.5 times faster than the
+   __vrs4_sinf of amd's ACML (which is only available in 64 bits). Not
+   too bad for an SSE1 function (with no special tuning) !
+   However the latter libraries probably have a much better handling of NaN,
+   Inf, denormalized and other special arguments..
+
+   On my core 1 duo, the execution of this function takes approximately 95 cycles.
+
+   From what I have observed on the experiments with Intel AMath lib, switching to an
+   SSE2 version would improve the perf by only 10%.
+
+   Since it is based on SSE intrinsics, it has to be compiled at -O2 to
+   deliver full speed.
+*/
+v4sf sin_ps(v4sf x) { // any x
+  v4sf xmm1, xmm2 = _mm_setzero_ps(), xmm3, sign_bit, y;
+
+#ifdef USE_SSE2
+  v4si emm0, emm2;
+#else
+  v2si mm0, mm1, mm2, mm3;
+#endif
+  sign_bit = x;
+  /* take the absolute value */
+  x = _mm_and_ps(x, *(v4sf*)_ps_inv_sign_mask);
+  /* extract the sign bit (upper one) */
+  sign_bit = _mm_and_ps(sign_bit, *(v4sf*)_ps_sign_mask);
+  
+  /* scale by 4/Pi */
+  y = _mm_mul_ps(x, *(v4sf*)_ps_cephes_FOPI);
+
+#ifdef USE_SSE2
+  /* store the integer part of y in mm0 */
+  emm2 = _mm_cvttps_epi32(y);
+  /* j=(j+1) & (~1) (see the cephes sources) */
+  emm2 = _mm_add_epi32(emm2, *(v4si*)_pi32_1);
+  emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_inv1);
+  y = _mm_cvtepi32_ps(emm2);
+
+  /* get the swap sign flag */
+  emm0 = _mm_and_si128(emm2, *(v4si*)_pi32_4);
+  emm0 = _mm_slli_epi32(emm0, 29);
+  /* get the polynom selection mask 
+     there is one polynom for 0 <= x <= Pi/4
+     and another one for Pi/4<x<=Pi/2
+
+     Both branches will be computed.
+  */
+  emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_2);
+  emm2 = _mm_cmpeq_epi32(emm2, _mm_setzero_si128());
+  
+  v4sf swap_sign_bit = _mm_castsi128_ps(emm0);
+  v4sf poly_mask = _mm_castsi128_ps(emm2);
+  sign_bit = _mm_xor_ps(sign_bit, swap_sign_bit);
+  
+#else
+  /* store the integer part of y in mm0:mm1 */
+  xmm2 = _mm_movehl_ps(xmm2, y);
+  mm2 = _mm_cvttps_pi32(y);
+  mm3 = _mm_cvttps_pi32(xmm2);
+  /* j=(j+1) & (~1) (see the cephes sources) */
+  mm2 = _mm_add_pi32(mm2, *(v2si*)_pi32_1);
+  mm3 = _mm_add_pi32(mm3, *(v2si*)_pi32_1);
+  mm2 = _mm_and_si64(mm2, *(v2si*)_pi32_inv1);
+  mm3 = _mm_and_si64(mm3, *(v2si*)_pi32_inv1);
+  y = _mm_cvtpi32x2_ps(mm2, mm3);
+  /* get the swap sign flag */
+  mm0 = _mm_and_si64(mm2, *(v2si*)_pi32_4);
+  mm1 = _mm_and_si64(mm3, *(v2si*)_pi32_4);
+  mm0 = _mm_slli_pi32(mm0, 29);
+  mm1 = _mm_slli_pi32(mm1, 29);
+  /* get the polynom selection mask */
+  mm2 = _mm_and_si64(mm2, *(v2si*)_pi32_2);
+  mm3 = _mm_and_si64(mm3, *(v2si*)_pi32_2);
+  mm2 = _mm_cmpeq_pi32(mm2, _mm_setzero_si64());
+  mm3 = _mm_cmpeq_pi32(mm3, _mm_setzero_si64());
+  v4sf swap_sign_bit, poly_mask;
+  COPY_MM_TO_XMM(mm0, mm1, swap_sign_bit);
+  COPY_MM_TO_XMM(mm2, mm3, poly_mask);
+  sign_bit = _mm_xor_ps(sign_bit, swap_sign_bit);
+  _mm_empty(); /* good-bye mmx */
+#endif
+  
+  /* The magic pass: "Extended precision modular arithmetic" 
+     x = ((x - y * DP1) - y * DP2) - y * DP3; */
+  xmm1 = *(v4sf*)_ps_minus_cephes_DP1;
+  xmm2 = *(v4sf*)_ps_minus_cephes_DP2;
+  xmm3 = *(v4sf*)_ps_minus_cephes_DP3;
+  xmm1 = _mm_mul_ps(y, xmm1);
+  xmm2 = _mm_mul_ps(y, xmm2);
+  xmm3 = _mm_mul_ps(y, xmm3);
+  x = _mm_add_ps(x, xmm1);
+  x = _mm_add_ps(x, xmm2);
+  x = _mm_add_ps(x, xmm3);
+
+  /* Evaluate the first polynom  (0 <= x <= Pi/4) */
+  y = *(v4sf*)_ps_coscof_p0;
+  v4sf z = _mm_mul_ps(x,x);
+
+  y = _mm_mul_ps(y, z);
+  y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p1);
+  y = _mm_mul_ps(y, z);
+  y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p2);
+  y = _mm_mul_ps(y, z);
+  y = _mm_mul_ps(y, z);
+  v4sf tmp = _mm_mul_ps(z, *(v4sf*)_ps_0p5);
+  y = _mm_sub_ps(y, tmp);
+  y = _mm_add_ps(y, *(v4sf*)_ps_1);
+  
+  /* Evaluate the second polynom  (Pi/4 <= x <= 0) */
+
+  v4sf y2 = *(v4sf*)_ps_sincof_p0;
+  y2 = _mm_mul_ps(y2, z);
+  y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p1);
+  y2 = _mm_mul_ps(y2, z);
+  y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p2);
+  y2 = _mm_mul_ps(y2, z);
+  y2 = _mm_mul_ps(y2, x);
+  y2 = _mm_add_ps(y2, x);
+
+  /* select the correct result from the two polynoms */  
+  xmm3 = poly_mask;
+  y2 = _mm_and_ps(xmm3, y2); //, xmm3);
+  y = _mm_andnot_ps(xmm3, y);
+  y = _mm_add_ps(y,y2);
+  /* update the sign */
+  y = _mm_xor_ps(y, sign_bit);
+  return y;
+}
+
+/* almost the same as sin_ps */
+v4sf cos_ps(v4sf x) { // any x
+  v4sf xmm1, xmm2 = _mm_setzero_ps(), xmm3, y;
+#ifdef USE_SSE2
+  v4si emm0, emm2;
+#else
+  v2si mm0, mm1, mm2, mm3;
+#endif
+  /* take the absolute value */
+  x = _mm_and_ps(x, *(v4sf*)_ps_inv_sign_mask);
+  
+  /* scale by 4/Pi */
+  y = _mm_mul_ps(x, *(v4sf*)_ps_cephes_FOPI);
+  
+#ifdef USE_SSE2
+  /* store the integer part of y in mm0 */
+  emm2 = _mm_cvttps_epi32(y);
+  /* j=(j+1) & (~1) (see the cephes sources) */
+  emm2 = _mm_add_epi32(emm2, *(v4si*)_pi32_1);
+  emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_inv1);
+  y = _mm_cvtepi32_ps(emm2);
+
+  emm2 = _mm_sub_epi32(emm2, *(v4si*)_pi32_2);
+  
+  /* get the swap sign flag */
+  emm0 = _mm_andnot_si128(emm2, *(v4si*)_pi32_4);
+  emm0 = _mm_slli_epi32(emm0, 29);
+  /* get the polynom selection mask */
+  emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_2);
+  emm2 = _mm_cmpeq_epi32(emm2, _mm_setzero_si128());
+  
+  v4sf sign_bit = _mm_castsi128_ps(emm0);
+  v4sf poly_mask = _mm_castsi128_ps(emm2);
+#else
+  /* store the integer part of y in mm0:mm1 */
+  xmm2 = _mm_movehl_ps(xmm2, y);
+  mm2 = _mm_cvttps_pi32(y);
+  mm3 = _mm_cvttps_pi32(xmm2);
+
+  /* j=(j+1) & (~1) (see the cephes sources) */
+  mm2 = _mm_add_pi32(mm2, *(v2si*)_pi32_1);
+  mm3 = _mm_add_pi32(mm3, *(v2si*)_pi32_1);
+  mm2 = _mm_and_si64(mm2, *(v2si*)_pi32_inv1);
+  mm3 = _mm_and_si64(mm3, *(v2si*)_pi32_inv1);
+
+  y = _mm_cvtpi32x2_ps(mm2, mm3);
+
+
+  mm2 = _mm_sub_pi32(mm2, *(v2si*)_pi32_2);
+  mm3 = _mm_sub_pi32(mm3, *(v2si*)_pi32_2);
+
+  /* get the swap sign flag in mm0:mm1 and the 
+     polynom selection mask in mm2:mm3 */
+
+  mm0 = _mm_andnot_si64(mm2, *(v2si*)_pi32_4);
+  mm1 = _mm_andnot_si64(mm3, *(v2si*)_pi32_4);
+  mm0 = _mm_slli_pi32(mm0, 29);
+  mm1 = _mm_slli_pi32(mm1, 29);
+
+  mm2 = _mm_and_si64(mm2, *(v2si*)_pi32_2);
+  mm3 = _mm_and_si64(mm3, *(v2si*)_pi32_2);
+
+  mm2 = _mm_cmpeq_pi32(mm2, _mm_setzero_si64());
+  mm3 = _mm_cmpeq_pi32(mm3, _mm_setzero_si64());
+
+  v4sf sign_bit, poly_mask;
+  COPY_MM_TO_XMM(mm0, mm1, sign_bit);
+  COPY_MM_TO_XMM(mm2, mm3, poly_mask);
+  _mm_empty(); /* good-bye mmx */
+#endif
+  /* The magic pass: "Extended precision modular arithmetic" 
+     x = ((x - y * DP1) - y * DP2) - y * DP3; */
+  xmm1 = *(v4sf*)_ps_minus_cephes_DP1;
+  xmm2 = *(v4sf*)_ps_minus_cephes_DP2;
+  xmm3 = *(v4sf*)_ps_minus_cephes_DP3;
+  xmm1 = _mm_mul_ps(y, xmm1);
+  xmm2 = _mm_mul_ps(y, xmm2);
+  xmm3 = _mm_mul_ps(y, xmm3);
+  x = _mm_add_ps(x, xmm1);
+  x = _mm_add_ps(x, xmm2);
+  x = _mm_add_ps(x, xmm3);
+  
+  /* Evaluate the first polynom  (0 <= x <= Pi/4) */
+  y = *(v4sf*)_ps_coscof_p0;
+  v4sf z = _mm_mul_ps(x,x);
+
+  y = _mm_mul_ps(y, z);
+  y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p1);
+  y = _mm_mul_ps(y, z);
+  y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p2);
+  y = _mm_mul_ps(y, z);
+  y = _mm_mul_ps(y, z);
+  v4sf tmp = _mm_mul_ps(z, *(v4sf*)_ps_0p5);
+  y = _mm_sub_ps(y, tmp);
+  y = _mm_add_ps(y, *(v4sf*)_ps_1);
+  
+  /* Evaluate the second polynom  (Pi/4 <= x <= 0) */
+
+  v4sf y2 = *(v4sf*)_ps_sincof_p0;
+  y2 = _mm_mul_ps(y2, z);
+  y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p1);
+  y2 = _mm_mul_ps(y2, z);
+  y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p2);
+  y2 = _mm_mul_ps(y2, z);
+  y2 = _mm_mul_ps(y2, x);
+  y2 = _mm_add_ps(y2, x);
+
+  /* select the correct result from the two polynoms */  
+  xmm3 = poly_mask;
+  y2 = _mm_and_ps(xmm3, y2); //, xmm3);
+  y = _mm_andnot_ps(xmm3, y);
+  y = _mm_add_ps(y,y2);
+  /* update the sign */
+  y = _mm_xor_ps(y, sign_bit);
+
+  return y;
+}
+
+/* since sin_ps and cos_ps are almost identical, sincos_ps could replace both of them..
+   it is almost as fast, and gives you a free cosine with your sine */
+void sincos_ps(v4sf x, v4sf *s, v4sf *c) {
+  v4sf xmm1, xmm2, xmm3 = _mm_setzero_ps(), sign_bit_sin, y;
+#ifdef USE_SSE2
+  v4si emm0, emm2, emm4;
+#else
+  v2si mm0, mm1, mm2, mm3, mm4, mm5;
+#endif
+  sign_bit_sin = x;
+  /* take the absolute value */
+  x = _mm_and_ps(x, *(v4sf*)_ps_inv_sign_mask);
+  /* extract the sign bit (upper one) */
+  sign_bit_sin = _mm_and_ps(sign_bit_sin, *(v4sf*)_ps_sign_mask);
+  
+  /* scale by 4/Pi */
+  y = _mm_mul_ps(x, *(v4sf*)_ps_cephes_FOPI);
+    
+#ifdef USE_SSE2
+  /* store the integer part of y in emm2 */
+  emm2 = _mm_cvttps_epi32(y);
+
+  /* j=(j+1) & (~1) (see the cephes sources) */
+  emm2 = _mm_add_epi32(emm2, *(v4si*)_pi32_1);
+  emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_inv1);
+  y = _mm_cvtepi32_ps(emm2);
+
+  emm4 = emm2;
+
+  /* get the swap sign flag for the sine */
+  emm0 = _mm_and_si128(emm2, *(v4si*)_pi32_4);
+  emm0 = _mm_slli_epi32(emm0, 29);
+  v4sf swap_sign_bit_sin = _mm_castsi128_ps(emm0);
+
+  /* get the polynom selection mask for the sine*/
+  emm2 = _mm_and_si128(emm2, *(v4si*)_pi32_2);
+  emm2 = _mm_cmpeq_epi32(emm2, _mm_setzero_si128());
+  v4sf poly_mask = _mm_castsi128_ps(emm2);
+#else
+  /* store the integer part of y in mm2:mm3 */
+  xmm3 = _mm_movehl_ps(xmm3, y);
+  mm2 = _mm_cvttps_pi32(y);
+  mm3 = _mm_cvttps_pi32(xmm3);
+
+  /* j=(j+1) & (~1) (see the cephes sources) */
+  mm2 = _mm_add_pi32(mm2, *(v2si*)_pi32_1);
+  mm3 = _mm_add_pi32(mm3, *(v2si*)_pi32_1);
+  mm2 = _mm_and_si64(mm2, *(v2si*)_pi32_inv1);
+  mm3 = _mm_and_si64(mm3, *(v2si*)_pi32_inv1);
+
+  y = _mm_cvtpi32x2_ps(mm2, mm3);
+
+  mm4 = mm2;
+  mm5 = mm3;
+
+  /* get the swap sign flag for the sine */
+  mm0 = _mm_and_si64(mm2, *(v2si*)_pi32_4);
+  mm1 = _mm_and_si64(mm3, *(v2si*)_pi32_4);
+  mm0 = _mm_slli_pi32(mm0, 29);
+  mm1 = _mm_slli_pi32(mm1, 29);
+  v4sf swap_sign_bit_sin;
+  COPY_MM_TO_XMM(mm0, mm1, swap_sign_bit_sin);
+
+  /* get the polynom selection mask for the sine */
+
+  mm2 = _mm_and_si64(mm2, *(v2si*)_pi32_2);
+  mm3 = _mm_and_si64(mm3, *(v2si*)_pi32_2);
+  mm2 = _mm_cmpeq_pi32(mm2, _mm_setzero_si64());
+  mm3 = _mm_cmpeq_pi32(mm3, _mm_setzero_si64());
+  v4sf poly_mask;
+  COPY_MM_TO_XMM(mm2, mm3, poly_mask);
+#endif
+
+  /* The magic pass: "Extended precision modular arithmetic" 
+     x = ((x - y * DP1) - y * DP2) - y * DP3; */
+  xmm1 = *(v4sf*)_ps_minus_cephes_DP1;
+  xmm2 = *(v4sf*)_ps_minus_cephes_DP2;
+  xmm3 = *(v4sf*)_ps_minus_cephes_DP3;
+  xmm1 = _mm_mul_ps(y, xmm1);
+  xmm2 = _mm_mul_ps(y, xmm2);
+  xmm3 = _mm_mul_ps(y, xmm3);
+  x = _mm_add_ps(x, xmm1);
+  x = _mm_add_ps(x, xmm2);
+  x = _mm_add_ps(x, xmm3);
+
+#ifdef USE_SSE2
+  emm4 = _mm_sub_epi32(emm4, *(v4si*)_pi32_2);
+  emm4 = _mm_andnot_si128(emm4, *(v4si*)_pi32_4);
+  emm4 = _mm_slli_epi32(emm4, 29);
+  v4sf sign_bit_cos = _mm_castsi128_ps(emm4);
+#else
+  /* get the sign flag for the cosine */
+  mm4 = _mm_sub_pi32(mm4, *(v2si*)_pi32_2);
+  mm5 = _mm_sub_pi32(mm5, *(v2si*)_pi32_2);
+  mm4 = _mm_andnot_si64(mm4, *(v2si*)_pi32_4);
+  mm5 = _mm_andnot_si64(mm5, *(v2si*)_pi32_4);
+  mm4 = _mm_slli_pi32(mm4, 29);
+  mm5 = _mm_slli_pi32(mm5, 29);
+  v4sf sign_bit_cos;
+  COPY_MM_TO_XMM(mm4, mm5, sign_bit_cos);
+  _mm_empty(); /* good-bye mmx */
+#endif
+
+  sign_bit_sin = _mm_xor_ps(sign_bit_sin, swap_sign_bit_sin);
+
+  
+  /* Evaluate the first polynom  (0 <= x <= Pi/4) */
+  v4sf z = _mm_mul_ps(x,x);
+  y = *(v4sf*)_ps_coscof_p0;
+
+  y = _mm_mul_ps(y, z);
+  y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p1);
+  y = _mm_mul_ps(y, z);
+  y = _mm_add_ps(y, *(v4sf*)_ps_coscof_p2);
+  y = _mm_mul_ps(y, z);
+  y = _mm_mul_ps(y, z);
+  v4sf tmp = _mm_mul_ps(z, *(v4sf*)_ps_0p5);
+  y = _mm_sub_ps(y, tmp);
+  y = _mm_add_ps(y, *(v4sf*)_ps_1);
+  
+  /* Evaluate the second polynom  (Pi/4 <= x <= 0) */
+
+  v4sf y2 = *(v4sf*)_ps_sincof_p0;
+  y2 = _mm_mul_ps(y2, z);
+  y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p1);
+  y2 = _mm_mul_ps(y2, z);
+  y2 = _mm_add_ps(y2, *(v4sf*)_ps_sincof_p2);
+  y2 = _mm_mul_ps(y2, z);
+  y2 = _mm_mul_ps(y2, x);
+  y2 = _mm_add_ps(y2, x);
+
+  /* select the correct result from the two polynoms */  
+  xmm3 = poly_mask;
+  v4sf ysin2 = _mm_and_ps(xmm3, y2);
+  v4sf ysin1 = _mm_andnot_ps(xmm3, y);
+  y2 = _mm_sub_ps(y2,ysin2);
+  y = _mm_sub_ps(y, ysin1);
+
+  xmm1 = _mm_add_ps(ysin1,ysin2);
+  xmm2 = _mm_add_ps(y,y2);
+ 
+  /* update the sign */
+  *s = _mm_xor_ps(xmm1, sign_bit_sin);
+  *c = _mm_xor_ps(xmm2, sign_bit_cos);
+}
+
Index: src/RealFFTf.cpp
===================================================================
--- src/RealFFTf.cpp	(revision 12939)
+++ src/RealFFTf.cpp	(working copy)
@@ -36,15 +36,48 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#ifndef USE_SSE2
+#define	USE_SSE2
+#endif
+
 #include <stdlib.h>
 #include <stdio.h>
 #include <math.h>
 #include "RealFFTf.h"
+#ifdef __WXMSW__
+#pragma warning(disable:4305)
+#endif
+#include "sse_mathfun.h"
+#ifdef __WXMSW__
+#pragma warning(default:4305)
+#endif
+#include <xmmintrin.h>
 
+
 #ifndef M_PI
 #define	M_PI		3.14159265358979323846  /* pi */
 #endif
 
+unsigned char smallReverseBitsTable[256];
+
+int tableMask=0;
+bool useBitReverseTable=false;
+bool useSinCosTable=false;
+
+void TableUsage(int iMask)
+{
+	tableMask=iMask;
+	useBitReverseTable=(bool)(iMask&1);
+	useSinCosTable=(bool)(iMask&2);
+}
+
+// note !!! number of bits must be between 9-16 
+int SmallReverseBits(int bits, int numberBits)
+{
+	return (smallReverseBitsTable[*((unsigned char *)&bits)]<<(numberBits-8))+(smallReverseBitsTable[*(((unsigned char *)&bits)+1)]>>(16-numberBits));
+}
+
+
 /*
  *  Initialize the Sine table and Twiddle pointers (bit-reversed pointers)
  *  for the FFT routine.
@@ -86,13 +119,51 @@
          temp=(temp >> 1) + (i&mask ? h->Points : 0);
 
       h->BitReversed[i]=temp;
+   } 
+
+/*   for(i=0;i<h->Points;i++)
+   {
+      temp=0;
+	  for(int maskLow=1, maskHigh=h->Points;maskLow<h->Points;maskLow<<=1,maskHigh>>=1) 
+		  if(i&maskLow)
+			  temp|=maskHigh;
+
+      h->BitReversed[i]=temp;
    }
+*/
+	// testing sin cos acc
 
    for(i=0;i<h->Points;i++)
    {
+/*	   int bRev=h->BitReversed[i];
+	   v4sf vx, sin4_2, cos4_2;
+	  vx.m128_f32[0]=M_PI*i/h->Points;
+	   sincos_ps(vx, &sin4_2, &cos4_2);
+	   h->SinTable[h->BitReversed[i]  ]=(fft_type)-sin4_2.m128_f32[0];
+      h->SinTable[h->BitReversed[i]+1]=(fft_type)-cos4_2.m128_f32[0];
+*/
       h->SinTable[h->BitReversed[i]  ]=(fft_type)-sin(2*M_PI*i/(2*h->Points));
       h->SinTable[h->BitReversed[i]+1]=(fft_type)-cos(2*M_PI*i/(2*h->Points));
    }
+
+   // this needs to move out but ehh... Andrew Hallendorff
+   for(i=0;i<256;i++) {
+	  smallReverseBitsTable[i]=0;
+	  for(int maskLow=1, maskHigh=128;maskLow<256;maskLow<<=1,maskHigh>>=1) 
+		  if(i&maskLow)
+			  smallReverseBitsTable[i]|=maskHigh;
+   }
+   // new SSE FFT routines work on live data
+   for(i=0;i<32;i++)
+	   if((1<<i)&fftlen)
+           h->pow2Bits=i;
+//   for(i=0;i<h->Points;i++) {
+//	   int bitRevTable=h->BitReversed[i], bitRevFun=SmallReverseBits(i,h->pow2Bits);
+//	   if(bitRevTable!=bitRevFun)
+//		   bitRevFun=bitRevTable;
+  // }
+
+
    return h;
 }
 
@@ -380,3 +451,296 @@
       TimeOut[i*2+1]=buffer[hFFT->BitReversed[i]+1];
    }
 }
+
+
+// 4x processing simd
+
+void RealFFTf4x(fft_type *buffer,HFFT h)
+{
+
+	__m128 *localBuffer=(__m128 *)buffer;
+
+   __m128 *A,*B;
+   fft_type *sptr;
+   __m128 *endptr1,*endptr2;
+   int br1Index, br2Index;
+   int br1Value, br2Value;
+   __m128 HRplus,HRminus,HIplus,HIminus;
+   __m128 v1,v2,sin,cos;
+   fft_type iToRad=2*M_PI/(2*h->Points);
+
+   int ButterfliesPerGroup=h->Points/2;
+
+   /*
+    *  Butterfly:
+    *     Ain-----Aout
+    *         \ /
+    *         / \
+    *     Bin-----Bout
+    */
+
+   endptr1=&localBuffer[h->Points*2];
+
+   while(ButterfliesPerGroup>0)
+   {
+      A=localBuffer;
+      B=&localBuffer[ButterfliesPerGroup*2];
+      sptr=h->SinTable;
+	  int iSinCosIndex=0;
+
+      while(A<endptr1)
+      {
+	   if(useSinCosTable) {
+		   sin=_mm_set1_ps(*(sptr++));
+		   cos=_mm_set1_ps(*(sptr++));
+	   } else {
+		   v4sf vx, sin4_2, cos4_2;
+		   vx.m128_f32[0]=((fft_type )SmallReverseBits(iSinCosIndex,h->pow2Bits-1))*iToRad;
+		   sincos_ps(vx, &sin4_2, &cos4_2);
+		   sin=_mm_set1_ps(-sin4_2.m128_f32[0]);
+		   cos=_mm_set1_ps(-cos4_2.m128_f32[0]);
+		   iSinCosIndex++;
+	   }
+         endptr2=B;
+         while(A<endptr2)
+         {
+			v1 = _mm_add_ps( _mm_mul_ps(*B, cos), _mm_mul_ps(*(B+1), sin));
+			v2 = _mm_sub_ps( _mm_mul_ps(*B, sin), _mm_mul_ps(*(B+1), cos));
+            *B=_mm_add_ps( *A, v1);
+			__m128 temp128 = _mm_set1_ps( 2.0); 
+            *(A++)=_mm_sub_ps(*(B++), _mm_mul_ps(temp128, v1));
+            *B=_mm_sub_ps(*A,v2);
+            *(A++)=_mm_add_ps(*(B++), _mm_mul_ps(temp128, v2));
+         }
+         A=B;
+         B=&B[ButterfliesPerGroup*2];
+      }
+      ButterfliesPerGroup >>= 1;
+   }
+   /* Massage output to get the output for a real input sequence. */
+   
+   br1Index=1; // h->BitReversed+1;
+   br2Index=h->Points-1;   //h->BitReversed+h->Points-1;
+
+   while(br1Index<br2Index)
+   {
+	   if(useBitReverseTable) {
+		   br1Value=h->BitReversed[br1Index];
+		   br2Value=h->BitReversed[br2Index];
+	   } else {
+		  br1Value=SmallReverseBits(br1Index,h->pow2Bits);
+		  br2Value=SmallReverseBits(br2Index,h->pow2Bits);
+	   }
+	   if(useSinCosTable) {
+			sin=_mm_set1_ps(h->SinTable[br1Value]);
+			cos=_mm_set1_ps(h->SinTable[br1Value+1]);
+	   } else {
+		   v4sf vx, sin4_2, cos4_2;
+		   vx.m128_f32[0]=br1Index*iToRad;
+		   sincos_ps(vx, &sin4_2, &cos4_2);
+		   sin=_mm_set1_ps(-sin4_2.m128_f32[0]);
+		   cos=_mm_set1_ps(-cos4_2.m128_f32[0]);
+	   }
+
+      A=&localBuffer[br1Value];
+      B=&localBuffer[br2Value];
+	  __m128 temp128 = _mm_set1_ps( 2.0);
+	  HRplus = _mm_add_ps(HRminus = _mm_sub_ps( *A, *B ), _mm_mul_ps(*B, temp128));
+      HIplus = _mm_add_ps(HIminus = _mm_sub_ps(*(A+1), *(B+1) ), _mm_mul_ps(*(B+1), temp128));
+      v1 = _mm_sub_ps(_mm_mul_ps(sin, HRminus), _mm_mul_ps(cos, HIplus));
+      v2 = _mm_add_ps(_mm_mul_ps(cos, HRminus), _mm_mul_ps(sin, HIplus));
+	  temp128 = _mm_set1_ps( 0.5);
+      *A = _mm_mul_ps(_mm_add_ps(HRplus, v1), temp128);
+      *B = _mm_sub_ps(*A, v1);
+      *(A+1) = _mm_mul_ps(_mm_add_ps(HIminus, v2), temp128);
+      *(B+1) = _mm_sub_ps(*(A+1), HIminus);
+
+      br1Index++;
+      br2Index--;
+   }
+   /* Handle the center bin (just need a conjugate) */
+   if(useBitReverseTable) 
+	   A=&localBuffer[h->BitReversed[br1Index]+1];
+   else
+	   A=&localBuffer[SmallReverseBits(br1Index,h->pow2Bits)+1];
+   // negate sse style
+   *A=_mm_xor_ps(*A, _mm_set1_ps(-0.f));
+   /* Handle DC and Fs/2 bins separately */
+   /* Put the Fs/2 value into the imaginary part of the DC bin */
+   v1=_mm_sub_ps(localBuffer[0], localBuffer[1]);
+   localBuffer[0]=_mm_add_ps(localBuffer[0], localBuffer[1]);
+   localBuffer[1]=v1;
+}
+
+
+/* Description: This routine performs an inverse FFT to real data.
+ *              This code is for floating point data.
+ *
+ *  Note: Output is BIT-REVERSED! so you must use the BitReversed to
+ *        get legible output, (i.e. wave[2*i]   = buffer[ BitReversed[i] ]
+ *                                  wave[2*i+1] = buffer[ BitReversed[i]+1 ] )
+ *        Input is in normal order, interleaved (real,imaginary) complex data
+ *        You must call InitializeFFT(fftlen) first to initialize some buffers!
+ *
+ * Input buffer[0] is the DC bin, and input buffer[1] is the Fs/2 bin
+ * - this can be done because both values will always be real only
+ * - this allows us to not have to allocate an extra complex value for the Fs/2 bin
+ *
+ *  Note: The scaling on this is done according to the standard FFT definition,
+ *        so a unit amplitude DC signal will output an amplitude of (N)
+ *        (Older revisions would progressively scale the input, so the output
+ *        values would be similar in amplitude to the input values, which is
+ *        good when using fixed point arithmetic)
+ */
+void InverseRealFFTf4x(fft_type *buffer,HFFT h)
+{
+
+   __m128 *localBuffer=(__m128 *)buffer;
+
+   __m128 *A,*B;
+   fft_type *sptr;
+   __m128 *endptr1,*endptr2;
+   int br1Index, br1Value;
+   __m128 HRplus,HRminus,HIplus,HIminus;
+   __m128 v1,v2,sin,cos;
+   fft_type iToRad=2*M_PI/(2*h->Points);
+
+
+   int ButterfliesPerGroup=h->Points/2;
+
+   /* Massage input to get the input for a real output sequence. */
+   A=localBuffer+2;
+   B=localBuffer+h->Points*2-2;
+   br1Index=1; //h->BitReversed+1;
+   while(A<B)
+   {
+	   if(useBitReverseTable) {
+		   br1Value=h->BitReversed[br1Index];
+	   } else {
+		  br1Value=SmallReverseBits(br1Index,h->pow2Bits);
+	   }
+	   if(useSinCosTable) {
+			sin=_mm_set1_ps(h->SinTable[br1Value]);
+	  		cos=_mm_set1_ps(h->SinTable[br1Value+1]);
+	   } else {
+		   v4sf vx, sin4_2, cos4_2;
+		   vx.m128_f32[0]=br1Index*iToRad;
+		   sincos_ps(vx, &sin4_2, &cos4_2);
+		   sin=_mm_set1_ps(-sin4_2.m128_f32[0]);
+		   cos=_mm_set1_ps(-cos4_2.m128_f32[0]);
+	   }
+	  HRminus = _mm_sub_ps(*A,  *B);
+      HRplus = _mm_add_ps(HRminus, _mm_mul_ps(*B,  _mm_set1_ps(2.0)));
+	  HIminus = _mm_sub_ps( *(A+1), *(B+1));
+      HIplus = _mm_add_ps(HIminus,  _mm_mul_ps(*(B+1), _mm_set1_ps(2.0)));
+      v1 = _mm_add_ps(_mm_mul_ps(sin, HRminus), _mm_mul_ps(cos, HIplus));
+      v2 = _mm_sub_ps(_mm_mul_ps(cos, HRminus), _mm_mul_ps(sin, HIplus));
+      *A = _mm_mul_ps(_mm_add_ps(HRplus, v1), _mm_set1_ps(0.5));
+      *B = _mm_sub_ps(*A, v1);
+      *(A+1) = _mm_mul_ps(_mm_sub_ps(HIminus, v2) , _mm_set1_ps(0.5));
+      *(B+1) = _mm_sub_ps(*(A+1), HIminus);
+
+      A=&A[2];
+      B=&B[-2];
+      br1Index++;
+   }
+   /* Handle center bin (just need conjugate) */
+      // negate sse style
+   *(A+1)=_mm_xor_ps(*(A+1), _mm_set1_ps(-0.f));
+
+   /* Handle DC bin separately - this ignores any Fs/2 component
+   buffer[1]=buffer[0]=buffer[0]/2;*/
+   /* Handle DC and Fs/2 bins specially */
+   /* The DC bin is passed in as the real part of the DC complex value */
+   /* The Fs/2 bin is passed in as the imaginary part of the DC complex value */
+   /* (v1+v2) = buffer[0] == the DC component */
+   /* (v1-v2) = buffer[1] == the Fs/2 component */
+   v1=_mm_mul_ps(_mm_set1_ps(0.5), _mm_add_ps(localBuffer[0], localBuffer[1]));
+   v2=_mm_mul_ps(_mm_set1_ps(0.5), _mm_sub_ps(localBuffer[0], localBuffer[1]));
+   localBuffer[0]=v1;
+   localBuffer[1]=v2;
+
+   /*
+    *  Butterfly:
+    *     Ain-----Aout
+    *         \ /
+    *         / \
+    *     Bin-----Bout
+    */
+
+   endptr1=localBuffer+h->Points*2;
+
+   while(ButterfliesPerGroup>0)
+   {
+      A=localBuffer;
+      B=localBuffer+ButterfliesPerGroup*2;
+      sptr=h->SinTable;
+	  int iSinCosIndex=0;
+
+      while(A<endptr1)
+      {
+		if(useSinCosTable) {
+			sin=_mm_set1_ps(*(sptr++));
+			cos=_mm_set1_ps(*(sptr++));
+		} else {
+		   v4sf vx, sin4_2, cos4_2;
+		   vx.m128_f32[0]=((fft_type )SmallReverseBits(iSinCosIndex,h->pow2Bits-1))*iToRad;
+		   sincos_ps(vx, &sin4_2, &cos4_2);
+		   sin=_mm_set1_ps(-sin4_2.m128_f32[0]);
+		   cos=_mm_set1_ps(-cos4_2.m128_f32[0]);
+		   iSinCosIndex++;
+		}
+         endptr2=B;
+         while(A<endptr2)
+         {
+            v1=_mm_sub_ps( _mm_mul_ps(*B, cos), _mm_mul_ps(*(B+1), sin));
+            v2=_mm_add_ps( _mm_mul_ps(*B, sin), _mm_mul_ps(*(B+1), cos));
+            *B=_mm_mul_ps( _mm_add_ps(*A, v1), _mm_set1_ps(0.5));
+            *(A++)=_mm_sub_ps(*(B++), v1);
+            *B=_mm_mul_ps(_mm_add_ps(*A, v2), _mm_set1_ps(0.5));
+            *(A++)=_mm_sub_ps(*(B++),v2);
+         }
+         A=B;
+         B=&B[ButterfliesPerGroup*2];
+      }
+      ButterfliesPerGroup >>= 1;
+   }
+}
+
+void ReorderToFreq4x(HFFT hFFT, fft_type *buffer, fft_type *RealOut, fft_type *ImagOut)
+{
+   __m128 *localBuffer=(__m128 *)buffer;
+   __m128 *localRealOut=(__m128 *)RealOut;
+   __m128 *localImagOut=(__m128 *)ImagOut;
+
+   // Copy the data into the real and imaginary outputs
+   for(int i=1;i<hFFT->Points;i++) {
+      int brValue;
+	   if(useBitReverseTable) 
+		   brValue=hFFT->BitReversed[i];
+	   else
+		   brValue=SmallReverseBits(i,hFFT->pow2Bits);
+      localRealOut[i]=localBuffer[brValue  ];
+      localImagOut[i]=localBuffer[brValue+1];
+   }
+   localRealOut[0] = localBuffer[0]; // DC component
+   localImagOut[0] = _mm_set1_ps(0.0);
+   localRealOut[hFFT->Points] = localBuffer[1]; // Fs/2 component
+   localImagOut[hFFT->Points] = _mm_set1_ps(0.0);
+}
+
+void ReorderToTime4x(HFFT hFFT, fft_type *buffer, fft_type *TimeOut)
+{
+   __m128 *localBuffer=(__m128 *)buffer;
+   __m128 *localTimeOut=(__m128 *)TimeOut;
+   // Copy the data into the real outputs
+   for(int i=0;i<hFFT->Points;i++) {
+      int brValue;
+	   if(useBitReverseTable) 
+		   brValue=hFFT->BitReversed[i];
+	   else
+		   brValue=SmallReverseBits(i,hFFT->pow2Bits);
+      localTimeOut[i*2  ]=localBuffer[brValue  ];
+      localTimeOut[i*2+1]=localBuffer[brValue+1];
+   }
+}
Index: src/RealFFTf.h
===================================================================
--- src/RealFFTf.h	(revision 12939)
+++ src/RealFFTf.h	(working copy)
@@ -6,6 +6,7 @@
    int *BitReversed;
    fft_type *SinTable;
    int Points;
+   int pow2Bits;
 } FFTParam;
 #define HFFT FFTParam *
 
@@ -19,5 +20,14 @@
 void ReorderToTime(HFFT hFFT, fft_type *buffer, fft_type *TimeOut);
 void ReorderToFreq(HFFT hFFT, fft_type *buffer, fft_type *RealOut, fft_type *ImagOut);
 
+
+// Added by Andrew Hallendorff
+int SmallReverseBits(int bits, int numberBits);
+void RealFFTf4x(fft_type *,HFFT);
+void InverseRealFFTf4x(fft_type *,HFFT);
+void ReorderToTime4x(HFFT hFFT, fft_type *buffer, fft_type *TimeOut);
+void ReorderToFreq4x(HFFT hFFT, fft_type *buffer, fft_type *RealOut, fft_type *ImagOut);
+void TableUsage(int iMask);
+
 #endif
 
