import numpy as np
import cv2
from  motorm import turn
from time import sleep
from bottclr import bottclr
from RED_BOTH import RED_BOTH
from targs import targs
from targx import targx
from WYC import Target2
from WYC import sectionB
from WYC import barrier
from WYC import mgray
from WYC import motcam
from WYC import RSturn
from WYC import LSturn

#********************** MAIN ************************
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
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('carTB.mp4', fourcc, 20.0, (640,480))
sleep(2)
ret,img=cap.read()
imgt=np.copy(img)
if(1==1):
    H8=0
    for j in range(10):
        ret,image=cap.read()
        #............................................
        imgt,TDR,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,TDR,tbox,(x,y,w,h),XT8,DT8,H8)
        #............................................
        res2,Bdat=barrier(image); 
        lenB=len(Bdat)
        print('j_NTR=',j,lenB,' Bdat=',Bdat)
        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)
        cv2.waitKey(0)

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








        #dr,lred,midl,sh,img1=RED_BOTH(img)
        #botclr(img)
        #print('dr,lred,sh=',dr,lred,sh)
        #cv2.imshow('img1',img1)
        #boxes,h2=targs(MSK,low1,upp1,low2,upp2,img)
        #boxes,h2=targx(MSK,low1,upp1,low2,upp2,img)
        #print(j,len(boxes),boxes)
        #cv2.imshow('h2',h2)
        #cv2.imwrite('targ_red_'+str(j)+'.png',h2)