from controller import Supervisor
import numpy as np
import math
import shapely.geometry
import shapely.affinity
import netstruct
import time
import itertools as IT
from collections import defaultdict
super = Supervisor()

robot = super.getFromDef('ROBOT2')
translationField = robot.getField('translation')
rotationField = robot.getField('rotation')
emitter = super.getEmitter("emitter") 
receiver = super.getReceiver("receiver")
receiver.enable(100)
timestep = 100	
#####################General global variables#######################
j = 0
i = 0
k = 0
tk = 0
ttime = 0
#####################Variables for trajectory execution#############
waypoints = [[-100,37], [-115,32], [-122,27], [-128,0], [-127,-100],[-125,-110], [-115,-122], [-104,-124], [14,-124], [28,-118], [35,-110], [37,-102], [37,-56], [32,-53], [-10,-53], [-32,-45], [-44,-33],
			[-53,-6],[-53,33]]
tx = 4
tz = 37
tphi = 1.5 * np.pi
x = [0] * 50
z = [0] * 50
phi = [0] * 50	
v = 12
nx = 0
nz = 0
x[0] = 4
z[0] = 37
phi[0] = 1.5 * np.pi

####################Car parameters#####################################
car_id = 2
mylane = 1
hislane = 1
w = 1
carl = 5
carw = 2
car_corners = [[0,0],[0,0],[0,0],[0,0]]
amax = 5.4
dmax = 4.6
dmin = 2.94
rho = 1
vr = 10
vf = 12

###################Algorithm specific variables##########################
global_counter = 0
counter = 0
XC = np.arange(130-float(w)/2,-150,-w)
ZC = np.arange(110-float(w)/2,-140,-w)
zones = [None] * 1500
disadv = {}
show_animation = False
tempdict = {}
mydict = {}
outerdict_zones = {}
outerdict_disadv = {}

ox = [5, 4, 3, 2, 1, 0, -1, -2, -3, -4, -5, -6, -7, -8, -9, -10, -11, -12, -13, -14, -15, -16, -17, -18, -19, -20, -21, -22, -23, -24, -25, -26, -27, -28, -29, -30, -31, -32, -33, -34, -35]
oz = [34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5, 34.5]


A = [[37.5,-35],[37.5,-32],[37.5,-29],[37.5,-26],[37.5,-23],[37.5,-20],[37.5,-17],[37.5,-14],[37.5,-11],[37.5,-8],[37.5,-5],[37.5,-2],
	[38.0,4.5],[38.0,5.7],[37.9,6.8],[37.8,8.0],[37.7,9.2],[37.5,10.3],[37.3,11.5],[37.0,12.6],[36.7,13.7],[36.4,14.9],[36.0,16.0],[35.6,17.0],[35.1,18.1],[34.6,19.2],[34.1,20.2],[33.5,21.2],[32.9,22.3],[32.3,23.2],[31.6,24.2],[30.9,25.1],[30.2,26.0],[29.4,26.9],[28.6,27.8],[27.8,28.6],[26.9,29.4],[26.0,30.2],[25.1,30.9],[24.2,31.6],[23.2,32.3],[22.3,32.9],[21.3,33.5],[20.2,34.1],[19.2,34.6],[18.1,35.1],[17.0,35.6],[16.0,36.0],[14.9,36.4],[13.7,36.7],[12.6,37.0],[11.5,37.3],[10.3,37.5],[9.2,37.7],[8.0,37.8],[6.8,37.9],[5.7,38.0],[4.5,38.0],
	[5,37],[2,37],[-1,37],[-4,37],[-7,37],[-10,37],[-13,37],[-16,37],[-19,37],[-22,37],[-25,37],[-28,37],[-31,37],[-34,37]]


B = [[-35,34],[-36,34],[-37,34],[-38,34],[-39,34],[-40,34],[-41,34],[-42,34],[-43,34],[-44,34],[-45,34],[-46,34],[-47,34],[-48,34],[-49,34],[-50,34],[-51,34],[-52,34],[-53,34],[-54,34],[-55,34],#20
	[-35,35],[-36,35],[-37,35],[-38,35],[-39,35],[-40,35],[-41,35],[-42,35],[-43,35],[-44,35],[-45,35],[-46,35],[-47,35],[-48,35],[-49,35],[-50,35],[-51,35],[-52,35],[-53,35],[-54,35],[-55,35],#41
	[-35,36],[-36,36],[-37,36],[-38,36],[-39,36],[-40,36],[-41,36],[-42,36],[-43,36],[-44,36],[-45,36],[-46,36],[-47,36],[-48,36],[-49,36],[-50,36],[-51,36],[-52,36],[-53,36],[-54,36],[-55,36],#62
	[-35,37],[-36,37],[-37,37],[-38,37],[-39,37],[-40,37],[-41,37],[-42,37],[-43,37],[-44,37],[-45,37],[-46,37],[-47,37],[-48,37],[-49,37],[-50,37],[-51,37],[-52,37],[-53,37],[-54,37],[-55,37],#83
	[-35,38],[-36,38],[-37,38],[-38,38],[-39,38],[-40,38],[-41,38],[-42,38],[-43,38],[-44,38],[-45,38],[-46,38],[-47,38],[-48,38],[-49,38],[-50,38],[-51,38],[-52,38],[-53,38],[-54,38],[-55,38],#104
	[-35,39],[-36,39],[-37,39],[-38,39],[-39,39],[-40,39],[-41,39],[-42,39],[-43,39],[-44,39],[-45,39],[-46,39],[-47,39],[-48,39],[-49,39],[-50,39],[-51,39],[-52,39],[-53,39],[-54,39],[-55,39],#125
	[-35,40],[-36,40],[-37,40],[-38,40],[-39,40],[-40,40],[-41,40],[-42,40],[-43,40],[-44,40],[-45,40],[-46,40],[-47,40],[-48,40],[-49,40],[-50,40],[-51,40],[-52,40],[-53,40],[-54,40],[-55,40],#146
	[-35,41],[-36,41],[-37,41],[-38,41],[-39,41],[-40,41],[-41,41],[-42,41],[-43,41],[-44,41],[-45,41],[-46,41],[-47,41],[-48,41],[-49,41],[-50,41],[-51,41],[-52,41],[-53,41],[-54,41],[-55,41],#167
	[-35,42],[-36,42],[-37,42],[-38,42],[-39,42],[-40,42],[-41,42],[-42,42],[-43,42],[-44,42],[-45,42],[-46,42],[-47,42],[-48,42],[-49,42],[-50,42],[-51,42],[-52,42],[-53,42],[-54,42],[-55,42],#188
	[-35,43],[-36,43],[-37,43],[-38,43],[-39,43],[-40,43],[-41,43],[-42,43],[-43,43],[-44,43],[-45,43],[-46,43],[-47,43],[-48,43],[-49,43],[-50,43],[-51,43],[-52,43],[-53,43],[-54,43],[-55,43],#209
	[-35,44],[-36,44],[-37,44],[-38,44],[-39,44],[-40,44],[-41,44],[-42,44],[-43,44],[-44,44],[-45,44],[-46,44],[-47,44],[-48,44],[-49,44],[-50,44],[-51,44],[-52,44],[-53,44],[-54,44],[-55,44],#230
	[-35,45],[-36,45],[-37,45],[-38,45],[-39,45],[-40,45],[-41,45],[-42,45],[-43,45],[-44,45],[-45,45],[-46,45],[-47,45],[-48,45],[-49,45],[-50,45],[-51,45],[-52,45],[-53,45],[-54,45],[-55,45],#251
	[-35,46],[-36,46],[-37,46],[-38,46],[-39,46],[-40,46],[-41,46],[-42,46],[-43,46],[-44,46],[-45,46],[-46,46],[-47,46],[-48,46],[-49,46],[-50,46],[-51,46],[-52,46],[-53,46],[-54,46],[-55,46],#272
	[-35,47],[-36,47],[-37,47],[-38,47],[-39,47],[-40,47],[-41,47],[-42,47],[-43,47],[-44,47],[-45,47],[-46,47],[-47,47],[-48,47],[-49,47],[-50,47],[-51,47],[-52,47],[-53,47],[-54,47],[-55,47],#293
	[-35,48],[-36,48],[-37,48],[-38,48],[-39,48],[-40,48],[-41,48],[-42,48],[-43,48],[-44,48],[-45,48],[-46,48],[-47,48],[-48,48],[-49,48],[-50,48],[-51,48],[-52,48],[-53,48],[-54,48],[-55,48],#314
	[-35,49],[-36,49],[-37,49],[-38,49],[-39,49],[-40,49],[-41,49],[-42,49],[-43,49],[-44,49],[-45,49],[-46,49],[-47,49],[-48,49],[-49,49],[-50,49],[-51,49],[-52,49],[-53,49],[-54,49],[-55,49],#335
	[-35,50],[-36,50],[-37,50],[-38,50],[-39,50],[-40,50],[-41,50],[-42,50],[-43,50],[-44,50],[-45,50],[-46,50],[-47,50],[-48,50],[-49,50],[-50,50],[-51,50],[-52,50],[-53,50],[-54,50],[-55,50],#356
	[-35,51],[-36,51],[-37,51],[-38,51],[-39,51],[-40,51],[-41,51],[-42,51],[-43,51],[-44,51],[-45,51],[-46,51],[-47,51],[-48,51],[-49,51],[-50,51],[-51,51],[-52,51],[-53,51],[-54,51],[-55,51],#377
	[-35,52],[-36,52],[-37,52],[-38,52],[-39,52],[-40,52],[-41,52],[-42,52],[-43,52],[-44,52],[-45,52],[-46,52],[-47,52],[-48,52],[-49,52],[-50,52],[-51,52],[-52,52],[-53,52],[-54,52],[-55,52],#398
	[-35,53],[-36,53],[-37,53],[-38,53],[-39,53],[-40,53],[-41,53],[-42,53],[-43,53],[-44,53],[-45,53],[-46,53],[-47,53],[-48,53],[-49,53],[-50,53],[-51,53],[-52,53],[-53,53],[-54,53],[-55,53],#419
	[-35,54],[-36,54],[-37,54],[-38,54],[-39,54],[-40,54],[-41,54],[-42,54],[-43,54],[-44,54],[-45,54],[-46,54],[-47,54],[-48,54],[-49,54],[-50,54],[-51,54],[-52,54],[-53,54],[-54,54],[-55,54],#440
	[-35,55],[-36,55],[-37,55],[-38,55],[-39,55],[-40,55],[-41,55],[-42,55],[-43,55],[-44,55],[-45,55],[-46,55],[-47,55],[-48,55],[-49,55],[-50,55],[-51,55],[-52,55],[-53,55],[-54,55],[-55,55]]#461

