# data[].attributes.body
print(frontLidar)

if L[2] < 235:
steer = 90
if R[2] < 235:
steer = -90

if V[3] <= 195 and R[1] == 321 and abs(V[3] - V[4]) < 13:
steer = 99

if V[3] <= 195 and L[1] == 320 and abs(V[3] - V[2]) < 13:
steer = -99

if V[3] <= 185 and V[0] < V[1] < V[2] < V[3] and abs(V[3] - V[5]) > 40:
steer = 99
if V[3] <= 185 and V[6] < V[5] < V[4] < V[3] and abs(V[3] - V[1]) > 40:
steer = -99

if L[1] == 320 and R[1] == 321 and L[2] == 320 and R[2] == 321:
steer = 15

if frontLidar < 5700 and L[0] < L[1] < L[2]:
steer = 99
if frontLidar < 5700 and R[0] < R[1] < R[2]:
steer = -99

# data[].attributes.body
p = linecast(canny, (320, 479), (320, 0))
x= p[0]
y = p[1]

if L[2] < 319 and R[2] < 319 :          #L[2] 와 R[2]가 있으면
if L[2] < 254 :
steer = 254 - L[2]
elif R[2] < 254 :
steer = R[2] - 254
else :
steer = 0
elif L[2] < 319 :                       #L[2] 있고, R[2] 없으면
if L[2] < 254 :
steer = 254 - L[2]
elif R[2] < 319:
if R[2] < 254:                  #R[2] 있고, L[2] 없으면
steer = R[2] - 254

if V[6] < 125 and V[1] < 175 and 110 < V[4] < 190 and R[0] > 319 :                        # 직각 좌회전
if R[1] < 270 :
steer = - 100
if V[0] < 125 and V[5] < 175 and 110 < V[2] < 180 and L[0] > 319 :                        # 직각 우회전
if L[1] < 270 :
steer = + 100

if L[2] > 319 and R[2] > 319 and L[1] > 319 and R[1] > 319 and 140 < V[2] < 194 :         #P턴 오른쪽 꺾음
steer = + 58
if L[2] > 319 and R[2] > 319 and L[1] > 319 and R[1] > 319 and 140 < V[4] < 194 :         #P턴 왼쪽 꺽음
steer = - 58

if 1000 < frontLidar < 3200 :
steer = -100
if R[2] > 319 and  R[1] > 319 and L[2] < 190 and V[5] <200 and L[0] >160 and L[1] > 319 :    #왼쪽 차선으로 넘어가야 할때
steer = - 100
if V[6] < 90 and V[5] < 120 and V[4] < 130 and 100 < V[3] < 200 and V[1] < 250 :
steer = -100

# data[].attributes.body
b = L[0]+L[1]+L[2]+R[0]+R[1]+R[2]

C = R[2] - L[2]
C1 = R[1] - L[1]
C0 = R[0] - L[0]
if self.t==62:
self.t = 100
self.x = 0

# print(self.t)
if frontLidar<3500:
steer = -80
if self.x == 0:
self.t = 3
self.x = 1
# print(self.t)

if self.t != 100:

if self.t<=15:
self.t += 1
steer = -60

elif 15<=self.t<=40:
if abs(C)>100:
# print("step 0")
if C < 0:
steer = C + 15
elif C > 0:
steer = C - 15
else:
steer = 0
self.t += 1

elif 55>self.t>=40:
self.t += 1
steer = 60

elif 55<=self.t<62:
steer = -65
self.t += 1

else:
if abs(C)>100:
print("step 0")
if C < 0:
steer = C + 15
elif C > 0:
steer = C - 15
else:
steer = 0

if (R[2]>=320 and (V[0]-V[6]>30) or (V[0]==V[1] or V[1]==V[2])):
print("step 1")
steer = -100+C1+abs(C0)

if (b>=1620 and (V[4] < 190 or 0<V[4]<30)):
print("step 2")
steer = 100

if (L[0]+L[1]>=608 and V[0]>=235 and (V[2]<=85 or V[2]>=235)):
print("step 3")
steer = 100

if (L[0]==320 and R[0]<40 and V[6]>200):
steer += 10
print(L)
print(R)
print(V)
print("==========")

# data[].attributes.body
k = R[2] -L[2]
if k < 30 and k > -30 :
steer = 0

elif k < 0 :
steer = - abs(k * 0.38)
else :
steer = abs(k * 0.38)

