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

#---------------------------------------------------------------
def bottom2(img):
    Y2=Y//10; 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); 
    RED1=[]; RED2=[] 
    for x in range(LX2):
        if(mask[Y,x]>200): RED1.append(x); 
        if(mask[Y-Y2,x]>200): RED2.append(x); 
    LRED1=len(RED1); LRED2=len(RED2); 
    cv2.line(img1,(0,F*(Y-Y2)),(LX,F*(Y-Y2)),(255,255,0),2)
    if(LRED1 > lenmin):
        xc1=int(np.median(RED1)); 
        if(LRED2>0): xc2=int(np.median(RED2))
        else: xc2=-1
        cv2.line(img1,(xc1*F,Y*F//4*3),(xc1*F,Y*F),(0,255,0),4)
        if(LRED2<=LRED1+LX2//20):
            if(XL<=xc1<=XR): DR=1
            elif(XR<xc1<LX2): DR=7
            elif(0<=xc1<XL): DR=3
            else: xc1=-1; DR=-2
        else:
            if(XL<=xc2<=XR): DR=1
            elif(xc2>=xc1): DR=72
            elif(xc2<xc1): DR=32
    else: xc1=-1; xc2=-1; DR=-2
      
    masked=cv2.bitwise_and(img2,img2,mask=mask)
    return DR,LRED1,LRED2,xc1,xc2,img1,masked
#---------------------------------------------------------------

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, 50,  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='red'
#color='green';low=np.copy(lowG); upp=np.copy(uppG)
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()
#cv2.imshow('frame',frame); cv2.waitKey(0)
print('sleep 1s initializing the camera...')
sleep(1)
n=0
while(n<250 and cap.isOpened()):
    key=cv2.waitKey(1) & 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,LRED1,LRED2,xc1,xc2,img1,masked=bottom2(frame)
    #DR=32
    maskr=cv2.resize(masked,(LX,LY))
    hst=np.hstack((img1,maskr))
    print(n,color,DR,LRED1,LRED2,xc1,xc2)
    cv2.imshow('hst',hst); 
    if cv2.waitKey(500) & 0xFF == ord('q'): break
    #if(DR==32): print('sleep 5s'); cv2.waitKey(5000)
    #cv2.waitKey(0)
    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);
    if(DR==32): turn(0.4,0.4,0.20); turn(-0.4,0.8,0.35); turn(0,0,1.1);
    if(DR==72): turn(0.4,0.4,0.20); turn(0.8,-0.4,0.35); turn(0,0,1.1);
    n+=1
cap.release()
cv2.destroyAllWindows()
print('N4 is done...')