'''
This script uses a registration XML to read bone and ligament STLs to plot ligament lengths and animate their insertion points from Open Knee Experimental Mechanics.

Inputs:
Registration xml with the appropriate locations of the segmented stls

USAGE:
python cartilage_analysis.py <Registration.xml> <text file of csv data to analyze>

Outputs:
Plots of ligaments lengths saved as .png

Written By:
Rodrigo Lopez-Navarro with Erica Neumann and Ariel Schwartz
Department of Biomedical Engineering, Cleveland Clinic
'''

import numpy as np
import os
import matplotlib.pyplot as plt
import xml.etree.ElementTree as ET
from tvtk.api import tvtk
import stl
import math
import pandas as pd
import sys
import csv
import vtk
import ConfigParser

def insertion_point(fem_stl, cart_stl, TBB_stl, c, app2):
    reader1 = vtk.vtkSTLReader()
    reader1.SetFileName(fem_stl)
    reader1.Update()

    reader2 = vtk.vtkSTLReader()
    reader2.SetFileName(cart_stl)
    reader2.Update()
    cart_data = reader2.GetOutput()

    implicitPolyDataDistance = vtk.vtkImplicitPolyDataDistance()
    fem_data = reader1.GetOutput()
    implicitPolyDataDistance.SetInput(fem_data)

    points = vtk.vtkPoints()
    a = np.zeros(3)
    for i in range(cart_data.GetNumberOfPoints()):
        cart_data.GetPoint(i, a)
        points.InsertNextPoint(a[0], a[1], a[2])

    # Add distances to each point
    signedDistances = vtk.vtkFloatArray()
    signedDistances.SetNumberOfComponents(1)
    signedDistances.SetName("SignedDistances")
    dist = np.zeros(points.GetNumberOfPoints())
    # Evaluate the signed distance function at all of the grid points
    for pointId in range(points.GetNumberOfPoints()):
        p = points.GetPoint(pointId)
        signedDistance = implicitPolyDataDistance.EvaluateFunction(p)
        signedDistances.InsertNextValue(signedDistance)
        dist[pointId] = signedDistance

    cart_data.GetPointData().SetScalars(signedDistances)

    #Calculte contact Area
    # CA1 = contact_area1(tib_data, c,app1)
    CA, centerArea, idxs = contact_area2(cart_data, c, app2)
    strain = strain_calc(TBB_stl, points,dist, idxs)
    np.asarray(dist)
    arr = []
    dp = []
    for i in range(0, 10, 1):
        idx = np.argmin(dist)
        arr.append(points.GetPoint(idx))
        dp.append(dist[idx])
        np.delete(dist, idx)
    np.asarray(arr)
    c = np.average(arr, axis=0)
    np.asarray(dp)
    dpp = np.average(dp, axis=0)
    cellId = vtk.reference(0)
    subId = vtk.reference(0)
    d = vtk.reference(0.0)
    cellLocator = vtk.vtkCellLocator()
    cellLocator.SetDataSet(fem_data)
    cellLocator.BuildLocator()
    mxfp = [0, 0, 0]
    cellId = vtk.reference(0)
    subId = vtk.reference(0)
    d = vtk.reference(0.0)
    cellLocator.FindClosestPoint(c, mxfp, cellId, subId, d)

    forcepoint = [mxfp[0], mxfp[1], mxfp[2], 1]
    return dpp, forcepoint, CA, centerArea, strain

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(stl_name, A):
    '''Transformation matrix needs to be in m and rad'''
    stl_file = stl.Mesh.from_file(stl_name, calculate_normals=True)
    A[0][3] = A[0][3] * 1000
    A[1][3] = A[1][3] * 1000
    A[2][3] = A[2][3] * 1000

    stl_file.transform(A)
    stl_file.save('test_1.stl')

    reader2 = tvtk.STLReader()
    reader2.file_name = 'test_1.stl'
    reader2.update()

def transformSTL_pat(stl_name, A):
    '''Transformation matrix needs to be in m and rad'''
    stl_file = stl.Mesh.from_file(stl_name, calculate_normals=True)
    A[0][3] = A[0][3] * 1000
    A[1][3] = A[1][3] * 1000
    A[2][3] = A[2][3] * 1000

    stl_file.transform(A)
    stl_file.save('test_pat.stl')

    reader2 = tvtk.STLReader()
    reader2.file_name = 'test_pat.stl'
    reader2.update()

def convertTOarray(data, r):

    data = data.replace("", '')
    data = data.split(" ")
    corrected_list = data[r:len(data)]
    corrected_list[-1] = corrected_list[-1][0:-1]
    data_float = map(float, corrected_list)

    return np.asarray(data_float)

