annotate bayesianArraySrc/BayesianArrayStructure.cpp @ 5:5ef00d1dfe68

cross update checked and working

/*
 *  BayesianArrayStructure.cpp
 *  midiCannamReader
 *
 *  Created by Andrew on 17/07/2011.
 *  Copyright 2011 QMUL. All rights reserved.
 *
 */

//look at reset speed to one - what does this do? - get rid of?


#include "BayesianArrayStructure.h"

BayesianArrayStructure::BayesianArrayStructure(){
	printf("Bayesian structure: DeFault constructor called");

	usingIntegratedTempoEstimate = false;// use max index
	
	relativeSpeedLikelihoodStdDev = 5.0;
	
	prior.createVector(1);
	likelihood.createVector(1);
	posterior.createVector(1);

	speedPriorValue = 1.0;//default value for the speed prior
	speedEstimate = speedPriorValue;
	
	lastEventTime = 0;//ofGetElapsedTimeMillis();

	tmpBestEstimate = 0;
	crossUpdateTimeThreshold = 60;
	priorWidth = 20;
	
}

BayesianArrayStructure::BayesianArrayStructure(int length){
	printf("BAYESIAN STURCTURE CREATED LENGTH: %i\n", length);
	//this constructor isnt called  it seems
	prior.createVector(length);
	likelihood.createVector(length);
	posterior.createVector(length);
	
	lastEventTime = 0;
	

}



void BayesianArrayStructure::resetSize(int length){
	printf("BAYESIAN STRUCTURE size is : %i\n", length);
	
	prior.createVector(length);
	likelihood.createVector(length);
	posterior.createVector(length);
	
	acceleration.createVector(length);
	
}

void BayesianArrayStructure::resetSpeedSize(int length){
	printf("BAYESIAN reset SPEED size is : %i\n", length);
	
	relativeSpeedPrior.createVector(length);
	relativeSpeedLikelihood.createVector(length);
	relativeSpeedPosterior.createVector(length);
	tmpPosteriorForStorage.createVector(length);
	
	

}

void BayesianArrayStructure::setRelativeSpeedScalar(double f){
	relativeSpeedPrior.scalar = f;
	relativeSpeedPosterior.scalar = f;
	relativeSpeedLikelihood.scalar = f;
}

void BayesianArrayStructure::resetSpeedToOne(){
	relativeSpeedPrior.zero();
	relativeSpeedPosterior.zero();
	relativeSpeedLikelihood.zero();
	
	
	relativeSpeedPosterior.addGaussianShape(100, 20, 0.8);
	relativeSpeedPosterior.renormalise();
	relativeSpeedPosterior.getMaximum();
	
	setSpeedPrior(speedPriorValue);
	speedEstimate = speedPriorValue;
	
	prior.zero();
	posterior.zero();
	
	posterior.addToIndex(0, 1);
	posterior.renormalise();
	
}

void BayesianArrayStructure::setSpeedPrior(double f){	
	speedPriorValue = f;
	int index = relativeSpeedPosterior.getRealTermsAsIndex(speedPriorValue);
	relativeSpeedPosterior.zero();
	relativeSpeedPosterior.addGaussianShape(index, priorWidth, 0.8);
	printf("speed adding to index for 1 = %f\n", relativeSpeedPosterior.getRealTermsAsIndex(1));
	relativeSpeedPosterior.addToIndex(relativeSpeedPosterior.getRealTermsAsIndex(1), 1);
	
	relativeSpeedPosterior.renormalise();
	relativeSpeedPosterior.getMaximum();
	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
	printf("BAYES STRUCTU ' SPEED PRIOR %f . index %i\n", speedPriorValue, index);
	
}


void BayesianArrayStructure::setPositionDistributionScalar(double f){
	if (f > 0){
	prior.scalar = f;
	posterior.scalar = f;
	likelihood.scalar = f;
	}
}

void BayesianArrayStructure::simpleExample(){
	relativeSpeedPosterior.getMaximum();
	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
}

void BayesianArrayStructure::copyPriorToPosterior(){

	for (int i = 0;i < prior.arraySize;i++){
		posterior.array[i] = prior.array[i];
	}
}

