from time import sleep
import cv2
import numpy as np
from PARAM import *
from motorm import turn
from LOCATE1 import locate
from LOCATE_goal import locate_g
from UturnBK import UturnBK
from DIST import distance1

def moving(DR):
    if(DR==0): turn(0.0,0.0,0.05); turn(0,0,0.0)
    if(DR==9): turn(0.0,0.0,0.05); turn(0,0,0.0)
    if(DR==1): turn(0.3,0.3 ,0.08); turn(0,0,0.0)
    if(DR==3): turn(-0.3,0.3,0.08); turn(0,0,0.0)
    if(DR==7): turn(0.3,-0.3,0.08); turn(0,0,0.0)

def FXGO2(cap):
    t=0
    LTX=640*0.3; RTX=640*0.7
    for j in range(400):
        dis1 = distance1()
        if(dis1<20): 
            turn(0.5,0.0,0.6); turn(0.0,0.5,0.8)
            #break
        ret, frame = cap.read()
        if(ret==False): break
        #cv2.imshow('frame',frame)
        img2=np.copy(frame)
        NN,xs,ys,WDS,img24=locate_g(img2)
        cv2.imshow('img24',img24)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        if(NN==0): DR=0
        else:
            if(xs[0]<LTX): DR=3
            elif(LTX<=xs[0]<=RTX): DR=1
            else: DR=7
        print(t,NN,xs,ys,WDS,xs,' DR=',DR)
        if(NN>0 and max(ys)>400 and max(WDS) > 70): 
            print('STOP max(ys,WDS)=',max(ys),max(WDS)); DR=9; break
        moving(DR)
        #turn(0.3,0.3,0.1); turn(0,0,0.1)
        #sleep(1)
        t+=1