'''
This script uses a registration XML to extract the necessary transformation matrices for data processing of OpenKnee Experimental Mechanics.

Inputs:
Registration xml with the appropriate locations of the segmented stls
Text file with list of desired tdms files to analyze

USAGE:
python resampling_plotting.py <Registration.xml> <text file of tdms data to analyze>

Outputs:
Process data saved as .csv file
Animation saved as .mp4 video file
Graphs of raw and processed data saved in sub-directory (named with tdms filename)
    .png and .svg formats

Written By:
Erica Neumann, MS
Computational Biomodeling (CoBi) Core
Department of Biomedical Engineering, Cleveland Clinic
morrile2@ccf.org
'''

# import warnings
# warnings.simplefilter(action='ignore', category=FutureWarning)

import numpy as np
from Random.tdms_contents import parseTDMSfile
from scipy.spatial.distance import cdist
import os
import matplotlib.pyplot as plt
from mayavi import mlab
import xml.etree.ElementTree as ET
from tvtk.tools import visual
from tvtk.api import tvtk
from tvtk.common import configure_input
from traits.api import on_trait_change
# import stl
from stl import mesh
from mayavi.modules.text import Text
import math
from argparse import Namespace
import moviepy.editor as mpy
import Random.tdms_plotting
import pandas as pd
import sys
import time

def simplify_mesh(my_mesh):
	"""Converts data from python mesh object to arrays of nodes and elements that Febio can understand.
		creates an n by 3 matrix nodes_array, with no repeated x,y,z points. n is number of nodes
		and save the index of those points and their connectivity in a matrix elem_array.
		Results save in a Leaflet object"""
	nodes_per_elem = 3  # always 3 when working with stl's
	points = my_mesh.points
	# find a better way to do this?
	#  right now we are changing the first value below, and then appending all others
	nodes_array = np.array([[0.0, 0.0, 0.0]])
	elem_array = np.zeros(shape=(len(points), nodes_per_elem))
	node_index = 0
	for c, p in enumerate(points):
		elem_index = np.zeros(nodes_per_elem)
		for i in range(0, nodes_per_elem * 3, 3):
			# get the vertex
			v = p[i:i + 3]
			# check if vertex already exists in nodes_mat, if yes, get index
			x_difference = nodes_array[:, 0] - v[0]
			y_difference = nodes_array[:, 1] - v[1]
			z_difference = nodes_array[:, 2] - v[2]
			other_node_index = np.where(np.logical_and
										(np.logical_and(x_difference == 0, y_difference == 0), z_difference == 0))
			if len(other_node_index[0]) > 0:
				elem_index[i / 3] = int(other_node_index[0])
				if node_index == 0:  # if it happens that the first point is 0,0,0
					node_index += 1
			# if not, add to nodes_array and get index for elem_array
			elif node_index == 0:
				nodes_array[0] = v
				elem_index[i / 3] = int(node_index)
				node_index += 1
			else:
				nodes_array = np.append(nodes_array, [v], axis=0)
				elem_index[int(i / 3)] = int(node_index)
				node_index += 1
		elem_array[c] = elem_index

	elem_array = elem_array.astype(int)
	return nodes_array