void BayesianArrayStructure::setStartPlaying(){
	
	lastEventTime = 0;
	bestEstimate = 0;
	lastBestEstimateUpdateTime = 0;
	
	if (*realTimeMode)
		lastBestEstimateUpdateTime = ofGetElapsedTimeMillis();
	//cannot just be zero - offline bug
	//printf("start playing - best estimate %f\n", lastBestEstimateUpdateTime);
	
	resetArrays();
}

void BayesianArrayStructure::resetArrays(){
	//called when we start playing
	
	prior.zero();
	likelihood.zero();
	posterior.zero();
	
	updateCounter = 0;
	
	posterior.offset = -1000;
	setNewDistributionOffsets(0);
	
	int zeroIndex = posterior.getRealTermsAsIndex(0);
	printf("ZERO INDEX %i\n", zeroIndex);
	
	posterior.addGaussianShapeFromRealTime(0, 500, 1);//one way to add at x msec
	posterior.addGaussianShape(posterior.getRealTermsAsIndex(10), 50, 1);//alternative way
	
	//posterior.addToIndex(0, 1);
	likelihood.addConstant(1);
	
	updateCounter = 0;
	

	printf("bayes reset arrays - best estimate %f\n", lastBestEstimateUpdateTime);
	
	setSpeedPrior(speedPriorValue);
	relativeSpeedPosterior.copyFromDynamicVector(relativeSpeedPrior);
	
}


void BayesianArrayStructure::zeroArrays(){
	prior.zero();
	likelihood.zero();
	posterior.zero();
	
	relativeSpeedPrior.zero();
	relativeSpeedPosterior.zero();
	relativeSpeedLikelihood.zero();

}

/*
void BayesianArrayStructure::updateTmpBestEstimate(const double& timeDifference){
	//input is the time since the start of playing
//	double timeDiff = ofGetElapsedTimeMillis() - lastEventTime;//lastBestEstimateUpdateTime;
	double timeDiff = timeDifference;
	if (*realTimeMode)
		timeDiff = ofGetElapsedTimeMillis() - lastBestEstimateUpdateTime;

	double tmp = relativeSpeedPosterior.getIntegratedEstimate();
	tmpBestEstimate = posterior.getIndexInRealTerms(posterior.MAPestimate) + timeDiff*relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
	// 
	//printf("tmp best %f and best %f time diff %f posterior MAP %f at speed %f\n", 0Estimate, bestEstimate, timeDifference, posterior.getIndexInRealTerms(posterior.MAPestimate), relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate));
	//lastBestEstimateUpdateTime = ofGetElapsedTimeMillis();
}
*/

void BayesianArrayStructure::updateBestEstimate(const double& timeDifference){
//	double timeDiff = ofGetElapsedTimeMillis() - lastEventTime;//
	double tmp = bestEstimate;
	printf("best est routine: posterior offset %f\n", posterior.offset);
	
	double timeDiff = timeDifference;
	
	//Using timedifferencfe here will make it go wrong. Is time since beginning of playing
	
	if (*realTimeMode)
		timeDiff = ofGetElapsedTimeMillis() - lastBestEstimateUpdateTime;
	
	//lastbest is time we started playing
	/*
	if (usingIntegratedTempoEstimate)
		speedEstimateIndex = relativeSpeedPosterior.getIntegratedEstimate();
	else
		speedEstimateIndex = relativeSpeedPosterior.getMAPestimate();
	*/
	speedEstimateIndex = getSpeedEstimateIndex();
	
	speedEstimate = relativeSpeedPosterior.getIndexInRealTerms(speedEstimateIndex);
	bestEstimate = posterior.getIndexInRealTerms(posterior.MAPestimate) + timeDiff*speedEstimate;
	
	printf("best estimate update from %f to %f; time diff %f MAP %i = %f ms speed index %f est %f SpeedxTime %f\n", tmp, bestEstimate, timeDiff,
		   posterior.MAPestimate, posterior.getIndexInRealTerms(posterior.MAPestimate), speedEstimateIndex, speedEstimate, timeDiff*speedEstimate);
}

