#!/usr/bin/python3

# OSC komunikacija
from pythonosc.osc_server import BlockingOSCUDPServer
from pythonosc.dispatcher import Dispatcher
from time import time
from threading import Thread

# Risanje grafa
import matplotlib
matplotlib.use('GTK4Agg')
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

dispecer = Dispatcher()

srv = BlockingOSCUDPServer(("127.0.0.1", 57120), dispecer)

# Zacetna (casovna) tocka
zacetek = time()

# use ggplot style for more sophisticated visuals
plt.style.use('ggplot')


size = 1000
x_vec = []
gx_vec = []
gy_vec = []
gz_vec = []
ax_vec = []
ay_vec = []
az_vec = []

x = 0
gx = 0
gy = 0
gz = 0
ax = 0
ay = 0
az = 0

interval = 60 / 1000
razlika = 0

# Shranim "trenutne" vrednosti
def beriSenzor(address, *args):
    global x, gx, gy, gz, ax, ay, az, razlika

    if (address == '/accel'):
        ax, ay, az = args

    if (address == '/euler'):
        gx, gy, gz = args
        print("euler", args)

    razlika = time() - zacetek
    if (razlika > interval):
        izrisi()
        razlika = razlika % interval

fig = plt.gcf()
#gxs = fig.add_subplot()
#gys = fig.add_subplot()

#gxp, = gxs.plot([])
#gyp, = gys.plot([])

fajl_g = open("graf_g.csv", "w")
fajl_g.write("t,gx,gy,gz,rgx,rgy,rgz\n")
fajl_a = open("graf_a.csv", "w")
fajl_a.write("t,ax,ay,az\n")
def izrisi():
    global x, gx, gy, gz, ax, ay, az, fig, plt, gxp, gyp, zacetek

    # Dodamo casovno tocko
    x = time() - zacetek
    #x_vec.append(x)

    fajl_g.write(str(x) + "," + str(gx) + "," + str(gy) + "," + str(gz) + "," + str(gx % 3.14) + "," + str(gy % 3.14) + "," + str(gz % 3.14) + "\n")

    fajl_a.write(str(x) + "," + str(ax) + "," + str(ay) + "," + str(az) + "\n")



    # Dodamo gyro in accel vrednosti v polja
    #gx_vec.append(gx)
    #gy_vec.append(gy)
    #gz_vec.append(gz)
    #ax_vec.append(ax)
    #ay_vec.append(ay)
    #az_vec.append(az)

    #plt.cla()
    #yplot.plot(x_vec, gx_vec)
    #yplot.plot(x_vec, gy_vec)
    #fig.set_xdata(x_vec)

    #gxp.set_data(x_vec, gx_vec)
    #gyp.set_data(x_vec, gy_vec)
    #print("RISEMO", x, gx)
    #plt.plot(x_vec, gx_vec, label="gyro x")
    #plt.plot(x_vec, gy_vec)
    #plt.plot(x_vec, gz_vec)
    #plt.plot(x_vec, ax_vec)
    #plt.plot(x_vec, ay_vec)
    #plt.plot(x_vec, az_vec)

    #plt.pause(0.0001)
    #fig.canvas.draw()
    #plt.show()
    #plt.pause(0.0001)
    #return gxp,

    #plt.gcf().autofmt_xdate()
    #plt.tight_layout()
    #plt.pause(0.001)
    
    #plt.gcf()
    #plt.pause(1)
    #return plt
    #plt.show()
    #return plt

# Animiraj na cca 60hz
#animacija = FuncAnimation(fig, izrisi, 500)
#animacija = FuncAnimation(plt.gcf(), izrisi, interval=0.05, repeat=True)
#print(animacija)
#animacija = FuncAnimation(fig, izrisi)

#plt.show()

dispecer.map("/*", beriSenzor)
# Zacni poslusat senzor
print("serving...")
srv.serve_forever()
#trd = Thread(target=srv.serve_forever).start()

# Kazi graf
#print("Kazi graf!")
#plt.tight_layout()
#plt.show()
#plt.plot([0, 0], [1, 5], [3, 7])
#print('kazem', plt)

# AJD!
file.close()
print("bye")