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('DIS.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]
lower1=np.array([60-20,20,10]);upper1=np.array([60+20,255,255]) #G
lower2=np.array([170-30,100,100]);upper2=np.array([170+30,255,255]) # R
#----21-1A
lower1=np.array([56-20,100,40]);upper1=np.array([56+20,255,255]) #G
lower2=np.array([178-20,150,50]);upper2=np.array([178+20,255,255]) # R


# MY j= 39 220 420 427 7 320 160 0 320 160=20cm


def mainc(image):
    #cv2.imshow('image',image)
    IMB=np.copy(image)
    img2 = cv2.resize(image, (Lx8,Ly8), interpolation=cv2.INTER_AREA)
    cv2.line(img2,(0,50),(319,50),(255,0,0),4)
    cv2.imshow('img2',img2)
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    for i in range(Lx8):
        print(i,img2[60,i,:],hsv[60,i,:])
    return

    gray = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
    blurred = cv2.GaussianBlur(gray, (5,5), 0)
    binaryIMG = cv2.Canny(blurred, 60, 100)
    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)]
    KK=Ly8-1
    #print('binary=',binaryIMG[:,MIDX])
    for i in range(Ly8-1,0,-1):
        #print('binary_i=',i,binaryIMG[i,int(Lx8/2)])
        if(binaryIMG[i,int(Lx8/2)]>10): KK=i; break
    print('KK=',KK); 
    TS=0; TV=0; NSW=0 ; NX=0
    for i in range(X1,X2):
        x=i; NX+=1; 
        for j in range(0,KK):
            NSW+=1
            h=hsv[j,x,0]; h2=int(h/10)
            ac[h2]+=1;
    #print('Nac,ac=',Nac,ac)
    maxa=0; ix=0
    for i in range(Nac):
        if(ac[i]>maxa): ix=i; maxa=ac[i]
        #print(i,ix,ix*10,maxa,ac[i])
    ix10=ix*10
    TS=0; TV=0; NSW=0;
    ad=[0 for i in range(Nac)]
    ae=[0 for i in range(Nac)]
    for i in range(X1,X2):
        x=i;
        for j in range(0,KK):
            h=hsv[j,x,0]; h2=int(h/10)
            if(h2==ix):
                NSW+=1;TS+=hsv[j,x,1];TV+=hsv[j,x,2];
                g=hsv[j,x,2]; g2=int(g/10)
                f=hsv[j,x,1]; f2=int(f/10)
                ad[g2]+=1; ae[f2]+=1
    VX=0; VXX=0; SX=0; SXX=0
    for i in range(Nac): 
        if(ad[i]>VX): VX=ad[i]; VXX=i
        if(ae[i]>SX): SX=ae[i]; SXX=i
        #=================
        #print(i,'ac[i]=',ac[i],' ad[i]=',ad[i],VX,VXX,SX,SXX)
    AV10=VXX*10; AS10=SXX*10
    AV=AV10; AS=AS10
    print(NX,'ix,maxa=',ix,maxa,' ix10=',ix10,' AS,AV=',AS,AV)
    lower=np.array([ix10-10,AS-10,AV-10])
    upper=np.array([ix10+10,AS+10,AV+10])
    if(ix10==0): lower[0]=0
    if(lower[1]<0): lower[1]=0
    if(lower[2]<0): lower[2]=0
    print('lower=',lower,' upper=',upper)
    #----------------------------------------------
    mask=cv2.inRange(hsv,lower,upper)
    #----------------------------------------------
    img3=cv2.line(img2,(X1,0),(X1,KK),(0,255,0),2)
    img3=cv2.line(img3,(X2,0),(X2,KK),(0,255,0),2)
    img3=cv2.line(img3,(X1,KK),(X2,KK),(0,0,255),2)
    img3=cv2.line(img2,(MIX,0),(MIX,KK),(0,255,255),2)
    hstack=np.hstack((blurred,binaryIMG))
    hstack1=np.hstack((hstack,mask))
    cv2.imshow('hstack1',hstack1)
    cnts,_ = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, 
          cv2.CHAIN_APPROX_SIMPLE) # cv2.RETR_LIST
    #print('len(cnts)=',len(cnts))
    #cv2.waitKey(0)
    #cv2.destroyAllWindows();        
    i=0; j=0; ax=0; cx=0; px=0; xx=0; mx=0; Kc=0
    for c in cnts:
        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)
        perimeter=cv2.arcLength(c,True); perimeter3=round(perimeter,3)
        (x,y,w,h)=cv2.boundingRect(c)
        if(y+w>KK+2): continue
        if(area>ax): ax=area3; cx=c; px=perimeter3; xx=(x,y,w,h); mx=(cX,cY) 
        Kc=1
    clone = img2.copy()
    if(Kc==1): 
        cv2.circle(clone,mx,6,(0,125,255),-1)
        x,y,w,h=xx[0],xx[1],xx[2],xx[3]
        cv2.rectangle(clone,(x,y),(x+w,y+h),(0,125,255),6)
    hstack2=np.hstack((img2,clone))
    cv2.imshow('hstack2',hstack2)
    #cv2.waitKey(0)
    if(ax==0): print('ax=0, STOP'); cv2.waitKey(0); input('ssss')
    else: print('XXX:',ax,px,xx,mx)
    box=[]
    for i in range(4):
        box.append(xx[i]*F8)
    KKDIS=(y+h)*F8
    box.append(KKDIS)
    return box,lower,upper