C = [[-55,37],[-58,37],[-61,37],[-64,37],[-67,37],[-70,37],[-73,37],[-76,37],[-79,37],[-82,37],[-85,37],[-88,37],[-91,37],[-94,37],[-97,37],
	[-104.0,38.0],[-104.8,38.0],[-105.7,37.9],[-106.5,37.9],[-107.3,37.8],[-108.2,37.6],[-109.0,37.5],[-109.8,37.3],[-110.6,37.1],[-111.4,36.8],[-112.2,36.6],[-113.0,36.3],[-113.8,35.9],[-114.5,35.6],[-115.3,35.2],[-116.0,34.8],[-116.7,34.4],[-117.4,33.9],[-118.1,33.4],[-118.8,32.9],[-119.4,32.4],[-120.1,31.8],[-120.7,31.3],[-121.3,30.7],[-121.8,30.1],[-122.4,29.4],[-122.9,28.8],[-123.4,28.1],[-123.9,27.4],[-124.4,26.7],[-124.8,26.0],[-125.2,25.3],[-125.6,24.5],[-125.9,23.8],[-126.3,23.0],[-126.6,22.2],[-126.8,21.4],[-127.1,20.6],[-127.3,19.8],[-127.5,19.0],[-127.6,18.2],[-127.8,17.3],[-127.9,16.5],[-127.9,15.7],[-128.0,14.8],[-128.0,14.0],
	[-128,-14],[-128,-17],[-128,-20],[-128,-23],[-128,-26],[-128,-29],[-128,-32],[-128,-35],[-128,-38],[-128,-41],[-128,-44],[-128,-47],[-128,-50],[-128,-53],[-128,-56],[-128,-59],[-128,-62],[-128,-65],[-128,-68],[-128,-71],[-128,-74],[-128,-77],[-128,-80],[-128,-83],[-128,-86],[-128,-89],[-128,-92],[-128,-95],[-128,-98],
	[-128.0,-100.0],[-128.0,-100.8],[-127.9,-101.7],[-127.9,-102.5],[-127.8,-103.3],[-127.6,-104.2],[-127.5,-105.0],[-127.3,-105.8],[-127.1,-106.6],[-126.8,-107.4],[-126.6,-108.2],[-126.3,-109.0],[-125.9,-109.8],[-125.6,-110.5],[-125.2,-111.3],[-124.8,-112.0],[-124.4,-112.7],[-123.9,-113.4],[-123.4,-114.1],[-122.9,-114.8],[-122.4,-115.4],[-121.8,-116.1],[-121.3,-116.7],[-120.7,-117.3],[-120.1,-117.8],[-119.4,-118.4],[-118.8,-118.9],[-118.1,-119.4],[-117.4,-119.9],[-116.7,-120.4],[-116.0,-120.8],[-115.3,-121.2],[-114.5,-121.6],[-113.8,-121.9],[-113.0,-122.3],[-112.2,-122.6],[-111.4,-122.8],[-110.6,-123.1],[-109.8,-123.3],[-109.0,-123.5],[-108.2,-123.6],[-107.3,-123.8],[-106.5,-123.9],[-105.7,-123.9],[-104.8,-124.0],[-104.0,-124.0],
	[-104,-124],[-101,-124],[-98,-124],[-95,-124],[-92,-124],[-89,-124],[-86,-124],[-83,-124],[-80,-124],[-77,-124],[-74,-124],[-71,-124],[-68,-124],[-65,-124],[-62,-124],[-59,-124],[-56,-124],[-53,-124],[-50,-124],[-47,-124],[-44,-124],[-41,-124],[-38,-124],[-35,-124],[-32,-124],[-29,-124],[-26,-124],[-23,-124],[-20,-124],[-17,-124],[-14,-124],[-11,-124],[-8,-124],[-5,-124],[-2,-124],[1,-124],[4,-124],[7,-124],[10,-124],[13,-124],
	[13.0,-124.0],[13.8,-124.0],[14.7,-123.9],[15.5,-123.9],[16.3,-123.8],[17.2,-123.6],[18.0,-123.5],[18.8,-123.3],[19.6,-123.1],[20.4,-122.8],[21.2,-122.6],[22.0,-122.3],[22.8,-121.9],[23.5,-121.6],[24.3,-121.2],[25.0,-120.8],[25.7,-120.4],[26.4,-119.9],[27.1,-119.4],[27.8,-118.9],[28.4,-118.4],[29.1,-117.8],[29.7,-117.3],[30.3,-116.7],[30.8,-116.1],[31.4,-115.4],[31.9,-114.8],[32.4,-114.1],[32.9,-113.4],[33.4,-112.7],[33.8,-112.0],[34.2,-111.3],[34.6,-110.5],[34.9,-109.8],[35.3,-109.0],[35.6,-108.2],[35.8,-107.4],[36.1,-106.6],[36.3,-105.8],[36.5,-105.0],[36.6,-104.2],[36.8,-103.3],[36.9,-102.5],[36.9,-101.7],[37.0,-100.8],[37.0,-100.0],
	[37,-100],[37,-97],[37,-94],[37,-91],[37,-88],[37,-85],[37,-82],[37,-79],[37,-76],[37,-73],[37,-70],[37,-67],[37,-64],[37,-61],[37,-58]]

