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)
BADISx=int(Ly8*0.8); RGX=0.06; RN=100; Kshow=1;
NTR=50; NBAR=0; TARG=0; win='imgcam'; WALL=int(Lx8/2)
yiscan=[Ly8-7-3*i for i in range(10)]; 
XLEFT=int(Lx8/8*3.5); XRIGHT=int(Lx8/8*4.5)


def hsec(BAX):
    Y=BAX.shape[0]; X=BAX.shape[1]; A=X*Y; #print('hsec_Y,X=',Y,X,A)
    dY=int(Y/8); Yi=[Y-1-dY*i for i in range(8)]; 
    Nmx=[]; BAXC=np.copy(BAX); NL=0
    #print('Yi=',Yi); 
    NC=0; NB=0; DH8=0
    for j in range(8):
        y=Yi[j]; red=[]
        for x in range(X):
            if(j>1 and (x<xs1-5 or x>xs2+5)): continue
            if(BAXC[y,x]==255): red.append(x)
        lred=len(red)
        if(lred>0): mx=int(np.median(red)); NB+=1
        else: mx=-1
        #print('hsec_j=',j,y,X,'lred=',lred,mx,'NB,NC=',NB,NC,red)
        if(j==0): Nmx.append(mx); NL+=1; 
        if(j==1): 
            mx0=mx; lred0=lred; red0=red.copy(); 
            if(lred0>2): xs1=red[0]; xs2=red[-1]
            else: MBAR=0; return 0,BAXC,-10,-10  
        if(j>0 and abs(mx-mx0)<5): 
            Nmx.append(mx); NL+=1; #print(j,y,NL,lred,mx)
            #print(j,'NC=',NC,y,lred,(mx,y))
            NC+=1
            cv2.circle(BAXC,(mx,y),3,(0,0,0),1)
            cv2.line(BAXC,(xs1,y),(xs2,y),(200,200,200),1)
    BAXC=cv2.resize(BAXC,(BAXC.shape[1]*6,BAXC.shape[0]*6)); NLx=len(Nmx)
    NB=8
    if(NB>0): ratB=round(NLx/NB,2)
    else: ratB=0
    BH8=int(Y*ratB/2)
    #print(Y,X,'shape=',BAXC.shape,' NLx=',NLx,NB,ratB,'BH8=',BH8)
    cv2.imshow('BAXC',BAXC)
    #cv2.waitKey(0)
    #std=np.std(Nmx); ratstd=round(std/Y,2)
    #if(NLx>2): MBAR=1; #print('Nmx=',Nmx)
    #else: MBAR=0
    if(BH8<20): MBAR=0; xs1=-1; xs2=-1
    else: MBAR=1
    #print('X,Y,lred0,red0=',X,Y,lred0,red0,'xs=',xs1,xs2)
    return MBAR,BAXC,xs1,xs2,BH8


