PartitionedConvolutionTrig.cpp

/*
	SuperCollider real time audio synthesis system
 Copyright (c) 2002 James McCartney. All rights reserved.
	http://www.audiosynth.com

 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.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program; if not, write to the Free Software
 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
 */

//Partitioned Convolution, Nick Collins mid October 2008
//Victor Bombi 2016
//PartConvT(in, fftsize, irbufnum, trig)


#include "FFT_UGens.h"

#include <stdio.h>

static SndBuf * GetBufferN(Unit * unit, uint32 bufnum, const char * ugenName)
{
	//printf("GetBufferN\n");
	SndBuf *buf;
	World *world = unit->mWorld;

	if (bufnum >= world->mNumSndBufs) {
		int localBufNum = bufnum - world->mNumSndBufs;
		Graph *parent = unit->mParent;
		if (localBufNum <= parent->localMaxBufNum) {
			buf = parent->mLocalSndBufs + localBufNum;
		} else {
			if (unit->mWorld->mVerbosity > -1)
				Print("GetBuffer %s: invalid buffer number (%d).\n", ugenName, bufnum);
			return NULL;
		}
	} else {
		buf = world->mSndBufs + bufnum;
	}

	if (buf->data == NULL) {
		if (unit->mWorld->mVerbosity > -1)
			Print("GetBuffer %s: uninitialized buffer (%i).\n", ugenName, bufnum);
		return NULL;
	}

	return buf;

}
void DoPreparePartConvT(World *world, float *data1, SndBuf* frombuf,int fftsize );

struct PartConvT : public Unit {
	int m_counter;
	uint32 m_specbufnumcheck;
	float* m_fd_accumulate; //will be exactly fftsize*frames in size
	float * m_irspectra;
	int m_fd_accum_pos;
	int m_partitions; //number of frames impulse response is partitioned into
	int m_fullsize;

	//sliding window code
	int m_fftsize; //must be power of two
	//int m_windowsize; //also half fftsize, was partition size, just use nover2 for this
	int m_nover2;

	//int m_hopsize; //hopsize will be half fftsize
	//int m_shuntsize;
	int m_pos;
	float * m_inputbuf;
	float * m_spectrum;
	scfft* m_scfft;
	float * m_inputbuf2;
	float * m_spectrum2;
	scfft* m_scifft; //inverse
	int m_outputpos;
	float * m_output;

	//amortisation state
	int m_blocksize, m_sr;
	int m_spareblocks;
	int m_numamort; //will relate number of partitions to number of spare blocks
	int m_lastamort;
	int m_amortcount;
	int m_partitionsdone;
	//
	int m_prevtrig;
	SndBuf * m_buf;
};

extern "C" {
	void PartConvT_next(PartConvT *unit, int inNumSamples);
	void PartConvT_Ctor(PartConvT* unit);
	void PartConvT_Dtor(PartConvT* unit);
}