D = [[47,-55],[47,-58],[47,-61],[47,-64],[47,-67],[47,-70],[47,-73],[47,-76],[47,-79],[47,-82],[47,-85],[47,-88],[47,-91],[47,-94],[47,-97],
	[47.0,-100.0],[47.0,-101.2],[46.9,-102.4],[46.8,-103.6],[46.7,-104.7],[46.5,-105.9],[46.3,-107.1],[46.0,-108.2],[45.7,-109.4],[45.3,-110.5],[44.9,-111.6],[44.5,-112.7],[44.1,-113.8],[43.6,-114.9],[43.0,-116.0],[42.4,-117.0],[41.8,-118.0],[41.2,-119.0],[40.5,-120.0],[39.8,-120.9],[39.0,-121.9],[38.3,-122.8],[37.5,-123.6],[36.6,-124.5],[35.8,-125.3],[34.9,-126.0],[33.9,-126.8],[33.0,-127.5],[32.0,-128.2],[31.0,-128.8],[30.0,-129.4],[29.0,-130.0],[27.9,-130.6],[26.8,-131.1],[25.7,-131.5],[24.6,-131.9],[23.5,-132.3],[22.4,-132.7],[21.2,-133.0],[20.1,-133.3],[18.9,-133.5],[17.7,-133.7],[16.6,-133.8],[15.4,-133.9],[14.2,-134.0],[13.0,-134.0],
	[13,-134],[10,-134],[7,-134],[4,-134],[1,-134],[-2,-134],[-5,-134],[-8,-134],[-11,-134],[-14,-134],[-17,-134],[-20,-134],[-23,-134],[-26,-134],[-29,-134],[-32,-134],[-35,-134],[-38,-134],[-41,-134],[-44,-134],[-47,-134],[-50,-134],[-53,-134],[-56,-134],[-59,-134],[-62,-134],[-65,-134],[-68,-134],[-71,-134],[-74,-134],[-77,-134],[-80,-134],[-83,-134],[-86,-134],[-89,-134],[-92,-134],[-95,-134],[-98,-134],[-101,-134],
	[-104.0,-134.0],[-105.2,-134.0],[-106.4,-133.9],[-107.6,-133.8],[-108.7,-133.7],[-109.9,-133.5],[-111.1,-133.3],[-112.2,-133.0],[-113.4,-132.7],[-114.5,-132.3],[-115.6,-131.9],[-116.7,-131.5],[-117.8,-131.1],[-118.9,-130.6],[-120.0,-130.0],[-121.0,-129.4],[-122.0,-128.8],[-123.0,-128.2],[-124.0,-127.5],[-124.9,-126.8],[-125.9,-126.0],[-126.8,-125.3],[-127.6,-124.5],[-128.5,-123.6],[-129.3,-122.8],[-130.0,-121.9],[-130.8,-120.9],[-131.5,-120.0],[-132.2,-119.0],[-132.8,-118.0],[-133.4,-117.0],[-134.0,-116.0],[-134.6,-114.9],[-135.1,-113.8],[-135.5,-112.7],[-135.9,-111.6],[-136.3,-110.5],[-136.7,-109.4],[-137.0,-108.2],[-137.3,-107.1],[-137.5,-105.9],[-137.7,-104.7],[-137.8,-103.6],[-137.9,-102.4],[-138.0,-101.2],[-138.0,-100.0],
	[-138,-100],[-138,-97],[-138,-94],[-138,-91],[-138,-88],[-138,-85],[-138,-82],[-138,-79],[-138,-76],[-138,-73],[-138,-70],[-138,-67],[-138,-64],[-138,-61],[-138,-58],[-138,-55],[-138,-52],[-138,-49],[-138,-46],[-138,-43],[-138,-40],[-138,-37],[-138,-34],[-138,-31],[-138,-28],[-138,-25],[-138,-22],[-138,-19],[-138,-16],[-138,-13],[-138,-10],[-138,-7],[-138,-4],[-138,-1],[-138,2],[-138,5],[-138,8],[-138,11],
	[-138.0,14.0],[-138.0,15.2],[-137.9,16.4],[-137.8,17.6],[-137.7,18.7],[-137.5,19.9],[-137.3,21.1],[-137.0,22.2],[-136.7,23.4],[-136.3,24.5],[-135.9,25.6],[-135.5,26.7],[-135.1,27.8],[-134.6,28.9],[-134.0,30.0],[-133.4,31.0],[-132.8,32.0],[-132.2,33.0],[-131.5,34.0],[-130.8,34.9],[-130.0,35.9],[-129.3,36.8],[-128.5,37.6],[-127.6,38.5],[-126.8,39.3],[-125.9,40.0],[-124.9,40.8],[-124.0,41.5],[-123.0,42.2],[-122.0,42.8],[-121.0,43.4],[-120.0,44.0],[-118.9,44.6],[-117.8,45.1],[-116.7,45.5],[-115.6,45.9],[-114.5,46.3],[-113.4,46.7],[-112.2,47.0],[-111.1,47.3],[-109.9,47.5],[-108.7,47.7],[-107.6,47.8],[-106.4,47.9],[-105.2,48.0],[-104.0,48.0],
	[-104,48],[-101,48],[-98,48],[-95,48],[-92,48],[-89,48],[-86,48],[-83,48],[-80,48],[-77,48],[-74,48],[-71,48],[-68,48],[-65,48],[-62,48],[-59,48],[-56,48]]

E = [[-35,48],[-32,48],[-29,48],[-26,48],[-23,48],[-20,48],[-17,48],[-14,48],[-11,48],[-8,48],[-5,48],[-2,48],[1,48],[4,48],
	[4.5,48.0],[6.0,48.0],[7.5,47.9],[9.0,47.8],[10.6,47.6],[12.1,47.3],[13.5,47.0],[15.0,46.7],[16.5,46.3],[17.9,45.9],[19.4,45.4],[20.8,44.8],[22.2,44.2],[23.6,43.6],[24.9,42.9],[26.3,42.2],[27.6,41.4],[28.8,40.6],[30.1,39.7],[31.3,38.8],[32.5,37.8],[33.6,36.8],[34.7,35.8],[35.8,34.7],[36.8,33.6],[37.8,32.5],[38.8,31.3],[39.7,30.1],[40.6,28.8],[41.4,27.6],[42.2,26.2],[42.9,24.9],[43.6,23.6],[44.2,22.2],[44.8,20.8],[45.4,19.4],[45.9,17.9],[46.3,16.5],[46.7,15.0],[47.0,13.5],[47.3,12.1],[47.6,10.6],[47.8,9.0],[47.9,7.5],[48.0,6.0],[48.0,4.5],
	[48,5],[48,2],[48,-1],[48,-4],[48,-7],[48,-10],[48,-13],[48,-16],[48,-19],[48,-22],[48,-25],[48,-28],[48,-31]]

F = [[34,-53],[31,-53],[28,-53],[25,-53],[22,-53],[19,-53],[16,-53],[13,-53],[10,-53],[7,-53],[4,-53],[1,-53],[-2,-53],
	[-4.5,-53.0],[-6.2,-53.0],[-7.9,-52.9],[-9.6,-52.7],[-11.2,-52.5],[-12.9,-52.3],[-14.6,-51.9],[-16.2,-51.6],[-17.9,-51.1],[-19.5,-50.6],[-21.1,-50.1],[-22.7,-49.5],[-24.2,-48.8],[-25.8,-48.1],[-27.3,-47.3],[-28.8,-46.5],[-30.2,-45.6],[-31.6,-44.7],[-33.0,-43.7],[-34.4,-42.7],[-35.7,-41.7],[-37.0,-40.5],[-38.2,-39.4],[-39.4,-38.2],[-40.5,-37.0],[-41.7,-35.7],[-42.7,-34.4],[-43.7,-33.0],[-44.7,-31.6],[-45.6,-30.2],[-46.5,-28.8],[-47.3,-27.3],[-48.1,-25.8],[-48.8,-24.2],[-49.5,-22.7],[-50.1,-21.1],[-50.6,-19.5],[-51.1,-17.9],[-51.6,-16.2],[-51.9,-14.6],[-52.3,-12.9],[-52.5,-11.2],[-52.7,-9.6],[-52.9,-7.9],
	[-53,-8],[-53,-5],[-53,-2],[-53,1],[-53,4],[-53,7],[-53,10],[-53,13],[-53,16],[-53,19],[-53,22],[-53,25],[-53,28],[-53,31],[-53,34]]	

