"""
Utility function to implement lane finding pipeline.

Some methods contain code or were inspired by code from Udacity's lane finding lessons.
"""
import cv2
import numpy as np


OUTPUT_DIR = 'output_images'
TEST_IMAGES_DIR = 'udacity/test_images'


def binarize(img, s_thresh=(150, 255), sx_thresh=(20, 100)):
    """
    Apply color and gradient thresholding to obtain binarized image.
    """
    img = np.copy(img)
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HLS).astype(np.float)
    l_channel = hsv[:, :, 1]
    s_channel = hsv[:, :, 2]

    sobelx = cv2.Sobel(l_channel, cv2.CV_64F, 1, 0)
    abs_sobelx = np.absolute(sobelx)
    scaled_sobel = np.uint8(255 * abs_sobelx / np.max(abs_sobelx))

    # Threshold x gradient
    sxbinary = np.zeros_like(scaled_sobel)
    sxbinary[(scaled_sobel >= sx_thresh[0]) & (scaled_sobel <= sx_thresh[1])] = 1

    # Threshold color channel
    s_binary = np.zeros_like(s_channel)
    s_binary[(s_channel >= s_thresh[0]) & (s_channel <= s_thresh[1])] = 1

    img_bin = np.zeros_like(sxbinary)
    img_bin[(sxbinary == 1) | (s_binary == 1)] = 1

    return img_bin


def warp(img, reverse=False):
    """
    Warp perspective using src and dst points.
    """

    img_size = (img.shape[1], img.shape[0])

    points = np.array(
        [(581, 460),
         (278, img_size[1] - 50),
         (1020, img_size[1] - 50),
         (700, 460)])
    src = np.float32([points])

    dst = np.float32(
        [[278, 0],
         [278, img_size[1]],
         [1020, img_size[1]],
         [1020, 0]])

    if reverse:
        src, dst = dst, src

    M = cv2.getPerspectiveTransform(src, dst)

    return cv2.warpPerspective(img, M, img_size)


def find_lane_slow(img):
    """
    Find left and right lane line fit.
    """
    histogram = np.sum(img[img.shape[0] // 2:, :], axis=0)

    # Find the peak of the left and right halves of the histogram
    # These will be the starting point for the left and right lines
    midpoint = np.int(histogram.shape[0] / 2)
    leftx_base = np.argmax(histogram[:midpoint])
    rightx_base = np.argmax(histogram[midpoint:]) + midpoint

    # Choose the number of sliding windows
    nwindows = 9
    # Set height of windows
    window_height = np.int(img.shape[0] / nwindows)
    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    # Current positions to be updated for each window
    leftx_current = leftx_base
    rightx_current = rightx_base
    # Set the width of the windows +/- margin
    margin = 150
    # Set minimum number of pixels found to recenter window
    minpix = 50
    # Create empty lists to receive left and right lane pixel indices
    left_lane_inds = []
    right_lane_inds = []

    # Step through the windows one by one
    for window in range(nwindows):
        # Identify window boundaries in x and y (and right and left)
        win_y_low = img.shape[0] - (window + 1) * window_height
        win_y_high = img.shape[0] - window * window_height
        win_xleft_low = leftx_current - margin
        win_xleft_high = leftx_current + margin
        win_xright_low = rightx_current - margin
        win_xright_high = rightx_current + margin

        # Identify the nonzero pixels in x and y within the window
        good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xleft_low) & (
        nonzerox < win_xleft_high)).nonzero()[0]
        good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) & (
        nonzerox < win_xright_high)).nonzero()[0]
        # Append these indices to the lists
        left_lane_inds.append(good_left_inds)
        right_lane_inds.append(good_right_inds)
        # If you found > minpix pixels, recenter next window on their mean position
        if len(good_left_inds) > minpix:
            leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
        if len(good_right_inds) > minpix:
            rightx_current = np.int(np.mean(nonzerox[good_right_inds]))

    # Concatenate the arrays of indices
    left_lane_inds = np.concatenate(left_lane_inds)
    right_lane_inds = np.concatenate(right_lane_inds)

    # Extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]

    # Fit a second order polynomial to each
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)

    return left_fit, right_fit


