# -*- coding: utf-8 -*-
from lane import *
from svm_pipeline import *
from yolo_pipeline import *
import numpy as np      # 导入numpy库
import cv2              # 导入Opencv库




#初始化我们要测的数据(可以理解成视频分成的图片帧)
IMAGE_PATHS = ["test1.jpg","test2.jpg", "test3.jpg","test4.jpg"]

class VehicleDistance:
    def __init__(self,known_distance):
        #初始估计一辆汽车的长和宽(单位:inches)
        self.KNOWN_HEIGHT = 60.00
        self.KNOWN_WIDTH = 137.00
        self.KNOWN_DISTANCE =known_distance

    #自定义目标函数
    def find_marker(self,image):
        img_undist, img_lane_augmented, lane_info = lane_process(image)
        boxes = vehicle_detection_yolo(img_undist, img_lane_augmented, lane_info)
        # [(yolo_center1, width_height1,angle1),(yolo_center2, width_height2,angle2),...]
        angle = 0
        # print(boxes)  #[((1154, 459), (216, 134)), ((884, 451), (138, 84))]
        # print(yolo_center,width_height,angle)
        return boxes

    # 定义距离函数
    def distance_to_camera(self,knownWidth, focalLength, perWidth):
        return (knownWidth * focalLength) / perWidth


    # 计算摄像头的焦距（内参）
    def calculate_focalDistance(self,img_path):
        # first_image = cv2.imread(img_path)      # 这里根据准备的第一张图片，计算焦距
        first_image = mpimg.imread(img_path)      # 这里根据准备的第一张图片，计算焦距
        # cv2.imshow('first image', first_image)
        marker = self.find_marker(first_image)       # 获取矩形的中心点坐标，长度，宽度和旋转角度
        focalLength = (marker[0][1][0] * KNOWN_DISTANCE) / self.KNOWN_WIDTH  # 获取摄像头的焦距
        # maker: [((1154, 459), (216, 134), 0), ((884, 451), (138, 84), 0)]
        print('焦距(focalLength) = ', focalLength)        # 打印焦距的值
        return focalLength


    # 计算摄像头到物体的距离
    def calculate_Distance(self,image_path, focalLength_value):
        # image = cv2.imread(image_path)
        image = mpimg.imread(image_path)
        # cv2.imshow("原图", image)
        marker = self.find_marker(image)     # 获取矩形的中心点坐标，长度，宽度和旋转角度， marke[0][1][0]代表宽度
        print('该图的矩形框有{}个：'.format(len(marker)))
        # print('maker:',marker)
        for i in range(len(marker)):
            distance_inches = self.distance_to_camera(self.KNOWN_WIDTH, focalLength_value, marker[i][1][0])
            box = cv2.boxPoints(marker[i])
            # print("Box = ", box)
            box = np.int0(box)
            print("Box = ", box)
            cv2.drawContours(image, [box], -1, (0, 255, 0), 2)      # 绘制物体轮廓
            #cv2.putText参数详解：图片、添加的文字、左上角坐标、字体、字体大小、颜色、字体粗细
            cv2.putText(image, "%.2fm" % (distance_inches * 2.54/100), (image.shape[1] - (i+1)*400, image.shape[0] - 20),
                        cv2.FONT_HERSHEY_SIMPLEX, 2.0, (0, 255, 0), 3)

        cv2.imshow("单目测距", image)
        cv2.imwrite("out" + str(count) + ".jpg", image)
        cv2.waitKey(2000)
        cv2.destroyAllWindows()

if __name__ == "__main__":

    # 这里我们假设可根据GPS大致定位到一辆车的位置
    # 米和英寸的单位转换：1 m ~= 39 inches
    KNOWN_DISTANCE = 500.00

    img_path = "test1.jpg"
    vehicle_distance = VehicleDistance(KNOWN_DISTANCE)
    focalLength = vehicle_distance.calculate_focalDistance(img_path)
    count = 1
    for imagePath in IMAGE_PATHS:
        print('第{}张图'.format(count))
        vehicle_distance.calculate_Distance(imagePath, focalLength)
        # cv2.waitKey(2000)
        count += 1
    cv2.destroyAllWindows()

    #调试阶段代码
    # img_path = "examples/test4.jpg"
    # image = mpimg.imread(img_path) #该方式可以读取到多个框框
    # image = cv2.imread(img_path)   #该方式只能读取到一个框框
    # find_marker(image)

    # focalLength = calculate_focalDistance(img_path)
    # calculate_Distance(img_path, focalLength)
    # cv2.waitKey(0)