# From Exemple 13 / Compte les véhicules.
import cv2
import numpy as np
import time

cap = cv2.VideoCapture(0)

C1=0
color_infos=(0, 0, 255)
#Dimension du Mask (+grand = moins de FPS)
# y = Hauteur (rappel résolution 480)
ymin=300#315
ymax=450#360
y_dim = ymax-ymin
# x=largeur (rappel résolution 640)
xmin=10
xmax = 600
x_dim = xmax-xmin

seuil=10
seuil2=3 # ajuster à la taille de ces fucking frelons
seuil3=seuil2 # seuil trop de différence => mvt camera => rachraichir fond

#initialisation matrice Tab_Image et Fond
tab_image=np.zeros([y_dim, x_dim], np.uint8)
fond=np.zeros([y_dim, x_dim], np.uint8)

#print("fond.shape1",fond.shape)
tab_image = tab_image + fond
#print("tab_image.shape1",tab_image.shape)

def calcul_mean(image):
    height, width=image.shape
    s=0
    for y in range(height):
        for x in range(width):
            s+=image[y][x]
    return s/(height*width)

def calcul_mask(image, fond, seuil):
    image=cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    height, width=image.shape
    height_i, width_i=fond.shape
    if height != height_i:
        print("dif_heigt",height-height_i)
    if width != width_i:
        print("dif_width",width - width_i)

    mask=np.zeros([height, width], np.uint8)
    image=image.astype(np.int32)
    for y in range(height):
        for x in range(width):
            if abs(fond[y][x]-image[y][x])>seuil:
                mask[y][x]=255
    kernel=np.ones((5, 5), np.uint8)
    mask=cv2.erode(mask, kernel, iterations=1)
    mask=cv2.dilate(mask, kernel, iterations=3)
    return mask

while True:
    ret, frame = cap.read()
    frame = cv2.cvtColor(frame, cv2.COLOR_BGRA2BGR) 
    tickmark=cv2.getTickCount()
    mask=calcul_mask(frame[ymin:ymax, xmin:xmax], fond, seuil)
    moyenne_mask = calcul_mean(mask[0:y_dim, 0:x_dim])
    if moyenne_mask > seuil3:
        image=cv2.cvtColor(frame[ymin:ymax, xmin:xmax], cv2.COLOR_BGR2GRAY)
        #print("image.shape",image.shape)
        #print("tab_image.shape",tab_image.shape)
        tab_image = (tab_image+image)/2
        fond=tab_image
        #print("tab_image",tab_image)        
        #print("fond_2",fond)   
    
    if calcul_mean(mask[0:y_dim, 0:x_dim])> seuil2:
        message="Detection Moy: "+str(moyenne_mask)
    else:
        message="RAS Moy: "+str(moyenne_mask)
        

    fps=cv2.getTickFrequency()/(cv2.getTickCount()-tickmark)
    cv2.putText(frame, "FPS: {:05.2f}  Seuil: {:d} Tick {:d}".format(fps, seuil, tickmark, message), (1, 30), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, color_infos, 1)
    cv2.putText(frame, "Detect : " + message, (1, 50), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, color_infos, 1)
    
    cv2.imshow('video', frame)
    cv2.imshow('mask', mask)
    cv2.imshow('fond', fond.astype(np.uint8))

    key=cv2.waitKey(1)&0xFF
    if key==ord('q'):
        break
    if key==ord('p'):
        seuil+=1
    if key==ord('m'):
        seuil-=1

cap.release()
cv2.destroyAllWindows()