def target(lower,upper,image):
    IMB=np.copy(image)
    img2 = cv2.resize(image, (Lx8,Ly8), interpolation=cv2.INTER_AREA)
    gray = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
    blurred = cv2.GaussianBlur(gray, (5,5), 0)
    binaryIMG = cv2.Canny(blurred, 60, 100)
    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)]
    KK=Ly8-1
    for i in range(Ly8-1,0,-1):
        if(binaryIMG[i,int(Lx8/2)]>10): KK=i; break
    KKDIS=KK
    #print('target: KK=',KK,' lower=',lower,' upper=',upper)
    #----------------------------------------------
    mask=cv2.inRange(hsv,lower,upper)
    #----------------------------------------------
    smask=np.sum(mask)
    img3=cv2.line(img2,(X1,0),(X1,KK),(0,255,0),2)
    img3=cv2.line(img3,(X2,0),(X2,KK),(0,255,0),2)
    img3=cv2.line(img3,(X1,KK),(X2,KK),(0,0,255),2)
    img3=cv2.line(img2,(MIX,0),(MIX,KK),(0,255,255),2)
    hstack=np.hstack((blurred,binaryIMG))
    hstack1=np.hstack((hstack,mask))
    cv2.imshow('hstack1',hstack1)
    cnts,_ = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, 
          cv2.CHAIN_APPROX_SIMPLE) # cv2.RETR_LIST
    lcnts=len(cnts); #print('lcnts=',lcnts)
    i=0; j=0; ax=0; cx=0; px=0; mx=0; Kc=0; xx=[0 for i in range(5)]
    for c in cnts:
        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)
        perimeter = cv2.arcLength(c, True); perimeter3=round(perimeter,3)
        (x,y,w,h) = cv2.boundingRect(c)
        #if(y+w>KK+2): continue
        if(area>ax): ax=area3;cx=c;px=perimeter3;xx=(x,y,w,h);mx=(cX,cY);Kc=1
    clone = img2.copy()
    if(Kc==1): 
        cv2.circle(clone,mx,6,(0,125,255),-1)
        x,y,w,h=xx[0],xx[1],xx[2],xx[3]
        cv2.rectangle(clone,(x,y),(x+w,y+h),(0,125,255),6)
    hstack2=np.hstack((img2,clone))
    cv2.imshow('hstack2',hstack2)
    AX=1
    if(ax==0): print('ax=0, STOP,lcnts,smask=',lcnts,smask);cv2.waitKey(0);AX=0
    #else: print('XXX:',ax,px,xx,mx)
    box=[]
    for i in range(4):
        box.append(xx[i]*F8)
    KKDIS=(xx[1]+xx[3])*F8
    box.append(KKDIS)
    box.append(AX)
    if(AX==0): print('AX=0',' box=',box)
    return box