def simplify_mesh_new(my_mesh):
	"""Converts data from python mesh object to arrays of nodes and elements that Febio can understand.
		creates an n by 3 matrix nodes_array, with no repeated x,y,z points. n is number of nodes
		and save the index of those points and their connectivity in a matrix elem_array.
		Results save in a Leaflet object"""
	print ''
	print 'Simplify Mesh'
	print '	old way'
	start = time.time()
	nodes_per_elem = 3  # always 3 when working with stl's
	points = my_mesh.points
	# find a better way to do this?
	#  right now we are changing the first value below, and then appending all others
	nodes_array = np.array([[0.0, 0.0, 0.0]])
	# elem_array = np.zeros(shape=(len(points), nodes_per_elem))
	node_index = 0
	for c, p in enumerate(points):
		# elem_index = np.zeros(nodes_per_elem)
		for i in range(0, nodes_per_elem * 3, 3):
			# get the vertex
			v = p[i:i + 3]
			# print v
			# check if vertex already exists in nodes_mat, if yes, get index
			x_difference = nodes_array[:, 0] - v[0]
			y_difference = nodes_array[:, 1] - v[1]
			z_difference = nodes_array[:, 2] - v[2]
			other_node_index = np.where(np.logical_and	(np.logical_and(x_difference == 0, y_difference == 0), z_difference == 0))
			if len(other_node_index[0]) > 0:
				# print other_node_index[0], type(other_node_index[0])
				# raw_input('...')
				# elem_index[i / 3] = int(other_node_index[0])
				if node_index == 0:  # if it happens that the first point is 0,0,0
					node_index += 1
			# if not, add to nodes_array and get index for elem_array
			elif node_index == 0:
				nodes_array[0] = v
				# elem_index[i / 3] = int(node_index)
				node_index += 1
			else:
				nodes_array = np.append(nodes_array, [v], axis=0)
				# elem_index[int(i / 3)] = int(node_index)
				node_index += 1
		# elem_array[c] = elem_index

	# elem_array = elem_array.astype(int)

	finish = time.time()
	print '	old way took', finish-start
	# return (nodes_array, elem_array)
	return nodes_array
	# print '	new way'
	# start = time.time()
	# newPoints = my_mesh.points.reshape(-1,3)
	# find a better way to do this?
	#  right now we are changing the first value below, and then appending all others
	# nodes_array = np.array([[0.0, 0.0, 0.0]])
	# elem_array = np.zeros(shape=(len(points), nodes_per_elem))
	# node_index = 0
	# for c, p in enumerate(points):
	# for p in newPoints:
	# 	pass
	# 	elem_index = np.zeros(nodes_per_elem)
	# 	for i in range(0, nodes_per_elem * 3, 3):
	# 		# get the vertex
	# 		v = p[i:i + 3]
	# 		# check if vertex already exists in nodes_mat, if yes, get index
	# 		x_difference = nodes_array[:, 0] - v[0]
	# 		y_difference = nodes_array[:, 1] - v[1]
	# 		z_difference = nodes_array[:, 2] - v[2]
	# 		other_node_index = np.where(
	# 			np.logical_and(np.logical_and(x_difference == 0, y_difference == 0), z_difference == 0))
	# 		if len(other_node_index[0]) > 0:
	# 			elem_index[i / 3] = int(other_node_index[0])
	# 			if node_index == 0:  # if it happens that the first point is 0,0,0
	# 				node_index += 1
	# 		# if not, add to nodes_array and get index for elem_array
	# 		elif node_index == 0:
	# 			nodes_array[0] = v
	# 			elem_index[i / 3] = int(node_index)
	# 			node_index += 1
	# 		else:
	# 			nodes_array = np.append(nodes_array, [v], axis=0)
	# 			elem_index[int(i / 3)] = int(node_index)
	# 			node_index += 1
	# 	elem_array[c] = elem_index
	#
	# elem_array = elem_array.astype(int)



	# finish = time.time()
	# print '	new way took', finish - start

def proximity_old(bone, ligament):
	''' Finds nodes which are within a threshold value away from each other between 2 meshes'''
	bone_mesh = mesh.Mesh.from_file(bone)
	bone_nodes = simplify_mesh(bone_mesh)

	lig_mesh = mesh.Mesh.from_file(ligament)
	lig_nodes = simplify_mesh(lig_mesh)
	print lig_nodes.shape
	#
	proximity_nodes = []
	min_data = []
	max_data = []
	for i in range(len(lig_nodes)):
		dist = cdist([lig_nodes[i]], bone_nodes)
		min_data.append(np.min(dist))
		max_data.append(np.max(dist))
	#
	thresh = 0.005 * (np.amax(max_data) - np.amax(min_data))
	#
	for i in range(len(lig_nodes)):
		if min_data[i] < thresh:
			proximity_nodes.append(lig_nodes[i])
	#
	return proximity_nodes

