import cv2
import numpy as np
import math
from matplotlib import pyplot as plt

def inicijalizacija():
    #otvaranje i provjera videa
    #funkcija vraca frame koji se dalje obraduje

    cap = cv2.VideoCapture(0)
    if cap.isOpened()== True:
        _, frame = cap.read()
    else:
        cap.Open()
        _, frame = cap.read()

    return frame

def prepoznavanje_zelene(img):
    #funkcija koja prepoznaje zelene dijelove slike i vraca ih u novoj slici

    #pretvaranje slike u format hsv
    img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    #zelena boja je definirana za h(80-160)
    lower_green = np.array([40, 100, 50])
    upper_green = np.array([80, 255, 255])
    maska = cv2.inRange(img_hsv, lower_green, upper_green)

    # vracanje crne slike gdjegod nema zelene boje
    output_img = img.copy()
    output_img[np.where(maska == 0)] = 0

    return output_img

def prepoznavanje_crvene(img):
    #funkcija koja prepoznaje crvene dijelove i vraca ih u novoj slici

    # prebacivanje slike u HSV
    img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    # prva maska (0-10)
    lower_red = np.array([0, 50, 50])
    upper_red = np.array([10, 255, 255])
    mask0 = cv2.inRange(img_hsv, lower_red, upper_red)

    # druga maska (170-180)
    lower_red = np.array([170, 50, 50])
    upper_red = np.array([180, 255, 255])
    mask1 = cv2.inRange(img_hsv, lower_red, upper_red)

    # spoji maske
    mask = mask0 + mask1

    # gdje god nema crvene boje vrati crnu sliku
    output_img = img.copy()
    output_img[np.where(mask == 0)] = 0

    return output_img

def zbrajanje_slika(img1, img2):
    #funkcija koja zbraja ulazne slike u odredenoj jacini i vraca ih u jednoj slici
    img = cv2.addWeighted(img1,0.5, img2,0.5,0)

    return(img)

def prikaz_slike(img):
    cv2.imshow('img', img)
    cv2.waitKey(0)
    cv2.destroyAllWindows()
    return()

def krugovi(img):
    #funkcija koja pomocu Houghovih krugova sa zadanim parametrima trazi krugove u slici i vrca sliku sa ucrtanim krugovima i detalje o nadenim krugovima

    #prebacivanje slike u sive nijanse kako bi mogli upotrijebiti filtar
    img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    img = cv2.medianBlur(img, 5)
    cimg = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)

    #koristenje funkcije za houghove krugove
    #parametri : min udaljenost izmedu centara,param1->upper treshold, param2->treshold for center detection
    #izlaz je vektor(x,y,radijus)
    fun = cv2.HOUGH_GRADIENT
    circles = cv2.HoughCircles(img, fun, 1, 45, param1=75, param2=37, minRadius=0, maxRadius=0)

    #crtanje nadenih krugova
    circles = np.uint16(np.around(circles))
    for i in circles[0, :]:
        #crtanje vanjskog kruga
        cv2.circle(cimg, (i[0], i[1]), i[2], (0, 0, 255), 3)
        #crtanje centra kruga
        cv2.circle(cimg, (i[0], i[1]), 2, (0, 255, 255), 3)

    return(cimg, circles)

def sredista(img):
    img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    img = cv2.medianBlur(img, 5)
    cimg = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)

    #trazen
    sredista_kruz = []
    funkcija = cv2.HOUGH_GRADIENT
    circles = cv2.HoughCircles(img, funkcija, 1, 45, param1=75, param2=37, minRadius=0, maxRadius=0)
    circles = np.uint16(np.around(circles))
    for i in circles[0, :]:
        # draw the center of the circle
        cv2.circle(cimg, (i[0], i[1]), 1, (255, 255, 255), 3)
        #stavi sredista u listu
        a=i[0]
        b=i[1]
        t=[a,b]
        sredista_kruz = sredista_kruz +t

    return(cimg, sredista_kruz)

