import cv2
import numpy as np


cap = cv2.VideoCapture(0)

def nothing(x):
    # any operation
    pass

'''
HoughLinesP:
threshold: The minimum number of intersections to "*detect*" a line
minLineLength: The minimum number of points that can form a line. Lines with less than this number of points are disregarded.
maxLineGap: The maximum gap between two points to be considered in the same line.
'''

cv2.namedWindow("Trackbars", cv2.WINDOW_AUTOSIZE)

cv2.createTrackbar("threshold", "Trackbars", 0, 255, nothing)
cv2.createTrackbar("minLineLength", "Trackbars", 0, 255, nothing)
cv2.createTrackbar("maxLineGap", "Trackbars", 0, 255, nothing)


'''
Canny:
threshold1	first threshold for the hysteresis procedure.
threshold2	second threshold for the hysteresis procedure.
apertureSize	aperture size for the Sobel operator.
'''

cv2.createTrackbar("threshold1", "Trackbars", 0, 255, nothing)
cv2.createTrackbar("threshold2", "Trackbars", 0, 255, nothing)
# cv2.createTrackbar("apertureSize", "Trackbars", 3, 7, nothing)



cv2.namedWindow("CircleTrackbars", cv2.WINDOW_AUTOSIZE)

'''
Hough Circle Transform:
Blur
param_1 = 200: Upper threshold for the internal Canny edge detector.
param_2 = 100*: Threshold for center detection.
min_radius = 0: Minimum radius to be detected. If unknown, put zero as default.
max_radius = 0: Maximum radius to be detected. If unknown, put zero as default.

'''
# cv2.createTrackbar("blur", "CircleTrackbars", 5, 16, nothing)
cv2.createTrackbar("param_1", "CircleTrackbars", 260, 500, nothing)
cv2.createTrackbar("param_2", "CircleTrackbars", 30, 255, nothing)
cv2.createTrackbar("min_radius", "CircleTrackbars", 0, 255, nothing)
cv2.createTrackbar("max_radius", "CircleTrackbars", 0, 255, nothing)

font = cv2.FONT_HERSHEY_COMPLEX


while True:

    #HoughLinesP:
    threshold = cv2.getTrackbarPos("threshold", "Trackbars")
    minLineLength = cv2.getTrackbarPos("minLineLength", "Trackbars")
    maxLineGap = cv2.getTrackbarPos("maxLineGap", "Trackbars")

    #Canny:
    threshold1 = cv2.getTrackbarPos("threshold1", "Trackbars")
    threshold2 = cv2.getTrackbarPos("threshold2", "Trackbars")
    # apertureSize = cv2.getTrackbarPos("apertureSize", "Trackbars")


    #Hough Circle Transform:
    # blur = cv2.getTrackbarPos("blur", "CircleTrackbars")
    param_1 = cv2.getTrackbarPos("param_1", "CircleTrackbars")
    param_2 = cv2.getTrackbarPos("param_2", "CircleTrackbars")
    min_radius = cv2.getTrackbarPos("min_radius", "CircleTrackbars")
    max_radius = cv2.getTrackbarPos("max_radius", "CircleTrackbars")

    _, frame = cap.read()
    src = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    # dst = cv2.Canny(src, 50, 200, None, 3)
    # dst = cv2.Canny(src, threshold1, threshold2, None, apertureSize)

    dst = cv2.Canny(src, threshold1, threshold2, None, 3)
    
    cdstP = cv2.cvtColor(dst, cv2.COLOR_GRAY2BGR)

    # linesP = cv2.HoughLinesP(dst, 1, np.pi / 180, 50, None, 50, 10)
    linesP = cv2.HoughLinesP(dst, 1, np.pi / 180, threshold, None, minLineLength, maxLineGap)
    
    if linesP is not None:
        for i in range(0, len(linesP)):
            l = linesP[i][0]
            cv2.line(cdstP, (l[0], l[1]), (l[2], l[3]), (0,0,255), 3, cv2.LINE_AA)

            #draw circles
            cv2.circle(cdstP, (l[0], l[1]), 10, (0,255,0), -1)
            cv2.circle(cdstP, (l[2], l[3]), 10, (0,255,0), -1)

    
    gray = cv2.medianBlur(src, 5)
    # gray = cv2.medianBlur(src, 9)
    rows = gray.shape[0]

    # cv2.imshow("gray Blur", gray)

    # circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, rows / 8,
    #                            param1=260, param2=30,
    #                            minRadius=10, maxRadius=150)


    circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, rows / 8,
                               param1=param_1, param2=param_2,
                               minRadius=min_radius, maxRadius=max_radius)

    if circles is not None:
        circles = np.uint16(np.around(circles))
        for i in circles[0, :]:
            center = (i[0], i[1])
            # circle center
            # cv2.circle(src, center, 1, (0, 100, 100), 3)
            cv2.circle(src, center, 1, (60, 100, 100), 3)
            # circle outline
            radius = i[2]
            #cv2.circle(src, center, radius, (255, 0, 255), 3)
            cv2.circle(src, center, radius, (190, 90, 190), 3)


    cv2.imshow("Source", src)
    cv2.imshow("Detected Lines (in red) - Probabilistic Line Transform", cdstP)
    

    key = cv2.waitKey(1)
    if key == 27:
        break

cap.release()
cv2.destroyAllWindows()