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

Lx=640; Ly=480; F=4; LX2=int(Lx/F); LY2=int(Ly/F)
#建立兩個紅色遮罩的面具所需要的顏色範圍
low1=np.array([160,60 ,50 ])
upp1=np.array([180,255,255])
low2=np.array([0,  70, 50 ])
upp2=np.array([10, 255,255])
print('low1=',low1,upp1,' low2=',low2,upp2)
FW2=open('KBS.txt','w')

cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480)
fourcc = cv2.VideoWriter_fourcc(*'XVID')
out = cv2.VideoWriter('redM.avi', fourcc, 20.0, (640, 480))
sleep(2)

#---------------------從hsv的編碼決定底線的紅色的像素
def RED(img):
    img1=np.copy(img)
    imgA=cv2.resize(img1,(320,240))
    img2=cv2.resize(img1,(80,60))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    mask1=cv2.inRange(hsv,low1,upp1)
    mask2=cv2.inRange(hsv,low2,upp2)
    mask3=mask1 | mask2
    mlow=int(60/2); mask3[0:mlow,:]=0; 
    red=[]; red2=[]; red3=[]; Y=60-1; blk=[]
    Y1=59; Y2=49; Y3=39
    for x in range(80): 
        if(mask3[Y1,x]>100): red.append(x)
        if(mask3[Y2,x]>100): red2.append(x)
        if(mask3[Y3,x]>100): red3.append(x)
        if(hsv[Y2,x,2]<50): blk.append(x)
        #print(x,img2[Y3,x,:],hsv[Y3,x,:])
    #print('blk=',blk)
    if(len(blk)>10): 
        cv2.line(imgA,(0,Y3*4),(319,Y3*4),(0,255,255),2)
        return -100,0,0,0,0,0,0,imgA
    lred1=len(red); lred2=len(red2); lred3=len(red3)
    if(lred1>0): mid1=int(np.median(red))
    else: mid1=-1; 
    if(lred2>0): mid2=int(np.median(red2))
    else: mid2=-1
    if(lred3>0): mid3=int(np.median(red3))
    else: mid3=-1
    #--------------------drdrdr------
    dr=-10
    dr=1
    if(mid1 == -1 and mid2 == -1 and mid3<6): dr=3
    if(mid1==-1 and mid3-mid2>20): dr=71
    if(mid1>1 and mid2>mid1+12 or mid3>mid1+22): dr=72
    if(mid1>12 and mid2>mid1+4 and mid3>10): dr=73
    if(mid1>30 and mid2>mid1+4): dr=74
    #if(mid1>30 and mid2 == -1 and mid3 == -1): dr=75

    '''
    if(9<mid2-mid1<13 or 9<mid3-mid2<13): dr=1
    if(mid1==-1): 
        if(11<mid3-mid2<16): dr=1
        if(mid2<30 and mid3-mid2>10): dr=7
    if(mid1 != -1): 
        if(mid2-mid1>10 and mid3-mid2>10): dr=7
    if(mid1==-1 and mid2==-1 and 10<mid3<20): dr=1 
    if(mid1==-1 and mid2==-1 and mid3<=10): dr=3 
    if(mid1 != -1 and mid2 != -1 and mid3 != -1):
        if(9<mid2-mid1<13 or 9<mid3-mid2<13): dr=1
    if(0<mid1<15 and 0<mid2<30):
        if(mid2-mid1>7): dr=1
        elif(0<mid2-mid1<=7): dr=3
    if(mid1>15 and mid2>30): dr=7
    if(mid1<15 and abs(mid2-mid1)<5 and abs(mid3-mid1)<5): dr=3
    '''

    #print('mid1,2=',mid1,mid2,lred1,lred2)
    a=[mid1,mid2,mid3,lred1,lred2,lred3]; sa=str(a)
    if(dr==-10): print('reset dr=-10',sa); FW2.write(sa+'\n'); dr=1
    mask3=cv2.resize(mask3,(320,240))
    redmask=cv2.bitwise_and(imgA,imgA,mask=mask3)
    cv2.circle(imgA,(mid1*4,Y1*4),8,(0,255,0),-1)
    cv2.circle(imgA,(mid2*4,Y2*4),8,(0,255,0),-1)
    cv2.circle(imgA,(mid3*4,Y3*4),8,(0,255,0),-1)
    cv2.line(imgA,(0,Y3*4),(79*4,Y3*4),(0,255,0),2)
    cv2.circle(redmask,(mid1*4,Y1*4),8,(0,255,0),-1)
    cv2.circle(redmask,(mid2*4,Y2*4),8,(0,255,0),-1)
    cv2.circle(redmask,(mid3*4,Y3*4),8,(0,255,0),-1)
    cv2.imshow('imgA',imgA)
    cv2.imshow('redmask',redmask)
    #cv2.waitKey(0)
    return dr,mid1,mid2,mid3,lred1,lred2,lred3,img1



