annotate src/BayesianArrayStructure.cpp @ 4:4a8e6a6cd224

optimised draw function in dynamic vector class. Added Gaussian lookup but not yet used.

/*
 *  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");
	
	prior.createVector(1);
	likelihood.createVector(1);
	posterior.createVector(1);

	speedPriorValue = 1.0;
	
	lastEventTime = ofGetElapsedTimeMillis();
	
	/*
	 tmpPrior.createVector(240);
	 tmpPrior.addGaussianShape(100, 40, 1);
	 tmpPrior.addGaussianShape(200, 10, 0.2);
	 tmpPrior.translateDistribution(20);
	 */
	tmpBestEstimate = 0;
	crossUpdateTimeThreshold = 100;
}

BayesianArrayStructure::BayesianArrayStructure(int length){
	printf("BAYESIAN STURTUCRE 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::resetSpeedToOne(){
	relativeSpeedPrior.zero();
	relativeSpeedPosterior.zero();
	relativeSpeedLikelihood.zero();
	
	//relativeSpeedPosterior.addGaussianShape(40, 5, 0.6);
	
	relativeSpeedPosterior.addGaussianShape(100, 20, 0.8);
//	relativeSpeedPosterior.addGaussianShape(50, 1, 0.8);
	
//	relativeSpeedPosterior.addToIndex(100, 1);
	//relativeSpeedPosterior.addToIndex(50, 1);
	relativeSpeedPosterior.renormalise();
	relativeSpeedPosterior.getMaximum();
	
	setSpeedPrior(speedPriorValue);
	
	prior.zero();
	posterior.zero();
//	posterior.offset = - 200;
//	posterior.addGaussianShape(200, 40, 1);
	posterior.addToIndex(0, 1);
	posterior.renormalise();
	
	//acceleration.addGaussianShape(2000, 20, 0.8);
	
}

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

void BayesianArrayStructure::resetSpeedSize(int length){
	printf("BAYESIAN 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::setPositionDistributionScalar(double f){
	if (f > 0){
	prior.scalar = f;
	posterior.scalar = f;
	likelihood.scalar = f;
	}
}

void BayesianArrayStructure::simpleExample(){
	//simple example
	
//	 prior.addGaussianShape(50, 10, 1);
//	prior.addGaussianShape(150, 30, 0.3);
//	prior.addGaussianShape(250, 30, 0.2);
	
//	likelihood.addGaussianShape(90, 20, 0.6);
//	likelihood.addConstant(0.02);
//	posterior.doProduct(prior, likelihood);
	
//	relativeSpeedPosterior.addToIndex(100, 1);
//	relativeSpeedPosterior.addToIndex(40, 0.7);	
	
//	relativeSpeedPosterior.addGaussianShape(100, 2, 1);
//	relativeSpeedPosterior.addGaussianShape(40, 2, 0.5);
	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;//ofGetElapsedTimeMillis();
	bestEstimate = 0;
	lastBestEstimateUpdateTime = ofGetElapsedTimeMillis();
	
	resetArrays();
}

void BayesianArrayStructure::resetArrays(){
	//called when we start playing
	
	prior.zero();
	likelihood.zero();
	posterior.zero();
	
	updateCounter = 0;
	
	posterior.offset = 0;
	setNewDistributionOffsets(0);
	
	int zeroIndex = posterior.getRealTermsAsIndex(0);
	
	posterior.addGaussianShape(zeroIndex, 500, 1);
	//	posterior.addToIndex(0, 1);
	likelihood.addConstant(1);
	
	updateCounter = 0;
	
	bestEstimate = 0;
//	lastBestEstimateUpdateTime = ofGetElapsedTimeMillis();
	
	setSpeedPrior(speedPriorValue);
}

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;
	
	tmpBestEstimate = posterior.getIndexInRealTerms(posterior.MAPestimate) + timeDifference*relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.MAPestimate);
	// 
	printf("tmp best %f and best %f time diff %f posterior MAP %f at speed %f\n", tmpBestEstimate, bestEstimate, timeDifference, posterior.getIndexInRealTerms(posterior.MAPestimate), relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.MAPestimate));
	//lastBestEstimateUpdateTime = ofGetElapsedTimeMillis();
}
	
void BayesianArrayStructure::updateBestEstimate(){
//	double timeDiff = ofGetElapsedTimeMillis() - lastEventTime;//
	double timeDiff = ofGetElapsedTimeMillis() - lastBestEstimateUpdateTime;
	bestEstimate = posterior.getIndexInRealTerms(posterior.MAPestimate) + timeDiff*relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.MAPestimate);
	
//	bestEstimate = tmpBestEstimate;
}

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


	 
}




void BayesianArrayStructure::setNewDistributionOffsets(const double& 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;
	

	prior.zero();//kill prior
	calculateNewPriorOffset(timeDifference);//set new prior offset here
	
	if (timeDifferenceInPositionVectorUnits > crossUpdateTimeThreshold)
		complexCrossUpdate(timeDifferenceInPositionVectorUnits);
	else
		translateByMaximumSpeed(timeDifferenceInPositionVectorUnits);	
			

	updateCounter++;
	prior.renormalise();

}

void BayesianArrayStructure::complexCrossUpdate(const double& timeDifferenceInPositionVectorUnits){
	int distanceMoved, newPriorIndex;
	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
	
		if (relativeSpeedPosterior.array[i] != 0){
			double speedContribution = relativeSpeedPosterior.array[i];
			//	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);
				}
				
			}
			
		}//if not zero
	}//end speed
}



void BayesianArrayStructure::translateByMaximumSpeed(const double& timeDifferenceInPositionVectorUnits){
	int distanceMoved, newPriorIndex;

		
		double speedValue = relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.MAPestimate);//using max value only
	//so for scalar 0.01, 50 -> speed value of 0.5
	double speedContribution = relativeSpeedPosterior.array[relativeSpeedPosterior.MAPestimate];
		//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.MAPestimate);
	//	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::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
	

}


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

void BayesianArrayStructure::updateTempoLikelihood(const double& speedRatio, const double& matchFactor){
	
	double index = relativeSpeedLikelihood.getRealTermsAsIndex(speedRatio);

	if (index >= 0 && index < relativeSpeedPrior.length){
		relativeSpeedLikelihood.addGaussianShape(index , 5, 0.5);//*matchFactor);
	}
}


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);
	
	
//	ofSetColor(255,255,255);
//	tmpPrior.drawVector(0,300);
	
}


void BayesianArrayStructure::drawTempoArrays(){
	ofSetColor(0,255,255);
	relativeSpeedPrior.drawVector(0, relativeSpeedPrior.arraySize);
	
	ofSetColor(255,0,255);
	relativeSpeedLikelihood.drawVector(0, relativeSpeedLikelihood.arraySize);
	
	ofSetColor(255,255,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(0, 255, 0);
	double 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(255, 255, 0);
	likelihood.drawConstrainedVector(startArrayIndex, endArrayIndex, startScreenPosition, endScreenPosition);

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

}