from matplotlib import pyplot as plt
import numpy as np

def plot_data_from(filename):
  data = []
  with open(filename) as f:
    lines = f.read().splitlines()
    for line in lines:
      positions = []
      for p in line.split(";")[:-1]:
        positions.append(list(map(float, p.split(", "))))
      data.append(positions)

  number_of_joints = len(data[0])
  x = [[] for i in range(number_of_joints)]
  y = [[] for i in range(number_of_joints)]
  z = [[] for i in range(number_of_joints)]

  dx = [[] for i in range(number_of_joints)]
  dy = [[] for i in range(number_of_joints)]
  dz = [[] for i in range(number_of_joints)]

  for time in range(len(data)):
    for joint in range(len(data[time])):
      x[joint].append(data[time][joint][0])
      y[joint].append(data[time][joint][1])
      z[joint].append(data[time][joint][2])
      if (time > 0):
        dx[joint].append(x[joint][-1] - x[joint][-2])
        dy[joint].append(y[joint][-1] - y[joint][-2])
        dz[joint].append(z[joint][-1] - z[joint][-2])

  labels = ["nose","leftShoulder","rightShoulder","leftElbow","rightElbow","leftWrist","rightWrist","leftHip","rightHip","leftKnee","rightKnee","leftAnkle","rightAnkle"]
  NUM_COLORS = len(labels)
  cm = plt.get_cmap('tab20')
  fig = plt.figure("x")
  ax = fig.add_subplot(111)
  ax.set_prop_cycle(color=[cm(1.*i/NUM_COLORS) for i in range(NUM_COLORS)])
  for time, v in enumerate(x):
    plt.plot(v, label=labels[time])
  plt.legend(loc="center left")
  fig = plt.figure("y")
  ax = fig.add_subplot(111)
  ax.set_prop_cycle(color=[cm(1.*i/NUM_COLORS) for i in range(NUM_COLORS)])
  for time, v in enumerate(y):
    plt.plot(v, label=labels[time])
  plt.legend(loc="center left")
  fig = plt.figure("z")
  ax = fig.add_subplot(111)
  ax.set_prop_cycle(color=[cm(1.*i/NUM_COLORS) for i in range(NUM_COLORS)])
  for time, v in enumerate(z):
    plt.plot(v, label=labels[time])
  plt.legend(loc="center left")
  fig = plt.figure("dx")
  ax = fig.add_subplot(111)
  ax.set_prop_cycle(color=[cm(1.*i/NUM_COLORS) for i in range(NUM_COLORS)])
  for time, v in enumerate(dx):
    plt.plot(v, label=labels[time])
  plt.legend(loc="center left")
  fig = plt.figure("dy")
  ax = fig.add_subplot(111)
  ax.set_prop_cycle(color=[cm(1.*i/NUM_COLORS) for i in range(NUM_COLORS)])
  for time, v in enumerate(dy):
    plt.plot(v, label=labels[time])
  plt.legend(loc="center left")
  fig = plt.figure("dz")
  ax = fig.add_subplot(111)
  ax.set_prop_cycle(color=[cm(1.*i/NUM_COLORS) for i in range(NUM_COLORS)])
  for time, v in enumerate(dz):
    plt.plot(v, label=labels[time])
  plt.legend(loc="center left")


  plt.show()

plot_data_from("meanFilteredData.csv")