#!/usr/bin/python

import cv
import sys
import math

sk_file = open(sys.argv[1])
pic_dir = sys.argv[2] + '/'
out_dir = '~/out/'
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)
        if frame != pframe:
            if j1 in limbs:
                print 'Monkey1'
            if j2 in limbs:
                print 'Monkey2'
            print 'end'
            if j1 in limbs and j2 in limbs:
                print 'Monkey3'
                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] - r),0)
                y1 = max(int(limbs[j1][1] - r),0)
                x2 = min(int(x1 + 2*r), img.width)
                y2 = min(int(y1 + 3*r), img.height)
                sub = cv.GetSubRect(img,(x1,y1,x2-x1,y2-y1))
                #cv.Rectangle(img,(x1,y1),(x2,y2),(0,255,0),3,8,0)
                cv.SaveImage(out_dir+str(run)+"-"+str(frame)+'.jpg',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]