# Path: pyKinectTools/utils/DepthUtils.py
# def skel2depth(skel, rez=(480,640)):
#     '''
#     '''
#     if type(skel) != np.ndarray:
#         skel = np.array(skel)
# 
#     mod_x = rez[0]/480.
#     mod_y = rez[1]/640.
# 
#     # 3d_d->3d_r
#     pts_r = np.dot(R, skel.T).T + T
#     # 3d_r->rgb
#     rgb_x = pts_r[:,0]/fx_rgb / pts_r[:,2] + cx_rgb
#     rgb_y = pts_r[:,1]/fy_rgb / pts_r[:,2] + cy_rgb
#     rgb_z = pts_r[:,2]
# 
#     rgb_x *= rez[1]/640.
#     rgb_y *= rez[0]/480.
# 
#     pts = np.round([rgb_x,rez[0]-rgb_y,rgb_z]).astype(np.int16).T
# 
#     return pts
# 
# def depthIm_to_colorIm(depthIm, rez=(480,640)):
#     # Convert depth->rgb and ensure points are within bounds
#     rgb_pts = world2rgb(depthIm2XYZ(depthIm), rez)
#     in_bounds = (0 <= rgb_pts[:,0]) * (rgb_pts[:,0] < rez[0]) * (0 <= rgb_pts[:,1]) * (rgb_pts[:,1] < rez[1])
#     im_out = np.zeros(rez)
#     im_out[rgb_pts[in_bounds,0], rgb_pts[in_bounds,1]] = rgb_pts[in_bounds,2]
# 
#     return im_out
# 
# Path: pyKinectTools/utils/SkeletonUtils.py
# def msr_to_kinect_skel(skel):
#     # SKEL_JOINTS = [0, 2, 3, 4, 5, 7, 8, 9, 11, 13, 15, 17, 19] # Low
# 
#     skel_kinect = np.zeros([N_KINECT_JOINTS, 3], dtype=np.int16)
#     skel_kinect[0,:] = skel[3,:] #head
#     # skel_kinect[1,:] = skel[1,:] #torso
#     skel_kinect[1,:] = skel[0,:] #torso
#     skel_kinect[2,:] = skel[4,:] #l shoulder
#     skel_kinect[3,:] = skel[5,:] #l elbow
#     skel_kinect[4,:] = skel[7,:] #l hand
#     skel_kinect[5,:] = skel[8,:] #r shoudler
#     skel_kinect[6,:] = skel[9,:] #r elbow
#     skel_kinect[7,:] = skel[11,:] #r hand
#     skel_kinect[8,:] = skel[12,:] #l hip
#     skel_kinect[9,:] = skel[13,:] #l knee
#     skel_kinect[10,:] = skel[15,:] #l foot
#     skel_kinect[11,:] = skel[16,:] #r hip
#     skel_kinect[12,:] = skel[17,:] #r knee
#     skel_kinect[13,:] = skel[18,:] #r foot
# 
#     return skel_kinect


import cv2
import numpy as np
import scipy.misc as sm
import itertools as it
from pyKinectTools.utils.DepthUtils import skel2depth, depthIm_to_colorIm#world2depth, depthIm2XYZ, depth2world
from pyKinectTools.utils.SkeletonUtils import msr_to_kinect_skel
from IPython import embed
from pyKinectTools.dataset_readers.BaseReader import BaseReader
		# Update frame
		try:
			for i in range(frames):
				self.player.next()
			return True
		except:
			return False

	def run(self):
		
		# Read data from new file
		while len(self.filenames) > 0:		
			if len(self.filenames) > 0 and self.depth_stack is None:
					print 'New video'
					name = self.filenames.pop()
					depth_file = self.base_dir + name + "depth.bin"
					color_file = self.base_dir + name + "rgb.avi"
					skeleton_file = self.base_dir + name + "skeleton.txt"
					self.depth_stack, self.mask_stack = read_MSR_depth_ims(depth_file)
					self.color_stack = read_MSR_color_ims(color_file)
					self.skel_stack,_ = read_MSR_skeletons(skeleton_file)
					# Offset!
					self.skel_stack[:,:,1] -= 75 
					framecount = np.min([self.depth_stack.shape[-1],self.color_stack.shape[-1]])

			for i in xrange(framecount):
				mask = self.mask_stack[:,:,i]
				if self.enable_bg_subtraction:
					self.depthIm = depthIm_to_colorIm(self.depth_stack[:,:,i]*mask)
				else:

