"""
*************************************************************************
// Example program using OpenCV library
//      python >3.7 - OpenCV 4.5
// @file	e5b.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:
//	- This example captures images from a camera, shows them in a window and...
//  - Enables a handler for mouse events
//  - Enables a trackbar
//  - Shows RGB color value under cursor in overlay
//  - Segments a region by color using Hue channel
//	- Calculates ConvexHull and convexDefects
//  - Draws convexHull
//
*************************************************************************
"""

# Import libraries
import cv2 as cv
import numpy as np
import argparse

# -----------------------------------------
# Functions
# -----------------------------------------
"""
//----------------------------------------------------------------------
// Mouse events handler  for image window
//----------------------------------------------------------------------
// event:	event type sent to the handler ->  cv.EVENT_MOUSEMOVE,
//          cv.EVENT_LBUTTONDOWN, cv.EVENT_LBUTTONUP, cv.EVENT_LBUTTONDBLCLK,
//          cv.EVENT_RBUTTONDOWN, cv.EVENT_RBUTTONUP, cv.EEVENT_RBUTTONDBLCLK,
//          cv.EVENT_MBUTTONDOWN, cv.EVENT_MBUTTONUP, cv.EEVENT_MBUTTONDBLCLK,
//          cv.EVENT_MOUSEWHEEL, cv.EVENT_MOUSEHWHEEL
// x:	X-coordinate position of the mouse in window
// y:	Y-coordinate position of the mouse in window
// flags: aditional flags sent to the handler ->
//          cv.EVENT_FLAG_SHIFTKEY, cv.EVENT_FLAG_CTRLKEY, cv.EVENT_FLAG_ALTKEY,
//          cv.EVENT_FLAG_LBUTTON, cv.EVENT_FLAG_RBUTTON, cv.EVENT_FLAG_MBUTTON, 
// param: set in cv.SetMouseCallback
//----------------------------------------------------------------------
"""
def onMouse(event, x, y, flags, param):
    global capture, ID_FILE         # global variables used in the mouse handler
    global CURSOR_POS, EXIT, BUTTON_POS, BUTTON_SIZE

    # print(f"{event=}, {x=}, {y=}, {flags=}, {param=}")

    # on click left mouse button and SHIFT key, saves image
    if event==cv.EVENT_LBUTTONDOWN and (flags & cv.EVENT_FLAG_SHIFTKEY):
        filename = f"Image{ID_FILE}.jpg"
        print(f"Saving image window in file: {filename}")
        cv.imwrite(filename, capture)   # save window image
        ID_FILE += 1

    # on moving the cursor over the image
    if event == cv.EVENT_MOUSEMOVE:
        CURSOR_POS = (y, x)     # save new cursor position in global variable cursorPos (row,col)

    # on click left mouse button
    if event == cv.EVENT_LBUTTONDOWN:
        # checks if Exit button is clicked
        if (x > BUTTON_POS[0] and x < (BUTTON_POS[0] + BUTTON_SIZE[0]) and
            y > BUTTON_POS[1] and y < (BUTTON_POS[1] + BUTTON_SIZE[1]) ):
            EXIT = True


#----------------------------------------------------------------------
# Trackbar events handler
# x: trackbar position
#----------------------------------------------------------------------
def onTrackbar(x):
    global ALPHA
    ALPHA = x

def onTrackbarNull(x):
    pass