void PartConvT_Ctor( PartConvT* unit )
{
	unit->m_fftsize= (int) ZIN0(1);
	unit->m_nover2= unit->m_fftsize>>1;

	unit->m_inputbuf= (float*)RTAlloc(unit->mWorld, unit->m_fftsize * sizeof(float));
	unit->m_spectrum= (float*)RTAlloc(unit->mWorld, unit->m_fftsize * sizeof(float));

	SCWorld_Allocator alloc(ft, unit->mWorld);
	unit->m_scfft = scfft_create(unit->m_fftsize, unit->m_fftsize, kRectWindow, unit->m_inputbuf, unit->m_spectrum, kForward, alloc);

	//inverse
	unit->m_inputbuf2= (float*)RTAlloc(unit->mWorld, unit->m_fftsize * sizeof(float));
	unit->m_spectrum2= (float*)RTAlloc(unit->mWorld, unit->m_fftsize * sizeof(float));
	//in place this time
	unit->m_scifft = scfft_create(unit->m_fftsize, unit->m_fftsize, kRectWindow, unit->m_inputbuf2, unit->m_spectrum2, kBackward, alloc);

	//debug test: changing scale factors in case amplitude summation is a problem
	//unit->m_scfft->scalefac=1.0/45.254833995939;
	//unit->m_scifft->scalefac=1.0/45.254833995939;

	unit->m_output= (float*)RTAlloc(unit->mWorld, unit->m_fftsize * sizeof(float));
	unit->m_outputpos=0;

	memset(unit->m_output, 0, unit->m_fftsize * sizeof(float));
	memset(unit->m_inputbuf, 0, unit->m_fftsize * sizeof(float));
	unit->m_pos=0;

	//get passed in buffer
	unit->m_fd_accumulate=NULL;

	uint32 bufnum = (uint32)ZIN0(2);
	SndBuf *buf = GetBufferN(unit, bufnum,"PartConvT");
	if (!buf) {
		printf("PartConvT Error: Spectral data buffer not allocated \n");
		SETCALC(*ClearUnitOutputs);
		unit->mDone = true;
		return;
	}
	unit->m_buf = buf;
	
	//allocate needed partconv buffer
	int nover2= unit->m_nover2;
	int numpartitions;
	int frames = buf->frames;
	if(frames % nover2 == 0){
		numpartitions= frames/nover2;
	}else{
		numpartitions= (frames/nover2)+1;
	}
	int irssize = numpartitions * unit->m_fftsize;
	unit->m_irspectra = (float*)RTAlloc(unit->mWorld, irssize * sizeof(float));
	unit->m_fullsize = irssize;
	unit->m_partitions = numpartitions;
	//printf("another check partitions %d irspecsize %d fftsize %d \n", unit->m_partitions,unit->m_fullsize, unit->m_fftsize);


	//CHECK SAMPLING RATE AND BUFFER SIZE
	unit->m_blocksize = unit->mWorld->mFullRate.mBufLength;
	//if(unit->m_blocksize!=64) printf("TPV complains: block size not 64, you have %d\n", unit->m_blocksize);
	unit->m_sr = unit->mWorld->mSampleRate;
	//if(unit->m_sr!=44100) printf("TPV complains: sample rate not 44100, you have %d\n", unit->m_sr);

	if(unit->m_nover2 % unit->m_blocksize !=0) {
		printf("PartConvT Error: block size doesn't divide partition size\n");
		SETCALC(*ClearUnitOutputs);
		unit->mDone = true;
		return;
	} else {

		//must be exact divisor
		int blocksperpartition = unit->m_nover2/unit->m_blocksize;

		unit->m_spareblocks = blocksperpartition-1;

		if(unit->m_spareblocks<1) {
			printf("PartConvT Error: no spareblocks, amortisation not possible! \n");
			SETCALC(*ClearUnitOutputs);
			unit->mDone = true;
			return;
		}

		//won't be exact
		unit->m_numamort = (unit->m_partitions-1)/unit->m_spareblocks; //will relate number of partitions to number of spare blocks
		unit->m_lastamort= (unit->m_partitions-1)- ((unit->m_spareblocks-1)*(unit->m_numamort)); //allow for error on last one
		unit->m_amortcount= -1; //starts as flag to avoid any amortisation before have first fft done
		unit->m_partitionsdone= 0;//1;

		//printf("Amortisation stats partitions %d nover2 %d blocksize %d spareblocks %d numamort %d lastamort %d \n", unit->m_partitions,unit->m_nover2, unit->m_blocksize, unit->m_spareblocks, unit->m_numamort, unit->m_lastamort);

		unit->m_fd_accumulate= (float*)RTAlloc(unit->mWorld, unit->m_fullsize * sizeof(float));
		memset(unit->m_fd_accumulate, 0, unit->m_fullsize * sizeof(float));
		unit->m_fd_accum_pos=0;

		SETCALC(PartConvT_next);
	}
	ZOUT0(0) = 0;
	unit->m_prevtrig = 0.0;
}

void PartConvT_Dtor(PartConvT *unit)
{
	RTFree(unit->mWorld, unit->m_inputbuf);
	RTFree(unit->mWorld, unit->m_inputbuf2);
	RTFree(unit->mWorld, unit->m_spectrum);
	RTFree(unit->mWorld, unit->m_spectrum2);
	RTFree(unit->mWorld, unit->m_output);
	if (unit->m_fd_accumulate) RTFree(unit->mWorld, unit->m_fd_accumulate);

	SCWorld_Allocator alloc(ft, unit->mWorld);
	if(unit->m_scfft)  scfft_destroy(unit->m_scfft, alloc);
	if(unit->m_scifft) scfft_destroy(unit->m_scifft, alloc);
	if(unit->m_irspectra) RTFree(unit->mWorld,unit->m_irspectra);
}

