# N4.py
import cv2
import numpy as np
from  motorm import turn

def RED_bot1(img):
    img1=np.copy(img)
    img2 = cv2.resize(img1, (LX2,LY2), interpolation=cv2.INTER_AREA)
    Y=LY2-1; XM=int(LX2/2); RED=[]; ra=1.5
    for x in range(LX2):
        if(img2[Y][x][2] > 50 and img2[Y][x][2] > ra*img2[Y][x][1]
            and img2[Y][x][2] > ra*img2[Y][x][0]): RED.append(x)
    LRED=len(RED)
    if(LRED > 0):
        MIDL=int(np.median(RED))
        cv2.line(img1,(MIDL*F,0),(MIDL*F,Y*F),(0,255,0),4)
        if(30<=MIDL<=50): DR=1
        elif(50<MIDL<80): DR=7
        elif(0<=MIDL<30): DR=3
    else: MIDL=-1; DR=-2
    return DR,LRED,MIDL,img1

F=8; LX=640; LY=480; LX2=int(LX/F); LY2=int(LY/F)
#cap = cv2.VideoCapture('REDL-1.mp4')
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('img1',frame)
if cv2.waitKey(2000) & 0xFF == ord('q'): pass
n=0
while(n<19 and cap.isOpened()):
    ret, frame = cap.read()
    if(ret==False): break
    DR,LRED,MIDL,img1=RED_bot1(frame)
    print(n,DR,LRED,MIDL)
    cv2.imshow('img1',img1)
    #if cv2.waitKey(1) & 0xFF == ord('q'): break
    cv2.waitKey(0)
    if(DR==1): turn(0.4,0.4,0.08); turn(0,0,0.01);
    if(DR==-2): turn(-0.25,-0.25,0.03); 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);
    n+=1
cap.release()
cv2.destroyAllWindows()
print('N4 is done...')