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

RGX=0.06; RN=100; Kshow=1
Lx=640; Ly=480; F=2; Lx2=int(Lx/F); Ly2=int(Ly/F); XLT=Lx2-100; XRT=Lx2+100
WD=0; F8=8; Lx8=int((Lx-2*WD)/F8); Ly8=int(Ly/F8); MIDX=int(Lx8/2)
WM=30; X1=MIDX-WM; X2=MIDX+WM+1; MIX=int((X1+X2)/2)
distx=int(Ly8*0.8); BADISx=int(Ly8*0.8); 
fourcc = cv2.VideoWriter_fourcc(*'XVID')
out=cv2.VideoWriter('LRD.mp4', fourcc, 20.0, (Lx,Ly))
# Green: [ 60 170  90]  # Red:   [170 200 200]
# 21-1A night room # Green: [ 56 184  83] # Red:   [178 220  72]
low1=np.array([60-20,20,10]);upp1=np.array([60+20,255,255]) #G
low2=np.array([170-30,100,100]);upp2=np.array([170+30,255,255]) # R
low3=np.array([0,150,50]);upp3=np.array([20,255,255]) # R
#----21-1A
low1=np.array([56-20,100,40]);upp1=np.array([56+20,255,255]) #G
low2=np.array([178-20,150,50]);upp2=np.array([178+20,255,255]) # R
low3=np.array([0,150,50]);upp3=np.array([20,255,255]) # R


def Target2(low1,upp1,low2,upp2,low3,upp3,image):
    Lxg=Lx; Lyg=Ly; Fg=1
    IMB=np.copy(image)
    img2 = cv2.resize(image, (Lxg,Lyg), interpolation=cv2.INTER_AREA)
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    # R=180; G=60; B=120
    Nac=int(256/10)+1
    ac=[0 for i in range(Nac)]
    #----------------------------------------------
    mask1=cv2.inRange(hsv,low1,upp1)
    mask2=cv2.inRange(hsv,low2,upp2)
    mask3=cv2.inRange(hsv,low3,upp3)
    #----------------------------------------------
    img3=cv2.line(img2,(MIX,0),(MIX,Ly-1),(0,255,255),2)
    tm123=np.hstack((mask1,mask2,mask3))
    tm123=cv2.resize(tm123,(160*3,120))
    cv2.imshow('im123',tm123)
    mask2=mask2 | mask3
    cnts1,_ = cv2.findContours(mask1.copy(), cv2.RETR_EXTERNAL, 
          cv2.CHAIN_APPROX_SIMPLE) # cv2.RETR_LIST
    cnts2,_ = cv2.findContours(mask2.copy(), cv2.RETR_EXTERNAL, 
          cv2.CHAIN_APPROX_SIMPLE) # cv2.RETR_LIST
    lcnts1=len(cnts1); #print('lcnts1=',lcnts1)
    lcnts2=len(cnts2); #print('lcnts2=',lcnts2)
    N1=0; ax=0; cx=0; px=0; mx1=0; Kc1=0; xx1=[0 for i in range(5)]
    clone1=img2.copy(); data1=[]
    HITA=0; ratRG=0
    for c in cnts1:
        M=cv2.moments(c)
        if(M['m00']==0): M['m00']=1
        cX=int(M['m10']/M['m00']); cY=int(M['m01']/M['m00'])
        area = cv2.contourArea(c); area3=round(area,3); #print(j,area3)
        area=int(area)
        if(area<100): continue
        Kc1=1; N1+=1; (x,y,w,h)=cv2.boundingRect(c);
        xx1=(x,y,w,h); mx1=(cX,cY)
        d=[area,xx1,mx1]; data1.append(d)
        cv2.circle(clone1,mx1,6,(0,125,255),-1)
        x,y,w,h=xx1[0],xx1[1],xx1[2],xx1[3]
        cv2.rectangle(clone1,(x,y),(x+w,y+h),(255,125,0),1)
        mk1=mask1[y:y+h,x:x+w]
        y2=y-10; x2=x-10; y3=y+h+10; x3=x+w+10
        if(y-10<0): y2=0
        if(x-10<0): x2=0
        if(y+h+10>mask1.shape[0]): y3=mask1.shape[0]-1
        if(x+w+10>mask1.shape[1]): x3=mask1.shape[1]-1
        mk2=mask2[y2:y3,x2:x3]
        #cv2.imshow('mk2',mk2)
        smk1=np.sum(mk1); smk2=np.sum(mk2); ratRG=round(smk2/smk1,2)
        #print('N1=',N1,' ratRG=',ratRG,smk1,smk2)
        if(ratRG>0.1):
            HITA=1; datag=d 
            dist1=y3
            #if(Kshow==1): cv2.imshow('mk2_'+str(N1),mk2)
            dh=int(h/5)
            cv2.rectangle(clone1,(x-dh,y-dh),(x+w+dh,y+h+dh),(255,0,0),1)
            #print(N1,' ratRG=',ratRG)
            break
    hstack21=np.hstack((img2,clone1))
    if(img2.shape[1]<700):
        imgs=cv2.resize(hstack21,(640,240),interpolation=cv2.INTER_AREA)
    else:
        mx2=int(1920/4)*2; my2=int(1080/4)
        imgs=cv2.resize(hstack21,(mx2,my2),interpolation=cv2.INTER_AREA)
    if(HITA==1):
        #cv2.imshow('mask_stack',hstack1)
        box=[]
        xx2=datag[1]
        for i in range(4):
            box.append(xx2[i]*Fg)
        KKDIS=(xx2[1]+xx2[3])*Fg
        KKDIS=dist1*Fg
        box.append(KKDIS)
        box.append(HITA)
    else: box=[0 for i in range(6)]
    #if(box[5]==0): print('HITA=0',' box=',box,' ratRG=',ratRG)
    return box





