import numpy as np
import cv2
from  motorm import turn
from time import sleep

Lx=640; Ly=480; F=4; LX2=int(Lx/F); LY2=int(Ly/F)
PIN=[171, 188, 184]
#建立兩個紅色遮罩的面具所需要的顏色範圍
#--------------------------- 161 < H < 181 ----
#lower = np.array([PIN[0]-60,0,0])
lower = np.array([PIN[0]-10,70,50])
upper = np.array([PIN[0]+10,255,255])
lower = np.array(lower, dtype="uint8")
upper = np.array(upper, dtype="uint8")
#--------------------------- 0 < H < 180 ----
low2 = np.array([0,70,50])
upp2 = np.array([10,255,255])
print('lower=',lower,' upper=',upper)

#---------------------從hsv的編碼決定底線的紅色和黑色的像素
def RED_BOTH(img):
    img1=np.copy(img)
    img2=cv2.resize(img1,(LX2,LY2))
    img3=img2.astype(int)
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    Y=LY2-1; XM=int(LX2/2); red=[]; sh=[0,0,0]; black=[]
    #NW1=np.sum(img3[Y,:,:]); STD1=np.std(img3[Y,:,:]);
    #if(NW1<30000 and STD1<15): return 100,0,0,sh,img1
    for x in range(LX2):
        #print(x,img2[Y,x,:],hsv[Y,x,:],len(black),len(red))
        if(hsv[Y,x,2]<70): black.append(x)
        if(lower[0]<hsv[Y,x,0]<upper[0] and lower[1]<hsv[Y,x,1]<upper[1]):
            red.append(x)
            for t in range(3): sh[t]+=hsv[Y,x,t]
    #cv2.imshow('img',img)
    #cv2.waitKey(0)
    #input('1111')
    if(len(black)>LX2/4): return 100,0,0,sh,img1
    lred=len(red)
    if(lred>0):
        for t in range(3): sh[t]=int(sh[t]/lred)
    if(lred > 0):
        midl=int(np.median(red))
        cv2.line(img1,(midl*F,0),(midl*F,Y*F),(0,255,0),2)
        if(60<=midl<=100): dr=1
        elif(100<midl<160): dr=7
        elif(0<=midl<60): dr=3
    else:
        midl=-1; dr=-2
    return dr,lred,midl,sh,img1


if __name__ == '__main__':
    from time import sleep
    print('bottclr as a main')
    cap = cv2.VideoCapture(0)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH,640)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480)
    sleep(2)
    ret,img=cap.read()
    dr,lred,midl,sh,img1=RED_BOTH(img)
    print('dr,lred,sh=',dr,lred,sh)
    cv2.imshow('img1',img1)
    cv2.waitKey(0)