def target1(lower1,upper1,lower2,upper2,image):
    IMB=np.copy(image)
    img2=np.copy(image)
    #img2 = cv2.resize(image, (Lxg,Lyg), interpolation=cv2.INTER_AREA)
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    Lxg=img2.shape[1]; Lyg=img2.shape[1]; Fg=1
    MIDX=int(Lxg/2); WM=0; X1=MIDX-WM; X2=MIDX+WM+1; MIX=int((X1+X2)/2)
    Nac=int(256/10)+1
    ac=[0 for i in range(Nac)]
    #----------------------------------------------
    mask1=cv2.inRange(hsv,lower1,upper1)
    mask2=cv2.inRange(hsv,lower2,upper2)
    #cv2.imshow('mask1',mask1)
    #cv2.imshow('mask2',mask2)
    #----------------------------------------------
    smask1=np.sum(mask1)
    smask2=np.sum(mask2)
    img3=cv2.line(img2,(MIX,0),(MIX,Lyg-1),(0,255,255),2)
    hmask=np.hstack((mask1,mask2))
    #imgs=cv2.resize(hmask,(640,480),interpolation=cv2.INTER_AREA)
    #cv2.imshow('hmask',imgs)
    #cv2.waitKey(0)
    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)
    print('lcnts1,2=',lcnts1,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,20,(255,0,0),-1)
        x,y,w,h=xx1[0],xx1[1],xx1[2],xx1[3]
        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(RGX<ratRG<0.7):
            HITA=1; datag=d 
            cv2.rectangle(clone1,(x-50,y-50),(x+w+50,y+h+50),(255,0,0),12)
            dist1=y3
            #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=',N1,' ratRG=',ratRG,(x,y,w,h),clone1.shape)
            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)
    cv2.imshow('target1_hst',imgs)
    #cv2.imshow('clone1',clone1)
    #if(HITA==1): print('datag=',datag) 
    #cv2.imshow('cloneD',clone1)
    #cv2.imshow('cloneD',clone1)
    cv2.waitKey(0)
    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 target2(lower1,upper1,lower2,upper2,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,lower1,upper1)
    mask2=cv2.inRange(hsv,lower2,upper2)
    #cv2.imshow('mask1',mask1)
    #cv2.imshow('mask2',mask2)
    #----------------------------------------------
    smask1=np.sum(mask1)
    smask2=np.sum(mask2)
    img3=cv2.line(img2,(MIX,0),(MIX,Ly-1),(0,255,255),2)
    hstack1=np.hstack((mask1,mask2))
    #cv2.imshow('mask1',mask1)
    #cv2.imshow('mask2',mask2)
    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)
    #print('lcnts1,2=',lcnts1,lcnts2)

    N1=0; ax=0; cx=0; px=0; mx1=0; Kc1=0; xx1=[0 for i in range(5)]
    clone1=img2.copy(); data1=[]
    #if(Kshow==1): cv2.imshow('mask1',mask1)
    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)
    #cv2.imshow('tagert2_hst',imgs)
    #cv2.imshow('clone1',clone1)
    #if(HITA==1): print('datag=',datag) 
    #if(Kshow==1): cv2.imshow('mask_stack',hstack1)
    #if(Kshow==1): cv2.imshow('cloneD',clone1)
    #cv2.imshow('cloneD',clone1)
    #cv2.waitKey(0)

    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 section(line):
    L=len(line); WT=255*5
    sec=[]
    if(line[0]==255): sec1=0; nsec=1
    else: nsec=0
    #print('in section:line=\n',line)
    for i in range(5,L-5):
        if(sum(line[i-5:i])==0 and sum(line[i:i+5])==WT): 
            nsec+=1; sec1=i; print(nsec,'i=',i,'sec1=',sec1)
        if(sum(line[i-5:i])==WT and sum(line[i:i+5])==0): 
            sec2=i-1; sec.append((sec1,sec2)); print('i=',i,' nsec=',nsec)
    print(nsec,'sec=',sec)
    


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,tmidx)
                    cv2.circle(res3,(midxi,yia),3,(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=180  # 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)
    #cv2.imshow('gray',gray)
    #'''
    #cv2.imshow('img2',img2)
    #cv2.imshow('img3',img3)
    pn=int(Ly8/3*2)
    #v2.line(img2,(0,pn+3),(Lx8-1,pn+3),(0,255,0),1)
    #cv2.imshow('img2',img2)
    #'''
    blurred = cv2.GaussianBlur(gray, (5,5), 0)
    binaryIMG = cv2.Canny(blurred, 40, 150)  #70 shadow
    bb0=np.copy(binaryIMG)
    b0=np.copy(binaryIMG)
    #cv2.imshow('b0',b0)
    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)
    #cv2.imshow('thr1',thr1)
    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)
    #op1[:,op1.shape[1]-4:op1.shape[1]]=100
    hbin1=np.hstack((b0,op2,op1))
    binaryIMG=np.copy(op1)
    #cv2.imshow('hbin1',hbin1)
    '''
    for i in range(Lx8):
        print(i,img2[pn,i,:],hsv[pn,i,:],img3[pn,i,:],gray[pn,i],
                thr1[pn,i],op1[pn,i],binaryIMG[pn,i])
    '''
    #cv2.imshow('op1',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=[]
    #print('baseline=\n',binaryIMG[210,:])
    #print('160_line=\n',binaryIMG[160,:])
    #print('yiscan=',yiscan)
    #cv2.waitKey(0)
    #input('bbbbb')

    '''
    for i in yiscan:
        yi=i
        cv2.line(img2,(xl,i),(xr,i),(255,0,0),1)
        print(N,yi,xl,xr,binaryIMG[yi,xl:xr])
    cv2.imshow('img2L',img2)
    cv2.waitKey(0)
    input('ttttt')
    '''

    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)
    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))
    s4=cv2.resize(hbin2,(Lx8*8,Ly8*4), interpolation=cv2.INTER_AREA)
    cv2.imshow('s4',s4)
    #for t in yiscan:
    #    cv2.line(res2,(xl,t),(xr,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), interpolation=cv2.INTER_AREA)
    cv2.imshow('s2',s2)
    #cv2.waitKey(0)
    return res2,Bdat




