import cv2
import numpy as np
import math
import scipy.io as sio
import matplotlib.pyplot as plt
from PIL import Image

# načítanie obrázkov
img = cv2.imread('vz01_078_uprav4.png')
lol = img
#fig = plt.figure()
#ax = fig.add_subplot(111, projection='3d')

# definície vstupných parametrov vstupojúcich do algoritmov
pocet_ruzove = 0
pocet_ine = 0
riadky_objektu = 0
pocet_riadkov_obrazka = 5434 #px
min_sirka = 3089 #px
rezerva = 100
sirka_obrazka = 4600 # px
spodny_riadok = 1306 # px
prvy_riadok = 1310 # px
centimeter = 1270 / 7 # cm

# definície polí, do ktorých sa budú vkladať údaje
max_riadka = list(range(pocet_riadkov_obrazka))
prava_strana = list(range(pocet_riadkov_obrazka))
lava_strana = list(range(pocet_riadkov_obrazka))
vzdialenost_lava_strana = list(range(pocet_riadkov_obrazka))
distance_obj_cm = list(range(pocet_riadkov_obrazka))
strana_c_cm = list(range(pocet_riadkov_obrazka))
vyska_obj_cm = list(range(pocet_riadkov_obrazka))
vzdialenost_prava_strana = list(range(pocet_riadkov_obrazka))
print("dlzka_pola_max: "+str(len(max_riadka)))

# zisťovanie rozmerov objektu v obrázku
#print("Počítanie rozmerov.")
print("Computing object dimensions.")
for i in range(len(img)):
    #print("Počítanie riadku: "+str(i))
    print("Computing line: " + str(i))
    for j in range(len(img[i])):
        if np.any(img[i, j] != (255, 0, 255)):
            pocet_ruzove = 0
            pocet_ine = pocet_ine + 1
            if (pocet_ine > spodny_riadok) & (j > (min_sirka)) & (i > 1107) & (j < 3177):
               max_riadka[i] = pocet_ine # počítanie pixelov, ktoré nie sú ružové
               prava_strana[i] = (sirka_obrazka - j) # vypočítanie dĺžky pravej strany obrázka v pixeloch
               lava_strana[i] = (sirka_obrazka - max_riadka[i] - int(prava_strana[i])) # vypočítanie dĺžky ľavej strany obrázka v pixeloch
        if np.any(img[i, j] == (255, 0, 255)):
            pocet_ruzove = pocet_ruzove + 1 # počítanie ružových pixelov
            pocet_ine = 0 # ak nájdeme ružový pixel v riadku, počítadlo sa resetuje a začne počítať pixely inej farby, aby sme zistili šírku objektu v riadku

# uloženie zistených dát ako txt a ako .mat pre spracovanie v Matlabe
#print("Ukladanie súborov.")
print("Saving files.")
with open('pole_x.txt', 'w') as f:
    for item in max_riadka:
        f.write("%s\n" % item)

adict = {}
adict['pole_x'] = max_riadka
sio.savemat('pole.mat', adict)

dict = {}
adict['lava_strana_x'] = lava_strana
sio.savemat('lava_strana_x.mat', adict)

dict = {}
adict['prava_strana_x'] = prava_strana
sio.savemat('prava_strana_x.mat', adict)

x = 10.19133858
y = 13.69133858

polovica_kamery_vyska = 1.84
dlzka_pohladu = 45.897 * (math.sin(math.radians(90)) / math.sin(math.radians(78)))
#print("Dĺžka pohľadu: " + str(dlzka_pohladu))
print("View length: " + str(dlzka_pohladu))

sirka_od_7_cm = y
vzdialenost_od_7_cm = math.sqrt(sirka_od_7_cm*sirka_od_7_cm + dlzka_pohladu*dlzka_pohladu)
#print("Šírka od 7 cm: " + str(sirka_od_7_cm))
#print("Vzdialenosť od pohľadu: " + str(vzdialenost_od_7_cm))
print("Width bewteen image edge and 7 cm area in the middle of the picture: " + str(sirka_od_7_cm))
print("Distance between camera and 7 cm area: " + str(vzdialenost_od_7_cm))

focal_length = 8588.194311 #focal_length = (1270 * vzdialenost_od_7_cm)/7
print("Focal length: " + str(focal_length))
polovica_kamery_sirka = 1.8