def road(image,tm12):
    from whsv import whsv
    low1,upp1,low2,upp2,low3,upp3,low_rd,upp_rd=whsv()
    BLOW=80; IMB=np.copy(image)
    img2 = cv2.resize(image, (160,120), interpolation=cv2.INTER_AREA)
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    #----------------------------------------------
    mask1=cv2.inRange(hsv,low_rd,upp_rd)
    #----------------------------------------------
    ret,thr1=cv2.threshold(mask1,BLOW,255,cv2.THRESH_BINARY)
    ret,bar1=cv2.threshold(mask1,BLOW,255,cv2.THRESH_BINARY_INV)
    bar1[0:20,:]=0; bar1[:,0:10]=0; bar1[:,130:160]=0
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3,3))
    op1=cv2.morphologyEx(thr1,cv2.MORPH_OPEN,kernel,iterations=3)
    barr=cv2.morphologyEx(bar1,cv2.MORPH_OPEN,kernel,iterations=3)
    hbin1=np.hstack((mask1,op1))
    res=cv2.bitwise_and(img2,img2,mask=mask1)
    #..........................................BARR.....
    clone1=cv2.bitwise_and(img2,img2,mask=barr)
    cnts1,_ = cv2.findContours(barr.copy(), cv2.RETR_EXTERNAL, 
          cv2.CHAIN_APPROX_SIMPLE) # cv2.RETR_LIST
    lcnts1=len(cnts1); #print('lcnts1=',lcnts1)
    N1=0; ax=0; cx=0; px=0; mx1=0; Kc1=0; xx1=[0 for i in range(5)]
    data1=[]; YN=0; datax=[]
    for c in cnts1:
        area = int(cv2.contourArea(c)); 
        if(area<100): continue
        Kc1=1; N1+=1; (x,y,w,h)=cv2.boundingRect(c); cy2=y+h
        cx1=x+int(w/2); cy1=y+int(h/2); xx1=[x,y,w,h]; mx1=[cx1,cy1]; Ar=w*h
        d=[Ar,area,xx1,mx1]; data1.append(d)
        if(cy2>YN): 
            YN=cy2; datax=d.copy()
        cv2.circle(clone1,mx1,4,(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,155,0),2)
        #print('N1=',N1,YN,d,datax)
    lendatax=len(datax)
    xc=0; yc=0; yb=0; KBARR=-2; MBAR=-1; BH8=0
    #print('lendatax=',lendatax,datax)
    if(len(datax)>0):
        x=datax[2][0]; y=datax[2][1]; w=datax[2][2]; h=datax[2][3];
        yb=y+h; xc=datax[3][0]; yc=datax[3][1]
        #print('yb=',yb,yc,xc)
        cv2.rectangle(clone1,(x,y),(x+w,y+h),(255,0,0),2)
        if(yb<70): KBARR=0; MBAR=-1; BDR=1;  # far BARRIER
        elif(70<=yb<100): 
            KBARR=-1; MBAR=-1; BDR=1;  # far BARRIER
            if(xc<80): BDR=37
            else: BDR=33
        else:
            if(100<=yb<110): KBARR=1
            elif(110<=yb<=120): KBARR=2
            cv2.rectangle(clone1,(x,y),(x+w,y+h),(50,255,0),2)
            BAX=barr[y:y+h,x:x+w]
            #............................................
            MBAR,BAXC,xs1,xs2,BH8=hsec(BAX); R=int((xs2-xs1)/2)
            #print('road_MBAR,BH8=',MBAR,BH8)
            cv2.rectangle(clone1,(x+xs1,y),(x+xs2,y+h),(255,50,50),1); 
            cv2.circle(clone1,(x+xs1+R,yc),int(R/2),(255,50,50),-1)
            ss=120/BAXC.shape[0]; sx=int(ss*BAXC.shape[1])
            BAXCs=cv2.resize(BAXC,(sx,120))
            tm123=np.hstack((tm12,BAXCs))
            BDR=1
            if(MBAR==0): KBARR=0
            if(MBAR==1 and KBARR==1):
                if(xc<80): BDR=17
                else: BDR=13
            if(MBAR==1 and KBARR==2):
                if(xc<80): BDR=27
                else: BDR=23
    else: KBARR=0; MBAR=-1; BDR=1
    if(lendatax>0):
        datax[2][0]=int(datax[2][0]/2); datax[2][1]=int(datax[2][1]/2);
        datax[2][2]=int(datax[2][2]/2); datax[2][3]=int(datax[2][3]/2);
    xc=int(xc/2); yc=int(yc/2); yb=int(yb/2)
    return KBARR,MBAR,BDR,BH8,xc,yc,yb,datax,clone1