def find_lane_fast(img, left_fit, right_fit):
    """
    Find left and right lane line fit (using the previous fit as a starting position).
    """

    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])

    margin = 150
    left_lane_inds = ((nonzerox > (left_fit[0] * (nonzeroy ** 2) + left_fit[1] * nonzeroy + left_fit[2] - margin)) & (
        nonzerox < (left_fit[0] * (nonzeroy ** 2) + left_fit[1] * nonzeroy + left_fit[2] + margin)))
    right_lane_inds = (
        (nonzerox > (right_fit[0] * (nonzeroy ** 2) + right_fit[1] * nonzeroy + right_fit[2] - margin)) & (
        nonzerox < (right_fit[0] * (nonzeroy ** 2) + right_fit[1] * nonzeroy + right_fit[2] + margin)))

    # Again, extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]

    # Fit a second order polynomial to each
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)

    return left_fit, right_fit


def draw_lane(warped, undist, ploty, left_fitx, right_fitx):
    """
    Draw a lane on top of the input image.
    """
    warp_zero = np.zeros_like(warped).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))

    # Recast the x and y points into usable format for cv2.fillPoly()
    pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
    pts = np.hstack((pts_left, pts_right))

    # Draw the lane onto the warped blank image
    cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0))

    # Warp the blank back to original image space using inverse perspective matrix (Minv)
    newwarp = warp(color_warp, reverse=True)
    # Combine the result with the original image
    result = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)

    return result


def compute_curvature(ploty, left_fitx, right_fitx):
    """
    Compute curvature of the lane.
    """
    ym_per_pix = 30 / 720  # meters per pixel in y dimension
    xm_per_pix = 3.7 / 700  # meters per pixel in x dimension

    y_eval = np.max(ploty)

    # Fit new polynomials to x,y in world space
    left_fit_cr = np.polyfit(ploty * ym_per_pix, left_fitx * xm_per_pix, 2)
    right_fit_cr = np.polyfit(ploty * ym_per_pix, right_fitx * xm_per_pix, 2)
    # Calculate the new radii of curvature
    left_curverad = ((1 + (2 * left_fit_cr[0] * y_eval * ym_per_pix + left_fit_cr[1]) ** 2) ** 1.5) / np.absolute(
        2 * left_fit_cr[0])
    right_curverad = ((1 + (2 * right_fit_cr[0] * y_eval * ym_per_pix + right_fit_cr[1]) ** 2) ** 1.5) / np.absolute(
        2 * right_fit_cr[0])

    curvature = (left_curverad + right_curverad) / 2

    return curvature


def measure_offset(image_middle_x, leftx, rightx):
    """
    Measure the offset of a car from the center of a lane.
    """
    xm_per_pix = 3.7 / 700  # meters per pixel in x dimension
    car_middle_x = (leftx[0] + rightx[0]) / 2
    return xm_per_pix * (image_middle_x - car_middle_x)


def draw_text(img, text, pos):
    """
    Draw text to an image at a given position.
    """
    font = cv2.FONT_HERSHEY_SIMPLEX
    return cv2.putText(img, text, pos, font, 1, (255, 255, 255), 1, cv2.LINE_AA)


def is_outlier(hist, value):
    """
    Determine if a value is an outlier based on the history.

    We declare a value on outlier when it's three std deviations away from the mean value.
    """
    stdev = np.std(hist, axis=0)
    avg = np.average(hist[-15:], axis=0)
    if any(lf for lf, avg, std in zip(value, avg, stdev) if lf > avg + 3 * std) or \
            any(lf for lf, avg, std in zip(value, avg, stdev) if lf < avg - 3 * std):
        return True
    return False

a2da067dc2ebd60142300f018e206e1c745bdd04
<mask token>


def warp(img, reverse=False):
    """
    Warp perspective using src and dst points.
    """
    img_size = img.shape[1], img.shape[0]
    points = np.array([(581, 460), (278, img_size[1] - 50), (1020, img_size
        [1] - 50), (700, 460)])
    src = np.float32([points])
    dst = np.float32([[278, 0], [278, img_size[1]], [1020, img_size[1]], [
        1020, 0]])
    if reverse:
        src, dst = dst, src
    M = cv2.getPerspectiveTransform(src, dst)
    return cv2.warpPerspective(img, M, img_size)


<mask token>