KB=open('KB.txt','w')
break_program = True
sp='A'
def on_press(key):
    global break_program
    global sp
    sp=str(key).strip("'")
    #KS=str(t)+'  '+str(key).strip("'")
    #print ('t=',t,'  key=',key,KS,'  sk=',str(key),break_program)
    #KB.write(KS+'\n')
    if sp == 'w' and break_program:
        turn(0.3,0.3,0.1); turn(0,0,0.1)
    if sp == 's' and break_program:
        turn(-0.3,-0.3,0.1); turn(0,0,0.1)
    if sp == 'a' and break_program:
        turn(0.0,0.3,0.1); turn(0,0,0.1)
    if sp == 'd' and break_program:
        turn(0.3,0.0,0.1); turn(0,0,0.1)
    if key == keyboard.Key.f1 and break_program:
        print ('end pressed')
        break_program = False
    #print(type(sp),'sp=',sp)

listener =  keyboard.Listener(on_press=on_press)
listener.start()


#=====================KKKKKKKKKKKKKKKKKK
if(1==2):
    print('into KY_RED...')
    t=0
    while(t < 50 and cap.isOpened()):
        ret,img=cap.read(); 
        if ret == False: break
        #out.write(img)
        dr,mid1,mid2,mid3,lred1,lred2,lred3,img1=RED(img)
        ta=[mid1,mid2,mid3,lred1,lred2,lred3]
        ws=str(t)+' '+sp+' '+str(ta)
        if(t>0): print(ws)
        KB.write(ws+'\n')
        cv2.imshow('img',img)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        cv2.waitKey(0)
        t+=1
    cap.release()
    out.release()
    cv2.destroyAllWindows()
    KB.close()
    input('sssss')








#---------使用兩個紅色遮罩尋找紅色軌跡，從而決定行走的方向DRS
#------------------- keep red inRange --------------
def MASK3(img):
    Y=LY2-1; 
    mlow=int(120/2)
    img1=np.copy(img)
    img1=cv2.resize(img1,(160,120))
    hsv=cv2.cvtColor(img1,cv2.COLOR_BGR2HSV)
    mask1=cv2.inRange(hsv,low1,upp1)
    mask3=cv2.inRange(hsv,low2,upp2)
    mask2=mask1 | mask3
    mask2[0:mlow,:]=0
    img2=cv2.bitwise_and(img1, img1, mask=mask2)
    thresh=np.copy(mask2)
    contours,hierarchy=cv2.findContours(thresh, 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)
        if(cv2.contourArea(c)>40):
            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)
            box=(x,y,w,h)
            MIDL=x+int(w/2); Y=y+h
            if(0<MIDL<60): DRS=3
            elif(60<=MIDL<=100): DRS=1
            elif(100<MIDL): DRS=7
            else: DRS=-2
        else: DRS=-2; #input('aaaa')
    else: DRS=-2
    cv2.imshow('img3',img3)
    return DRS,mask2,img3,box

#-------------------車輛迴轉，當紅色最大輪廓的中心點在中間時，結束迴轉的迴圈
def QUT(cap):
    turn(-0.2,0.7,0.5); turn(0.3,0.3,1.6); turn(0,0,0.01);
    for j in range(100):
        ret,img=cap.read()
        #DRU,mask2,img3,box=MASK3(img)
        DRU,mid1,mid2,mid3,lred1,lred2,lred3,img1=RED(img)
        if(int(DRU/10)==7): DRU=7
        print('QUT,j=',j,DRU,mid1,mid2,mid3)
        img3s=cv2.resize(img1,(320,240))
        cv2.imshow('UT',img3s)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        cv2.waitKey(0)
        if(DRU==1 or DRU==7): return
        turn(-0.3,0.5,0.08); turn(0,0,0.05);
    print('Unfortunate QUT()...')
    return


