# ===================RGB-tracks.py
import cv2
import numpy as np
from  motorm import turn
from time import sleep
def cw(k): cv2.waitKey(k) 
def sh0(win,im): cv2.imshow(win,im)
#---------------------------------------------------------------
def bottom1(img):
    img1=np.copy(img); img2=cv2.resize(img1,(LX2,LY2))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV); RED=[];
    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);
    for x in range(LX2):
        if(mask[Y,x]>200): RED.append(x);
    LRED=len(RED);
    if(LRED > lenmin):
        xc=int(np.median(RED))
        cv2.line(img1,(xc*F,Y*F//4*3),(xc*F,Y*F),(0,255,0),4)
        if(XL<=xc<=XR): DR=1
        elif(XR<xc<LX2): DR=7
        elif(0<=xc<XL): DR=3
    else: xc=-1; DR=-2
    masked=cv2.bitwise_and(img2,img2,mask=mask)
    return DR,LRED,xc,img1,masked
#---------------------------------------------------------------
def cntr(img):
    cntsT,_=cv2.findContours(img,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
    return cntsT

F=1; 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(LX/3); XR=int(LX*2/3)
print("L's=",LX,LY,LX2,LY2,' lenmin=',lenmin,'XL,R,M=',XL,XR,XM)
lowB=np.array([105, 100, 100])        # blue
uppB=np.array([125, 255, 255])      # blue
lowG=np.array([80, 100, 50])        # blue-green
uppG=np.array([105, 255, 255])      # blue-green
lowR1=np.array([0, 80, 50])
uppR1=np.array([10, 255, 255])
lowR2=np.array([170, 80, 50])
uppR2=np.array([180, 255, 255])
#color='blue'
#color='green'
color='red'
def color_tracks(img):
    imgF=np.copy(img); img2=np.copy(img); img2=cv2.resize(imgF,(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
    if(color=='blue' ): mask=cv2.inRange(hsv,lowB,uppB); 
    if(color=='green'): mask=cv2.inRange(hsv,lowG,uppG); 
    maskF=cv2.resize(mask,(LX,LY))
    masked=cv2.bitwise_and(imgF,imgF,mask=maskF)
    cnts=cntr(maskF); 
    if(len(cnts)<1): return imgF,img2,masked
    C=max(cnts,key=cv2.contourArea)
    x,y,w,h=cv2.boundingRect(C); 
    cv2.rectangle(imgF,(x,y),(x+w,y+h),(255,255,255),4)
    return imgF,img2,masked

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 1s initializing the camera...')
sleep(2)
t=0
while(t<400):
    ret,frame=cap.read()
    if(ret==False): break
    DR,LRED,MIDL,img1,masked=bottom1(frame)
    maskr=cv2.resize(masked,(LX,LY))
    vst=np.vstack((img1,maskr)); cv2.imshow('vst',vst); cw(10)
    if(t%20==0): print(t,color,DR,LRED)
    t+=1
#cw(0)
n=0
while(n<100):
    ret,frame=cap.read()
    if(ret==False): break
    #imgF,img2,masked=color_tracks(frame)
    DR,LRED,MIDL,img1,masked=bottom1(frame)
    maskr=cv2.resize(masked,(LX,LY))
    #hst=np.hstack((img1,maskr)); cv2.imshow('hst',hst); 
    vst=np.vstack((img1,maskr)); cv2.imshow('vst',vst); 
    print(n,color,DR,LRED)
    #cv2.waitKey(0)
    key=cv2.waitKey(1000) & 0xFF
    if key == ord('q'): break
    elif key == ord('1'): color='red'
    elif key == ord('2'): color='green'
    elif key == ord('3'): color='blue'
    if(DR==1): turn(0.4,0.4,0.08); turn(0,0,0.1);
    if(DR==-2): turn(-0.3,-0.3,0.05); turn(0,0,0.1);
    if(DR==3): turn(0.0,0.4,0.05); turn(0,0,0.1);
    if(DR==7): turn(0.4,0.0,0.05); turn(0,0,0.1);
    cw(0)
    n+=1
cap.release()
cv2.destroyAllWindows()