G = [[-37,34],[-37,31],[-37,28],[-37,25],[-37,22],[-37,19],[-37,16],[-37,13],[-37,10],[-37,7],[-37,4],[-37,1],[-37,-2],[-37,-5],
	[-37.0,-4.5],[-37.0,-5.6],[-36.9,-6.8],[-36.8,-7.9],[-36.7,-9.0],[-36.5,-10.1],[-36.3,-11.3],[-36.0,-12.4],[-35.7,-13.5],[-35.4,-14.5],[-35.0,-15.6],[-34.6,-16.7],[-34.2,-17.7],[-33.7,-18.7],[-33.2,-19.8],[-32.6,-20.8],[-32.1,-21.7],[-31.4,-22.7],[-30.8,-23.6],[-30.1,-24.5],[-29.4,-25.4],[-28.7,-26.2],[-27.9,-27.1],[-27.1,-27.9],[-26.2,-28.7],[-25.4,-29.4],[-24.5,-30.1],[-23.6,-30.8],[-22.7,-31.4],[-21.7,-32.1],[-20.8,-32.6],[-19.8,-33.2],[-18.7,-33.7],[-17.7,-34.2],[-16.7,-34.6],[-15.6,-35.0],[-14.5,-35.4],[-13.5,-35.7],[-12.4,-36.0],[-11.3,-36.3],[-10.1,-36.5],[-9.0,-36.7],[-7.9,-36.8],[-6.8,-36.9],[-5.6,-37.0],[-4.5,-37.0],
	[-5,-37],[-2,-37],[1,-37],[4,-37],[7,-37],[10,-37],[13,-37],[16,-37],[19,-37],[22,-37],[25,-37],[28,-37],[31,-37],[34,-37]]

H = [[55,-56],[54,-56],[53,-56],[52,-56],[51,-56],[50,-56],[49,-56],[48,-56],[47,-56],[46,-56],[45,-56],[44,-56],[43,-56],[42,-56],[41,-56],[40,-56],[39,-56],[38,-56],[37,-56],[36,-56],[35,-56],
	[55,-55],[54,-55],[53,-55],[52,-55],[51,-55],[50,-55],[49,-55],[48,-55],[47,-55],[46,-55],[45,-55],[44,-55],[43,-55],[42,-55],[41,-55],[40,-55],[39,-55],[38,-55],[37,-55],[36,-55],[35,-55],
	[55,-54],[54,-54],[53,-54],[52,-54],[51,-54],[50,-54],[49,-54],[48,-54],[47,-54],[46,-54],[45,-54],[44,-54],[43,-54],[42,-54],[41,-54],[40,-54],[39,-54],[38,-54],[37,-54],[36,-54],[35,-54],
	[55,-53],[54,-53],[53,-53],[52,-53],[51,-53],[50,-53],[49,-53],[48,-53],[47,-53],[46,-53],[45,-53],[44,-53],[43,-53],[42,-53],[41,-53],[40,-53],[39,-53],[38,-53],[37,-53],[36,-53],[35,-53],
	[55,-52],[54,-52],[53,-52],[52,-52],[51,-52],[50,-52],[49,-52],[48,-52],[47,-52],[46,-52],[45,-52],[44,-52],[43,-52],[42,-52],[41,-52],[40,-52],[39,-52],[38,-52],[37,-52],[36,-52],[35,-52],
	[55,-51],[54,-51],[53,-51],[52,-51],[51,-51],[50,-51],[49,-51],[48,-51],[47,-51],[46,-51],[45,-51],[44,-51],[43,-51],[42,-51],[41,-51],[40,-51],[39,-51],[38,-51],[37,-51],[36,-51],[35,-51],
	[55,-50],[54,-50],[53,-50],[52,-50],[51,-50],[50,-50],[49,-50],[48,-50],[47,-50],[46,-50],[45,-50],[44,-50],[43,-50],[42,-50],[41,-50],[40,-50],[39,-50],[38,-50],[37,-50],[36,-50],[35,-50],
	[55,-49],[54,-49],[53,-49],[52,-49],[51,-49],[50,-49],[49,-49],[48,-49],[47,-49],[46,-49],[45,-49],[44,-49],[43,-49],[42,-49],[41,-49],[40,-49],[39,-49],[38,-49],[37,-49],[36,-49],[35,-49],
	[55,-48],[54,-48],[53,-48],[52,-48],[51,-48],[50,-48],[49,-48],[48,-48],[47,-48],[46,-48],[45,-48],[44,-48],[43,-48],[42,-48],[41,-48],[40,-48],[39,-48],[38,-48],[37,-48],[36,-48],[35,-48],
	[55,-47],[54,-47],[53,-47],[52,-47],[51,-47],[50,-47],[49,-47],[48,-47],[47,-47],[46,-47],[45,-47],[44,-47],[43,-47],[42,-47],[41,-47],[40,-47],[39,-47],[38,-47],[37,-47],[36,-47],[35,-47],
	[55,-46],[54,-46],[53,-46],[52,-46],[51,-46],[50,-46],[49,-46],[48,-46],[47,-46],[46,-46],[45,-46],[44,-46],[43,-46],[42,-46],[41,-46],[40,-46],[39,-46],[38,-46],[37,-46],[36,-46],[35,-46],
	[55,-45],[54,-45],[53,-45],[52,-45],[51,-45],[50,-45],[49,-45],[48,-45],[47,-45],[46,-45],[45,-45],[44,-45],[43,-45],[42,-45],[41,-45],[40,-45],[39,-45],[38,-45],[37,-45],[36,-45],[35,-45],
	[55,-44],[54,-44],[53,-44],[52,-44],[51,-44],[50,-44],[49,-44],[48,-44],[47,-44],[46,-44],[45,-44],[44,-44],[43,-44],[42,-44],[41,-44],[40,-44],[39,-44],[38,-44],[37,-44],[36,-44],[35,-44],
	[55,-43],[54,-43],[53,-43],[52,-43],[51,-43],[50,-43],[49,-43],[48,-43],[47,-43],[46,-43],[45,-43],[44,-43],[43,-43],[42,-43],[41,-43],[40,-43],[39,-43],[38,-43],[37,-43],[36,-43],[35,-43],
	[55,-42],[54,-42],[53,-42],[52,-42],[51,-42],[50,-42],[49,-42],[48,-42],[47,-42],[46,-42],[45,-42],[44,-42],[43,-42],[42,-42],[41,-42],[40,-42],[39,-42],[38,-42],[37,-42],[36,-42],[35,-42],
	[55,-41],[54,-41],[53,-41],[52,-41],[51,-41],[50,-41],[49,-41],[48,-41],[47,-41],[46,-41],[45,-41],[44,-41],[43,-41],[42,-41],[41,-41],[40,-41],[39,-41],[38,-41],[37,-41],[36,-41],[35,-41],
	[55,-40],[54,-40],[53,-40],[52,-40],[51,-40],[50,-40],[49,-40],[48,-40],[47,-40],[46,-40],[45,-40],[44,-40],[43,-40],[42,-40],[41,-40],[40,-40],[39,-40],[38,-40],[37,-40],[36,-40],[35,-40],
	[55,-39],[54,-39],[53,-39],[52,-39],[51,-39],[50,-39],[49,-39],[48,-39],[47,-39],[46,-39],[45,-39],[44,-39],[43,-39],[42,-39],[41,-39],[40,-39],[39,-39],[38,-39],[37,-39],[36,-39],[35,-39],
	[55,-38],[54,-38],[53,-38],[52,-38],[51,-38],[50,-38],[49,-38],[48,-38],[47,-38],[46,-38],[45,-38],[44,-38],[43,-38],[42,-38],[41,-38],[40,-38],[39,-38],[38,-38],[37,-38],[36,-38],[35,-38],
	[55,-37],[54,-37],[53,-37],[52,-37],[51,-37],[50,-37],[49,-37],[48,-37],[47,-37],[46,-37],[45,-37],[44,-37],[43,-37],[42,-37],[41,-37],[40,-37],[39,-37],[38,-37],[37,-37],[36,-37],[35,-37],
	[55,-36],[54,-36],[53,-36],[52,-36],[51,-36],[50,-36],[49,-36],[48,-36],[47,-36],[46,-36],[45,-36],[44,-36],[43,-36],[42,-36],[41,-36],[40,-36],[39,-36],[38,-36],[37,-36],[36,-36],[35,-36],]

