"""
*************************************************************************
// Example program using OpenCV library
//      python >3.7 - OpenCV 4.5
// @file	e4c.py
// @author Luis M. Jimenez
// @date 2022
//
// @brief Course: Computer Vision (1782)
// Dept. of Systems Engineering and Automation
// Automation, Robotics and Computer Vision Lab (ARVC)
// http://arvc.umh.es
// University Miguel Hernandez
//
// @note Description:
//	- Capture images from a camera
//	- Detects edges (Canny detector)
//	- Extract straight lines and circles (Hough Transform)
//
*************************************************************************
"""

# Import libraries
import cv2 as cv
import numpy as np
import argparse

# -----------------------------------------
# Global variables
# -----------------------------------------

WINDOW_CAMERA1 = '(W1) Camera 1'   # window id
CAMERA_ID = 0	                   # default camera

# check command line parameters (camera id)
parser = argparse.ArgumentParser(description='OpenCV example: captures and filter images from a camera')
parser.add_argument('-c', dest='cameraID', type=int, default=CAMERA_ID, metavar='id', help='camera id')
CAMERA_ID = parser.parse_args().cameraID

# -----------------------------------------
# Put here the code to Initialize objets
# -----------------------------------------

# Open camera object
camera = cv.VideoCapture(CAMERA_ID)
if not camera.isOpened():
    print("you need to connect a camera, sorry.")
    exit()

# Getting camera resolution
cameraWidth = int(camera.get(cv.CAP_PROP_FRAME_WIDTH))
cameraHeight = int(camera.get(cv.CAP_PROP_FRAME_HEIGHT))

# Creating visualization windows
cv.namedWindow(WINDOW_CAMERA1, cv.WINDOW_AUTOSIZE)

print(f"Capturing images from camera {CAMERA_ID} ({cameraWidth},{cameraHeight})")
print("...Hit q/Q/Esc to exit.")

# -----------------------------------------
# Main Loop
# while there are images ...
# -----------------------------------------
while True:
    # Capture frame-by-frame
    ret, capture = camera.read()

    # if frame is read correctly ret is True
    if not ret:
        print("Can't receive frame (stream end?). Exiting ...")
        break
    # -----------------------------------------
    # Put your image processing code here
    # -----------------------------------------

    #  transform to gray level
    gray_image = cv.cvtColor(capture, cv.COLOR_BGR2GRAY);

    # Apply filter to image
    edge_image = cv.Canny(gray_image, threshold1=80, threshold2=150)  # Canny border detector

    # find straight lines (Hough Transform)
    lines = cv.HoughLines(edge_image, rho=1, theta=np.pi/180, threshold=200)

    # find circles (Hough Transform)
    circles = cv.HoughCircles(gray_image, cv.HOUGH_GRADIENT, dp=1, minDist=20,
                              param1=150, param2=30, minRadius=20, maxRadius=100)

    # -----------------------------------------
    # Put your visualization code here
    # -----------------------------------------
    # Draw  detected lines
    if lines is not None:
        for line in lines:
            rho, theta = line[0]       # rho = x*cos(theta)+ y*sin(theta)
            a = np.cos(theta)
            b = np.sin(theta)
            x0 = a * rho           # central point (rho is orthogonal to the line)
            y0 = b * rho
            x1 = int(x0 + 1000 * (-b))  # two far away points of the line
            y1 = int(y0 + 1000 * (a))
            x2 = int(x0 - 1000 * (-b))
            y2 = int(y0 - 1000 * (a))
            # cv.line  do  automatic clipping to the image size

            color = np.random.randint(low=0, high=256, size=3).tolist()
            cv.line(capture, (x1, y1), (x2, y2), color, thickness=2)


    # Draw  detected circles
    if circles is not None:
        circles = np.around(circles).astype(np.uint16)      # integer approximation
        for circle in circles[0, :]:
            x, y, r = circle
            color = np.random.randint(low=0, high=256, size=3).tolist()
            cv.circle(capture, (x, y), r, color, thickness=2)     # draw the outer circle
            cv.circle(capture, (x, y), 2, color, thickness=2)     # draw the center of the circle


    cv.imshow(WINDOW_CAMERA1, capture)     # Display the resulting frame

    # check keystroke to exit (image window must be on focus)
    key = cv.pollKey()
    if key == ord('q') or key == ord('Q') or key == 27:
        break

# End while (main loop)

# -----------------------------------------
# free windows and camera resources
# -----------------------------------------
cv.destroyAllWindows()
if camera.isOpened():  camera.release()