import socket
import struct
import matplotlib.pyplot as plot
import matplotlib.animation as animation

KP = 700
KD = 300

KPD = 65
KID = 60

KPQ = 65
KIQ = 60

data = bytearray(800)
A_values = [0 for i in range(100)]
A_data = [0 for i in range(1000)]

B_values = [0 for i in range(100)]
B_data = [0 for i in range(1000)]

C_values = [0 for i in range(100)]
C_data = [0 for i in range(1000)]

D_values = [0 for i in range(100)]
D_data = [0 for i in range(1000)]

def read_data(self, lines_A, lines_B, lines_C, lines_D):
    global A_data, B_data, C_data, D_data
    global data

    if(socket.recv_into(data) == 800):
        values = struct.unpack("<" + "h" * 400, data)
        
        for i in range(100):
            A_values[i] = values[i * 4] * (24 / 1024) # 1024 LSB ~ 24 V
            B_values[i] = values[(i * 4) + 1] * (24 / 1024)
            C_values[i] = values[(i * 4) + 2] * (42 / 1820) # 1820 LSB ~ 42 A
            D_values[i] = values[(i * 4) + 3] * (42 / 1820)
            
            print("Ud: " + str("{0:.2f}".format(A_values[i])) + " V", end = "")
            print(" Uq: " + str("{0:.2f}".format(B_values[i])) + " V", end = "")
            print(" Id: " + str("{0:.2f}".format(C_values[i])) + " A", end = "")
            print(" Iq: " + str("{0:.2f}".format(D_values[i])) + " A")
            
        A_data.extend(A_values)
        if(len(A_data) >= 1000):
            A_data = A_data[100:]
            
        B_data.extend(B_values)
        if(len(B_data) >= 1000):
            B_data = B_data[100:]
            
        C_data.extend(C_values)
        if(len(C_data) >= 1000):
            C_data = C_data[100:]
            
        D_data.extend(D_values)
        if(len(D_data) >= 1000):
            D_data = D_data[100:]

        lines_A.set_data(range(1000), A_data)
        lines_B.set_data(range(1000), B_data)
        lines_C.set_data(range(1000), C_data)
        lines_D.set_data(range(1000), D_data)
        

socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
socket.bind(("192.168.5.5", 4001))
#socket.sendto(struct.pack("<HHHHHH", KPD, KID, KPQ, KIQ, KP, KD), ("192.168.5.1", 4000))
        
figure = plot.figure()
figure.suptitle("Časová závislost proudů Id, Iq a napětí Ud, Uq")

axes_A = plot.axes(xlim = (0, 1000), ylim = (-2000, 2000))
axes_A.set_xlabel("Čas [ms]")
axes_A.set_ylabel("Id, Iq [A], Ud, Uq [V]")
lines_A, = axes_A.plot([], [], label = "Ud")
axes_A.legend()

axes_B = plot.axes(xlim = (0, 1000), ylim = (-2000, 2000))
lines_B, = axes_B.plot([], [], label = "Uq")
axes_B.legend()

axes_C = plot.axes(xlim = (0, 1000), ylim = (-2000, 2000))
lines_C, = axes_C.plot([], [], label = "Id")
axes_C.legend()

axes_D = plot.axes(xlim = (0, 1000), ylim = (-2000, 2000))
lines_D, = axes_D.plot([], [], label = "Iq")
axes_D.legend()

anim = animation.FuncAnimation(figure, read_data, fargs = (lines_A, lines_B, lines_C, lines_D), interval = 50)

plot.show()

socket.close()

print("Nashledanou příště!")
