andrew@0: /*
andrew@0:  *  BayesianArrayStructure.cpp
andrew@0:  *  midiCannamReader
andrew@0:  *
andrew@0:  *  Created by Andrew on 17/07/2011.
andrew@0:  *  Copyright 2011 QMUL. All rights reserved.
andrew@0:  *
andrew@0:  */
andrew@0: 
andrew@0: //look at reset speed to one - what does this do? - get rid of?
andrew@0: 
andrew@0: 
andrew@0: #include "BayesianArrayStructure.h"
andrew@0: 
andrew@0: BayesianArrayStructure::BayesianArrayStructure(){
andrew@0: 	printf("Bayesian structure: DeFault constructor called");
andrew@0: 
andrew@4: 	usingIntegratedTempoEstimate = false;// use max index
andrew@7: 	updatingSpeedDistribution = false;
andrew@0: 	
andrew@0: 	relativeSpeedLikelihoodStdDev = 5.0;
andrew@0: 	
andrew@0: 	prior.createVector(1);
andrew@0: 	likelihood.createVector(1);
andrew@0: 	posterior.createVector(1);
andrew@0: 
andrew@0: 	speedPriorValue = 1.0;//default value for the speed prior
andrew@0: 	speedEstimate = speedPriorValue;
andrew@0: 	
andrew@0: 	lastEventTime = 0;//ofGetElapsedTimeMillis();
andrew@0: 
andrew@0: 	tmpBestEstimate = 0;
andrew@5: 	crossUpdateTimeThreshold = 60;
andrew@39: 	priorWidth = 0.1;
andrew@36: 	
andrew@36: 
andrew@0: 	
andrew@0: }
andrew@0: 
andrew@0: BayesianArrayStructure::BayesianArrayStructure(int length){
andrew@0: 	printf("BAYESIAN STURCTURE CREATED LENGTH: %i\n", length);
andrew@0: 	//this constructor isnt called  it seems
andrew@0: 	prior.createVector(length);
andrew@0: 	likelihood.createVector(length);
andrew@0: 	posterior.createVector(length);
andrew@0: 	
andrew@0: 	lastEventTime = 0;
andrew@0: 	
andrew@0: 
andrew@0: }
andrew@0: 
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::resetSize(int length){
andrew@0: 	printf("BAYESIAN STRUCTURE size is : %i\n", length);
andrew@0: 	
andrew@0: 	prior.createVector(length);
andrew@0: 	likelihood.createVector(length);
andrew@0: 	posterior.createVector(length);
andrew@0: 	
andrew@0: 	acceleration.createVector(length);
andrew@36: 	printf("scalar %f\n", posterior.scalar);
andrew@0: 	
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::resetSpeedSize(int length){
andrew@0: 	printf("BAYESIAN reset SPEED size is : %i\n", length);
andrew@0: 	
andrew@0: 	relativeSpeedPrior.createVector(length);
andrew@0: 	relativeSpeedLikelihood.createVector(length);
andrew@0: 	relativeSpeedPosterior.createVector(length);
andrew@0: 	tmpPosteriorForStorage.createVector(length);
andrew@0: 	
andrew@0: 	
andrew@0: 
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::setRelativeSpeedScalar(double f){
andrew@0: 	relativeSpeedPrior.scalar = f;
andrew@0: 	relativeSpeedPosterior.scalar = f;
andrew@0: 	relativeSpeedLikelihood.scalar = f;
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::resetSpeedToOne(){
andrew@0: 	relativeSpeedPrior.zero();
andrew@0: 	relativeSpeedPosterior.zero();
andrew@0: 	relativeSpeedLikelihood.zero();
andrew@0: 	
andrew@0: 	
andrew@20: //	relativeSpeedPosterior.addGaussianShape(100, 8, 0.1);
andrew@20: //	relativeSpeedPosterior.renormalise();
andrew@20: //	relativeSpeedPosterior.getMaximum();
andrew@0: 	
andrew@0: 	setSpeedPrior(speedPriorValue);
andrew@0: 	speedEstimate = speedPriorValue;
andrew@0: 	
andrew@0: 	prior.zero();
andrew@0: 	posterior.zero();
andrew@0: 	
andrew@0: 	posterior.addToIndex(0, 1);
andrew@0: 	posterior.renormalise();
andrew@0: 	
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::setSpeedPrior(double f){	
andrew@0: 	speedPriorValue = f;
andrew@0: 	int index = relativeSpeedPosterior.getRealTermsAsIndex(speedPriorValue);
andrew@0: 	relativeSpeedPosterior.zero();
andrew@37: 	
andrew@37: 	//changed these a bit - still need to figure tempo process out properly
andrew@39: 	relativeSpeedPosterior.addGaussianShapeFromRealTime(1, priorWidth, 4);
andrew@39: 		
andrew@39: 	//relativeSpeedPosterior.addGaussianShape(relativeSpeedPosterior.getRealTermsAsIndex(1), 4, 1);
andrew@36: 	
andrew@4: 	printf("speed adding to index for 1 = %f\n", relativeSpeedPosterior.getRealTermsAsIndex(1));
andrew@36: //	relativeSpeedPosterior.addToIndex(relativeSpeedPosterior.getRealTermsAsIndex(1), 0.5);
andrew@20: 	//relativeSpeedPosterior.addGaussianShapeFromRealTime(1, 3, 0.5);
andrew@11: 
andrew@11: 	
andrew@0: 	relativeSpeedPosterior.renormalise();
andrew@0: 	relativeSpeedPosterior.getMaximum();
andrew@11: 	
andrew@11: 	
andrew@0: 	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
andrew@11: 	
andrew@11: 	
andrew@11: 	
andrew@0: 	printf("BAYES STRUCTU ' SPEED PRIOR %f . index %i\n", speedPriorValue, index);
andrew@0: 	
andrew@0: }
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::setPositionDistributionScalar(double f){
andrew@0: 	if (f > 0){
andrew@0: 	prior.scalar = f;
andrew@0: 	posterior.scalar = f;
andrew@0: 	likelihood.scalar = f;
andrew@36: 	printf("SET POS DISTBN SCALAR %f\n", f);
andrew@0: 	}
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::simpleExample(){
andrew@0: 	relativeSpeedPosterior.getMaximum();
andrew@0: 	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::copyPriorToPosterior(){
andrew@0: 
andrew@0: 	for (int i = 0;i < prior.arraySize;i++){
andrew@0: 		posterior.array[i] = prior.array[i];
andrew@0: 	}
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::setStartPlaying(){
andrew@0: 	
andrew@0: 	lastEventTime = 0;
andrew@0: 	bestEstimate = 0;
andrew@0: 	lastBestEstimateUpdateTime = 0;
andrew@3: 	
andrew@0: 	if (*realTimeMode)
andrew@0: 		lastBestEstimateUpdateTime = ofGetElapsedTimeMillis();
andrew@0: 	//cannot just be zero - offline bug
andrew@0: 	//printf("start playing - best estimate %f\n", lastBestEstimateUpdateTime);
andrew@0: 	
andrew@0: 	resetArrays();
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::resetArrays(){
andrew@0: 	//called when we start playing
andrew@0: 	
andrew@0: 	prior.zero();
andrew@0: 	likelihood.zero();
andrew@0: 	posterior.zero();
andrew@0: 	
andrew@0: 	updateCounter = 0;
andrew@0: 	
andrew@42: 	posterior.offset = -1 * matchWindowWidth / 2;//is set from the audioEventMatcher class
andrew@0: 	setNewDistributionOffsets(0);
andrew@0: 	
andrew@0: 	int zeroIndex = posterior.getRealTermsAsIndex(0);
andrew@3: 	printf("ZERO INDEX %i\n", zeroIndex);
andrew@39: 	int offset = posterior.getIndexInRealTerms(0);
andrew@39: 	printf("offset is %i\n", offset);
andrew@0: 	
andrew@42: 	posterior.addGaussianShapeFromRealTime(0, startingWindowWidth, 1);//one way to add at x msec
andrew@7: //	posterior.addGaussianShape(posterior.getRealTermsAsIndex(10), 50, 1);//alternative way
andrew@3: 	
andrew@0: 	likelihood.addConstant(1);
andrew@0: 	
andrew@0: 	updateCounter = 0;
andrew@0: 	
andrew@0: 
andrew@0: 	printf("bayes reset arrays - best estimate %f\n", lastBestEstimateUpdateTime);
andrew@0: 	
andrew@39: 	setSpeedPrior(speedPriorValue);//setting the prior speed
andrew@3: 	relativeSpeedPosterior.copyFromDynamicVector(relativeSpeedPrior);
andrew@3: 	
andrew@0: }
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::zeroArrays(){
andrew@0: 	prior.zero();
andrew@0: 	likelihood.zero();
andrew@0: 	posterior.zero();
andrew@0: 	
andrew@0: 	relativeSpeedPrior.zero();
andrew@0: 	relativeSpeedPosterior.zero();
andrew@0: 	relativeSpeedLikelihood.zero();
andrew@0: 
andrew@0: }
andrew@0: 
andrew@4: /*
andrew@0: void BayesianArrayStructure::updateTmpBestEstimate(const double& timeDifference){
andrew@0: 	//input is the time since the start of playing
andrew@0: //	double timeDiff = ofGetElapsedTimeMillis() - lastEventTime;//lastBestEstimateUpdateTime;
andrew@0: 	double timeDiff = timeDifference;
andrew@0: 	if (*realTimeMode)
andrew@0: 		timeDiff = ofGetElapsedTimeMillis() - lastBestEstimateUpdateTime;
andrew@0: 
andrew@0: 	double tmp = relativeSpeedPosterior.getIntegratedEstimate();
andrew@0: 	tmpBestEstimate = posterior.getIndexInRealTerms(posterior.MAPestimate) + timeDiff*relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
andrew@0: 	// 
andrew@0: 	//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));
andrew@0: 	//lastBestEstimateUpdateTime = ofGetElapsedTimeMillis();
andrew@0: }
andrew@4: */
andrew@4: 
andrew@0: void BayesianArrayStructure::updateBestEstimate(const double& timeDifference){
andrew@0: //	double timeDiff = ofGetElapsedTimeMillis() - lastEventTime;//
andrew@3: 	double tmp = bestEstimate;
andrew@10: //	printf("best est routine: posterior offset %f\n", posterior.offset);
andrew@0: 	
andrew@0: 	double timeDiff = timeDifference;
andrew@0: 	
andrew@0: 	//Using timedifferencfe here will make it go wrong. Is time since beginning of playing
andrew@0: 	
andrew@0: 	if (*realTimeMode)
andrew@0: 		timeDiff = ofGetElapsedTimeMillis() - lastBestEstimateUpdateTime;
andrew@0: 	
andrew@0: 	//lastbest is time we started playing
andrew@4: 	/*
andrew@0: 	if (usingIntegratedTempoEstimate)
andrew@0: 		speedEstimateIndex = relativeSpeedPosterior.getIntegratedEstimate();
andrew@0: 	else
andrew@4: 		speedEstimateIndex = relativeSpeedPosterior.getMAPestimate();
andrew@4: 	*/
andrew@4: 	speedEstimateIndex = getSpeedEstimateIndex();
andrew@0: 	
andrew@0: 	speedEstimate = relativeSpeedPosterior.getIndexInRealTerms(speedEstimateIndex);
andrew@0: 	bestEstimate = posterior.getIndexInRealTerms(posterior.MAPestimate) + timeDiff*speedEstimate;
andrew@0: 	
andrew@10: //	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,
andrew@10: //		   posterior.MAPestimate, posterior.getIndexInRealTerms(posterior.MAPestimate), speedEstimateIndex, speedEstimate, timeDiff*speedEstimate);
andrew@17: 
andrew@17: 		printf("BEST ESTIMATE %f\n", bestEstimate);
andrew@0: }
andrew@0: 
andrew@10: 
andrew@0: void BayesianArrayStructure::calculatePosterior(){
andrew@0: 	//posterior.doProduct(prior, likelihood);
andrew@4: 	assert(posterior.offset == prior.offset);
andrew@0: 	
andrew@0: 	int i;
andrew@0: 	for (i = 0;i < posterior.length;i++){
andrew@0: 		posterior.array[i] = likelihood.array[i] * prior.array[i];
andrew@0: 	}
andrew@0: 	
andrew@0: 	posterior.renormalise();
andrew@0: 	
andrew@17: 	//	printf("After CALC");
andrew@17: 	//	printPostOffset();	
andrew@6: 	
andrew@0: }
andrew@0: 
andrew@0: 
andrew@4: double  BayesianArrayStructure::getSpeedEstimateIndex(){
andrew@4: 	if (usingIntegratedTempoEstimate)
andrew@4: 		return relativeSpeedPosterior.getIntegratedEstimate();
andrew@4: 	else
andrew@4: 		return relativeSpeedPosterior.getMAPestimate();
andrew@4: }
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::setNewDistributionOffsets(const double& newOffset){
andrew@4: 	
andrew@4: 	printf("prior offset was %f now %f\n", prior.offset, newOffset);
andrew@4: 	
andrew@0: 	prior.offset = newOffset;
andrew@0: 	likelihood.offset = newOffset;
andrew@0: 	//posterior.offset = newOffset;
andrew@0: }
andrew@0: 
andrew@53: void BayesianArrayStructure::zeroDistributionAtPosition(DynamicVector& distribution, const double& position){
andrew@53: 	distribution.zero();
andrew@53: 	setNewDistributionOffsets(max(0., position - (distribution.scalar*distribution.arraySize/2)));
andrew@53: }
andrew@0: 
andrew@6: void BayesianArrayStructure::updateBayesianDistributions(const double& newEventTime){
andrew@6: 	
andrew@6: 	//NEED TO CHECK HERE THAT THEY HAVE THE SAME OFFSETS
andrew@6: 	prior.copyFromDynamicVector(posterior);//try the otehr way
andrew@6: 	
andrew@6: 	//bayesianStruct.copyPriorToPosterior();
andrew@6: 	//need to get new MAP position and set the offset of the arrays
andrew@6: 	//currently bestEstimate is the approx for the new MAP position
andrew@6: //	int tmpMap = bayesianStruct.posterior.getMAPestimate();
andrew@6: 	
andrew@6: 	double timeDifference = newEventTime - lastEventTime; 
andrew@17: //	printf("updating distributions at time %f diff %f offset %f\n", newEventTime, timeDifference, posterior.offset);
andrew@6: 	
andrew@6: 	//addnoise to the tempo distribution
andrew@6: 	//bayesianStruct.decaySpeedDistribution(timeDifference);
andrew@6: 	
andrew@7: 	if (timeDifference > 50 && updatingSpeedDistribution){
andrew@6: 		addGaussianNoiseToSpeedPosterior(timeDifference * 10.0 / 100.);
andrew@6: 	}
andrew@6: 
andrew@6: 	printPostOffset();
andrew@6: 	
andrew@6: 	updateBestEstimate(timeDifference);
andrew@6: 	lastBestEstimateUpdateTime = newEventTime;//getTimeNow(timePlayed);
andrew@6: 
andrew@8: 	//set TARGETS - commented tenmporarily
andrew@8: 	setNewDistributionOffsets(max(0., bestEstimate - (prior.scalar*prior.arraySize/2)));
andrew@6: 	
andrew@6: 	crossUpdateArrays(posterior, relativeSpeedPosterior, timeDifference);
andrew@6: 	
andrew@6: 	//i.e. using the same offset as prior
andrew@53: 	posterior.offset = prior.offset;// NOW in SRT NEW DISTBN OFFSET
andrew@6: 	
andrew@6: 	//	float tmpPrior = max(0., bestEstimate - (prior.scalar*prior.arraySize/2));//	prior.offset = max(0., bestEstimate - (prior.scalar*prior.arraySize/2));
andrew@6: 	//	printf("Using prior offset of %f not %f\n", tmpPrior, prior.offset);
andrew@6: 	
andrew@6: 	lastEventTime = newEventTime;//lastEventTime = ofGetElapsedTimeMillis();
andrew@6: 	
andrew@6: }
andrew@6: 
andrew@6: 
andrew@10: 
andrew@10: 
andrew@0: void BayesianArrayStructure::crossUpdateArrays(DynamicVector& position, DynamicVector& speed, double timeDifference){
andrew@8: 
andrew@0: 	//set the cutoff for offset of position first! XXX
andrew@0: 	
andrew@0: 	double timeDifferenceInPositionVectorUnits = timeDifference / prior.scalar;
andrew@0: 	
andrew@17: 	//printf("CROSS UPDATE time diff %f ms is %f units; ", timeDifference, timeDifferenceInPositionVectorUnits);
andrew@0: 	prior.zero();//kill prior
andrew@4: 	
andrew@4: //	calculateNewPriorOffset(timeDifference);//dioesnt do anything
andrew@4: 	
andrew@5: //	printf("new prior offset %f and post offset %f\n", prior.offset, posterior.offset);
andrew@0: 	
andrew@0: 	if (timeDifferenceInPositionVectorUnits > crossUpdateTimeThreshold)
andrew@0: 		complexCrossUpdate(timeDifferenceInPositionVectorUnits);
andrew@0: 	else
andrew@0: 		translateByMaximumSpeed(timeDifferenceInPositionVectorUnits);	
andrew@0: 			
andrew@0: 
andrew@0: 	updateCounter++;
andrew@8: 	prior.renormalise();//not strictly necessary??
andrew@0: 
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::complexCrossUpdate(const double& timeDifferenceInPositionVectorUnits){
andrew@4: 
andrew@5: 	
andrew@17: //	printf("before cross c : posterior map is %i = %f ms time diff pos vec %f\n", posterior.getMAPestimate(), posterior.getIndexInRealTerms(prior.getMAPestimate()), timeDifferenceInPositionVectorUnits);
andrew@5: 	
andrew@0: 	int distanceMoved, newPriorIndex;
andrew@0: 	
andrew@0: 	double speedValue = relativeSpeedPosterior.offset;
andrew@0: 	
andrew@0: 	for (int i = 0;i < relativeSpeedPosterior.arraySize;i++){
andrew@0: 		
andrew@0: 	//	double speedValue = relativeSpeedPosterior.getIndexInRealTerms(i);//so for scalar 0.01, 50 -> speed value of 0.5
andrew@0: 		
andrew@0: 		//so we have moved 
andrew@0: 		distanceMoved = round(timeDifferenceInPositionVectorUnits * speedValue);//round the value
andrew@5: 	//	printf("Speed value %f time %f gives distance %i\n", speedValue, timeDifferenceInPositionVectorUnits, distanceMoved);
andrew@5: 		
andrew@0: 		if (relativeSpeedPosterior.array[i] != 0){
andrew@4: 			
andrew@0: 			double speedContribution = relativeSpeedPosterior.array[i];
andrew@0: 			
andrew@4: 		//	printf("speed [%i](val[%f]) gives %f moved %i in %f units \n", i, relativeSpeedPosterior.array[i], speedValue, distanceMoved, timeDifferenceInPositionVectorUnits);
andrew@0: 			
andrew@4: 			//1/2/12 deleted line
andrew@15: 			newPriorIndex = (int)posterior.millisToVectorUnits(posterior.offset - prior.offset) + distanceMoved;//i.e. where post[0] goes to in terms of prior at this speed
andrew@5: 			int postIndex = 0;//index of posterior that will contribute
andrew@5: 			
andrew@4: 			while (postIndex < posterior.arraySize && newPriorIndex < prior.arraySize){
andrew@4: 
andrew@4: 			//did use a for loop
andrew@4: 			//	for (postIndex = 0;postIndex < posterior.arraySize;postIndex++){
andrew@0: 				//old posterior contributing to new prior
andrew@4: 			
andrew@4: 				//would use this method
andrew@4: 				//newPriorIndex = postIndex + posterior.offset - prior.offset + distanceMoved;
andrew@0: 				
andrew@4: 				if (newPriorIndex >= 0){
andrew@0: 					prior.addToIndex(newPriorIndex, posterior.array[postIndex]*speedContribution);
andrew@4: 			//		printf("speed index %i new prior index %i post val %f speed contrib %f dist %i\n", i, newPriorIndex, posterior.array[postIndex], speedContribution, distanceMoved);
andrew@0: 				}
andrew@4: 				//but we actually do this for simplicity
andrew@4: 				newPriorIndex++;
andrew@4: 				postIndex++;
andrew@4: 			}//end for. now while
andrew@0: 			
andrew@0: 			
andrew@0: 		}//if not zero
andrew@5: 		
andrew@5: 		speedValue += relativeSpeedPosterior.scalar;//optimised line
andrew@0: 		//as we wanted:
andrew@0: 		//	double speedValue = relativeSpeedPosterior.getIndexInRealTerms(i);//so for scalar 0.01, 50 -> speed value of 0.5
andrew@5: 		
andrew@0: 	}//end speed
andrew@5: 	
andrew@5: 	
andrew@17: //	printf("after cross c : prior map is %i = %f ms\n", prior.getMAPestimate(), prior.getIndexInRealTerms(prior.getMAPestimate()));
andrew@0: }
andrew@0: 
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::translateByMaximumSpeed(const double& timeDifferenceInPositionVectorUnits){
andrew@0: 
andrew@8: 	
andrew@0: 	int distanceMoved, newPriorIndex;
andrew@8: 	double speedIndex = getSpeedEstimateIndex();
andrew@8: 	double speedValue = relativeSpeedPosterior.getIndexInRealTerms(speedIndex);
andrew@0: 	
andrew@8: //	double speedValue = relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
andrew@8: 	
andrew@0: 	//so for scalar 0.01, 50 -> speed value of 0.5
andrew@8: 	double speedContribution = relativeSpeedPosterior.array[(int)round(speedIndex)];
andrew@0: 		//so we have moved 
andrew@0: 		distanceMoved = round(timeDifferenceInPositionVectorUnits * speedValue);//round the value
andrew@8: 				
andrew@8: 	//	printf("speed [%i] gives %f moved %i in %f units \n", i, speedValue, distanceMoved, timeDifferenceInPositionVectorUnits);
andrew@0: 			
andrew@0: 			for (int postIndex = 0;postIndex < posterior.arraySize;postIndex++){
andrew@0: 				//old posterior contributing to new prior
andrew@15: 				newPriorIndex = (int)round(posterior.millisToVectorUnits(posterior.offset - prior.offset)) + postIndex  + distanceMoved;
andrew@0: 				if (newPriorIndex >= 0 && newPriorIndex < prior.arraySize){
andrew@0: 					prior.addToIndex(newPriorIndex, posterior.array[postIndex]*speedContribution);
andrew@0: 				}
andrew@0: 				
andrew@0: 			}
andrew@0: 	
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::addGaussianNoiseToSpeedPosterior(const double& std_dev){
andrew@0: 	tmpPosteriorForStorage.copyFromDynamicVector(relativeSpeedPosterior);
andrew@0: 	
andrew@0: 	for (int i = 0;i < relativeSpeedPosterior.length;i++){
andrew@0: 		tmpPosteriorForStorage.addGaussianShape(i, std_dev, relativeSpeedPosterior.array[i]);
andrew@0: 		}
andrew@0: 												
andrew@0: 	tmpPosteriorForStorage.renormalise();
andrew@0: 	
andrew@0: 	relativeSpeedPosterior.copyFromDynamicVector(tmpPosteriorForStorage);											
andrew@0: }
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::addTriangularNoiseToSpeedPosterior(const double& std_dev){
andrew@0: 	tmpPosteriorForStorage.copyFromDynamicVector(relativeSpeedPosterior);
andrew@0: 	
andrew@0: 	for (int i = 0;i < relativeSpeedPosterior.length;i++){
andrew@0: 		//adding a linear amount depending on distance
andrew@0: 		tmpPosteriorForStorage.addTriangularShape(i, std_dev*2.0, relativeSpeedPosterior.array[i]);
andrew@0: 	}
andrew@0: 	
andrew@0: 	tmpPosteriorForStorage.renormalise();
andrew@0: 	
andrew@0: 	relativeSpeedPosterior.copyFromDynamicVector(tmpPosteriorForStorage);											
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::calculateNewPriorOffset(const double& timeDifference){
andrew@0: 	
andrew@4: //	double maxSpeed = relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
andrew@4: 	
andrew@4: 	double maxSpeed = relativeSpeedPosterior.getIndexInRealTerms(getSpeedEstimateIndex());//either integrated or MAP
andrew@0: 	//	printf("Maxspeed is %f\n", maxSpeed);
andrew@0: 	
andrew@0: 	double priorMax = posterior.getMaximum();
andrew@0: 	double distanceTravelled = maxSpeed * (timeDifference / prior.scalar);
andrew@0: 	double newMaxLocation = posterior.MAPestimate + distanceTravelled;
andrew@0: 	//	printf("MAP: %i, tim df %f, distance %f, new location %f\n", posterior.MAPestimate, timeDifference, distanceTravelled, newMaxLocation);
andrew@0: 	
andrew@0: }
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::decaySpeedDistribution(double timeDifference){
andrew@0: 	
andrew@0: 	// commented for the moment
andrew@0: 	 double relativeAmount = max(1.0, timeDifference/1000.);
andrew@0: //	printf("decay %f around %i \n", timeDifference, relativeSpeedPosterior.MAPestimate);
andrew@0: 	relativeAmount *= speedDecayAmount;
andrew@0: 	relativeSpeedPosterior.renormalise();
andrew@0: 	relativeSpeedPosterior.addGaussianShape(relativeSpeedPosterior.MAPestimate, speedDecayWidth, relativeAmount);
andrew@0: 	
andrew@0: 	relativeSpeedPosterior.renormalise();
andrew@0: 	double newMax = relativeSpeedPosterior.getMaximum();
andrew@0: 	
andrew@0: 	//old code
andrew@0: //	relativeSpeedPosterior.addGaussianShape(relativeSpeedPosterior.MAPestimate, speedDecayWidth, 10);
andrew@0: 	//relativeSpeedPosterior.addConstant(1);
andrew@0: 	
andrew@0: 	/*
andrew@0: 	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
andrew@0: 	relativeSpeedLikelihood.zero();
andrew@0: 	relativeSpeedLikelihood.addConstant(0.2);
andrew@0: 	relativeSpeedLikelihood.addGaussianShape(relativeSpeedPosterior.maximumValue, speedDecayWidth, relativeAmount);
andrew@0: 	relativeSpeedPosterior.doProduct(relativeSpeedPrior, relativeSpeedLikelihood);
andrew@0: 	relativeSpeedPosterior.renormalise();
andrew@0: 	 */
andrew@0: 	
andrew@0: 
andrew@0: 	
andrew@0: }
andrew@0: 
andrew@0: void BayesianArrayStructure::setLikelihoodToConstant(){
andrew@0: 	//set new likelihood
andrew@0: 	relativeSpeedLikelihood.zero();
andrew@0: 	relativeSpeedLikelihood.addConstant(speedLikelihoodNoise);
andrew@0: }
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::updateTempoLikelihood(const double& speedRatio, const double& matchFactor){
andrew@0: 	
andrew@0: 	//speedratio is speed of played relative to the recording
andrew@0: 	
andrew@0: 	double index = relativeSpeedLikelihood.getRealTermsAsIndex(speedRatio);
andrew@0: 	//	printf("index of likelihood would be %f for ratio %f\n", index, speedRatio);
andrew@0: 	if (index >= 0 && index < relativeSpeedPrior.length){	
andrew@0: 		relativeSpeedLikelihood.addGaussianShape(index , relativeSpeedLikelihoodStdDev, matchFactor);
andrew@0: 	}
andrew@0: }
andrew@0: 	
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::updateTempoDistribution(){
andrew@0: 
andrew@0: 	//copy posterior to prior
andrew@0: 	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
andrew@0: 
andrew@0: 	//update
andrew@0: 	relativeSpeedPosterior.doProduct(relativeSpeedPrior, relativeSpeedLikelihood);
andrew@0: 
andrew@0: 	//normalise
andrew@0: 	relativeSpeedPosterior.renormalise();
andrew@0: 		
andrew@0: 	relativeSpeedPosterior.getMaximum();	
andrew@0: 	
andrew@0: 	//relativeSpeedPosterior.updateIntegratedEstimate(); - could be swayed when off to side 
andrew@0: 	//so now limited to where there is room sround the MAP estimate
andrew@0: 	relativeSpeedPosterior.updateLimitedIntegratedEstimate();
andrew@0: 	
andrew@0: 	speedEstimate = relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
andrew@0: }
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::calculateTempoUpdate(){
andrew@0: 	//copy posterior to prior
andrew@0: 	relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
andrew@0: 	
andrew@0: 	//update
andrew@0: 	relativeSpeedPosterior.doProduct(relativeSpeedPrior, relativeSpeedLikelihood);
andrew@0: 	
andrew@0: 	//normalise
andrew@0: 	relativeSpeedPosterior.renormalise();
andrew@0: 	
andrew@0: 	relativeSpeedPosterior.getMaximum();	
andrew@0: 	
andrew@0: }
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::drawArrays(){
andrew@0: 	
andrew@0: 	//bayesArray.drawFloatArray(&bayesArray.prior[0], 0, 200);
andrew@0: 	//bayesArray.drawFloatArray(&bayesArray.prior[0], 0, 200);
andrew@0: 	
andrew@0: 	int displaySize = prior.arraySize;
andrew@0: 	ofSetColor(0,0,255);
andrew@0: 	prior.drawVector(0, displaySize);
andrew@0: 	ofSetColor(0,255,0);
andrew@0: 	likelihood.drawVector(0, displaySize);
andrew@0: 	ofSetColor(255,0,255);
andrew@0: 	posterior.drawVector(0, displaySize);
andrew@0: 	
andrew@0: 	
andrew@0: 	
andrew@0: }
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::drawTempoArrays(){
andrew@0: 	ofSetColor(0,0,255);
andrew@0: //	relativeSpeedPrior.drawVector(0, relativeSpeedPrior.arraySize);
andrew@0: 	
andrew@0: 	ofSetColor(0,150,255);
andrew@0: 	relativeSpeedLikelihood.drawVector(0, relativeSpeedLikelihood.arraySize);
andrew@0: 	
andrew@0: //	relativeSpeedLikelihood.drawConstrainedVector(0, 199, 0, 1000);// relativeSpeedLikelihood.arraySize);	
andrew@0: 	ofSetColor(255,0,0);
andrew@0: 	relativeSpeedPosterior.drawVector(0, relativeSpeedPosterior.arraySize);
andrew@0: 	
andrew@0: //	ofSetColor(0,0,255);
andrew@0: //	tmpPosteriorForStorage.drawVector(0, tmpPosteriorForStorage.arraySize);
andrew@0: 	
andrew@0: 	ofSetColor(255,255, 255);
andrew@0: 	ofLine(screenWidth/2, 0, screenWidth/2, ofGetHeight());//middle of screen
andrew@0: 	
andrew@0: 	ofSetColor(25, 0, 250);
andrew@0: 	double fractionOfScreen = ((double)relativeSpeedPosterior.integratedEstimate / relativeSpeedPosterior.length);
andrew@0: 	ofLine(screenWidth * fractionOfScreen, 0, screenWidth * fractionOfScreen, ofGetHeight());
andrew@0: 	
andrew@0: 	ofSetColor(255, 0, 20);
andrew@0: 	fractionOfScreen = ((double)relativeSpeedPosterior.MAPestimate / relativeSpeedPosterior.length);
andrew@0: 	ofLine(screenWidth * fractionOfScreen, 0, screenWidth * fractionOfScreen, ofGetHeight());
andrew@0: 	 
andrew@0: 	
andrew@0: }
andrew@0: 
andrew@0: 
andrew@0: void BayesianArrayStructure::drawArraysRelativeToTimeframe(const double& startTimeMillis, const double& endTimeMillis){
andrew@0: 
andrew@0: 	screenWidth = ofGetWidth();
andrew@0: 	
andrew@0: 	int startArrayIndex = 0;
andrew@0: 	
andrew@0: 	if (prior.getIndexInRealTerms(prior.arraySize-1) > startTimeMillis){
andrew@0: 		//i.e. the array is on the page
andrew@0: 	
andrew@0: 	while (prior.getIndexInRealTerms(startArrayIndex) < startTimeMillis){
andrew@0: 		startArrayIndex++;
andrew@0: 	}
andrew@0: 	int endArrayIndex = prior.arraySize-1;
andrew@0: 	//could find constraints here
andrew@0: 	if (prior.getIndexInRealTerms(prior.arraySize-1) > endTimeMillis)
andrew@0: 		endArrayIndex = (floor)((endTimeMillis - prior.offset)/prior.scalar);
andrew@0: 	
andrew@0: 	//so we need to figure where start and end array are on screen
andrew@0: 	int startScreenPosition, endScreenPosition;
andrew@0: 	double screenWidthMillis = endTimeMillis - startTimeMillis;
andrew@0: 		
andrew@0: 	startScreenPosition = (prior.getIndexInRealTerms(startArrayIndex) - startTimeMillis)*screenWidth/screenWidthMillis;
andrew@0: 	endScreenPosition = (double)(prior.getIndexInRealTerms(endArrayIndex) - startTimeMillis)*screenWidth/screenWidthMillis;
andrew@0: 		
andrew@0: 	ofSetColor(0,0,100);
andrew@0: 	string relativeString = " offset "+ofToString(prior.offset, 1);//starttimes("+ofToString(startTimeMillis)+", "+ofToString(endTimeMillis);
andrew@0: 	relativeString += ": index "+ofToString(startArrayIndex)+" , "+ofToString(endArrayIndex)+" [";
andrew@0: //	relativeString += ofToString(prior.getIndexInRealTerms(endArrayIndex), 3)+"] (sc-width:"+ofToString(screenWidthMillis, 1)+")  ";
andrew@0: 	relativeString += " mapped to screen "+ofToString(startScreenPosition)+" , "+ofToString(endScreenPosition);
andrew@0: //	ofDrawBitmapString(relativeString, 100, 180);
andrew@0: 	
andrew@0: 		
andrew@0: 		
andrew@0: 	ofSetColor(100,100,100);//255, 255, 0);
andrew@0: 	likelihood.drawConstrainedVector(startArrayIndex, endArrayIndex, startScreenPosition, endScreenPosition);
andrew@0: 
andrew@0: //	ofSetColor(0,0,200);
andrew@0: 	ofSetColor(170,170,170);//00,200);
andrew@0: 	prior.drawConstrainedVector(startArrayIndex, endArrayIndex, startScreenPosition, endScreenPosition);
andrew@0: 		
andrew@0: 		ofSetColor(0,0,150);	
andrew@0: //	ofSetColor(200, 0, 0);
andrew@0: 	posterior.drawConstrainedVector(startArrayIndex, endArrayIndex, startScreenPosition, endScreenPosition);
andrew@0: 		
andrew@0: 		
andrew@0: //	ofSetColor(0, 200, 255);
andrew@0: //	acceleration.drawConstrainedVector(startArrayIndex, endArrayIndex, startScreenPosition, endScreenPosition);
andrew@0: 		
andrew@0: 		
andrew@0: 	}
andrew@0: 
andrew@0: }
andrew@0: 
andrew@0: 
andrew@6: void BayesianArrayStructure::printPostOffset(){
andrew@6: 	double tmp = posterior.getMAPestimate();
andrew@6: 	printf(" MAP index %i post offset %f == %f ms\n", posterior.MAPestimate, posterior.offset, posterior.getIndexInRealTerms(posterior.MAPestimate));
andrew@6: }
andrew@6: 
andrew@10: //PROJECT PRIOR CODE
andrew@11: 
andrew@11: void BayesianArrayStructure::projectDistribution(const double& newEventTime, const double& newAlignmentPosition, DynamicVector& projectedPrior){
andrew@10: 	
andrew@10: 	projectedPrior.copyFromDynamicVector(posterior);
andrew@10: 
andrew@10: 	double timeDifference = newEventTime - lastEventTime; 
andrew@10: 	
andrew@10: //	if (timeDifference > 50 && updatingSpeedDistribution){
andrew@10: //		addGaussianNoiseToSpeedPosterior(timeDifference * 10.0 / 100.);
andrew@10: //	}
andrew@10: 
andrew@10: 	
andrew@11: //	updateBestEstimate(timeDifference);
andrew@11: //	lastBestEstimateUpdateTime = newEventTime;//getTimeNow(timePlayed);
andrew@10: 	
andrew@10: 	//set TARGETS - commented tenmporarily
andrew@11: 
andrew@11: 	//setNewDistributionOffsets(max(0., bestEstimate  - (prior.scalar*prior.arraySize/2)));
andrew@11: 	projectedPrior.offset = posterior.offset;//max(0., newAlignmentPosition - (projectedPrior.scalar*projectedPrior.arraySize/2));
andrew@11: 							
andrew@11: //	int timeDifference = newEventTime - lastEventTime;
andrew@11: 								
andrew@11: 		double timeDifferenceInPositionVectorUnits = timeDifference / projectedPrior.scalar;
andrew@11: 								
andrew@11: 	//	printf("CROSS UPDATE time diff %f ms is %f units; ", timeDifference, timeDifferenceInPositionVectorUnits);
andrew@11: 		projectedPrior.zero();//kill prior
andrew@11: 								
andrew@11: 								//	calculateNewPriorOffset(timeDifference);//dioesnt do anything
andrew@11: 								
andrew@11: 								//	printf("new prior offset %f and post offset %f\n", prior.offset, posterior.offset);
andrew@11: 								
andrew@11: 				if (timeDifferenceInPositionVectorUnits > crossUpdateTimeThreshold)
andrew@11: 					complexCrossUpdateProjection(projectedPrior, timeDifferenceInPositionVectorUnits);
andrew@11: 				else
andrew@11: 					translatePosteriorByMaximumSpeed(projectedPrior, timeDifferenceInPositionVectorUnits);	
andrew@11: 								
andrew@11: 								
andrew@11: 		//			updateCounter++;
andrew@11: 		projectedPrior.renormalise();//not strictly necessary??
andrew@11: 								
andrew@11: 								
andrew@10: 	//i.e. using the same offset as prior
andrew@11: //	posterior.offset = prior.offset;// 
andrew@10: 	
andrew@10: 	//	float tmpPrior = max(0., bestEstimate - (prior.scalar*prior.arraySize/2));//	prior.offset = max(0., bestEstimate - (prior.scalar*prior.arraySize/2));
andrew@10: 	//	printf("Using prior offset of %f not %f\n", tmpPrior, prior.offset);
andrew@10: 	
andrew@11: //	lastEventTime = newEventTime;//lastEventTime = ofGetElapsedTimeMillis();
andrew@10: 	
andrew@10: }
andrew@10: 
andrew@10: 
andrew@10: 
andrew@11: void BayesianArrayStructure::complexCrossUpdateProjection(DynamicVector& projectedPrior, const double& timeDifferenceInPositionVectorUnits){
andrew@10: 	
andrew@10: 	int distanceMoved, newPriorIndex;
andrew@11: 
andrew@10: 	double speedValue = relativeSpeedPosterior.offset;
andrew@10: 	
andrew@10: 	for (int i = 0;i < relativeSpeedPosterior.arraySize;i++){
andrew@10: 		
andrew@10: 		distanceMoved = round(timeDifferenceInPositionVectorUnits * speedValue);//round the value
andrew@10: 		
andrew@10: 		if (relativeSpeedPosterior.array[i] != 0){
andrew@10: 			
andrew@10: 			double speedContribution = relativeSpeedPosterior.array[i];
andrew@10: 			
andrew@11: 			newPriorIndex = posterior.offset - projectedPrior.offset + distanceMoved;//i.e. where post[0] goes to in terms of prior at this speed
andrew@10: 			int postIndex = 0;//index of posterior that will contribute
andrew@10: 			
andrew@11: 			while (postIndex < posterior.arraySize && newPriorIndex < projectedPrior.arraySize){
andrew@11: 
andrew@10: 				//would use this method
andrew@10: 				//newPriorIndex = postIndex + posterior.offset - prior.offset + distanceMoved;
andrew@10: 				
andrew@10: 				if (newPriorIndex >= 0){
andrew@11: 					projectedPrior.addToIndex(newPriorIndex, posterior.array[postIndex]*speedContribution);
andrew@10: 					//		printf("speed index %i new prior index %i post val %f speed contrib %f dist %i\n", i, newPriorIndex, posterior.array[postIndex], speedContribution, distanceMoved);
andrew@10: 				}
andrew@10: 				//but we actually do this for simplicity
andrew@10: 				newPriorIndex++;
andrew@10: 				postIndex++;
andrew@10: 			}//end for. now while
andrew@10: 			
andrew@10: 			
andrew@10: 		}//if not zero
andrew@10: 		
andrew@10: 		speedValue += relativeSpeedPosterior.scalar;//optimised line
andrew@10: 		//as we wanted:
andrew@10: 		//	double speedValue = relativeSpeedPosterior.getIndexInRealTerms(i);//so for scalar 0.01, 50 -> speed value of 0.5
andrew@10: 		
andrew@10: 	}//end speed
andrew@11: 
andrew@10: }
andrew@10: 
andrew@10: 
andrew@10: 
andrew@11: void BayesianArrayStructure::translatePosteriorByMaximumSpeed(DynamicVector& translatedPosterior, const double& timeDifferenceInPositionVectorUnits){
andrew@10: 	
andrew@10: 	
andrew@10: 	int distanceMoved, newPriorIndex;
andrew@10: 	double speedIndex = getSpeedEstimateIndex();
andrew@10: 	double speedValue = relativeSpeedPosterior.getIndexInRealTerms(speedIndex);
andrew@10: 	
andrew@10: 	//	double speedValue = relativeSpeedPosterior.getIndexInRealTerms(relativeSpeedPosterior.integratedEstimate);
andrew@10: 	
andrew@10: 	//so for scalar 0.01, 50 -> speed value of 0.5
andrew@10: 	double speedContribution = relativeSpeedPosterior.array[(int)round(speedIndex)];
andrew@10: 	//so we have moved 
andrew@10: 	distanceMoved = round(timeDifferenceInPositionVectorUnits * speedValue);//round the value
andrew@10: 	
andrew@10: 	//	printf("speed [%i] gives %f moved %i in %f units \n", i, speedValue, distanceMoved, timeDifferenceInPositionVectorUnits);
andrew@10: 	
andrew@10: 	for (int postIndex = 0;postIndex < posterior.arraySize;postIndex++){
andrew@10: 		//old posterior contributing to new prior
andrew@10: 		newPriorIndex = postIndex + posterior.offset - prior.offset + distanceMoved;
andrew@10: 		if (newPriorIndex >= 0 && newPriorIndex < prior.arraySize){
andrew@11: 			translatedPosterior.addToIndex(newPriorIndex, posterior.array[postIndex]*speedContribution);
andrew@10: 		}
andrew@10: 		
andrew@10: 	}
andrew@10: 	
andrew@10: }
andrew@11: 
andrew@10: 
andrew@10: //END PROJECT PRIOR CODE
andrew@10: 
andrew@10: 
andrew@11: 
andrew@0: /*
andrew@0:  
andrew@0:  void BayesianArrayStructure::updateTempoDistribution(const double& speedRatio, const double& matchFactor){
andrew@0:  //speedratio is speed of played relative to the recording
andrew@0:  
andrew@0:  double index = relativeSpeedLikelihood.getRealTermsAsIndex(speedRatio);
andrew@0:  //	printf("\nindex of likelihood would be %f\n", index);
andrew@0:  if (index >= 0 && index < relativeSpeedPrior.length){
andrew@0:  //then we can do update
andrew@0:  
andrew@0:  //set new likelihood
andrew@0:  relativeSpeedLikelihood.zero();
andrew@0:  relativeSpeedLikelihood.addConstant(speedLikelihoodNoise);
andrew@0:  
andrew@0:  relativeSpeedLikelihood.addGaussianShape(index , 5, 0.5*matchFactor);
andrew@0:  
andrew@0:  
andrew@0:  //copy posterior to prior
andrew@0:  relativeSpeedPrior.copyFromDynamicVector(relativeSpeedPosterior);
andrew@0:  
andrew@0:  //update
andrew@0:  relativeSpeedPosterior.doProduct(relativeSpeedPrior, relativeSpeedLikelihood);
andrew@0:  
andrew@0:  //normalise
andrew@0:  relativeSpeedPosterior.renormalise();
andrew@0:  
andrew@0:  relativeSpeedPosterior.getMaximum();	
andrew@0:  }//end if within range
andrew@0:  
andrew@0:  
andrew@0:  }
andrew@0:  
andrew@0:  */