#!/usr/bin/env python

import sys

sys.path.insert(0, "/afs/ms.mff.cuni.cz/u/z/zwin6678/BIG/numarray-1.4.1/build/lib.linux-i686-2.4/")
sys.path.insert(0, "/afs/ms.mff.cuni.cz/u/z/zwin6678/BIG/pygame-1.7.1release/build/lib.linux-i686-2.4/")

import os, math, pygame, numarray
from numarray import array,Float64,reshape,arange,ones,concatenate
from numarray import random_array
from pygame.locals import *

sample_color = (255,0,0)
sample_size = 2
tile_size = 0.05

def draw(surface, samples):
	for s in samples:
		surface.fill(sample_color, (to_px(s[0]), to_px(s[1]), sample_size, sample_size))

def to_px(x): return int(x*2000)

def main():
	try: 
		data = numarray.fromfile("mcl.log", Float64)
		data.setshape((-1,300,4))
		#print data
	except IOError:
		# generate testing data
		num = 300
		mean = (.05,.05,0)
		var = (.01, .01, math.radians(15))
		pose = random_array.normal(mean, var, (1,num,3))
		weight = ones((1, num,1))
		data = concatenate((pose,weight), 2)
	
	# load pygame
	pygame.init()
	size = 800, 600
	os.environ['SDL_VIDEO_CENTERED'] = '1'
	#screen = pygame.display.set_mode(size, NOFRAME, 0)
	screen = pygame.display.set_mode(size)

	# create backgroud
	background = pygame.Surface(screen.get_size())
	background.fill((35, 35, 35))
	for x in range(0,size[0],to_px(tile_size)):
		for y in range(0,size[1],to_px(tile_size)):
			if x % to_px(tile_size*2) == y % to_px(tile_size*2):
				background.fill((235, 235, 235), Rect(x,y,to_px(tile_size),to_px(tile_size)))

	# create foreground
	foreground = pygame.Surface(screen.get_size())
	foreground.set_colorkey((0,0,0))
	index = 0
	draw(foreground, data[index])

	# display everything
	screen.blit(background, (0, 0))
	screen.blit(foreground, (0, 0))
	pygame.display.flip()

	pygame.event.set_blocked(MOUSEMOTION) # keep our queue cleaner
	pygame.key.set_repeat ( 200, 20 )	

	while 1:
		event = pygame.event.wait()
		if event.type == QUIT: return
		if event.type == KEYDOWN:
			if event.key in (K_ESCAPE,K_q): return
			if event.key == K_RIGHT and index < data.getshape()[0]-1: 
				index = index + 1
			if event.key == K_LEFT and index > 0: 
				index = index - 1
			if event.key == K_PAGEDOWN:
				if index < data.getshape()[0]-10:
					index = index + 10
				else:
					index = data.getshape()[0] - 1
			if event.key == K_PAGEUP:
				if index >= 10:
					index = index - 10
				else:
					index = 0
			foreground.fill((0,0,0))
			draw(foreground, data[index])
			screen.blit(background, (0, 0))
			screen.blit(foreground, (0, 0))
			pygame.display.flip()


if __name__ == "__main__": 
	main()