def crtanje_linija(cimg, circles):
    #crta ravne linije koje povezuju centre krugova
    for i in circles[0, :]:
        for j in circles[0, :]:
            cv2.line(cimg, (i[0], i[1]), (j[0], j[1]), (0, 255, 255), 2)

    return (cimg)

def duljina_nacrtane_linije(circles):
    #izracunava duljine nacrtanih linija i stavljanje vrijednosti u matricu
    #takoder vraca i broj krugova

    #inicijalizacija matrice za upis duljine linija
    kraj = 0
    for i in circles[0, :]:
        kraj= kraj +1
    print "broj krugova  %s" %kraj
    matrix = [[0 for x in range(kraj)] for y in range(kraj)]

    #racunanje udaljenosti sredista kruznica i vracanje u matrici
    x=-1
    y=-1
    for i in circles[0, :]:
        x=x+1
        y=-1
        for j in circles[0, :]:
            y=y+1
            matrix[x][y]= round (math.sqrt((i[0] - j[0])^2 + (i[1] - j[1])^2), 1)
    #print(matrix)

    return (matrix, kraj)

def prikaz_plohe(sredistaa, img):
    krug = []
    for i in range(len(sredistaa)):
        if (i % 2 == 0) :
            krug.append([sredistaa[i], sredistaa[i + 1]])
    krugi = np.array([krug], np.int32)
    cv2.fillPoly(img, krugi, (0,100,255))
    print "koordinate sredista ledica %s" % krugi
    plt.imshow(img)
    plt.show()
    return ()

def najkraca_udaljenost(matrix, kraj):
    #trazenje najmanje udaljenosti medu sredistima
    min = matrix[0][1]
    for i in range(kraj):
        for j in range(kraj):
            if (matrix[i][j]>0) and (matrix[i][j]<min):
                min = matrix[i][j]

    return (min)

def mjerenje_udaljenosti(mindist, zarisna_duljina):
    #vraca udaljenost kamere od promatranog robota
    stvarna_udaljenost = input('Unesi stvarnu udaljenost izmedu ledica: ')
    stvarna_udaljenost = int(stvarna_udaljenost)
    udaljenost = (zarisna_duljina*stvarna_udaljenost)/mindist
    return (udaljenost)

def video_kraj():
    cap.release()
    out.release()
    cv2.destroyAllWindows()
    return()

def main():
    frame = inicijalizacija()
    slika_crveno = prepoznavanje_crvene(frame)
    slika_zeleno = prepoznavanje_zelene(frame)
    slika_ledice = zbrajanje_slika(slika_crveno, slika_zeleno)
    prikaz_slike(slika_ledice)

    slika_sredista_kruznice, sredista_kruz = sredista(slika_ledice)
    prikaz_slike(slika_sredista_kruznice)
    print "sredista  %s" % sredista_kruz
    slika_zaokruzene_ledice, circles = krugovi(slika_ledice)
    prikaz_slike(slika_zaokruzene_ledice)

    slika_linije = crtanje_linija(slika_sredista_kruznice, circles)
    prikaz_slike(slika_linije)
    print "koordinate i polumjer krugova  %s" % circles

    matrica_duljina, broj_krugova = duljina_nacrtane_linije(circles)
    print "duljine linija na slici %s" % matrica_duljina

    mindist = najkraca_udaljenost(matrica_duljina, broj_krugova)
    print "najmanja udaljenost na slici %s" % mindist

    prikaz_plohe(sredista_kruz, slika_sredista_kruznice)

    zarisna_duljina = input('Unesi zarisnu duljinu kamere u pikselima ')
    zarisna_duljina = int(zarisna_duljina)
    udaljenost = mjerenje_udaljenosti(mindist, zarisna_duljina)
    print "udaljenost kamere od objekta %s" % udaljenost

    video_kraj()


main()