def sd_screenshot(FMBC, TBC_L_data, TBC_M_data, pct, data_folder):

    reader3 = vtk.vtkSTLReader()
    reader3.SetFileName(FMBC)
    reader3.Update()

    implicitPolyDataDistance = vtk.vtkImplicitPolyDataDistance()
    implicitPolyDataDistance.SetInput(reader3.GetOutput())

    # Setup a grid TBC_M
    TBC_M_points = vtk.vtkPoints()
    a = np.zeros(3)
    for i in range(TBC_M_data.GetNumberOfPoints()):
        TBC_M_data.GetPoint(i, a)
        TBC_M_points.InsertNextPoint(a[0], a[1], a[2])

    # Add distances to each point
    TBC_M_signedDistances = vtk.vtkFloatArray()
    TBC_M_signedDistances.SetNumberOfComponents(1)
    TBC_M_signedDistances.SetName("TBC_M_SignedDistances")
    TBC_M_dist = []
    # Evaluate the signed distance function at all of the grid points
    for pointId in range(TBC_M_points.GetNumberOfPoints()):
        p = TBC_M_points.GetPoint(pointId)
        TBC_M_signedDistance = implicitPolyDataDistance.EvaluateFunction(p)
        TBC_M_signedDistances.InsertNextValue(TBC_M_signedDistance)
        TBC_M_dist.append(TBC_M_signedDistance)

    # Setup a grid TBC_L
    TBC_L_points = vtk.vtkPoints()
    b = np.zeros(3)
    for i in range(TBC_L_data.GetNumberOfPoints()):
        TBC_L_data.GetPoint(i, b)
        TBC_L_points.InsertNextPoint(b[0], b[1], b[2])

    # Add distances to each point
    TBC_L_signedDistances = vtk.vtkFloatArray()
    TBC_L_signedDistances.SetNumberOfComponents(1)
    TBC_L_signedDistances.SetName("TBC_L_SignedDistances")
    TBC_L_dist = []
    # Evaluate the signed distance function at all of the grid points
    for pointId in range(TBC_L_points.GetNumberOfPoints()):
        p = TBC_L_points.GetPoint(pointId)
        TBC_L_signedDistance = implicitPolyDataDistance.EvaluateFunction(p)
        TBC_L_signedDistances.InsertNextValue(TBC_L_signedDistance)
        TBC_L_dist.append(TBC_L_signedDistance)

    TBC_L_data.GetPointData().SetScalars(TBC_L_signedDistances)
    TBC_M_data.GetPointData().SetScalars(TBC_M_signedDistances)

    #Determining color map scale
    mindL = np.min(TBC_L_dist)
    maxdL = np.max(TBC_L_dist)
    mindM = np.min(TBC_M_dist)
    maxdM = np.max(TBC_M_dist)
    rangdM = maxdM - mindM
    rangdL = maxdL - mindL
    if mindL < mindM:
        mind = mindL
        maxd = maxdL
    else:
        mind = mindM
        maxd = maxdM
    rangd = maxd - mind
    # transfer function (lookup table) for mapping point scalar data
    # to colors (parent class is vtkScalarsToColors)
    lut = vtk.vtkColorTransferFunction()
    lut.AddRGBPoint(0, 0.0, 0.0, 1.0)
    lut.AddRGBPoint(-1, 0.0, 1.0, 1.0)
    lut.AddRGBPoint(-2, 0.0, 1.0, 0.0)
    lut.AddRGBPoint(-3, 1.0, 1.0, 0.0)
    lut.AddRGBPoint(-4, 1.0, 0.0, 0.0)

    # the mapper that will use the lookup table
    mapperL = vtk.vtkPolyDataMapper()
    mapperL.SetLookupTable(lut)
    mapperL.SetScalarRange(mind, maxd)
    mapperL.SetInputData(TBC_L_data)
    mapperL.SetScalarModeToUsePointData()

    mapperM = vtk.vtkPolyDataMapper()
    mapperM.SetLookupTable(lut)
    mapperM.SetScalarRange(mind, maxd)
    mapperM.SetInputData(TBC_M_data)
    mapperM.SetScalarModeToUsePointData()

    myActorL = vtk.vtkActor()
    myActorL.SetMapper(mapperL)

    myActorM = vtk.vtkActor()
    myActorM.SetMapper(mapperM)

    # a colorbar to display the colormap
    scalarBar = vtk.vtkScalarBarActor()
    scalarBar.SetLookupTable(mapperL.GetLookupTable())
    scalarBar.SetTitle("Point scalar value [mm]")
    scalarBar.SetOrientationToHorizontal()
    scalarBar.GetLabelTextProperty().SetColor(0, 0, 1)
    scalarBar.GetTitleTextProperty().SetColor(0, 0, 1)

    # position it in window
    coord = scalarBar.GetPositionCoordinate()
    coord.SetCoordinateSystemToNormalizedViewport()
    coord.SetValue(0.1, 0.05)
    scalarBar.SetWidth(.8)
    scalarBar.SetHeight(.10)

    renderer = vtk.vtkRenderer()
    renderer.AddActor(myActorM)
    renderer.AddActor(myActorL)
    renderer.AddActor(scalarBar)

    renderWindow = vtk.vtkRenderWindow()
    renderWindow.AddRenderer(renderer)
    renderWindow.Render()

    windowtoimage = vtk.vtkWindowToImageFilter()
    windowtoimage.SetInput(renderWindow)
    windowtoimage.Update()

    pct *= 10
    pct = str(pct)
    writer = vtk.vtkPNGWriter()
    writer.SetFileName(data_folder + '/' + 'TBCML_' + pct + '.png')
    writer.SetInputConnection(windowtoimage.GetOutputPort())
    writer.Write()