# Path: pyKinectTools/utils/DepthUtils.py
# def skel2depth(skel, rez=(480,640)):
#     '''
#     '''
#     if type(skel) != np.ndarray:
#         skel = np.array(skel)
# 
#     mod_x = rez[0]/480.
#     mod_y = rez[1]/640.
# 
#     # 3d_d->3d_r
#     pts_r = np.dot(R, skel.T).T + T
#     # 3d_r->rgb
#     rgb_x = pts_r[:,0]/fx_rgb / pts_r[:,2] + cx_rgb
#     rgb_y = pts_r[:,1]/fy_rgb / pts_r[:,2] + cy_rgb
#     rgb_z = pts_r[:,2]
# 
#     rgb_x *= rez[1]/640.
#     rgb_y *= rez[0]/480.
# 
#     pts = np.round([rgb_x,rez[0]-rgb_y,rgb_z]).astype(np.int16).T
# 
#     return pts
# 
# def depthIm_to_colorIm(depthIm, rez=(480,640)):
#     # Convert depth->rgb and ensure points are within bounds
#     rgb_pts = world2rgb(depthIm2XYZ(depthIm), rez)
#     in_bounds = (0 <= rgb_pts[:,0]) * (rgb_pts[:,0] < rez[0]) * (0 <= rgb_pts[:,1]) * (rgb_pts[:,1] < rez[1])
#     im_out = np.zeros(rez)
#     im_out[rgb_pts[in_bounds,0], rgb_pts[in_bounds,1]] = rgb_pts[in_bounds,2]
# 
#     return im_out
# 
# Path: pyKinectTools/utils/SkeletonUtils.py
# def msr_to_kinect_skel(skel):
#     # SKEL_JOINTS = [0, 2, 3, 4, 5, 7, 8, 9, 11, 13, 15, 17, 19] # Low
# 
#     skel_kinect = np.zeros([N_KINECT_JOINTS, 3], dtype=np.int16)
#     skel_kinect[0,:] = skel[3,:] #head
#     # skel_kinect[1,:] = skel[1,:] #torso
#     skel_kinect[1,:] = skel[0,:] #torso
#     skel_kinect[2,:] = skel[4,:] #l shoulder
#     skel_kinect[3,:] = skel[5,:] #l elbow
#     skel_kinect[4,:] = skel[7,:] #l hand
#     skel_kinect[5,:] = skel[8,:] #r shoudler
#     skel_kinect[6,:] = skel[9,:] #r elbow
#     skel_kinect[7,:] = skel[11,:] #r hand
#     skel_kinect[8,:] = skel[12,:] #l hip
#     skel_kinect[9,:] = skel[13,:] #l knee
#     skel_kinect[10,:] = skel[15,:] #l foot
#     skel_kinect[11,:] = skel[16,:] #r hip
#     skel_kinect[12,:] = skel[17,:] #r knee
#     skel_kinect[13,:] = skel[18,:] #r foot
# 
#     return skel_kinect


# Path: pyKinectTools/dataset_readers/SMMCPlayer_tmp.py
import cv2
import numpy as np
import scipy.misc as sm
import itertools as it
from pyKinectTools.utils.DepthUtils import skel2depth, depthIm_to_colorIm#world2depth, depthIm2XYZ, depth2world
from pyKinectTools.utils.SkeletonUtils import msr_to_kinect_skel
from IPython import embed
from pyKinectTools.dataset_readers.BaseReader import BaseReader

		# Update frame
		try:
			for i in range(frames):
				self.player.next()
			return True
		except:
			return False

	def run(self):
		
		# Read data from new file
		while len(self.filenames) > 0:		
			if len(self.filenames) > 0 and self.depth_stack is None:
					print 'New video'
					name = self.filenames.pop()
					depth_file = self.base_dir + name + "depth.bin"
					color_file = self.base_dir + name + "rgb.avi"
					skeleton_file = self.base_dir + name + "skeleton.txt"
					self.depth_stack, self.mask_stack = read_MSR_depth_ims(depth_file)
					self.color_stack = read_MSR_color_ims(color_file)
					self.skel_stack,_ = read_MSR_skeletons(skeleton_file)
					# Offset!
					self.skel_stack[:,:,1] -= 75 
					framecount = np.min([self.depth_stack.shape[-1],self.color_stack.shape[-1]])

			for i in xrange(framecount):
				mask = self.mask_stack[:,:,i]
				if self.enable_bg_subtraction:
					self.depthIm = depthIm_to_colorIm(self.depth_stack[:,:,i]*mask)
				else:

					self.depthIm = depthIm_to_colorIm(self.depth_stack[:,:,i])