def QLT(cap):
    #turn(-0.2,0.7,0.5); turn(0.3,0.3,1.0); turn(0,0,0.01);
    for j in range(100):
        ret,img=cap.read()
        DRU,mid1,mid2,mid3,lred1,lred2,lred3,img1=RED(img)
        if(int(DRU/10)==7): DRU=7
        print('QLT,j=',j,DRU,mid1,mid2,mid3)
        img3s=cv2.resize(img1,(320,240))
        cv2.imshow('LT',img3s)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
        if(DRU==1 or DRU==7): return
        turn(-0.3,0.3,0.05); turn(0,0,0.10);
    print('Unfortunate QLT()...')
    return






# method=1 僅使用底邊的hsv紅色決定行駛方向dr
# method=2 同時使用紅色底邊dr和最大輪廓DRS兩種行駛方向，共同決定最優的行駛方向, DR
method=2
Ymin=120-50
ret,img=cap.read()
#cv2.imshow('img_in',img)
#-------------------RRRR------
if(1==1):
    FW1=open('redM.txt','w')
    for j in range(1520):
        ret,img=cap.read()
        #cv2.imshow('imgin',img)
        DRj,mid1,mid2,mid3,lred1,lred2,lred3,img1=RED(img)
        cv2.imshow('img1',img1)
        a=[j,DRj,mid1,mid2,mid3,lred1,lred2,lred3]; sa=str(a)
        DR=DRj
        if(int(DR/10)==7): DR=7
        print('j=',j,DRj,DR,mid1,mid2,mid3,lred1,lred2,lred3)
        FW1.write(sa+'\n')
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
        #if(j==0): QUT(cap)
        #DR=0
        if(DR==-100): QUT(cap)
        if(DR==-10): turn(0.0,0.0,0.10); turn(0,0,0.05);
        if(DR==0): turn(0.0,0.0,0.10); turn(0,0,0.05);
        if(DR==1): turn(0.3,0.3,0.10); turn(0,0,0.00);
        if(DR==3): 
            QLT(cap)
            #turn(0.4,0.4,0.30); turn(-0.4,0.4,0.15); turn(0,0,0.05);
        if(DR==7): turn(0.4,0.0,0.10); turn(0,0,0.10);
        if(DR==-2): turn(-0.3,-0.3,0.05); turn(0,0,0.05);
    FW1.close()
    FW2.close()
    cv2.destroyAllWindows()
    cap.release()
    input('2222')


















DRS,mask2,img3,box=MASK3(img)
cv2.waitKey(0)

FW=open('GL.txt','w')
for j in range(440):
    fl='GL_'+str(j)+'.png'
    ret,img=cap.read()
    #img=cv2.imread('G1.png')
    imgA=np.copy(img)
    cv2.imshow('imgA1',imgA)
    #imgA=cv2.resize(imgA,(160,120))
    #--------------------------------
    DRS,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)
    if(dr==100): print('QUT dr=',dr); sleep(0.5); QUT(cap); continue
    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)
    if cv2.waitKey(1) & 0xFF == ord('q'): break
    cv2.imwrite(fl,hstk2)
    if(method==2):
        DR=DRS
        KK=0
        #只要dr > 1 表示底邊可以看見紅色，就接受底邊的判斷
        if(dr>0): KK=1; DR=dr 
        #else: print(j,'fail dr=',dr)
        #雖然底邊可以看見紅色區塊，但是輪廓顯示矩形的寬度非常寬，表示
        #底線前方的軌跡發生大角度的轉彎，因此要做特殊的處理
        #正常情況軌道的寬度只有20px，如果大於40就是大轉彎
        if(dr>0 and w>20*2 and h<40): 
            KK=0
            if(MIDL<midl-5): DR=13 #大角度的左轉定義為DR=13
            if(MIDL>midl+5): DR=17 #大角度的右轉定義為DR=17
        Sbox=np.sum(box) #Sbox=0 表示整個圖像中沒有足夠大的紅色區塊應該要後退,DR=-2
        if(Sbox==0):DR=-2;print('vacant box=',box,Sbox);#sleep(20);break 
        #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)
    #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);
    #DR=13 or DR=17 大角度的轉彎
    if(DR>10): print('*******大角度轉彎steep turn, DR=',DR)
    if(dr==-2 and DR!=-2):print('advantage:DR,dr,DRS=',DR,dr,DRS); sleep(1)
    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)
            DRSt,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)
            DRSt,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()