from robot import Robot
from parser import aip_parser
from laby import Laby

from subprocess import Popen, PIPE
import fcntl
import os
from time import sleep

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

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

screen = pygame.display.set_mode(size)

# 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)

laby = Laby((1.5,0))

# le robot
r = Robot(laby)
r.light_pos = (laby.pos[0]+laby.L8+laby.L3/2,laby.L17+laby.L4/2)
r.pos = (0.1, laby.L17+laby.L4/2)

# 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'@')
    ard_process.stdin.flush()

    # 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)
    laby.draw(screen)
    robotShape = pygame.Rect(20,15, 10,20);
    rsurf = pygame.Surface((50,50)).convert_alpha();
    rsurf.fill((255,255,255,0))
    # 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,(-0.1,0.1))],[100*(a+b) for a,b in zip(r.light_pos,(-0.1,-0.1))]))
    # ultrasonic sensors
    for usrs in r.ultr_sense_rays:
        endl = (r.pos[0]+r.ultr_sense_max*sin(r.rot+usrs+r.ultr_sense_scatter), r.pos[1]+r.ultr_sense_max*cos(r.rot+usrs+r.ultr_sense_scatter))
        endr = (r.pos[0]+r.ultr_sense_max*sin(r.rot+usrs-r.ultr_sense_scatter), r.pos[1]+r.ultr_sense_max*cos(r.rot+usrs-r.ultr_sense_scatter))
        pygame.draw.line(screen, (255,0,0), [p*100 for p in r.pos], [p*100 for p in endl])
        pygame.draw.line(screen, (255,0,0), [p*100 for p in r.pos], [p*100 for p in endr])
        pygame.draw.line(screen, (255,0,0), [p*100 for p in endl], [p*100 for p in endr])
    for c in r.ci:
        if c:
            pygame.draw.circle(screen, (255,0,0), [p*100 for p in c], 3)
    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()