def Lcurve(cap):
    tLR=0.08; tLF=0.08
    for j in range(6):
        turn(-0.4,0.4,tLR); turn(0.0,0.0,0.05);
        ret,image=cap.read()
        cv2.imshow('imgcam',image)
        out.write(image)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
    for j in range(9):
        turn(0.4,0.4,tLF); turn(0.0,0.0,0.05);
        ret,image=cap.read()
        cv2.imshow('imgcam',image)
        out.write(image)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
    for j in range(40):
        turn(0.4,-0.4,0.10); turn(0.0,0.0,0.50);
        ret,image=cap.read()
        cv2.imshow('imgcam',image)
        out.write(image)
        box=target2(lower1,upper1,lower2,upper2,image); 
        x=box[0]; y=box[1]; w=box[2]; h=box[3]; dist=box[4]; XC=int(x+WD+w/2)
        if(XLT<XC<XRT): return
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        



def Rcurve(cap):
    tRR=0.05; tRF=0.05
    for j in range(15):
        turn(0.6,0.2,tRR); turn(0.0,0.0,0.05);
        ret,image=cap.read()
        cv2.imshow('imgcam',image)
        out.write(image)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
    for j in range(15):
        turn(0.4,0.4,tRF); turn(0.0,0.0,0.05);
        ret,image=cap.read()
        cv2.imshow('imgcam',image)
        out.write(image)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
    for j in range(100):
        turn(-0.4,0.4,0.10); turn(0.0,0.0,0.50);
        ret,image=cap.read()
        cv2.imshow('imgcam',image)
        out.write(image)
        box=target2(lower1,upper1,lower2,upper2,image); 
        x=box[0]; y=box[1]; w=box[2]; h=box[3]; dist=box[4]; XC=int(x+WD+w/2)
        if(XLT<XC<XRT): return
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        


