from robot import Robot
from parser import aip_parser

from subprocess import Popen, PIPE
import fcntl
import os

# Pygame
from math import degrees, cos, sin
import sys, pygame
from pygame.locals import *
pygame.init()

size = width, height = 1000, 300
black = 0, 0, 0

screen = pygame.display.set_mode(size)

# le robot
r = Robot(1/60)

# on lance le programme arduino
ard_process = Popen(["build/app"], stdin=PIPE, stdout=PIPE);

# stdout du programme arduino en non bloquant pour pouvoir arreter la lecture si il y as plus rien à lire
fd = ard_process.stdout.fileno()
fl = fcntl.fcntl(fd, fcntl.F_GETFL)
fcntl.fcntl(fd, fcntl.F_SETFL, fl | os.O_NONBLOCK)

# le parser wrapper arduino->simulateur
parser = aip_parser(ard_process.stdin, ard_process.stdout, r);

while True:
    for event in pygame.event.get():
        if event.type == pygame.QUIT: sys.exit()
        if event.type == pygame.KEYDOWN:
            if event.key == K_q:
                sys.exit()
    
    # On envoie le états des pins
    parser.send_pin_state()
    
    # On demarre un tour de simulation
    ard_process.stdin.write(b'@')
    
    # tant qu'il y as qqu chose sur le bus on le parse
    while(parser.poll()):
        pass
    
    # on met à jour la simulation    
    r.update()
    
    # On dessine le bordel
    screen.fill(black)
    robotShape = pygame.Rect(20,15, 10,20);
    rsurf = pygame.Surface((50,50)).convert_alpha();
    # robot
    pygame.draw.rect(rsurf, (255,255,255), robotShape);
    # heading
    pygame.draw.line(rsurf, (255,255,255), (25,25), (25,50))
    # dir2light
    pygame.draw.line(rsurf, (255,255,255), (25,25), (25+25*sin(r.angle2spot),25+25*cos(r.angle2spot)))
    # light
    pygame.draw.polygon(screen, (255,255,0), ([100*a for a in r.light_pos],[100*(a+b) for a,b in zip(r.light_pos,(-1,1))],[100*(a+b) for a,b in zip(r.light_pos,(-1,-1))]))
    rsurf_r = pygame.transform.rotate(rsurf, degrees(r.rot));
    center = rsurf_r.get_rect().center
    screen.blit(rsurf_r, (r.pos[0]*100-center[0],r.pos[1]*100-center[1]))
    pygame.display.flip()