#kalibrácia objektu v X osi spolu s počítaním výšky objektu na základe zistených dát
#print("Kalibrácia objektu")
print("Calibrating object.")
for x in range(len(img)):
    if (x < 1010) & (x > 4260):
        vyska_obj_cm[x] = 0
    if (x > 1109) & (x < 4259):
        if (max_riadka[x] == prvy_riadok) or (max_riadka[x] < prvy_riadok):
            continue

        # počítanie výšky objektu
        vzdialenost_prava_strana[x] = prava_strana[x] / centimeter # pravá strana v pixeloch sa prepočíta na centimetre
        distance_obj_cm[x] = (7 * focal_length) / max_riadka[x] # vypočíta sa vzdialenosť medzi kamerou a objektom, focal length = 8588.194311 pri výške kamery 45.897 cm
        a = math.sqrt(distance_obj_cm[x]*distance_obj_cm[x] - vzdialenost_prava_strana[x]*vzdialenost_prava_strana[x]) # vypočítanie strany trojuholníka pomocou cosínusovej vety, ktorá tvorí časť pohľadu kamery na kraji obrázka
        strana_c_cm[x] = dlzka_pohladu - a - polovica_kamery_sirka# vypočítanie prepony v malom trojuholníku, v ktorom je jedna strana výškou objektu
        vyska_obj_cm[x] = strana_c_cm[x] * (math.sin(math.radians(78)) / math.sin(math.radians(90))) # výška objektu vypočítaná pomocou sínusovej vety
        print("Calibrating line: " + str(x) + " vyska_obj_cm: " + str(vyska_obj_cm[x]) + " prava_strana: " + str(vzdialenost_prava_strana[x]) + " max_riadka: " + str(max_riadka[x]))

        # kalibrácia objektu
        pomer = max_riadka[x]/prvy_riadok # pomer šírky objektu v riadku s riadkom, ktorý by mal udávať skutočný rozmer objektu v pixeloch
        nova_sirka = round(sirka_obrazka/pomer) # vypočítanie novej šírky objektu v riadku na základe pomeru
        riadok_pixelov = lol[x,0:sirka_obrazka] # vybranie riadka, ktorý meníme
        novy_riadok = cv2.resize(riadok_pixelov, (1, nova_sirka)) # zmena riadka
        lava_strana_sirka = round((sirka_obrazka-nova_sirka)/2) # výpočet novej dĺžky ľavej strany
        prava_strana_sirka = sirka_obrazka-lava_strana_sirka # výpočet novej dĺžky pravej strany
        for y in range(len(novy_riadok)):
            nova_suradnica_j = lava_strana_sirka + y # určenie súradnice, na ktoré vložíme pixely z nového riadka
            lol[x,nova_suradnica_j] = novy_riadok[y, 0] # vkladanie nových pixelov z nového riadka do pôvodného obrázka

print("Saving files.")
dict = {}
adict['vzdialenost_lava_strana'] = vzdialenost_lava_strana
sio.savemat('vzdialenost_lava_strana.mat', adict)

dict = {}
adict['vzdialenost_prava_strana'] = vzdialenost_prava_strana
sio.savemat('vzdialenost_prava_strana.mat', adict)

adict = {}
adict['distance_obj_cm'] = distance_obj_cm
sio.savemat('distance_obj_cm.mat', adict)

adict = {}
adict['strana_c_cm'] = strana_c_cm
sio.savemat('strana_c_cm.mat', adict)

adict = {}
adict['vyska_obj_cm'] = vyska_obj_cm
sio.savemat('vyska_obj_cm.mat', adict)

width = round(4600/3)
height = round(5434/3)
dimension = (width,height)
img_pil = Image.open('vz01_078_uprav4.png') # načítanie obrázka vo formáte PIL
img_pil_res = img_pil.resize(dimension, Image.ANTIALIAS) # zmenšenie jeho rozmerov o 1/3
img_rgb = img_pil_res.convert("RGB") # získanie zložiek RGB z obrázka

mierka = 3 # v mierka, v akej zmenšujeme obrázok
vyska = round(5434/mierka) # 1811
sirka = round(4600/mierka) # 1533
objekt_hore = round(1124/mierka) # 375
objekt_dole = round(4259/mierka) # 1420
centimeter = 1270 / 7 # cca 181px

dimension = (sirka,vyska)
img_pil = Image.open('vz01_078_uprav4.png') # načítanie obrázka vo formáte PIL
img_pil_res = img_pil.resize(dimension, Image.ANTIALIAS) # zmenšenie jeho rozmerov o 1/3
img_rgb = img_pil_res.convert("RGB") # získanie zložiek RGB z obrázka