void BayesianArrayStructure::calculatePosterior(){
	//posterior.doProduct(prior, likelihood);
	assert(posterior.offset == prior.offset);
	
	int i;
	for (i = 0;i < posterior.length;i++){
		posterior.array[i] = likelihood.array[i] * prior.array[i];
	}
	
	posterior.renormalise();
	
}


double  BayesianArrayStructure::getSpeedEstimateIndex(){
	if (usingIntegratedTempoEstimate)
		return relativeSpeedPosterior.getIntegratedEstimate();
	else
		return relativeSpeedPosterior.getMAPestimate();
}


void BayesianArrayStructure::setNewDistributionOffsets(const double& newOffset){
	
	printf("prior offset was %f now %f\n", prior.offset, newOffset);
	
	prior.offset = newOffset;
	likelihood.offset = newOffset;
	
	//posterior.offset = newOffset;
}


void BayesianArrayStructure::crossUpdateArrays(DynamicVector& position, DynamicVector& speed, double timeDifference){
	//set the cutoff for offset of position first! XXX
	
//	printf("time difference %f, ", timeDifference);
	
	double timeDifferenceInPositionVectorUnits = timeDifference / prior.scalar;
	
	printf("CROSS UPDATE time diff %f ms is %f units; ", timeDifference, timeDifferenceInPositionVectorUnits);
	prior.zero();//kill prior
	
//	calculateNewPriorOffset(timeDifference);//dioesnt do anything
	
//	printf("new prior offset %f and post offset %f\n", prior.offset, posterior.offset);
	
	if (timeDifferenceInPositionVectorUnits > crossUpdateTimeThreshold)
		complexCrossUpdate(timeDifferenceInPositionVectorUnits);
	else
		translateByMaximumSpeed(timeDifferenceInPositionVectorUnits);	
			

	updateCounter++;
	prior.renormalise();

}

void BayesianArrayStructure::complexCrossUpdate(const double& timeDifferenceInPositionVectorUnits){

	
	printf("before cross c : posterior map is %i = %f ms time diff pos vec %f\n", posterior.getMAPestimate(), posterior.getIndexInRealTerms(prior.getMAPestimate()), timeDifferenceInPositionVectorUnits);
	
	int distanceMoved, newPriorIndex;
	
	double speedValue = relativeSpeedPosterior.offset;
	
	for (int i = 0;i < relativeSpeedPosterior.arraySize;i++){
		
	//	double speedValue = relativeSpeedPosterior.getIndexInRealTerms(i);//so for scalar 0.01, 50 -> speed value of 0.5
		
		//so we have moved 
		distanceMoved = round(timeDifferenceInPositionVectorUnits * speedValue);//round the value
	//	printf("Speed value %f time %f gives distance %i\n", speedValue, timeDifferenceInPositionVectorUnits, distanceMoved);
		
		if (relativeSpeedPosterior.array[i] != 0){
			
			double speedContribution = relativeSpeedPosterior.array[i];
			
		//	printf("speed [%i](val[%f]) gives %f moved %i in %f units \n", i, relativeSpeedPosterior.array[i], speedValue, distanceMoved, timeDifferenceInPositionVectorUnits);
			
			//1/2/12 deleted line
			newPriorIndex = posterior.offset - prior.offset + distanceMoved;//i.e. where post[0] goes to in terms of prior at this speed
			int postIndex = 0;//index of posterior that will contribute
			
			while (postIndex < posterior.arraySize && newPriorIndex < prior.arraySize){

			//did use a for loop
			//	for (postIndex = 0;postIndex < posterior.arraySize;postIndex++){
				//old posterior contributing to new prior
			
				//would use this method
				//newPriorIndex = postIndex + posterior.offset - prior.offset + distanceMoved;
				
				if (newPriorIndex >= 0){
					prior.addToIndex(newPriorIndex, posterior.array[postIndex]*speedContribution);
			//		printf("speed index %i new prior index %i post val %f speed contrib %f dist %i\n", i, newPriorIndex, posterior.array[postIndex], speedContribution, distanceMoved);
				}
				//but we actually do this for simplicity
				newPriorIndex++;
				postIndex++;
			}//end for. now while
			
			
		}//if not zero
		
		speedValue += relativeSpeedPosterior.scalar;//optimised line
		//as we wanted:
		//	double speedValue = relativeSpeedPosterior.getIndexInRealTerms(i);//so for scalar 0.01, 50 -> speed value of 0.5
		
	}//end speed
	
	
	printf("after cross c : prior map is %i = %f ms\n", prior.getMAPestimate(), prior.getIndexInRealTerms(prior.getMAPestimate()));
}



