import cv2
import numpy as np



def adjust_gamma(image, gamma=1.0):
    invGamma = 1.0 / gamma
    table = np.array([((i / 255.0) ** invGamma) * 255
                          for i in np.arange(0, 256)]).astype("uint8")
    return cv2.LUT(image, table)

def pattern_detect(cam_device=0):

    # RETURN 0 only if the test is ok
    msg="0"
    # Capture the corresponding camera device [0,1]
    capture = cv2.VideoCapture(0)
    try:
        _, image = capture.read()
    except:
        msg="Camera error"
        print(msg)
        return msg

    # If the USB cam is not connected the capture image doesn't not have size atribbute
    if not hasattr(image, 'shape'):
        msg = "USB camera connection error"
        print(msg)
        return msg
    else:
        size_img=image.shape
    # Create little squares of each color section
    # The size of the suare will be 1/6 of the total Y size on the y axis
    # and 1/25 of the total X size on the x axis
    y1 = int(size_img[0] / 2 - size_img[0] / 12)
    y2 = int(size_img[0] / 2 + size_img[0] / 12)

    # Square for RED COLOR
    xr1 = int(size_img[1] / 6 - size_img[1] / 25)
    xr2 = int(size_img[1] / 6 + size_img[1] / 25)
    red_cal = image[y1:y2, xr1:xr2]
    # Square for GREEN COLOR
    xg1 = int(size_img[1] / 2 - size_img[1] / 25)
    xg2 = int(size_img[1] / 2 + size_img[1] / 25)
    green_cal = image[y1:y2, xg1:xg2]
    # Square for BLUE COLOR
    xb1 = int(5 * size_img[1] / 6 - size_img[1] / 25)
    xb2 = int(5 * size_img[1] / 6 + size_img[1] / 25)
    blue_cal = image[y1:y2, xb1:xb2]
    # Get the average color in the box as repeting the np.average funtion
    # Average color for red
    avg_color_per_row = np.average(red_cal, axis=0)
    avg_color_rawr = np.average(avg_color_per_row, axis=0)
    # Average color for green
    avg_color_per_row = np.average(green_cal, axis=0)
    avg_color_rawg = np.average(avg_color_per_row, axis=0)
    # Average color for blue
    avg_color_per_row = np.average(blue_cal, axis=0)
    avg_color_rawb = np.average(avg_color_per_row, axis=0)

    # In cas  of the illumintion is not correct, it is performed a gamma filter in order to
    # correct this illumination problem
    # After testing the gamma factor correction is computed with the green color
    # gamma = gamma_factor / blue+red into green part
    # gamma factor = 50-100
    gamma = (100 / (avg_color_rawg[0] + avg_color_rawg[2] + 1))
    # Adjust the image acording to this gamma value
    adjusted = adjust_gamma(image, gamma=gamma)

    # Calculate again the average color using the gamma adjusted image
    # Crop the gamma adjusted image for wach color section
    red_cal = adjusted[y1:y2, xr1:xr2]
    green_cal = adjusted[y1:y2, xg1:xg2]
    blue_cal = adjusted[y1:y2, xb1:xb2]
    # Calculate the average for the red
    avg_color_per_row = np.average(red_cal, axis=0)
    avg_color_red = np.average(avg_color_per_row, axis=0)
    # Calculate the average for the green
    avg_color_per_row = np.average(green_cal, axis=0)
    avg_color_green = np.average(avg_color_per_row, axis=0)
    # Calculate the average for the blue
    avg_color_per_row = np.average(blue_cal, axis=0)
    avg_color_blue = np.average(avg_color_per_row, axis=0)
    # In order to count colour use the hsv conversion
    hsv = cv2.cvtColor(adjusted, cv2.COLOR_BGR2HSV)
    # Create a mask for each color, definig the upper and lower bound of each color in hsv space
    # Create the blue mask with the bounds
    lower_blue = np.array([100, 50, 50])
    upper_blue = np.array([130, 255, 255])
    mask_b = cv2.inRange(hsv, lower_blue, upper_blue)
    # Create the green mask with the bounds
    lower_green = np.array([50, 50, 50])
    upper_green = np.array([80, 255, 255])
    mask_g = cv2.inRange(hsv, lower_green, upper_green)
    # Create the red mask with the bounds. In this case we use a composed mask, sum of two different
    # group of bounds
    # First red mask
    lower_red = np.array([0, 50, 50])
    upper_red = np.array([10, 255, 255])
    mask0 = cv2.inRange(hsv, lower_red, upper_red)
    # Second red mask
    lower_red = np.array([170, 50, 50])
    upper_red = np.array([180, 255, 255])
    mask1 = cv2.inRange(hsv, lower_red, upper_red)
    # compose both masks
    mask_r = mask0 + mask1
    # mask_r = cv2.inRange(hsv, lower_red, upper_red)
    # Perform a morphological open to expand
    kernel = np.ones((15, 15), np.uint8)
    closing_r = cv2.morphologyEx(mask_r, cv2.MORPH_OPEN, kernel)
    closing_b = cv2.morphologyEx(mask_b, cv2.MORPH_OPEN, kernel)
    closing_g = cv2.morphologyEx(mask_g, cv2.MORPH_OPEN, kernel)
    # Count the number of pixels that are not of the corresponding color (black)
    count_r = cv2.countNonZero(closing_r)
    count_b = cv2.countNonZero(closing_b)
    count_g = cv2.countNonZero(closing_g)
    if (count_r < 5):
        msg = "RED COUNT FAIL"
        return msg
    if (count_g < 5):
        msg = "GREEN COUNT FAIL"
        return msg
    if (count_b < 5):
        msg = "BLUE COUNT FAIL"
        return msg
    if (avg_color_red[2] < 150 or avg_color_rawr[2] < 200):
        msg = "AVG RED COUNT FAIL"
        return msg
    if (avg_color_green[1] < 200 or avg_color_rawg[1] < 200):
        msg = "AVG GREEN COUNT FAIL"
        return msg
    if (avg_color_blue[0] < 200 or avg_color_rawb[0] < 200):
        msg = "AVG BLUE COUNT FAIL"
        return msg
    return msg