#------rotation search-----
def rotsch(cap,lower1,upper1,lower2,upper2):
    box=[0 for i in range(6)]
    for j in range(6):
        ret,image=cap.read()
        imgcam=np.copy(image)
        box=target1(lower1,upper1,lower2,upper2,image); 
        print('***********ROT_j=',j,'*********** boax=',box)
        if(box[5]==1): 
            print('TARGET found in search_rotation, box=',box); 
            return 50,box
        turn(0.4,-0.4,0.25); turn(0,0,0.5);
    print('rotate exhausted, TARGET not found')
    return 0,box


#------Translation search-----
def translsch(cap,lower1,upper1,lower2,upper2):
    NTR=20; NBAR=0 
    distx=int(Ly8*0.8); BADISx=int(Ly8*0.8); 
    box=[0 for i in range(6)]
    for j in range(NTR):
        ret,image=cap.read()
        imgcam=np.copy(image)

        tbox=target2(lower1,upper1,lower2,upper2,image); 
        if(box[5]==1): 
            print('TARGET found in search_transl, box=',box); 
            return 100,tbox
        #KBARR,XB8,YB8,DX8=barrier(image); BADIS=YB8
        res2,Bdat=barrier(image); 
        if(len(Bdat)>0): 
            KBARR=1; BADIS=Bdat[0][1]
        if(KBARR==0): BADIS=0

        '''
        x=box[0]; y=box[1]; w=box[2]; h=box[3]; dist=box[4]; XC=int(x+WD+w/2)
        if(box[5]==1):
            cv2.rectangle(imgcam,(x+WD,y),(x+WD+w,y+h),(255,125,0),4)
            cv2.circle(imgcam,(int(x+w/2),int(y+h/2)),6,(255,0,0),-1)
        imgcam=cv2.line(imgcam,(Lx2,0),(Lx2,Ly-1),(255,0,0),1)
        imgcam=cv2.line(imgcam,(XLT,0),(XLT,Ly-1),(255,0,0),1)
        imgcam=cv2.line(imgcam,(XRT,0),(XRT,Ly-1),(255,0,0),1)
        '''
        print('Transl_j=',j,'Tr=',box[5],'KBA=',KBARR,YB8,XB8,DX8)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
        tro=0.4; tfw=0.3
        if(BADIS>BADISx): 
            NBAR+=1
            print('NBAR=',NBAR,'BADIS stop, BADIS=',BADIS,(XB8,YB8)) 
            if(NBAR==1): cv2.waitKey(0)
            if(XB8<Lx2): 
                #Rcurve(cap)
                turn(0.8,0.2,0.6); turn(0,0,0.1)
                turn(0.2,0.8,1.5); turn(0,0,0.1)
                turn(0.4,-0.4,0.45); turn(0,0,0.1)
            else: 
                #Lcurve(cap)
                turn(0.2,0.8,0.6); turn(0,0,0.1)
                turn(0.8,0.2,1.5); turn(0,0,0.1)
                turn(-0.4,0.4,0.45); turn(0,0,0.1)
        turn(0.4,0.4,0.10); turn(0,0,0.5);
        if(j>0 and j%10==0): rotsch(cap,lower1,upper1,lower2,upper2)
    return box





def search():
    cap = cv2.VideoCapture(0)
    sleep(2)
    ret,image=cap.read()
    # ST216 
    #lower1=np.array([60-20,20,10]);upper1=np.array([60+20,255,255]) #G
    #lower2=np.array([160,100,100]);upper2=np.array([190,255,255]) # R
    #----21-1A
    #lower1=np.array([56-20,100,40]);upper1=np.array([56+20,255,255]) #G
    #lower2=np.array([178-20,150,50]);upper2=np.array([178+20,255,255]) # R
    print('lower1=',lower1,upper1,'lower2=',lower2,upper2)
    box=[0 for t in range(6)]
    ret,image=cap.read()
    box=target1(lower1,upper1,lower2,upper2,image); 
    #sleep(1)
    #cv2.destroyAllWindows();        
    if(box[5]==1): print('TARGET found in search, box=',box); return box
    #cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
    #cap.set(cv2.CAP_PROP_FRAME_HEIGHT,1080)
    #rotsch(cap,lower1,upper1,lower2,upper2)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480)
    translsch(cap,lower1,upper1,lower2,upper2)
    input('ssss')
    return box



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)