def sd_screenshot_pat(FMBC, cart, pct, data_folder):
    reader = vtk.vtkSTLReader()
    reader.SetFileName(cart)
    reader.Update()
    cart_data = reader.GetOutput()

    reader3 = vtk.vtkSTLReader()
    reader3.SetFileName(FMBC)
    reader3.Update()

    implicitPolyDataDistance = vtk.vtkImplicitPolyDataDistance()
    implicitPolyDataDistance.SetInput(reader3.GetOutput())

    # Setup a grid TBC_L
    cart_points = vtk.vtkPoints()
    b = np.zeros(3)
    for i in range(cart_data.GetNumberOfPoints()):
        cart_data.GetPoint(i, b)
        cart_points.InsertNextPoint(b[0], b[1], b[2])

    # Add distances to each point
    PAT_signedDistances = vtk.vtkFloatArray()
    PAT_signedDistances.SetNumberOfComponents(1)
    PAT_signedDistances.SetName("PAT_SignedDistances")
    PAT_dist = []
    # Evaluate the signed distance function at all of the grid points
    for pointId in range(cart_points.GetNumberOfPoints()):
        p = cart_points.GetPoint(pointId)
        PAT_signedDistance = implicitPolyDataDistance.EvaluateFunction(p)
        PAT_signedDistances.InsertNextValue(PAT_signedDistance)
        PAT_dist.append(PAT_signedDistance)

    cart_data.GetPointData().SetScalars(PAT_signedDistances)

    #Determining color map scale
    mind = np.min(PAT_dist)
    maxd = np.max(PAT_dist)

    # transfer function (lookup table) for mapping point scalar data
    # to colors (parent class is vtkScalarsToColors)
    lut = vtk.vtkColorTransferFunction()
    lut.AddRGBPoint(0, 0.0, 0.0, 1.0)
    lut.AddRGBPoint(-1, 0.0, 1.0, 1.0)
    lut.AddRGBPoint(-2, 0.0, 1.0, 0.0)
    lut.AddRGBPoint(-3, 1.0, 1.0, 0.0)
    lut.AddRGBPoint(-4, 1.0, 0.0, 0.0)

    # the mapper that will use the lookup table
    mapper = vtk.vtkPolyDataMapper()
    mapper.SetLookupTable(lut)
    mapper.SetScalarRange(mind, maxd)
    mapper.SetInputData(cart_data)
    mapper.SetScalarModeToUsePointData()

    myActor = vtk.vtkActor()
    myActor.SetMapper(mapper)

    # a colorbar to display the colormap
    scalarBar = vtk.vtkScalarBarActor()
    scalarBar.SetLookupTable(mapper.GetLookupTable())
    scalarBar.SetTitle("Point scalar value [mm]")
    scalarBar.SetOrientationToHorizontal()
    scalarBar.GetLabelTextProperty().SetColor(0, 0, 1)
    scalarBar.GetTitleTextProperty().SetColor(0, 0, 1)

    # position it in window
    coord = scalarBar.GetPositionCoordinate()
    coord.SetCoordinateSystemToNormalizedViewport()
    coord.SetValue(0.1, 0.05)
    scalarBar.SetWidth(.8)
    scalarBar.SetHeight(.10)

    renderer = vtk.vtkRenderer()
    renderer.AddActor(myActor)
    renderer.AddActor(scalarBar)

    renderWindow = vtk.vtkRenderWindow()
    renderWindow.AddRenderer(renderer)
    renderWindow.Render()

    windowtoimage = vtk.vtkWindowToImageFilter()
    windowtoimage.SetInput(renderWindow)
    windowtoimage.Update()

    pct *= 10
    pct = str(pct)
    writer = vtk.vtkPNGWriter()
    writer.SetFileName(data_folder + '/' + 'PAT_' + pct + '.png')
    writer.SetInputConnection(windowtoimage.GetOutputPort())
    writer.Write()

def center_act(center):
    cornerSource = vtk.vtkSphereSource()
    cornerSource.SetCenter(center[0], center[1], center[2])
    cornerSource.SetRadius(0.5)
    cornerSource.Update()
    cornerMapper = vtk.vtkPolyDataMapper()
    cornerMapper.SetInputConnection(cornerSource.GetOutputPort())
    cornerMapper.ScalarVisibilityOff()

    cornerActor = vtk.vtkActor()
    cornerActor.SetMapper(cornerMapper)
    cornerActor.GetProperty().SetOpacity(1)
    cornerActor.GetProperty().SetColor(1, 0, 0)

    return cornerActor