def find_lane_fast(img, left_fit, right_fit):
    """
    Find left and right lane line fit (using the previous fit as a starting position).
    """
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    margin = 150
    left_lane_inds = (nonzerox > left_fit[0] * nonzeroy ** 2 + left_fit[1] *
        nonzeroy + left_fit[2] - margin) & (nonzerox < left_fit[0] * 
        nonzeroy ** 2 + left_fit[1] * nonzeroy + left_fit[2] + margin)
    right_lane_inds = (nonzerox > right_fit[0] * nonzeroy ** 2 + right_fit[
        1] * nonzeroy + right_fit[2] - margin) & (nonzerox < right_fit[0] *
        nonzeroy ** 2 + right_fit[1] * nonzeroy + right_fit[2] + margin)
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    return left_fit, right_fit


def draw_lane(warped, undist, ploty, left_fitx, right_fitx):
    """
    Draw a lane on top of the input image.
    """
    warp_zero = np.zeros_like(warped).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
    pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx,
        ploty])))])
    pts = np.hstack((pts_left, pts_right))
    cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0))
    newwarp = warp(color_warp, reverse=True)
    result = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)
    return result


def compute_curvature(ploty, left_fitx, right_fitx):
    """
    Compute curvature of the lane.
    """
    ym_per_pix = 30 / 720
    xm_per_pix = 3.7 / 700
    y_eval = np.max(ploty)
    left_fit_cr = np.polyfit(ploty * ym_per_pix, left_fitx * xm_per_pix, 2)
    right_fit_cr = np.polyfit(ploty * ym_per_pix, right_fitx * xm_per_pix, 2)
    left_curverad = (1 + (2 * left_fit_cr[0] * y_eval * ym_per_pix +
        left_fit_cr[1]) ** 2) ** 1.5 / np.absolute(2 * left_fit_cr[0])
    right_curverad = (1 + (2 * right_fit_cr[0] * y_eval * ym_per_pix +
        right_fit_cr[1]) ** 2) ** 1.5 / np.absolute(2 * right_fit_cr[0])
    curvature = (left_curverad + right_curverad) / 2
    return curvature


<mask token>


def draw_text(img, text, pos):
    """
    Draw text to an image at a given position.
    """
    font = cv2.FONT_HERSHEY_SIMPLEX
    return cv2.putText(img, text, pos, font, 1, (255, 255, 255), 1, cv2.LINE_AA
        )


def is_outlier(hist, value):
    """
    Determine if a value is an outlier based on the history.

    We declare a value on outlier when it's three std deviations away from the mean value.
    """
    stdev = np.std(hist, axis=0)
    avg = np.average(hist[-15:], axis=0)
    if any(lf for lf, avg, std in zip(value, avg, stdev) if lf > avg + 3 * std
        ) or any(lf for lf, avg, std in zip(value, avg, stdev) if lf < avg -
        3 * std):
        return True
    return False

<mask token>


def warp(img, reverse=False):
    """
    Warp perspective using src and dst points.
    """
    img_size = img.shape[1], img.shape[0]
    points = np.array([(581, 460), (278, img_size[1] - 50), (1020, img_size
        [1] - 50), (700, 460)])
    src = np.float32([points])
    dst = np.float32([[278, 0], [278, img_size[1]], [1020, img_size[1]], [
        1020, 0]])
    if reverse:
        src, dst = dst, src
    M = cv2.getPerspectiveTransform(src, dst)
    return cv2.warpPerspective(img, M, img_size)


<mask token>


def find_lane_fast(img, left_fit, right_fit):
    """
    Find left and right lane line fit (using the previous fit as a starting position).
    """
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    margin = 150
    left_lane_inds = (nonzerox > left_fit[0] * nonzeroy ** 2 + left_fit[1] *
        nonzeroy + left_fit[2] - margin) & (nonzerox < left_fit[0] * 
        nonzeroy ** 2 + left_fit[1] * nonzeroy + left_fit[2] + margin)
    right_lane_inds = (nonzerox > right_fit[0] * nonzeroy ** 2 + right_fit[
        1] * nonzeroy + right_fit[2] - margin) & (nonzerox < right_fit[0] *
        nonzeroy ** 2 + right_fit[1] * nonzeroy + right_fit[2] + margin)
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    return left_fit, right_fit


def draw_lane(warped, undist, ploty, left_fitx, right_fitx):
    """
    Draw a lane on top of the input image.
    """
    warp_zero = np.zeros_like(warped).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
    pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx,
        ploty])))])
    pts = np.hstack((pts_left, pts_right))
    cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0))
    newwarp = warp(color_warp, reverse=True)
    result = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)
    return result


def compute_curvature(ploty, left_fitx, right_fitx):
    """
    Compute curvature of the lane.
    """
    ym_per_pix = 30 / 720
    xm_per_pix = 3.7 / 700
    y_eval = np.max(ploty)
    left_fit_cr = np.polyfit(ploty * ym_per_pix, left_fitx * xm_per_pix, 2)
    right_fit_cr = np.polyfit(ploty * ym_per_pix, right_fitx * xm_per_pix, 2)
    left_curverad = (1 + (2 * left_fit_cr[0] * y_eval * ym_per_pix +
        left_fit_cr[1]) ** 2) ** 1.5 / np.absolute(2 * left_fit_cr[0])
    right_curverad = (1 + (2 * right_fit_cr[0] * y_eval * ym_per_pix +
        right_fit_cr[1]) ** 2) ** 1.5 / np.absolute(2 * right_fit_cr[0])
    curvature = (left_curverad + right_curverad) / 2
    return curvature


def measure_offset(image_middle_x, leftx, rightx):
    """
    Measure the offset of a car from the center of a lane.
    """
    xm_per_pix = 3.7 / 700
    car_middle_x = (leftx[0] + rightx[0]) / 2
    return xm_per_pix * (image_middle_x - car_middle_x)


def draw_text(img, text, pos):
    """
    Draw text to an image at a given position.
    """
    font = cv2.FONT_HERSHEY_SIMPLEX
    return cv2.putText(img, text, pos, font, 1, (255, 255, 255), 1, cv2.LINE_AA
        )


def is_outlier(hist, value):
    """
    Determine if a value is an outlier based on the history.

    We declare a value on outlier when it's three std deviations away from the mean value.
    """
    stdev = np.std(hist, axis=0)
    avg = np.average(hist[-15:], axis=0)
    if any(lf for lf, avg, std in zip(value, avg, stdev) if lf > avg + 3 * std
        ) or any(lf for lf, avg, std in zip(value, avg, stdev) if lf < avg -
        3 * std):
        return True
    return False

<mask token>


def warp(img, reverse=False):
    """
    Warp perspective using src and dst points.
    """
    img_size = img.shape[1], img.shape[0]
    points = np.array([(581, 460), (278, img_size[1] - 50), (1020, img_size
        [1] - 50), (700, 460)])
    src = np.float32([points])
    dst = np.float32([[278, 0], [278, img_size[1]], [1020, img_size[1]], [
        1020, 0]])
    if reverse:
        src, dst = dst, src
    M = cv2.getPerspectiveTransform(src, dst)
    return cv2.warpPerspective(img, M, img_size)


def find_lane_slow(img):
    """
    Find left and right lane line fit.
    """
    histogram = np.sum(img[img.shape[0] // 2:, :], axis=0)
    midpoint = np.int(histogram.shape[0] / 2)
    leftx_base = np.argmax(histogram[:midpoint])
    rightx_base = np.argmax(histogram[midpoint:]) + midpoint
    nwindows = 9
    window_height = np.int(img.shape[0] / nwindows)
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    leftx_current = leftx_base
    rightx_current = rightx_base
    margin = 150
    minpix = 50
    left_lane_inds = []
    right_lane_inds = []
    for window in range(nwindows):
        win_y_low = img.shape[0] - (window + 1) * window_height
        win_y_high = img.shape[0] - window * window_height
        win_xleft_low = leftx_current - margin
        win_xleft_high = leftx_current + margin
        win_xright_low = rightx_current - margin
        win_xright_high = rightx_current + margin
        good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
            (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero(
            )[0]
        good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high
            ) & (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)
            ).nonzero()[0]
        left_lane_inds.append(good_left_inds)
        right_lane_inds.append(good_right_inds)
        if len(good_left_inds) > minpix:
            leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
        if len(good_right_inds) > minpix:
            rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
    left_lane_inds = np.concatenate(left_lane_inds)
    right_lane_inds = np.concatenate(right_lane_inds)
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    return left_fit, right_fit


def find_lane_fast(img, left_fit, right_fit):
    """
    Find left and right lane line fit (using the previous fit as a starting position).
    """
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    margin = 150
    left_lane_inds = (nonzerox > left_fit[0] * nonzeroy ** 2 + left_fit[1] *
        nonzeroy + left_fit[2] - margin) & (nonzerox < left_fit[0] * 
        nonzeroy ** 2 + left_fit[1] * nonzeroy + left_fit[2] + margin)
    right_lane_inds = (nonzerox > right_fit[0] * nonzeroy ** 2 + right_fit[
        1] * nonzeroy + right_fit[2] - margin) & (nonzerox < right_fit[0] *
        nonzeroy ** 2 + right_fit[1] * nonzeroy + right_fit[2] + margin)
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    return left_fit, right_fit


def draw_lane(warped, undist, ploty, left_fitx, right_fitx):
    """
    Draw a lane on top of the input image.
    """
    warp_zero = np.zeros_like(warped).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
    pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx,
        ploty])))])
    pts = np.hstack((pts_left, pts_right))
    cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0))
    newwarp = warp(color_warp, reverse=True)
    result = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)
    return result


def compute_curvature(ploty, left_fitx, right_fitx):
    """
    Compute curvature of the lane.
    """
    ym_per_pix = 30 / 720
    xm_per_pix = 3.7 / 700
    y_eval = np.max(ploty)
    left_fit_cr = np.polyfit(ploty * ym_per_pix, left_fitx * xm_per_pix, 2)
    right_fit_cr = np.polyfit(ploty * ym_per_pix, right_fitx * xm_per_pix, 2)
    left_curverad = (1 + (2 * left_fit_cr[0] * y_eval * ym_per_pix +
        left_fit_cr[1]) ** 2) ** 1.5 / np.absolute(2 * left_fit_cr[0])
    right_curverad = (1 + (2 * right_fit_cr[0] * y_eval * ym_per_pix +
        right_fit_cr[1]) ** 2) ** 1.5 / np.absolute(2 * right_fit_cr[0])
    curvature = (left_curverad + right_curverad) / 2
    return curvature


def measure_offset(image_middle_x, leftx, rightx):
    """
    Measure the offset of a car from the center of a lane.
    """
    xm_per_pix = 3.7 / 700
    car_middle_x = (leftx[0] + rightx[0]) / 2
    return xm_per_pix * (image_middle_x - car_middle_x)


def draw_text(img, text, pos):
    """
    Draw text to an image at a given position.
    """
    font = cv2.FONT_HERSHEY_SIMPLEX
    return cv2.putText(img, text, pos, font, 1, (255, 255, 255), 1, cv2.LINE_AA
        )


def is_outlier(hist, value):
    """
    Determine if a value is an outlier based on the history.

    We declare a value on outlier when it's three std deviations away from the mean value.
    """
    stdev = np.std(hist, axis=0)
    avg = np.average(hist[-15:], axis=0)
    if any(lf for lf, avg, std in zip(value, avg, stdev) if lf > avg + 3 * std
        ) or any(lf for lf, avg, std in zip(value, avg, stdev) if lf < avg -
        3 * std):
        return True
    return False

<mask token>
OUTPUT_DIR = 'output_images'
TEST_IMAGES_DIR = 'udacity/test_images'


def binarize(img, s_thresh=(150, 255), sx_thresh=(20, 100)):
    """
    Apply color and gradient thresholding to obtain binarized image.
    """
    img = np.copy(img)
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HLS).astype(np.float)
    l_channel = hsv[:, :, 1]
    s_channel = hsv[:, :, 2]
    sobelx = cv2.Sobel(l_channel, cv2.CV_64F, 1, 0)
    abs_sobelx = np.absolute(sobelx)
    scaled_sobel = np.uint8(255 * abs_sobelx / np.max(abs_sobelx))
    sxbinary = np.zeros_like(scaled_sobel)
    sxbinary[(scaled_sobel >= sx_thresh[0]) & (scaled_sobel <= sx_thresh[1])
        ] = 1
    s_binary = np.zeros_like(s_channel)
    s_binary[(s_channel >= s_thresh[0]) & (s_channel <= s_thresh[1])] = 1
    img_bin = np.zeros_like(sxbinary)
    img_bin[(sxbinary == 1) | (s_binary == 1)] = 1
    return img_bin


