import os, time
import numpy as np
import cv2 as cv

###############################################################################
# cam_bille-test.py :
# @title: Détection de la couleur rouge (bille peinte en rouge) par vision (caméra + OpenCV)
# @project: Blender-EduTech - Tutoriel : Tutoriel 6 : Labyrinthe à bille - Développement de jumeau numérique
# @lang: fr
# @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
# @copyright: Copyright (C) 2023 Philippe Roy
# @license: GNU GPL
###############################################################################

###
# Installation : 
# - pip3 install opencv-python
###

###############################################################################
# Paramètres de reconnaissance de la bille
###############################################################################

# Rayon pour une bille de 8 mm
rayon_min, rayon_max = 6, 8

# Couleur à détecter
rouge = [0, 0, 255]  # rouge dans l'espace BGR

# Cadre carré du labyrinthe avec une bordure de 10 px et un image de 640x480 px
cadre_cote = 460
cadre_x0= round((480/2)-(cadre_cote/2))
cadre_x1= round((480/2)+(cadre_cote/2))
cadre_y0 = round((640/2)-(cadre_cote/2))
cadre_y1 = round((640/2)+(cadre_cote/2))

###############################################################################
# get_limits
###############################################################################

def get_color_limits(color):
    c = np.uint8([[color]])  # BGR values
    hsvC = cv.cvtColor(c, cv.COLOR_BGR2HSV)

    hue = hsvC[0][0][0]  # Get the hue value

    # Handle red hue wrap-around
    if hue >= 165:  # Upper limit for divided red hue
        lowerLimit = np.array([hue - 10, 100, 100], dtype=np.uint8)
        upperLimit = np.array([180, 255, 255], dtype=np.uint8)
    elif hue <= 15:  # Lower limit for divided red hue
        lowerLimit = np.array([0, 100, 100], dtype=np.uint8)
        upperLimit = np.array([hue + 10, 255, 255], dtype=np.uint8)
    else:
        lowerLimit = np.array([hue - 10, 100, 100], dtype=np.uint8)
        upperLimit = np.array([hue + 10, 255, 255], dtype=np.uint8)

    return lowerLimit, upperLimit

###############################################################################
# Initialisation
###############################################################################

# Init de la caméra
cam_id = 0 # 0 pour la 1ere camera, 1 pour la seconde ...
cam = cv.VideoCapture(cam_id) # 0 pour la 1ere camera, 1 pour la
assert cam.isOpened(), "Erreur lors de l'ouverture de la camera !"

# Création de la fenêtre d'affichage
# cv.namedWindow("Caméra")

###############################################################################
# Affichage
###############################################################################

# Capture vidéo
echap=''

lower_limit, upper_limit = get_color_limits(color=rouge)

while cam.isOpened():
    cam_actif, cam_img_orig = cam.read()
    cam_img = cv.rotate(cam_img_orig, cv.ROTATE_90_CLOCKWISE)
    cam_hsv = cv.cvtColor(cam_img, cv.COLOR_BGR2HSV)
    cam_mask=cv.inRange(cam_hsv, lower_limit, upper_limit)

    # Dessin de la zone de détection
    cv.rectangle(cam_img, (cadre_x0, cadre_y0), (cadre_x1, cadre_y1), (0, 0, 255), 2) # Contour du cadre de détection

    # Détection de la bille rouge
    elements=cv.findContours(cam_mask, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)[-2]
    if len(elements) > 0:
        c=max(elements, key=cv.contourArea)
        ((cx, cy), r)=cv.minEnclosingCircle(c)
        cx=int(cx)
        cy=int(cy)
        r=int(r)
        if r>= rayon_min and r<= rayon_max:
            if cx > cadre_x0 and cx< cadre_x1 and cy > cadre_y0 and cy< cadre_y1: # Supression en dehors de la zone de détection
                cv.circle(cam_img, (cx, cy), 1, (255, 0, 255), 2) # Point des centres
                cv.circle(cam_mask, (cx, cy), 1, (255, 0, 255), 2) # Point des centres
                print ("Rayon :", r, "- Centre :", cx, cy)
                cv.circle(cam_img, (cx, cy), r, (255, 0, 255), 2) # Contour des cercles
                cv.circle(cam_mask, (cx, cy), r, (255, 0, 255), 2) # Contour des cercles

    # cv.imshow("Detection de la couleur rouge", cam_img) # "Détection" -> bug !
    cv.imshow("Detection de la couleur rouge", cam_mask) # "Détection" -> bug !

    # Sortir
    echap = cv.waitKey(1) # Saisie clavier avec un timeout de 1 ms
    if echap & 0xFF == ord('q') or echap == 27 :
        break

###############################################################################
# Quitter
###############################################################################

cam.release()
cv.destroyAllWindows()