andrewm@0: /*
andrewm@0:  * render.cpp
andrewm@0:  *
andrewm@0:  *  Created on: May 28, 2014
andrewm@0:  *      Author: Victor Zappi
andrewm@0:  */
andrewm@0: 
andrewm@0: #include "../../include/RTAudio.h"
andrewm@0: #include "../../include/PRU.h"
andrewm@0: #include "StatusLED.h"
andrewm@0: #include "config.h"
andrewm@0: #include "OscillatorBank.h"
andrewm@0: #include "FeedbackOscillator.h"
andrewm@0: #include "ADSR.h"
andrewm@0: #include "FIRfilter.h"
andrewm@0: #include <assert.h>
andrewm@0: #include <cmath>
andrewm@0: #include <vector>
andrewm@0: 
andrewm@0: #undef DBOX_CAPE_TEST
andrewm@0: 
andrewm@0: #define N_OCT		4.0	// maximum number of octaves on sensor 1
andrewm@0: 
andrewm@0: extern vector<OscillatorBank*> gOscBanks;
andrewm@0: extern int gCurrentOscBank;
andrewm@0: extern int gNextOscBank;
andrewm@0: extern PRU *gPRU;
andrewm@0: extern StatusLED gStatusLED;
andrewm@0: extern bool gIsLoading;
andrewm@0: extern bool gAudioIn;
andrewm@0: extern int gPeriodSize;
andrewm@0: 
andrewm@0: float *gOscillatorBuffer1, *gOscillatorBuffer2;
andrewm@0: float *gOscillatorBufferRead, *gOscillatorBufferWrite;
andrewm@0: int gOscillatorBufferReadPointer		= 0;
andrewm@0: int gOscillatorBufferReadCurrentSize	= 0;
andrewm@0: int gOscillatorBufferWriteCurrentSize	= 0;
andrewm@0: bool gOscillatorNeedsRender				= false;
andrewm@0: 
andrewm@0: int gMatrixSampleCount = 0;		// How many samples have elapsed on the matrix
andrewm@0: 
andrewm@0: // Wavetable which changes in response to an oscillator
andrewm@0: float *gDynamicWavetable;
andrewm@0: int gDynamicWavetableLength;
andrewm@0: bool gDynamicWavetableNeedsRender = false;
andrewm@0: 
andrewm@0: // These variables handle the hysteresis oscillator used for setting the playback speed
andrewm@0: bool gSpeedHysteresisOscillatorRising	= false;
andrewm@0: int gSpeedHysteresisLastTrigger			= 0;
andrewm@0: 
andrewm@0: // These variables handle the feedback oscillator used for controlling the wavetable
andrewm@0: FeedbackOscillator gFeedbackOscillator;
andrewm@0: float *gFeedbackOscillatorTable;
andrewm@0: int gFeedbackOscillatorTableLength;
andrewm@0: 
andrewm@0: // This comes from sensor.cpp where it records the most recent touch location on
andrewm@0: // sensor 0.
andrewm@0: extern float gSensor0LatestTouchPos;
andrewm@0: extern int gSensor0LatestTouchNum;
andrewm@0: uint16_t gPitchLatestInput = 0;
andrewm@0: 
andrewm@0: extern float gSensor1LatestTouchPos[];
andrewm@0: //extern float gSensor1LatestTouchSizes[];
andrewm@0: extern int gSensor1LatestTouchCount;
andrewm@0: extern int gSensor1LatestTouchIndex;
andrewm@0: int gSensor1LastTouchIndex		= -1;
andrewm@0: int gSensor1InputDelayCounter	= -1;
andrewm@0: int gSensor1InputIndex			= 0;
andrewm@0: float gSensor1MatrixTouchPos[5]	= {0};
andrewm@0: 
andrewm@0: // FSR value from matrix input
andrewm@0: extern int gLastFSRValue;
andrewm@0: 
andrewm@0: // Loop points from matrix input 4
andrewm@0: const int gLoopPointsInputBufferSize	= 256;
andrewm@0: uint16_t gLoopPointsInputBuffer[gLoopPointsInputBufferSize];
andrewm@0: int gLoopPointsInputBufferPointer		= 0;
andrewm@0: int gLoopPointMin = 0, gLoopPointMax	= 0;
andrewm@0: 
andrewm@0: // multiplier to activate or mute audio in
andrewm@0: int audioInStatus = 0;
andrewm@0: 
andrewm@0: // xenomai timer
andrewm@0: SRTIME prevChangeNs = 0;
andrewm@0: 
andrewm@0: // pitch vars
andrewm@0: float octaveSplitter;
andrewm@0: u_int16_t semitones[((int)N_OCT*12)+1];
andrewm@0: float deltaTouch	= 0;
andrewm@0: float deltaWeightP	= 0.5;
andrewm@0: float deltaWeightI	= 0.0005;
andrewm@0: 
andrewm@0: // filter vars
andrewm@0: ne10_fir_instance_f32_t filter[2];
andrewm@0: ne10_float32_t *filterIn[2];
andrewm@0: ne10_float32_t *filterOut[2];
andrewm@0: ne10_uint32_t blockSize;
andrewm@0: ne10_float32_t *filterState[2];
andrewm@0: ne10_float32_t prevFiltered[2];
andrewm@0: int filterGain = 80;
andrewm@0: ADSR PeakBurst[2];
andrewm@0: float peak[2];
andrewm@0: float peakThresh = 0.2;
andrewm@0: 
andrewm@0: // Tasks for lower-priority calculation
andrewm@0: AuxiliaryTask gMediumPriorityRender, gLowPriorityRender;
andrewm@0: 
andrewm@0: 
andrewm@0: extern "C" {
andrewm@0: 	// Function prototype for ARM assembly implementation of oscillator bank
andrewm@0: 	void oscillator_bank_neon(int numAudioFrames, float *audioOut,
andrewm@0: 							  int activePartialNum, int lookupTableSize,
andrewm@0: 							  float *phases, float *frequencies, float *amplitudes,
andrewm@0: 							  float *freqDerivatives, float *ampDerivatives,
andrewm@0: 							  float *lookupTable);
andrewm@0: 
andrewm@0: 	void wavetable_interpolate_neon(int numSamplesIn, int numSamplesOut,
andrewm@0: 	                              float *tableIn, float *tableOut);
andrewm@0: }
andrewm@0: 
andrewm@0: void wavetable_interpolate(int numSamplesIn, int numSamplesOut,
andrewm@0:                               float *tableIn, float *tableOut,
andrewm@0:                               float *sineTable, float sineMix);
andrewm@0: 
andrewm@0: inline uint16_t hysteresis_oscillator(uint16_t input, uint16_t risingThreshold,
andrewm@0: 									uint16_t fallingThreshold, bool *rising);
andrewm@0: 
andrewm@0: #ifdef DBOX_CAPE_TEST
andrewm@0: void render_capetest(int numMatrixFrames, int numAudioFrames, float *audioIn, float *audioOut,
andrewm@0: 			uint16_t *matrixIn, uint16_t *matrixOut);
andrewm@0: #endif
andrewm@0: 
andrewm@14: bool initialise_render(int numMatrixChannels, int numAudioChannels,
andrewm@14: 					   int numMatrixFramesPerPeriod,
andrewm@14: 					   int numAudioFramesPerPeriod,
andrewm@14: 					   float matrixSampleRate, float audioSampleRate,
andrewm@14: 					   void *userData) {
andrewm@0: 	int oscBankHopSize = *(int *)userData;
andrewm@0: 
andrewm@14: 	if(numMatrixChannels != 8) {
andrewm@14: 		printf("Error: D-Box needs matrix enabled with 8 channels.\n");
andrewm@14: 		return false;
andrewm@14: 	}
andrewm@14: 
andrewm@0: 	// Allocate two buffers for rendering oscillator bank samples
andrewm@0: 	// One will be used for writing in the background while the other is used for reading
andrewm@0: 	// on the audio thread. 8-byte alignment needed for the NEON code.
andrewm@14: 	if(posix_memalign((void **)&gOscillatorBuffer1, 8, oscBankHopSize * gNumAudioChannels * sizeof(float))) {
andrewm@0: 		printf("Error allocating render buffers\n");
andrewm@0: 		return false;
andrewm@0: 	}
andrewm@14: 	if(posix_memalign((void **)&gOscillatorBuffer2, 8, oscBankHopSize * gNumAudioChannels * sizeof(float))) {
andrewm@0: 		printf("Error allocating render buffers\n");
andrewm@0: 		return false;
andrewm@0: 	}
andrewm@0: 	gOscillatorBufferWrite	= gOscillatorBuffer1;
andrewm@0: 	gOscillatorBufferRead	= gOscillatorBuffer2;
andrewm@0: 
andrewm@14: 	memset(gOscillatorBuffer1, 0, oscBankHopSize * gNumAudioChannels * sizeof(float));
andrewm@14: 	memset(gOscillatorBuffer2, 0, oscBankHopSize * gNumAudioChannels * sizeof(float));
andrewm@0: 
andrewm@0: 	// Initialise the dynamic wavetable used by the oscillator bank
andrewm@0: 	// It should match the size of the static one already allocated in the OscillatorBank object
andrewm@0: 	// Don't forget a guard point at the end of the table
andrewm@0: 	gDynamicWavetableLength = gOscBanks[gCurrentOscBank]->lookupTableSize;
andrewm@0: 	if(posix_memalign((void **)&gDynamicWavetable, 8, (gDynamicWavetableLength + 1) * sizeof(float))) {
andrewm@0: 		printf("Error allocating wavetable\n");
andrewm@0: 		return false;
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	gFeedbackOscillator.initialise(8192, 10.0, matrixSampleRate);
andrewm@0: 
andrewm@0: 	for(int n = 0; n < gDynamicWavetableLength + 1; n++)
andrewm@0: 		gDynamicWavetable[n] = 0;
andrewm@0: 
andrewm@0: 	// pitch
andrewm@0: 	float midPos		= (float)65535/2.0;
andrewm@0: 	octaveSplitter		= round((float)65535/(N_OCT));
andrewm@0: 	int numOfSemi		= 12*N_OCT;
andrewm@0: 	int middleSemitone	= 12*N_OCT/2;
andrewm@0: 	int lastSemitone	= middleSemitone+numOfSemi/2;
andrewm@0: 	float inc 			= (float)65535/(N_OCT*12.0);
andrewm@0: 	int i 				= -1;
andrewm@0: 	for(int semi=middleSemitone; semi<=lastSemitone; semi++)
andrewm@0: 		semitones[semi] = ( midPos + (++i)*inc) + 0.5;
andrewm@0: 	i 					= 0;
andrewm@0: 	for(int semi=middleSemitone-1; semi>=0; semi--)
andrewm@0: 		semitones[semi] = ( midPos - (++i)*inc) + 0.5;
andrewm@0: 
andrewm@0: 	if(gAudioIn)
andrewm@0: 		audioInStatus = 1;
andrewm@0: 
andrewm@0: 	// filter
andrewm@0: 	blockSize		= 2*gPeriodSize;
andrewm@0: 	filterState[0]	= (ne10_float32_t *) NE10_MALLOC ((FILTER_TAP_NUM+blockSize-1) * sizeof (ne10_float32_t));
andrewm@0: 	filterState[1]	= (ne10_float32_t *) NE10_MALLOC ((FILTER_TAP_NUM+blockSize-1) * sizeof (ne10_float32_t));
andrewm@0: 	filterIn[0]		= (ne10_float32_t *) NE10_MALLOC (blockSize * sizeof (ne10_float32_t));
andrewm@0: 	filterIn[1]		= (ne10_float32_t *) NE10_MALLOC (blockSize * sizeof (ne10_float32_t));
andrewm@0: 	filterOut[0]	= (ne10_float32_t *) NE10_MALLOC (blockSize * sizeof (ne10_float32_t));
andrewm@0: 	filterOut[1]	= (ne10_float32_t *) NE10_MALLOC (blockSize * sizeof (ne10_float32_t));
andrewm@0: 	ne10_fir_init_float(&filter[0], FILTER_TAP_NUM, filterTaps, filterState[0], blockSize);
andrewm@0: 	ne10_fir_init_float(&filter[1], FILTER_TAP_NUM, filterTaps, filterState[1], blockSize);
andrewm@0: 
andrewm@0: 	// peak outputs
andrewm@0: 	PeakBurst[0].setAttackRate(.00001 * matrixSampleRate);
andrewm@0: 	PeakBurst[1].setAttackRate(.00001 * matrixSampleRate);
andrewm@0: 	PeakBurst[0].setDecayRate(.5 * matrixSampleRate);
andrewm@0: 	PeakBurst[1].setDecayRate(.5 * matrixSampleRate);
andrewm@0: 	PeakBurst[0].setSustainLevel(0.0);
andrewm@0: 	PeakBurst[1].setSustainLevel(0.0);
andrewm@0: 
andrewm@0: 	// Initialise auxiliary tasks
andrewm@0: 	if((gMediumPriorityRender = createAuxiliaryTaskLoop(&render_medium_prio, 90, "dbox-calculation-medium")) == 0)
andrewm@0: 		return false;
andrewm@0: 	if((gLowPriorityRender = createAuxiliaryTaskLoop(&render_low_prio, 85, "dbox-calculation-low")) == 0)
andrewm@0: 		return false;
andrewm@0: 
andrewm@0: 	return true;
andrewm@0: }
andrewm@0: 
andrewm@0: void render(int numMatrixFrames, int numAudioFrames, float *audioIn, float *audioOut,
andrewm@0: 			uint16_t *matrixIn, uint16_t *matrixOut)
andrewm@0: {
andrewm@0: #ifdef DBOX_CAPE_TEST
andrewm@0: 	render_capetest(numMatrixFrames, numAudioFrames, audioIn, audioOut, matrixIn, matrixOut);
andrewm@0: #else
andrewm@0: 	if(gOscBanks[gCurrentOscBank]->state==bank_toreset)
andrewm@0: 		gOscBanks[gCurrentOscBank]->resetOscillators();
andrewm@0: 
andrewm@0: 	if(gOscBanks[gCurrentOscBank]->state==bank_playing)
andrewm@0: 	{
andrewm@14: 		assert(gNumAudioChannels == 2);
andrewm@0: 
andrewm@0: #ifdef OLD_OSCBANK
andrewm@14: 		memset(audioOut, 0, numAudioFrames * gNumAudioChannels * sizeof(float));
andrewm@0: 
andrewm@0: 		/* Render the oscillator bank. The oscillator bank function is written in NEON assembly
andrewm@0: 		 * and it strips out all extra checks, so find out in advance whether we can render a whole
andrewm@0: 		 * block or whether the frame will increment in the middle of this buffer.
andrewm@0: 		 */
andrewm@0: 
andrewm@0: 		int framesRemaining = numAudioFrames;
andrewm@0: 		float *audioOutWithOffset = audioOut;
andrewm@0: 
andrewm@0: 		while(framesRemaining > 0) {
andrewm@0: 			if(gOscBanks[gCurrentOscBank]->hopCounter >= framesRemaining) {
andrewm@0: 				/* More frames left in this hop than we need this time. Render and finish */
andrewm@0: 				oscillator_bank_neon(framesRemaining, audioOutWithOffset,
andrewm@0: 									 gOscBanks[gCurrentOscBank]->actPartNum, gOscBanks[gCurrentOscBank]->lookupTableSize,
andrewm@0: 									 gOscBanks[gCurrentOscBank]->oscillatorPhases, gOscBanks[gCurrentOscBank]->oscillatorNormFrequencies,
andrewm@0: 									 gOscBanks[gCurrentOscBank]->oscillatorAmplitudes,
andrewm@0: 									 gOscBanks[gCurrentOscBank]->oscillatorNormFreqDerivatives,
andrewm@0: 									 gOscBanks[gCurrentOscBank]->oscillatorAmplitudeDerivatives,
andrewm@0: 									 gDynamicWavetable/*gOscBanks[gCurrentOscBank]->lookupTable*/);
andrewm@0: 				gOscBanks[gCurrentOscBank]->hopCounter -= framesRemaining;
andrewm@0: 				if(gOscBanks[gCurrentOscBank]->hopCounter <= 0)
andrewm@0: 					gOscBanks[gCurrentOscBank]->nextHop();
andrewm@0: 				framesRemaining = 0;
andrewm@0: 			}
andrewm@0: 			else {
andrewm@0: 				/* More frames to render than are left in this hop. Render and decrement the
andrewm@0: 				 * number of remaining frames; then advance to the next oscillator frame.
andrewm@0: 				 */
andrewm@0: 				oscillator_bank_neon(gOscBanks[gCurrentOscBank]->hopCounter, audioOutWithOffset,
andrewm@0: 									 gOscBanks[gCurrentOscBank]->actPartNum, gOscBanks[gCurrentOscBank]->lookupTableSize,
andrewm@0: 									 gOscBanks[gCurrentOscBank]->oscillatorPhases, gOscBanks[gCurrentOscBank]->oscillatorNormFrequencies,
andrewm@0: 									 gOscBanks[gCurrentOscBank]->oscillatorAmplitudes,
andrewm@0: 									 gOscBanks[gCurrentOscBank]->oscillatorNormFreqDerivatives,
andrewm@0: 									 gOscBanks[gCurrentOscBank]->oscillatorAmplitudeDerivatives,
andrewm@0: 									 gDynamicWavetable/*gOscBanks[gCurrentOscBank]->lookupTable*/);
andrewm@0: 				framesRemaining -= gOscBanks[gCurrentOscBank]->hopCounter;
andrewm@14: 				audioOutWithOffset += gNumAudioChannels * gOscBanks[gCurrentOscBank]->hopCounter;
andrewm@0: 				gOscBanks[gCurrentOscBank]->sampleCount += gOscBanks[gCurrentOscBank]->hopCounter;
andrewm@0: 				gOscBanks[gCurrentOscBank]->nextHop();
andrewm@0: 			}
andrewm@0: 		}
andrewm@0: #else
andrewm@0: 		for(int n = 0; n < numAudioFrames; n++) {
andrewm@0: 			audioOut[2*n] 	  = gOscillatorBufferRead[gOscillatorBufferReadPointer++]+audioIn[2*n]*audioInStatus;
andrewm@0: 			audioOut[2*n + 1] = gOscillatorBufferRead[gOscillatorBufferReadPointer++]+audioIn[2*n+1]*audioInStatus;
andrewm@0: 
andrewm@0: 			filterIn[0][n] = fabs(audioIn[2*n]);	// rectify for peak detection in 1
andrewm@0: 			filterIn[1][n] = fabs(audioIn[2*n+1]);	// rectify for peak detection in 2
andrewm@0: 
andrewm@0: 			/* FIXME why doesn't this work? */
andrewm@0: 			/*
andrewm@0: 			if(gOscillatorBufferReadPointer == gOscillatorBufferCurrentSize/2) {
andrewm@0: 				gOscillatorNeedsRender = true;
andrewm@0: 				scheduleAuxiliaryTask(gLowPriorityRender);
andrewm@0: 			} */
andrewm@0: 
andrewm@0: 			if(gOscillatorBufferReadPointer >= gOscillatorBufferReadCurrentSize) {
andrewm@0: 				// Finished reading from the buffer: swap to the next buffer
andrewm@0: 				if(gOscillatorBufferRead == gOscillatorBuffer1) {
andrewm@0: 					gOscillatorBufferRead = gOscillatorBuffer2;
andrewm@0: 					gOscillatorBufferWrite = gOscillatorBuffer1;
andrewm@0: 				}
andrewm@0: 				else {
andrewm@0: 					gOscillatorBufferRead = gOscillatorBuffer1;
andrewm@0: 					gOscillatorBufferWrite = gOscillatorBuffer2;
andrewm@0: 				}
andrewm@0: 
andrewm@0: 				// New buffer size is whatever finished writing last hop
andrewm@0: 				gOscillatorBufferReadCurrentSize = gOscillatorBufferWriteCurrentSize;
andrewm@0: 				gOscillatorBufferReadPointer = 0;
andrewm@0: 
andrewm@0: 				gOscillatorNeedsRender = true;
andrewm@0: 				scheduleAuxiliaryTask(gMediumPriorityRender);
andrewm@0: 			}
andrewm@0: 		}
andrewm@0: #endif
andrewm@0: 	}
andrewm@0: 	else
andrewm@0: 	{
andrewm@0: 		for(int n = 0; n < numAudioFrames; n++) {
andrewm@0: 			audioOut[2*n] 	  = audioIn[2*n]*audioInStatus;
andrewm@0: 			audioOut[2*n + 1] = audioIn[2*n+1]*audioInStatus;
andrewm@0: 
andrewm@0: 			filterIn[0][n] = fabs(audioIn[2*n]);	// rectify for peak detection in 1
andrewm@0: 			filterIn[1][n] = fabs(audioIn[2*n+1]);	// rectify for peak detection in 2
andrewm@0: 		}
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	// low pass filter audio in 1 and 2 for peak detection
andrewm@0: 	ne10_fir_float_neon(&filter[0], filterIn[0], filterOut[0], blockSize);
andrewm@0: 	ne10_fir_float_neon(&filter[1], filterIn[1], filterOut[1], blockSize);
andrewm@0: 
andrewm@0: 	for(int n = 0; n < numMatrixFrames; n++) {
andrewm@0: 
andrewm@0: 
andrewm@0: 		/* Matrix Out 0, In 0
andrewm@0: 		 *
andrewm@0: 		 * CV loop
andrewm@0: 		 * Controls pitch of sound
andrewm@0: 		 */
andrewm@0: 		int touchPosInt = gSensor0LatestTouchPos * 65536.0;
andrewm@0: 		if(touchPosInt < 0) touchPosInt = 0;
andrewm@0: 		if(touchPosInt > 65535) touchPosInt = 65535;
andrewm@0: 		matrixOut[n*8 + DAC_PIN0] = touchPosInt;
andrewm@0: 
andrewm@0: 		gPitchLatestInput = matrixIn[n*8 + ADC_PIN0];
andrewm@0: 
andrewm@0: 
andrewm@0: 		/* Matrix Out 7
andrewm@0: 		 *
andrewm@0: 		 * Loop feedback with Matrix In 0
andrewm@0: 		 * Controls discreet pitch
andrewm@0: 		 */
andrewm@0: 		float deltaTarget = 0;
andrewm@0: 		int semitoneIndex = 0;
andrewm@0: 		if(gSensor0LatestTouchNum>0)
andrewm@0: 		{
andrewm@0: 			// current pitch is gPitchLatestInput, already retrieved
andrewm@0: 			semitoneIndex	= (  ( (float)gPitchLatestInput / 65535)*12*N_OCT  )+0.5;	// closest semitone
andrewm@0: 			deltaTarget		= (semitones[semitoneIndex]-gPitchLatestInput);				// delta between pitch and target
andrewm@0: 			deltaTouch 		+= deltaTarget*deltaWeightI;								// update feedback [previous + current]
andrewm@0: 		}
andrewm@0: 		else
andrewm@0: 			deltaTouch = 0;
andrewm@0: 
andrewm@0: 		int nextOut = touchPosInt  + deltaTarget*deltaWeightP + deltaTouch;			// add feedback to touch -> next out
andrewm@0: 		if(nextOut < 0) nextOut = 0;												// clamp
andrewm@0: 		if(nextOut > 65535) nextOut = 65535;										// clamp
andrewm@0: 		matrixOut[n*8 + DAC_PIN7] = nextOut;										// send next nextOut
andrewm@0: 
andrewm@0: 
andrewm@0: 		/*
andrewm@0: 		 * Matrix Out 1, In 1
andrewm@0: 		 *
andrewm@0: 		 * Hysteresis (comparator) oscillator
andrewm@0: 		 * Controls speed of playback
andrewm@0: 		 */
andrewm@0: 		bool wasRising = gSpeedHysteresisOscillatorRising;
andrewm@0: 		matrixOut[n*8 + DAC_PIN1] = hysteresis_oscillator(matrixIn[n*8 + ADC_PIN1], 48000, 16000, &gSpeedHysteresisOscillatorRising);
andrewm@0: 
andrewm@0: 		// Find interval of zero crossing
andrewm@0: 		if(wasRising && !gSpeedHysteresisOscillatorRising) {
andrewm@0: 			int interval = gMatrixSampleCount - gSpeedHysteresisLastTrigger;
andrewm@0: 
andrewm@0: 			// Interval since last trigger will be the new hop size; calculate to set speed
andrewm@0: 			if(interval < 1)
andrewm@0: 				interval = 1;
andrewm@0: 			//float speed = (float)gOscBanks[gCurrentOscBank]->getHopSize() / (float)interval;
andrewm@0: 			float speed = 144.0 / interval;	// Normalise to a fixed expected speed
andrewm@0: 			gOscBanks[gCurrentOscBank]->setSpeed(speed);
andrewm@0: 
andrewm@0: 			gSpeedHysteresisLastTrigger = gMatrixSampleCount;
andrewm@0: 		}
andrewm@0: 
andrewm@0: 		/*
andrewm@0: 		 * Matrix Out 2, In 2
andrewm@0: 		 *
andrewm@0: 		 * Feedback (phase shift) oscillator
andrewm@0: 		 * Controls wavetable used for oscillator bank
andrewm@0: 		 */
andrewm@0: 
andrewm@0: 		int tableLength = gFeedbackOscillator.process(matrixIn[n*8 + ADC_PIN2], &matrixOut[n*8 + DAC_PIN2]);
andrewm@0: 		if(tableLength != 0) {
andrewm@0: 			gFeedbackOscillatorTableLength = tableLength;
andrewm@0: 			gFeedbackOscillatorTable = gFeedbackOscillator.wavetable();
andrewm@0: 			gDynamicWavetableNeedsRender = true;
andrewm@0: 			scheduleAuxiliaryTask(gLowPriorityRender);
andrewm@0: 		}
andrewm@0: 
andrewm@0: 		/*
andrewm@0: 		 * Matrix Out 3, In 3
andrewm@0: 		 *
andrewm@0: 		 * CV loop with delay for time alignment
andrewm@0: 		 * Touch positions from sensor 1
andrewm@0: 		 * Change every 32 samples (ca. 1.5 ms)
andrewm@0: 		 */
andrewm@0: 		volatile int touchCount = gSensor1LatestTouchCount;
andrewm@0: 		if(touchCount == 0)
andrewm@0: 			matrixOut[n*8 + DAC_PIN3] = 0;
andrewm@0: 		else {
andrewm@0: 			int touchIndex = (gMatrixSampleCount >> 5) % touchCount;
andrewm@0: 			matrixOut[n*8 + DAC_PIN3] = gSensor1LatestTouchPos[touchIndex] * 56000.0f;
andrewm@0: 			if(touchIndex != gSensor1LastTouchIndex) {
andrewm@0: 				// Just changed to a new touch output. Reset the counter.
andrewm@0: 				// It will take 2*matrixFrames samples for this output to come back to the
andrewm@0: 				// ADC input. But we also want to read near the end of the 32 sample block;
andrewm@0: 				// let's say 24 samples into it.
andrewm@0: 
andrewm@0: 				// FIXME this won't work for p > 2
andrewm@0: 				gSensor1InputDelayCounter = 24 + 2*numMatrixFrames;
andrewm@0: 				gSensor1InputIndex = touchIndex;
andrewm@0: 			}
andrewm@0: 			gSensor1LastTouchIndex = touchIndex;
andrewm@0: 		}
andrewm@0: 
andrewm@0: 		if(gSensor1InputDelayCounter-- >= 0 && touchCount > 0) {
andrewm@0: 			gSensor1MatrixTouchPos[gSensor1InputIndex] = (float)matrixIn[n*8 + ADC_PIN3] / 65536.0f;
andrewm@0: 		}
andrewm@0: 
andrewm@0: 		/* Matrix Out 4
andrewm@0: 		 *
andrewm@0: 		 * Sensor 1 last pos
andrewm@0: 		 */
andrewm@0: 		touchPosInt = gSensor1LatestTouchPos[gSensor1LatestTouchIndex] * 65536.0;
andrewm@0: 		if(touchPosInt < 0) touchPosInt = 0;
andrewm@0: 		if(touchPosInt > 65535) touchPosInt = 65535;
andrewm@0: 		matrixOut[n*8 + DAC_PIN4] = touchPosInt;
andrewm@0: 
andrewm@0: 		/* Matrix In 4
andrewm@0: 		 *
andrewm@0: 		 * Loop points selector
andrewm@0: 		 */
andrewm@0: 		gLoopPointsInputBuffer[gLoopPointsInputBufferPointer++] = matrixIn[n*8 + ADC_PIN4];
andrewm@0: 		if(gLoopPointsInputBufferPointer >= gLoopPointsInputBufferSize) {
andrewm@0: 			// Find min and max values
andrewm@0: 			uint16_t loopMax = 0, loopMin = 65535;
andrewm@0: 			for(int i = 0; i < gLoopPointsInputBufferSize; i++) {
andrewm@0: 				if(gLoopPointsInputBuffer[i] < loopMin)
andrewm@0: 					loopMin = gLoopPointsInputBuffer[i];
andrewm@0: 				if(gLoopPointsInputBuffer[i] > loopMax/* && gLoopPointsInputBuffer[i] != 65535*/)
andrewm@0: 					loopMax = gLoopPointsInputBuffer[i];
andrewm@0: 			}
andrewm@0: 
andrewm@0: 			if(loopMin >= loopMax)
andrewm@0: 				loopMax = loopMin;
andrewm@0: 
andrewm@0: 			gLoopPointMax = loopMax;
andrewm@0: 			gLoopPointMin = loopMin;
andrewm@0: 			gLoopPointsInputBufferPointer = 0;
andrewm@0: 		}
andrewm@0: 
andrewm@0: 		/* Matrix Out 5
andrewm@0: 		 *
andrewm@0: 		 * Audio In 1 peak detection and peak burst output
andrewm@0: 		 */
andrewm@0: 
andrewm@0: 		filterOut[0][n*2+1]	*= filterGain;
andrewm@0: 		float burstOut		= PeakBurst[0].getOutput();
andrewm@0: 		if( burstOut < 0.1)
andrewm@0: 		{
andrewm@0: 			if( (prevFiltered[0]>=peakThresh) && (prevFiltered[0]>=filterOut[0][n*2+1]) )
andrewm@0: 			{
andrewm@0: 				peak[0] = prevFiltered[0];
andrewm@0: 				PeakBurst[0].gate(1);
andrewm@0: 			}
andrewm@0: 		}
andrewm@0: 
andrewm@0: 		PeakBurst[0].process(1);
andrewm@0: 
andrewm@0: 		int convAudio = burstOut*peak[0]*65535;
andrewm@0: 		matrixOut[n*8 + DAC_PIN5] = convAudio;
andrewm@0: 		prevFiltered[0] = filterOut[0][n*2+1];
andrewm@0: 		if(prevFiltered[0]>1)
andrewm@0: 			prevFiltered[0] = 1;
andrewm@0: 
andrewm@0: 		/* Matrix In 5
andrewm@0: 		 *
andrewm@0: 		 * Dissonance, via changing frequency motion of partials
andrewm@0: 		 */
andrewm@0: 		float amount = (float)matrixIn[n*8 + ADC_PIN5] / 65536.0f;
andrewm@0: 		gOscBanks[gCurrentOscBank]->freqMovement = 1-amount;
andrewm@0: 
andrewm@0: 
andrewm@0: 
andrewm@0: 
andrewm@0: 		/* Matrix Out 6
andrewm@0: 		 *
andrewm@0: 		 * Audio In 2 peak detection and peak burst output
andrewm@0: 		 */
andrewm@0: 
andrewm@0: 		filterOut[1][n*2+1]	*= filterGain;
andrewm@0: 		burstOut			= PeakBurst[1].getOutput();
andrewm@0: 		if( burstOut < 0.1)
andrewm@0: 		{
andrewm@0: 			if( (prevFiltered[1]>=peakThresh) && (prevFiltered[1]>=filterOut[1][n*2+1]) )
andrewm@0: 			{
andrewm@0: 				peak[1] = prevFiltered[1];
andrewm@0: 				PeakBurst[1].gate(1);
andrewm@0: 			}
andrewm@0: 		}
andrewm@0: 
andrewm@0: 		PeakBurst[1].process(1);
andrewm@0: 
andrewm@0: 		convAudio = burstOut*peak[1]*65535;
andrewm@0: 		matrixOut[n*8 + DAC_PIN6] = convAudio;
andrewm@0: 		prevFiltered[1] = filterOut[1][n*2+1];
andrewm@0: 		if(prevFiltered[1]>1)
andrewm@0: 			prevFiltered[1] = 1;
andrewm@0: 
andrewm@0: 		/* Matrix In 6
andrewm@0: 		 *
andrewm@0: 		 * Sound selector
andrewm@0: 		 */
andrewm@0: 		if(!gIsLoading) {
andrewm@0: 			// Use hysteresis to avoid jumping back and forth between sounds
andrewm@0: 			if(gOscBanks.size() > 1) {
andrewm@0: 				int input = matrixIn[n*8 + ADC_PIN6];
andrewm@0: 				const int hystValue = 16000;
andrewm@0: 
andrewm@0: 				int upHysteresisValue = ((gCurrentOscBank + 1) * 65536 + hystValue) / gOscBanks.size();
andrewm@0: 				int downHysteresisValue = (gCurrentOscBank * 65536 - hystValue) / gOscBanks.size();
andrewm@0: 
andrewm@0: 				if(input > upHysteresisValue || input < downHysteresisValue) {
andrewm@0: 					gNextOscBank = input * gOscBanks.size() / 65536;
andrewm@0: 					if(gNextOscBank < 0)
andrewm@0: 						gNextOscBank = 0;
andrewm@0: 					if((unsigned)gNextOscBank >= gOscBanks.size())
andrewm@0: 						gNextOscBank = gOscBanks.size() - 1;
andrewm@0: 				}
andrewm@0: 			}
andrewm@0: 		}
andrewm@0: 
andrewm@0: 		/*
andrewm@0: 		 * Matrix In 7
andrewm@0: 		 *
andrewm@0: 		 * FSR from primary touch sensor
andrewm@0: 		 * Value ranges from 0-1799
andrewm@0: 		 */
andrewm@0: 		gLastFSRValue = matrixIn[n*8 + ADC_PIN7] * (1799.0 / 65535.0);
andrewm@0: 		//gLastFSRValue = 1799 - matrixIn[n*8 + ADC_PIN7] * (1799.0 / 65535.0);
andrewm@0: 		//dbox_printf("%i\n",gLastFSRValue);
andrewm@0: 
andrewm@0: 		gMatrixSampleCount++;
andrewm@0: 	}
andrewm@0: 
andrewm@0: #endif /* DBOX_CAPE_TEST */
andrewm@0: }
andrewm@0: 
andrewm@0: // Medium-priority render function used for audio hop calculations
andrewm@0: void render_medium_prio()
andrewm@0: {
andrewm@0: 
andrewm@0: 	if(gOscillatorNeedsRender) {
andrewm@0: 		gOscillatorNeedsRender = false;
andrewm@0: 
andrewm@0: 		/* Render one frame into the write buffer */
andrewm@14: 		memset(gOscillatorBufferWrite, 0, gOscBanks[gCurrentOscBank]->hopCounter * gNumAudioChannels * sizeof(float));
andrewm@0: 
andrewm@0: 		oscillator_bank_neon(gOscBanks[gCurrentOscBank]->hopCounter, gOscillatorBufferWrite,
andrewm@0: 							 gOscBanks[gCurrentOscBank]->actPartNum, gOscBanks[gCurrentOscBank]->lookupTableSize,
andrewm@0: 							 gOscBanks[gCurrentOscBank]->oscillatorPhases, gOscBanks[gCurrentOscBank]->oscillatorNormFrequencies,
andrewm@0: 							 gOscBanks[gCurrentOscBank]->oscillatorAmplitudes,
andrewm@0: 							 gOscBanks[gCurrentOscBank]->oscillatorNormFreqDerivatives,
andrewm@0: 							 gOscBanks[gCurrentOscBank]->oscillatorAmplitudeDerivatives,
andrewm@0: 							 /*gOscBanks[gCurrentOscBank]->lookupTable*/gDynamicWavetable);
andrewm@0: 
andrewm@14: 		gOscillatorBufferWriteCurrentSize = gOscBanks[gCurrentOscBank]->hopCounter * gNumAudioChannels;
andrewm@0: 
andrewm@0: 		/* Update the pitch right before the hop
andrewm@0: 		 * Total CV range +/- N_OCT octaves
andrewm@0: 		 */
andrewm@0: 		float pitch = (float)gPitchLatestInput / octaveSplitter - N_OCT/2;
andrewm@0: 		//gOscBanks[gCurrentOscBank]->pitchMultiplier = powf(2.0f, pitch);
andrewm@0: 		gOscBanks[gCurrentOscBank]->pitchMultiplier = pow(2.0f, pitch);
andrewm@0: 
andrewm@0: #ifdef FIXME_LATER // This doesn't work very well yet
andrewm@0: 		gOscBanks[gCurrentOscBank]->filterNum = gSensor1LatestTouchCount;
andrewm@0: 		float freqScaler = gOscBanks[gCurrentOscBank]->getFrequencyScaler();
andrewm@0: 		for(int i=0; i < gOscBanks[gCurrentOscBank]->filterNum; i++)
andrewm@0: 		{
andrewm@0: 			// touch pos is linear but freqs are log
andrewm@0: 			gOscBanks[gCurrentOscBank]->filterFreqs[i] = ((expf(gSensor1MatrixTouchPos[i]*4)-1)/(expf(4)-1))*gOscBanks[gCurrentOscBank]->filterMaxF*freqScaler;
andrewm@0: 			gOscBanks[gCurrentOscBank]->filterQ[i] = gSensor1LatestTouchSizes[i];
andrewm@0: 			if(gOscBanks[gCurrentOscBank]->filterFreqs[i]>500*freqScaler)
andrewm@0: 				gOscBanks[gCurrentOscBank]->filterPadding[i] = 1+100000*( (gOscBanks[gCurrentOscBank]->filterFreqs[i]-500*freqScaler)/(gOscBanks[gCurrentOscBank]->filterMaxF-500)*freqScaler );
andrewm@0: 			else
andrewm@0: 				gOscBanks[gCurrentOscBank]->filterPadding[i] = 1;
andrewm@0: 		}
andrewm@0: #endif
andrewm@0: 
andrewm@0: 		RTIME ticks		= rt_timer_read();
andrewm@0: 		SRTIME ns		= rt_timer_tsc2ns(ticks);
andrewm@0: 		SRTIME delta 	= ns-prevChangeNs;
andrewm@0: 
andrewm@0: 		// switch to next bank cannot be too frequent, to avoid seg fault! [for example sef fault happens when removing both VDD and GND from breadboard]
andrewm@0: 		if(gNextOscBank != gCurrentOscBank && delta>100000000) {
andrewm@0: 
andrewm@0: 			/*printf("ticks %llu\n", (unsigned long long)ticks);
andrewm@0: 			printf("ns %llu\n", (unsigned long long)ns);
andrewm@0: 			printf("prevChangeNs %llu\n", (unsigned long long)prevChangeNs);
andrewm@0: 			printf("-------------------------->%llud\n", (unsigned long long)(ns-prevChangeNs));*/
andrewm@0: 
andrewm@0: 			prevChangeNs = ns;
andrewm@0: 			dbox_printf("Changing to bank %d...\n", gNextOscBank);
andrewm@0: 			if(gOscBanks[gCurrentOscBank]->state==bank_playing){
andrewm@0: 				gOscBanks[gCurrentOscBank]->stop();
andrewm@0: 			}
andrewm@0: 
andrewm@0: 			gCurrentOscBank = gNextOscBank;
andrewm@0: 			gOscBanks[gCurrentOscBank]->hopNumTh = 0;
andrewm@0: 		}
andrewm@0: 		else {
andrewm@0: 			/* Advance to the next oscillator frame */
andrewm@0: 			gOscBanks[gCurrentOscBank]->nextHop();
andrewm@0: 		}
andrewm@0: 	}
andrewm@0: }
andrewm@0: 
andrewm@0: // Lower-priority render function which performs matrix calculations
andrewm@0: // State should be transferred in via global variables
andrewm@0: void render_low_prio()
andrewm@0: {
andrewm@0: 	gPRU->setGPIOTestPin();
andrewm@0: 	if(gDynamicWavetableNeedsRender) {
andrewm@0: 		// Find amplitude of wavetable
andrewm@0: 		float meanAmplitude = 0;
andrewm@0: 		float sineMix;
andrewm@0: 
andrewm@0: 		for(int i = 0; i < gFeedbackOscillatorTableLength; i++) {
andrewm@0: 			//meanAmplitude += fabsf(gFeedbackOscillatorTable[i]);
andrewm@0: 			meanAmplitude += fabs(gFeedbackOscillatorTable[i]);
andrewm@0: 		}
andrewm@0: 		meanAmplitude /= (float)gFeedbackOscillatorTableLength;
andrewm@0: 
andrewm@0: 		if(meanAmplitude > 0.35)
andrewm@0: 			sineMix = 0;
andrewm@0: 		else
andrewm@0: 			sineMix = (.35 - meanAmplitude) / .35;
andrewm@0: 
andrewm@0: 		//dbox_printf("amp %f mix %f\n", meanAmplitude, sineMix);
andrewm@0: 
andrewm@0: 		// Copy to main wavetable
andrewm@0: 		wavetable_interpolate(gFeedbackOscillatorTableLength, gDynamicWavetableLength,
andrewm@0: 				gFeedbackOscillatorTable, gDynamicWavetable,
andrewm@0: 				gOscBanks[gCurrentOscBank]->lookupTable, sineMix);
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	if(gLoopPointMin >= 60000 && gLoopPointMax >= 60000) {
andrewm@0: 		// KLUDGE!
andrewm@0: 		if(gCurrentOscBank == 0)
andrewm@0: 			gOscBanks[gCurrentOscBank]->setLoopHops(50, ((float)gOscBanks[gCurrentOscBank]->getLastHop() * 0.6) - 1);
andrewm@0: 		else
andrewm@0: 			gOscBanks[gCurrentOscBank]->setLoopHops(5, ((float)gOscBanks[gCurrentOscBank]->getLastHop() * 0.7) - 1);
andrewm@0: 	}
andrewm@0: 	else {
andrewm@0: 		float normLoopPointMin = (float)gLoopPointMin * gOscBanks[gCurrentOscBank]->getLastHop() / 65535.0;
andrewm@0: 		float normLoopPointMax = (float)gLoopPointMax * gOscBanks[gCurrentOscBank]->getLastHop() / 65535.0;
andrewm@0: 
andrewm@0: 		int intLoopPointMin = normLoopPointMin;
andrewm@0: 		if(intLoopPointMin < 1)
andrewm@0: 			intLoopPointMin = 1;
andrewm@0: 		int intLoopPointMax = normLoopPointMax;
andrewm@0: 		if(intLoopPointMax <= intLoopPointMin)
andrewm@0: 			intLoopPointMax = intLoopPointMin + 1;
andrewm@0: 		if(intLoopPointMax > gOscBanks[gCurrentOscBank]->getLastHop() - 1)
andrewm@0: 			intLoopPointMax =  gOscBanks[gCurrentOscBank]->getLastHop() - 1;
andrewm@0: 
andrewm@0: 		//dbox_printf("Loop points %d-%d / %d-%d\n", gLoopPointMin, gLoopPointMax, intLoopPointMin, intLoopPointMax);
andrewm@0: 
andrewm@0: 		/* WORKS, jsut need to fix the glitch when jumps!
andrewm@0: 		 * *int currentHop = gOscBanks[gCurrentOscBank]->getCurrentHop();
andrewm@0: 		if(currentHop < intLoopPointMin -1 )
andrewm@0: 			gOscBanks[gCurrentOscBank]->setJumpHop(intLoopPointMin + 1);
andrewm@0: 		else if(currentHop > intLoopPointMax + 1)
andrewm@0: 			gOscBanks[gCurrentOscBank]->setJumpHop(intLoopPointMax - 1);*/
andrewm@0: 		gOscBanks[gCurrentOscBank]->setLoopHops(intLoopPointMin, intLoopPointMax);
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	if(gIsLoading)
andrewm@0: 		gStatusLED.blink(25, 75);	// Blink quickly until load finished
andrewm@0: 	else
andrewm@0: 		gStatusLED.blink(250 / gOscBanks[gCurrentOscBank]->getSpeed(), 250 / gOscBanks[gCurrentOscBank]->getSpeed());
andrewm@0: 	gPRU->clearGPIOTestPin();
andrewm@0: 
andrewm@0: //	static int counter = 32;
andrewm@0: //	if(--counter == 0) {
andrewm@0: //		for(int i = 0; i < gLoopPointsInputBufferSize; i++) {
andrewm@0: //			dbox_printf("%d ", gLoopPointsInputBuffer[i]);
andrewm@0: //			if(i % 32 == 31)
andrewm@0: //				dbox_printf("\n");
andrewm@0: //		}
andrewm@0: //		dbox_printf("\n\n");
andrewm@0: //		counter = 32;
andrewm@0: //	}
andrewm@0: 
andrewm@0: 	//dbox_printf("min %d max %d\n", gLoopPointMin, gLoopPointMax);
andrewm@0: }
andrewm@0: 
andrewm@0: // Clean up at the end of render
andrewm@0: void cleanup_render()
andrewm@0: {
andrewm@0: 	free(gOscillatorBuffer1);
andrewm@0: 	free(gOscillatorBuffer2);
andrewm@0: 	free(gDynamicWavetable);
andrewm@0: }
andrewm@0: 
andrewm@0: // Interpolate one wavetable into another. The output size
andrewm@0: // does not include the guard point at the end which will be identical
andrewm@0: // to the first point
andrewm@0: void wavetable_interpolate(int numSamplesIn, int numSamplesOut,
andrewm@0:                            float *tableIn, float *tableOut,
andrewm@0:                            float *sineTable, float sineMix)
andrewm@0: {
andrewm@0: 	float fractionalScaler = (float)numSamplesIn / (float)numSamplesOut;
andrewm@0: 
andrewm@0: 	for(int k = 0; k < numSamplesOut; k++) {
andrewm@0: 		float fractionalIndex = (float) k * fractionalScaler;
andrewm@0: 		//int sB = (int)floorf(fractionalIndex);
andrewm@0: 		int sB = (int)floor(fractionalIndex);
andrewm@0: 		int sA = sB + 1;
andrewm@0: 		if(sA >= numSamplesIn)
andrewm@0: 			sA = 0;
andrewm@0: 		float fraction = fractionalIndex - sB;
andrewm@0: 		tableOut[k] = fraction * tableIn[sA] + (1.0f - fraction) * tableIn[sB];
andrewm@0: 		tableOut[k] = sineMix * sineTable[k] + (1.0 - sineMix) * tableOut[k];
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	tableOut[numSamplesOut] = tableOut[0];
andrewm@0: }
andrewm@0: 
andrewm@0: // Create a hysteresis oscillator with a matrix input and output
andrewm@0: inline uint16_t hysteresis_oscillator(uint16_t input, uint16_t risingThreshold, uint16_t fallingThreshold, bool *rising)
andrewm@0: {
andrewm@0: 	uint16_t value;
andrewm@0: 
andrewm@0: 	if(*rising) {
andrewm@0: 		if(input > risingThreshold) {
andrewm@0: 			*rising = false;
andrewm@0: 			value = 0;
andrewm@0: 		}
andrewm@0: 		else
andrewm@0: 			value = 65535;
andrewm@0: 	}
andrewm@0: 	else {
andrewm@0: 		if(input < fallingThreshold) {
andrewm@0: 			*rising = true;
andrewm@0: 			value = 65535;
andrewm@0: 		}
andrewm@0: 		else
andrewm@0: 			value = 0;
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	return value;
andrewm@0: }
andrewm@0: 
andrewm@0: #ifdef DBOX_CAPE_TEST
andrewm@0: // Test the functionality of the D-Box cape by checking each input and output
andrewm@0: // Loopback cable from ADC to DAC needed
andrewm@0: void render_capetest(int numMatrixFrames, int numAudioFrames, float *audioIn, float *audioOut,
andrewm@0: 			uint16_t *matrixIn, uint16_t *matrixOut)
andrewm@0: {
andrewm@0: 	static float phase = 0.0;
andrewm@0: 	static int sampleCounter = 0;
andrewm@0: 	static int invertChannel = 0;
andrewm@0: 
andrewm@0: 	// Play a sine wave on the audio output
andrewm@0: 	for(int n = 0; n < numAudioFrames; n++) {
andrewm@0: 		audioOut[2*n] = audioOut[2*n + 1] = 0.5*sinf(phase);
andrewm@0: 		phase += 2.0 * M_PI * 440.0 / 44100.0;
andrewm@0: 		if(phase >= 2.0 * M_PI)
andrewm@0: 			phase -= 2.0 * M_PI;
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	for(int n = 0; n < numMatrixFrames; n++) {
andrewm@0: 		// Change outputs every 512 samples
andrewm@0: 		if(sampleCounter < 512) {
andrewm@0: 			for(int k = 0; k < 8; k++) {
andrewm@0: 				if(k == invertChannel)
andrewm@0: 					matrixOut[n*8 + k] = 50000;
andrewm@0: 				else
andrewm@0: 					matrixOut[n*8 + k] = 0;
andrewm@0: 			}
andrewm@0: 		}
andrewm@0: 		else {
andrewm@0: 			for(int k = 0; k < 8; k++) {
andrewm@0: 				if(k == invertChannel)
andrewm@0: 					matrixOut[n*8 + k] = 0;
andrewm@0: 				else
andrewm@0: 					matrixOut[n*8 + k] = 50000;
andrewm@0: 			}
andrewm@0: 		}
andrewm@0: 
andrewm@0: 		// Read after 256 samples: input should be low
andrewm@0: 		if(sampleCounter == 256) {
andrewm@0: 			for(int k = 0; k < 8; k++) {
andrewm@0: 				if(k == invertChannel) {
andrewm@0: 					if(matrixIn[n*8 + k] < 50000) {
andrewm@0: 						dbox_printf("FAIL channel %d -- output HIGH input %d (inverted)\n", k, matrixIn[n*8 + k]);
andrewm@0: 					}
andrewm@0: 				}
andrewm@0: 				else {
andrewm@0: 					if(matrixIn[n*8 + k] > 2048) {
andrewm@0: 						dbox_printf("FAIL channel %d -- output LOW input %d\n", k, matrixIn[n*8 + k]);
andrewm@0: 					}
andrewm@0: 				}
andrewm@0: 			}
andrewm@0: 		}
andrewm@0: 		else if(sampleCounter == 768) {
andrewm@0: 			for(int k = 0; k < 8; k++) {
andrewm@0: 				if(k == invertChannel) {
andrewm@0: 					if(matrixIn[n*8 + k] > 2048) {
andrewm@0: 						dbox_printf("FAIL channel %d -- output LOW input %d (inverted)\n", k, matrixIn[n*8 + k]);
andrewm@0: 					}
andrewm@0: 				}
andrewm@0: 				else {
andrewm@0: 					if(matrixIn[n*8 + k] < 50000) {
andrewm@0: 						dbox_printf("FAIL channel %d -- output HIGH input %d\n", k, matrixIn[n*8 + k]);
andrewm@0: 					}
andrewm@0: 				}
andrewm@0: 			}
andrewm@0: 		}
andrewm@0: 
andrewm@0: 		if(++sampleCounter >= 1024) {
andrewm@0: 			sampleCounter = 0;
andrewm@0: 			invertChannel++;
andrewm@0: 			if(invertChannel >= 8)
andrewm@0: 				invertChannel = 0;
andrewm@0: 		}
andrewm@0: 	}
andrewm@0: }
andrewm@0: #endif
andrewm@0: 
andrewm@0: