# 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 depth2world(pts, rez=(480,640)):
#     ''' Convert depth coordinates to world coordinates standard Kinect calibration '''
#     assert type(pts) == np.ndarray, "Wrong type into depth2world"
# 
#     pts = pts.astype(np.float)
#     pts[:,0] *= 480./rez[0]
#     pts[:,1] *= 640./rez[1]
# 
#     y = np.array(pts[:,1])
#     x = np.array(pts[:,0])
#     d = np.array(pts[:,2])
# 
#     xo =  ((x - cx_d) * d * fx_d)
#     yo = ((y - cy_d) * d * fy_d)
#     zo = d
# 
#     return np.round([xo, yo, zo]).astype(np.int16).T
# 
# def world2depth(x_, rez=[480, 640]):
#     '''
#     Convert world coordinates to depth coordinates using the standard Kinect calibration
#     ---Parameters---
#     x_ : numpy array (Nx3)
#     rez : resolution of depth image
#     '''
#     assert type(x_) == np.ndarray, "world2depth input must be a numpy array"
# 
#     y = np.array(x_[:,1])
#     x = np.array(x_[:,0])
#     ''' Make sure z coordinates are positive and greater than 0'''
#     z = np.array(x_[:,2])
#     z = np.maximum(1, z)
# 
#     xo = (( x / (z * fx_d)) + cx_d)
#     yo = ((y / (z * fy_d)) + cy_d)
# 
#     yo *= rez[1]/640.
#     xo *= rez[0]/480.
# 
#     output = np.round([xo, yo, z]).astype(np.int16).T
#     output = output.clip([0,0,0], [rez[0]-1,rez[1]-1, 99999])
# 
#     return output


import numpy as np
import cv2
import cv2.cv as cv
import time
import networkx as nx
from pyKinectTools.utils.DepthUtils import skel2depth, depth2world, world2depth
from skimage.draw import circle, line
                    from pyKinectTools.utils.SkeletonUtils import display_skeletons
        # Remove zero nodes
        if not ( (skel[c[0],0]==0 and skel[c[0],1]==0) or (skel[c[1],0]==0 and skel[c[1],1]==0)):
        # if not ( (skel[c[0],0]<=0 and skel[c[0],1]<=0) or (skel[c[1],0]<=0 and skel[c[1],1]<=0)):
            if confidence[c[0]] and confidence[c[1]]:
                cv2.line(img, (skel[c[0],1], skel[c[0],0]), (skel[c[1],1], skel[c[1],0]), color, tube_radius)
            else:
                cv2.line(img, (skel[c[0],1], skel[c[0],0]), (skel[c[1],1], skel[c[1],0]), alt_color, tube_radius)

    return img


def plotUsers(image, users):
    # embed()
    # if type(users) == dict:
        # users = [u for u in users.values()]

    # for u in users:
    # uvw = [-1]
    for u in users.keys():
        if users[u]['tracked']:
            xyz = users[u]['com']
            uvw = skel2depth(np.array([xyz]), image.shape)[0]
            ''' Only plot if there are valid coordinates (not [0,0,0])'''
            if uvw[0] > 0:
                if users[u]['tracked'] and len(users[u]['jointPositions'].keys()) > 0:

                    '''Colorize COM'''
                    cv2.rectangle(image, tuple([uvw[0]-3, uvw[1]-3]), tuple([uvw[0]+3, uvw[1]+3]), (4000))
                    '''Plot skeleton'''
                    pts = [j for j in users[u]['jointPositions'].values()]

# 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 depth2world(pts, rez=(480,640)):
#     ''' Convert depth coordinates to world coordinates standard Kinect calibration '''
#     assert type(pts) == np.ndarray, "Wrong type into depth2world"
# 
#     pts = pts.astype(np.float)
#     pts[:,0] *= 480./rez[0]
#     pts[:,1] *= 640./rez[1]
# 
#     y = np.array(pts[:,1])
#     x = np.array(pts[:,0])
#     d = np.array(pts[:,2])
# 
#     xo =  ((x - cx_d) * d * fx_d)
#     yo = ((y - cy_d) * d * fy_d)
#     zo = d
# 
#     return np.round([xo, yo, zo]).astype(np.int16).T
# 
# def world2depth(x_, rez=[480, 640]):
#     '''
#     Convert world coordinates to depth coordinates using the standard Kinect calibration
#     ---Parameters---
#     x_ : numpy array (Nx3)
#     rez : resolution of depth image
#     '''
#     assert type(x_) == np.ndarray, "world2depth input must be a numpy array"
# 
#     y = np.array(x_[:,1])
#     x = np.array(x_[:,0])
#     ''' Make sure z coordinates are positive and greater than 0'''
#     z = np.array(x_[:,2])
#     z = np.maximum(1, z)
# 
#     xo = (( x / (z * fx_d)) + cx_d)
#     yo = ((y / (z * fy_d)) + cy_d)
# 
#     yo *= rez[1]/640.
#     xo *= rez[0]/480.
# 
#     output = np.round([xo, yo, z]).astype(np.int16).T
#     output = output.clip([0,0,0], [rez[0]-1,rez[1]-1, 99999])
# 
#     return output


# Path: pyKinectTools/utils/SkeletonUtils.py
import numpy as np
import cv2
import cv2.cv as cv
import time
import networkx as nx
from pyKinectTools.utils.DepthUtils import skel2depth, depth2world, world2depth
from skimage.draw import circle, line
                    from pyKinectTools.utils.SkeletonUtils import display_skeletons

        # Remove zero nodes
        if not ( (skel[c[0],0]==0 and skel[c[0],1]==0) or (skel[c[1],0]==0 and skel[c[1],1]==0)):
        # if not ( (skel[c[0],0]<=0 and skel[c[0],1]<=0) or (skel[c[1],0]<=0 and skel[c[1],1]<=0)):
            if confidence[c[0]] and confidence[c[1]]:
                cv2.line(img, (skel[c[0],1], skel[c[0],0]), (skel[c[1],1], skel[c[1],0]), color, tube_radius)
            else:
                cv2.line(img, (skel[c[0],1], skel[c[0],0]), (skel[c[1],1], skel[c[1],0]), alt_color, tube_radius)

    return img


def plotUsers(image, users):
    # embed()
    # if type(users) == dict:
        # users = [u for u in users.values()]

    # for u in users:
    # uvw = [-1]
    for u in users.keys():
        if users[u]['tracked']:
            xyz = users[u]['com']
            uvw = skel2depth(np.array([xyz]), image.shape)[0]
            ''' Only plot if there are valid coordinates (not [0,0,0])'''
            if uvw[0] > 0:
                if users[u]['tracked'] and len(users[u]['jointPositions'].keys()) > 0:

                    '''Colorize COM'''
                    cv2.rectangle(image, tuple([uvw[0]-3, uvw[1]-3]), tuple([uvw[0]+3, uvw[1]+3]), (4000))
                    '''Plot skeleton'''
                    pts = [j for j in users[u]['jointPositions'].values()]

                    skel = skel2depth(np.array(pts), image.shape)