void PartConvT_next( PartConvT *unit, int inNumSamples )
{
	float *in = IN(0);
	float *out = OUT(0);
	int pos = unit->m_pos;
	int currtrig = ZIN0(3);
	
	if (unit->m_prevtrig <= 0.f && currtrig > 0.f){
		DoPreparePartConvT(unit->mWorld,unit->m_irspectra,unit->m_buf,unit->m_fftsize);
	}
	unit->m_prevtrig = currtrig;
	/*
	//safety check
	if (!(unit->mWorld->mSndBufs + unit->m_specbufnumcheck)->data) {
		printf("PartConvT Error: Spectral data buffer not allocated \n");
		ClearUnitOutputs(unit, inNumSamples);
		SETCALC(*ClearUnitOutputs);
		unit->mDone = true;
		return;
	}
	*/
	
	float * input= unit->m_inputbuf;
	float * output= unit->m_output;
	int outputpos= unit->m_outputpos;

	//into input buffer
	memcpy(input+pos, in, inNumSamples * sizeof(float));

	pos += inNumSamples;

	//if ready for new FFT
	int nover2 = unit->m_nover2;

	//assumes that blocksize perfectly divides windowsize
	if (pos == nover2) {
		//FFT this input, second half of input always zero
		//memset(input+unit->m_nover2, 0, sizeof(float)*unit->m_nover2);
		scfft_dofft(unit->m_scfft);

		//reset pos into input buffer
		pos=0;
		//reset outputpos
		outputpos= 0;

		float * spectrum = unit->m_spectrum;
		float * spectrum2 = unit->m_spectrum2;

		//multiply spectra and accumulate for all ir spectra across storage buffer

		int fftsize = unit->m_fftsize;
		int accumpos = unit->m_fd_accum_pos;
		float * accumbuffer = unit->m_fd_accumulate;

		float * irspectrum = unit->m_irspectra;

		int fullsize = unit->m_fullsize;

		//JUST DO FIRST ONE FOR NOW, AMORTISED FOR OTHERS
		//frames
		for (int i=0; i<1; ++i) {

			int irpos= (i*fftsize);
			int posnow= (accumpos+irpos)%fullsize;
			
			float * target= accumbuffer+posnow;
			float * ir= irspectrum+irpos;
			//printf("1 posnow: %d irpos: %d accumpos: %d\n",posnow,irpos,accumpos);
			//real multiply for dc and nyquist
			target[0] += ir[0]*spectrum[0];
			target[1] += ir[1]*spectrum[1];

			//complex multiply for frequency bins
			for (int j=1; j<nover2; ++j) {
				int binposr= 2*j;
				int binposi= binposr+1;
				target[binposr] += (ir[binposr]*spectrum[binposr]) - (ir[binposi]*spectrum[binposi]);
				target[binposi] += (ir[binposi]*spectrum[binposr]) + (ir[binposr]*spectrum[binposi]);
			}
		}

		//IFFT this partition
		float * input2 = unit->m_inputbuf2;
		memcpy(input2, accumbuffer+accumpos, fftsize * sizeof(float));
		scfft_doifft(unit->m_scifft);

		//shunt output data down and zero top bit
		memcpy(output, output+nover2, nover2 * sizeof(float));
		memset(output+nover2, 0, nover2 * sizeof(float));

		//sum into output
		for (int j=0; j<fftsize; ++j)
			output[j] += spectrum2[j];

		//zero this partition
		memset(accumbuffer+accumpos, 0, fftsize * sizeof(float));

		//ONLY DO AT END OF AMORTISATION???? no, amort code has extra -1 in indexing to cope
		//update partition counter
		accumpos= (accumpos+fftsize)%fullsize;
		unit->m_fd_accum_pos= accumpos;

		//set up for amortisation (calculate output for other partitions of impulse response)
		unit->m_amortcount=0;
		unit->m_partitionsdone=1;
	} else {
		//amortisation steps:
		//complex multiply of this new fft spectrum against existing irspectrums and sum to accumbuffer
		if (unit->m_amortcount>=0) {
		//if(unit->m_partitionsdone>=1){
			float * spectrum= unit->m_spectrum;

			//multiply spectra and accumulate for all ir spectra across storage buffer

			int fftsize= unit->m_fftsize;
			int nover2= unit->m_nover2;
			//int frames= unit->m_partitions;
			int accumpos= unit->m_fd_accum_pos;
			float * accumbuffer= unit->m_fd_accumulate;

			float * irspectrum= unit->m_irspectra;

			int fullsize= unit->m_fullsize;

			int starti, stopi;
			int number;

			if(unit->m_amortcount==(unit->m_spareblocks-1)) {
				number= unit->m_lastamort;
			}else{
				number= unit->m_numamort;
			}

			starti= unit->m_partitionsdone;//-1;
			stopi= starti+number-1;
			//starti= 1;
			//stopi= unit->m_partitions-1;
			//printf("amort check count %d starti %d stopi %d number %d framesdone %d \n",unit->m_amortcount, starti, stopi, number, unit->m_partitionsdone);

			unit->m_partitionsdone += number;
			++unit->m_amortcount;

			for (int i=starti; i<=stopi; ++i) {
				int posnow= (accumpos+((i-1)*fftsize))%fullsize;
				
				float * target= accumbuffer+posnow;
				int irpos= (i*fftsize);
				float * ir= irspectrum+irpos;
				//printf("posnow: %d irpos: %d accumpos: %d\n",posnow,irpos,accumpos);
				//real multiply for dc and nyquist
				target[0]+=  ir[0]*spectrum[0];
				target[1]+=	 ir[1]*spectrum[1];
				//complex multiply for frequency bins
				for (int j=1; j<nover2; ++j) {
					int binposr= 2*j;
					int binposi= binposr+1;
					target[binposr]+=  (ir[binposr]*spectrum[binposr]) - (ir[binposi]*spectrum[binposi]);
					target[binposi]+=  (ir[binposi]*spectrum[binposr]) + (ir[binposr]*spectrum[binposi]);
				}
			}

			//unit->m_partitionsdone = 0;
		}
	}

	//do this second!
	memcpy(out, output+outputpos, inNumSamples * sizeof(float));

	//debugging tests: output values tend to be fftsize too big, probably due to complex multiply and also summation over all partitions
	//	RGen& rgen = *unit->mParent->mRGen;
	//	int testindex= rgen.irand(inNumSamples-1);
	//	printf("inNumSamples %d testindex %d out %f output %f \n",inNumSamples, testindex, out[testindex], *(output+outputpos+testindex));

	outputpos+=inNumSamples;

	unit->m_outputpos= outputpos;
	unit->m_pos= pos;
}

