video_pipeline.py

import numpy as np
import cv2
import matplotlib.pyplot as plt
import pickle
import glob
import re
import os.path
import time
from matplotlib.lines import Line2D 
from collections import deque
from matplotlib.backends.backend_agg import FigureCanvasAgg as FigureCanvas
from moviepy.editor import VideoFileClip
from statemachine import StateMachine
from matplotlib.patches import Polygon

image = cv2.imread('../examples/snap_golf.png')
settings = pickle.load(open("../pickle_files/project_data/fianal_params_for_lane_lines.pickle", "rb"))

'''
code for the state machine, Limited to the presented course material so far!  
'''

def img_proc(img):
    return img

input_string = '../project_videos/project_video_*.mp4'
foldername   = re.search(r'.+\/', input_string).group()
outdir       = foldername+'processed/'
timestamp    = time.strftime("%Y%m%d-%H%M%S")
outdir      += timestamp

if not os.path.isdir(outdir):
    print("creating folder: ", outdir)
    os.makedirs(outdir)


# create a class for the lane finder
class LaneFinder(StateMachine):
    '''
    @brief: Initializer for lane finder relies on input to set itself up
    @input: A dictionary containing essential parameters, generated by the other tools
             
    '''
    def __init__(self, params):
        super().__init__()

        self.mtx                 = params['mtx_']                      # camera matrix
        self.dist                = params['dist_']                     # distortion coefficents
        self.theta_min           = params['theta_min']                 # sobel filter x derivative low threshold 
        self.theta_max           = params['theta_max']                 # sobel filter x derivative high threshold
        self.sat_lo_threshold    = params['saturation_min']            # HSL saturation Low threshold 
        self.sat_hi_threshold    = params['saturation_max']            # HSL sat high thresh
        self.M_perspective       = params['M_perspective_transform']   # prespective tf for bird's eye view
        self.HUE1_low            = params['hue1_low']                  # hue range 1 low threshold 
        self.HUE1_hi             = params['hue1_high']
        self.HUE2_low            = params['hue2_low']
        self.HUE2_hi             = params['hue2_high']
        self.warped_img_width    = params['dst_img_width']
        self.warped_img_height   = params['dst_img_height']

        self.warp_x_expand_ = 100  # additional x space in the warped image (adjustments to warping)
        self.warp_y_expand_ = 0
        self.slice_vertical_offset = 0 # tweak slice height i.e when the road is very bendy
        self.warp_dif_shown = False;

        self.use_previous_reference = False

        # DWA params
        self.nwindows = 9  # maybe 9 is two small. hmm.. )
        self.margin   = 40
        self.minpix   = 15

        # Polynomal params 
        self.left_fit_current  = None
        self.right_fit_current = None

        self.midpoint = None
        self.slice_width = None

        self.useDWA = False
        

        self.consecutive_total_failure_count = 0;
        self.consecutive_total_failure_count_limit = 50;  # todo: cure magic number

        self.coefficent_buffer_left = deque(maxlen= 6) # buffer to hold second order coefficents for filtering out outliers
        self.coefficent_buffer_right = deque(maxlen= 6) # buffer to hold second order coefficents for filtering out outliers
        self.coef_0_max_deviation = 1e-5 # Ballpark figures to detet lane deviation
        self.coef_1_max_deviation = 0.13  # 
        self.coef_2_max_deviation = 40   #

        # PIX to m, mappings 
        self.ym_per_pix          = 3.0/210 # meters per pixel in y dimension (for perspective transformed image)
        self.xm_per_pix          = 3.7/259 # meters per pixel in x dimension (for perspective transformed image)
        
        self.lanewidth_estimated_pixels = 210 # this is a guestimation 

        minimum_lookahead_m = 5
        self.minimum_lookahead_pixels = minimum_lookahead_m/self.ym_per_pix

        self.max_lane_pixels     = 30000

        self.left_fit_m  = None
        self.right_fit_m = None

        self.sobel_kernel_size   = 15
        self.m_ppt_inv           = np.linalg.pinv(self.M_perspective)
        self.derivative_view_thresh = 1
        self.saturation_threshold   = 1
        self.last_dwa_hits = 0
        self.last_window_x = np.empty((self.nwindows, 2), dtype=int) # leftx_current and rightx_current
        self.window_mix_ratio = 0.5

        self.fig, (self.ax) = plt.subplots(1, 1, figsize=(8,14))

        self.ax.margins(x=0)

        self.hist_x = [];
        self.hist_y = [];

        # Text stuff 
        self.font                   = cv2.FONT_HERSHEY_SIMPLEX
        self.fontScale              = 1
        self.debugFontScale         = 0.5
        self.fontColor              = (255,255,255)
        self.lineType_              = 2

        linewidth  = 3
        linecolor  = 'yellow'
        self.left_lane  = Line2D(np.empty((0, 1)), np.empty((0, 1)), lw=linewidth, color=linecolor)
        self.right_lane = Line2D(np.empty((0, 1)), np.empty((0, 1)), lw=linewidth, color=linecolor)

        self.ax.add_line(self.right_lane)
        self.ax.add_line(self.left_lane)

        self.im2 = self.ax.imshow(np.empty((0, 2)), cmap='gray')
        self.lane_area_polygon = Polygon(np.empty((0, 2)),  facecolor=None, fill=False, alpha=0.4, hatch='//', color='greenyellow')

        placeholder_image = np.zeros(shape=[self.warped_img_width, self.warped_img_height,3], dtype=np.uint8)
        self.imsrc = self.ax.imshow(placeholder_image)

        # Good fits 
        self.last_good_left_fit = None
        self.last_good_right_fit = None
        self.baseline_landewidth_pixels = None

        self.last_succesful_lane_polygon = None
        self.failures_since_last_polygon = None
        self.failures_since_last_polygon_max = 15

        # Filtered coefs
        self.left_fit_filtered  = None
        self.right_fit_filtered = None

        # Error counting 
        self.left_lane_errors = 0
        self.right_lane_errors = 0

        self.entry_run_next    = 0                      # Which methoud should I call next!, [0 - DWA | 1 - proximitiy fit | 2 - terminate] @todo: use signal to do exit stuff
        self.entry_fatal_error_count = 0                # Consecutive DWA failures to produce plygon
        self.entry_fatal_error_count_limit = 20         
        self.entry_proxi_fit_misses_count = 0           # Proximity fit had to queue DWA occurences
        self.entry_proxi_fit_misses_count_limit = 300          
        self.entry_proxi_fit_recovery_by_dwa_count = 0  # Proxy fit resumed after DWA
        self.entry_dwa_next_frame_recovery = 0          # DWA recovered by reciving a good frame
        self.entry_total_error_count = 0
        self.entry_total_error_count_limit = 50
     
    '''
    @brief: Handler for preparing images, All input images need to be undistorted,
            Perspective transformed and cropped

            @input:image ,BGR input image 
    '''
    def prepare_image(self, image):

        '''
        @todo: to save processing 3 Channels grayscale first
        @totest: undistort first and then HLS and convert_to gray
        '''
        undistorted_img  = cv2.undistort(image, self.mtx, self.dist, None, self.mtx)
        gray             = cv2.cvtColor(undistorted_img, cv2.COLOR_RGB2GRAY);

        hls = cv2.cvtColor(undistorted_img, cv2.COLOR_RGB2HLS)

        H_CH = hls[:,:,0]
        S_CH = hls[:,:,2]

        HUE_filtered = np.zeros_like(H_CH)
        HUE_filtered[((H_CH >= self.HUE1_low) & (H_CH <= self.HUE1_hi)) | ((H_CH >= self.HUE2_low) & (H_CH <= self.HUE2_hi ))] = 255
        norm_gray         = cv2.normalize(gray, None, alpha=0, beta=1, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_64F)
        blurred_gray      = cv2.GaussianBlur(norm_gray, (3,3), cv2.BORDER_DEFAULT)
        sobel_x           = cv2.Sobel(blurred_gray, cv2.CV_64F, 1, 0, ksize=self.sobel_kernel_size)
        sobel_y           = cv2.Sobel(blurred_gray, cv2.CV_64F, 0, 1, ksize=self.sobel_kernel_size)
        abs_sobel_x       = np.absolute(sobel_y)
        abs_sobel_y       = np.absolute(sobel_x)
        scaled_sobel_x    = ((255*abs_sobel_x)/np.max(abs_sobel_x))  # hope this gets beautifully optimised 
        scaled_sobel_y    = ((255*abs_sobel_y)/np.max(abs_sobel_y))
        arct_img          = np.arctan2(scaled_sobel_y, scaled_sobel_x)
        
        '''
        Do the color and gradient opeations without warping the image, howerver it may be intersting to see what happens when we
        do gradient operatios on a warped image. (this is a potential @todo)
        '''

        filtered_gradient = np.zeros_like(arct_img)
        filtered_gradient[(arct_img >= self.theta_min*np.pi/180) & (arct_img <= self.theta_max*np.pi/180)] = self.derivative_view_thresh ;

        s_binary_t = np.zeros_like(S_CH)
        s_binary_t[((S_CH >= self.sat_lo_threshold) & (S_CH <=  self.sat_hi_threshold))] = 1

        combined_hue_sat_grad_binary = np.zeros_like(arct_img)
        combined_hue_sat_grad_binary[(HUE_filtered == 255) & (filtered_gradient == self.derivative_view_thresh) & (s_binary_t == self.saturation_threshold)] = 255

        '''
        Do the warping and croppping the ROI
        '''

        warped_width  = self.warp_x_expand_ + self.warped_img_width  if (np.shape(combined_hue_sat_grad_binary)[1] > self.warp_x_expand_  + self.warped_img_width)  else np.shape(combined_hue_sat_grad_binary)[1]
        warped_height = self.warp_y_expand_ + self.warped_img_height if (np.shape(combined_hue_sat_grad_binary)[0] > self.warp_y_expand_  + self.warped_img_height) else np.shape(combined_hue_sat_grad_binary)[0]

        self.hist_x = np.arange(0, warped_width, 1)
        self.slice_width = warped_width

        if((warped_width - self.warped_img_width is not 0 or warped_height - self.warped_img_height is not 0) and self.warp_dif_shown is False):
            print("modified | warped_width = ", warped_width, ", warped_height = ", warped_height, " | orignal self.warped_img_width ", self.warped_img_width, ", self.warped_img_height = ", self.warped_img_height);
            self.warp_dif_shown = True;

        warped = cv2.warpPerspective(combined_hue_sat_grad_binary, self.M_perspective, (warped_width, warped_height));

        warp_slice = warped[self.slice_vertical_offset:, :]

        self.midpoint = np.int(warp_slice.shape[1]//2)

        norm_warped = warp_slice/255;
        hist = np.sum(warp_slice, axis=0);
        
        return [norm_warped, hist,  undistorted_img] # warped for processing, undistorted for backprojection


    def filter_outliers(self, inputarray, cutoff_threshold):
        deviation        = np.abs(inputarray - np.median(inputarray))
        median_deviation = np.median(deviation)
        devdev = deviation/median_deviation if np.abs(median_deviation) > 0 else 0
        return inputarray[devdev > cutoff_threshold]


    def find_fit(self, slice_length, starting_row, leftx, lefty, rightx, righty ):
        ploty      = None
        left_fit   = None
        left_fitx  = None
        right_fit  = None
        right_fitx = None
        rightx_ok  = False
        leftx_ok   = False
        y_eval = None
        left_r_contrib = 0
        right_r_contrib = 0

        left_x = 0;
        right_x = 0;

        avg_radius_m = None
        debugmsg   = "Debug:\n" # debug message for the currnt frame

        lefty_  = np.asarray(lefty)
        leftx_  = np.asarray(leftx)
        righty_ = np.asarray(righty)
        rightx_ = np.asarray(rightx)

        lefty_clipped  = lefty_[lefty_  > starting_row]
        leftx_clipped  = leftx_[lefty_  > starting_row]

        righty_clipped = righty_[righty_ > starting_row]
        rightx_clipped = rightx_[righty_ > starting_row]

        do_left  = True if np.size(leftx_clipped)  > self.minpix else False
        do_right = True if np.size(rightx_clipped) > self.minpix else False

        debugmsg = "%sStarting row = %d\n" %(debugmsg, starting_row)

        '''
        in the case of an error (assuming it's a sudden effect from the norm we give some leeway)
        '''

        try:
            if do_left == True or do_right == True:
                ploty = np.linspace(starting_row, slice_length - 1, slice_length - starting_row) # @todo: [save compute] doesn't have to be this many

            if do_left == True :
                left_fit  = np.polyfit(lefty_clipped, leftx_clipped, 2)                          # @idea: merge with last succesful fit to get better coeffs      

            if do_right == True:
                right_fit = np.polyfit(righty_clipped, rightx_clipped, 2)

        except TypeError:
            print("Polyfit failed")


        if self.last_good_left_fit is None:
            self.last_good_left_fit = left_fit
        if self.last_good_right_fit is None:
            self.last_good_right_fit = right_fit

        '''
        He we do a rudementary check to see if the estmates are any good, Throughout the code there are several 
        checks probing varying depths via different approaches i.e. coeffcents, baselne lane width etc., this is because the code kept growing 
        as I kept experimenting with it.
        I belive that there is definetely an oppurtunity to consoldate these many tests
        @todo: Consolidate tests 
        '''
        
        if do_left == True or do_right == True:
            y_eval  = ploty[-1]                 # bottom position 
            y_eval2 = ploty[-100]               # 100 px from bottom,  @todo heal magic number 
            y_eval_m = y_eval * self.ym_per_pix;

        if do_left == True :
            left_x  = left_fit[0]*y_eval**2  + left_fit[1]  * y_eval + left_fit[2]
            left_x2  = left_fit[0]*y_eval2**2  + left_fit[1]  * y_eval2 + left_fit[2]

            left_x1_ok = True if left_x >= 0 and left_x < self.midpoint else False
            left_x2_ok = True if left_x2 >= 0 and left_x2 < self.midpoint else False

            leftx_ok = True if left_x1_ok == True and left_x2_ok == True else False # @todo: modify for better estimates
            do_left = leftx_ok

        if do_right == True:
            right_x = right_fit[0]*y_eval**2 + right_fit[1] * y_eval + right_fit[2]
            right_x2  = right_fit[0]*y_eval2**2  + right_fit[1]  * y_eval2 + right_fit[2]

            right_x1_ok = True if right_x > self.midpoint and right_x < self.slice_width else False
            right_x2_ok = True if right_x2 > self.midpoint and right_x2 < self.slice_width else False

            rightx_ok = True if right_x1_ok == True and right_x2_ok == True else False
            do_right = rightx_ok

        '''
        here we check if the coeffcents are stable , we buffer them and check latest addition against the median value,
        Note: I omitted the first coefficent from this test because it seems to change onla a lttle and my experiments showed that 
              I may get away by just using the last two.
        '''
            
        if leftx_ok:
            self.coefficent_buffer_left.append(left_fit) # add to right side
            l_coefs_buffer = np.asarray(self.coefficent_buffer_left)
            l_coefs_median = np.median(l_coefs_buffer, axis=0)
            l_coefs_dev = l_coefs_buffer[-1]- l_coefs_median # compare the last entry
            comment = "STABLE"
            if np.abs(l_coefs_dev[1]) > self.coef_1_max_deviation or np.abs(l_coefs_dev[2]) > self.coef_2_max_deviation:
                leftx_ok = False
                do_left = False
                comment = "UNSTABLE WARNING"
                self.coefficent_buffer_left.pop() # remove from the right
            debugmsg = "%sLeft deviation  = %f  %f  %f %s\n" %(debugmsg, l_coefs_dev[0], l_coefs_dev[1], l_coefs_dev[2], comment)


        if rightx_ok:
            self.coefficent_buffer_right.append(right_fit)
            r_coefs_buffer = np.asarray(self.coefficent_buffer_right)
            r_coefs_median = np.median(r_coefs_buffer, axis=0)
            r_coefs_dev = r_coefs_buffer[-1] - r_coefs_median # compare the last entry
            comment = "STABLE "
            if np.abs(r_coefs_dev[1]) > self.coef_1_max_deviation or np.abs(r_coefs_dev[2]) > self.coef_2_max_deviation:
                rightx_ok = False
                do_right =False
                comment = "UNSTABLE WARNING"
                self.coefficent_buffer_right.pop()
            debugmsg = "%sRight deviation = %f  %f  %f %s\n" %(debugmsg, r_coefs_dev[0], r_coefs_dev[1], r_coefs_dev[2], comment)

        left_fit_dwa   = None
        left_fitx_dwa  = None
        right_fit_dwa  = None
        right_fitx_dwa = None

        '''
        @todo Note: This maybe redundant or even detrimental, check for necessity and sufficency!!
        '''

        if self.useDWA == True and (leftx_ok or rightx_ok):
            '''
            If one of the lines are stable 
            '''
            if leftx_ok == False and leftx_clipped.any() and righty_clipped.any():
                '''
                rely on DWA produced latest data only
                '''
                left_fit_dwa  = np.polyfit(lefty_clipped, leftx_clipped, 2)  # @idea: merge with last succesful fit to get better coeffs      
                left_fitx_dwa  = left_fit[0]*ploty**2  + left_fit[1]  * ploty + left_fit[2]
            
            if rightx_ok == False and rightx_clipped.any()  and righty_clipped.any():

                right_fit_dwa  = np.polyfit(righty_clipped, rightx_clipped, 2)  # @idea: merge with last succesful fit to get better coeffs      
                right_fitx_dwa  = right_fit[0]*ploty**2  + right_fit[1]  * ploty + right_fit[2]


        '''
        if both are ok we check the coefficents and the pixel lane width to make sure they are resonable 
        '''
        if do_left == True and do_right == True :
            Coef_diff = right_fit - left_fit
            '''
            if lanes are parallel first two should be similar
            '''
            debugmsg = "%sC0-diff = %.6f, C1-diff = %.3f, C2-diff = %3.1f\n" % (debugmsg, Coef_diff[0], Coef_diff[1], Coef_diff[2])

            if self.left_fit_filtered is None: # Also worth checking for filterred age 
                self.left_fit_filtered = left_fit

            if self.right_fit_filtered is None:
                self.right_fit_filtered = right_fit

            '''
            Through visual observation we can see that 
            if the second coeffcicent is greater than 0.1 and thrrd greater then 40 we are in danger 
            at this point if the other lane is good we can have a hint about the correct shape goto lane rescue
            Note: here PROXI-FIT consecutive misses count only resets when proximity fit returns a polygon 
            '''
        debugmsg = "%sbaseline lane width = %1.2f,  PROXI-FIT consecutive misses count = %d\n" % (debugmsg, self.baseline_landewidth_pixels * self.xm_per_pix if self.baseline_landewidth_pixels is not None else -1., self.entry_proxi_fit_misses_count)

        '''
        Now 'do_left' and 'do_right' have a new additional meaning by adding a sense of temporal stability to that, based on this if one is good and the other is bad we can try creating the bad one based on the good one.
        What is happening below is a using the stable values to rescue the one that seems off. 
        I have omitted the DWA mod from this as it is supposed to be a fresh look in to the picture but this may be detrimantal in cases where DWA assumoptions and actual conditions are not matching, for example 
        in the hard-challenge video the lanes are winding so hard that in instances some frames only have a fraction of one lane. Therefore to go into such a frame a naive system like ours needs a very good prior state understanding.
        which means unlike humans who benifit from a better view, better information density  and superior compute and can recover from any state we need to recognize we can launch from a subset of perspectives in a subset of lighting conditions.  
        '''
        # if (do_left == True or do_right == True) and self.useDWA == False and self.baseline_landewidth_pixels is not None: # @todo: heal magic number
        if (do_left == True or do_right == True) and self.baseline_landewidth_pixels is not None: 
            if do_left == False and do_right == True:
                '''
                if there is a good right fit use that  
                '''
                left_fit = np.array([right_fit[0], right_fit[1] , right_fit[2] - self.baseline_landewidth_pixels])
                self.coefficent_buffer_left.append(left_fit)
                debugmsg = "%sFixed: LEFT, Width = %d px \n" % (debugmsg, self.baseline_landewidth_pixels)
                leftx_ok = True
                do_left = True

            elif do_right == False and do_left == True:
                '''
                if there is a good left fit use that
                '''
                right_fit = np.array([left_fit[0], left_fit[1] , left_fit[2] + self.baseline_landewidth_pixels])
                self.coefficent_buffer_right.append(right_fit)
                debugmsg = "%sFixed: RIGHT, Width = %d px \n" % (debugmsg, self.baseline_landewidth_pixels)
                rightx_ok = True
                do_right = True

            elif do_right == False and do_left == False and self.consecutive_total_failure_count <  self.consecutive_total_failure_count_limit:
                '''
                Both lanes are approximated Rely on the past, @todo: (tothink) to be more accurate may need momentum taken into account 
                '''
                ploty = np.linspace(starting_row, slice_length - 1, slice_length - starting_row) # @todo: duplicates, bunch into convenience method!!
                right_fit = self.last_good_right_fit;  # @todo: Check for NoneType/Availability 
                left_fit  = self.last_good_left_fit;
                self.coefficent_buffer_left.append(left_fit)
                self.coefficent_buffer_right.append(right_fit)

                self.consecutive_total_failure_count = self.consecutive_total_failure_count + 1 if self.consecutive_total_failure_count <  self.consecutive_total_failure_count_limit else self.consecutive_total_failure_count_limit

                leftx_ok  = True
                rightx_ok = True 

                debugmsg = "%sFixed: BOTH SUDES | consecutive total faliurs %d/%d limit \n" % (debugmsg, self.consecutive_total_failure_count, self.consecutive_total_failure_count_limit)

        debugmsg = "%sDO LEFT = %d, DO RIGHT = %d\n" % (debugmsg, do_left, do_right)

      
        if do_left == True or do_right == True:
            y_eval = ploty[-1]
            y_eval_m = y_eval * self.ym_per_pix;

        '''
        Calculating function payload      
        '''
        if leftx_ok:
            self.last_good_left_fit = np.copy(left_fit)
            self.left_fit_current = left_fit;
            if self.coefficent_buffer_left is not None:
                self.left_fit_filtered = np.mean(self.coefficent_buffer_left, axis=0)
                left_fitx  = self.left_fit_filtered[0]*ploty**2  + self.left_fit_filtered[1]  * ploty + self.left_fit_filtered[2]
                self.left_lane.set_data(left_fitx, np.flipud(ploty) - starting_row )
                self.left_fit_m  = np.copy(self.left_fit_filtered)
                self.left_fit_m[0] = self.left_fit_m[0] * self.xm_per_pix * (1/self.ym_per_pix)**2
                self.left_fit_m[1] = self.left_fit_m[1] * self.xm_per_pix/self.ym_per_pix
                self.left_fit_m[2] = self.left_fit_m[2] * self.xm_per_pix
                self.coefficent_buffer_left.append(self.left_fit_filtered)
                left_curverad_m  = np.sqrt((1 + (2*self.left_fit_m[0]*y_eval_m + self.left_fit_m[1])**2)**3)/np.fabs(2*self.left_fit_m[0])
                left_r_contrib = left_curverad_m/2;
                left_x  = self.left_fit_filtered[0]*y_eval**2  + self.left_fit_filtered[1]  * y_eval + self.left_fit_filtered[2]
                
        if rightx_ok:
            self.last_good_right_fit = np.copy(right_fit)
            self.right_fit_current = right_fit;
            if self.coefficent_buffer_right is not None:
                self.right_fit_filtered = np.mean(self.coefficent_buffer_right, axis=0)
                right_fitx = self.right_fit_filtered[0]*ploty**2 + self.right_fit_filtered[1] * ploty + self.right_fit_filtered[2]
                self.right_lane.set_data(right_fitx, np.flipud(ploty) - starting_row) 
                self.right_fit_m = np.copy(self.right_fit_filtered)
                self.right_fit_m[0] = self.right_fit_m[0] * self.xm_per_pix * (1/self.ym_per_pix)**2
                self.right_fit_m[1] = self.right_fit_m[1] * self.xm_per_pix/self.ym_per_pix
                self.right_fit_m[2] = self.right_fit_m[2] * self.xm_per_pix
                self.coefficent_buffer_right.append(self.right_fit_filtered)
                right_curverad_m  = np.sqrt((1 + (2*self.right_fit_m[0]*y_eval_m + self.right_fit_m[1])**2)**3)/np.fabs(2*self.right_fit_m[0])
                right_r_contrib = right_curverad_m/2;
                right_x  = self.right_fit_filtered[0]*y_eval**2  + self.right_fit_filtered[1]  * y_eval + self.right_fit_filtered[2]

        if y_eval is not None:
            avg_radius_m = left_r_contrib + right_r_contrib;

        lane_centre             = (left_x + right_x) // 2;
        vehicle_offset          = (self.midpoint - lane_centre) *1.
        vehicle_offset_m_signed = vehicle_offset * self.xm_per_pix * 1.

        if self.useDWA == True and (leftx_ok or rightx_ok):
            if leftx_ok == False:
                left_fit = left_fit_dwa
                left_fitx = left_fitx_dwa

            if rightx_ok == False:
                right_fit = right_fit_dwa 
                right_fitx = right_fitx_dwa

        return ploty, left_fitx, right_fitx, vehicle_offset_m_signed, avg_radius_m, leftx_ok, rightx_ok, self.left_fit_filtered, self.right_fit_filtered, debugmsg

    '''
    This is common to both dwa and polynomial proximity fit, once two coordinate vectos had been identified we can look for a fit
    then we caluclate the radiuses and the offsets from centre
    '''
    def fit_poly(self, slice_length, leftx, lefty, rightx, righty):

        ploty      = None
        left_fitx  = None
        right_fitx = None
        slice_start__pixel_row = 0
        max_tries = 3
        tries = 0

        while tries < max_tries:

            ploty, left_fitx, right_fitx, vehicle_offset_m_signed, avg_radius_m, leftx_ok, rightx_ok, left_fit, right_fit, debugmsg  = self.find_fit(slice_length,slice_start__pixel_row, leftx, lefty, rightx, righty)
            '''
            to make sure lane lines do not get distorted away we will force the polynomials to go throgh y_bottom
            if this is not satisfied we will reduce the gain of the blending, maybe it's a good idea to define a max curve we can handle
            '''
            update_lane_polygon = False
            img_lane_poly_xy = []

            if leftx_ok == True and rightx_ok == True:

                '''
                now we take the lane width to examine if it has a consistant width 
                '''
                width = right_fitx - left_fitx
                width_y_eval_m = width[-1] * self.xm_per_pix
                checkpoints = np.linspace(width[0], width[-1], self.nwindows)
                update_lane_polygon = True
                alpha = 0.6

                if self.baseline_landewidth_pixels is None:
                    self.baseline_landewidth_pixels = (checkpoints[-2] + width[-1])/2.
                else:
                    self.baseline_landewidth_pixels = width[-1] * (1-alpha) + self.baseline_landewidth_pixels * alpha;
                lane_tolarence_pixels  = self.baseline_landewidth_pixels/8 # @todo: this is a magic number to represent 20% deviation

                deviations = np.abs(checkpoints - self.baseline_landewidth_pixels) < lane_tolarence_pixels

                '''
                because we measure distances starting from the car 
                '''
                deviations_fronm_car = np.flipud(deviations)
                clear_distance_slices = 0

                for checkpoint in range(self.nwindows):
                    clear_distance_slices += 1
                    if deviations_fronm_car[checkpoint] == False:
                        break

                debugmsg = "%sCLEAR SLICES IN NORMAL MODE = %d \n" % (debugmsg, clear_distance_slices)

                if(clear_distance_slices < self.nwindows):

                    clear_pixels = clear_distance_slices * slice_length//self.nwindows
                    if clear_pixels < self.minimum_lookahead_pixels:  # @todo: missed frame count < x
                        '''
                        we just repeat the last succesful capture
                        '''
                        None
                        update_lane_polygon = False

                        break # abort unusable frame
                    else:
                        '''
                        we try another fit with with a cropped slice
                        '''
                        slice_start__pixel_row = slice_length - clear_pixels
                        self.last_good_left_fit = left_fit*alpha + self.last_good_left_fit*(1-alpha)      
                        self.last_good_right_fit = right_fit*alpha + self.last_good_right_fit*(1-alpha)
                        tries += 1
                else: # clear distance is full
                    self.last_good_left_fit = left_fit
                    self.last_good_right_fit = right_fit
                    self.coefficent_buffer_left.append(left_fit)
                    self.coefficent_buffer_right.append(right_fit)
                    update_lane_polygon = True
                    self.entry_run_next = 1
                    break  # good frame - no need to look further
            else:
                break # bad frame

        if self.useDWA == True and self.baseline_landewidth_pixels is not None and right_fitx is not None and left_fitx is not None and (leftx_ok == True or rightx_ok == True):
            width = right_fitx - left_fitx  # @todo: check for NoneType?
            width_y_eval_m = width[-1] * self.xm_per_pix
            checkpoints = np.linspace(width[0], width[-1], self.nwindows)
            deviations = np.abs(checkpoints - self.baseline_landewidth_pixels) < 100 # @todo: heal magic number

            deviations_fronm_car = np.flipud(deviations)
            clear_distance_slices = 0

            for checkpoint in range(self.nwindows):
                clear_distance_slices += 1
                if deviations_fronm_car[checkpoint] == False:
                    break

            debugmsg = "%sDWA lane rescue\n" % (debugmsg)
            debugmsg = "%sCLEAR SLICES IN RECOVERY MODE = %d\n" % (debugmsg, clear_distance_slices)

            if clear_distance_slices > 2: # @todo: heal magic number
                leftx_ok = True
                rightx_ok = True 
                update_lane_polygon = True
                    
        if leftx_ok == True and rightx_ok == True and update_lane_polygon == True:

            ploty_augmented = ploty 
            rightx_flipped = np.flipud(right_fitx)
            xs = np.concatenate((left_fitx, rightx_flipped));
            y_flipped = np.flipud(ploty_augmented)
            ys = np.concatenate((ploty_augmented , y_flipped));
            ys += self.slice_vertical_offset
            poly_points = np.concatenate((xs.reshape(xs.shape[0], 1), ys.reshape(ys.shape[0], 1)), axis=1, out=None) # can't set datatype in this version (< 1.20.0)
            hom_augment_ones = np.ones((poly_points.shape[0], 1), dtype=float)
            hom_poly_points = np.concatenate((poly_points, hom_augment_ones), axis=1).T # we have to transpose because we multilpy with the 3*3 matrix
            img_lane_poly = self.m_ppt_inv.dot(hom_poly_points)
            img_lane_poly_hom = (img_lane_poly/img_lane_poly[-1]).T
            img_lane_poly_xy = img_lane_poly_hom[:,0:2]
            return left_fitx, right_fitx, ploty, avg_radius_m, vehicle_offset_m_signed, update_lane_polygon, img_lane_poly_xy, width_y_eval_m, debugmsg

        return left_fitx if leftx_ok == True else None, right_fitx if rightx_ok == True else None, ploty, avg_radius_m, None , False , None, None, debugmsg

    '''
    Dynamic window function, DWA 
    '''
    def dwa(self, slice, hist):

        dwadebug = "DWA-DEBUG\n"


        '''
        Note: Theoratical minimum for slice height is self.nwindows  
        '''
        midpoint = np. midpoint = np.int(hist.shape[0]//2)
        out_img = np.dstack((slice, slice, slice))
        leftx_base = np.argmax(hist[:midpoint])
        rightx_base = np.argmax(hist[midpoint:]) + midpoint
        window_height = np.int(slice.shape[0]//self.nwindows)
        leftx_current = leftx_base
        rightx_current = rightx_base

        LHS_lane_x_coords = deque(maxlen=self.max_lane_pixels)
        LHS_lane_y_coords = deque(maxlen=self.max_lane_pixels)

        RHS_lane_x_coords = deque(maxlen=self.max_lane_pixels)
        RHS_lane_y_coords = deque(maxlen=self.max_lane_pixels)
        t_margin = self.margin
        alfa = 0.7 # influence of new data on the ongoing estimate (totals to 1)
        both_lane_fits_count = 0
        any_lane_hits_count = 0

        is_dwadwa = self.useDWA

        # self.last_dwa_hits = np.empty((0,2))
        # self.last_window_x = np.empty((self.nwindows, 2), dtype=int) # left window start and right window start
        # self.window_mix_ratio = 0.5

        if is_dwadwa == False:
            self.last_window_x = np.empty((self.nwindows, 2), dtype=int) # leftx_current and rightx_current
            self.last_dwa_hits = 0
        
        dwadebug = "%sUSING DWA MEMORY\n" % (dwadebug)
        dwadebug = "%sLAST ITERATION DETECTIONS = %d\n" % (dwadebug, self.last_dwa_hits)

        keep_looking_ahead = True # clear distance from the front of vehicle 

        for window in range(self.nwindows):

            left_window_minpix_ok = False
            right_window_minpix_ok = False

            winy_low  = slice.shape[0] - (window + 1) * window_height
            winy_high = slice.shape[0] - window * window_height
            '''
            usually at this point we have to do a fresh scan
            '''
            if is_dwadwa == False or self.last_dwa_hits <= 2:

                winx_left_low  = leftx_current - t_margin
                winx_left_high = leftx_current + t_margin
                self.last_window_x[window, 0] = leftx_current

                winx_right_low  = rightx_current - t_margin 
                winx_right_high = rightx_current + t_margin
                self.last_window_x[window, 1] = rightx_current

            if is_dwadwa == True and self.last_dwa_hits > 2:
                '''
                we take into account the past window locations and constrain the new locations 
                '''
                winx_left_low  = np.int(leftx_current * self.window_mix_ratio + self.last_window_x[window, 0] * (1  - self.window_mix_ratio) - t_margin) 
                winx_left_high = np.int(leftx_current * self.window_mix_ratio + self.last_window_x[window, 0] * (1  - self.window_mix_ratio) + t_margin)

                winx_right_low   = np.int(rightx_current * self.window_mix_ratio + self.last_window_x[window, 1] * (1  - self.window_mix_ratio) - t_margin) 
                winx_right_high  = np.int(rightx_current * self.window_mix_ratio + self.last_window_x[window, 1] * (1  - self.window_mix_ratio) + t_margin) 
            '''
            y coordinates are determined by the window slice, to find the x-coordinates 
            we will get the slice and check how much non-zero pixels fall within the rectangle
            '''

            subset_ = slice[(self.nwindows - window - 1 ) * window_height: (self.nwindows-window ) * window_height, :]
            subset_nonzero = subset_.nonzero()
            subset_nzx = subset_nonzero[1]
            subset_nzy = subset_nonzero[0]

            non_zero_rect_intersects_left  = ((subset_nzx >= winx_left_low)  & (subset_nzx <= winx_left_high)).nonzero()[0]
            non_zero_rect_intersects_right = ((subset_nzx >= winx_right_low) & (subset_nzx <= winx_right_high)).nonzero()[0]

            '''
            now we know the nonzero x coordinates and the index of pixels that are within the box and are not zero
            and these indexes are valid for the base image.
            '''

            left_lane_xs  = subset_nzx[non_zero_rect_intersects_left]
            left_lane_ys  = subset_nzy[non_zero_rect_intersects_left]  + (self.nwindows - window - 1 ) * window_height
            right_lane_xs = subset_nzx[non_zero_rect_intersects_right]
            right_lane_ys = subset_nzy[non_zero_rect_intersects_right] + (self.nwindows - window - 1 ) * window_height

            if(len(non_zero_rect_intersects_left)) > self.minpix:
                leftx_current = np.int(leftx_current*(1-alfa) + np.mean(subset_nzx[non_zero_rect_intersects_left]) * alfa)
                LHS_lane_x_coords.extend(left_lane_xs)
                LHS_lane_y_coords.extend(left_lane_ys)
                cv2.rectangle(out_img, (winx_left_low, winy_low), (winx_left_high, winy_high), (0, 255/255, 0), 2)
                any_lane_hits_count += 1
                left_window_minpix_ok = True

                '''
                try propsing a location for the right lane too
                '''
                
            else:
                cv2.rectangle(out_img, (winx_left_low, winy_low), (winx_left_high, winy_high), (1., 0., 0), 2)
            
            '''
            In the case of an actual application we should track individual windows across images (frame/time wise) not window (space)
            wise, tracing across space in the same window only creates stability for one frame, it has a penalty on real world accuracy.
            In other words this is not realy great for a real world scenario at all as it is.
            '''

            if(len(non_zero_rect_intersects_right)) > self.minpix:
                rightx_current = np.int(rightx_current*(1-alfa) + np.mean(subset_nzx[non_zero_rect_intersects_right]) * alfa)
                RHS_lane_x_coords.extend(right_lane_xs)
                RHS_lane_y_coords.extend(right_lane_ys)
                cv2.rectangle(out_img, (winx_right_low, winy_low), (winx_right_high, winy_high), (0., 1., 0), 2)
                any_lane_hits_count += 1
                right_window_minpix_ok = True

                
            else:
                cv2.rectangle(out_img, (winx_right_low, winy_low), (winx_right_high, winy_high), (1., 0., 0), 2)

            if left_window_minpix_ok == True and right_window_minpix_ok == False and (is_dwadwa == False or  self.last_dwa_hits > 2):

                if window == 0:
                    rightx_current = leftx_current + self.lanewidth_estimated_pixels # will be overwritten if right has minpixels 

                if window > 0 : 
                    rightx_current = np.int((leftx_current + self.lanewidth_estimated_pixels) * alfa + rightx_current * ( 1 - alfa ))

            if right_window_minpix_ok == True and left_window_minpix_ok == False and (is_dwadwa == False or  self.last_dwa_hits > 2):

                if window == 0:
                    leftx_current = rightx_current - self.lanewidth_estimated_pixels
                if window > 0 : 
                    leftx_current = np.int((rightx_current - self.lanewidth_estimated_pixels) * alfa + leftx_current * ( 1 - alfa ))

            if left_window_minpix_ok == False or right_window_minpix_ok == False:
                keep_looking_ahead = False

            if((len(non_zero_rect_intersects_right)) > self.minpix) and keep_looking_ahead == True and  ((len(non_zero_rect_intersects_right)) > self.minpix):
                  both_lane_fits_count += 1


            self.last_window_x[window, 0] = leftx_current
            self.last_window_x[window, 1] = rightx_current
        
        self.last_dwa_hits = any_lane_hits_count
        dwadebug = "%sTHIS ITERATION DETECTIONS = %d\n" % (dwadebug, self.last_dwa_hits)

        self.useDWA = True
        left_fitx, right_fitx, ploty, avg_radius_m, vehicle_offset_m_signed, update_lane_polygon, lane_polygon, lane_width, debugmsg = self.fit_poly(slice.shape[0], LHS_lane_x_coords, LHS_lane_y_coords, RHS_lane_x_coords, RHS_lane_y_coords)


        if both_lane_fits_count > 2 and any_lane_hits_count >= 1 and update_lane_polygon == False:
            dwadebug = "%sPOLYFIT FAILED: MAYBE CLEAR AHEAD FOR %d UNITS (%1.1fm) \n" % (dwadebug, both_lane_fits_count, both_lane_fits_count * window_height * self.ym_per_pix)
            '''
            dwa suggested polygon
            '''

        debugmsg = "%s%s\n" % (debugmsg, dwadebug)

        return out_img, avg_radius_m, vehicle_offset_m_signed, update_lane_polygon, lane_polygon, lane_width, debugmsg

    '''
    *Polynomal proximity fit function [PROXI-FIT]* 
    This function needs an existing fit to work with (need dwa to to run scuccesfullly at least once)
    @Idea: I guess an ideal Self deiving car would be able to somehow determine this for any givn frame, therefore pocess an contextual 
    understandnig of it's surroundings 
    '''
    def search_near_poly(self, slice):
        self.useDWA = False
        # Grab activated pixels
        nonzero  = slice.nonzero()
        nonzeroy = np.array(nonzero[0])
        nonzerox = np.array(nonzero[1])

        '''
        search for activated x values within the margin constained by the polynomial 
        function. 
        '''
        left_lane_y_inds  = ((nonzerox >= self.left_fit_current[0] * nonzeroy**2 + self.left_fit_current[1] * nonzeroy + self.left_fit_current[2] - self.margin) & (nonzerox <= self.left_fit_current[0] * nonzeroy**2 + self.left_fit_current[1] * nonzeroy + self.left_fit_current[2] + self.margin)).nonzero()[0]
        right_lane_y_inds = ((nonzerox >= self.right_fit_current[0] * nonzeroy**2 + self.right_fit_current[1] * nonzeroy + self.right_fit_current[2] - self.margin) & (nonzerox <= self.right_fit_current[0] * nonzeroy**2 + self.right_fit_current[1] * nonzeroy + self.right_fit_current[2] + self.margin)).nonzero()[0]
        
        '''
        extract the pixel coordinates
        '''
        leftx  = nonzerox[left_lane_y_inds]
        lefty  = nonzeroy[left_lane_y_inds]

        rightx = nonzerox[right_lane_y_inds]
        righty = nonzeroy[right_lane_y_inds]

        '''
        fit_new_polynomials, get lane curvature radius estimate,  get vehicle offset estimate 
        ''' 
        left_fitx, right_fitx, ploty, avg_radius_m, vehicle_offset_m_signed, update_lane_polygon, lane_polygon, lane_width, debugmsg = self.fit_poly(slice.shape[0], leftx, lefty, rightx, righty)

        '''
        calculate polygons
        1. Lane polygons (boundary of the proximity search) - for the binary image
        2. Lane area polygon - to be backprojected into the source image
        '''

        if left_fitx is not None or right_fitx is not None:
            view_img = np.dstack((slice, slice, slice))  # multiply by 255 if thresholded to 1
            poly_img = np.zeros_like(view_img)

        if left_fitx is not None:

            left_lane_window1 = np.array([np.transpose(np.vstack([left_fitx-self.margin, ploty]))])
            left_lane_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+self.margin, ploty])))])
            left_lane_pts     = np.hstack((left_lane_window1, left_lane_window2))
            cv2.fillPoly(poly_img, np.int_([left_lane_pts]) , (0, 255, 0)) 

        if right_fitx is not None:

            right_lane_window1 = np.array([np.transpose(np.vstack([right_fitx-self.margin, ploty]))])
            right_lane_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+self.margin, ploty])))])
            right_lane_pts     = np.hstack((right_lane_window1, right_lane_window2))
            cv2.fillPoly(poly_img, np.int_([right_lane_pts]), (0, 255, 0))

        if left_fitx is not None or right_fitx is not None:

            warped_result_annotated  =  (cv2.addWeighted(view_img, 0.7, poly_img, 0.3, 0 ))/255.
            return warped_result_annotated, avg_radius_m, vehicle_offset_m_signed, update_lane_polygon, lane_polygon, lane_width, debugmsg
        else:
            return np.dstack((slice, slice, slice)), avg_radius_m, vehicle_offset_m_signed, update_lane_polygon, lane_polygon, lane_width, debugmsg
    
    '''
    state machine image prep function 
    wraps around Lane finder's image prep function
    '''
    def prep_image(self,image):

        roi_warped = self.prepare_image(image)

        return "select_lane_finder", roi_warped;

    '''
    Choose which lane finder to use:
    '''
    def select_lane_finder(self, image):

        if(self.use_previous_reference == True):
            return "find_lanes_from_prev_ref", image;
        else:
            return "find_lanes_initial", image;

    '''
    Run this when no previous lane refences are not available.
    Because this is not a realtime app, find both (othewise only the lane it can't lock onto) 
    '''
    def find_lanes_initial(self,image):
        None

    '''
    when thigs are going good, use learningsfrom previous frame, 
    The state evolution of coefficents are assumed to be markovian, 
    I think this may at least work in most cases but not all!
    '''
    def find_lanes_from_previous_references(self):
        None

    # To get the Next image this function has to return an image to be replaced.
    def entry(self, image):
        # First we get a filtered and warped image. 

        debugmsg = None

        warped, hist, undistorted_image = self.prepare_image(image)

        '''
        at startup or in a fallback, do dwa for m frames! and if seperation is stable 
        hand over to proxi-fit
        '''
        if (self.entry_run_next == 0):
            # first iteration or a recovery request
            annotated, avg_radius_m, offset_m, update_lane_polygon, lane_polygon, lane_width_m, debugmsg = self.dwa(warped, hist)
            debugmsg = "%sMODE: DWA\n" % (debugmsg)
            if update_lane_polygon == False :
                self.entry_fatal_error_count = self.entry_fatal_error_count + 1 if self.entry_fatal_error_count < self.entry_fatal_error_count_limit else self.entry_fatal_error_count_limit
                if self.entry_fatal_error_count > 1 :
                    self.entry_total_error_count += 1 # just logging 
            elif update_lane_polygon == True and (self.entry_proxi_fit_misses_count > 0) :
                self.entry_fatal_error_count = 0
                self.entry_proxi_fit_recovery_by_dwa_count += 1 # just logging
            elif update_lane_polygon == True and (self.entry_fatal_error_count > 0) :
                self.entry_fatal_error_count = 0
                self.entry_dwa_next_frame_recovery += 1

        if (self.entry_run_next == 1):
            annotated, avg_radius_m, offset_m, update_lane_polygon, lane_polygon, lane_width_m, debugmsg = self.search_near_poly(warped)
            debugmsg = "%sMODE: PROXI-FIT\n" % (debugmsg)
            if update_lane_polygon == False :
                self.entry_proxi_fit_misses_count = self.entry_proxi_fit_misses_count + 1 if self.entry_proxi_fit_misses_count < self.entry_proxi_fit_misses_count_limit else self.entry_proxi_fit_misses_count_limit
            elif update_lane_polygon == True :
                self.entry_proxi_fit_misses_count = 0

        alfa = 0.6
        lane_polygon_t = None
        if lane_polygon is not None and self.last_succesful_lane_polygon is not None and update_lane_polygon == True:
            try:
                lane_polygon_t = lane_polygon * alfa + self.last_succesful_lane_polygon * (1-alfa)
            except:
                None

        lane_polygon = lane_polygon_t if lane_polygon_t is not None else lane_polygon

        offset_msg = ""
        radius_msg = ""
        topsLeftCornerText  = (10,50)
        topsLeftCornerText1 = (10,100)
        topsLeftCornerText2 = (10,150)
        LaneWidthMText  = (10,200)
        ProxiFitRecoveryByDWANumText = (10,250)
        DWANextFrameRecoveryNumText  = (10,300)

        
        
        self.imsrc.set_data(annotated)
        augmented_image = np.zeros_like(undistorted_image)    
        radius_msg  = 'Avg R = %.1f m' % avg_radius_m if avg_radius_m is not None else "Avg R = NOT COMPUTED" ;
        lane_msg = "NO LANE MSG"
        lane_width_msg = "Lane Width = NA"

        interop_tries = 0

        if update_lane_polygon == True:

            self.entry_run_next = 1
            self.lane_area_polygon.set_xy(lane_polygon)
            poly_img = np.zeros_like(undistorted_image)
            cv2.fillPoly(poly_img, np.int_([lane_polygon]) , (0, 255, 0))  
            augmented_image  =  cv2.addWeighted(undistorted_image, 1.0 , poly_img, 0.3, 0 )

            self.last_succesful_lane_polygon = np.copy(lane_polygon)
            self.failures_since_last_polygon = 0

            offset_msg  = "Vehicle is " + "%.2f" % np.abs(offset_m )  + "m " +  ("left" if (offset_m > 0) else "right") + " of centre";
            lane_msg    = "LANE SEPERATION: YES"

            if lane_width_m is not None:
                lane_width_msg = "Lane Width = %.2fm" % lane_width_m

            cv2.putText(augmented_image, offset_msg,
                        topsLeftCornerText ,
                        self.font,
                        self.fontScale,
                        self.fontColor,
                        self.lineType_)

        elif self.failures_since_last_polygon is not None: 
            if self.failures_since_last_polygon < self.failures_since_last_polygon_max :

                self.lane_area_polygon.set_xy(self.last_succesful_lane_polygon)
                poly_img = np.zeros_like(undistorted_image)
                cv2.fillPoly(poly_img, np.int_([self.last_succesful_lane_polygon]) , (0, 255, 0))  
                augmented_image  =  cv2.addWeighted(undistorted_image, 1.0 , poly_img, 0.3, 0 )
                lane_msg  = "LANE SEPERATION: APPROXIMATED"
                self.entry_run_next = 1
                self.failures_since_last_polygon = self.failures_since_last_polygon + 1 if self.failures_since_last_polygon < self.failures_since_last_polygon_max else self.failures_since_last_polygon_max
                debugmsg = "%sConsecutive failures since last polygon = %d/%d\n" % (debugmsg, self.failures_since_last_polygon, self.failures_since_last_polygon_max)

            else:
                lane_msg  = "LANE SEPERATION: NON DETECTIONS OVER THRESHOLD [%d]" % (self.failures_since_last_polygon_max)
                self.entry_run_next = 0;
                self.left_lane.set_data(0,0)
                self.right_lane.set_data(0,0)
                debugmsg = "%sFATAL ERROR: COMPUTING ABORTED\n" % (debugmsg)
                augmented_image = undistorted_image
                radius_msg = "Avg R = NA"
       
        else:

            if self.failures_since_last_polygon is not None:
                self.failures_since_last_polygon = self.failures_since_last_polygon + 1 if self.failures_since_last_polygon < self.failures_since_last_polygon_max else self.failures_since_last_polygon_max

            self.entry_run_next = 0
            augmented_image = undistorted_image
            lane_msg  = "LANE SEPERATION: NO"
        

        proxi_dwa_recovery_num_text = "PROXI-DWA Recovery Count          = %d" % self.entry_proxi_fit_recovery_by_dwa_count;
        dwa_dwa_recovery_num_text   = "DWA-DWA Recovery Attempts Count   = %d" % self.entry_total_error_count;
        if debugmsg is None:
            debugmsg = ""

        cv2.putText(augmented_image, lane_msg,
            topsLeftCornerText2 ,
            self.font,
            self.fontScale,
            self.fontColor,
            self.lineType_)

        cv2.putText(augmented_image, radius_msg,
            topsLeftCornerText1 ,
            self.font,
            self.fontScale,
            self.fontColor,
            self.lineType_)

        cv2.putText(augmented_image, proxi_dwa_recovery_num_text,
            ProxiFitRecoveryByDWANumText ,
            self.font,
            self.fontScale,
            self.fontColor,
            self.lineType_)

        cv2.putText(augmented_image, dwa_dwa_recovery_num_text,
            DWANextFrameRecoveryNumText ,
            self.font,
            self.fontScale,
            self.fontColor,
            self.lineType_)

        cv2.putText(augmented_image, lane_width_msg,
            LaneWidthMText ,
            self.font,
            self.fontScale,
            self.fontColor,
            self.lineType_)

        debugmsgText = (10, 350)
        y0, dy = 350, 25
        for i, line in enumerate(debugmsg.split('\n')):
            y = y0 + i*dy
            cv2.putText(augmented_image, line,
                (10, y) ,
                self.font,
                self.debugFontScale,
                self.fontColor,
                self.lineType_)
                   
        '''
        if we have a a prior reference we use that, If not we try to find lanes from scratch using dynamic windows.
        @note: It might be a good idea to find LEFT and RIGHT lanes seperately because usually if 
               we are in an end lane we may have a strong continous lane on one side thus saving compute.
        @todo: Implement seperate lane search 
        '''
        ex = (0, np.shape(warped)[1], 0, np.shape(warped)[0])
        self.imsrc.set_extent(ex);

        self.fig.canvas.draw()
        image_ = np.frombuffer(self.fig.canvas.tostring_rgb(), dtype='uint8')         # extract plot from canvas to crate image
        plot_image  = image_.reshape(self.fig.canvas.get_width_height()[::-1] + (3,)) # plot as an image
        aspect = np.shape(augmented_image)[1]/np.shape(augmented_image)[0]
        concat_img = cv2.hconcat([plot_image, cv2.resize(augmented_image, (np.int(aspect * np.shape(plot_image)[0]) , np.shape(plot_image)[0]) ) ]) # place the images horizontally 

        return concat_img
    
        