def warp(img, reverse=False):
    """
    Warp perspective using src and dst points.
    """
    img_size = img.shape[1], img.shape[0]
    points = np.array([(581, 460), (278, img_size[1] - 50), (1020, img_size
        [1] - 50), (700, 460)])
    src = np.float32([points])
    dst = np.float32([[278, 0], [278, img_size[1]], [1020, img_size[1]], [
        1020, 0]])
    if reverse:
        src, dst = dst, src
    M = cv2.getPerspectiveTransform(src, dst)
    return cv2.warpPerspective(img, M, img_size)


def find_lane_slow(img):
    """
    Find left and right lane line fit.
    """
    histogram = np.sum(img[img.shape[0] // 2:, :], axis=0)
    midpoint = np.int(histogram.shape[0] / 2)
    leftx_base = np.argmax(histogram[:midpoint])
    rightx_base = np.argmax(histogram[midpoint:]) + midpoint
    nwindows = 9
    window_height = np.int(img.shape[0] / nwindows)
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    leftx_current = leftx_base
    rightx_current = rightx_base
    margin = 150
    minpix = 50
    left_lane_inds = []
    right_lane_inds = []
    for window in range(nwindows):
        win_y_low = img.shape[0] - (window + 1) * window_height
        win_y_high = img.shape[0] - window * window_height
        win_xleft_low = leftx_current - margin
        win_xleft_high = leftx_current + margin
        win_xright_low = rightx_current - margin
        win_xright_high = rightx_current + margin
        good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
            (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero(
            )[0]
        good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high
            ) & (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)
            ).nonzero()[0]
        left_lane_inds.append(good_left_inds)
        right_lane_inds.append(good_right_inds)
        if len(good_left_inds) > minpix:
            leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
        if len(good_right_inds) > minpix:
            rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
    left_lane_inds = np.concatenate(left_lane_inds)
    right_lane_inds = np.concatenate(right_lane_inds)
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    return left_fit, right_fit


def find_lane_fast(img, left_fit, right_fit):
    """
    Find left and right lane line fit (using the previous fit as a starting position).
    """
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    margin = 150
    left_lane_inds = (nonzerox > left_fit[0] * nonzeroy ** 2 + left_fit[1] *
        nonzeroy + left_fit[2] - margin) & (nonzerox < left_fit[0] * 
        nonzeroy ** 2 + left_fit[1] * nonzeroy + left_fit[2] + margin)
    right_lane_inds = (nonzerox > right_fit[0] * nonzeroy ** 2 + right_fit[
        1] * nonzeroy + right_fit[2] - margin) & (nonzerox < right_fit[0] *
        nonzeroy ** 2 + right_fit[1] * nonzeroy + right_fit[2] + margin)
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    return left_fit, right_fit


def draw_lane(warped, undist, ploty, left_fitx, right_fitx):
    """
    Draw a lane on top of the input image.
    """
    warp_zero = np.zeros_like(warped).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
    pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx,
        ploty])))])
    pts = np.hstack((pts_left, pts_right))
    cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0))
    newwarp = warp(color_warp, reverse=True)
    result = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)
    return result


def compute_curvature(ploty, left_fitx, right_fitx):
    """
    Compute curvature of the lane.
    """
    ym_per_pix = 30 / 720
    xm_per_pix = 3.7 / 700
    y_eval = np.max(ploty)
    left_fit_cr = np.polyfit(ploty * ym_per_pix, left_fitx * xm_per_pix, 2)
    right_fit_cr = np.polyfit(ploty * ym_per_pix, right_fitx * xm_per_pix, 2)
    left_curverad = (1 + (2 * left_fit_cr[0] * y_eval * ym_per_pix +
        left_fit_cr[1]) ** 2) ** 1.5 / np.absolute(2 * left_fit_cr[0])
    right_curverad = (1 + (2 * right_fit_cr[0] * y_eval * ym_per_pix +
        right_fit_cr[1]) ** 2) ** 1.5 / np.absolute(2 * right_fit_cr[0])
    curvature = (left_curverad + right_curverad) / 2
    return curvature


def measure_offset(image_middle_x, leftx, rightx):
    """
    Measure the offset of a car from the center of a lane.
    """
    xm_per_pix = 3.7 / 700
    car_middle_x = (leftx[0] + rightx[0]) / 2
    return xm_per_pix * (image_middle_x - car_middle_x)


