import numpy as np
import cv2


def find_marker(Img):
    kernel_2 = np.ones((2,2),np.uint8)#2x2的卷积核
    kernel_3 = np.ones((3,3),np.uint8)#3x3的卷积核
    kernel_4 = np.ones((4,4),np.uint8)#4x4的卷积核
    if Img is not None:#判断图片是否读入
        HSV = cv2.cvtColor(Img, cv2.COLOR_BGR2HSV)#把BGR图像转换为HSV格式
        '''
        HSV模型中颜色的参数分别是：色调（H），饱和度（S），明度（V）
        下面两个值是要识别的颜色范围
        '''
        Lower = np.array([0, 128, 46])#要识别红色颜色的下限
        Upper = np.array([5, 255,  255])#要识别红色颜色的上限
        #mask是把HSV图片中在颜色范围内的区域变成白色，其他区域变成黑色
        mask = cv2.inRange(HSV, Lower, Upper)
        #下面四行是用卷积进行滤波
        erosion = cv2.erode(mask,kernel_4,iterations = 1)
        erosion = cv2.erode(erosion,kernel_4,iterations = 1)
        dilation = cv2.dilate(erosion,kernel_4,iterations = 1)
        dilation = cv2.dilate(dilation,kernel_4,iterations = 1)
        #target是把原图中的非目标颜色区域去掉剩下的图像
        target = cv2.bitwise_and(Img, Img, mask=dilation)
        #将滤波后的图像变成二值图像放在binary中
        ret, binary = cv2.threshold(dilation,127,255,cv2.THRESH_BINARY)
        #在binary中发现轮廓，轮廓按照面积从小到大排列
        (_, cnts, _)= cv2.findContours(binary,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
        
        if cnts==[]:
           return 0
    c= max(cnts, key = cv2.contourArea) 
    return cv2.minAreaRect(c)
 
def distance_to_camera(knownWidth, focalLength, perWidth):  
    return (knownWidth * focalLength) / perWidth            
 
KNOWN_DISTANCE = 102
KNOWN_WIDTH = 19
KNOWN_HEIGHT = 8.27
image = cv2.imread("/home/pi/Pictures/distanceTest.jpeg") 
marker = find_marker(image)           
focalLength = (marker[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH

camera = cv2.VideoCapture(0)
while camera.isOpened():
    (grabbed, frame) = camera.read()
    marker = find_marker(frame)
    
    if marker == 0:
       cv2.imshow("captureR", frame)
       cv2.destroyWindow("captureR")
    
       continue
    inches = distance_to_camera(KNOWN_WIDTH, focalLength, marker[1][0])
   
    box = cv2.boxPoints(marker)
    box = np.int0(box)
    cv2.drawContours(frame, [box], -1, (0, 255, 0), 2)
    cv2.putText(frame, "%.2fcm" % (inches),
             (frame.shape[1] - 600, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX,
         2.0, (0, 255, 0), 3)
    cv2.imshow("capture", frame)
   
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
camera.release()
cv2.destroyWindow("capture")