//buffer preparation

void DoPreparePartConvT(World *world, float *data1, SndBuf* frombuf,int fftsize )
{
	//printf("DoPreparePartConvT fftsize: %d\n",fftsize);
	int frames2 = frombuf->frames;
	float *data2 = frombuf->data;

	//scfft
	int nover2= fftsize>>1;

	int numpartitions;
	if(frames2 % nover2 == 0){
		numpartitions= frames2/nover2;
	}else{
		numpartitions= (frames2/nover2)+1;
	}

	//printf("reality check numpartitions %d fftsize %d product %d numinputframes %d \n", numpartitions, fftsize, numpartitions*fftsize, frames2);

	float * inputbuf= (float*)RTAlloc(world, fftsize * sizeof(float));
	float * spectrum= (float*)RTAlloc(world, fftsize * sizeof(float));

	SCWorld_Allocator alloc(ft, world);
	scfft* m_scfft = scfft_create(fftsize, fftsize, kRectWindow, inputbuf, spectrum, kForward, alloc);

	memset(inputbuf, 0, sizeof(float)*fftsize); // for zero padding

	//run through input data buffer, taking nover2 chunks, zero padding each
	for (int i=0; i<numpartitions; ++i) {
		int indexnow= nover2*i;
		int indexout= fftsize*i;

		if(i<(numpartitions-1)){
			memcpy(inputbuf, data2+indexnow, nover2 * sizeof(float));
		}else{
			int takenow = frames2 % nover2;
			if(takenow == 0){
				takenow = nover2;
			}

			memcpy(inputbuf, data2+indexnow, takenow * sizeof(float));
			if(takenow<nover2){
				memset(inputbuf+takenow, 0, (nover2-takenow)*sizeof(float));
			}
		}
		scfft_dofft(m_scfft);
		memcpy(data1+indexout, spectrum, fftsize * sizeof(float));
	}
	//clean up
	RTFree(world, inputbuf);
	RTFree(world, spectrum);

	if(m_scfft) scfft_destroy(m_scfft, alloc);
}


void initPartConvT(InterfaceTable *inTable)
{
	ft = inTable;
	DefineDtorCantAliasUnit(PartConvT);
}