text: import numpy as np
  import cv2
  import matplotlib.pyplot as plt
  import matplotlib.image as mpimg
  import pickle
  import argparse
  import glob
  from camera_calibration import undistort
  from perspective_transformation import unwarp
  from image_processing import color_thresolding
  
  
  
  
  
  def hist(img):
  	# Grab only the bottom half of the image
  	# Lane lines are likely to be mostly vertical nearest to the car
  	bottom_half = img[img.shape[0]//2:,:]
  
  	# Sum across image pixels vertically - make sure to set `axis`
  	# i.e. the highest areas of vertical lines should be larger values
  	histogram = np.sum(bottom_half, axis=0)    
  	return histogram
  
  
  def find_lane_pixels(binary_warped):
  	# Take a histogram of the bottom half of the image
  	histogram = np.sum(binary_warped[binary_warped.shape[0]//2:,:], axis=0)
  	# Create an output image to draw on and visualize the result
  	out_img = np.dstack((binary_warped, binary_warped, binary_warped))
  	# 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
  
  	# HYPERPARAMETERS
  	# Choose the number of sliding windows
  	nwindows = 9
  	# Set the width of the windows +/- margin
  	margin = 100
  	# Set minimum number of pixels found to recenter window
  	minpix = 50
  
  	# Set height of windows - based on nwindows above and image shape
  	window_height = np.int(binary_warped.shape[0]//nwindows)
  	# Identify the x and y positions of all nonzero pixels in the image
  	nonzero = binary_warped.nonzero()
  	nonzeroy = np.array(nonzero[0])
  	nonzerox = np.array(nonzero[1])
  	# Current positions to be updated later for each window in nwindows
  	leftx_current = leftx_base
  	rightx_current = rightx_base
  
  	# 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 = binary_warped.shape[0] - (window+1)*window_height
  		win_y_high = binary_warped.shape[0] - window*window_height
  		### Find the four below boundaries of the window ### 
  		win_xleft_low = leftx_current-margin  # Update this
  		win_xleft_high = leftx_current+margin  # Update this
  		win_xright_low = rightx_current-margin  # Update this
  		win_xright_high = rightx_current+margin  # Update this
  		
  		# Draw the windows on the visualization image
  		cv2.rectangle(out_img,(win_xleft_low,win_y_low),
  		(win_xleft_high,win_y_high),(0,255,0), 2) 
  		cv2.rectangle(out_img,(win_xright_low,win_y_low),
  		(win_xright_high,win_y_high),(0,255,0), 2) 
  		
  		### 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 ###
  		### (`right` or `leftx_current`) 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 (previously was a list of lists of pixels)
  	try:
  		left_lane_inds = np.concatenate(left_lane_inds)
  		right_lane_inds = np.concatenate(right_lane_inds)
  	except ValueError:
  		# Avoids an error if the above is not implemented fully
  		pass
  
  	# 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]
  
  	return leftx, lefty, rightx, righty, out_img
  
  def fit_poly(img_shape, leftx, lefty, rightx, righty):
  	### Fit a second order polynomial to each with np.polyfit() ###
  	left_fit = np.polyfit(lefty, leftx, 2)
  	right_fit = np.polyfit(righty, rightx, 2)
  	# Generate x and y values for plotting
  	ploty = np.linspace(0, img_shape[0]-1, img_shape[0])
  	###  Calc both polynomials using ploty, left_fit and right_fit ###
  	left_fitx = left_fit[0]*ploty**2+left_fit[1]*ploty+left_fit[2]
  	right_fitx = right_fit[0]*ploty**2+right_fit[1]*ploty+right_fit[2]
  	
  	return left_fit, right_fit, left_fitx, right_fitx, ploty
  
  def check_line_coherence(left_fit, right_fit, left_fitx, right_fitx, ploty)
  	
  
  def search_around_poly(binary_warped, left_line, right_line):
  
  	# HYPERPARAMETER
  	margin = 100
  
  	# Grab activated pixels
  	nonzero = binary_warped.nonzero()
  	nonzeroy = np.array(nonzero[0])
  	nonzerox = np.array(nonzero[1])
  
  
  	# Find pixels reltive to left and right line using histogram
  	leftx, lefty, rightx, righty, out_img = find_lane_pixels(binary_warped)
  	left_fit = []
  	right_fit = []
  	ploty = []
  	# Testing if there is left and right pixels related to line
  	if leftx.size == 0 or rightx.size == 0 : 
  		if left_line.allx == [] or right_line.allx == []: 
  			return leftx, lefty, rightx, righty, left_fit, right_fit, ploty, binary_warped
  		else: 
  			leftx = left_line.allx[-1]
  			lefty = left_line.ally[-1]
  			rightx = right_line.allx[-1]
  			righty = right_line.ally[-1]
  	else:
  		left_line.allx.append(leftx)
  		left_line.ally.append(lefty)
  		right_line.allx.append(rightx)
  		right_line.ally.append(righty)
  
  
  	left_fit = np.polyfit(lefty, leftx, 2)
  	right_fit = np.polyfit(righty,rightx, 2)
  	
  	### Set the area of search based on activated x-values ###
  	### within the +/- margin of our polynomial function ###
  	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 new polynomials
  	#left_fitx, right_fitx, ploty = fit_poly(binary_warped.shape, leftx, lefty, rightx, righty)
  	if leftx.size == 0 or rightx.size == 0 : 
  		if left_line.allx == [] or right_line.allx == []: 
  			return leftx, lefty, rightx, righty, left_fit, right_fit, ploty, binary_warped
  		else: 
  			leftx = left_line.allx[-1]
  			lefty = left_line.ally[-1]
  			rightx = right_line.allx[-1]
  			righty = right_line.ally[-1]
  	else:
  		left_line.allx.append(leftx)
  		left_line.ally.append(lefty)
  		right_line.allx.append(rightx)
  		right_line.ally.append(righty)
  	
  	left_fit, right_fit, left_fitx, right_fitx, ploty = fit_poly(binary_warped.shape, leftx, lefty, rightx, righty)
  
  	# Check coherence between left and right line
  
  	## Visualization ##
  	# Create an image to draw on and an image to show the selection window
  	out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
  	window_img = np.zeros_like(out_img)
  	# Color in left and right line pixels
  	out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
  	out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]
  
  	# Generate a polygon to illustrate the search window area
  	# And recast the x and y points into usable format for cv2.fillPoly()
  	left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, ploty]))])
  	left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin, 
  							  ploty])))])
  	left_line_pts = np.hstack((left_line_window1, left_line_window2))
  	right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, ploty]))])
  	right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin, 
  							  ploty])))])
  	right_line_pts = np.hstack((right_line_window1, right_line_window2))
  
  	# Draw the lane onto the warped blank image
  	cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0))
  	cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0))
  	result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)
  
  	return leftx, lefty, rightx, righty, left_fit, right_fit, ploty, result
  
  
  def fit_polynomial(binary_warped):
  	# Find our lane pixels first
  	leftx, lefty, rightx, righty, out_img = find_lane_pixels(binary_warped)
  
  	left_fit = np.polyfit(lefty, leftx, 2)
  	right_fit = np.polyfit(righty,rightx, 2)
  
  	# Generate x and y values for plotting
  	ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
  	try:
  		left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
  		right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
  	except TypeError:
  		# Avoids an error if `left` and `right_fit` are still none or incorrect
  		print('The function failed to fit a line!')
  		left_fitx = 1*ploty**2 + 1*ploty
  		right_fitx = 1*ploty**2 + 1*ploty
  
  	## Visualization ##
  	# Colors in the left and right lane regions
  	out_img[lefty, leftx] = [255, 0, 0]
  	out_img[righty, rightx] = [0, 0, 255]
  
  	# Plots the left and right polynomials on the lane lines
  	#plt.plot(left_fitx, ploty, color='yellow')
  	#plt.plot(right_fitx, ploty, color='yellow')
  
  	return left_fitx, right_fitx, out_img
  
  
  
  def measure_curvature_real(binary_warped, ploty, left_fit, right_fit):
  	'''
  	Calculates the curvature of polynomial functions in meters.
  	'''
  	# Define conversions in x and y from pixels space to meters
  	ym_per_pix = 30/720 # meters per pixel in y dimension
  	xm_per_pix = 3.7/700 # meters per pixel in x dimension
  
  
  	leftx = left_fit[0]*ploty**2 + left_fit[1]*ploty +left_fit[2]
  	rightx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
  	positionCar= binary_warped.shape[1]/2
  	# Fit new polynomials to x,y in world space
  	left_fit_cr = np.polyfit(ploty*ym_per_pix, leftx*xm_per_pix, 2)
  	right_fit_cr = np.polyfit(ploty*ym_per_pix, rightx*xm_per_pix, 2)
  
  
  
  	# Define y-value where we want radius of curvature
  	# We'll choose the maximum y-value, corresponding to the bottom of the image
  	y_eval = np.max(ploty)
  
  	##### Calculation of R_curve (radius of curvature) #####
  	left_curverad = ((1+(2*left_fit_cr[0]*y_eval*ym_per_pix+left_fit_cr[1])**2)**1.5)/(np.absolute(left_fit_cr[0]*2))  ## Implement the calculation of the left line here
  	right_curverad = ((1+(2*right_fit_cr[0]*y_eval*ym_per_pix+right_fit_cr[1])**2)**1.5)/(np.absolute(right_fit_cr[0]*2))  ## Implement the calculation of the right line here
  
  	left_lane_bottom = (left_fit[0]*y_eval)**2 + left_fit[0]*y_eval + left_fit[2]
  	right_lane_bottom = (right_fit[0]*y_eval)**2 + right_fit[0]*y_eval + right_fit[2]
  
  	actualPosition= (left_lane_bottom+right_lane_bottom)/2
  
  	distance= (positionCar - actualPosition)* xm_per_pix
  
  
  	return (left_curverad + right_curverad)/2, distance
  
  if __name__ == '__main__':
  	# Read in an image and grayscale it
  	image = mpimg.imread('output_images/image_processing_output/thresold_test3.jpg')
  
  	# Create histogram of image binary activations
  	histogram = hist(image)
  
  	# Visualize the resulting histogram
  	plt.plot(histogram)
  	plt.show()