def proximity(bone_nodes, lig_nodes):
	''' Finds nodes which are within a threshold value away from each other between 2 meshes'''
	proximity_nodes = []
	min_data = []
	max_data = []
	begin = time.time()
	for i in lig_nodes:
		dist = cdist([i], bone_nodes)
		min_data.append(np.min(dist))
		max_data.append(np.max(dist))
	end = time.time()
	print 'first loop', end-begin,

	#
	thresh = 0.005 * (np.amax(max_data) - np.amax(min_data))
	#
	begin = time.time()
	for i in range(len(lig_nodes)):
		if min_data[i] < thresh:
			proximity_nodes.append(lig_nodes[i])
	end = time.time()
	print 'second loop', end - begin
	#
	return proximity_nodes


def centroid(center, nodes):
	'''Finds node closest to average of proximity nodes'''
	return nodes[cdist([center], nodes).argmin()]

def insertion_point(lig_nodes, bone1_nodes, bone2_nodes):
	# Calculates proximity between Bone and ligament
	begin = time.time()
	nodes1 = proximity(bone1_nodes, lig_nodes)
	nodes2 = proximity(bone2_nodes, lig_nodes)
	center1 = np.average(nodes1, axis=0)
	center2 = np.average(nodes2, axis=0)
	centroid1 = centroid(center1, nodes1)
	centroid2 = centroid(center2, nodes2)
	x = np.array([centroid1[0], centroid1[1], centroid1[2], 1])
	y = np.array([centroid2[0], centroid2[1], centroid2[2], 1])
	finish = time.time()
	print 'insertion point took', finish-begin
	return x, y

def get_RB_transformation_matrix(q1, q2, q3, q4, q5, q6):
	''' Convert relative positions into transformation matrix'''

	T = np.zeros((4, 4))

	T[0, 0] = math.cos(q5) * math.cos(q6)
	T[0, 1] = -math.sin(q6) * math.cos(q5)
	T[0, 2] = math.sin(q5)

	T[1, 0] = math.cos(q6) * math.sin(q5) * math.sin(q4) + math.sin(q6) * math.cos(q4)
	T[1, 1] = -math.sin(q6) * math.sin(q5) * math.sin(q4) + math.cos(q6) * math.cos(q4)
	T[1, 2] = -math.cos(q5) * math.sin(q4)

	T[2, 0] = -math.cos(q6) * math.sin(q5) * math.cos(q4) + math.sin(q6) * math.sin(q4)
	T[2, 1] = math.sin(q6) * math.sin(q5) * math.cos(q4) + math.cos(q6) * math.sin(q4)
	T[2, 2] = math.cos(q5) * math.cos(q4)

	T[0, 3] = q3 * math.sin(q5) + q1
	T[1, 3] = -q3 * math.sin(q4) * math.cos(q5) + q2 * math.cos(q4)
	T[2, 3] = q3 * math.cos(q4) * math.cos(q5) + q2 * math.sin(q4)
	T[3, 3] = 1

	return T

def convert_left_2_right(T):
	T[0, 1] *= -1
	T[0, 2] *= -1
	T[0, 3] *= -1
	T[1, 0] *= -1
	T[2, 0] *= -1
	return T

def transformSTL_get_actor(v, stl_name, A, prop):
	'''Transformation matrix needs to be in m and rad'''
	stl_file = mesh.Mesh.from_file(stl_name, calculate_normals=True)
	# mymesh = mesh.Mesh.from_file(stl_name)
	A[0][3] = A[0][3] * 1000
	A[1][3] = A[1][3] * 1000
	A[2][3] = A[2][3] * 1000
	# points = mymesh.points
	# xp = points[0][0:3]
	# print 'Original'
	# print xp
	# print '---------------'
	stl_file.transform(A)
	stl_file.save('test_1.stl')
	reader2 = tvtk.STLReader()
	reader2.file_name = 'test_1.stl'
	reader2.update()
	# mymeshtr = mesh.Mesh.from_file('test_1.stl')
	# xptr = mymeshtr.points[0][0:3]
	# print 'With transform function'
	# print xptr
	# print '----'
	mapper2 = tvtk.PolyDataMapper()
	configure_input(mapper2, reader2.output)

	# Add ultrasound probe to Mayavi scene
	actor2 = tvtk.Actor(mapper=mapper2, property=prop)
	return actor2