def terminate(lanefinder):
    print("Terminating program")
    end_tasks(lanefinder)
    raise SystemExit

def end_tasks(lanefnider):
    None

def reset(lanefinder):

    end_tasks(lanefinder)
    new_lanefinder = LaneFinder(settings)
    lanefinder = new_lanefinder; 
    return "init", None


if __name__ == "__main__":

    plt.subplots_adjust(left=0.1, bottom=0.30)

    lf1 = LaneFinder(settings)
    # extract the videos from source folder

    videos = glob.glob(input_string)
    for video in videos:
        filename = re.search(r'[^/]+$', video).group()
        outfile_name    = outdir+'/OUT_'+filename
        symfile_dir =  foldername + 'processed/latest/'
        outsymlink_name = symfile_dir + 'OUT_'+filename

        if not os.path.isdir(symfile_dir):
            print("creating folder: ", symfile_dir)
            os.makedirs(symfile_dir)

        if not os.path.isfile(outfile_name):
            clip = VideoFileClip(video)
            '''
                The output of the lambda function should be a 2D array with depth 3
                for interim results try e.g. dstacking 
            '''
            processed_clip =  clip.fl_image(lambda image: lf1.entry(image))
            processed_clip.write_videofile(outfile_name, audio=False)
            if(os.path.isfile(outsymlink_name)):
                os.remove(outsymlink_name)
            os.symlink('../'+timestamp+'/OUT_'+filename, outsymlink_name)
            print('symlink updated at ', outsymlink_name)

        else:
            print("File already exists: ", outfile_name)

        '''
        cleanup before running next video?
        '''