from mpmath import *
import filter
from math import pi
from robotModel import motionSample, observationSample

aList=[[1,0,0],[0, 0, pi/2],[1, 0, 0],[0,1,0],[0,0,pi],[1,0,0],[0,0,pi/2],[1,0,0],[0, 0, pi/2],[1, 0, 0],[0,0,pi/2],[1,0,0],[0,0,pi/2],[1,0,0],[0,0,pi/2],[1,0,0],[0, 0, pi/2],[1, 0, 0],[0,0,pi/2],[1,0,0],[0,0,pi/2],[1,0,0],[0,0,pi/2],[1,0,0],[0, 0, pi/2],[1, 0, 0],[0,0,pi/2],[1,0,0],[0,0,pi/2],[1,0,0],[0,0,pi/2]]

# filter.setInitialStateGaussian(matrix([0,0,0]), eye(3)*0.001)
filter.setInitialStateUniform()

print "Initial filter mean and covariance:"
print filter.getMean()
print filter.getSigma()

step = 0

state = matrix([0,0,0])

aoList = []

for a in aList:
	
	print "True State:", state.T
	
	am = matrix(a)
	
	print "A:", am.T

	# sample a 'true' location for the robot given the selected action
	state = motionSample(state, am)
	# sample an observation given that location
	obs = observationSample(state)

	aoList = aoList + [[am, obs]]

	# Now use that action and observation to track the robot
	filter.motionUpdate(am)
	
	print "mu:",filter.getMean().T
	print "Sig:",filter.getSigma()

	print "O:", obs.T
	
	filter.observationUpdate(obs)
	
	print "mu:",filter.getMean().T
	print "Sig:",filter.getSigma()
	
	#print "Chol",cholesky(filter.getSigma())
	
	step = step + 1

# output an aoList ready for main.py
print "aoList: ", aoList