void BayesianArrayStructure::translateByMaximumSpeed(const double& timeDifferenceInPositionVectorUnits){

	int distanceMoved, newPriorIndex;
	
	double speedValue = relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
	//so for scalar 0.01, 50 -> speed value of 0.5
	double speedContribution = relativeSpeedPosterior.array[relativeSpeedPosterior.integratedEstimate];
		//so we have moved 
		distanceMoved = round(timeDifferenceInPositionVectorUnits * speedValue);//round the value
					//	printf("speed [%i] gives %f moved %i in %f units \n", i, speedValue, distanceMoved, timeDifferenceInPositionVectorUnits);
			
			for (int postIndex = 0;postIndex < posterior.arraySize;postIndex++){
				//old posterior contributing to new prior
				newPriorIndex = postIndex + posterior.offset - prior.offset + distanceMoved;
				if (newPriorIndex >= 0 && newPriorIndex < prior.arraySize){
					prior.addToIndex(newPriorIndex, posterior.array[postIndex]*speedContribution);
				}
				
			}
	
}

void BayesianArrayStructure::addGaussianNoiseToSpeedPosterior(const double& std_dev){
	tmpPosteriorForStorage.copyFromDynamicVector(relativeSpeedPosterior);
	
	for (int i = 0;i < relativeSpeedPosterior.length;i++){
		tmpPosteriorForStorage.addGaussianShape(i, std_dev, relativeSpeedPosterior.array[i]);
		}
												
	tmpPosteriorForStorage.renormalise();
	
	relativeSpeedPosterior.copyFromDynamicVector(tmpPosteriorForStorage);											
}


void BayesianArrayStructure::addTriangularNoiseToSpeedPosterior(const double& std_dev){
	tmpPosteriorForStorage.copyFromDynamicVector(relativeSpeedPosterior);
	
	for (int i = 0;i < relativeSpeedPosterior.length;i++){
		//adding a linear amount depending on distance
		tmpPosteriorForStorage.addTriangularShape(i, std_dev*2.0, relativeSpeedPosterior.array[i]);
	}
	
	tmpPosteriorForStorage.renormalise();
	
	relativeSpeedPosterior.copyFromDynamicVector(tmpPosteriorForStorage);											
}

void BayesianArrayStructure::calculateNewPriorOffset(const double& timeDifference){
	
//	double maxSpeed = relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
	
	double maxSpeed = relativeSpeedPosterior.getIndexInRealTerms(getSpeedEstimateIndex());//either integrated or MAP
	//	printf("Maxspeed is %f\n", maxSpeed);
	
	double priorMax = posterior.getMaximum();
	double distanceTravelled = maxSpeed * (timeDifference / prior.scalar);
	double newMaxLocation = posterior.MAPestimate + distanceTravelled;
	//	printf("MAP: %i, tim df %f, distance %f, new location %f\n", posterior.MAPestimate, timeDifference, distanceTravelled, newMaxLocation);
	
}