def contact_area1(polydata,c, app):
    threshold = vtk.vtkThresholdPoints()
    threshold.SetInputData(polydata)
    threshold.ThresholdByLower(c)
    threshold.Update()
    thresh = threshold.GetOutput()

    points = vtk.vtkPoints()
    d = np.zeros(3)
    for i in range(thresh.GetNumberOfPoints()):
        thresh.GetPoint(i, d)
        points.InsertNextPoint(d[0], d[1], d[2])

    polyData = vtk.vtkPolyData()
    polyData.SetPoints(points)

    CA = app*polyData.GetNumberOfPoints()

    return CA

def contact_area2(polydata,c,apc):
    threshold = vtk.vtkThresholdPoints()
    threshold.SetInputData(polydata)
    threshold.ThresholdByLower(c)
    threshold.Update()
    thresh = threshold.GetOutput()

    points = vtk.vtkPoints()
    d = np.zeros(3)
    THRESH_POINTS = np.zeros([thresh.GetNumberOfPoints(), 3])
    for i in range(thresh.GetNumberOfPoints()):
        thresh.GetPoint(i, d)
        points.InsertNextPoint(d[0], d[1], d[2])
        THRESH_POINTS[i] = d
    centerArea = np.average(THRESH_POINTS, axis=0)
    polyData = vtk.vtkPolyData()
    polyData.SetPoints(points)
    point_weight = np.zeros(polydata.GetNumberOfPoints())

    for i in range(polydata.GetNumberOfPoints()):
        cellIdList = vtk.vtkIdList()
        polydata.GetPointCells(i, cellIdList)
        point_weight[i] = cellIdList.GetNumberOfIds()

    thresh_area = 0
    idxs = []
    tp = np.zeros(3)
    for i in range(polyData.GetNumberOfPoints()):
        polyData.GetPoint(i, tp)
        index = polydata.FindPoint(tp[0], tp[1], tp[2])
        idxs.append(index)
        thresh_area += ((0.33 * apc * point_weight[index]))

    total_area = 0.33*apc*(np.sum(point_weight))
    area = thresh_area/total_area
    return area, centerArea, idxs

def printing_centers(actorsM, actorsL, reader1, reader2,data_folder):
    ''' Renders the center of contact area as a point, and saves figure as .png'''
    reader1mapper = vtk.vtkPolyDataMapper()
    reader1mapper.SetInputConnection(reader1.GetOutputPort())
    reader1mapper.ScalarVisibilityOff()
    reader1act = vtk.vtkActor()
    reader1act.SetMapper(reader1mapper)

    reader2mapper = vtk.vtkPolyDataMapper()
    reader2mapper.SetInputConnection(reader2.GetOutputPort())
    reader2mapper.ScalarVisibilityOff()
    reader2act = vtk.vtkActor()
    reader2act.SetMapper(reader2mapper)

    renderer = vtk.vtkRenderer()
    renderer.AddActor(reader2act)
    renderer.AddActor(reader1act)
    renderer.AddActor(actorsL[0])
    renderer.AddActor(actorsL[1])
    renderer.AddActor(actorsL[2])
    renderer.AddActor(actorsL[3])
    renderer.AddActor(actorsL[4])
    renderer.AddActor(actorsL[5])
    renderer.AddActor(actorsL[6])
    renderer.AddActor(actorsL[7])
    renderer.AddActor(actorsL[8])
    renderer.AddActor(actorsL[9])
    renderer.AddActor(actorsM[0])
    renderer.AddActor(actorsM[1])
    renderer.AddActor(actorsM[2])
    renderer.AddActor(actorsM[3])
    renderer.AddActor(actorsM[4])
    renderer.AddActor(actorsM[5])
    renderer.AddActor(actorsM[6])
    renderer.AddActor(actorsM[7])
    renderer.AddActor(actorsM[8])
    renderer.AddActor(actorsM[9])

    renderWindow = vtk.vtkRenderWindow()
    renderWindow.AddRenderer(renderer)
    renderWindow.Render()

    # Save Image as .png
    windowtoimage = vtk.vtkWindowToImageFilter()
    windowtoimage.SetInput(renderWindow)
    windowtoimage.Update()

    writer = vtk.vtkPNGWriter()
    writer.SetFileName(data_folder + '/' + 'Areacenters.png')
    writer.SetInputConnection(windowtoimage.GetOutputPort())
    writer.Write()

def strain_calc(bone_stl, points, pdist, idxs):
    reader1 = vtk.vtkSTLReader()
    reader1.SetFileName(bone_stl)
    reader1.Update()
    bone_data = reader1.GetOutput()

    implicitPolyDataDistance = vtk.vtkImplicitPolyDataDistance()
    implicitPolyDataDistance.SetInput(bone_data)

    # Add distances to each point
    signedDistances = vtk.vtkFloatArray()
    signedDistances.SetNumberOfComponents(1)
    signedDistances.SetName("SignedDistances")

    dist = np.zeros(points.GetNumberOfPoints())
    # Evaluate the signed distance function at all of the grid points
    for pointId in range(points.GetNumberOfPoints()):
        p = points.GetPoint(pointId)
        signedDistance = implicitPolyDataDistance.EvaluateFunction(p)
        signedDistances.InsertNextValue(signedDistance)
        dist[pointId] = signedDistance
    pen = np.zeros(np.size(idxs))
    depth = np.zeros(np.size(idxs))
    j=0
    for i in idxs:
        depth[j] = dist[i]
        pen[j] = pdist[i]
        j += 1
    strains = np.divide(np.divide(pen,2),depth)
    max_strain = np.max(np.abs(strains))
    return max_strain