I =[[-52.5,56],[-52.5,59],[-52.5,62],
	[-52.5,64.5],[-52.5,66.2],[-52.4,67.8],[-52.2,69.5],[-52.0,71.2],[-51.8,72.8],[-51.5,74.5],[-51.1,76.1],[-50.6,77.7],[-50.2,79.3],[-49.6,80.9],[-49.0,82.5],[-48.4,84.0],[-47.6,85.5],[-46.9,87.0],[-46.1,88.5],[-45.2,89.9],[-44.3,91.3],[-43.3,92.7],[-42.3,94.1],[-41.3,95.4],[-40.2,96.6],[-39.0,97.8],[-37.8,99.0],[-36.6,100.2],[-35.4,101.3],[-34.1,102.3],[-32.7,103.3],[-31.3,104.3],[-29.9,105.2],[-28.5,106.1],[-27.0,106.9],[-25.5,107.6],[-24.0,108.4],[-22.5,109.0],[-20.9,109.6],[-19.3,110.2],[-17.7,110.6],[-16.1,111.1],[-14.5,111.5],[-12.8,111.8],[-11.2,112.0],[-9.5,112.2],[-7.8,112.4],[-6.2,112.5],[-4.5,112.5],
	[-4,112.5],[-1,112.5],[2,112.5],[5,112.5],[8,112.5],[11,112.5],[14,112.5],[17,112.5],[20,112.5],[23,112.5],[26,112.5],[29,112.5],[32,112.5],[35,112.5],[38,112.5],[41,112.5],[44,112.5],[47,112.5],[50,112.5],[53,112.5],[56,112.5],[59,112.5],[62,112.5],[65,112.5],[68,112.5],[71,112.5],[74,112.5],[77,112.5],[80,112.5],[83,112.5],[86,112.5],
	[87.0,112.5],[88.4,112.5],[89.8,112.4],[91.1,112.3],[92.5,112.1],[93.9,111.9],[95.2,111.6],[96.6,111.3],[97.9,111.0],[99.2,110.6],[100.5,110.1],[101.8,109.6],[103.1,109.1],[104.3,108.5],[105.5,107.9],[106.8,107.2],[107.9,106.5],[109.1,105.7],[110.2,105.0],[111.3,104.1],[112.4,103.3],[113.4,102.4],[114.4,101.4],[115.4,100.4],[116.4,99.4],[117.3,98.4],[118.1,97.3],[119.0,96.2],[119.7,95.1],[120.5,93.9],[121.2,92.8],[121.9,91.5],[122.5,90.3],[123.1,89.1],[123.6,87.8],[124.1,86.5],[124.6,85.2],[125.0,83.9],[125.3,82.6],[125.6,81.2],[125.9,79.9],[126.1,78.5],[126.3,77.1],[126.4,75.8],[126.5,74.4],[126.5,73.0],
	[126.5,73],[126.5,70],[126.5,67],[126.5,64],[126.5,61],[126.5,58],[126.5,55],[126.5,52],[126.5,49],[126.5,46],[126.5,43],[126.5,40],[126.5,37],[126.5,34],[126.5,31],[126.5,28],[126.5,25],[126.5,22],[126.5,19],[126.5,16],[126.5,13],[126.5,10],[126.5,7],[126.5,4],[126.5,1],[126.5,-2],[126.5,-5],[126.5,-8],[126.5,-11],
	[126.5,-13.5],[126.5,-14.9],[126.4,-16.3],[126.3,-17.6],[126.1,-19.0],[125.9,-20.4],[125.6,-21.7],[125.3,-23.1],[125.0,-24.4],[124.6,-25.7],[124.1,-27.0],[123.6,-28.3],[123.1,-29.6],[122.5,-30.8],[121.9,-32.0],[121.2,-33.3],[120.5,-34.4],[119.7,-35.6],[119.0,-36.7],[118.1,-37.8],[117.3,-38.9],[116.4,-39.9],[115.4,-40.9],[114.4,-41.9],[113.4,-42.9],[112.4,-43.8],[111.3,-44.6],[110.2,-45.5],[109.1,-46.2],[107.9,-47.0],[106.8,-47.7],[105.5,-48.4],[104.3,-49.0],[103.1,-49.6],[101.8,-50.1],[100.5,-50.6],[99.2,-51.1],[97.9,-51.5],[96.6,-51.8],[95.2,-52.1],[93.9,-52.4],[92.5,-52.6],[91.1,-52.8],[89.8,-52.9],[88.4,-53.0],[87.0,-53.0],
	[87,-53],[84,-53],[81,-53],[78,-53],[75,-53],[72,-53],[69,-53],[66,-53],[63,-53],[60,-53],[57,-53]]