# CL-type curve ..................................
def CLturn(cap,win):
    motcam(cap,win,8,0.2,0.7,0.1)
    motcam(cap,win,8,0.7,0.2,0.1); turn(0.0,0.0,0.5)
    motcam(cap,win,10,0.7,0.0,0.1)
    motcam(cap,win,10,0.0,0.7,0.1)
    return

# CR-type curve ..................................
def CRturn(cap,win):
    motcam(cap,win,8,0.7,0.2,0.1)
    motcam(cap,win,8,0.2,0.7,0.1); turn(0.0,0.0,0.5)
    motcam(cap,win,10,0.0,0.7,0.1)
    motcam(cap,win,10,0.7,0.0,0.1)
    return

# WALL-type curve ..................................
def WALLturn(cap,win):
    print('in WALL turn...',LXM,LYM)
    for j in range(100):
        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(0.4,-0.4,0.1); turn(0,0,0.05)
        KBARR,XB8,YB8,DX8=barrier(image); BADIS=YB8
        print('WALL_j=',j,KBARR,XB8,YB8,DX8)
        if(BADIS>BADISx and XB8<LXM/4 and DX8<LXM/4): 
            print('WALL barr on left, make right turn') 
            turn(0.4,0.0,0.2); turn(0,0,0.05); continue
        if(XB8>LXM-LXM/4 and DX8<LXM/4): 
            turn(0.0,0.4,0.2); turn(0,0,0.05); continue
        if(KBARR==0 or BADIS<BADISx/2): print('ESCAPE WALL'); break
    return


def RSturn(cap,win):
    motcam(cap,win,4,0.8,0.0,0.1)
    motcam(cap,win,4,0.0,0.7,0.1); turn(0.0,0.0,0.5)

def LSturn(cap,win):
    motcam(cap,win,4,0.0,0.8,0.1)
    motcam(cap,win,4,0.8,0.0,0.1); turn(0.0,0.0,0.5)

def WRturn(cap,win):
    for j in range(5):
        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
        res2,Bdat=barrier(image); 
        if(len(Bdat)==0): return
        XB8=Bdat[0][3]; DX8=Bdat[0][4]
        if(len(Bdat)==1 and DX8<WALL*0.5): print('WR_return_j=',j);return
        turn(0.4,-0.4,0.1); turn(0,0,0.10)


def WLturn(cap,win):
    for j in range(5):
        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
        res2,Bdat=barrier(image); 
        if(len(Bdat)==0): return
        XB8=Bdat[0][3]; DX8=Bdat[0][4]
        if(len(Bdat)==1 and DX8<WALL*0.5): print('WR_return_j=',j);return
        turn(-0.4,0.4,0.1); turn(0,0,0.10)


def Wall(cap,win):
    for j in range(50):
        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
        res2,Bdat=barrier(image); 
        if(len(Bdat)==0): print('escape,j_Wall=',j); return
        print('j_Wall=',j,Bdat[0][0]); 
        XB8=Bdat[0][3]; DX8=Bdat[0][4]
        if(len(Bdat)==1 and DX8<WALL*0.5): print('WR_return_j=',j);return
        turn(-0.4,0.4,0.1); turn(0,0,0.10)












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

#search()
#input('22222')

