import serial
import time
import math

s = serial.Serial("/dev/ttyACM0", 115200)
s.write('\r\nahrs_dump\r\n')

import pygame
pygame.init()
screen = pygame.display.set_mode((200, 200))

def draw(pos):
    screen.fill((0,0,0))
    pygame.draw.line(screen, pygame.Color("red"), (100, 0), (100,200))
    pygame.draw.line(screen, pygame.Color("red"), (0,100), (200,100))
    pygame.draw.circle(screen, pygame.Color("red"), (100, 100), 5)
    pygame.draw.circle(screen, pygame.Color("blue"), pos, 5)
    pygame.display.flip()

for line in s:
    print line
    
    if not line.strip() or 'ch>' in line or '#' in line:
        continue
    
    parts = [int(p) for p in line.split() if p.strip()]
    y = int(-parts[1] / 10 + 100)
    x = int(parts[2] / 10 + 100)
    
    draw((x,y))
    
    event = pygame.event.poll()
    if event.type == pygame.QUIT:
        break