#!/usr/bin/python

import os
import cv
import sys
import math

sk_file = open(sys.argv[1])
pic_dir = sys.argv[2] + '/'
out_dir = '/home/jbw/out/'+sys.argv[1].split('/')[-2] + '/'
pframe = 0
limbs = {}
j1 = 'Head'
j2 = 'ShoulderCenter'


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)
        fout = out_dir+str(frame)+'.jpg'
        if frame != pframe: #and not os.path.exists(fout):
            if j1 in limbs and j2 in limbs:
                dx = limbs[j1][0] - limbs[j2][0]
                dy = limbs[j1][1] - limbs[j2][1]

                img = cv.LoadImage(pic_dir+str(pframe)+".jpg")
                r = math.sqrt(dx*dx+dy*dy)
                x1 = max(int(limbs[j1][0] - 1.5*r),0)
                y1 = max(int(limbs[j1][1] - 1.5*r),0)
                x2 = min(int(x1 + 3*r), img.width-1)
                y2 = min(int(y1 + 3*r), img.height-1)
                sub = cv.GetSubRect(img,(x1,y1,max(x2-x1,1),max(y2-y1,1)))
                #cv.Rectangle(img,(x1,y1),(x2,y2),(0,255,0),3,8,0)
                print fout
                cv.SaveImage(fout,sub)
                #cv.ShowImage('video', img)
                cv.WaitKey(5)
            pframe = frame 
            limbs = {}
        id = int(nid)

        limbs[joint] = [int(X)*640.0/400.0,int(Y)*480.0/300.0]