# C4.py --- inRange of green or/and red. use waitKey() / key == ord('1')
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 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//40;
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]);
MK=1
while(t<2501 and cap.isOpened()):
    t+=1; ret,frame=cap.read(); imgb=np.copy(frame)
    if ret == False: break
    img2=cv2.resize(frame,(Lx2,Ly2)); sh0('img2',img2)
    hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV);
    maskG=cv2.inRange(hsv,lowG,uppG)
    maskR1=cv2.inRange(hsv,lowR1,uppR1)
    maskR2=cv2.inRange(hsv,lowR2,uppR2)
    maskR=maskR1 | maskR2 
    mask3=maskG | maskR
    mask4=maskG & maskR
    if(MK==1): mask2=np.copy(maskG)
    if(MK==2): mask2=np.copy(maskR)
    if(MK==3): mask2=np.copy(mask3)
    if(MK==4): mask2=np.copy(mask4)
    print(t,MK)
    cnts=cntr(mask2)
    imgc=cv2.bitwise_and(imgb,imgb,mask=mask2)
    #.........................................
    for j,C in enumerate(cnts):
        x,y,w,h=cv2.boundingRect(C); cx=x+w//2; cy=y+h//2; 
        if(w<Lx//10): continue
        cv2.rectangle(imgc,(x,y),(x+w,y+h),(255,128,0),4)
    #.........................................
    maskc=cv2.cvtColor(maskG,cv2.COLOR_GRAY2BGR)
    hst=np.hstack((frame,maskc,imgc))
    hst2=cv2.resize(hst,(int(Lx*0.8*3),int(Ly*0.8)))
    sh0('hst2',hst2); 
    key = cv2.waitKey(1) & 0xFF
    if key == ord('q'): break
    elif key == ord('1'): MK=1
    elif key == ord('2'): MK=2
    elif key == ord('3'): MK=3
    elif key == ord('4'): MK=4
cap.release()
cv2.destroyAllWindows()