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]; black_2=10; black_1=10; black_0=10
#建立兩個紅色遮罩的面具所需要的顏色範圍
#--------------------------- 161 < H < 181 ----
#lower = np.array([PIN[0]-60,0,0])
lower = np.array([PIN[0]-10,100,50])
upper = np.array([180,255,255])
lower = np.array(lower, dtype="uint8")
upper = np.array(upper, dtype="uint8")
#--------------------------- 0 < H < 180 ----
low2 = np.array([0,100,50])
upp2 = np.array([10,255,255])
print('lower=',lower,' upper=',upper)


##################GET the color code on the bottom line########
def botclr(img):
    img1=np.copy(img)
    img2=cv2.resize(img1,(160,120))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    Y=119; 
    for x in range(160):
        print(x,img2[Y,x,:],hsv[Y,x,:])
    cv2.line(img2,(0,Y-6),(159,Y-6),(0,255,0),1)
    cv2.imshow('botclr',img2)
    cv2.waitKey(0)


low_rd = np.array([0,  0,   50])
upp_rd = np.array([255,20,  255])

def floor(img):
    BLOW=180  # night_lamp=150; night_celling=100
    IMB=np.copy(img)
    img2=cv2.resize(img,(160,120))
    ori=np.copy(img2)
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    gray=cv2.cvtColor(img2,cv2.COLOR_BGR2GRAY)
    blurred = cv2.GaussianBlur(gray, (5,5), 0)
    binaryIMG = cv2.Canny(blurred, 40, 150)  #70 shadow
    bb0=np.copy(binaryIMG)
    b0=np.copy(binaryIMG)
    thr1=cv2.threshold(gray,30,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)[1]
    thr2=cv2.threshold(gray,30,255,cv2.THRESH_BINARY_INV+cv2.THRESH_OTSU)[1]
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5))
    op1=cv2.morphologyEx(thr1,cv2.MORPH_OPEN,kernel,iterations=3)
    op2=cv2.morphologyEx(thr2,cv2.MORPH_OPEN,kernel,iterations=3)
    img3=cv2.bitwise_and(img2, img2, mask=op1)
    img4=cv2.bitwise_and(img2, img2, mask=op2)
 
    imgA=np.copy(img2)
    contour1,hierarchy=cv2.findContours(thr1, cv2.RETR_EXTERNAL,
                cv2.CHAIN_APPROX_NONE)
    c1x=max(contour1, key = cv2.contourArea)
    cv2.drawContours(imgA,c1x,-1,(0,255,0),2)
    contours,hierarchy=cv2.findContours(thr2, cv2.RETR_EXTERNAL,
                cv2.CHAIN_APPROX_NONE)
    print('lencont=',len(contours))

    if len(contours) != 0:
        cx=max(contours, key = cv2.contourArea)
        cv2.drawContours(img2,cx,-1,(0,255,0),2)
        x,y,w,h = cv2.boundingRect(cx)
        img3=cv2.rectangle(img3,(x,y),(x+w,y+h),(255,0,0),2)
        A=w*h
        box=[x,y,w,h,A]
        print('max_cx_area=',box)
    else:
        print('lencontour=0')
        Ax=150*110; box1=[0,0,0,0,0]; A1=0
    for c in contours:
        carea=cv2.contourArea(c)

    h1=np.hstack((b0,op1,op2))
    h2=np.hstack((imgA,img2,img3,img4))
    cv2.imshow('h1',h1)
    cv2.imshow('h2',h2)
    cv2.waitKey(0)







def floor2(img):
    img1=np.copy(img)
    img2=cv2.resize(img1,(160,120))
    hsv=cv2.cvtColor(img2,cv2.COLOR_BGR2HSV)
    mask1=cv2.inRange(hsv,low_rd,upp_rd)
    img3=cv2.bitwise_and(img2, img2, mask=mask1)
    thresh=np.copy(mask1)
    contours,hierarchy=cv2.findContours(thresh, cv2.RETR_EXTERNAL,
                cv2.CHAIN_APPROX_NONE)
    print('lencont=',len(contours))

    if len(contours) != 0:
        cx=max(contours, key = cv2.contourArea)
        print('max_cx_area=',cv2.contourArea(cx))
    else:
        print('encontour=0')
    nc=0; Ax=150*110; box1=[0,0,0,0,0]; A1=0
    for c in contours:
        carea=cv2.contourArea(c)
        if(carea>40):
            x,y,w,h = cv2.boundingRect(c)
            A=w*h
            if(A>Ax): continue
            cv2.drawContours(img3,c,-1,(0,255,0),2)
            nc+=1
            img3=cv2.rectangle(img3,(x,y),(x+w,y+h),(255,50*nc,0),2)
            box=[x,y,w,h,A]
            if(A>A1): A1=A; box1=box.copy()
            print(nc,box,box1)
    cv2.imshow('mask1',mask1)
    cv2.imshow('img3',img3)
    cv2.waitKey(0)





#********************** MAIN ************************

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('STF.mp4', fourcc, 20.0, (640,480))
sleep(2)
ret,img=cap.read()

#botclr(img)
for j in range(10):
    ret,img=cap.read()
    floor(img)