def draw_text(img, text, pos):
    """
    Draw text to an image at a given position.
    """
    font = cv2.FONT_HERSHEY_SIMPLEX
    return cv2.putText(img, text, pos, font, 1, (255, 255, 255), 1, cv2.LINE_AA
        )


def is_outlier(hist, value):
    """
    Determine if a value is an outlier based on the history.

    We declare a value on outlier when it's three std deviations away from the mean value.
    """
    stdev = np.std(hist, axis=0)
    avg = np.average(hist[-15:], axis=0)
    if any(lf for lf, avg, std in zip(value, avg, stdev) if lf > avg + 3 * std
        ) or any(lf for lf, avg, std in zip(value, avg, stdev) if lf < avg -
        3 * std):
        return True
    return False

"""
Utility function to implement lane finding pipeline.

Some methods contain code or were inspired by code from Udacity's lane finding lessons.
"""
import cv2
import numpy as np


OUTPUT_DIR = 'output_images'
TEST_IMAGES_DIR = 'udacity/test_images'


def binarize(img, s_thresh=(150, 255), sx_thresh=(20, 100)):
    """
    Apply color and gradient thresholding to obtain binarized image.
    """
    img = np.copy(img)
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HLS).astype(np.float)
    l_channel = hsv[:, :, 1]
    s_channel = hsv[:, :, 2]

    sobelx = cv2.Sobel(l_channel, cv2.CV_64F, 1, 0)
    abs_sobelx = np.absolute(sobelx)
    scaled_sobel = np.uint8(255 * abs_sobelx / np.max(abs_sobelx))

    # Threshold x gradient
    sxbinary = np.zeros_like(scaled_sobel)
    sxbinary[(scaled_sobel >= sx_thresh[0]) & (scaled_sobel <= sx_thresh[1])] = 1

    # Threshold color channel
    s_binary = np.zeros_like(s_channel)
    s_binary[(s_channel >= s_thresh[0]) & (s_channel <= s_thresh[1])] = 1

    img_bin = np.zeros_like(sxbinary)
    img_bin[(sxbinary == 1) | (s_binary == 1)] = 1

    return img_bin


def warp(img, reverse=False):
    """
    Warp perspective using src and dst points.
    """

    img_size = (img.shape[1], img.shape[0])

    points = np.array(
        [(581, 460),
         (278, img_size[1] - 50),
         (1020, img_size[1] - 50),
         (700, 460)])
    src = np.float32([points])

    dst = np.float32(
        [[278, 0],
         [278, img_size[1]],
         [1020, img_size[1]],
         [1020, 0]])

    if reverse:
        src, dst = dst, src

    M = cv2.getPerspectiveTransform(src, dst)

    return cv2.warpPerspective(img, M, img_size)


def find_lane_slow(img):
    """
    Find left and right lane line fit.
    """
    histogram = np.sum(img[img.shape[0] // 2:, :], axis=0)

    # Find the peak of the left and right halves of the histogram
    # These will be the starting point for the left and right lines
    midpoint = np.int(histogram.shape[0] / 2)
    leftx_base = np.argmax(histogram[:midpoint])
    rightx_base = np.argmax(histogram[midpoint:]) + midpoint

    # Choose the number of sliding windows
    nwindows = 9
    # Set height of windows
    window_height = np.int(img.shape[0] / nwindows)
    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    # Current positions to be updated for each window
    leftx_current = leftx_base
    rightx_current = rightx_base
    # Set the width of the windows +/- margin
    margin = 150
    # Set minimum number of pixels found to recenter window
    minpix = 50
    # Create empty lists to receive left and right lane pixel indices
    left_lane_inds = []
    right_lane_inds = []

    # Step through the windows one by one
    for window in range(nwindows):
        # Identify window boundaries in x and y (and right and left)
        win_y_low = img.shape[0] - (window + 1) * window_height
        win_y_high = img.shape[0] - window * window_height
        win_xleft_low = leftx_current - margin
        win_xleft_high = leftx_current + margin
        win_xright_low = rightx_current - margin
        win_xright_high = rightx_current + margin

        # Identify the nonzero pixels in x and y within the window
        good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xleft_low) & (
        nonzerox < win_xleft_high)).nonzero()[0]
        good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) & (
        nonzerox < win_xright_high)).nonzero()[0]
        # Append these indices to the lists
        left_lane_inds.append(good_left_inds)
        right_lane_inds.append(good_right_inds)
        # If you found > minpix pixels, recenter next window on their mean position
        if len(good_left_inds) > minpix:
            leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
        if len(good_right_inds) > minpix:
            rightx_current = np.int(np.mean(nonzerox[good_right_inds]))

    # Concatenate the arrays of indices
    left_lane_inds = np.concatenate(left_lane_inds)
    right_lane_inds = np.concatenate(right_lane_inds)

    # Extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]

    # Fit a second order polynomial to each
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)

    return left_fit, right_fit


