import numpy as np
import cv2
from  motorm import turn
from time import sleep

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


#------------------- keep red inRange --------------
def MASK3(img):
    img1=np.copy(img)
    img1=cv2.resize(img1,(320,240))
    hsv=cv2.cvtColor(img1,cv2.COLOR_BGR2HSV)
    mask1=cv2.inRange(hsv,lower,upper)
    Y=img1.shape[0]-1; 

    #low2 = np.array([0,70,50]); upp2 = np.array([10,255,255])
    #mask3=cv2.inRange(hsv,low2,upp2)
    #mask2=mask1 | mask3
    for x in range(img1.shape[1]):
        print(x,img1[Y,x,:],hsv[Y,x,:],mask1[Y,x])
        #print(x,img1[Y,x,:],hsv[Y,x,:],mask2[Y,x],mask1[Y,x],mask3[Y,x])

    mask2=np.copy(mask1)
    cv2.imshow('mask2',mask2)
    img2=cv2.bitwise_and(img1, img1, mask=mask2)
    contours,hierarchy=cv2.findContours(mask2, cv2.RETR_EXTERNAL,
                cv2.CHAIN_APPROX_NONE)
    img3=np.copy(img2)
    box=(0,0,0,0)
    if len(contours) != 0:
        c=max(contours, key = cv2.contourArea)
        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



def ptext(img1,s,pos,sz):
    img2=cv2.putText(img=img1,text=s,org=pos,
      fontFace = cv2.FONT_HERSHEY_DUPLEX,
      fontScale=sz,color=(125,246,55),thickness=1)
    return img2

img=np.zeros((200,600,3),dtype=np.uint8)
for j in range(1):
    B=50; G=10; R=200
    bgr=np.array([50,10,200], dtype=np.uint8)
    img[0:100,:]=bgr
    bgr2=np.array([50,70,200], dtype=np.uint8)
    img[100:200,:]=bgr2
    hsv=cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
    cv2.imshow('img',img)
    cv2.imwrite('RED_2thr.png',img)
    cv2.waitKey(0)
input('0000')

FW=open('red_A.txt','w')
img=np.zeros((200,600,3),dtype=np.uint8)
for j in range(5):
    fl='red_'+str(j)+'.png'
    B=50; G=10+20*j; R=200
    bgr=np.array([B,G,R], dtype=np.uint8)
    img[:,:]=bgr
    hsv=cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
    a=str(j)+' '+str(bgr)+' '+str(hsv[0,0,:])
    img2=ptext(img,a,(30,30),0.5)
    print(a); FW.write(a+'\n')
    cv2.imshow('img2',img2)
    cv2.imwrite(fl,img)
    cv2.waitKey(0)
FW.close()
input('1111')


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)
sleep(2)
for j in range(10):
    fl='red_'+str(j)+'.png'
    ret,img=cap.read()
    mask2,img3,box=MASK3(img)
    cv2.waitKey(0)
input('1111')



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
        #else: print(j,'fail dr=',dr)
        if(dr>0 and w>20*2 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); 
    if(method==1):
        DR=dr
    a=str(([j,DR,dr,DRS],[lred,w,h],[midl,MIDL,Y],sh))
    print(a)
    FW.write(a+'\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): 
        ret,imgt=cap.read()
        img1ts=cv2.resize(imgt,(320,240))
        cv2.imshow('img1ts',img1ts)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        cv2.waitKey(0)
        turn(0.4,0.4,0.30); turn(0,0,0.05);
        for t in range(20):
            ret,imgt=cap.read()
            drt,lredt,midlt,sht,img1t=RED_BOTH(imgt)
            maskt,img3t,boxt=MASK3(imgt)
            img1ts=cv2.resize(img1t,(320,240))
            cv2.imshow('img1ts',img1ts)
            if cv2.waitKey(1) & 0xFF == ord('q'): break
            print(t,drt,lredt,midlt,sht,boxt)
            #if(drt==1 and boxt[2]<80): 
            if(drt==1): 
                cv2.destroyAllWindows()
                break
            turn(-0.4,0.4,0.05); turn(0,0,0.15)
        cv2.destroyAllWindows()

    if(DR==0): turn(0.0,0.0,0.10); turn(0,0,0.05);
    if(DR==17): 
        ret,imgt=cap.read()
        img1ts=cv2.resize(imgt,(320,240))
        cv2.imshow('img1ts',img1ts)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        cv2.waitKey(0)
        turn(0.4,0.4,0.30); turn(0,0,0.05);
        for t in range(20):
            ret,imgt=cap.read()
            drt,lredt,midlt,sht,img1t=RED_BOTH(imgt)
            maskt,img3t,boxt=MASK3(imgt)
            img1ts=cv2.resize(img1t,(320,240))
            cv2.imshow('img1ts',img1ts)
            if cv2.waitKey(1) & 0xFF == ord('q'): break
            print(t,drt,lredt,midlt,sht,boxt)
            #if(drt==1 and boxt[2]<80): 
            if(drt==1): 
                cv2.destroyAllWindows()
                break
            turn(0.4,-0.4,0.05); turn(0,0,0.15)
        cv2.destroyAllWindows()

    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()