# C5.py  --- Target of 2 colors
import cv2
import numpy as np
from gethsv import hsvcolor
def sh0(win,img): cv2.imshow(win,img);
def cw(k): cv2.waitKey(k)
def rd2(x): return round(x,2)
#----------------------------------------------
def cntr(img):
    cntsT,_=cv2.findContours(img,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
    return cntsT
#----------------------------------------------
def hsvcolor(img,XC,YC,R):
    Lx=img.shape[1]; Ly=img.shape[0];
    img1=np.copy(img)
    x1=XC-R; x2=XC+R; y1=YC-R; y2=YC+R
    img2=img[y1:y2,x1:x2,:]
    cv2.rectangle(img1,(x1,y1),(x2,y2),(255,255,255),2)
    h=[0,0,0]
    hsv=cv2.cvtColor(img2, cv2.COLOR_BGR2HSV)
    for i in range(3): h[i]=int(np.average(hsv[:,:,i]))
    return h,img1
#--------------------------------------------------------
def setlowup():
    if(1==2):
        for j in range(6):
            ret,frame=cap.read()
            h,img1=hsvcolor(frame,XC,YC,R)
            print(j,h)
            cv2.imshow('img1',img1); cv2.waitKey(0)
        #print('Lx...=',(Lx,Ly),XC,YC,R)
    else:
        h=np.array([56, 121, 149])
        h=np.array([35, 168, 121]) # green on floor
        h=np.array([46, 102, 170]) #green on desk 216
    lowT=np.array([h[0]-10, h[1]-50, 50])
    uppT=np.array([h[0]+10, h[1]+50, 255])
    return h,lowT,uppT

################### main ##############################
Lx=640; Ly=480; F=8; Lx2=int(Lx/F); Ly2=int(Ly/F); t=0
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly)
XC=int(Lx*0.5); YC=int(Ly*0.5); R=Lx//80;
h,lowT,uppT=setlowup(); print('h,low,up=',h,lowT,uppT)
lowR1=np.array([170, 120,  20])
uppR1=np.array([180,255, 255]); 
lowR2=np.array([0,  120,  20])
uppR2=np.array([10, 255, 255]); 
lowG=np.array([45-15, 30, 30])
uppG=np.array([45+15, 255, 255])
lowB=np.array([110-10, 60,  60])
uppB=np.array([110+10, 255, 255]); 
ret,frame=cap.read();
hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV);
maskB=cv2.inRange(hsv,lowB,uppB); #sh0('maskB',maskB);cw(0)

while(t<2501 and cap.isOpened()):
    t+=1; ret,frame=cap.read(); imgb=np.copy(frame)
    if ret == False: break
    hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV);
    maskg=cv2.inRange(hsv,lowG,uppG)
    cnts=cntr(maskg)
    im0=np.copy(frame); im0[:,:,:]=255
    imgc=cv2.bitwise_and(imgb,imgb,mask=maskg)
    imgg=cv2.bitwise_and(imgb,imgb,mask=maskg)
    nC=0; imT=np.copy(im0); 
    for j,C in enumerate(cnts):
        x,y,w,h=cv2.boundingRect(C); cx=x+w//2; cy=y+h//2; Arj=w*j
        if(w<Lx//10): continue
        nC+=1
        im=np.copy(im0)
        im[0:h,0:w,:]=frame[y:y+h,x:x+w,:]
        hsvj=cv2.cvtColor(im,cv2.COLOR_BGR2HSV);
        maskB=cv2.inRange(hsvj,lowB,uppB)
        SUMjC=np.sum(maskB)
        if(SUMjC/255<Arj*0.01): continue
        imT[y:y+h,x:x+w,:]=frame[y:y+h,x:x+w,:]; #sh0('imT',imT)
    hst=np.hstack((frame,imgg,imT))
    hst2=cv2.resize(hst,(int(Lx*0.8*3),int(Ly*0.8)))
    sh0('hst2',hst2); 
    if cv2.waitKey(1) & 0xFF == ord('q'): break
    continue
cap.release()
cv2.destroyAllWindows()