void BayesianArrayStructure::decaySpeedDistribution(double timeDifference){
	
	// commented for the moment
	 double relativeAmount = max(1.0, timeDifference/1000.);
//	printf("decay %f around %i \n", timeDifference, relativeSpeedPosterior.MAPestimate);
	relativeAmount *= speedDecayAmount;
	relativeSpeedPosterior.renormalise();
	relativeSpeedPosterior.addGaussianShape(relativeSpeedPosterior.MAPestimate, speedDecayWidth, relativeAmount);
	
	relativeSpeedPosterior.renormalise();
	double newMax = relativeSpeedPosterior.getMaximum();
	
	//old code
//	relativeSpeedPosterior.addGaussianShape(relativeSpeedPosterior.MAPestimate, speedDecayWidth, 10);
	//relativeSpeedPosterior.addConstant(1);
	
	/*
	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
	relativeSpeedLikelihood.zero();
	relativeSpeedLikelihood.addConstant(0.2);
	relativeSpeedLikelihood.addGaussianShape(relativeSpeedPosterior.maximumValue, speedDecayWidth, relativeAmount);
	relativeSpeedPosterior.doProduct(relativeSpeedPrior, relativeSpeedLikelihood);
	relativeSpeedPosterior.renormalise();
	 */
	

	
}

void BayesianArrayStructure::setLikelihoodToConstant(){
	//set new likelihood
	relativeSpeedLikelihood.zero();
	relativeSpeedLikelihood.addConstant(speedLikelihoodNoise);
}


void BayesianArrayStructure::updateTempoLikelihood(const double& speedRatio, const double& matchFactor){
	
	//speedratio is speed of played relative to the recording
	
	double index = relativeSpeedLikelihood.getRealTermsAsIndex(speedRatio);
	//	printf("index of likelihood would be %f for ratio %f\n", index, speedRatio);
	if (index >= 0 && index < relativeSpeedPrior.length){	
		relativeSpeedLikelihood.addGaussianShape(index , relativeSpeedLikelihoodStdDev, matchFactor);
	}
}
	


void BayesianArrayStructure::updateTempoDistribution(){

	//copy posterior to prior
	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);

	//update
	relativeSpeedPosterior.doProduct(relativeSpeedPrior, relativeSpeedLikelihood);

	//normalise
	relativeSpeedPosterior.renormalise();
		
	relativeSpeedPosterior.getMaximum();	
	
	//relativeSpeedPosterior.updateIntegratedEstimate(); - could be swayed when off to side 
	//so now limited to where there is room sround the MAP estimate
	relativeSpeedPosterior.updateLimitedIntegratedEstimate();
	
	speedEstimate = relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
}


void BayesianArrayStructure::calculateTempoUpdate(){
	//copy posterior to prior
	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
	
	//update
	relativeSpeedPosterior.doProduct(relativeSpeedPrior, relativeSpeedLikelihood);
	
	//normalise
	relativeSpeedPosterior.renormalise();
	
	relativeSpeedPosterior.getMaximum();	
	
}


void BayesianArrayStructure::drawArrays(){
	
	//bayesArray.drawFloatArray(&bayesArray.prior[0], 0, 200);
	//bayesArray.drawFloatArray(&bayesArray.prior[0], 0, 200);
	
	int displaySize = prior.arraySize;
	ofSetColor(0,0,255);
	prior.drawVector(0, displaySize);
	ofSetColor(0,255,0);
	likelihood.drawVector(0, displaySize);
	ofSetColor(255,0,255);
	posterior.drawVector(0, displaySize);
	
	
	
}


void BayesianArrayStructure::drawTempoArrays(){
	ofSetColor(0,0,255);
//	relativeSpeedPrior.drawVector(0, relativeSpeedPrior.arraySize);
	
	ofSetColor(0,150,255);
	relativeSpeedLikelihood.drawVector(0, relativeSpeedLikelihood.arraySize);
	
//	relativeSpeedLikelihood.drawConstrainedVector(0, 199, 0, 1000);// relativeSpeedLikelihood.arraySize);	
	ofSetColor(255,0,0);
	relativeSpeedPosterior.drawVector(0, relativeSpeedPosterior.arraySize);
	
//	ofSetColor(0,0,255);
//	tmpPosteriorForStorage.drawVector(0, tmpPosteriorForStorage.arraySize);
	
	ofSetColor(255,255, 255);
	ofLine(screenWidth/2, 0, screenWidth/2, ofGetHeight());//middle of screen
	
	ofSetColor(25, 0, 250);
	double fractionOfScreen = ((double)relativeSpeedPosterior.integratedEstimate / relativeSpeedPosterior.length);
	ofLine(screenWidth * fractionOfScreen, 0, screenWidth * fractionOfScreen, ofGetHeight());
	
	ofSetColor(255, 0, 20);
	fractionOfScreen = ((double)relativeSpeedPosterior.MAPestimate / relativeSpeedPosterior.length);
	ofLine(screenWidth * fractionOfScreen, 0, screenWidth * fractionOfScreen, ofGetHeight());
	 
	
}


