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

def RED_bot2(img):
    img1=np.copy(img)
    img2 = cv2.resize(img1, (LX2,LY2), interpolation=cv2.INTER_AREA)
    Y=LY2-1; Y2=Y-15; XM=int(LX2/2); RED1=[]; RED2=[]; ra=1.5
    img3=img2.astype(int)
    NW1=np.sum(img3[Y,:,:]); NW2=np.sum(img3[Y2,:,:]); 
    STD1=np.std(img3[Y,:,:]); STD2=np.std(img3[Y2,:,:])
    #print('NW1,2=',NW1,NW2,STD2)
    if(NW2>42000 and STD2<20): 
        print('NW1,2=',NW1,NW2,STD1,STD2); return 100,0,0,0,0,img1
    for x in range(LX2):
        if(img2[Y][x][2] > 255*0.2 and img2[Y][x][2] > ra*img2[Y][x][1]
            and img2[Y][x][2] > ra*img2[Y][x][0]):
            RED1.append(x)
        if(img2[Y2][x][2] > 255*0.2 and img2[Y2][x][2] > ra*img2[Y2][x][1]
            and img2[Y2][x][2] > ra*img2[Y2][x][0]):
            RED2.append(x)
    LRED1=len(RED1); LRED2=len(RED2);
    if(LRED1==0): MIDL1=-1; DR=-2; MIDL2=-1
    if(LRED1 > 0):
        MIDL1=int(np.median(RED1))
        if(30<=MIDL1<=50): DR=1
        elif(50<MIDL1<80): DR=7
        elif(0<=MIDL1<30): DR=3
        DR1=DR
        if(LRED2 > 0):
            MIDL2=int(np.median(RED2))
            cv2.line(img1,(MIDL1*F,Y*F),(MIDL2*F,Y2*F),(0,255,0),4)
            if(30<=MIDL2<=50): 
                DR2=1
                if(DR1==1): DR=11
                if(DR1==3): DR=31
                if(DR1==7): DR=71
            elif(50<MIDL2<80): 
                DR2=7
                if(DR1==1): DR=17
                if(DR1==3): DR=37
                if(DR1==7): DR=77
            elif(0<=MIDL2<30): 
                DR2=3
                if(DR1==1): DR=13
                if(DR1==3): DR=33
                if(DR1==7): DR=73
        else:
            MIDL2=-1; 
            DR2=-2
            if(DR1==1): DR=12
            if(DR1==3): DR=32
            if(DR1==7): DR=72
    if(LRED1>0 and LRED2>0 and LRED2>LRED1+4):
        if(RED2[-1]>RED1[-1]+6): DR=53
        elif(RED2[0]<RED1[0]-6): DR=57
    return DR,LRED1,LRED2,MIDL1,MIDL2,img1

def QUT(cap):
    turn(0.4,-0.4,0.25); turn(0,0,0.01);
    for j in range(100):
        ret,frame = cap.read()
        DR,LRED1,LRED2,MIDL1,MIDL2,img1=RED_bot2(frame)
        print(j,DR)
        if(int(DR/10)==1): return
        turn(0.3,-0.3,0.08); turn(0,0,0.05);
    print('Unfortunate QUT()...')
    return

def QRT(cap):
    turn(0.4,0.4,0.35); turn(0.3,-0.3,0.30); turn(0,0,0.01);
    #sleep(1)
    for j in range(100):
        ret,frame = cap.read()
        DR,LRED1,LRED2,MIDL1,MIDL2,img1=RED_bot2(frame)
        print(j,DR)
        if(int(DR/10)==1): return
        turn(0.3,-0.3,0.08); turn(0,0,0.05);
    print('Unfortunate QRT()...')
    return

def QLT(cap):
    turn(0.4,0.4,0.25); turn(-0.3,0.3,0.30); turn(0,0,0.01);
    #sleep(1)
    for j in range(100):
        ret,frame = cap.read()
        DR,LRED1,LRED2,MIDL1,MIDL2,img1=RED_bot2(frame)
        print(j,DR)
        if(int(DR/10)==1): return
        turn(-0.3,0.3,0.08); turn(0,0,0.05);
    print('Unfortunate QLT()...')
    return

F=8; LX=640; LY=480; LX2=int(LX/F); LY2=int(LY/F)
#cap = cv2.VideoCapture('REDL.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(*'XVID')
out = cv2.VideoWriter('C7.mp4', fourcc, 20.0, (LX,LY))
ret,frame = cap.read()
cv2.imshow('img1',frame)
if cv2.waitKey(2000) & 0xFF == ord('q'): pass
cv2.imshow('img1',frame)
n=0
while(n<200 and cap.isOpened()):
    n+=1
    ret, frame = cap.read()
    if(ret==False): break
    #if(n<90): continue
    out.write(frame)
    DR,LRED1,LRED2,MIDL1,MIDL2,img1=RED_bot2(frame)
    print(n,DR,MIDL1,MIDL2,LRED1,LRED2,LRED2-LRED1)
    cv2.imshow('img1',img1)
    if cv2.waitKey(1) & 0xFF == ord('q'): break  
    if(DR==-2): turn(-0.25,-0.25,0.03); turn(0,0,0.03);
    if(DR==100): print('QUT DR=',DR); sleep(2); QUT(cap); 
    if(DR==53): print('QRT DR=',DR); sleep(1); QRT(cap); 
    if(DR==57): print('QLT DR=',DR); sleep(1); QLT(cap); 
    if(DR==11): turn(0.5,0.5,0.05); turn(0,0,0.01);
    if(DR==13): turn(0.5,0.4,0.05); turn(0,0,0.01);
    if(DR==17): turn(0.4,0.5,0.05); turn(0,0,0.01);
    if(DR==33): turn(-0.4,0.4,0.05); turn(0.4,0.0,0.04); turn(0,0,0.01);
    if(DR==31): turn(-0.4,0.4,0.05); turn(0.4,0.0,0.08); turn(0,0,0.01);
    if(DR==37): turn(-0.4,0.4,0.05); turn(0.4,0.0,0.12); turn(0,0,0.01);
    if(DR==77): turn(0.4,-0.4,0.05); turn(0.0,0.4,0.04); turn(0,0,0.01);
    if(DR==71): turn(0.4,-0.4,0.05); turn(0.0,0.4,0.08); turn(0,0,0.01);
    if(DR==73): turn(0.4,-0.4,0.05); turn(0.0,0.4,0.12); turn(0,0,0.01);
    if(DR==12): turn(-0.3,-0.3,0.05); turn(0,0,0.01);
    if(DR==32): turn(0.4,-0.4,0.08); turn(0,0,0.01);
    if(DR==72): turn(-0.4,0.4,0.08); turn(0,0,0.01);
cap.release()
out.release()
cv2.destroyAllWindows()
print('C6 is done...')