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('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)
    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)); 
        else: mx=-1
        #print(j,lred,mx)
        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,NL,lred,mx)
        #print(j,y,lred,(mx,y))
        cv2.circle(BAXC,(mx,y),5,(100,100,100),-1)
        cv2.line(BAXC,(0,y),(30,y),(100,100,100),2)
    BAXC=cv2.resize(BAXC,(BAXC.shape[1]*2,BAXC.shape[0]*2)); NLx=len(Nmx)
    #std=np.std(Nmx); ratstd=round(std/Y,2)
    if(NLx>2): MBAR=1
    else: MBAR=0
    #print('X,Y,lred0,red0=',X,Y,lred0,red0,'xs=',xs1,xs2)
    return MBAR,BAXC,xs1,xs2


def road(low_rd,upp_rd,image,tm12):
    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:40,:]=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,125,0),2)
        #print('N1=',N1,YN,d,datax)
    lendatax=len(datax)
    xc=0; yc=0; yb=0; KBARR=-1; MBAR=-1;
    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]
        if(yb<100): KBARR=0; MBAR=-1; BDR=1;  # far BARRIER
        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=hsec(BAX); #cv2.imshow('BAXC',BAXC)
            cv2.rectangle(clone1,(x+xs1,y),(x+xs2,y+h),(255,50,50),1); R=int((xs2-xs1)/2)
            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))
            #cv2.imshow('tm123_road',tm123)
            #cv2.imshow('tm12_road',tm12)
            BDR=1
            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
        #print(KBARR,x,y,w,h,datax)
    else: KBARR=0; MBAR=-1; BDR=1
    #print('KBARR,MBAR=',KBARR,MBAR,' BDR=',BDR,'xc=',xc,yc,yb)
    #mk2=np.hstack((thr1,barr))
    #h2=np.hstack((res,clone1))
    #h2=cv2.resize(h2,(320*2,240))
    #cv2.imshow('h2',h2)
    #cv2.waitKey(0)
    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,xc,yc,yb,datax,clone1


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)); 
    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,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 Rturn(cap,win):
    motcam(cap,win,3,0.8,0.0,0.1); motcam(cap,win,3,0.4,0.4,0.1)
    #motcam(cap,win,3,0.0,0.8,0.1); turn(0.0,0.0,0.5)
    for i in range(40):
        ret,image=cap.read()
        imgt,tm12,TDR,tbox=Target2(low1,upp1,low2,upp2,low3,upp3,image); 
        KBARR,MBAR,BDR,xc,yc,yb,datax,clone1=road(low_rd,upp_rd,image,tm12)
        if(i==0): TDR0=TDR
        if(TDR0<1 and BDR>0): 
            motcam(cap,win,4,0.0,0.8,0.1); turn(0.0,0.0,0.05)
            return
        print('R_i=',i,TDR,tbox)
        if(TDR==1): return
        motcam(cap,win,1,-0.4,0.4,0.05); turn(0.0,0.0,0.05)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)

def Lturn(cap,win):
    motcam(cap,win,3,0.0,0.8,0.1); motcam(cap,win,3,0.4,0.4,0.1)
    #motcam(cap,win,3,0.8,0.0,0.1); turn(0.0,0.0,0.05)
    for i in range(40):
        ret,image=cap.read()
        imgt,tm12,TDR,tbox=Target2(low1,upp1,low2,upp2,low3,upp3,image); 
        KBARR,MBAR,BDR,xc,yc,yb,datax,clone1=road(low_rd,upp_rd,image,tm12)
        if(i==0): TDR0=TDR
        if(TDR0<1 and BDR>0): 
            motcam(cap,win,4,0.8,0.0,0.1); turn(0.0,0.0,0.05)
            return
        print('L_i=',i,TDR,BDR,tbox,datax)
        if(TDR==1): cv2.waitKey(0); return
        motcam(cap,win,1,0.4,-0.4,0.05); turn(0.0,0.0,0.05)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)

def RSturn(cap,win):
    print('IN RSturn...')
    motcam(cap,win,4,0.8,0.0,0.1); motcam(cap,win,4,0.4,0.4,0.1)
    #motcam(cap,win,4,0.0,0.8,0.1); turn(0.0,0.0,0.05)
    for i in range(40):
        ret,image=cap.read()
        imgt,tm12,TDR,tbox=Target2(low1,upp1,low2,upp2,low3,upp3,image); 
        print('RS_i=',i,TDR,tbox)
        KBARR,MBAR,BDR,xc,yc,yb,datax,clone1=road(low_rd,upp_rd,image,tm12)
        if(i==0): TDR0=TDR
        if(TDR0<1 and BDR>0): 
            motcam(cap,win,5,0.0,0.8,0.1); turn(0.0,0.0,0.05)
            return
        if(TDR==1): return
        motcam(cap,win,1,-0.4,0.4,0.05); turn(0.0,0.0,0.05)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
    print('Failed RSturn...')
    return

def LSturn(cap,win):
    print('IN LSturn...')
    motcam(cap,win,4,0.0,0.8,0.1); motcam(cap,win,4,0.4,0.4,0.1)
    #motcam(cap,win,4,0.8,0.0,0.1); turn(0.0,0.0,0.5)
    for i in range(40):
        ret,image=cap.read()
        imgt,tm12,TDR,tbox=Target2(low1,upp1,low2,upp2,low3,upp3,image); 
        print('LS_i=',i,TDR,tbox)
        KBARR,MBAR,BDR,xc,yc,yb,datax,clone1=road(low_rd,upp_rd,image,tm12)
        if(i==0): TDR0=TDR
        if(TDR0<1 and BDR>0): 
            motcam(cap,win,5,0.8,0.0,0.1); turn(0.0,0.0,0.05)
            return
        if(TDR==1): return
        motcam(cap,win,1,0.4,-0.4,0.05); turn(0.0,0.0,0.5)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        #cv2.waitKey(0)
    print('Failed RSturn...')
    return



################################## 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('LRD.mp4',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
    for k in range(140):
        ret,image=cap.read()
        imgt,tm12,TDR,tbox=Target2(low1,upp1,low2,upp2,low3,upp3,image); 
        KBARR,MBAR,BDR,xc,yc,yb,datax,clone1=road(low_rd,upp_rd,image,tm12)
        #print(tbox,'datax=',datax)
        H8a=tbox[-1]; #print('tbox=',tbox,H8a)
        if(KTARG==1 and tbox[5]==0): Nmiss+=1; print('Nmiss=',Nmiss); 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): 
            dx=abs(tbox[0]-datax[2][0]); dy=abs(tbox[1]-datax[2][1])
            #if(H8<10 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==0): DR=TDR
        s1=cv2.resize(image,(320,240))
        s2=cv2.resize(imgt,(320,240))
        s3=cv2.resize(clone1,(320,240))
        hs123=np.hstack((s1,s2,s3))
        cv2.imshow('hs123',hs123)
        if(H8>20): print('k=',k,'REACH TARGET, H8=',H8); DR=0; sleep(2)
        else: print('k=',k,'DR=',DR,'TDR=',TDR,BDR,KBARR,MBAR,xc,yc,yb,'H8=',H8)
        if cv2.waitKey(1) & 0xFF == ord('q'): break
        if(k==0): cv2.waitKey(0); sleep(5)
        if(DR>10): cv2.waitKey(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.1); 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): Lturn(cap,win)
        if(DR==17): Rturn(cap,win)
        if(DR==23): LSturn(cap,win)
        if(DR==27): RSturn(cap,win)
        if(DR==0): turn(0.0,0.0,0.10); turn(0,0,0.05);

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