def sectionB(yis,LLa,res2):
    res3=np.copy(res2)
    Ln=len(LLa); bdt=[]
    for j in range(Ln):
        lenj=len(LLa[j])
        #print(j,yis[j],lenj,LLa[j])
        if(lenj==0): continue
        nsec=0; sec=[]; x1=0; nx1=LLa[j][x1]; secn=[]
        for i in range(lenj-1):
            if(LLa[j][i+1]-LLa[j][i]>10):
                nsec+=1; x2=i; nx2=LLa[j][x2]; sec.append((x1,x2)); 
                mx=int((nx1+nx2)/2); wx=nx2-nx1;
                secn.append((mx,wx,nx1,nx2)); x1=i+1; nx1=LLa[j][x1]
        nsec+=1;x2=lenj-1; nx2=LLa[j][x2]; mx=int((nx1+nx2)/2);wx=nx2-nx1;
        sec.append((x1,x2)); secn.append((mx,wx,nx1,nx2));
        #print(j,yis[j],lenj,nsec,secn)
        bdt.append((yis[j],nsec,secn))
    #print('LLa=',LLa,'SEC_len=',len(bdt),bdt)
    if(len(bdt)<2): return res2,[]
    pin=1; lenb=len(bdt); yipin=bdt[1][0]; Bdat=[]; NBA=0
    for j in range(bdt[pin][1]):
        midx=bdt[pin][2][j][0]; widx=bdt[pin][2][j][1]
        #print(bdt[pin],midx,widx)
        nk=0; kin=[]; tmidx=0; twidx=0
        for i in range(lenb):
            yia=bdt[i][0]; Kji=0; 
            for k in range(bdt[i][1]):
                midxi=bdt[i][2][k][0]
                widxi=bdt[i][2][k][1]
                if(abs(midxi-midx)<int(Lx8/20)): 
                    tmidx+=midxi; twidx+=widxi
                    #print(yipin,j,midx,i,yia,k,' midxi=',midxi,midxi,tmidx)
                    cv2.circle(res3,(midxi,yia),1,(0,255,0),1)
                    nk+=1; kin.append(yia); Kji=1; break
            if(Kji==1): 
                amidx=int(tmidx/nk); awidx=int(twidx/nk)
        if(nk>5): NBA+=1; dat=(NBA,yipin,nk,amidx,awidx); Bdat.append(dat)
        #print(j,i,nk,amidx,awidx,kin); 
    #print('Bdat=',Bdat)
    return res3,Bdat
     


def mgray(img2,hsv):
    img3=np.copy(img2); hsv3=np.copy(hsv)
    for i in range(img3.shape[0]):
        for j in range(img3.shape[1]):
            if(hsv3[i,j,2]>50 and hsv3[i,j,1]/hsv3[i,j,2]<0.30): 
                img3[i,j,:]=[255,255,255]
    return img3



