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]-20,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('lower=',lower,' upper=',upper)

def RED_bot1(img):
    img1=np.copy(img)
    img2=cv2.resize(img1,(LX2,LY2))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    Y=LY2-1; XM=int(LX2/2); RED=[]; ra=1.5; RED2=[]
    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]): RED2.append(x)
        if(lower[0]<hsv[Y,x,0]<upper[0]): RED.append(x)
    LRED=len(RED); LRED2=len(RED2)
    print('LRED=',LRED,LRED2,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
    if(LRED2 > 0):
        MIDL2=int(np.median(RED2))
        if(30<=MIDL2<=50): DR2=1
        elif(50<MIDL2<80): DR2=7
        elif(0<=MIDL2<30): DR2=3
    else: MIDL2=-1; DR2=-2
    return DR,DR2,LRED,MIDL,img1


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


def RED_botA(img,IMG):
    #----------------------img
    img1=np.copy(img)
    img2=cv2.resize(img1,(LX2,LY2))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    Y=LY2-1; XM=int(LX2/2); RED=[]; ra=1.5; RED2=[]; sh=[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]): RED2.append(x)
        if(lower[0]<hsv[Y,x,0]<upper[0]): RED.append(x)
    LRED=len(RED); LRED2=len(RED2)
    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
    if(LRED2 > 0):
        MIDL2=int(np.median(RED2))
        if(30<=MIDL2<=50): DR2=1
        elif(50<MIDL2<80): DR2=7
        elif(0<=MIDL2<30): DR2=3
    else: MIDL2=-1; DR2=-2
    #----------------------IMG
    img1=np.copy(IMG)
    img2=cv2.resize(img1,(LX2,LY2))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    Y=LY2-1; XM=int(LX2/2); RED=[]; ra=1.5; RED2=[]; sr=[0,0,0]; sh=[0,0,0]
    NH=0; NHA=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]): 
            RED2.append(x)
            for t in range(3): sr[t]+=img2[Y,x,t]
        if(120<hsv[Y,x,0]): 
            NHA+=1
            #print(x,hsv[Y,x,:])
            for t in range(3): sha[t]+=hsv[Y,x,t]; 
        if(lower[0]<hsv[Y,x,0]<upper[0]): 
            NH+=1; RED.append(x)
            for t in range(3): sh[t]+=hsv[Y,x,t]
    LRED=len(RED); LRED2=len(RED2)
    print('LRED2,NH=',LRED2,NH,' sh=',sh,' NHA=',NHA,sha)
    if(LRED2>0):
        for t in range(3): sr[t]=int(sr[t]/LRED2)
    if(NH>0):
        for t in range(3): sh[t]=int(sh[t]/NH)
    for t in range(3): sha[t]=int(sha[t]/NHA)
    print('LRED2,NH=',LRED2,NH,'sr=',sr,' sh=',sh,sha)
    if(LRED > 0):
        NIDL=int(np.median(RED))
        cv2.line(img1,(NIDL*F,0),(NIDL*F,Y*F),(0,255,0),4)
        if(30<=NIDL<=50): FR=1
        elif(50<NIDL<80): FR=7
        elif(0<=NIDL<30): FR=3
    else: NIDL=-1; FR=-2
    if(LRED2 > 0):
        NIDL2=int(np.median(RED2))
        if(30<=NIDL2<=50): FR2=1
        elif(50<NIDL2<80): FR2=7
        elif(0<=NIDL2<30): FR2=3
    else: NIDL2=-1; FR2=-2
    return DR,DR2,FR,FR2,LRED,MIDL,img1


#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):
    ret,img=cap.read()
    mask=MASK1(img)
    fl='GLARE_'+str(j)+'.png'
    cv2.imwrite(fl,img)
    dst=cv2.inpaint(img, mask,3,1)
    #DR,DR2,LRED,MIDL,img1=RED_bot1(dst)
    DR,DR2,FR,FR2,LRED,MIDL,img1=RED_botA(dst,img)
    hstk=np.hstack((img,dst))
    hstk2=cv2.resize(hstk,(640,240))
    cv2.imshow('hstk2',hstk2)
    print('j=',j,DR,DR2,FR,FR2)
    a=(j,DR,DR2,FR,FR2)
    s='j='+str(a)+'\n'
    FW.write(s)
    cv2.waitKey(0)
FW.close()