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

###############################################################################
# cam_bille-test.py :
# @title: Détection de la bille 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 9 mm
rayon_min, rayon_max = 8, 10

# 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))

###############################################################################
# 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=''
while cam.isOpened():
    cam_actif, cam_img_orig = cam.read()
    cam_img = cv.rotate(cam_img_orig, cv.ROTATE_90_COUNTERCLOCKWISE)
    cam_gray = cv.cvtColor(cam_img, cv.COLOR_BGR2GRAY)
    # cam_gray = cv.medianBlur(cam_gray, 5) # Réductoin de la netteté 
    # (thresh, cam_bw) = cv.threshold(gray, 127, 255, cv.THRESH_BINARY) # Noir et blanc
    rows = cam_gray.shape[0]

    # Version initiale
    # cercles = cv.HoughCircles(cam_gray, cv.HOUGH_GRADIENT, 1, rows / 8, param1=100, param2=30, minRadius=rayon_min, maxRadius=rayon_max)

    # Archives ...
    # cercles = cv.HoughCircles(cam_gray, cv.HOUGH_GRADIENT, 1, rows / 8, param1=30, param2=15, minRadius=rayon_min, maxRadius=rayon_max)
    # cercles = cv.HoughCircles(cam_gray, cv.HOUGH_GRADIENT, 1.5, rows / 8, param1=100, param2=30, minRadius=rayon_min, maxRadius=rayon_max)
    # cercles = cv.HoughCircles(cam_gray, cv.HOUGH_GRADIENT, 1, rows / 8, param1=100, param2=30, minRadius=5, maxRadius=10)

    # Un peu lent, fiable -> bien pour la bille de 9 mm
    cercles = cv.HoughCircles(cam_gray, cv.HOUGH_GRADIENT, 1, rows/10, param1=100, param2=15, minRadius=rayon_min, maxRadius=rayon_max) # un peu lent

    # Rapide mais avec beaucoup de faux positif -> bien pour la bille de 4 mm avec le contrôle cinématique
    # cercles = cv.HoughCircles(cam_gray, cv.HOUGH_GRADIENT, 1.5, rows/10, param1=100, param2=15, minRadius=rayon_min, maxRadius=rayon_max)

    # 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
    cv.rectangle(cam_gray, (cadre_x0, cadre_y0), (cadre_x1, cadre_y1), (0, 0, 255), 2) # Contour du cadre de détection

    # Détection de la bille
    if cercles is not None:
        cercles = np.uint16(np.around(cercles))
        for i in cercles[0, :]:
            cx, cy, r = i[0], i[1], i[2]
            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_gray, (cx, cy), 1, (255, 0, 255), 2) # Point des centres
                r = i[2]
                print ("Rayon :", r, "- Centre :", cx, cy)
                cv.circle(cam_img, (cx, cy), r, (255, 0, 255), 2) # Contour des cercles
                cv.circle(cam_gray, (cx, cy), r, (255, 0, 255), 2) # Contour des cercles
  
    cv.imshow("Detection de cercles", cam_img) # "Détection" -> bug !
    # cv.imshow("Detection de cercles", cam_gray) # "Détection" -> bug !
    # cv.imwrite("camera.png", cam_img) # Enregister l'image

    # 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()