#!usr/bin/python
# -*- coding: utf-8 -*-
 
#import the necessary packages
import numpy as np
import cv2
import imutils

# 找到目标函数
def find_marker(image):
    # convert the image to grayscale, blur it, and detect edges
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)  
    gray = cv2.GaussianBlur(gray, (5, 5), 0)        
    edged = cv2.Canny(gray, 35, 125) 

    # 显示灰度图
    # cv2.imshow("image", edged)
    # cv2.waitKey(0)         
    #        
    # find the contours in the edged image and keep the largest one;
    # we'll assume that this is our piece of paper in the image 
    ret, binary = cv2.threshold(edged, 127, 255, 0) # 扫描不到纸张轮廓时，要更改阈值，直到方框紧密框住纸张
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(3, 3))
    binary = cv2.dilate(binary, kernel, iterations=2) # 形态学膨胀
    cnts, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

    # 求最大面积 
    c = max(cnts, key = cv2.contourArea)
    # compute the bounding box of the of the paper region and return it
    # cv2.minAreaRect() c代表点集，返回rect[0]是最小外接矩形中心点坐标，
    # rect[1][0]是width，rect[1][1]是height，rect[2]是角度
    return cv2.minAreaRect(c)
 
# 距离计算函数 
def distance_to_camera(knownWidth, focalLength, perWidth):  
    # compute and return the distance from the maker to the camera
    return (knownWidth * focalLength) / perWidth   
  
# 显示轮廓
def draw_contours(image,marker):
    # draw a bounding box around the image and display it
    box = cv2.cv.BoxPoints(marker) if imutils.is_cv2() else cv2.boxPoints(marker)
    box = np.intp(box)
    cv2.drawContours(image, [box], -1, (0, 255, 0), 2)

# 显示距离
def draw_distance(image,marker):
    inches = distance_to_camera(KNOWN_WIDTH, focalLength, marker[1][0])
    # draw a bounding box around the image and display it
    box = cv2.cv.BoxPoints(marker) if imutils.is_cv2() else cv2.boxPoints(marker)
    box = np.intp(box)
    cv2.drawContours(image, [box], -1, (0, 255, 0), 2)
    cv2.putText(image, "%.2fcm" % (inches * 2.54),
        (image.shape[1] - 350, image.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX,
        2.0, (0, 255, 0), 3)
    # cv2.imshow("image", image)
    # cv2.waitKey(0)  

# 计算焦距
def calculate_focalDistance(image,marker):
    # 得到最小外接矩形的中心点坐标，长宽，旋转角度
    # 其中marker[1][0]是该矩形的宽度，单位为像素
    focalLength = (marker[1][0] * KNOWN_DISTANCE / 2.54) / KNOWN_WIDTH
    cv2.putText(image, "%.2f" % (focalLength),
        (20 , 70), cv2.FONT_HERSHEY_SIMPLEX,
        2.0, (0, 255, 0), 3)
    # 将计算得到的焦距打印出来
    return focalLength

# 通过固定照片标定
def read_photo():
   # 图片路径
    img_path = "img/pic_1.jpg"
    # load the image, find the marker in the image, then compute the
    # distance to the marker from the camera
    image = cv2.imread(img_path)
    # 获取屏幕尺寸
    screen_width, screen_height = 1920, 1080  # 替换成实际屏幕的尺寸
    # 计算缩放比例
    scale = min(screen_width / image.shape[1], screen_height / image.shape[0])
    # 缩放图像
    image = cv2.resize(image, None, fx=scale, fy=scale)
    # 获取目标
    marker = find_marker(image)
    # 显示轮廓
    draw_contours(image,marker)
    # 显示摄像头焦矩
    calculate_focalDistance(image,marker)
    # # # 显示距离
    # draw_distance(image,marker)
    # 显示图片
    cv2.imshow("image", image)
    cv2.waitKey(0)  

# 通过摄像头直接标定
def read_camera():
    cap = cv2.VideoCapture(0)
    while(1):
        # get a frame
        ret, image = cap.read()
        if not ret:
            print("无法打开摄像头")
            break 
        # 获取目标
        marker = find_marker(image)
        # 显示轮廓
        draw_contours(image,marker)
        # 显示距离
        draw_distance(image,marker)
        # 显示摄像头焦矩
        calculate_focalDistance(image,marker)
        cv2.imshow("capture", image)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    cap.release()
    cv2.destroyAllWindows() 

if __name__ == "__main__":
    # A4纸的长和宽(单位:cm)
    KNOWN_WIDTH = 29.7
    KNOWN_HEIGHT = 21.0
    # 已知距离(单位:cm)
    # KNOWN_DISTANCE = 11.81 # 30 cm
    # KNOWN_DISTANCE = 15.74 # 40 cm
    KNOWN_DISTANCE = 50 # 50 cm
    # 焦距
    focalLength = 0
    # 通过摄像头标定
    # read_photo()

    # 通过摄像头标定
    read_camera()