"""
//----------------------------------------------------------------------
// make a copy of input image as background (converts it to color if necessary)
// Draws in overlay color information (under cursor) of input image 
// blends two images: disp image with overlay (24 bits BGR) and returns background image
// alpha -> 0-1  (0 means only background, 1: only overlay) [0.6 default]
// imageQuery: optional, peeks the color from this matrix instead of image
//----------------------------------------------------------------------
"""
def drawOverlay(image, alpha=0.6, imageQuery=None):
    global CURSOR_POS, BUTTON_POS, BUTTON_SIZE
    global IMAGE_ID     # 0: capture, 1:HSV, 2:Hue, 3:RES

    background = image.copy()  # creates a copy to preserve original image

    # Allocates memory for overlay image of the same size as input image
    overlay = np.zeros((image.shape[0], image.shape[1], 3), dtype=np.uint8)

    # if input image is not BGR, it is converted to BGR
    if image.ndim < 3:
        background = cv.cvtColor(background, cv.COLOR_GRAY2BGR)

    # Drawing the overlay image
    # draw button
    cv.rectangle(overlay, BUTTON_POS, BUTTON_POS + BUTTON_SIZE, (0, 120, 120), cv.FILLED)
    cv.rectangle(overlay, BUTTON_POS, BUTTON_POS + BUTTON_SIZE, (0, 255, 255), 1)

    # text with the pixel color under the cursor
    if imageQuery is None:
        color = image[CURSOR_POS]
    else:
        color = imageQuery[CURSOR_POS]

    if np.ndim(color) == 0:     # color is scalar
        if IMAGE_ID == 3 and imageQuery is None:
            cursorColor = f"Sat[{color}]"
        elif IMAGE_ID == 4 and imageQuery is None:
            cursorColor = f"Gray[{color}]"
        else:
            cursorColor = f"Hue[{color}]"  # Gray scale level
    else:
        if IMAGE_ID == 1:
            cursorColor = f"HSV{color}"
        else:
            cursorColor = f"RGB{color[::-1]}"  # reverse BGR tuple

    # print(cursorColor)

    # offset to draw text centered in the rectangle
    textsize, baseline = cv.getTextSize(cursorColor, cv.FONT_HERSHEY_DUPLEX, 0.4, 1)
    offset = BUTTON_SIZE * 0.5 + np.array(textsize) * [-0.5, 0.5]

    cv.putText(overlay, cursorColor, BUTTON_POS + offset.astype(int),
               cv.FONT_HERSHEY_DUPLEX, 0.4, (255, 255, 255), 1, cv.LINE_AA)

    # blending both images
    mask = (overlay != 0).any(axis=2)  # logical mask with non black pixels (0,0,0) in overlay
    background[mask] = cv.addWeighted(background[mask], 1 - alpha, overlay[mask], alpha, 0)

    return background

# End drawOverlay function


# -----------------------------------------
# Global variables
# -----------------------------------------
WINDOW_CAMERA1 = '(W1) Camera 1'   # window id
WINDOW_RES = '(W2) Result'         # window id
CAMERA_ID = 0	                   # default camera
KEY_F5 = 7602176                   # F5 unicode key code
ID_FILE = 1                        # filename id

# Trackbar variables
ALPHA = 60		        # % level of transparency
IMAGE_ID = 0	        # Select image to visualize -> 0:capture, 1:HSV, 2:Hue, 3:Sat, 4:Int
AREA_MIN = 1000         # min area
HSV_THRESHOLD_LOW = [110, 30, 30]   # Low and High  HSV threshold
HSV_THRESHOLD_HIGH = [130, 255, 255]   # Low and High  HSV threshold
HUE_CENTER = 10         # Hue valued to be centered in HUE normalization

EXIT = False            # exit the program
CURSOR_POS = (0, 0)     # current position of the cursor over the window (row,col)
BUTTON_SIZE = np.array((160, 25))    # Overlay button Size (width,heigth)
BUTTON_POS = np.array((0, 0)) 	 # overlay button upper left corner position (x,y)


