#!/usr/bin/env python
# vim: ts=8 sts=4 sw=4 expandtab autoindent smartindent
# plotting for the motor tracking CSV file
# Author Douglas Clowes 2013 (dcl@ansto.gov.au)
#
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
import argparse

def make_delta(q):
  delta_q = np.zeros(len(q))
  delta_q[0] = 0.0
  for i in range(1, len(q)):
    delta_q[i] = q[i] - q[i - 1]
  return delta_q

parser = argparse.ArgumentParser()
parser.add_argument("csv_file", help="TrackingFile.csv", nargs="?")
args = parser.parse_args()
if args.csv_file:
    csv_file = args.csv_file
else:
    csv_file = "s2_tracking.csv"

data = np.loadtxt(csv_file, delimiter=",", skiprows=1, unpack=True)

t, p, s, c = data

if False:
  print dir(data)
  #print data.T
  print len(data)
  print len(t)

delta_t = make_delta(t)
delta_p = make_delta(p)
delta_s = make_delta(s)
delta_c = make_delta(c)

fig = plt.figure()
ax1 = fig.add_subplot(111)

ax1.set_title("Motion Tracking (" + csv_file + ")")
ax1.set_xlabel("position (%.3f to %.3f)" % (p[0], p[-1]))
ax1.set_ylabel("Steps/Count")

steps_per_count = ((s[-1] - s[0]) / (c[-1] - c[0]))
print "StepsPerCount: %.6f" % steps_per_count

delta_spc = np.zeros(len(t))
for i in range(len(delta_spc)):
  if delta_c[i] == 0:
    delta_spc[i] = 0
  else:
    delta_spc[i] = delta_s[i] / delta_c[i]
if False:
  print len(t), len(delta_s), len(delta_spc)
  print delta_spc
  for i in range(len(t)):
    print "%4d: %18.6f %18.6f %18.6f %18.6f %18.6f" % (i, t[i], delta_t[i], delta_s[i], delta_c[i], delta_spc[i])

ax1.plot(p, delta_spc - steps_per_count, c='r', label='delta steps per count')
ax1.plot(p, (s - s[0])/(c - c[0]) - steps_per_count, c='b', label='steps / counts')
ax1.get_xaxis().grid(True, 'both')
ax1.get_yaxis().grid(True, 'both')
leg = ax1.legend()

plt.show()