import cv2
import numpy as np

cam = cv2.VideoCapture(4)


kernel_edge_extraction = np.array(
    [[-1, -1, -1],
     [-1,  8, -1],
     [-1, -1, -1]]
)


kernel_sharpen = np.array(
    [[0, -1, 0],
     [-1, 5, -1],
     [0, -1, 0]]
)


img_counter = 0

while True:
    ret, frame = cam.read()

    if not ret:
        print("failed to grab frame")
        break
    
    enhance = cv2.filter2D(frame, -1, kernel_sharpen)
    edge_detect = cv2.filter2D(enhance, -1, kernel_edge_extraction)
    edge_detect_hls = cv2.cvtColor(edge_detect, cv2.COLOR_BGR2HLS)
    HLS_filter_mask =  cv2.inRange(edge_detect_hls[:,:,1], 150, 255)
    frame_grey_filtered = cv2.bitwise_and(cv2.cvtColor(edge_detect, cv2.COLOR_BGR2GRAY), HLS_filter_mask)
    
    lines = cv2.HoughLinesP(frame_grey_filtered, 1, np.pi/180, 60, None, 200, 5)
    display_lines = frame
    
    #for l in lines:
    #    i = l[0]
    #    cv2.line(display_lines, (i[0], i[1]), (i[2], i[3]), (0,0,255), 1, cv2.LINE_AA)
    
    res = np.concatenate((
        np.concatenate((enhance, edge_detect), axis=1),
        np.concatenate((cv2.cvtColor(frame_grey_filtered, cv2.COLOR_GRAY2BGR), display_lines), axis=1),
    ), axis=0)
    
    
    cv2.imshow("orignal", res)

    k = cv2.waitKey(1)
    if k%256 == 27:
        # ESC pressed
        print("Escape hit, closing...")
        break
    elif k%256 == 32:
        # SPACE pressed
        img_name = "opencv_frame_{}.png".format(img_counter)
        cv2.imwrite(img_name, res)
        print("{} written!".format(img_name))
        img_counter += 1

cam.release()

cv2.destroyAllWindows()