#print("Plním pole farieb.")
print("Filling color array.")
x = 0
#colors = list(range(vyska*sirka)) # definícia poľa, do ktorého vkladáme zložky RGB
red = list(range(vyska*sirka)) # definícia poľa, do ktorého vkladáme zložku R
green = list(range(vyska*sirka)) # definícia poľa, do ktorého vkladáme zložku G
blue = list(range(vyska*sirka)) # definícia poľa, do ktorého vkladáme zložku B
for a in range(vyska):
    for b in range(sirka):
        r_pixel,g_pixel,b_pixel = img_rgb.getpixel((b, a))
        red[x] = r_pixel
        green[x] = g_pixel
        blue[x] = b_pixel
        print("Line: " + str(a) + " Column: " + str(b) + " r: " + str(red[x]) + " g: " + str(green[x]) + " b: " + str(blue[x]))
        x = x + 1

#print("Plnim pole súradnice X.")
print("Filling coordinate X array.")
pocet_0 = 0
riadok = 0
suradnica_x = list(range(vyska*sirka))
for i in range(len(suradnica_x)):
    suradnica_x[i] = riadok
    pocet_0 = pocet_0 + 1
    if  (pocet_0 == sirka):
        pocet_0 = 0
        riadok = riadok + 1

#print("Plním pole súradnice Y.")
print("Filling coordinate Y array.")
pocet_1 = 0
stlpec = 0
suradnica_y = list(range(vyska*sirka))
for i in range(len(suradnica_y)):
    suradnica_y[i] = pocet_1
    pocet_1 = pocet_1 + 1
    if  (pocet_1 == sirka):
        pocet_1 = 0

#print("Triedenie výšky o tretinu.")
print("Shortening object height array by 1/3.")
tretina_z = list(range(1046))
x = 1124
for y in range(1046):
    if (x > 1123) & (x < 4260):
        tretina_z[y] = vyska_obj_cm[x] * centimeter/3
        print("Object height: " + str(tretina_z[y]) + " Line of height: " + str(x) + " New line: " + str(y))
    x = x + 3

#print("Triedenie ľavej strany o tretinu.")
print("Shortening left side of object array by 1/3.")
tretina_lava_strana = list(range(1046))
x = 1124
for y in range(1046):
    if (x > 1123) & (x < 4260):
        tretina_lava_strana[y] = lava_strana[x]
    x = x + 3

#print("Triedenie pravej strany o tretinu.")
print("Shortening right side of object array by 1/3.")
tretina_prava_strana = list(range(1046))
x = 1124
for y in range(1046):
    if (x > 1123) & (x < 4260):
        tretina_prava_strana[y] = prava_strana[x]
    x = x + 3

#print("Plním pole súradnice Z.")
print("Filling coordinate Z array.")
x = 0
suradnica_z = list(range(vyska*sirka))
for i in range(len(suradnica_z)):
    suradnica_z[i] = 0
for a in range(vyska):
    for b in range(sirka):
        suradnica_z[x] = 0
        if (objekt_hore < a) & (a < objekt_dole):
            objekt_lava_strana = round(tretina_lava_strana[a - objekt_hore] / mierka)
            objekt_prava_strana = (sirka - round(tretina_prava_strana[a - objekt_hore] / mierka))
            if (objekt_lava_strana < b) & (b < objekt_prava_strana):
                suradnica_z[x] = tretina_z[a - objekt_hore]
        #print("Riadok: " + str(a) + " Stĺpec: " + str(b) + " Výška: " + str(suradnica_z[x]))
        print("Line: " + str(a) + " Column: " + str(b) + " Height in pixels: " + str(suradnica_z[x]))
        x = x + 1

#print("Ukladanie súradníc a farieb 3D modelu.")
print("Saving coordinates and colors of 3D model.")
replicate = 5
with open("output_data" + str(replicate) + ".txt", "w") as out_file:
    for i in range(len(suradnica_x)):
        out_string = ""
        out_string += str(suradnica_x[i])
        out_string += " " + str(suradnica_y[i])
        out_string += " " + str(suradnica_z[i])
        out_string += " " + str(red[i])
        out_string += " " + str(green[i])
        out_string += " " + str(blue[i])
        out_string += "\n"
        out_file.write(out_string)

cv2.imshow("Calibrated image",lol)
while True:
    if cv2.waitKey(1) & 0xFF == ord('q'):
        cv2.imwrite('calibration.png', lol)
        break