def Target2(image):
    from whsv import whsv
    low1,upp1,low2,upp2,low3,upp3,low_rd,upp_rd=whsv()
    Lxg=Lx; Lyg=Ly; Fg=1
    IMB=np.copy(image)
    img2=cv2.resize(image,(Lxg,Lyg)); 
    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)
    mask2=mask2 | mask3
    mkGR=np.copy(mask2)
    mkGR=cv2.resize(mkGR,(160,120))
    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]
        mkGR=mask1[y2:y3,x2:x3] | mask2[y2:y3,x2:x3]
        mkGR=cv2.resize(mkGR,(160,120))
        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
    mask1=cv2.resize(mask1,(160,120))
    mask2=cv2.resize(mask2,(160,120))
    ims=mask1[:,0:10]; ims[:,:]=100
    tm12=np.hstack((mask1,ims,mask2,ims))
    tm123=np.hstack((mask1,mask2,mkGR))
    tm123=cv2.resize(tm123,(160*3,120))
    #cv2.imshow('tm12',tm12)
    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):
        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=[-80 for i in range(6)]
    #if(box[5]==0): print('HITA=0',' box=',box,' ratRG=',ratRG)
    (x,y,w,h)=box[0:4]
    img1=np.copy(image)
    imgs=cv2.resize(img1,(Lx,Ly),interpolation=cv2.INTER_AREA)
    cv2.rectangle(imgs,(x,y),(x+w,y+h),(255,50,50),4)
    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)
    XT8=int(x/8); DT8=int(w/8); YT8=int(y/8); H8a=int(h/8); box.append(H8a)
    box[0]=XT8;box[1]=YT8;box[2]=DT8;box[3]=H8a
    TDR=0
    if(0<=XT8<XLEFT): TDR=3
    elif(XLEFT<=XT8<=XRIGHT): TDR=1
    elif(XT8>XRIGHT): TDR=7
    #print(TDR,XLEFT,XRIGHT,' Tar_XT8=',XT8,DT8,YT8,H8a)
    return imgt,tm12,TDR,box

def motcam(cap,win,out,Ns,lsp,rsp,dur):
    #print(type(out))
    for j in range(Ns):
        ret,image=cap.read()
        s1=cv2.resize(image,(320,240))
        s2=np.copy(s1); s3=np.copy(s1); s4=np.copy(s1)
        hs12=np.hstack((s1,s2))
        hs34=np.hstack((s3,s4))
        hs1234=np.vstack((hs12,hs34))
        cv2.imshow('hs1234',hs1234); out.write(hs1234)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        turn(lsp,rsp,dur); turn(0,0,0.20)



def Rturn(cap,win,out):
    #sleep(1)
    motcam(cap,win,out,4,0.4,-0.4,0.1); motcam(cap,win,out,5,0.4,0.4,0.1)
    motcam(cap,win,out,3,-0.4,0.4,0.1); turn(0.0,0.0,0.5)
    for i in range(40):
        ret,image=cap.read()
        imgt,tm12,TDR,tbox=Target2(image); 
        KBARR,MBAR,BDR,BH8,xc,yc,yb,datax,clone1=road(image,tm12)
        s1=cv2.resize(image,(320,240))
        s2=np.copy(s1); s3=np.copy(s1); s4=np.copy(s1)
        hs12=np.hstack((s1,s2))
        hs34=np.hstack((s3,s4))
        hs1234=np.vstack((hs12,hs34))
        cv2.imshow('hs1234',hs1234); out.write(hs1234)
        if(i==0): TDR0=TDR
        print('R_i:TDR0,BDR=',i,TDR0,BDR)
        if(TDR0 != 1 and BDR>1): 
            motcam(cap,win,out,1,-0.4,0.4,0.05); turn(0.0,0.0,0.10)
        if(TDR0 != 1 and BDR==1): print('BDR=1, return R_i=',i,BDR); return
        if(TDR==1): 
            print('R_i:return TDR0,BDR=',i,TDR0,BDR)
            return
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)

def Lturn(cap,win,out):
    #sleep(1)
    motcam(cap,win,out,4,-0.4,0.4,0.1); motcam(cap,win,out,5,0.4,0.4,0.1)
    motcam(cap,win,out,3,0.4,-0.4,0.1); turn(0.0,0.0,0.05)
    for i in range(40):
        ret,image=cap.read()
        imgt,tm12,TDR,tbox=Target2(image); 
        KBARR,MBAR,BDR,BH8,xc,yc,yb,datax,clone1=road(image,tm12)
        s1=cv2.resize(image,(320,240))
        s2=np.copy(s1); s3=np.copy(s1); s4=np.copy(s1)
        hs12=np.hstack((s1,s2))
        hs34=np.hstack((s3,s4))
        hs1234=np.vstack((hs12,hs34))
        cv2.imshow('hs1234',hs1234); out.write(hs1234)
        if(i==0): TDR0=TDR
        print('L_i: TDR0,BDR=',i,TDR0,BDR)
        if(TDR0 != 1 and BDR>1): 
            motcam(cap,win,out,1,0.4,-0.4,0.05); turn(0.0,0.0,0.10)
        if(TDR0 != 1 and BDR==1): print('BDR=1, return L_i=',i,BDR); return
        if(TDR==1): 
            print('L_i:return TDR0,BDR=',i,TDR0,BDR)
            return
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)

