import time
import math
import sys

import pygame

import jog2d


def simu(ns, shared_regs, change_regs):
    print "simu started"
    twopi = 2.0 * math.pi
    pygame.init()
    xmax = 1000
    ymax = 600
    size = (xmax, ymax)
    screen = pygame.display.set_mode(size)

    tpict = []
    tpictply = []
    pict1 = jog2d.Picture(800, 50, 950, 200)
    tpict.append(pict1)
    tpictply.append(pict1.ply)

    tobs = []
    tobsply = []

    maze = False
    if maze:
        obs1 = jog2d.Obstacle(240, 20, 260, 450)
        tobs.append(obs1)
        tobsply.append(obs1.ply)

        obs2 = jog2d.Obstacle(490, 150, 510, 580)
        tobs.append(obs2)
        tobsply.append(obs2.ply)

        obs3 = jog2d.Obstacle(740, 20, 760, 450)
        tobs.append(obs3)
        tobsply.append(obs3.ply)

    obs_l = jog2d.Obstacle(0, 0, 10, ymax)
    tobs.append(obs_l)
    tobsply.append(obs_l.ply)

    obs_r = jog2d.Obstacle(xmax - 10, 0, xmax, ymax)
    tobs.append(obs_r)
    tobsply.append(obs_r.ply)

    obs_t = jog2d.Obstacle(11, 0, xmax - 11, 10)
    tobs.append(obs_t)
    tobsply.append(obs_t.ply)

    obs_b = jog2d.Obstacle(11, ymax - 10, xmax - 11, ymax)
    tobs.append(obs_b)
    tobsply.append(obs_b.ply)

    jog = jog2d.Jog2d()
    leds = [0, 0, 0, 0]  # leds off

    # init location
    xjog = 500
    yjog = 250
    xjogi = int(round(xjog))
    yjogi = int(round(yjog))
    jog.translate(xjogi, yjogi)

    # init heading
    hdjog0 = jog.head
    hdjog = 110.0
    angrot = (hdjog - hdjog0)
    jog.rotate(angrot)

    # init sonars
    sonar_hits = jog.obstacle(tobsply)
    sonar_set = []
    sonar_time = []
    for ison in range(5):