def main(args):
    # Extract STLs from xml
    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]
    test_prot = root.find("Test_protocol").text
    cartilage_stls = ['fem_joint.stl', 'tib_L_cart_split.stl', 'tib_M_cart_split.stl', 'pat_cart_split.stl']
    femc_stl_name = root.find("Cartilages").find("Fem-cart").find("file").text
    tibl_stl_name = root.find("Cartilages").find("Tibia-L").find("file").text
    tibm_stl_name = root.find("Cartilages").find("Tibia-M").find("file").text
    pat_stl_name = root.find("Cartilages").find("Pat-cart").find("file").text

    femur_stl_name = root.find("Bones").find("Femur").find("file").text
    tibia_stl_name = root.find("Bones").find("Tibia").find("file").text
    fibula_stl_name = root.find("Bones").find("Fibula").find("file").text
    patella_stl_name = root.find("Bones").find("Patella").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), os.path.join(knee_dir, 'mri-'+knee_id, fibula_stl_name)]
    cart_full_stls = [os.path.join(knee_dir, 'mri-'+knee_id, femc_stl_name),os.path.join(knee_dir, 'mri-'+knee_id, tibl_stl_name),os.path.join(knee_dir, 'mri-'+knee_id, tibm_stl_name),os.path.join(knee_dir, 'mri-'+knee_id, pat_stl_name)]
    knee_side = root.find("Knee_side").text

    merged_stl = knee_id + '_MRC_FMBC_01.stl'
    print merged_stl

    R = dict(np.load(args[-2][:-3]+'npz'))

    #Create directory to save data to
    data_folder = os.path.join(os.path.dirname(args[-1]), test_prot, 'Data', "Cartilage Analysis")
    if not os.path.exists(data_folder):
        os.makedirs(data_folder)

    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])
    csv_name = f.read().replace('\n', '')
    csv_file = os.path.join(os.path.dirname(args[-1]), csv_name)
    f.close()

    df = pd.read_csv(csv_file)
    M = df['Knee JCS Medial  [mm]']
    P = df['Knee JCS Posterior  [mm]']
    S = df['Knee JCS Superior  [mm]']
    F = df['Knee JCS Flexion  [deg]']
    V = df['Knee JCS Valgus  [deg]']
    IR = df['Knee JCS Internal Rotation  [deg]']
    time = df['Extracted Time Points [s]']

    config = ConfigParser.RawConfigParser()
    if not config.read(
            os.path.join(os.path.dirname(args[-1]), csv_name.split('/')[0], 'Configuration', 'State.cfg')):
        raise IOError, "Cannot load configuration file... Check path."
    Offset_FEMORI_TIB = convertTOarray(config.get('Knee JCS', 'Position Offset (m,rad)'), 1)

    if knee_side == "Left":
        M = -1 * np.array(M) / 1000 - Offset_FEMORI_TIB[0]
        P = np.array(P) / 1000 + Offset_FEMORI_TIB[1]
        S = np.array(S) / 1000 + Offset_FEMORI_TIB[2]
        F = np.radians(np.array(F)) + Offset_FEMORI_TIB[3]
        V = np.radians(-1 * np.array(V)) - Offset_FEMORI_TIB[4]
        IR = np.radians(-1 * np.array(IR)) - Offset_FEMORI_TIB[5]

    else:
        M = np.array(M) / 1000 - Offset_FEMORI_TIB[0]
        P = np.array(P) / 1000 + Offset_FEMORI_TIB[1]
        S = np.array(S) / 1000 + Offset_FEMORI_TIB[2]
        F = np.radians(np.array(F)) + Offset_FEMORI_TIB[3]
        V = np.radians(np.array(V)) - Offset_FEMORI_TIB[4]
        IR = np.radians(np.array(IR)) - Offset_FEMORI_TIB[5]

    # csvFile.close()
    T_I_TIB = np.dot(R["T_I_TIBOS"], R["T_TIBOS_TIB"])

    # if os.path.basename(args[-1]) == 'oks001_TF_csv.txt':

    positions_TF = np.empty([np.size(IR), 4, 4])

    reader1 = vtk.vtkSTLReader()
    reader1.SetFileName(cart_full_stls[1])
    reader1.Update()
    TBC_L_data = reader1.GetOutput()

    reader2 = vtk.vtkSTLReader()
    reader2.SetFileName(cart_full_stls[2])
    reader2.Update()
    TBC_M_data = reader2.GetOutput()

    # Calculate Area of Tibial Cartilages and Area/Cell
    areasM = []
    for i in range(TBC_M_data.GetNumberOfCells()):
        cell = TBC_M_data.GetCell(i)
        p1 = cell.GetPoints().GetPoint(0)
        p2 = cell.GetPoints().GetPoint(1)
        p3 = cell.GetPoints().GetPoint(2)
        triangle = vtk.vtkTriangle()
        triangle.GetPoints().InsertPoint(0, p1[0], p1[1], p1[2])
        triangle.GetPoints().InsertPoint(1, p2[0], p2[1], p2[2])
        triangle.GetPoints().InsertPoint(2, p3[0], p3[1], p3[2])
        area = triangle.TriangleArea(p1, p2, p3)
        areasM.append([area])

    np.asarray(areasM)
    appM = np.sum(areasM) / (TBC_M_data.GetNumberOfPoints())
    apcM = np.sum(areasM) / (TBC_M_data.GetNumberOfCells())

    areasL = []
    for i in range(TBC_L_data.GetNumberOfCells()):
        cell = TBC_L_data.GetCell(i)
        p1 = cell.GetPoints().GetPoint(0)
        p2 = cell.GetPoints().GetPoint(1)
        p3 = cell.GetPoints().GetPoint(2)
        triangle = vtk.vtkTriangle()
        triangle.GetPoints().InsertPoint(0, p1[0], p1[1], p1[2])
        triangle.GetPoints().InsertPoint(1, p2[0], p2[1], p2[2])
        triangle.GetPoints().InsertPoint(2, p3[0], p3[1], p3[2])
        area = triangle.TriangleArea(p1, p2, p3)
        areasL.append([area])

    np.asarray(areasL)
    appL = np.sum(areasL) / (TBC_L_data.GetNumberOfPoints())
    apcL = np.sum(areasM) / (TBC_M_data.GetNumberOfCells())

    # Read split STLs and convert to PolyData
    reader3 = vtk.vtkSTLReader()
    reader3.SetFileName(cartilage_stls[1])
    reader3.Update()
    TBC_L_Polydata = reader3.GetOutput()

    reader4 = vtk.vtkSTLReader()
    reader4.SetFileName(cartilage_stls[2])
    reader4.Update()
    TBC_M_Polydata = reader4.GetOutput()

    num = np.size(M)
    pct = 0
    dppL = np.zeros(num)
    dppM = np.zeros(num)
    CAM_data = np.zeros(num)
    CAL_data = np.zeros(num)
    strainM_data = np.zeros(num)
    strainL_data = np.zeros(num)
    # CAM1_data = np.zeros(num)
    # CAL1_data = np.zeros(num)
    center_actM = []
    center_actL = []
    i = 0
    for ii in range(num):

        T_FEMORI_TIB = get_RB_transformation_matrix(M[ii], P[ii], S[ii], F[ii], V[ii],
                                                    IR[
                                                        ii])  # Transform between FEM CS and TIB CS Relative angles/positions b/w bones

        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)))  # Define the positions of the femur in the image CS

        # 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!
        transformSTL(merged_stl, positions_TF[ii])

        dpM, point2, CAM, centerM, strainM = insertion_point('test_1.stl', cartilage_stls[2], bone_stls[1], 0, apcM)
        dpL, point, CAL, centerL, strainL = insertion_point('test_1.stl', cartilage_stls[1], bone_stls[1], 0, apcL)
        CAL_data[ii] = CAL
        CAM_data[ii] = CAM
        strainL_data[ii] = strainL
        strainM_data[ii] = strainM
        # CAM1_data[ii] = CAM1
        # CAL1_data[ii] = CAL1

        if ii == int(pct * num):
            # center_areaM[i] = centerM
            sd_screenshot('test_1.stl', TBC_L_Polydata, TBC_M_Polydata, pct, data_folder)
            actM = center_act(centerM)
            actL = center_act(centerL)
            center_actM.append(actM)
            center_actL.append(actL)
            pct += 0.1
        dppL[ii] = dpL / 2
        dppM[ii] = dpM / 2

    plt.figure()

    # Lateral
    plt.subplot(121)
    plt.plot(time, CAL_data)
    plt.title('Lateral')
    # plt.ylabel('Length [mm]')
    plt.grid(True)
    plt.gca().axes.get_xaxis().set_visible(False)
    # plt.ylabel('Area as Fraction of Total Cartilage Area')
    plt.ylabel('Fractional Contact Area')
    # Medial
    plt.subplot(122)
    plt.plot(time, CAM_data)
    plt.title('Medial')
    plt.grid(True)
    plt.gca().axes.get_xaxis().set_visible(False)

    plt.suptitle('Contact Area')

    plt.savefig(data_folder + '/' + 'Contact_Area' + '.png')

    plt.figure()

    # Lateral
    plt.subplot(121)
    plt.plot(time, strainL_data)
    plt.title('Lateral')
    # plt.ylabel('Length [mm]')
    plt.grid(True)
    plt.gca().axes.get_xaxis().set_visible(False)
    plt.ylabel('Strain []')
    # Medial
    plt.subplot(122)
    plt.plot(time, strainM_data)
    plt.title('Medial')
    plt.grid(True)
    plt.gca().axes.get_xaxis().set_visible(False)

    plt.suptitle('Tibial Cartilage strain')

    plt.savefig(data_folder + '/' + 'Max_strain' + '.png')

    plt.figure()

    # Lateral
    plt.subplot(121)
    plt.plot(time, dppL)
    plt.title('Lateral')
    # plt.ylabel('Length [mm]')
    plt.grid(True)
    plt.gca().axes.get_xaxis().set_visible(False)
    plt.ylabel('Signed distance [mm]')
    # Medial
    plt.subplot(122)
    plt.plot(time, dppM)
    plt.title('Medial')
    plt.grid(True)
    plt.gca().axes.get_xaxis().set_visible(False)

    plt.suptitle('Maximum cartilage penetration')

    plt.savefig(data_folder + '/' + 'Maximum_Cartilage_Penetration' + '.png')
    printing_centers(center_actM, center_actL, reader1, reader2, data_folder)

    ## PatelloFemoral
    # if os.path.basename(args[-1]) == 'oks003_PF_csv.txt':
    #
    #     df = pd.read_csv(csv_file)
    #     M_pat = df['Knee PTFJ Medial  [mm]']
    #     P_pat = df['Knee PTFJ Posterior  [mm]']
    #     S_pat = df['Knee PTFJ Superior  [mm]']
    #     F_pat = df['Knee PTFJ Flexion  [deg]']
    #     V_pat = df['Knee PTFJ Valgus  [deg]']
    #     IR_pat = df['Knee PTFJ Internal Rotation  [deg]']
    #     time = df['Extracted Time Points [s]']
    #     # #Open motion data from CSV file
    #     # M_pat = []
    #     # P_pat = []
    #     # S_pat = []
    #     # F_pat = []
    #     # V_pat = []
    #     # IR_pat = []
    #     # time = []
    #     # f = open(args[-1])
    #     # csv_name = f.read().replace('\n', '')
    #     # csv_file = os.path.join(os.path.dirname(args[-1]), csv_name)
    #     # with open(csv_file, 'r') as csvFile:
    #     #     reader = csv.DictReader(csvFile)
    #     #     for row in reader:
    #     #         M_pat.append(row['Knee PTFJ Medial  [mm]'])
    #     #         P_pat.append(row['Knee PTFJ Posterior  [mm]'])
    #     #         S_pat.append(row['Knee PTFJ Superior  [mm]'])
    #     #         F_pat.append(row['Knee PTFJ Flexion  [deg]'])
    #     #         V_pat.append(row['Knee PTFJ Valgus  [deg]'])
    #     #         IR_pat.append(row['Knee PTFJ Internal Rotation  [deg]'])
    #     #         time.append(row['Extracted Time Points [s]'])
    #
    #     config = ConfigParser.RawConfigParser()
    #     if not config.read(os.path.join(os.path.dirname(args[-1]), csv_name.split('/')[0], 'Configuration', 'State.cfg')):
    #         raise IOError, "Cannot load configuration file... Check path."
    #     Offset_FEMORI_PAT = convertTOarray(config.get('Knee PTFJ', 'Position Offset (m,rad)'), 1)
    #
    #     if knee_side == "Left":
    #         M_pat = -1 * np.array(M_pat) / 1000 - Offset_FEMORI_PAT[0]
    #         P_pat = np.array(P_pat) / 1000 + Offset_FEMORI_PAT[1]
    #         S_pat = np.array(S_pat) / 1000 + Offset_FEMORI_PAT[2]
    #         F_pat = np.radians(np.array(F_pat)) + Offset_FEMORI_PAT[3]
    #         V_pat = np.radians(-1 * np.array(V_pat)) - Offset_FEMORI_PAT[4]
    #         IR_pat = np.radians(-1 * np.array(IR_pat)) - Offset_FEMORI_PAT[5]
    #
    #     else:
    #         M_pat = np.array([map(float, M_pat)]) / 1000 - Offset_FEMORI_PAT[0]
    #         P_pat = np.array([map(float, P_pat)]) / 1000 + Offset_FEMORI_PAT[1]
    #         S_pat = np.array([map(float, S_pat)]) / 1000 + Offset_FEMORI_PAT[2]
    #         F_pat = np.radians(np.array([map(float, F_pat)])) + Offset_FEMORI_PAT[3]
    #         V_pat = np.radians(np.array([map(float, V_pat)])) - Offset_FEMORI_PAT[4]
    #         IR_pat = np.radians(np.array([map(float, IR_pat)])) - Offset_FEMORI_PAT[5]
    #
    #     # csvFile.close()
    #
    #     T_I_PAT = np.dot(R["T_I_PATOS"], R["T_PATOS_PAT"])
    #
    #     positions_PF = np.empty([np.size(IR), 4, 4])
    #     positions_TF = np.empty([np.size(IR), 4, 4])
    #
    #     reader1 = vtk.vtkSTLReader()
    #     reader1.SetFileName(cart_full_stls[3])
    #     reader1.Update()
    #     pat_data = reader1.GetOutput()
    #
    #     #Calculate Area of Tibial Cartilages and Area/Cell
    #     areas = []
    #     for i in range(pat_data.GetNumberOfCells()):
    #         cell = pat_data.GetCell(i)
    #         p1 = cell.GetPoints().GetPoint(0)
    #         p2 = cell.GetPoints().GetPoint(1)
    #         p3 = cell.GetPoints().GetPoint(2)
    #         triangle = vtk.vtkTriangle()
    #         triangle.GetPoints().InsertPoint(0, p1[0], p1[1], p1[2])
    #         triangle.GetPoints().InsertPoint(1, p2[0], p2[1], p2[2])
    #         triangle.GetPoints().InsertPoint(2, p3[0], p3[1], p3[2])
    #         area = triangle.TriangleArea(p1, p2, p3)
    #         areas.append([area])
    #
    #     np.asarray(areas)
    #     apc = np.sum(areas) / (pat_data.GetNumberOfCells())
    #
    #     num = np.size(M)
    #     pct = 0
    #     dpp = np.zeros(num)
    #     CA_data = np.zeros(num)
    #     strain_data = np.zeros(num)
    #     center_actor = []
    #     # i = 0
    #     for ii in range(num):
    #         T_FEMORI_TIB = get_RB_transformation_matrix(M[ii], P[ii], S[ii], F[ii], V[ii],
    #                                                     IR[
    #                                                         ii])  # Transform between FEM CS and TIB CS Relative angles/positions 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])  # Transform between FEM CS and PAT CS Relative angles/positions_TF b/w bones
    #
    #         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)))  # Define the positions 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))  # Define the positions_TF of the femur in the image CS
    #
    #         transformSTL('oks003_MRC_FMBC_SKC_01.stl', positions_TF[ii])
    #
    #         # 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!
    #         transformSTL_pat(bone_stls[2], positions_PF[ii])
    #
    #         dp, point, CA, center, strain = insertion_point('test_1.stl', 'test_pat.stl', bone_stls[2], 2, apc)
    #         CA_data[ii] = CA
    #         strain_data[ii] = strain
    #
    #         if ii == int(pct*num):
    #             # center_areaM[i] = centerM
    #             sd_screenshot_pat('test_1.stl', 'test_pat.stl', pct, data_folder)
    #             center_actor.append(center_act(center))
    #             pct += 0.1
    #         dpp[ii] = dp/2
    #
    #     plt.figure()
    #
    #     plt.plot(time, CA_data)
    #     plt.grid(True)
    #     plt.gca().axes.get_xaxis().set_visible(False)
    #     plt.ylabel('Fractional Contact Area')
    #     plt.grid(True)
    #     plt.gca().axes.get_xaxis().set_visible(False)
    #     plt.suptitle('Contact Area')
    #     plt.savefig(data_folder + '/' + 'Contact_Area' + '.png')
    #
    #     plt.figure()
    #     plt.plot(time, strain_data)
    #     plt.grid(True)
    #     plt.gca().axes.get_xaxis().set_visible(False)
    #     plt.ylabel('Strain []')
    #     plt.suptitle('Tibial Cartilage strain')
    #     plt.savefig(data_folder + '/' + 'Max_strain' + '.png')
    #
    #     plt.figure()
    #     plt.plot(time, dpp)
    #     plt.grid(True)
    #     plt.gca().axes.get_xaxis().set_visible(False)
    #     plt.ylabel('Signed distance [mm]')
    #     plt.grid(True)
    #     plt.gca().axes.get_xaxis().set_visible(False)
    #     plt.suptitle('Maximum cartilage penetration')
    #     plt.savefig(data_folder + '/' + 'Maximum_Cartilage_Penetration' + '.png')

    # Accuracy analysis
    # diffM = (np.subtract(CAM_data,CAM1_data)/CAM_data)*100
    # diffL = (np.subtract(CAL_data,CAL1_data)/CAL_data)*100
    #
    # plt.figure()
    #
    # # Lateral
    # plt.subplot(121)
    # plt.plot(time, diffL)
    # plt.title('Lateral')
    # # plt.ylabel('Length [mm]')
    # plt.grid(True)
    # plt.gca().axes.get_xaxis().set_visible(False)
    # plt.ylabel('Percentage difference')
    # # Medial
    # plt.subplot(122)
    # plt.plot(time, diffM)
    # plt.title('Medial')
    # plt.grid(True)
    # plt.gca().axes.get_xaxis().set_visible(False)
    #
    # plt.suptitle('Difference')
    #
    # plt.savefig(data_folder + '/' + 'Area_difference' + '.png')

if __name__ == "__main__":

    main(sys.argv)

    # main(['callfunction', '/home/lopezr3/Documents/CC/Registration/oks001_registration_01.xml', '/home/lopezr3/Documents/CC/oks001/joint_mechanics-oks001/TibiofemoralJoint/KinematicsKinetics/oks001_TF_csv.txt'])