def RSturn(cap,win,out):
    #sleep(1)
    print('IN RSturn... type out=',type(out))
    motcam(cap,win,out,4,0.4,-0.4,0.1); motcam(cap,win,out,5,0.4,0.4,0.1)
    motcam(cap,win,out,4,-0.0,0.4,0.1); turn(0.0,0.0,0.05)
    for i in range(40):
        ret,image=cap.read()
        imgt,tm12,TDR,tbox=Target2(image); 
        KBARR,MBAR,BDR,BH8,xc,yc,yb,datax,clone1=road(image,tm12)
        s1=cv2.resize(image,(320,240))
        s2=np.copy(s1); s3=np.copy(s1); s4=np.copy(s1)
        hs12=np.hstack((s1,s2))
        hs34=np.hstack((s3,s4))
        hs1234=np.vstack((hs12,hs34))
        cv2.imshow('hs1234',hs1234); out.write(hs1234);#print('RS write done.')
        if(i==0): TDR0=TDR
        print('RS_i:TDR0,BDR=',i,TDR0,BDR)
        if(TDR0<1 and BDR>1): 
            motcam(cap,win,out,1,-0.4,0.4,0.1); turn(0.0,0.0,0.05)
        if(TDR0 != 1 and BDR==1): print('BDR=1, return RS_i=',i,BDR); return
        if(TDR==1): 
            print('RS_i:return TDR0,BDR=',i,TDR0,BDR)
            return
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
    print('Failed RSturn...')
    return

def LSturn(cap,win,out):
    #sleep(1)
    print('IN LSturn...')
    motcam(cap,win,out,4,-0.4,0.4,0.1); motcam(cap,win,out,5,0.4,0.4,0.1)
    motcam(cap,win,out,4,0.4,-0.4,0.1); turn(0.0,0.0,0.1)
    for i in range(40):
        ret,image=cap.read()
        imgt,tm12,TDR,tbox=Target2(image); 
        KBARR,MBAR,BDR,BH8,xc,yc,yb,datax,clone1=road(image,tm12)
        s1=cv2.resize(image,(320,240))
        s2=np.copy(s1); s3=np.copy(s1); s4=np.copy(s1)
        hs12=np.hstack((s1,s2))
        hs34=np.hstack((s3,s4))
        hs1234=np.vstack((hs12,hs34))
        cv2.imshow('hs1234',hs1234); out.write(hs1234);#print('LS write done.')
        if(i==0): TDR0=TDR
        print('LS_i:TDR0,BDR=',i,TDR0,BDR)
        if(TDR0<1 and BDR>1): 
            motcam(cap,win,out,4,0.4,-0.4,0.1); turn(0.0,0.0,0.10)
        if(TDR0 != 1 and BDR==1): print('BDR=1, return LS_i=',i,BDR); return
        if(TDR==1): 
            print('LS_i:return TDR0,BDR=',i,TDR0,BDR)
            return
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
    print('Failed RSturn...')
    return


def BTLturn(cap,win,out):
    print('IN BTLturn...')
    #cv2.waitKey(0)
    motcam(cap,win,out,6,-0.4,0.4,0.1); motcam(cap,win,out,10,0.4,0.4,0.1)
    motcam(cap,win,out,6,0.4,-0.4,0.1); turn(0.0,0.0,0.1)


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

