import time
from time import sleep
import cv2
import numpy as np
from PARAM import *
from motorm import turn
from LOCATE_goal import locate_g

def SGETDR(xs0):
    if(xs0<35): DR=3
    elif(35<=xs0<=45): DR=1
    else: DR=7
    if(xs0==0): DR=-2
    return DR

def GETDR(xs0):
    if(xs0<30): DR=3
    elif(30<=xs0<=50): DR=1
    else: DR=7
    if(xs0==0): DR=-2
    return DR

def aimgoal(KSHOW,cap):
    maxsec=0
    print('In aimgoal func....')
    for i in range(9990):
        ret, frame = cap.read()
        if(ret==False): print('ret False break...'); break
        img2=np.copy(frame)
        NN,xs,ys,WDS,img24=locate_g(img2)
        if(NN==0): xs0=0; ys0=0; WDS0=0
        elif(NN==1): xs0=xs[0]/F; ys0=ys[0]/F; WDS0=WDS[0]/F
        else: xs0=np.min(xs)/F; ys0=ys[0]/F; WDS0=WDS[0]/F
        DR=GETDR(xs0)
        print('560:',i,NN,xs0,ys0,WDS0,' DR=',DR)
        if(DR==1): break
        if(KSHOW==1):
            cv2.imshow('img24',img24)
            #cv2.waitKey(0)
            if cv2.waitKey(1) & 0xFF == ord('q'): break
        turn(0.5,-0.5,0.06); turn(0,0,0.05)
    NN,xs,ys,WDS,img24=locate_g(img2)
    if(ys[0]/F<36): turn(0.3,0.3,0.3); turn(0,0,0.01)
    print('aimgoal_end1:',NN,xs,ys,WDS)
    #sleep(5)
    maxsec=0
    for i in range(560):
        ret, frame = cap.read()
        if(ret==False): print('562_2, ret=',ret); break
        img2=np.copy(frame)
        NN,xs,ys,WDS,img24=locate_g(img2)
        #print('i=',i,NN,xs,ys,WDS)
        if(KSHOW==1):
            cv2.imshow('img24',img24)
            #cv2.waitKey(0)
            if cv2.waitKey(1) & 0xFF == ord('q'): break
        if(NN==0): xs0=0; ys0=0; WDS0=0; maxsec=0
        else:
            xsm=np.median(xs)
            xs0=xsm/F; ys0=np.max(ys)/F; WDS0=WDS[0]/F
            maxsec=WDS[-1]
        if(maxsec>10): 
            print('maxsec_final=',maxsec);
            sleep(4)
            break
        DR=SGETDR(xs0)
        if(DR==1): turn(0.3,0.3,0.1); turn(0,0,0.01)
        if(DR==-2): turn(-0.3,-0.3,0.05); turn(0,0,0.05)
        if(DR==3): turn(-0.3,0.3,0.05); turn(0,0,0.05)
        if(DR==7): turn(0.3,-0.3,0.05); turn(0,0,0.05)
        print(i,NN,xs0,ys0,WDS0,' DR=',DR,' maxsec=',maxsec)
    print('aimgoal_end2:',NN,xs,ys,WDS)