# TODO: Build your pipeline that will draw lane lines on the test_images
# then save them to the test_images_output directory.

class Line:
    def __init__(self, x1, y1, x2, y2):
        self.x1 = np.float32(x1)
        self.x2 = np.float32(x2)
        self.y1 = np.float32(y1)
        self.y2 = np.float32(y2)

        self.get_slope()

    def get_slope(self):
        self.slope = (self.y2 - self.y1) / (self.x2 - self.x1 + np.finfo(float).eps)

# Get all images
images = []
for image_name in os.listdir("test_images/"):
    image = mpimg.imread('test_images/solidWhiteRight.jpg')
    images.append(image)
np_images = np.array(images)

# Loop over all images
for image in images:
    # Convert to grayscale
    image_gray = grayscale(image)

    # Gaussian smooth the image
    image_smoothed = gaussian_blur(image_gray, 7)

    # Run canny edge detector on image
    image_canny = canny(image_smoothed, 50, 150)

    # Run an "opening" morphological filter
    #image_canny = cv2.morphologyEx(image_canny, cv2.MORPH_OPEN, (50, 50))
    #image_canny = cv2.dilate(image_canny, (50,50), iterations = 5)

    # Apply hough transform to get image with lines
    #def hough_lines(img, rho, theta, threshold, min_line_len, max_line_gap):
    #image = hough_lines(image_canny, 400, 45, 5, 5, 100)
    detected_lines, line_image = hough_lines(img=image_canny,
                                            rho=1,
                                            theta=np.pi / 180,
                                            threshold=1,
                                            min_line_len=15,
                                            max_line_gap=5)

    # Run RANSAC on lines?
    # python scikit: linear_model.RANSACRegressor(linear_model.LinearRegression()

    # Filter lines and interpolate lines to find both lanes
    preferred_lines = np.array([])
    for detected_line in detected_lines:
        # Only lines with slope between 30 and 120 degrees
        line = Line(detected_line[0][0], detected_line[0][1], detected_line[0][2], detected_line[0][3])
        if (np.pi / 6) <= np.abs(line.slope) <= (2 * np.pi / 3):
            np.append(preferred_lines, [detected_line[0][0],
                                        detected_line[0][1],
                                        detected_line[0][2],
                                        detected_line[0][3]])
    # interpolate lines candidates to find both lanes
    #lane_lines = compute_lane_from_candidates(candidate_lines, img_gray.shape)

    # Draw lines on a clear mask
    image_with_lines = np.zeros((image.shape[0], image.shape[1], 3), dtype=np.uint8)
    draw_lines(image_with_lines, preferred_lines)

    # Select the region of interest in image
    region = [[[0, int(image.shape[0] / 2)],
               [image.shape[1], int(image.shape[0] / 2)],
               [image.shape[1], image.shape[1]],
               [0, image.shape[1]]]]
    image_roi = region_of_interest(image_with_lines, np.array(region))

    # Created weighted masked_image
    weighted_image = weighted_img(image_roi, image, α=0.8, β=1., γ=0.)

    # Show and save image to the new folder
    plt.figure()
    plt.imshow(image_roi, cmap='gray')