if __name__ == '__main__':

    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('WYC.avi',fourcc,20.0,(Lx,Ly))
    ret,image=cap.read()
    LXM=image.shape[1]; LYM=image.shape[0]; WALL=int(LXM*0.5)
    sleep(2)
    low1=np.array([50,100,40]);upp1=np.array([70,255,255]) #G
    low2=np.array([170,150,50]);upp2=np.array([180,255,255]) # R
    low3=np.array([0,150,50]);upp3=np.array([10,255,255]) # R
    low1=np.array([50,100,40]);upp1=np.array([70,255,255]) #G
    low_rd = np.array([0  ,  0,100]); upp_rd = np.array([255, 40,255])
    print('yiscan=',yiscan,'  BADISx=',BADISx)
    #--------------------
    H8=0; KTARG=0; Nmiss=0; DR=0; NRCH=0
    for k in range(1803):
        DR=-5
        if(NRCH>5): break
        ret,image=cap.read()
        imgt,tm12,TDR,tbox=Target2(image); 
        KBARR,MBAR,BDR,BH8,xc,yc,yb,datax,clone1=road(image,tm12)
        #print('k_KTARG=',k,KTARG,tbox[5],'KBARR=',KBARR,'T,BDR=',TDR,BDR,tbox)
        H8a=tbox[-1]; #print('tbox=',tbox,H8a)
        if(KTARG==1 and tbox[5]<1): 
            if(Nmiss>5): 
                print('skip KTARG; reset... KBARR=',KBARR,BDR,datax)
                KTARG=0; 
                BTLturn(cap,win,out)
                continue
            else: 
                Nmiss+=1; 
                print('Nmiss=',Nmiss,TDR,tbox[5]); sleep(0.2); continue
        if(tbox[5]==1): KTARG=tbox[5]; Nmiss=0
        if(H8a>H8): H8=H8a
        if(TDR==0): DR=BDR
        if(TDR==0 and KBARR==-1): DR=BDR
        if(TDR>0 and KBARR>0): 
            dx=abs(tbox[0]-datax[2][0]); dy=abs(tbox[1]-datax[2][1])
            if(H8<12 and BDR>10): DR=BDR
            if(dx<8 and dy<8 and H8>9): DR=TDR; #print('T=B,dx,dy=',dx,dy)
            else: 
                DR=BDR
                if(BDR>20 and (xc<15 or xc>65 or H8>7)): DR=TDR
        if(TDR>0 and KBARR<1): DR=TDR

        s1=cv2.resize(image,(320,240))
        s2=cv2.resize(image,(320,240))
        s3=cv2.resize(imgt,(320,240))
        s4=cv2.resize(clone1,(320,240))
        hs12=np.hstack((s1,s2))
        hs34=np.hstack((s3,s4))
        hs1234=np.vstack((hs12,hs34))
        cv2.imshow('hs1234',hs1234); out.write(hs1234)

        if(H8>20): print('k=',k,'REACH TG,H8=',H8,NRCH);DR=0;NRCH+=1;sleep(2)
        else:print(k,'DR=',DR,TDR,BDR,'T,B=',KTARG,KBARR,
             'H8,BH8,yb=',H8,BH8,yb)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        if(k==0): cv2.waitKey(0); sleep(4)
        #if(DR>10): cv2.waitKey(0)
        cv2.waitKey(0)
        #Lturn(cap,win,out)
        #input('eeee')
        #DR=0
        if(DR==11):turn(0.3,0.3,0.10); turn(0,0,0.05);
        if(DR==1):turn(0.3,0.3,0.10); turn(0,0,0.10);
        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==13): Lturn(cap,win,out)
        if(DR==17): Rturn(cap,win,out)
        if(DR==33): turn(0.0,0.4,0.05); turn(0,0,0.05);
        if(DR==37): turn(0.4,0.0,0.05); turn(0,0,0.05);
        if(DR==23): LSturn(cap,win,out)
        if(DR==27): RSturn(cap,win,out)
        if(DR==0): turn(0.0,0.0,0.10); turn(0,0,0.05);

    #cv2.waitKey(0)
    cv2.destroyAllWindows();        
    cap.release()
    out.release()