"""
TOMÁŠ PITRUN
VYSOKÉ UČENÍ TECHNICKÉ V BRNĚ
BAKALÁŘSKÁ PRÁCE - KONTROLA KVALITY VÝROBKU
"""

#IMPORT nezbytných knihoven
import RPi.GPIO as GPIO
from time import time, sleep
import cv2 as cv
import numpy as np
#from matplotlib import pyplot as plt
import os


# Konstany
CAM_WIDTH = 1920        # [PX]
CAM_HEIGH = 1440        # [PX]
CAM_FRAMERATE = 5       # MAX FPS

#promenne
global kvalita
global zpracovavam
global status
global dokonceno_vyhodnoceni
global cekam_na_siganl
global img_counter
global running


#-------------------definvani vstupu/vystupů GPIO-------------#
GPIO.setmode(GPIO.BCM)
gpio_osvetleni = 23
gpio_OK = 24
gpio_NOK = 22
gpio_vyfot = 27

#GPIO pin 16 - VÝSTUP - slouží pro spínání osvětlení
GPIO.setup(gpio_osvetleni, GPIO.OUT)
#GPIO pin 18 - VÝSTUP - slouží pro informaci "OK" pro manipulator 
GPIO.setup(gpio_OK, GPIO.OUT)
#GPIO pin 15 - VÝSTUP - slouží pro informaci "NOK" pro manipulator
GPIO.setup(gpio_NOK, GPIO.OUT)
#GPIO pin 13 - VSTUP - informace od manipulátoru "foťů
GPIO.setup(gpio_vyfot, GPIO.IN)
#--------------------------------------------------------------#


#------------------nastaveni parametru kamery -----------------#
cap = cv.VideoCapture(0)
cap.set(cv.CAP_PROP_FRAME_WIDTH(CAM_WIDTH))
cap.set(cv.CAP_PROP_FRAME_HEIGHT(CAM_HEIGH))


def GPIO_Status():
    message = ""
    #informace o osvětlení
    if (GPIO.input(gpio_osvetleni)) == 1:
        message += "Osvetleni - ON,"
    else:
        message += "Osvetleni - OFF,"        
    #informace OK
    if (GPIO.input(gpio_OK)) == 1:
        message += "OK - 1,"
    else:
        message += "OK - 0,"
    #informace NOK
    if (GPIO.input(gpio_NOK)) == 1:
        message += "NOK - 1,"
    else:
        message += "NOK - 0,"
    #iformace o signálu pro focení
    if (GPIO.input(gpio_vyfot)) == 1:
        message += "VYFOT - 1,"
    else:
        message += "VYFOT - 0,"
    return message


def Light_control(x):
    GPIO.output(gpio_osvetleni, x)
        
def Is_signal_to_capture():
    if (GPIO.input(gpio_vyfot)) == 1:
        return True
    else:
        return False
def Wait_for_signal_to_capture(x):
    while True:
        if (GPIO.input(gpio_vyfot) == x):
            return x
        k = cv.waitKey(1)
        if k%256 == 27:
            print("Escape hit, closing ..")
            myExit()
            
def myExit():
    Light_control(GPIO.LOW)
    cap.release()
    cv.destroyAllWindows()
    GPIO.cleanup()
    exit()

def GPIO_Comunication():
    global status
    global kvalita
    if status == 'cekam_na_signal':
        GPIO.output(gpio_OK, GPIO.LOW)
        GPIO.output(gpio_NOK, GPIO.LOW)
    elif status == 'zpracovavam':
        GPIO.output(gpio_OK,GPIO.HIGH)
        GPIO.output(gpio_NOK,GPIO.HIGH)
    elif status == 'dokonceno_zpracovani':
        if kvalita == 1:
            GPIO.output(gpio_OK, GPIO.HIGH)
            GPIO.output(gpio_NOK, GPIO.LOW)
        if kvalita == 0:
            GPIO.output(gpio_OK, GPIO.LOW)
            GPIO.output(gpio_NOK, GPIO.HIGH)
    else:
        GPIO.output(gpio_OK, GPIO.LOW)
        GPIO.output(gpio_NOK, GPIO.LOW)
            
def image_to_hist(image):
    global zpracovavam
    zpracovavam = 1
    mask = np.zeros(image.shape[:2], np.uint8)
    cv.rectangle(mask, (300,650), (600,900), 255,-1)
    cv.rectangle(mask, (1360,630), (1624,850), 255, -1)
    hist_blue = cv.calcHist([image],[0], mask, [256], [0,256])
    hist_green = cv.calcHist([image], [1], mask, [256], [0,256])
    hist_red = cv.calcHist([image], [2], mask, [256], [0,256])
    sum_hist_obrazku = np.concatenate((hist_blue, hist_green, hist_red))
    return sum_hist_obrazku

