import numpy as np
import cv2
from  motorm import turn

Lx=640; Ly=480; F=4; LX2=int(Lx/F); LY2=int(Ly/F)
PIN=[171, 188, 184]
lower = np.array([PIN[0]-40,PIN[1]-100,120])
upper = np.array([PIN[0]+20,255,255])
lower = np.array(lower, dtype="uint8")
upper = np.array(upper, dtype="uint8")
print('lower=',lower,' upper=',upper)

def RED_BOTH(img):
    img1=np.copy(img)
    img2=cv2.resize(img1,(LX2,LY2))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    Y=LY2-1; XM=int(LX2/2); red=[]; sh=[0,0,0]
    for x in range(LX2):
        if(lower[0]<hsv[Y,x,0]<upper[0] and lower[1]<hsv[Y,x,1]<upper[1]): 
            red.append(x)
            for t in range(3): sh[t]+=hsv[Y,x,t] 
    lred=len(red)
    if(lred>0):
        for t in range(3): sh[t]=int(sh[t]/lred)
    if(lred > 0):
        midl=int(np.median(red))
        cv2.line(img1,(midl*F,0),(midl*F,Y*F),(0,255,0),2)
        if(60<=midl<=100): dr=1
        elif(100<midl<160): dr=7
        elif(0<=midl<60): dr=3
    else: 
        midl=-1; dr=-2
    return dr,lred,midl,sh,img1


#------------------- keep red inRange --------------
def MASK3(img):
    mlow=int(120/2)
    img1=np.copy(img)
    img1=cv2.resize(img1,(160,120))
    hsv=cv2.cvtColor(img1,cv2.COLOR_BGR2HSV)
    mask2=cv2.inRange(hsv,lower,upper)
    mask2[0:mlow,:]=0
    img2=cv2.bitwise_and(img1, img1, mask=mask2)
    #ret,thresh = cv2.threshold(mask, 40, 255, 0)
    thresh=np.copy(mask2)
    contours,hierarchy=cv2.findContours(thresh, cv2.RETR_EXTERNAL,
                cv2.CHAIN_APPROX_NONE)
    #print('lencont=',len(contours))
    #for t in contours: print(cv2.contourArea(t))
    img3=np.copy(img2)
    box=(0,0,0,0)
    if len(contours) != 0:
        c=max(contours, key = cv2.contourArea)
        #print('max_c_area=',cv2.contourArea(c))
        cv2.drawContours(img2,c,-1,(0,255,0),2)
        x,y,w,h = cv2.boundingRect(c)
        img3=cv2.rectangle(img3,(x,y),(x+w,y+h),(255,0,0),2)
        #cv2.imshow('img3',img3)
        box=(x,y,w,h)
    return mask2,img3,box

method=2
Ymin=120-50
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly)
FW=open('GL.txt','w')
for j in range(140):
    fl='GL_'+str(j)+'.png'
    ret,img=cap.read()
    #img=cv2.imread('G1.png')
    imgA=np.copy(img)
    imgA=cv2.resize(imgA,(160,120))
    mask2,img3,box=MASK3(img)
    x,y,w,h=box
    MIDL=x+int(w/2); Y=y+h
    imgA=cv2.circle(imgA,(MIDL,Y),5,(255,0,0),-1)
    dr,lred,midl,sh,img1=RED_BOTH(img)
    #print('dr,lred,midl,sh=',dr,lred,midl,sh)
    res2=cv2.bitwise_and(imgA,imgA,mask=mask2)
    img1=cv2.resize(img1,(160,120))
    img3=cv2.resize(img3,(160,120))
    hres=np.hstack((imgA,img1,img3))
    hstk1=cv2.resize(mask2,(320,240))
    hstk2=cv2.resize(hres,(960,240))
    #cv2.imshow('hstk1',hstk1)
    cv2.imshow('hstk2',hstk2)
    #cv2.imshow('img3',img3)
    cv2.imwrite(fl,hstk2)
    if(0<MIDL<60): DRS=3
    elif(60<=MIDL<=100): DRS=1
    elif(100<MIDL): DRS=7
    else: DRS=-2
    if(method==2):
        DR=DRS
        KK=0
        if(dr>0): KK=1; DR=dr
        if(dr>0 and w>80 and h<40): 
            KK=0
            if(MIDL<midl-5): DR=13
            if(MIDL>midl+5): DR=17
        Sbox=np.sum(box)
        if(Sbox==0): DR=-2;print('vacant box=',box,'Sbox=',Sbox,' DR=',DR); 
        if(Y<Ymin): DR=-2;print('ESCAPE from Y,Ymin=',Y,Ymin,' DR=',DR); 
        #a=STR(KK)+str((j,[DR,dr,DRS],[lred,w,h],[midl,MIDL]))
        #print(KK,'j=',j,DR,MIDL,(w,lred),' Y=',(Y,Ymin),(dr,lred,midl),h)
    if(method==1):
        #print(method,'j=',j,'DR=',DR,' dr=',(dr,lred,midl))
        DR=dr
    aj=str((j,[method,KK],[DR,dr,DRS],[lred,w,h],[midl,MIDL]))
    print(aj)
    FW.write(aj+'\n')
    cv2.waitKey(0)
    if cv2.waitKey(1) & 0xFF == ord('q'): break
    #DR=0
    if(DR==1): turn(0.3,0.3,0.10); turn(0,0,0.05);
    if(DR==3): turn(0.0,0.4,0.10); turn(0,0,0.05);
    if(DR==7): turn(0.4,0.0,0.10); turn(0,0,0.05);
    if(DR==13): turn(0.3,0.3,0.2); turn(-0.4,0.4,0.10); turn(0,0,0.05);
    if(DR==17): 
        turn(0.3,0.3,0.4); turn(0,0,0.05);
        for t in range(20):
            turn(0.4,-0.4,0.05); turn(0,0,0.05);
            ret,imt=cap.read()
            dr,lred,midl,sh,imgt=RED_BOTH(imt)
            mask2,img3,box=MASK3(imt)
            print('DR=',DR,' t=',t,dr,lred,midl,box)
            imgt2=cv2.resize(imgt,(320,240))
            cv2.imshow('imgt2',imgt2)
            cv2.waitKey(0)
            if(dr==1 and box[2]<90): cv2.destroyAllWindows(); break
    if(DR==0): turn(0.0,0.0,0.10); turn(0,0,0.05);
    if(DR==-2): turn(-0.3,-0.3,0.05); turn(0,0,0.05);

FW.close()