# ===================Cbttm.py
import cv2
import numpy as np
from  motorm import turn
from time import sleep

F=8; LX=640; LY=480; LX2=LX//F; LY2=LY//F; lenmin=int(LX2*0.05)
Y=LY2-1; XM=int(LX2/2); XL=int(LX2/3); XR=int(LX2*2/3)
print('Ls=',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([85, 100, 100])        # blue-green
uppG=np.array([105, 255, 255])      # blue-green
#----------------------------------------------------
lowR1=np.array([0, 50, 50])
uppR1=np.array([5, 255, 255])
lowR2=np.array([170, 50, 50])
uppR2=np.array([180, 255, 255])
lowR3=np.array([120, 40, 160])
uppR3=np.array([180, 110, 255])
#lowR4=np.copy(lowR3)
#lowR4=np.array([150, 60, 140])
#uppR4=np.array([180, 255, 200])
#color='blue'
#color='green'
color='red'
def RED_bot1(img):
    img1=np.copy(img); img2=np.copy(img); 
    img2=cv2.resize(img1,(LX2,LY2)); img3=np.copy(img2);
    hsv=cv2.cvtColor(img2, cv2.COLOR_BGR2HSV)
    if(color == 'red'): 
        mask3=cv2.inRange(hsv,lowR3,uppR3);
        #mask4=cv2.inRange(hsv,lowR4,uppR4);
        mask1=cv2.inRange(hsv,lowR1,uppR1);
        mask2=cv2.inRange(hsv,lowR2,uppR2); 
        #mask=mask1 | mask2 | mask3 | mask4
        mask=mask1 | mask2 | mask3 
    if(color=='blue'): mask=cv2.inRange(hsv,lowB,uppB)
    if(color=='green'): mask=cv2.inRange(hsv,lowG,uppG)
    img4=np.copy(img2); img4[:,:,:]=255; RED=[]
    for x in range(LX2):
        if(mask[Y,x]>200): RED.append(x); img4[Y-5:Y,x,:]=img2[Y-5:Y,x,:]
    LRED=len(RED)
    if(n<10):
        for i in range(LX2): 
            #if(mask[Y,i]>200):
            print(i,hsv[Y,i,:],mask[Y,i],mask1[Y,i],mask2[Y,i],mask3[Y,i])
        #cv2.waitKey(0)
    if(LRED > lenmin):
        MIDL=int(np.median(RED))
        cv2.line(img2,(MIDL*F,Y*F//4),(MIDL*F,Y*F),(0,255,0),4)
        if(XL<=MIDL<=XR): DR=1
        elif(XR<MIDL<LX2): DR=7
        elif(0<=MIDL<XL): DR=3
    else: 
        MIDL=-1; DR=-2;
    masked=cv2.bitwise_and(img3,img3,mask=mask)
    return DR,LRED,MIDL,img1,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(1)
n=0
while(n<69 and cap.isOpened()):
    ret, frame = cap.read()
    if(ret==False): break
    DR,LRED,MIDL,img1,img2,img4=RED_bot1(frame)
    hst=np.hstack((img2,img4))
    hst2=cv2.resize(hst,(LX*2,LY))
    print(n,DR,LRED,MIDL)
    cv2.imshow('hst2',hst2); 
    if(n<10): cv2.waitKey(0)
    if(DR==1): turn(0.4,0.4,0.06); turn(0,0,0.01);
    if(DR==-2): turn(-0.3,-0.3,0.05); turn(0,0,0.03);
    if(DR==3): turn(0.0,0.4,0.05); turn(0,0,0.01);
    if(DR==7): turn(0.4,0.0,0.05); turn(0,0,0.01);
    if cv2.waitKey(500) & 0xFF == ord('q'): break
    n+=1
cap.release()
cv2.destroyAllWindows()
print('Cbttm.py is done...')