def compare_with_refferance(histogram):
    # ani OK ani NOK
    refference = image_to_hist(cv.imread('/home/pi/DISTANCE/OpenCV_Frame_266.png',1)) 
    distance = np.linalg.norm(refference-histogram)
    if distance > 12000:
        return 1
    else:
        return 0

def make_capture():
    if not cap.isOpened():
        cap.open()
    img_counter = 0
    ret,frame = cap.read()
    if not ret:
        print("failed to grab frame")
        myExit()
    img_name = "OpenCV_Frame_{}.png".format(img_counter)
    cv.imwrite(img_name, frame)
    print ("{} written!".format(img_name))
    img_counter +=1
    return frame

#------------------------------------KNN-------------------------------------#
def KNN_algoritmus(OK_field_histograms, NOK_field_histograms, img_to_control_hist):    
    vzdalenost_od_OK = np.array(euclidean_distance(OK_field_histograms, img_to_control_hist))
    vzdalenost_od_NOK = np.array(euclidean_distance(NOK_field_histograms, img_to_control_hist))
    k = 3
    nearest_neighbor_ids_1 = vzdalenost_od_OK.argsort()[:k]
    nearest_neighbor_rings_1 = vzdalenost_od_OK[nearest_neighbor_ids_1]
    nearest_neighbor_ids_2 = vzdalenost_od_NOK.argsort()[:k]
    nearest_neighbor_rings_2 = vzdalenost_od_NOK[nearest_neighbor_ids_2]
    prubeh = ""
    soucet1 = 0
    soucet2 = 0
    kvalita_vyrobku = 2
    for i in range (k):
        soucet1 = soucet1 + nearest_neighbor_rings_1[i]
        soucet2 = soucet2 + nearest_neighbor_rings_2[i]
    if soucet1 > soucet2:
        kvalita_vyrobku = 0  #NOK
        prubeh += "NOK"
    else:
        kvalita_vyrobku = 1  #OK
        prubeh += "OK"
    return kvalita_vyrobku

def load_images_from_folder(folder):
    images = []
    for filename in os.listdir(folder):
        img = cv.imread(os.path.join(folder,filename))
        if img is not None:
            images.append(img)
    return images

def calculating_histograms_array(array_of_images):
    field_of_histograms = []
    for i in array_of_images:
        sum_hist = pre_proccesing(i)
        field_of_histograms.append(sum_hist)
    return field_of_histograms

def pre_proccesing(image):
    mask = np.zeros(image.shape[:2], np.uint8)
    cv.rectangle(mask, (300,650), (600,900), 255,-1)
    cv.rectangle(mask, (1360,630), (1624,850), 255, -1)
    hist_blue = cv.calcHist([image],[0],mask, [256], [0,256])
    hist_green = cv.calcHist([image], [1], mask, [256], [0,256])
    hist_red = cv.calcHist([image], [2], mask, [256], [0,256])
    sum_hist_obrazku = np.concatenate((hist_blue, hist_green, hist_red))
    return sum_hist_obrazku

        
def euclidean_distance(references,element):
    pole_vzdalenosti = []
    for i in references:    
        distance = np.linalg.norm(i-element)
        pole_vzdalenosti.append(distance)
    return pole_vzdalenosti

def set_status(x):
    global status
    status = x
    GPIO_Comunication()
    GPIO_Status()
    
def set_kvalita(x):
    global kvalita
    kvalita = x
    GPIO_Comunication()
    GPIO_Status()   

Light_control(GPIO.HIGH)

KNN_OK = load_images_from_folder("/home/pi/KNN/OK")
KNN_NOK = load_images_from_folder("/home/pi/KNN/NOK")
Hist_OK = calculating_histograms_array(KNN_OK)
Hist_NOK = calculating_histograms_array(KNN_NOK)

while True:
    set_status('cekam_na_signal')
    set_kvalita(0)
    Wait_for_signal_to_capture(1)
    snimek = make_capture()
    set_status('zpracovavam')
    hist = image_to_hist(snimek)
    # Pro algoritmus KNN
    # KNN_algoritmus(Hist_OK, Hist_NOK, hist)
    y = compare_with_refferance(hist)
    if y == 1:
        set_kvalita(1)
    else:
        set_kvalita(0)  
    set_status('dokonceno_vyhodnoceni')
    Wait_for_signal_to_capture(0)
myExit()
       
cap.release()
cv.destroyAllWindows()
GPIO.cleanup()

