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


##################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):
    IMB=np.copy(img)
    img2=cv2.resize(img,(160,120))
    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
    op0=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]

    thr1=cv2.threshold(blurred,30,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)[1]
    thr2=cv2.threshold(blurred,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)
 
    #---for floor----------
    imgA=np.copy(img2) # floor with contour
    #contour1,hierarchy=cv2.findContours(thr1, cv2.RETR_EXTERNAL,
    contour1,hierarchy=cv2.findContours(op1, cv2.RETR_EXTERNAL,
                cv2.CHAIN_APPROX_NONE)
    c1x=max(contour1, key = cv2.contourArea)
    cv2.drawContours(imgA,c1x,-1,(255,125,0),2)

    #--- for barriers -------
    #contours,hierarchy=cv2.findContours(thr2, cv2.RETR_EXTERNAL,
    contours,hierarchy=cv2.findContours(op2, 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,125,255),2) # barriers with contours
        x,y,w,h = cv2.boundingRect(cx)
        #---place a rectangle on the largest barrier
        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((op0,op1,op2))
    h2=np.hstack((imgA,img2,img3,img4))
    cv2.imshow('h1',h1)
    cv2.imshow('h2',h2)
    cv2.waitKey(0)




#********************** 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()

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