import cv2
from RED2 import RED2
from motorm import turn
from P1Uturn1 import Uturn

def moving(DR):
    if(DR==1): turn(0.3,0.3,0.05); turn(0,0,0.01)
    if(DR==3): turn(-0.2,0.4,0.05); turn(0,0,0.05)
    if(DR==7): turn(0.4,-0.2,0.05); turn(0,0,0.05)
    if(DR==-2): turn(0.2,-0.2,0.1); turn(0,0,0.05)

FL=open('/home/pi/P1-RED/CAR.txt','r')
car=FL.read()
car=int(car)
print('car=',car,type(car))

Lx2=80; Ly2=60; F=8; Lx=Lx2*F; Ly=Ly2*F
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx/2)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly/2)
t=0; DRS=[]; NBK=0
while(car > 1 and t < 201 and cap.isOpened()):
    if(NBK>5): DRS=Uturn(cap)
    ret, frame = cap.read(); t+=1
    if ret == False: break
    #cv2.imshow('frame',frame)
    if cv2.waitKey(1) & 0xFF == ord('q'): break
    DR,LRED,MIDL,img1,img2=RED2(0,frame)
    DRS.append(DR)
    TAIL=DRS[-10:-1]
    NBK=TAIL.count(-2)
    if(t%20==0): print('car,t,DR,NBK=',car,t,DR,NBK)
    moving(DR)
cap.release()
cv2.destroyAllWindows()