void BayesianArrayStructure::drawArraysRelativeToTimeframe(const double& startTimeMillis, const double& endTimeMillis){

	screenWidth = ofGetWidth();
	
	int startArrayIndex = 0;
	
	if (prior.getIndexInRealTerms(prior.arraySize-1) > startTimeMillis){
		//i.e. the array is on the page
	
	while (prior.getIndexInRealTerms(startArrayIndex) < startTimeMillis){
		startArrayIndex++;
	}
	int endArrayIndex = prior.arraySize-1;
	//could find constraints here
	if (prior.getIndexInRealTerms(prior.arraySize-1) > endTimeMillis)
		endArrayIndex = (floor)((endTimeMillis - prior.offset)/prior.scalar);
	
	//so we need to figure where start and end array are on screen
	int startScreenPosition, endScreenPosition;
	double screenWidthMillis = endTimeMillis - startTimeMillis;
		
	startScreenPosition = (prior.getIndexInRealTerms(startArrayIndex) - startTimeMillis)*screenWidth/screenWidthMillis;
	endScreenPosition = (double)(prior.getIndexInRealTerms(endArrayIndex) - startTimeMillis)*screenWidth/screenWidthMillis;
		
	ofSetColor(0,0,100);
	string relativeString = " offset "+ofToString(prior.offset, 1);//starttimes("+ofToString(startTimeMillis)+", "+ofToString(endTimeMillis);
	relativeString += ": index "+ofToString(startArrayIndex)+" , "+ofToString(endArrayIndex)+" [";
//	relativeString += ofToString(prior.getIndexInRealTerms(endArrayIndex), 3)+"] (sc-width:"+ofToString(screenWidthMillis, 1)+")  ";
	relativeString += " mapped to screen "+ofToString(startScreenPosition)+" , "+ofToString(endScreenPosition);
//	ofDrawBitmapString(relativeString, 100, 180);
	
		
		
	ofSetColor(100,100,100);//255, 255, 0);
	likelihood.drawConstrainedVector(startArrayIndex, endArrayIndex, startScreenPosition, endScreenPosition);

//	ofSetColor(0,0,200);
	ofSetColor(170,170,170);//00,200);
	prior.drawConstrainedVector(startArrayIndex, endArrayIndex, startScreenPosition, endScreenPosition);
		
		ofSetColor(0,0,150);	
//	ofSetColor(200, 0, 0);
	posterior.drawConstrainedVector(startArrayIndex, endArrayIndex, startScreenPosition, endScreenPosition);
		
		
//	ofSetColor(0, 200, 255);
//	acceleration.drawConstrainedVector(startArrayIndex, endArrayIndex, startScreenPosition, endScreenPosition);
		
		
	}

}


/*
 
 void BayesianArrayStructure::updateTempoDistribution(const double& speedRatio, const double& matchFactor){
 //speedratio is speed of played relative to the recording
 
 double index = relativeSpeedLikelihood.getRealTermsAsIndex(speedRatio);
 //	printf("\nindex of likelihood would be %f\n", index);
 if (index >= 0 && index < relativeSpeedPrior.length){
 //then we can do update
 
 //set new likelihood
 relativeSpeedLikelihood.zero();
 relativeSpeedLikelihood.addConstant(speedLikelihoodNoise);
 
 relativeSpeedLikelihood.addGaussianShape(index , 5, 0.5*matchFactor);
 
 
 //copy posterior to prior
 relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
 
 //update
 relativeSpeedPosterior.doProduct(relativeSpeedPrior, relativeSpeedLikelihood);
 
 //normalise
 relativeSpeedPosterior.renormalise();
 
 relativeSpeedPosterior.getMaximum();	
 }//end if within range
 
 
 }
 
 */