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

# pour caméra infrarouge 
picam2 = Picamera2()
# Video COnfig : Moins de def mais très bon FPS // Format BGR => BLUE/GREEN/RED avec caméra IR de toute façon tout est déja monochrome rouge.
video_config = picam2.create_video_configuration(main={"format": 'XRGB8888', "size": (640, 480)})
# Injection Config video
picam2.configure(video_config)
# Démarrage Cam
picam2.start()

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=1
seuil3=5 # seuil trop de différence => mvt camera
nb_image=5

#fond=common.moyenne_image(video, 500)
#fond=common.moyenne_image(video, 500)
tab_image=np.zeros([y_dim, x_dim], np.uint8)
fond=np.zeros([y_dim, x_dim], np.uint8)

fond = picam2.capture_array("main")
fond = cv2.cvtColor(fond, cv2.COLOR_BGRA2BGR) 
fond = cv2.cvtColor(fond, cv2.COLOR_BGR2GRAY)
fond=fond[ymin:ymax, xmin:xmax]

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

#tab_image = np.array(tab_image)
#fond=fond.astype(np.int32)
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:
    frame = picam2.capture_array("main")
    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.putText(frame, "{:04d} {:04d} {:04d}".format(vehicule1, vehicule2, vehicule3), (xmin1, ymin-10), cv2.FONT_HERSHEY_COMPLEX_SMALL, 2, (255, 255, 255), 2)
    #cv2.rectangle(frame, (xmin1, ymin), (xmax1, ymax), (0, 0, 255) if old_1 else (255, 0, 0), 3)
    #cv2.rectangle(frame, (xmin2, ymin), (xmax2, ymax), (0, 0, 255) if old_2 else (255, 0, 0), 3)
    #cv2.rectangle(frame, (xmin3, ymin), (xmax3, ymax), (0, 0, 255) if old_3 else (255, 0, 0), 3)
    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()