# ===================RGB-tracks.py
import cv2
import numpy as np
from  motorm import turn
from time import sleep

def cw(k): cv2.waitKey(k)
#---------------------------------------------------------------
def blurred(img):
    img1=np.copy(img); img2=cv2.resize(img1,(LX2,LY2))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    if(color == 'red'): 
        mask1=cv2.inRange(hsv,lowR1,uppR1)
        mask2=cv2.inRange(hsv,lowR2,uppR2); mask=mask1 | mask2
    else: mask=cv2.inRange(hsv,low,upp);
    #cv2.imshow('mask',mask); 
    RED=[]; cc=[]
    for x in range(LX2):
        #print(x,img2[Y,x,:],hsv[Y,x,:])
        if(mask[Y,x]>200): RED.append(x); 
    LRED=len(RED); masked=cv2.bitwise_and(img2,img2,mask=mask)
    if(LRED > lenmin):
        xc=int(np.median(RED))        
        hsv2=cv2.cvtColor(masked,cv2.COLOR_BGR2HSV); 
        for i in range(3): cc.append(int(np.average(hsv2[55:60,40:43,i])))
        cv2.rectangle(masked,(40,55),(43,60),(0,255,0),1);
        if(XL<=xc<=XR): DR=1
        elif(XR<xc<LX2): DR=7
        elif(0<=xc<XL): DR=3
    else: xc=-1; DR=-2  
    return DR,LRED,xc,img1,masked,cc
#---------------------------------------------------------------

F=8; LX=640; LY=480; LX2=int(LX/F); LY2=int(LY/F); lenmin=int(LX2*0.05)
Y=LY2-1; XM=int(LX2/2); RED=[]; XL=int(LX2/3); XR=int(LX2*2/3)
print("L's=",LX,LY,LX2,LY2,' lenmin=',lenmin,'XL,R,M=',XL,XR,XM)
lowR1=np.array([0, 50, 50])
uppR1=np.array([5, 255, 255])
lowR2=np.array([160, 50, 50])
uppR2=np.array([180, 255, 255])
lowB= np.array([105, 60, 50])        # blue
uppB= np.array([125, 255, 255])      # blue
lowG= np.array([85,  50,  50 ])        # blue-green
uppG= np.array([105, 255, 255])      # blue-green
color='blue'
#color='green'
#color='red'
if(color=='green'): low=np.copy(lowG); upp=np.copy(uppG)
if(color=='blue'):  low=np.copy(lowB); upp=np.copy(uppB)
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, LX)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, LY)
ret,frame = cap.read()
print('sleep 2s initializing the camera...')
sleep(2)
n=0
while(n<6250 and cap.isOpened()):
    key=cv2.waitKey(5) & 0xFF
    if key == ord('q'): break
    elif key == ord('1'): color='red'; 
    elif key == ord('2'): color='green'; low=np.copy(lowG); upp=np.copy(uppG)
    elif key == ord('3'): color='blue'; low=np.copy(lowB); upp=np.copy(uppB)
    ret,frame = cap.read()
    if(ret==False): break
    DR,LRED,MIDL,img1,masked,cc=blurred(frame)
    maskr=cv2.resize(masked,(LX,LY))
    hst=np.hstack((img1,maskr)); hst2=cv2.resize(hst,(1000,400))
    print(n,key,color,'DR=',DR,'LRED=',LRED,MIDL,'low,up=',low,upp,'cc=',cc)
    cv2.imshow('hst2',hst2); cw(0); 
    n+=1
cap.release()
cv2.destroyAllWindows()