J = [[55,-37],[58,-37],[61,-37],[64,-37],[67,-37],[70,-37],[73,-37],[76,-37],[79,-37],[82,-37],[85,-37],
	[87.0,-37.0],[87.8,-37.0],[88.6,-36.9],[89.5,-36.9],[90.3,-36.8],[91.1,-36.6],[91.9,-36.5],[92.7,-36.3],[93.5,-36.1],[94.3,-35.8],[95.0,-35.6],[95.8,-35.3],[96.6,-35.0],[97.3,-34.6],[98.0,-34.2],[98.8,-33.9],[99.5,-33.4],[100.1,-33.0],[100.8,-32.5],[101.5,-32.0],[102.1,-31.5],[102.7,-31.0],[103.3,-30.4],[103.9,-29.8],[104.5,-29.2],[105.0,-28.6],[105.5,-28.0],[106.0,-27.3],[106.5,-26.6],[106.9,-26.0],[107.4,-25.3],[107.7,-24.5],[108.1,-23.8],[108.5,-23.1],[108.8,-22.3],[109.1,-21.5],[109.3,-20.8],[109.6,-20.0],[109.8,-19.2],[110.0,-18.4],[110.1,-17.6],[110.3,-16.8],[110.4,-16.0],[110.4,-15.1],[110.5,-14.3],[110.5,-13.5],
	[111,-12],[111,-9],[111,-6],[111,-3],[111,0],[111,3],[111,6],[111,9],[111,12],[111,15],[111,18],[111,21],[111,24],[111,27],[111,30],[111,33],[111,36],[111,39],[111,42],[111,45],[111,48],[111,51],[111,54],[111,57],[111,60],[111,63],[111,66],[111,69],[111,72],[111,75],
	[110.5,73.0],[110.5,73.8],[110.4,74.6],[110.4,75.5],[110.3,76.3],[110.1,77.1],[110.0,77.9],[109.8,78.7],[109.6,79.5],[109.3,80.3],[109.1,81.0],[108.8,81.8],[108.5,82.6],[108.1,83.3],[107.7,84.0],[107.4,84.8],[106.9,85.5],[106.5,86.1],[106.0,86.8],[105.5,87.5],[105.0,88.1],[104.5,88.7],[103.9,89.3],[103.3,89.9],[102.7,90.5],[102.1,91.0],[101.5,91.5],[100.8,92.0],[100.1,92.5],[99.5,92.9],[98.8,93.4],[98.0,93.7],[97.3,94.1],[96.6,94.5],[95.8,94.8],[95.0,95.1],[94.3,95.3],[93.5,95.6],[92.7,95.8],[91.9,96.0],[91.1,96.1],[90.3,96.3],[89.5,96.4],[88.6,96.4],[87.8,96.5],[87.0,96.5],
	[87,96.5],[84,96.5],[81,96.5],[78,96.5],[75,96.5],[72,96.5],[69,96.5],[66,96.5],[63,96.5],[60,96.5],[57,96.5],[54,96.5],[51,96.5],[48,96.5],[45,96.5],[42,96.5],[39,96.5],[36,96.5],[33,96.5],[30,96.5],[27,96.5],[24,96.5],[21,96.5],[18,96.5],[15,96.5],[12,96.5],[9,96.5],[6,96.5],[3,96.5],[0,96.5],[-3,96.5],
	[-4.5,96.5],[-5.6,96.5],[-6.7,96.4],[-7.8,96.3],[-9.0,96.2],[-10.1,96.0],[-11.2,95.8],[-12.2,95.5],[-13.3,95.3],[-14.4,94.9],[-15.4,94.6],[-16.5,94.2],[-17.5,93.7],[-18.5,93.3],[-19.5,92.8],[-20.5,92.2],[-21.5,91.6],[-22.4,91.0],[-23.3,90.4],[-24.2,89.7],[-25.1,89.0],[-25.9,88.3],[-26.7,87.5],[-27.5,86.7],[-28.3,85.9],[-29.0,85.1],[-29.7,84.2],[-30.4,83.3],[-31.0,82.4],[-31.6,81.5],[-32.2,80.5],[-32.8,79.5],[-33.3,78.5],[-33.7,77.5],[-34.2,76.5],[-34.6,75.4],[-34.9,74.4],[-35.3,73.3],[-35.5,72.2],[-35.8,71.2],[-36.0,70.1],[-36.2,69.0],[-36.3,67.8],[-36.4,66.7],[-36.5,65.6],[-36.5,64.5],
	[-36.5,65],[-36.5,62],[-36.5,59],[-36.5,56]]	
class Graph():
	def __init__(self):
		"""
		self.edges is a dict of all possible next nodes
		e.g. {'X': ['A', 'B', 'C', 'E'], ...}
		self.weights has all the weights between two nodes,
		with the two nodes as a tuple as the key
		e.g. {('X', 'A'): 7, ('X', 'B'): 2, ...}
		"""
		self.edges = defaultdict(list)
		self.weights = {}
	
	def add_edge(self, from_node, to_node, weight):
		# Note: assumes edges are bi-directional
		self.edges[from_node].append(to_node)
		self.weights[(from_node, to_node)] = weight
		self.weights[(to_node, from_node)] = weight

class IGraph():
	def __init__(self):
		"""
		self.edges is a dict of all possible next nodes
		e.g. {'X': ['A', 'B', 'C', 'E'], ...}
		self.weights has all the weights between two nodes,
		with the two nodes as a tuple as the key
		e.g. {('X', 'A'): 7, ('X', 'B'): 2, ...}
		"""
		self.edges = defaultdict(list)
		self.weights = {}
	
	def add_edge(self, from_node, to_node, weight):
		# Note: assumes edges are bi-directional
		self.edges[from_node].append(to_node)
		self.edges[to_node].append(from_node)
		self.weights[(from_node, to_node)] = weight
		self.weights[(to_node, from_node)] = weight

A_graph = Graph()
A_edges = []
for i in range(len(A)):
	A_edges.append([i,i+1,1])

for edge in A_edges:
	A_graph.add_edge(*edge)

B_graph = IGraph()
B_edges = []

for j in range(21):
	B_edges=[]
	for i in range(0,len(B)-21,21):
		B_edges.append([j+i,j+i+21,1])
	for edge in B_edges:
		B_graph.add_edge(*edge)

for i in range(0,len(B),21):
	B_edges = []
	for j in range(21):
		B_edges.append([j+i,j+i+1,1])
	for edge in B_edges:
		B_graph.add_edge(*edge)	

for j in range(21):
	B_edges=[]
	for i in range(0,len(B)-21,21):
		B_edges.append([j+i,j+i+22,1])	
	for edge in B_edges:
		B_graph.add_edge(*edge)

for i in range(0,len(B),21):
	B_edges = []
	for j in range(21):
		B_edges.append([j+i,j+i+2,1])
	for edge in B_edges:
		B_graph.add_edge(*edge)	

C_graph = Graph()
C_edges = []
for i in range(len(C)):
	C_edges.append([i,i+1,1])

for edge in C_edges:
	C_graph.add_edge(*edge)

D_graph = Graph()
D_edges = []
for i in range(len(D)):
	D_edges.append([i,i+1,1])

for edge in D_edges:
	D_graph.add_edge(*edge)

E_graph = Graph()
E_edges = []
for i in range(len(E)):
	E_edges.append([i,i+1,1])

for edge in E_edges:
	E_graph.add_edge(*edge)

F_graph = Graph()
F_edges = []
for i in range(len(F)):
	F_edges.append([i,i+1,1])

for edge in F_edges:
	F_graph.add_edge(*edge)

G_graph = Graph()
G_edges = []
for i in range(len(G)):
	G_edges.append([i,i+1,1])

for edge in G_edges:
	G_graph.add_edge(*edge)

H_graph = Graph()
H_edges = []
for i in range(21):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(21,42):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(42,63):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(63,84):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(63,84):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)

H_edges = []
for i in range(461,440,-1):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(440,419,-1):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(419,398,-1):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(398,377,-1):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(377,356,-1):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)

H_edges = []
for i in range(441,-1,-21):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(442,-1,-21):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(2,461,21):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(3,461,21):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(4,461,21):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)

H_edges = []
for i in range(461,1,-21):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(460,1,-21):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(459,1,-21):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(458,1,-21):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)
H_edges = []
for i in range(457,1,-21):
	H_edges.append([i,i+1,1])
for edge in H_edges:
	H_graph.add_edge(*edge)

I_graph = Graph()
I_edges = []
for i in range(len(I)):
	I_edges.append([i,i+1,1])

for edge in I_edges:
	I_graph.add_edge(*edge)

J_graph = Graph()
J_edges = []
for i in range(len(J)):
	J_edges.append([i,i+1,1])

for edge in J_edges:
	J_graph.add_edge(*edge)

def sendmessage(id,zone,timein,timeout):
	message = netstruct.pack(b"ib$ff",id,bytes(zone,'utf-8'),timein,timeout)
	emitter.send(message) 

# def receivemessage():
# 	while receiver.getQueueLength()>0:
# 		data = receiver.getData()
# 		message = netstruct.unpack(b"ib$ff",data)
# 		hisdict[message[1].decode('utf-8')] = [round(message[2],1),round(message[3],1)]	
# 		receiver.nextPacket()
def receivemessage():
	while receiver.getQueueLength()>0:
		data = receiver.getData()
		message = netstruct.unpack(b"ib$ff",data)
		message[1] = message[1].decode('utf-8')
		if message[0] in outerdict_zones.keys():
			key = message[1]
			innerdict_zones[key] = [round(message[2],1),round(message[3],1)]
			outerdict_zones[message[0]].update(innerdict_zones)
		else:
			innerdict_zones = {}
			key = message[1]
			innerdict_zones[key] = [round(message[2],1),round(message[3],1)]
			outerdict_zones[message[0]] = innerdict_zones
		receiver.nextPacket()

def mindistcalc():
	temp = (vr +(rho*amax)) * (vr + (rho*amax))
	distmin = (vr * rho) + (0.5 * amax * rho *rho) +(temp/(2 * dmin)) - ((vf*vf)/(2*dmax))
	return distmin

