import cv2
import numpy as np
import sys

import pygame
from pygame.locals import *

class Display(object):

    def __init__(self,Width,Height):
        pygame.init()
        pygame.display.set_caption('Drive Video')
        self.screen = pygame.display.set_mode((Width,Height),0,32)
    def paint(self,draw):
        self.screen.fill([0,0,0])

        draw = cv2.transpose(draw)
        draw = pygame.surfarray.make_surface(draw)
        self.screen.blit(draw,(0,0))
        pygame.display.update()



if __name__ == "__main__":
    solid_white_right_video_path = "test_videos/丹成學(xué)長(zhǎng)車道線檢測(cè).mp4"
    cap = cv2.VideoCapture(solid_white_right_video_path)
    Width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
    Height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))

    display = Display(Width,Height)

    while True:
        ret, draw = cap.read()
        draw = cv2.cvtColor(draw,cv2.COLOR_BGR2RGB)
        if ret == False:
            break
        display.paint(draw)
        for event in pygame.event.get():
                if event.type == QUIT:
                    sys.exit()

def color_select(img,red_threshold=200,green_threshold=200,blue_threshold=200):
    ysize,xsize = img.shape[:2]

    color_select = np.copy(img)

    rgb_threshold = [red_threshold, green_threshold, blue_threshold]

    thresholds = (img[:,:,0] < rgb_threshold[0]) \
            | (img[:,:,1] < rgb_threshold[1]) \
            | (img[:,:,2] < rgb_threshold[2])
    color_select[thresholds] = [0,0,0]

    return color_select

# 示例代碼，作者丹成學(xué)長(zhǎng)：Q746876041
def canny_edge_detect(img):
    gray = cv2.cvtColor(img,cv2.COLOR_RGB2GRAY)
    kernel_size = 5
    blur_gray = cv2.GaussianBlur(gray,(kernel_size, kernel_size),0)

    low_threshold = 180
    high_threshold = 240
    edges = cv2.Canny(blur_gray, low_threshold, high_threshold)

    return edges

# 示例代碼，作者丹成學(xué)長(zhǎng)：Q746876041
mask = np.zeros_like(edges)

    ignore_mask_color = 255 
    # 獲取圖片尺寸
    imshape = img.shape
    # 定義 mask 頂點(diǎn)
    vertices = np.array([[(0,imshape[0]),(450, 290), (490, 290), (imshape[1],imshape[0])]], dtype=np.int32)
    # 使用 fillpoly 來(lái)繪制 mask
    cv2.fillPoly(mask, vertices, ignore_mask_color)
    masked_edges = cv2.bitwise_and(edges, mask)
    # 定義Hough 變換的參數(shù)
    rho = 1 
    theta = np.pi/180
    threshold = 2

    min_line_length = 4 # 組成一條線的最小像素?cái)?shù)
    max_line_gap = 5    # 可連接線段之間的最大像素間距
    # 創(chuàng)建一個(gè)用于繪制車道線的圖片
    line_image = np.copy(img)*0 

    # 對(duì)于 canny 邊緣檢測(cè)結(jié)果應(yīng)用 Hough 變換
    # 輸出“線”是一個(gè)數(shù)組，其中包含檢測(cè)到的線段的端點(diǎn)
    lines = cv2.HoughLinesP(masked_edges, rho, theta, threshold, np.array([]),
                                min_line_length, max_line_gap)

    # 遍歷“線”的數(shù)組來(lái)在 line_image 上繪制
    for line in lines:
        for x1,y1,x2,y2 in line:
            cv2.line(line_image,(x1,y1),(x2,y2),(255,0,0),10)

    color_edges = np.dstack((edges, edges, edges)) 

import math
import cv2
import numpy as np

"""
Gray Scale
Gaussian Smoothing
Canny Edge Detection
Region Masking
Hough Transform
Draw Lines [Mark Lane Lines with different Color]
"""

