import numpy as np
import cv2
from  motorm import turn

Lx=640; Ly=480; F=8; LX2=int(Lx/F); LY2=int(Ly/F)
PIN=[171, 188, 184]
lower = np.array([PIN[0]-50,PIN[1]-100,120])
upper = np.array([PIN[0]+20,255,255])
lower = np.array(lower, dtype="uint8")
upper = np.array(upper, dtype="uint8")
print('PIN=',PIN,' lower=',lower,' upper=',upper)

def RED_both1(img):
    img1=np.copy(img)
    imgA=np.copy(img)
    imgB=np.copy(img)
    img2=cv2.resize(img1,(LX2,LY2))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    Y=LY2-1; XM=int(LX2/2); RED1=[]; ra=1.5; RED2=[]; NHA=0; 
    sh1=[0,0,0]; sh2=[0,0,0]; sha=[0,0,0]
    for x in range(LX2):
        if(img2[Y][x][2] > 255*0.2 and img2[Y][x][2] > 1.5*img2[Y][x][1]
            and img2[Y][x][2] > 1.5*img2[Y][x][0]): 
            RED1.append(x)
            for t in range(3): sh1[t]+=img2[Y,x,t] 
        if(lower[0]<hsv[Y,x,0]<upper[0]): 
            RED2.append(x)
            for t in range(3): sh2[t]+=hsv[Y,x,t] 
        if(hsv[Y,x,0]>100): 
            NHA+=1; 
            for t in range(3): sha[t]+=hsv[Y,x,t] 
    LRED1=len(RED1); LRED2=len(RED2); 
    if(LRED1>0):
        for t in range(3): sh1[t]=int(sh1[t]/LRED1)
    if(LRED2>0):
        for t in range(3): sh2[t]=int(sh2[t]/LRED2)
    if(NHA>0):
        for t in range(3): sha[t]=int(sha[t]/NHA)
    #print('LRED1,sh1=',LRED1,sh1,LRED2,sh2,NHA,sha)
    if(LRED1 > 0):
        MIDL1=int(np.median(RED1))
        cv2.line(imgA,(MIDL1*F,0),(MIDL1*F,Y*F),(0,255,0),2)
        if(30<=MIDL1<=50): DR1=1
        elif(50<MIDL1<80): DR1=7
        elif(0<=MIDL1<30): DR1=3
    else: 
        MIDL1=-1; DR1=-2
    if(LRED2 > 0):
        MIDL2=int(np.median(RED2))
        cv2.line(imgB,(MIDL2*F,0),(MIDL2*F,Y*F),(0,255,0),2)
        if(30<=MIDL2<=50): DR2=1
        elif(50<MIDL2<80): DR2=7
        elif(0<=MIDL2<30): DR2=3
    else: MIDL2=-1; DR2=-2
    #print('LRED1,2=',LRED1,LRED2,RED1,MIDL1,RED2,MIDL2)
    return DR1,LRED1,MIDL1,DR2,LRED2,MIDL2,imgA,sh1,sh2



#------------------- glare: retain glare in white, otherwise black ---
def MASK1(img):
    BLOW=180  # night_lamp=150; night_celling=100
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    blurred = cv2.GaussianBlur(gray, (5,5), 0)
    binc = cv2.Canny(blurred, 40, 150)  #70 shadow
    ret,thr1=cv2.threshold(gray,BLOW,255,cv2.THRESH_BINARY_INV)
    ret,thrw=cv2.threshold(gray,BLOW,255,cv2.THRESH_BINARY)
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (2,2))
    op1=cv2.morphologyEx(thr1,cv2.MORPH_OPEN,kernel,iterations=3)
    opw=cv2.morphologyEx(thrw,cv2.MORPH_OPEN,kernel,iterations=3)
    hbin1=np.hstack((gray,opw))
    binaryIMG=np.copy(op1)
    hbin2=cv2.resize(hbin1,(640,240))
    #cv2.imshow('hbin2',hbin2)
    mask=np.copy(opw)
    return mask


#------------------- retain red cv2.inRange --------------
def MASK2(img):
    img1=np.copy(img)
    hsv=cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
    mask2=cv2.inRange(hsv,lower,upper)
    return mask2





#img=cv2.imread('RED_14.png')
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, Lx)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Ly)
FW=open('GLARE.txt','w')
for j in range(10):
    fl='GLARE_'+str(j)+'.png'
    ret,img=cap.read()
    #img=cv2.imread('G1.png')
    imgA=np.copy(img)
    mask1=MASK1(img)
    mask2=MASK2(img)
    hmask=np.hstack((mask1,mask2))
    DR1,LRED1,MIDL1,DR2,LRED2,MIDL2,img1,sh1,sh2=RED_both1(img)
    res2=cv2.bitwise_and(imgA,imgA,mask=mask2)
    hstk1=cv2.resize(hmask,(640,240))
    hstk2=cv2.resize(img1,(320,240))
    cv2.imshow('hstk1',hstk1)
    cv2.imshow('hstk2',hstk2)
    a=(j,DR1,LRED1,MIDL1,DR2,LRED2,MIDL2,sh1,sh2)
    print(a)
    s='j='+str(a)+'\n'
    FW.write(s)
    cv2.imwrite(fl,img1)
    cv2.waitKey(0)
    DR=DR1
    if(DR==1): turn(0.3,0.3,0.10); turn(0,0,0.05);
    if(DR==3): turn(0.0,0.4,0.10); turn(0,0,0.05);
    if(DR==7): turn(0.4,0.0,0.10); turn(0,0,0.05);
    if(DR==0): turn(0.0,0.0,0.10); turn(0,0,0.05);
    if(DR==-2): turn(-0.3,-0.3,0.05); turn(0,0,0.05);

FW.close()