def calculateCorner(cx,cz,rotation):
	
	car_corners[0][0] = cx + -(float(carl)/2)*np.sin(rotation) + (float(carw)/2)*np.cos(rotation)
	car_corners[0][1] = cz + -(float(carl)/2)*np.cos(rotation) - (float(carw)/2)*np.sin(rotation)
 
	car_corners[1][0] = cx + (float(carl)/2)*np.sin(rotation) + (float(carw)/2)*np.cos(rotation)
	car_corners[1][1] = cz + (float(carl)/2)*np.cos(rotation) - (float(carw)/2)*np.sin(rotation)

 
	car_corners[2][0] = cx + (float(carl)/2)*np.sin(rotation) - (float(carw)/2)*np.cos(rotation)
	car_corners[2][1] = cz + (float(carl)/2)*np.cos(rotation) + (float(carw)/2)*np.sin(rotation)
 
	car_corners[3][0] = cx + (float(carl)/2)*np.sin(rotation) + (float(carw)/2)*np.cos(rotation)
	car_corners[3][1] = cz + (float(carl)/2)*np.cos(rotation) + (float(carw)/2)*np.sin(rotation)

def intersection(cx,cy,w,h,angle,cx1,cy1,w1,h1,angle1):
	global counter
	global global_counter
	global tempdict
	
	c = shapely.geometry.box(-w/2.0, -h/2.0, w/2.0, h/2.0)
	rc = shapely.affinity.rotate(c, angle)
	tc = shapely.affinity.translate(rc, cx, cy)
	c1 = shapely.geometry.box(-w1/2.0, -h1/2.0, w1/2.0, h1/2.0)
	rc1 = shapely.affinity.rotate(c1, angle1)
	tc1 = shapely.affinity.translate(rc1, cx1, cy1)

	if tc.intersects(tc1):
		temp =  'x' + str(int(cx1)) + 'z'+ str(int(cy1))
		if temp in tempdict:
			tempdict[temp].append(round(super.getTime()+0.1*global_counter,1))
		else:
			tempdict[temp] = [round(super.getTime()+0.1*global_counter,1)]	

def getbearing(x1,x2,z1,z2):
	X = x2 - x1
	Z = z2 - z1
	rad = np.arctan2(X,Z)
	bearing = (rad)/ np.pi * 180
	if bearing < 0:
		bearing = bearing + 360	
	return bearing

def getheading(x,z):
	rad = np.arctan2(x,z)
	heading = (rad - 1.5708)/ np.pi * 180
	print("H:" +heading)
	if heading < 0:
		heading = heading + 360
	heading = 360 - heading
	return heading		 

def calcdist(x1,x2,z1,z2):
	dist = math.sqrt(pow((x1-x2),2) + pow((z1-z2),2))
	return dist

def error(heading,bearing):
	err = bearing - heading

	if err < -180:
		err = err + 360
	if err > 180:	
		err = err - 360

	return err	

def applyPID(err):
	Kp = 0.55	
	Ki = 0.01
	Kd = 0

		
	if 'oldvalue' not in applyPID.__dict__:
		applyPID.oldvalue = 0
	diff = err - applyPID.oldvalue
	
	if 'integral' not in applyPID.__dict__:
		applyPID.integral = 0

	applyPID.integral = applyPID.integral + applyPID.oldvalue
	
	if applyPID.integral > 30: 
		applyPID.integral = 30
	if applyPID.integral < -30:
		applyPID.integral = -30		
	# print("Integral: " + str(applyPID.integral))	

	applyPID.oldvalue = err	

	return Kp*err + Ki*applyPID.integral + Kd*diff

# def tempadv(zone):
# 	global mydict
# 	global mylane
# 	global hislane
# 	global disadv
# 	mylist = mydict[zone]
# 	hislist = hisdict[zone]
# 	if mylane == hislane:
# 		if mylist[0] >= hislist[0]:
# 			disadv[key] = [mylist[0],hislist[1]]

# 	else:
# 		if mylist[0] >= hislist[0] and d>mindistcalc():
# 			if zone not in disadv:
# 				disadv.append(zone)	

def tempadv(zone,name):
	disadv[zone] = []
	if mydict[zone][0] > outerdict_zones[name][zone][0]:
		disadv[zone].append(name)
		disadv[zone].append(car_id)
	else:
		disadv[zone].append(car_id)
		disadv[zone].append(name)
def global_planner(x,z):
	global A
	global B
	global C

	dist = {}
	Atemp_dist = []
	for point in A:
		Atemp_dist.append(calcdist(x,point[0],z,point[1]))	
	dist['A'] = min(Atemp_dist)

	Btemp_dist = []
	for point in B:
		Btemp_dist.append(calcdist(x,point[0],z,point[1]))
	dist['B'] = min(Btemp_dist)
	Ctemp_dist = []

	for point in C:
		Ctemp_dist.append(calcdist(x,point[0],z,point[1]))
	dist['C'] = min(Ctemp_dist)
	
	key_min = min(dist.keys(), key=(lambda k: dist[k]))
	return key_min
			
def localplanner(node,initial_node,end_node):
	global A
	global B
	grid_size = 2
	robot_radius = 3.0
	init_node_dist = []
	end_node_dist = []
	
	if node == 'A':
		init_node_graph = A.index(initial_node)
		end_node_graph = A.index(end_node)
		nodes = dijsktra(A_graph,init_node_graph,end_node_graph)
		d = 0
		for i in range(len(nodes) - 1):
			d = d + calcdist(A[nodes[i]][0],A[nodes[i+1]][0],A[nodes[i]][1],A[nodes[i+1]][1]) 
		return d
			
	elif node == 'B':
		init_node_graph = B.index(initial_node)
		end_node_graph = B.index(end_node)
		nodes = dijsktra(B_graph,init_node_graph,end_node_graph)
		d = 0
		for i in range(len(nodes)-1):
			d = d + calcdist(B[nodes[i]][0],B[nodes[i+1]][0],B[nodes[i]][1],B[nodes[i+1]][1]) 
		return d
		
	elif node == 'C':
		init_node_graph = C.index(initial_node)
		end_node_graph = C.index(end_node)
		nodes = dijsktra(C_graph,init_node_graph,end_node_graph)
		d = 0
		for i in range(len(nodes)-1):
			d = d + calcdist(C[nodes[i]][0],C[nodes[i+1]][0],C[nodes[i]][1],C[nodes[i+1]][1]) 
		return d
		

def motionplanning(zone):
	global outerdict_zones
	global tx
	global tz
	intx = ''
	intz = ''
	
	for i in range(0,len(zone)):
		if zone[i] == 'z':
			posz = i

	for i in range(1,posz):
		intx = intx + zone[i]

	for i in range(posz+1,len(zone)):
		intz = intz + zone[i]
	intx = int(intx)
	intz = int(intz)
	
	source_node = global_planner(tx,tz)
	dest_node = global_planner(int(intx),int(intz))

	nodes = dijsktra(graph,source_node,dest_node)
	dist = 0
	for node in nodes:
		if node == 'A':
			init_node_dist = []
			end_node_dist = []
			for point in A:
				init_node_dist.append(calcdist(tx,point[0],tz,point[1]))
			initial_node = A[init_node_dist.index(min(init_node_dist))]
		
			for point in A:
				end_node_dist.append(calcdist(int(intx),point[0],int(intz),point[1]))
			end_node = A[end_node_dist.index(min(end_node_dist))]
			
			dist = dist + localplanner(node,initial_node,end_node)
		
		elif node == 'B':
			init_node_dist = []
			end_node_dist = []
			for point in B:
				init_node_dist.append(calcdist(tx,point[0],tz,point[1]))
			initial_node = B[init_node_dist.index(min(init_node_dist))]
		
			for point in B:
				end_node_dist.append(calcdist(int(intx),point[0],int(intz),point[1]))
			end_node = B[end_node_dist.index(min(end_node_dist))]
			dist = dist + localplanner(node,initial_node,end_node)
	
		elif node == 'C':
			init_node_dist = []
			end_node_dist = []
			for point in C:
				init_node_dist.append(calcdist(tx,point[0],tz,point[1]))
			initial_node = C[init_node_dist.index(min(init_node_dist))]
		
			for point in C:
				end_node_dist.append(calcdist(int(intx),point[0],int(intz),point[1]))
			end_node = C[end_node_dist.index(min(end_node_dist))]
			dist = dist + localplanner(node,initial_node,end_node)

	if disadv[zone][0] == car_id:
		t = mydict[zone][1] - super.getTime() + 0.5
	else:
		t = outerdict_zones[disadv[zone][0]][zone][1] - super.getTime() + 0.5

	print("My new time in for:" + str(zone) + " is " + str(t + super.getTime()))
	v = dist/t 
	print("My new velocity is:" + str(v))
	return v