########################fbarr
def barrier(image):
    BLOW=80  # night_lamp=150; night_celling=100
    IMB=np.copy(image)
    img2 = cv2.resize(image, (Lx8,Ly8), interpolation=cv2.INTER_AREA)
    ori=np.copy(img2)
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    #print(img2.shape,hsv.shape)
    img3=mgray(img2,hsv)
    gray = cv2.cvtColor(img3, cv2.COLOR_BGR2GRAY)
    blurred = cv2.GaussianBlur(gray, (5,5), 0)
    binaryIMG = cv2.Canny(blurred, 40, 150)  #70 shadow
    bb0=np.copy(binaryIMG)
    b0=np.copy(binaryIMG)
    b0[:,b0.shape[1]-4:b0.shape[1]]=100
    thresh=cv2.threshold(gray,100,255,cv2.THRESH_BINARY_INV+cv2.THRESH_OTSU)[1]
    thr2=cv2.threshold(gray,30,255,cv2.THRESH_BINARY_INV+cv2.THRESH_OTSU)[1]
    ret,thr1=cv2.threshold(gray,BLOW,255,cv2.THRESH_BINARY_INV)
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (2,2))
    opening = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel, iterations=3)
    op2=cv2.morphologyEx(thr2,cv2.MORPH_OPEN,kernel,iterations=3)
    op1=cv2.morphologyEx(thr1,cv2.MORPH_OPEN,kernel,iterations=3)
    hbin1=np.hstack((b0,op2,op1))
    binaryIMG=np.copy(op1)
    b0=np.copy(binaryIMG)
    bs=np.copy(binaryIMG)
    b0[:,b0.shape[1]-4:b0.shape[1]]=100
    xl=0; xr=Lx8-xl-1; xl=5; xr=Lx8-xl
    yis=[]; ssy=[]; N=0; Z=0; XB=0; YB=0; NZ=0
    Zdata=[]; DX1=0; DX2=0; DX=0; lzdata=len(Zdata); LLa=[]
    for i in yiscan:
        KY=0; N+=1
        LL=[]; yi=i; yis.append(yi);
        #print(i,'xl,xr=',xl,xr,lzdata)
        #print(N,yi,xl,xr,binaryIMG[yi,xl:xr])
        nzt=0
        for t in range(xl,xr):
            if(binaryIMG[yi,t]==255): nzt+=1
        if(nzt>4):
            for t in range(xl,xr):
                #if(t==100): print('t100:',t,binaryIMG[yi:yi+20,t])
                if(binaryIMG[yi,t]==255): 
                    LL.append(t); 
                    #print(t,binaryIMG[yi,t],LL, binaryIMG[yi-20:yi,t])
                else: 
                    #print('zero:',t,binaryIMG[yi,t],LL)
                    st0=0; st=0
                    if(lzdata==0): 
                        if(binaryIMG[yi-1,t]==255): st+=1
                        if(binaryIMG[yi-2,t]==255): st+=1
                        if(binaryIMG[yi-3,t]==255): st+=1
                        #if(binaryIMG[yi-10,t]==255): st+=1
                        #if(binaryIMG[yi-20,t]==255): st+=1
                        #if(binaryIMG[yi-30,t]==255): st+=1
                        if(st>0): 
                            binaryIMG[yi,t]=255; 
                            LL.append(t); #print(yi,'ST=',st0,st)
        LLL=len(LL); 
        LLa.append(LL)
        if(LLL>2): 
            NZ+=1
            if(NZ==1): 
                #xl=np.min(LL); xr=np.max(LL); 
                Yi=yi
        #print(i,yi,LLL,xl,xr)
    #--------------------------------yiscan-loop done
    result=cv2.bitwise_and(ori,ori,mask=bs)
    res2=np.copy(result)
    if(len(LLa)==0): print('0 Bdat: LLa=',len(LLa)); return res2,[]
    res3,Bdat=sectionB(yis,LLa,res2)
    res2=np.copy(res3)
    hbin2=np.hstack((op1,binaryIMG))
    bina=cv2.resize(hbin2,(Lx8*8,Ly8*4), interpolation=cv2.INTER_AREA)
    cv2.imshow('bina',bina)
    for t in yiscan:
        cv2.line(res2,(xl,t),(xl*2,t),(0,255,255),1)
    cv2.line(res2,(xl,yiscan[1]),(xr,yiscan[1]),(0,255,255),1)
    #cv2.line(res2,(xl,Yi),(xr,Yi),(0,255,0),2)
    s2=cv2.resize(res2, (Lx8*4,Ly8*4))
    #cv2.waitKey(0)
    return res2,Bdat



def motcam(cap,win,Ns,lsp,rsp,dur):
    for j in range(Ns):
        ret,image=cap.read()
        imgs=cv2.resize(image,(320,240),interpolation=cv2.INTER_AREA)
        cv2.imshow(win,imgs)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        turn(lsp,rsp,dur); turn(0,0,0.00)



