# ===================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 bottom6(img):
    img1=np.copy(img); img2=cv2.resize(img1,(LX2,LY2))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV); hh=hsv.tolist()
    mask=cv2.inRange(hsv,low,upp);
    masked=cv2.bitwise_and(img2,img2,mask=mask); mk2=np.copy(masked)
    #cv2.imshow('mask',mask); 
    DRS=[]; blus=[]
    for j,y in enumerate(YS):
        cv2.line(img1,(0,y*F),(LX,y*F),(0,255,0),1)
        RED=[]; blu=[]
        for x in range(LX2):
            if(mask[y,x]>200): RED.append(x); 
            if(hsv[y,x,2]>230 and hsv[y,x,1]<60): blu.append(x); 
        LRED=len(RED); blus.append(len(blu))
        if(len(blu)>10): 
            #print(y,blu,[hh[y][x][:] for x in blu]);input('1')
            cv2.line(mk2,(blu[0],y),(blu[-1],y),(0,100,0),2)
        if(LRED > 2):
            xc=int(np.median(RED))        
            hsv2=cv2.cvtColor(masked,cv2.COLOR_BGR2HSV); 
            #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  
        mk3=cv2.resize(mk2,(LX,LY)); sh0('mk3',mk3)
        DRS.append((xc,LRED,DR))
    cv2.line(masked,(0,y),(LX,y),(0,255,0),1); #print('blus=',blus)
    xc0=DRS[0][0]; #print('DRS[0]=',DRS[0],'xc0=',xc0,xc0*F,F)
    cv2.line(img1,(xc0*F,Y*F//4*3),(xc0*F,Y*F),(0,255,0),4)
    #sh0('img1',img1); cw(0)
    return DRS,img1,masked
#---------------------------------------------------------------
def go5(cap):
    for j in range(8):
        ret,frame=cap.read()
        DRS,ima,mka=bottom6(frame)
        dr=[]; md=[]; ln=[]
        for i in range(6): 
            md.append(DRS[i][0]); ln.append(DRS[i][1]); dr.append(DRS[i][2]);
        print(j,dr)
        turn(0.4,0.4,0.05); turn(0,0,0.1);
        sh0('ima',ima); cw(500)
        if(sum(dr)>-12): return
    cw(0)

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)
YS=[59,55,50,45,40,35]
print("L's=",LX,LY,LX2,LY2,' lenmin=',lenmin,'XL,R,M=',XL,XR,XM)
lowB= np.array([110, 100, 50])        # blue
uppB= np.array([125, 255, 255])      # 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()
img1=np.copy(frame)
print('sleep 2s initializing the camera...')
sleep(2)
n=0; fw=open('DRS.txt','w')
while(n<775 and cap.isOpened()):
    ret,frame = cap.read(); 
    if(ret==False): break
    DRS,img2,masked2=bottom6(frame)
    maskr2=cv2.resize(masked2,(LX,LY))
    vline=np.full((480,10,3),255,np.uint8);
    if(n>0): 
        hst=np.hstack((img1,vline,img2)); 
        hst=np.hstack((maskr1,vline,maskr2,vline,img2)); 
        hst2=cv2.resize(hst,(1500,400))
        cv2.imshow('hst2',hst2); 
    else: cv2.imshow('hst2',img2)
    dr=[]; md=[]; ln=[]
    for i in range(6): 
        md.append(DRS[i][0]); ln.append(DRS[i][1]); dr.append(DRS[i][2]);
    ss=str([n,dr])+'\n'; fw.write(ss)
    DR=DRS[0][2]; DR1=DR
    if(DR==-2): 
        if(dr[1]>-2): DR=dr[1]
        elif(dr[2]>-2): DR=dr[2] 
        elif(dr[3]>-2): DR=dr[3] 
        elif(dr[4]>-2): DR=dr[4] 
        elif(dr[5]>-2): DR=dr[5] 
        elif(sum(dr)==-12): go5(cap); continue
            
    n+=1; img1=np.copy(img2); maskr1=np.copy(maskr2)
    if(n==1): continue
    print(n,DR1,DR,'md=',md,ln)
    cw(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);
fw.close()
cap.release()
cv2.destroyAllWindows()