#!/usr/bin/python

import sys
import math
import pickle
import numpy as np

sk_file = open(sys.argv[1])
out_dir = sys.argv[1][0:sys.argv[1].rfind('/')+1]
labels = []
for f in sys.argv[2:]:
    labels += [pickle.load(open(f))]
flabel = labels[0]
for label in labels:
    for run in label:
        if label[run] != flabel[run] or label[run] == '?' or label[run] == 'None':
            flabel[run] = 'None'

pframe = 0
vals = {}
data = {}
limbs = (('Head','ShoulderCenter'),\
        ('ShoulderCenter','ShoulderLeft'),('ShoulderLeft','ElbowLeft'),('ElbowLeft','WristLeft'),\
        ('ShoulderCenter','ShoulderRight'),('ShoulderRight','ElbowRight'),('ElbowRight','WristRight'),\
        ('ShoulderCenter','Spine'),('Spine','HipCenter'),\
        ('HipCenter','HipLeft'),('HipLeft','KneeLeft'),('KneeLeft','AnkleLeft'),\
        ('HipCenter','HipRight'),('HipRight','KneeRight'),('KneeRight','AnkleRight'))
dupes = ((1,4),(2,5),(3,6),(9,12),(10,13),(11,14))
#xlabels = ('h-sc','sc-sl','sl-el','el-wl','sc-sr','sr-er','er-wr','sc-s','s-hc','hc-hl','hl-kl','kl-al','hc-hr','hr-kr','kr-ar')
xlabels = ('h-sc','sc-sh','sh-el','el-wr','sc-s','s-hc','hc-hi','hi-kn','kn-an')
print '# '+','.join(['name','run','frame','z-value','z-var','z-diff']+list(xlabels))


for line in sk_file:
    run, frame, nid, joint, state, x, y, z, X, Y = line.strip().split(',')
    if frame != 'Frame':
        run = int(run)
        frame = int(frame)
        if frame != pframe and vals != {} and run in flabel and flabel[run] != 'None':
            out = []
            zv = map(lambda xyz: xyz[2], vals.values())
            zd = []
            for l in range(len(limbs)):
                j1, j2 = limbs[l]
                if j1 in vals and j2 in vals:
                    dx = vals[j1][0] - vals[j2][0]
                    dy = vals[j1][1] - vals[j2][1]
                    dz = vals[j1][2] - vals[j2][2]
                    zd += [math.fabs(dz)]
                    limb = math.sqrt(dx*dx + dy*dy + dz*dz)
                    out += [str(limb)]
                    #zv += [(vals[j1][2] + vals[j2][2])/2]
                    #dist = math.fabs((dz + 2*vals[j2][2])/2)
                    #if limb < 1000 and limb > 100:
                    #X += [dist]
                    #data[name][l] += [limb]
                    #print str(dist) + " " + str(limb)
                    #print str(dist) + "\t" + str(limb)
                else:
                    out += ['-1']
            for l in range(len(dupes)-1,-1,-1):
                l1, l2 = dupes[l]
                if out[l1] != '-1' and out[l2] != '-1':
                    out[l1] = str((float(out[l1])+float(out[l2]))/2)
                elif out[l1] == '-1' and out[l2] != '-1':
                    out[l1] = out[l2]
                out.pop(l2)
            if len(zd) >= 1:
                print ','.join([flabel[run],str(run),str(frame),str(np.average(zv)),str(np.var(zv)),str(max(zd))] + out)
            vals = {}
        if state == 'Tracked':
            vals[joint] = [float(x), float(y), float(z)]
        pframe = frame