cap = cv2.VideoCapture(0)
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=600; NBAR=0; TARG=0; win='imgcam'; Xcen=int(Lx8/2); WALL=int(Xcen*1.0)
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)
cv2.imshow(win,imgs)
cv2.waitKey(0)
#--------------------
if(1==1):
    box=[0 for i in range(6)]
    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(lower1,upper1,lower2,upper2,image); 
        #-----------------------------------------------
        print('j_=',j,tbox)
        if(tbox[5]==1): 
            print('TARGET found in search_transl, tbox=',tbox); 
            (x,y,w,h)=tbox[0:4]
            print((x,y,w,h))
            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)
            imgs=cv2.resize(imgs,(320,240),interpolation=cv2.INTER_AREA)
            cv2.imshow('target',imgs)
            cv2.waitKey(0)
            TARG=100
            #break
        cv2.waitKey(0)
        continue

        tm1=time.time()
        #-----------------------------------------------
        res2,Bdat=barrier(image); 
        #-----------------------------------------------
        tm2=time.time()
        dtm=round(tm2-tm1,3)
        #print('j_NTR=',j,len(Bdat),' dtm=',dtm)
        BTP=0
        if(len(Bdat)==0): 
            KBARR=0 
        if(len(Bdat)>0): 
            #print('lenB=',len(Bdat))
            KBARR=1; BADIS=Bdat[0][1]; YB8=BADIS
            if(len(Bdat)==1): 
                XB8=Bdat[0][3]; DX8=Bdat[0][4]
                BTP=1
                if(XB8<Xcen-5): DR=17
                else: DR=13
                if(YB8>BADISx):
                    print(j,'1 BA YB8,XB8,Lx22,DR=',YB8,XB8,DX8,Lx22,DR,dtm)
                else:
                    print(j,'1 BA turn, YB8,XB8,Lx22,DR=',YB8,XB8,DX8,Lx22,DR,dtm)
            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,dtm)
            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,dtm)
        if(KBARR==0): 
            BADIS=0; DR=1; BTP=0; #print('KBARR=0',BADIS,DR,BTP)
            print(j,'0 BA KBARR=0 ,DR=',DR,dtm)
        else:
            if(Xcen+10<XB8): BTURN='L'
            elif(Xcen-10<=XB8<=Xcen+10): BTURN='C'
            else: BTURN='R'
            if(DX8>WALL): print('DX8,WALL=',DX8,WALL); KWALL=1
            else: KWALL=0
            cv2.imshow('res2',res2)
            if(BTP==21 and BTURN=='C'): 
                print('C and 21, ready to drive through...'); #sleep(1)
                for i in range(16):
                    turn(0.3,0.3,0.1); turn(0,0,0.1)
            if(YB8<BADISx): DR=11; print(YB8,'<',BADISx,'FAR,fwd:DR=',DR)
            if((BTP==22 or KWALL==1) and YB8>=BADISx): 
                print(BTP,KWALL,YB8,'>=',BADISx,'near WALL_BTURN=',BTURN)
                DR=200
                cv2.waitKey(0)
                Wall(cap,win)
                '''
                #input('KWALL==1 and YB8>=BADISx')
                if(BTURN=='R'): print('WRturn...'); WRturn(cap,win)
                else: print('WLturn...'); WLturn(cap,win)  
                '''
        if(DR%10 != 1): cv2.waitKey(0)
        #cv2.waitKey(0)
        #DR=0
        if(DR==17): RSturn(cap,win)
        if(DR==13): LSturn(cap,win)
        if(DR==1): turn(0.3,0.3,0.20); 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.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==0): turn(0.0,0.0,0.10); turn(0,0,0.05);

    if(TARG==1): print('TARG is found, tbox=',tbox)
input('ttttt')








#---- S-type-turn ----
#motcam(cap,win,10,0.2,0.6,0.1)
#motcam(cap,win,10,0.6,0.2,0.1)


#motcam(cap,win,Ns,lsp,rsp,dur):
win='img'; Ns=20; lsp=0.0; rsp=0.4; dur=0.10



'''
# C-type curve ..................................
print('starting a C-type curve...')
turn(0,0,3)
motcam(cap,win,8,0.2,0.7,0.1)
motcam(cap,win,8,0.7,0.2,0.1); turn(0.0,0.0,0.5)
motcam(cap,win,10,0.7,0.0,0.1)
motcam(cap,win,10,0.0,0.7,0.1)
turn(0,0,3)
# S-type curve ..................................
print('starting a S-type curve...')
motcam(cap,win,10,0.2,0.8,0.1)
motcam(cap,win,10,0.8,0.2,0.1); turn(0.0,0.0,2.5)
'''

input('mmmmm')


cv2.destroyAllWindows();        
out.release()