def settrajectory():
	global tx
	global tz
	global k
	global waypoints
	global tphi
	global v

	distance = calcdist(tx,waypoints[k][0],tz,waypoints[k][1])
	###########finding the current heading#################
	myangle =tphi * 180/np.pi
	if myangle < 0:
		myangle = myangle + 360

	###########trajectory execution#################
	refangle = getbearing(tx,waypoints[k][0],tz,waypoints[k][1])
	err = error(myangle,refangle)
	steer = applyPID(err)
	rad = steer * np.pi/180
	
	nx = tx + v * (timestep/1000.0) * np.sin(tphi)
	nz = tz + v * (timestep/1000.0) * np.cos(tphi)
	nphi = tphi + (v/4) * (timestep/1000.0) * np.tan(rad) 

	
	tx = nx
	tz = nz
	tphi = nphi
	if distance<1.35:
		k = k + 1
	rotationField.setSFRotation([0,1,0,tphi])
	translationField.setSFVec3f([tx,1.4,tz])
graph = Graph()
edges = [('A','B',1),
		 ('B','C',1),
		 ('D','B',1),
		 ('B','E',1),
		 ('F','B',1),
		 ('B','G',1),
		 ('G','H',1),
		 ('H','F',1),
		 ('C','H',1),
		 ('H','D',1),
		 ('H','A',1),
		 ('E','H',1),
		 ('H','I',1),
		 ('J','H',1),
		 ('B','J',1),
		 ('I','B',1),]

for edge in edges:
	graph.add_edge(*edge)		
def dijsktra(graph, initial, end):
	# shortest paths is a dict of nodes
	# whose value is a tuple of (previous node, weight)
	shortest_paths = {initial: (None, 0)}
	current_node = initial
	visited = set()
	
	while current_node != end:
		visited.add(current_node)
		destinations = graph.edges[current_node]
		weight_to_current_node = shortest_paths[current_node][1]

		for next_node in destinations:
			weight = graph.weights[(current_node, next_node)] + weight_to_current_node
			if next_node not in shortest_paths:
				shortest_paths[next_node] = (current_node, weight)
			else:
				current_shortest_weight = shortest_paths[next_node][1]
				if current_shortest_weight > weight:
					shortest_paths[next_node] = (current_node, weight)
		
		next_destinations = {node: shortest_paths[node] for node in shortest_paths if node not in visited}
		if not next_destinations:
			return "Route Not Possible"
		# next node is the destination with the lowest weight
		current_node = min(next_destinations, key=lambda k: next_destinations[k][1])
	
	# Work back through destinations in shortest path
	path = []
	while current_node is not None:
		path.append(current_node)
		next_node = shortest_paths[current_node][0]
		current_node = next_node
	# Reverse path
	path = path[::-1]
	return path


break_i = 0

while super.step(timestep) != -1:
	counter = 0
	settrajectory()
	flag = 0
	
	for global_counter in range(0,3):
		
		distance = calcdist(x[global_counter],waypoints[tk][0],z[global_counter],waypoints[tk][1])
		myangle = phi[global_counter] * 180/np.pi
		if myangle < 0:
			myangle = myangle + 360
		calculateCorner(x[global_counter],z[global_counter],phi[global_counter])

		K = x[global_counter]
		closest_numberX = XC[min(range(len(XC)), key = lambda i: abs(XC[i]-K))]
		closest_indexX = np.where(XC == closest_numberX)
		
		K = z[global_counter]
		closest_numberZ = ZC[min(range(len(ZC)), key = lambda i: abs(ZC[i]-K))]
		closest_indexZ = np.where(ZC == closest_numberZ)	
		start = time.time()
		X = np.array([-2.5,2.5,2.5,-2.5]) 
		Y = np.array([-1,-1,1,1]) 
		th = phi[global_counter]

		RX = X * np.cos(th) - Y * np.sin(th)
		RY = X * np.sin(th) + Y * np.cos(th)
		TX = RX + x[global_counter]
		TY = RY + z[global_counter]
		xmin = math.floor(min(TX));
		ymin = math.floor(min(TY));
		xmax = math.ceil(max(TX));
		ymax = math.ceil(max(TY));

		CX = []
		CY=[]
		for i in range(xmin-1,xmax+1):
			for j in range(ymin-1,ymax+1):
				CX.append(i+0.5)
				CY.append(j+0.5)
				
		for i in range(len(CX)):
			temp =  'x' + str(int(CX[i])) + 'z'+ str(int(CY[i]))
			if temp in tempdict:
				tempdict[temp].append(round(super.getTime()+0.1*global_counter,1))
			else:
				tempdict[temp] = [round(super.getTime()+0.1*global_counter,1)] 
		
		# for i in range(closest_indexZ[0][0] - 3,closest_indexZ[0][0] + 3):
		# 	for j in range(closest_indexX[0][0] - 5,closest_indexX[0][0] + 5):
		# 		intersection(x[global_counter],z[global_counter],carw,carl,myangle,XC[j],ZC[i],w,w,0)
		
		refangle = getbearing(x[global_counter],waypoints[k][0],z[global_counter],waypoints[k][1])
		err = error(myangle,refangle)
		steer = applyPID(err)
		rad = steer * np.pi/180

		x[global_counter + 1] = x[global_counter] + v * (timestep/1000.0) * np.sin(phi[global_counter])
		z[global_counter + 1] = z[global_counter] + v * (timestep/1000.0) * np.cos(phi[global_counter])
		phi[global_counter + 1] = phi[global_counter] + (v/4) * (timestep/1000.0) * np.tan(rad) 


		if distance<1.35:
			tk = tk + 1
				
	x[0] = x[1]
	z[0] = z[1]
	phi[0] = phi[1]

	for item in tempdict:
		mydict[item] = [min(tempdict[item])]
		if max(tempdict[item]) != min(tempdict[item]):
			mydict[item].append(max(tempdict[item])) 
	
	for item in mydict:
		values = mydict[item]
		if len(values) == 2:
			sendmessage(car_id,item,values[0],values[1])
	
	receivemessage()
	
	for name in outerdict_zones:
		for zone in outerdict_zones[name]:
			if zone in mydict:
				if mydict[zone][0] < outerdict_zones[name][zone][1]:
					tempadv(zone,name)
	key = None
	if bool(disadv) == True:
		positions = []
		min_value = float("inf")
		for k1,v1 in disadv.items():
			if v1[0] != car_id:
				positions.append(k1)
		dist = []
		for pos in positions:
			intx = ''
			intz = ''
			for i in range(0,len(pos)):
				if pos[i] == 'z':
					posz = i

			for i in range(1,posz):	
				intx = intx + pos[i]

			for i in range(posz+1,len(pos)):
				intz = intz + pos[i]
			intx = int(intx)
			intz = int(intz)
			dist.append(calcdist(tx,intx,tz,intz))

		key = positions[dist.index(min(dist))]
	if key != None:
		print("Motion Planner called for zone:" + str(key))
		v = motionplanning(key)
		flag = 1
		print(v)

	if flag == 1:
		disadv.clear()
		mydict.clear()
		tempdict.clear()
	outerdict_zones.clear()
	