#p회전
if R[2] > 300 and L[2] > 300 :
steer = 0
if V[1] < 200 and V[5] > 200 :
steer = 70
if V[1] > 200 and V[5] < 200 :
steer = -70

#직각
if V[1] < 180 and V[3] < 180 and V[5] < 180 :
if V[2] < V[4] :
steer = 95
if V[2] > V[4] :
steer = -95

if frontLidar < 6000 :
steer = -100

# data[].attributes.body
def life(self):
steer = 0
velocity = 250
t = -100
t2 = -100
Ldt = 0
Rdt = 0
lastLider = -100

yield
#######################################################

while True:
r = linecast_debug(self.canny_life, (self.center, 400), (639, 400), self._front_img)[0]
l = linecast_debug(self.canny_life, (self.center, 400), (0, 400), self._front_img)[0]
self.center = (r + l) // 2
d = (r + l) - 640
steer = d / 100
steer *= math.sqrt(abs(steer))
steer *= 25
steer = min(100, max(-100, steer))

#steer += -37
yield (steer, velocity)

#rect
dV = (abs(self.V_life[2] - self.V_life[3]) + abs(self.V_life[3] - self.V_life[4])) / 2
if dV < 20 and self.V_life[3] < 170 and self.time - t > 150 / velocity and self.time - lastLider > 4:
sl = 0
sr = 0
for p in [0, 639]:
maxX = 0
for y in range(200, 400, 30):
x = linecast_debug(self.canny_life, (self.center, 400), (p, y), self._front_img)[0]
x = abs(x - 320) + (400 - y) * 0.1
if x > maxX:
maxX = x
if p == 0:
sl = maxX
else:
sr = maxX

self.dir = sr - sl

t = self.time
while self.time - t < 1.2:
steer = -100 if self.dir < 0 else 100
#steer += -37
yield (steer, velocity)

#dotted
L = []
L_f = 0
R = []
R_f = 0
for p in [0, 639]:
for y in range(100, 400, 15):
x = linecast(self.canny_life, (self.center, 400), (p, y))[0]
if p == 0:
L.append(x)
if len(L) > 1 and abs(L[-2] - x) > 50:
L_f += 1
else:
R.append(x)
if len(R) > 1 and abs(R[-2] - x) > 50:
R_f += 1
L = np.array(L)
R = np.array(R)
Ldt = Ldt - 1 if Ldt > 0 else 0
Rdt = Rdt - 1 if Rdt > 0 else 0
if L_f > 2:
Ldt = 3
if R_f > 2:
Rdt = 3
L_dotted = Ldt > 0
R_dotted = Rdt > 0

if self.frontLider_life < 6000:
lastLider = self.time

if (L_dotted or R_dotted) and not (L_dotted and R_dotted) and self.time - t2 > 2.5 and self.time - lastLider < 10:
if max(self.V_life[2:5]) < 210 and dV < 30:
#print(L_dotted, R_dotted)
t2 = self.time
while self.time - t2 < 0.7:
steer = -100 if L_dotted else 100
#steer += -37
yield (steer, velocity)
while self.time - t2 < 1.2:
steer = 80 if L_dotted else -80
#steer += -37
yield (steer, velocity)
Ldt = 0
Rdt = 0

r = linecast_debug(self.canny_life, (self.center, 400), (639, 250), self._front_img)[0]
l = linecast_debug(self.canny_life, (self.center, 400), (0, 250), self._front_img)[0]
length = r - l
if length > 500 and r > 540 and l < 100:
t = self.time
while self.time - t < 0.5:
yield (steer, velocity)

return

# data[].attributes.body
# [2] 주행 처리
e = 0
L_origin = 200
R_origin = 200

left = L[2] / L_origin
right = R[2] / R_origin

if R[2] == 316:
if L[2] < 325:
e = 210 * (1 - left)
else:
e = 0
elif L[2] == 325:
e = -210 * (1 - right)
else:
e = (right - left) * 200

if V[3] < 160 or V[3] == 255:
if V[0] > V[1]:
e = -360
print("4")

if V[3] < 160 or V[3] == 255:
if V[3] <= V[4]:
e = 340
print("5")
if V[4] <= V[5]:
e = 340
print("6")
velocity = 250

obstacle = 5536

if frontLidar < obstacle:
steer = -100
velocity = 250
print("8")
else:
steer = int(e / 3) - 20
print("9")
if steer > 100:
steer = 100
velocity = 250
elif steer < -100:
steer = -100
velocity = 250