import cv2
import numpy as np
from time import sleep

def BGR2HSV():
    img2=np.full((200,200,3),0,dtype=np.uint8)
    for j in range(1):
        BGR=input('(BGR='); #G=input('G='); R=input('R=')
        a=BGR.split()
        B=int(a[0]); G=int(a[1]); R=int(a[2])
        img2[:,:]=np.array([B,G,R], dtype=np.uint8)
        hsv = cv2.cvtColor(img2, cv2.COLOR_BGR2HSV)
        print('j=',j,img2[0,0],hsv[0,0])
        cv2.imshow('img2',img2)
        cv2.waitKey(0)
        sleep(1)
        cv2.destroyWindow("img2")
        #cv2.destroyWindow('image')
        #cv2.destroyAllWindows()


def HSV2BGR():
    hsv=np.full((200,200,3),0,dtype=np.uint8)
    for j in range(1):
        HSV=input('(HSV='); 
        a=HSV.split()
        H=int(a[0]); S=int(a[1]); V=int(a[2])
        hsv[:,:]=np.array([H,S,V], dtype=np.uint8)
        img2 = cv2.cvtColor(hsv, cv2.COLOR_BGR2HSV)
        print('j=',j,hsv[0,0],img2[0,0])
        cv2.imshow('img2',img2)
        cv2.waitKey(0)
        sleep(1)
        cv2.destroyWindow("img2")
        #cv2.destroyWindow('image')
        #cv2.destroyAllWindows()


#--------------- GET PIN color in HSV
def Fcolor(img):
    img2=cv2.resize(img,(80,60))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    PIN=[0,0,0]
    for i in range(3):
        a=int(np.average(img2[30:40,35:45,i]))
        b=int(np.average(hsv[30:40,35:45,i]))
        print(i,a,b)
        PIN[i]=b
    cv2.circle(img2,(40,35), 5, (255,0,0), 2)
    img4=cv2.resize(img2,(640,480))
    cv2.imshow('img4',img4)
    cv2.waitKey(0)
    print('PIN_Fcolor_hsv=',PIN)
    return PIN


def showhsv(pin):
    HSV=np.full((100,100,3),0,dtype=np.uint8)
    for j in range(10):
        H,S,V=pin[0]+3*j,pin[1], pin[2]
        print('j=',j,[H,S,V])
        HSV[:,:]=np.array([H,S,V], dtype=np.uint8)
        IMG = cv2.cvtColor(HSV, cv2.COLOR_HSV2BGR)
        cv2.imshow('IMG',IMG)
        cv2.waitKey(0)
        cv2.destroyWindow('IMG')
        sleep(1)
    input('11111')





#-----This part helps us learn the relationship between the two color encodings
if(1==2):
    for j in range(10):
        K=int(input('1=BGR2HSV; 2=HSV2BGR \n K='))
        if(K==1): BGR2HSV()
        if(K==2): HSV2BGR()
    input('BBBBBB')


Lx=640; Ly=480; F=8; Lx2=int(Lx/F); Ly2=int(Ly/F)
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly)
sleep(2)

#----------- Get the HSV_PIN of the target
if(1==1):
    for j in range(1):
        _,image=cap.read()
        PIN=Fcolor(image)
    input('1111')


#lower_blue = np.array([110,50,50])
#lower_blue = np.array([50,50,110])
#upper_blue = np.array([255,255,130])

#PIN=[175,190,130]
#[176, 198, 125]
# PIN_Fcolor_hsv= [176, 233, 113]  21-1A no lamp
#PIN=[176, 233, 113]

print('PIN_LOWER=',PIN)
#lower_red = np.array([PIN[0]-30,PIN[1]-50,PIN[2]-100])
#upper_red = np.array([PIN[0]+30,PIN[1]+50,PIN[2]+100])
lower_red = np.array([PIN[0]-30,PIN[1]-50,20])
upper_red = np.array([PIN[0]+30,PIN[1]+50,250])


while(1):
    _, frame = cap.read()
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    # Threshold the HSV image to get only red colors
    #mask = cv2.inRange(hsv, lower_blue, upper_blue)
    mask = cv2.inRange(hsv, lower_red, upper_red)
    # Bitwise-AND mask and original image
    res = cv2.bitwise_and(frame,frame, mask= mask)
    cv2.imshow('frame',frame)
    cv2.imshow('mask',mask)
    cv2.imshow('res',res)
    k = cv2.waitKey(5) & 0xFF
    if k == 27: break
cv2.destroyAllWindows()