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

def RED_bot1(img):
    img1=np.copy(img)
    img2 = cv2.resize(img1, (LX2,LY2), interpolation=cv2.INTER_AREA)
    img3=img2.astype(int)
    Y=LY2-1; XM=int(LX2/2); RED=[]; ra=1.5
    NW1=np.sum(img3[Y,:,:]); STD1=np.std(img3[Y,:,:]); 
    if(NW1<10000 and STD1<35): return 100,0,0,img1
    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


def QUT(cap):
    turn(0.4,-0.4,0.45); turn(0,0,0.01);
    for j in range(100):
        ret,frame = cap.read()
        DR,LRED,MIDL,img1=RED_bot1(frame)
        print('QUT,j=',j,DR)
        if(DR==1): return
        turn(0.3,-0.3,0.08); turn(0,0,0.05);
    print('Unfortunate QUT()...')
    return


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)
fourcc=cv2.VideoWriter_fourcc('m', 'p', '4', 'v')
out = cv2.VideoWriter('C4B_A.mp4', fourcc, 20.0, (LX,LY))
ret,frame = cap.read()
cv2.imshow('img1',frame)
if cv2.waitKey(2000) & 0xFF == ord('q'): pass
n=0
while(n<690 and cap.isOpened()):
    ret, frame = cap.read()
    if(ret==False): break
    DR,LRED,MIDL,img1=RED_bot1(frame)
    print(n,DR,LRED,MIDL)
    out.write(frame)
    cv2.imshow('img1',img1)
    if(DR==100): print('QUT DR=',DR); sleep(0.5); QUT(cap);
    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);
    if cv2.waitKey(1) & 0xFF == ord('q'): break
    n+=1
cap.release()
out.release()
cv2.destroyAllWindows()
print('C4B is done...')