class SimpleLaneLineDetector(object):
    def __init__(self):
        pass

    def detect(self,img):
        # 圖像灰度處理
        gray_img = self.grayscale(img)
        print(gray_img)
        #圖像高斯平滑處理
        smoothed_img = self.gaussian_blur(img = gray_img, kernel_size = 5)
        #canny 邊緣檢測(cè)
        canny_img = self.canny(img = smoothed_img, low_threshold = 180, high_threshold = 240)
        #區(qū)域 Mask
        masked_img = self.region_of_interest(img = canny_img, vertices = self.get_vertices(img))
        #霍夫變換
        houghed_lines = self.hough_lines(img = masked_img, rho = 1, theta = np.pi/180, threshold = 20, min_line_len = 20, max_line_gap = 180)
        # 繪制車道線
        output = self.weighted_img(img = houghed_lines, initial_img = img, alpha=0.8, beta=1., gamma=0.)
        
        return output
    def grayscale(self,img):
        return cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)

    def canny(self,img, low_threshold, high_threshold):
        return cv2.Canny(img, low_threshold, high_threshold)

    def gaussian_blur(self,img, kernel_size):
        return cv2.GaussianBlur(img, (kernel_size, kernel_size), 0)

    def region_of_interest(self,img, vertices):
        mask = np.zeros_like(img)   
    
        if len(img.shape) > 2:
            channel_count = img.shape[2]  
            ignore_mask_color = (255,) * channel_count
        else:
            ignore_mask_color = 255
            
        cv2.fillPoly(mask, vertices, ignore_mask_color)
        
        masked_image = cv2.bitwise_and(img, mask)
        return masked_image
    def draw_lines(self,img, lines, color=[255, 0, 0], thickness=10):
        for line in lines:
            for x1,y1,x2,y2 in line:
                cv2.line(img, (x1, y1), (x2, y2), color, thickness)

    def slope_lines(self,image,lines):
        img = image.copy()
        poly_vertices = []
        order = [0,1,3,2]

        left_lines = [] 
        right_lines = [] 
        for line in lines:
            for x1,y1,x2,y2 in line:

                if x1 == x2:
                    pass 
                else:
                    m = (y2 - y1) / (x2 - x1)
                    c = y1 - m * x1

                    if m < 0:
                        left_lines.append((m,c))
                    elif m >= 0:
                        right_lines.append((m,c))

        left_line = np.mean(left_lines, axis=0)
        right_line = np.mean(right_lines, axis=0)


        for slope, intercept in [left_line, right_line]:

            rows, cols = image.shape[:2]
            y1= int(rows) 

            y2= int(rows*0.6)

            x1=int((y1-intercept)/slope)
            x2=int((y2-intercept)/slope)
            poly_vertices.append((x1, y1))
            poly_vertices.append((x2, y2))
            self.draw_lines(img, np.array([[[x1,y1,x2,y2]]]))
        
        poly_vertices = [poly_vertices[i] for i in order]
        cv2.fillPoly(img, pts = np.array([poly_vertices],'int32'), color = (0,255,0))
        return cv2.addWeighted(image,0.7,img,0.4,0.)

    def hough_lines(self,img, rho, theta, threshold, min_line_len, max_line_gap):
        lines = cv2.HoughLinesP(img, rho, theta, threshold, np.array([]), minLineLength=min_line_len, maxLineGap=max_line_gap)
        line_img = np.zeros((img.shape[0], img.shape[1], 3), dtype=np.uint8)
        line_img = self.slope_lines(line_img,lines)
        return line_img

    def weighted_img(self,img, initial_img, alpha=0.1, beta=1., gamma=0.):

        lines_edges = cv2.addWeighted(initial_img, alpha, img, beta, gamma)
        return lines_edges
        
    def get_vertices(self,image):
        rows, cols = image.shape[:2]
        bottom_left  = [cols*0.15, rows]
        top_left     = [cols*0.45, rows*0.6]
        bottom_right = [cols*0.95, rows]
        top_right    = [cols*0.55, rows*0.6] 
        
        ver = np.array([[bottom_left, top_left, top_right, bottom_right]], dtype=np.int32)
        return ver

lines = cv2.HoughLinesP(cropped_image,2,np.pi/180,100,np.array([]),minLineLength=40,maxLineGap=5)

def disply_lines(image,lines):
    pass

line_image = disply_lines(lane_image,lines)

def disply_lines(image,lines):
    line_image = np.zeros_like(image)
    if lines is not None:
        for line in lines:
            print(line)

[[704 418 927 641]]
[[704 426 791 516]]
[[320 703 445 494]]
[[585 301 663 381]]
[[630 341 670 383]]

def disply_lines(image,lines):
    line_image = np.zeros_like(image)
    if liness is not None:
        for line in lines:
            x1,y1,x2,y2 = line.reshape(4)
            cv2.line(line_image,(x1,y1),(x2,y2),(255,0,0),10)
    return line_image

line_image = disply_lines(lane_image,lines)
cv2.imshow('result',line_image)

def average_slope_intercept(image,lines):
    left_fit = []
    right_fit = []
    for line in lines:
        x1, y1, x2, y2 = line.reshape(4)
        parameters = np.polyfit((x1,x2),(y1,y2),1)
        print(parameters)

[   1. -286.]
[   1.03448276 -302.27586207]
[  -1.672 1238.04 ]
[   1.02564103 -299.

[   1.02564103 -299.

def average_slope_intercept(image,lines):
    left_fit = []
    right_fit = []
    for line in lines:
        x1, y1, x2, y2 = line.reshape(4)
        parameters = np.polyfit((x1,x2),(y1,y2),1)
        # print(parameters)
        slope = parameters[0]
        intercept = parameters[1]
        if slope < 0:
            left_fit.append((slope,intercept))
        else:
            right_fit.append((slope,intercept))
        print(left_fit)
        print(right_fit)

def make_coordinates(image, line_parameters):
    slope, intercept = line_parameters
    y1 = image.shape[0]
    y2 = int(y1*(3/5)) 
    x1 = int((y1 - intercept)/slope)
    x2 = int((y2 - intercept)/slope)
    # print(image.shape)
    return np.array([x1,y1,x2,y2])

averaged_lines = average_slope_intercept(lane_image,lines);
line_image = disply_lines(lane_image,averaged_lines)
combo_image = cv2.addWeighted(lane_image,0.8, line_image, 1, 1,1)

cv2.imshow('result',combo_image)