#def motcam(cap,win,Ns,lsp,rsp,dur):
def RSturn(cap,win):
    motcam(cap,win,5,0.8,0.0,0.1); motcam(cap,win,3,0.4,0.4,0.1)
    motcam(cap,win,6,0.0,0.8,0.1); turn(0.0,0.0,0.5)

def LSturn(cap,win):
    motcam(cap,win,5,0.0,0.8,0.1); motcam(cap,win,3,0.4,0.4,0.1)
    motcam(cap,win,6,0.8,0.0,0.1); turn(0.0,0.0,0.5)



################################## MAIN #################

cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480)
ret,image=cap.read()
LXM=image.shape[1]; LYM=image.shape[0]; WALL=int(LXM*0.5)
sleep(2)
distx=int(Ly8*0.8); BADISx=int(Ly8*0.8); RGX=0.06; RN=100; Kshow=1; Lx22=160
BADISx2=int(BADISx/2)
NTR=50; NBAR=0; TARG=0; win='imgcam'; Xcen=int(Lx8/2); WALL=int(Xcen*1.0)
XLEFT=int(Lx8/8*3); XRIGHT=int(Lx8/8*5)
print('\n\n NTR=',NTR,' BADIDx,distx=',BADISx,distx,' BADISx2=',BADISx2,WALL); 
yiscan=[Ly8-7-3*i for i in range(10)]
print('yiscan=',yiscan,'  BADISx=',BADISx)
#--------------------
ret,image=cap.read()
imgcam=np.copy(image)
imgs=cv2.resize(imgcam,(320,240),interpolation=cv2.INTER_AREA)
imgt=np.copy(imgs)
cv2.imshow(win,imgs)
#cv2.destroyWindow(win);        
#--------------------
if(1==1):
    box=[0 for i in range(6)]
    H8=0; KTARG=0; FAR=0
    for j in range(NTR):
        ret,image=cap.read(); imgcam=np.copy(image)
        imgs=cv2.resize(imgcam,(320,240),interpolation=cv2.INTER_AREA)
        #cv2.imshow(win,imgs)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #-----------------------------------------------
        tbox=Target2(low1,upp1,low2,upp2,low3,upp3,image); 
        #-----------------------------------------------
        (x,y,w,h)=tbox[0:4]
        XT8=int(x/8); DT8=int(w/8); YT8=int(y/8); H8a=int(h/8); YTG=YT8+H8
        if(H8a>H8): H8=H8a
        #print('###j_=',j,tbox,XLEFT,XRIGHT,(x,y,w,h),XT8,DT8,H8)
        TDR=0
        if(KTARG==1 and tbox[5]!=1): 
            print(j,'ERROR TARG,tbox=',tbox,'\n'); sleep(0.5)
            #turn(-0.4,0.4,0.1); turn(0.4,0.4,0.05);turn(0.4,-0.4,0.1);turn(0,0,0.05);
            continue
        if(tbox[5]==1): 
            KTARG=1
            (x,y,w,h)=tbox[0:4]
            XT8=int(x/8); DT8=int(w/8); YT8=int(y/8); H8=int(h/8); YTG=YT8+H8
            if(H8>20): print('REACH TARGET, terminate, H8,YTG=',H8,YTG); break 
            if(0<=XT8<XLEFT): TDR=3
            elif(XLEFT<=XT8<=XRIGHT): TDR=1
            else: TDR=7
            #print('###tJ=',j,'TARG found=',tbox,' TDR,XT8,H8=',TDR,XT8,H8); 
            img1=np.copy(image)
            imgs=cv2.resize(img1,(Lx,Ly),interpolation=cv2.INTER_AREA)
            cv2.rectangle(imgs,(x,y),(x+w,y+h),(0,255,255),2)
            cv2.circle(imgs,(x+int(w/2),y+int(h/2)),8,(255,0,0),-1)
            imgt=cv2.resize(imgs,(320,240),interpolation=cv2.INTER_AREA)
            #cv2.imshow('target',imgt)
            #cv2.waitKey(0)
            TARG=100
            #cv2.waitKey(0)
            #break
            #continue

        tm1=time.time()
        #-----------------------------------------------
        res2,Bdat=barrier(image); 
        #-----------------------------------------------
        tm2=time.time()
        dtm=round(tm2-tm1,3)
        lenB=len(Bdat)
        #print('j_NTR=',j,lenB,' dtm=',dtm)
        BTP=0
        if(lenB==0): KBARR=0 
        if(len(Bdat)>0): 
            #print('lenB=',len(Bdat))
            KBARR=1; BADIS=Bdat[0][1]; YB8=BADIS
            if(YB8<BADISx): FAR=1; 
            else: FAR=0
            if(H8<10 and len(Bdat)==1): 
                XB8=Bdat[0][3]; DX8=Bdat[0][4]
                #print('1BA,Bdat=',Bdat,' XB8=',XB8,YB8,BADISx)
                BTP=1
                if(10<XB8<40): DR=17
                elif(40<=XB8<70): DR=13
                #print('1BA,DR=',DR,' TDR,XT8,H8=',TDR,XT8,H8,' XB8=',XB8)
                if(YB8>BADISx):
                    print('BB',j,'1 BA,turnY8,X8,DR=',YB8,XB8,DX8,DR,dtm,FAR,'\n\n')
                else:
                    print('bb',j,'1 BA, Y8,X8,DR=',YB8,XB8,DX8,DR,dtm,FAR,'\n')
                #cv2.waitKey(0)
            if(len(Bdat)==2): 
                BTP=2
                XB8=int((Bdat[0][3]+Bdat[1][3])/2); 
                DX8=int(abs(Bdat[0][3]-Bdat[1][3]))
                XM1=Bdat[0][3]; XM2=Bdat[1][3]
                XG1=int(XM1+Bdat[0][4]/2); XG2=int(XM2-Bdat[1][4]/2)
                GAP=XG2-XG1; GAP2=int(GAP/2)
                cv2.line(res2,(XG1,YB8-10),(XG2,YB8-10),(255,0,0),4)
                if(GAP>150): BTP=21
                if(DX8>WALL): BTP=22
                print('%%%',j,'2 BA:',YB8,(XM1,XM2),(XG1,XG2),XB8,DX8,'GAP=',GAP,BTP)
            if(len(Bdat)==3): 
                BTP=3
                XB8=int((Bdat[0][3]+Bdat[1][3])/2); 
                XB8=int((Bdat[0][3]+Bdat[2][3])/2); 
                DX8=int(abs(Bdat[0][3]-Bdat[2][3]))+Bdat[0][4]+Bdat[2][4];
                print('%%%',j,'3 BA:',YB8,Bdat[0][3],Bdat[1][3],Bdat[2][3],XB8,DX8)
                cv2.waitKey(0)

        s1=cv2.resize(image,(320,240))
        s2=cv2.resize(imgt,(320,240))
        s3=cv2.resize(res2,(320,240))
        hs123=np.hstack((s1,s2,s3))
        cv2.imshow('hs123',hs123)
        out.write(hs123)
        if(KBARR==0): 
            BADIS=0; DR=1; BTP=0; #print('KBARR=0',BADIS,DR,BTP)
            #print('%%%',j,'KBARR=0 ,DR=',DR,dtm,' TDR=',TDR)
        else:
            if(FAR==1): DR=11; print(FAR,YB8,'<',BADISx,'FAR,fwd:DR=',DR)
            else: FAR=0
        #-------------------------------
        #if(j==0): cv2.waitKey(0); #sleep(5)

        KTG=0
        if((KBARR==0 or (KBARR==1 and FAR==1)) and TDR>0): KTG=1
        print('J=',j,' KBARR,FAR,TDR,DR,KTG=',KBARR,FAR,TDR,DR,KTG)
        cv2.waitKey(0)
        if(j==0): cv2.waitKey(0); sleep(5)
        #if(DR%10 != 1): cv2.waitKey(0)
        #if(DR%10 != 1): print('non 1 DR=',DR); cv2.waitKey(0)
        #DR=0

        #--------------------TARG-GO---------------
        if(KTG==1):
            #print('TDR_go_j_NTR=',j,'KBARR=',KBARR,' TDR=',TDR)
            if(TDR==1): turn(0.3,0.3,0.10); turn(0,0,0.05);
            if(TDR==3): turn(-0.4,0.4,0.10); turn(0,0,0.05);
            if(TDR==7): turn(0.4,-0.4,0.10); turn(0,0,0.05);
        #--------------------BARR-GO---------------
        else:
            if(DR==17): RSturn(cap,win)
            if(DR==13): LSturn(cap,win)
            if(DR==1): turn(0.3,0.3,0.10); turn(0,0,0.05);
            if(DR==11):turn(0.3,0.3,0.10); turn(0,0,0.05);
            if(DR==3): turn(0.0,0.3,0.10); turn(0,0,0.05);
            if(DR==7): turn(0.3,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(TARG==1): print('TARG is found, tbox=',tbox)
    cv2.waitKey(0)
    cv2.destroyAllWindows();        
    cap.release()
    out.release()