# check command line parameters (camera id)
parser = argparse.ArgumentParser(description='OpenCV example: captures 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)
cv.namedWindow(WINDOW_RES, cv.WINDOW_AUTOSIZE)


# enable a trackbar associated to variable ALPHA
cv.createTrackbar("Transp", WINDOW_CAMERA1, ALPHA, 100, onTrackbar)
cv.createTrackbar("c/HS/H/S/I", WINDOW_CAMERA1, IMAGE_ID, 4, onTrackbarNull)
cv.createTrackbar("(p)Im/Hue", WINDOW_CAMERA1, 0, 1, onTrackbarNull)
cv.createTrackbar("HueLowThr", WINDOW_RES, HSV_THRESHOLD_LOW[0], 255, onTrackbarNull)
cv.createTrackbar("HueHighThr", WINDOW_RES, HSV_THRESHOLD_HIGH[0], 255, onTrackbarNull)
cv.createTrackbar("SatLowThr", WINDOW_RES, HSV_THRESHOLD_LOW[1], 255, onTrackbarNull)
cv.createTrackbar("SatHighThr", WINDOW_RES, HSV_THRESHOLD_HIGH[1], 255, onTrackbarNull)
cv.createTrackbar("Min. Area", WINDOW_RES, AREA_MIN, 5000, onTrackbarNull)
cv.createTrackbar("Hue Center", WINDOW_RES, HUE_CENTER, 255, onTrackbarNull)


# Setting Mouse Handler
cv.setMouseCallback(WINDOW_CAMERA1, onMouse)

print(f"Capturing images from camera {CAMERA_ID} ({cameraWidth},{cameraHeight})")
print("...Hit F5 or Space bar to capture and save the image")
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
    # -----------------------------------------
    # update trackbar variables
    IMAGE_ID = cv.getTrackbarPos("c/HS/H/S/I", WINDOW_CAMERA1)
    HSV_THRESHOLD_LOW[0] = cv.getTrackbarPos("HueLowThr", WINDOW_RES)
    HSV_THRESHOLD_HIGH[0] = cv.getTrackbarPos("HueHighThr", WINDOW_RES)
    HSV_THRESHOLD_LOW[1] = cv.getTrackbarPos("SatLowThr", WINDOW_RES)
    HSV_THRESHOLD_HIGH[1] = cv.getTrackbarPos("SatHighThr", WINDOW_RES)
    AREA_MIN = cv.getTrackbarPos("Min. Area", WINDOW_RES)
    HUE_CENTER = cv.getTrackbarPos("Hue Center", WINDOW_RES)

    hsv = cv.cvtColor(capture, cv.COLOR_BGR2HSV_FULL)  # color transform to HSV (Hue scaled  180º -> 255)
    hue, sat, intensity = cv.split(hsv)

    # Normalize HUE channel (selected hue value in the middle(128))
    lookUpTable = np.zeros(256, dtype=np.uint8)
    for i in range(len(lookUpTable)):
        lookUpTable[i] = (i+128-HUE_CENTER) % 256

    hue = cv.LUT(hue, lookUpTable)
    hsv = cv.merge((hue, sat, intensity))  # Hue shifted HSV image for inRange segmentation

    res = cv.inRange(hsv, tuple(HSV_THRESHOLD_LOW), tuple(HSV_THRESHOLD_HIGH))

    # morphological filter(opening / closing)
    kernel = cv.getStructuringElement(cv.MORPH_CROSS, (3, 3))   # [[0,1,0], [1,1,1], [0,1,0]]
    res = cv.morphologyEx(res, cv.MORPH_OPEN, kernel,  iterations=2)
    res = cv.morphologyEx(res, cv.MORPH_CLOSE, kernel, iterations=2)

    # find contours
    edge_image = cv.Canny(res, threshold1=50, threshold2=200)

    # find contours
    contours, hierarchy = cv.findContours(edge_image, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_NONE)

    # Filter small contours
    contours_filtered = list()      # empty list (remember tuples are immutable)

    # search for the biggest area contour
    id_max = 0; area_max = 0.0
    contours_filtered = list()
    for idx, cnt in enumerate(contours):
        area = cv.contourArea(cnt)
        if area > AREA_MIN:
            contours_filtered.append(cnt)
            if area > area_max:
                id_max = idx;  area_max = area


    # Calculates Convex Hull of the bigest contour (if any)
    if len(contours_filtered) > 0:
        # Convex Hull indexes clockwise
        convexhull = cv.convexHull(contours[id_max], clockwise=True, returnPoints=False)
        convexitydefects = cv.convexityDefects(contours[id_max], convexhull)

		# calculates  mean of convexity  defects
        convexity = 0.0     # covexity shape descriptor (mean of convexitydefects)
        for item in convexitydefects:
            convexity += item[0,3]/256.0

        convexity /= len(convexitydefects)
        print(f"Convexity Defect: {convexity}")

    # -----------------------------------------
    # Put your visualization code here
    # -----------------------------------------
    if IMAGE_ID == 1:
        dispImage = hsv
    elif IMAGE_ID == 2:
        dispImage = hue
    elif IMAGE_ID == 3:
        dispImage = sat
    elif IMAGE_ID == 4:
        dispImage = intensity
    else:
        dispImage = capture

    # And Select here the image to be used to query the  Gray/RGB value under cursor
    if cv.getTrackbarPos("(p)Im/Hue", WINDOW_CAMERA1) == 1:
        queryImage = hue
    else:
        queryImage = None

    # draws overlay with pixel color under cursor on capture image
    dispImage = drawOverlay(dispImage, alpha=float(ALPHA) / 100, imageQuery=queryImage)

    # Bold Draw bigger contour and convexhull
    if len(contours_filtered)>0:
        cv.drawContours(dispImage, contours, id_max, (0, 255, 0), thickness=1, lineType=cv.LINE_AA)
        # Draws convex hull for the biggest contour
        for id in convexhull[:-1,0]:
            cv.line(dispImage, tuple(contours[id_max][id,0,:]), tuple(contours[id_max][id+1,0,:]),
                    (0, 0, 255), thickness=2, lineType=cv.LINE_AA)

    cv.imshow(WINDOW_CAMERA1, dispImage)     # Display the resulting frame
    cv.imshow(WINDOW_RES, res)  # Display the processed 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 or EXIT:
        break
    elif key == KEY_F5 or key == ord(' '):
        filename = f"Image{ID_FILE}.jpg"
        print(f"Saving image window in file: {filename}")
        cv.imwrite(filename, capture)   # save window image
        ID_FILE += 1

# End while (main loop)

# -----------------------------------------
# free windows and camera resources
# -----------------------------------------
cv.destroyAllWindows()
if camera.isOpened():  camera.release()