def find_lane_fast(img, left_fit, right_fit):
    """
    Find left and right lane line fit (using the previous fit as a starting position).
    """

    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])

    margin = 150
    left_lane_inds = ((nonzerox > (left_fit[0] * (nonzeroy ** 2) + left_fit[1] * nonzeroy + left_fit[2] - margin)) & (
        nonzerox < (left_fit[0] * (nonzeroy ** 2) + left_fit[1] * nonzeroy + left_fit[2] + margin)))
    right_lane_inds = (
        (nonzerox > (right_fit[0] * (nonzeroy ** 2) + right_fit[1] * nonzeroy + right_fit[2] - margin)) & (
        nonzerox < (right_fit[0] * (nonzeroy ** 2) + right_fit[1] * nonzeroy + right_fit[2] + margin)))

    # Again, extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]

    # Fit a second order polynomial to each
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)

    return left_fit, right_fit


def draw_lane(warped, undist, ploty, left_fitx, right_fitx):
    """
    Draw a lane on top of the input image.
    """
    warp_zero = np.zeros_like(warped).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))

    # Recast the x and y points into usable format for cv2.fillPoly()
    pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
    pts = np.hstack((pts_left, pts_right))

    # Draw the lane onto the warped blank image
    cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0))

    # Warp the blank back to original image space using inverse perspective matrix (Minv)
    newwarp = warp(color_warp, reverse=True)
    # Combine the result with the original image
    result = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)

    return result


def compute_curvature(ploty, left_fitx, right_fitx):
    """
    Compute curvature of the lane.
    """
    ym_per_pix = 30 / 720  # meters per pixel in y dimension
    xm_per_pix = 3.7 / 700  # meters per pixel in x dimension

    y_eval = np.max(ploty)

    # Fit new polynomials to x,y in world space
    left_fit_cr = np.polyfit(ploty * ym_per_pix, left_fitx * xm_per_pix, 2)
    right_fit_cr = np.polyfit(ploty * ym_per_pix, right_fitx * xm_per_pix, 2)
    # Calculate the new radii of curvature
    left_curverad = ((1 + (2 * left_fit_cr[0] * y_eval * ym_per_pix + left_fit_cr[1]) ** 2) ** 1.5) / np.absolute(
        2 * left_fit_cr[0])
    right_curverad = ((1 + (2 * right_fit_cr[0] * y_eval * ym_per_pix + right_fit_cr[1]) ** 2) ** 1.5) / np.absolute(
        2 * right_fit_cr[0])

    curvature = (left_curverad + right_curverad) / 2

    return curvature


def measure_offset(image_middle_x, leftx, rightx):
    """
    Measure the offset of a car from the center of a lane.
    """
    xm_per_pix = 3.7 / 700  # meters per pixel in x dimension
    car_middle_x = (leftx[0] + rightx[0]) / 2
    return xm_per_pix * (image_middle_x - car_middle_x)


def draw_text(img, text, pos):
    """
    Draw text to an image at a given position.
    """
    font = cv2.FONT_HERSHEY_SIMPLEX
    return cv2.putText(img, text, pos, font, 1, (255, 255, 255), 1, cv2.LINE_AA)


def is_outlier(hist, value):
    """
    Determine if a value is an outlier based on the history.

    We declare a value on outlier when it's three std deviations away from the mean value.
    """
    stdev = np.std(hist, axis=0)
    avg = np.average(hist[-15:], axis=0)
    if any(lf for lf, avg, std in zip(value, avg, stdev) if lf > avg + 3 * std) or \
            any(lf for lf, avg, std in zip(value, avg, stdev) if lf < avg - 3 * std):
        return True
    return False