def insertion_point_anim(p):
	'''Animates insertion points'''
	# Create a first sphere
	# The source generates data points
	sphere = tvtk.SphereSource(center=(p[0], p[1], p[2]), radius=0.5)
	# The mapper converts them into position in, 3D with optionally color (if
	# scalar information is available).
	sphere_mapper = tvtk.PolyDataMapper()
	configure_input(sphere_mapper, sphere.output)
	sphere.update()
	#
	# The Property will give the parameters of the material.
	p = tvtk.Property(opacity=1, color=(1, 0, 0))
	# The actor is the actually object in the scene.
	sphere_actor = tvtk.Actor(mapper=sphere_mapper, property=p)
	return sphere_actor

def point_transf_act(A, B, x):
	"""points transformations and animation process"""
	A_CS = np.dot(x, A)  # Transformed point A (in mm)
	A_CS_act = insertion_point_anim(A_CS)
	B_CS_act = insertion_point_anim(B)
	return A_CS, A_CS_act, B_CS_act

def main(args):
	input_xml = ET.parse(args[-2])
	root = input_xml.getroot()
	#
	knee_dir = root.find("Knee_directory").text
	knee_id = os.path.split(knee_dir)[1]
	#
	femur_stl_name = root.find("Bones").find("Femur").find("file").text
	tibia_stl_name = root.find("Bones").find("Tibia").find("file").text
	patella_stl_name = root.find("Bones").find("Patella").find("file").text
	ACL_stl_name = root.find("Ligaments").find("ACL").find("file").text
	PCL_stl_name = root.find("Ligaments").find("PCL").find("file").text
	MCL_stl_name = root.find("Ligaments").find("MCL").find("file").text
	#
	bone_stls = [os.path.join(knee_dir, 'mri-' + knee_id, femur_stl_name),
				 os.path.join(knee_dir, 'mri-' + knee_id, tibia_stl_name),
				 os.path.join(knee_dir, 'mri-' + knee_id, patella_stl_name)]
	ligament_stls = [os.path.join(knee_dir, 'mri-' + knee_id, ACL_stl_name),
					 os.path.join(knee_dir, 'mri-' + knee_id, PCL_stl_name),
					 os.path.join(knee_dir, 'mri-' + knee_id, MCL_stl_name)]
	#
	knee_side = root.find("Knee_side").text
	#
	R = dict(np.load(args[-2][:-3] + 'npz'))
	#
	import time
	start = time.time()
	bone_nodes = [	simplify_mesh(mesh.Mesh.from_file(bone_stls[0])),
					simplify_mesh(mesh.Mesh.from_file(bone_stls[1])) ]
	ligament_nodes = [simplify_mesh(mesh.Mesh.from_file(ligament_stls[0])),
					  simplify_mesh(mesh.Mesh.from_file(ligament_stls[1])),
					  simplify_mesh(mesh.Mesh.from_file(ligament_stls[2])),]
	end = time.time()
	print 'getting nodes took', end-start
	print 'nodes done'

	# bone1_mesh = mesh.Mesh.from_file(bone1_stl)
	# # bone1_nodes,e = simplify_mesh(bone1_mesh)
	# bone1_nodes = bone1_mesh.points.reshape(-1,3)
	#
	# bone2_mesh = mesh.Mesh.from_file(bone2_stl)
	# # bone2_nodes,e = simplify_mesh(bone2_mesh)
	# bone2_nodes = bone2_mesh.points.reshape(-1, 3)
	#
	# lig_mesh = mesh.Mesh.from_file(lig_stl)
	# lig_nodes,e = simplify_mesh(lig_mesh)



	A_ACL, B_ACL = insertion_point(ligament_nodes[0], bone_nodes[0], bone_nodes[1])
	A_PCL, B_PCL = insertion_point(ligament_nodes[1], bone_nodes[0], bone_nodes[1])
	A_MCL, B_MCL = insertion_point(ligament_nodes[2], bone_nodes[0], bone_nodes[1])
	finish = time.time()
	print 'Total insertion time', finish-start
	#
	if knee_side == "Left":
		R['T_TIBOS_TIB'] = convert_left_2_right(R["T_TIBOS_TIB"])
		R["T_FEMOS_FEMORI"] = convert_left_2_right(R["T_FEMOS_FEMORI"])
		R["T_PATOS_PAT"] = convert_left_2_right(R["T_PATOS_PAT"])
	#
	f = open(args[-1])
	tdms_list = f.read().splitlines()
	#
	length_ACL = []
	length_PCL = []
	length_MCL = []
	#
	for tdms_name in tdms_list:
		print '---------------'
		print tdms_name
		tdms_file = os.path.join(os.path.dirname(args[-1]), tdms_name)
		data = parseTDMSfile(tdms_file)
		if knee_side == "Left":
			M = -np.array(data[u'State.Knee JCS'][u'Knee JCS Medial'])    / 1000 - R["Offset_FEMORI_TIB"][0]
			P =  np.array(data[u'State.Knee JCS'][u'Knee JCS Posterior']) / 1000 + R["Offset_FEMORI_TIB"][1]
			S =  np.array(data[u'State.Knee JCS'][u'Knee JCS Superior'])  / 1000 + R["Offset_FEMORI_TIB"][2]
			F =  np.radians(np.array(data[u'State.Knee JCS'][u'Knee JCS Flexion']))                + R["Offset_FEMORI_TIB"][3]
			V =  np.radians(-1 * np.array(data[u'State.Knee JCS'][u'Knee JCS Valgus']))            - R["Offset_FEMORI_TIB"][4]
			IR = np.radians(-1 * np.array(data[u'State.Knee JCS'][u'Knee JCS Internal Rotation'])) - R["Offset_FEMORI_TIB"][5]
			try:
				M_pat = -np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Medial'])    / 1000 - R["Offset_FEMORI_PAT"][0]
				P_pat =  np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Posterior']) / 1000 + R["Offset_FEMORI_PAT"][1]
				S_pat =  np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Superior'])  / 1000 + R["Offset_FEMORI_PAT"][2]
				F_pat =  np.radians( np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Flexion']))           + R["Offset_FEMORI_PAT"][3]
				V_pat =  np.radians(-np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Valgus']))            - R["Offset_FEMORI_PAT"][4]
				IR_pat = np.radians(-np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Internal Rotation'])) - R["Offset_FEMORI_PAT"][5]
				patella = True
			except:		patella = False
		else:
			M = np.array(data[u'State.Knee JCS'][u'Knee JCS Medial'])    / 1000 + R["Offset_FEMORI_TIB"][0]
			P = np.array(data[u'State.Knee JCS'][u'Knee JCS Posterior']) / 1000 + R["Offset_FEMORI_TIB"][1]
			S = np.array(data[u'State.Knee JCS'][u'Knee JCS Superior'])  / 1000 + R["Offset_FEMORI_TIB"][2]
			F =  np.radians(np.array(data[u'State.Knee JCS'][u'Knee JCS Flexion']))           + R["Offset_FEMORI_TIB"][3]
			V =  np.radians(np.array(data[u'State.Knee JCS'][u'Knee JCS Valgus']))            + R["Offset_FEMORI_TIB"][4]
			IR = np.radians(np.array(data[u'State.Knee JCS'][u'Knee JCS Internal Rotation'])) + R["Offset_FEMORI_TIB"][5]
			try:
				M_pat = np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Medial'])    / 1000 + R["Offset_FEMORI_PAT"][0]
				P_pat = np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Posterior']) / 1000 + R["Offset_FEMORI_PAT"][1]
				S_pat = np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Superior'])  / 1000 + R["Offset_FEMORI_PAT"][2]
				F_pat =  np.radians(np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Flexion']))           + R["Offset_FEMORI_PAT"][3]
				V_pat =  np.radians(np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Valgus']))            + R["Offset_FEMORI_PAT"][4]
				IR_pat = np.radians(np.array(data[u'State.Knee PTFJ'][u'Knee PTFJ Internal Rotation'])) + R["Offset_FEMORI_PAT"][5]
				patella = True
			except:		patella = False
		v = mlab.figure()
		if patella:			T_I_PAT = np.dot(R["T_I_PATOS"], R["T_PATOS_PAT"])
		T_I_TIB = np.dot(R["T_I_TIBOS"], R["T_TIBOS_TIB"])
		#
		idx = Random.tdms_plotting.tdms_contents([[tdms_file]])[0]
		df = pd.read_csv(tdms_file[:-4] + 'csv')
		time = df['Extracted Time Points [s]']
		#
		positions_TF = np.empty([len(IR[idx]), 4, 4])
		positions_PF = np.empty([len(IR[idx]), 4, 4])
		#
		actors = []
		p_act_A_ACL = []
		p_act_B_ACL = []
		p_act_A_PCL = []
		p_act_B_PCL = []
		p_act_A_MCL = []
		p_act_B_MCL = []
		#
		act = transformSTL_get_actor(v, bone_stls[1], np.identity(4), tvtk.Property(opacity=1, color=(1, 1, 1)))
		v.scene.add_actor(act)

		for ii in range(len(M[idx])):
			# Transform between FEM CS and TIB CS Relative angles/positions b/w bones
			T_FEMORI_TIB = get_RB_transformation_matrix(M[idx][ii], P[idx][ii], S[idx][ii], F[idx][ii], V[idx][ii],	IR[idx][ii])
			#
			# Define the positions of the femur in the image CS
			positions_TF[ii] = np.dot(T_I_TIB, np.linalg.inv(np.dot(np.dot(R["T_I_FEMOS"], R["T_FEMOS_FEMORI"]), T_FEMORI_TIB)))
			#
			# IMPORTANT!!!!
			# This function converts the positions_TF transformation matrix into millimeters instead of meters.
			# Therefore this line must run prior to taking the dot product for the point transformation since the points are given in mm!
			act = transformSTL_get_actor(v, bone_stls[0], positions_TF[ii], tvtk.Property(opacity=1, color=(1, 1, 1)))
			#
			# ACL points transformations and animation process
			A_CS_ACL, A_CS_act_ACL, B_CS_act_ACL = point_transf_act(A_ACL, B_ACL, positions_TF[ii])
			p_act_A_ACL.append([A_CS_act_ACL])
			p_act_B_ACL.append([B_CS_act_ACL])
			length_ACL.append(np.sqrt(np.sum((A_CS_ACL - B_ACL) ** 2)))
			#
			# PCL points transformations and animation process
			A_CS_PCL, A_CS_act_PCL, B_CS_act_PCL = point_transf_act(A_PCL, B_PCL, positions_TF[ii])
			p_act_A_PCL.append([A_CS_act_PCL])
			p_act_B_PCL.append([B_CS_act_PCL])
			length_PCL.append(np.sqrt(np.sum((A_CS_PCL - B_PCL) ** 2)))
			#
			# ACL points transformations and animation process
			A_CS_MCL, A_CS_act_MCL, B_CS_act_MCL = point_transf_act(A_MCL, B_MCL, positions_TF[ii])
			p_act_A_MCL.append([A_CS_act_MCL])
			p_act_B_MCL.append([B_CS_act_MCL])
			length_MCL.append(np.sqrt(np.sum((A_CS_MCL - B_MCL) ** 2)))

			if patella:
				# Transform between FEM CS and PAT CS Relative angles/positions_TF b/w bones
				T_FEMORI_PAT = get_RB_transformation_matrix(M_pat[ii], P_pat[ii], S_pat[ii], F_pat[ii], V_pat[ii], IR_pat[ii])
				#
				# Define the positions_TF of the femur in the image CS
				positions_PF[ii] = np.dot(np.dot(T_I_TIB, np.dot(np.linalg.inv(T_FEMORI_TIB), T_FEMORI_PAT)), np.linalg.inv(T_I_PAT))
				act_Pat = transformSTL_get_actor(v, bone_stls[2], positions_PF[ii], tvtk.Property(opacity=0.5, color=(1, 1, 1)))
				actors.append([act, act_Pat])
			else:	actors.append([act])
		#
		@mlab.show
		@mlab.animate(delay=100)
		@on_trait_change('scene.activated')
		def anim():
			"""Animate the b1 box."""
			t = 0
			engine = mlab.get_engine()
			scene = engine.current_scene
			timetext = Text()
			timetext.text = 'Time = %f sec' % (float(time[t]))
			v.scene.add_actor(timetext.actor)
			# timetext = mlab.text(0.7, 0.01, 'Time = %f sec' % (float(time[t]) / 1000.0), figure = scene)
			while 1:
				if 'actor_old' in globals():
					v.scene.remove_actor(actor_old[0])
					if patella:		v.scene.remove_actor(actor_old[1])
				if 'p_act_A_ACL_old' in globals():	v.scene.remove_actor(p_act_A_ACL_old[0])
				if 'p_act_B_ACL_old' in globals():	v.scene.remove_actor(p_act_B_ACL_old[0])
				if 'p_act_A_PCL_old' in globals():	v.scene.remove_actor(p_act_A_PCL_old[0])
				if 'p_act_B_PCL_old' in globals():	v.scene.remove_actor(p_act_B_PCL_old[0])
				if 'p_act_A_MCL_old' in globals():	v.scene.remove_actor(p_act_A_MCL_old[0])
				if 'p_act_B_MCL_old' in globals():	v.scene.remove_actor(p_act_B_MCL_old[0])

				for act_i   in actors[t]:			v.scene.add_actor(act_i)
				for pacta_i in p_act_A_ACL[t]:		v.scene.add_actor(pacta_i)
				for pactb_i in p_act_B_ACL[t]:		v.scene.add_actor(pactb_i)
				for pacta_i in p_act_A_PCL[t]:		v.scene.add_actor(pacta_i)
				for pactb_i in p_act_B_PCL[t]:		v.scene.add_actor(pactb_i)
				for pacta_i in p_act_A_MCL[t]:		v.scene.add_actor(pacta_i)
				for pactb_i in p_act_B_MCL[t]:		v.scene.add_actor(pactb_i)

				global actor_old
				global p_act_A_ACL_old
				global p_act_B_ACL_old
				global p_act_A_PCL_old
				global p_act_B_PCL_old
				global p_act_A_MCL_old
				global p_act_B_MCL_old

				actor_old = [actors[t], timetext.actor]
				p_act_A_ACL_old = [p_act_A_ACL[t], timetext.actor]
				p_act_B_ACL_old = [p_act_B_ACL[t], timetext.actor]
				p_act_A_PCL_old = [p_act_A_PCL[t], timetext.actor]
				p_act_B_PCL_old = [p_act_B_PCL[t], timetext.actor]
				p_act_A_MCL_old = [p_act_A_MCL[t], timetext.actor]
				p_act_B_MCL_old = [p_act_B_MCL[t], timetext.actor]
				timetext.text = 'Time = %f sec' % (float(time[t]))
				t = (t + 1) % len(time)
				yield
		anim()
		#
		del globals()['actor_old']
		del globals()['p_act_A_ACL_old']
		del globals()['p_act_B_ACL_old']
		del globals()['p_act_A_PCL_old']
		del globals()['p_act_B_PCL_old']
		del globals()['p_act_A_MCL_old']
		del globals()['p_act_B_MCL_old']
		# del globals()['view']
		# except:
		#     print "Error with tdms file: "
		#     print tdms_file
	length_ACL = np.array(length_ACL)
	plt.plot(time, length_ACL)
	hold('on')
	plt.plot(time, length_PCL)
	plt.ylabel("Ligament length [mm]")
	plt.xlabel("time [s]")
	plt.autoscale(enable=True)
	plt.grid(True)
	plt.savefig('ACL_length.png')


if __name__ == "__main__":
	# main(sys.argv)
	main(['callfunction', '/home/landisb/PycharmProjects/PythonScripts/Random/oks003_registration_01.xml',
		  '/home/landisb/PycharmProjects/PythonScripts/Random/oks003/joint_mechanics-oks003/TibiofemoralJoint/KinematicsKinetics/oks003_TF.txt'])