#import sys
import numpy as np
from lxml import etree as et
import LogPostProcessing as LPP
from mpl_toolkits.mplot3d import axes3d
import matplotlib.pyplot as plt
import os


def FindStiffness(logfile, rb_name = 'FMB'):

    rigid_bodies = LPP.find_rigid_bodies(logfile)

    all_data, time_steps = LPP.collect_data(logfile)
    all_data, time_steps = LPP.add_initial_values(all_data, time_steps, rigid_bodies)

    # get the femur reaction forces and center of mass locations
    Reaction_Forces = all_data['Reaction_Forces']
    com_data = all_data['center_of_mass']

    moving_rb = rigid_bodies[rb_name]
    moving_rb_reaction_force = Reaction_Forces[moving_rb.id]
    moving_rb_com = com_data[moving_rb.id]

    # get the displacement of the femur bone
    displacement_vec = moving_rb_com - moving_rb_com[0]
    displacement_mag = np.linalg.norm(displacement_vec, axis=1)

    # will determine the fiber direction from the end point of the displacement
    # since we know displacement was prescribed along the fiber direction
    fiber_dir = displacement_vec[-1]
    fiber_dir = fiber_dir/np.linalg.norm(fiber_dir)

    # get the reaction force of the femur along the ligament axis
    # dot product of the reaction force with the fiber dir
    force_along_ligament = np.matmul(moving_rb_reaction_force, fiber_dir.T) # dot product
    # force_along_ligament = np.linalg.norm(moving_rb_reaction_force, axis=1)



    # plot_force_displacement(displacement_mag, force_along_ligament)

    # get the final 1/3 of the data as the "linear portion"
    # starting_idx = int(2*len(displacement_mag)/3)
    # displacement_linear = displacement_mag[starting_idx:]
    # force_linear = force_along_ligament[starting_idx:]
    starting_disp = 2*np.max(displacement_mag)/3
    displacement_linear_idx = np.where(displacement_mag>starting_disp)[0]
    displacement_linear = displacement_mag[displacement_linear_idx]
    force_linear = force_along_ligament[displacement_linear_idx]

    # fit a line to the linear portion
    p = np.polyfit(displacement_linear, force_linear, 1)
    stiffness = p[0] # the slope of the line

    print(" Linear Stiffness: ")
    print(stiffness)

    return displacement_mag, force_along_ligament, p, stiffness

def plot_mesh_convergence():

    logfiles =["C:\\Users\schwara2\Documents\Open_Knees\oks003_calibration\MeshDensity\models\ACL\Febio_IP4\FeBio_custom.log",
               "C:\\Users\schwara2\Documents\Open_Knees\oks003_calibration\MeshDensity\models\ACL\Febio_IP6\FeBio_custom.log",
               "C:\\Users\schwara2\Documents\Open_Knees\oks003_calibration\MeshDensity\models\ACL\Febio_IP8\FeBio_custom.log",
               "C:\\Users\schwara2\Documents\Open_Knees\oks003_calibration\MeshDensity\models\ACL\Febio_IP10\FeBio_custom.log"]

    names = ['IPSR 4', 'IPSR 6', 'IPSR 8', 'IPSR 10']
    rb_name = 'FMB'
    fig =plt.figure()
    plt.title("ACL Mesh Convergence")
    plt.xlabel('Displacement [mm]')
    plt.ylabel('Reaction Force [N]')
    for i, lf in enumerate(logfiles):
        disp, force, _ , _= FindStiffness(lf, rb_name)
        plt.plot(disp, force, label=names[i])
    plt.legend(loc='best')
    plt.show()


def plot_mat_props_calib():

    logfiles= ["C:\\Users\schwara2\Documents\Open_Knees\oks003_calibration\MaterialProperties\ACL_IP8\FeBio_custom.log",
               "C:\\Users\schwara2\Documents\Open_Knees\oks003_calibration\MaterialProperties\ACL_IP8\FeBio_custom_orig.log"]


    rb_name = 'FMB'
    expected = (242,28) # mean and SD

    names = ["c5=5000", "c5=535"]
    colors = ['r','b']

    plt.title("ACL Material Properites Confirmation")
    plt.xlabel('Displacement [mm]')
    plt.ylabel('Reaction Force [N]')

    for i, lf in enumerate(logfiles):

        disp, force, p, _ = FindStiffness(lf, rb_name)

        plt.plot(disp, force, label=names[i], color=colors[i])

        # plot the linear stiffness line
        y = (p[0] * disp) + p[1]
        plt.plot(disp, y, label='stiffness = {0:.2f} N/mm'.format(p[0]), color=colors[i], linestyle='dashed')

        # # plot the expected range
        # d0 = disp[start_idx]
        # d1 = disp[-1]
        #
        # F0 = force[start_idx]
        # F1_l = F0 + (d1-d0)*(expected[0] - 2*expected[1])
        # F1_u = F0 +(d1-d0) *(expected[0] + 2*expected[1])
        # plt.fill_between([d0,d1], [F0,F1_u], [F0, F1_l], alpha=0.2, label="expected range")

    plt.ylim(bottom=0)
    plt.legend(loc='best')
    plt.show()
    plt.close()


def plot_mat_props():
    """ function used to plot the force displacement curve of one or more models using the log files"""

    # scaling each poperty
    # #scaling c3 tension
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_01.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_02.log"]
    # #scaling c3 compression
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_01.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_02.log"]
    # #scaling c4 tension
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_03.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_04.log"]
    # #scaling c4 compression
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_03.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_04.log"]
    # #scaling lam_max tension
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_05.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_06.log"]
    # #scaling lam_max compression
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_05.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_06.log"]
    # #scaling c1,k tension
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_07.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_08.log"]
    # #scaling c1, k compression
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_07.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_08.log"]
    # #scaling c1 tension
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_09.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_10.log"]
    #scaling c1 compression
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_09.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_10.log"]
    #scaling c5 tension
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_11.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_12.log"]
    # #scaling c5 compression
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_11.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_12.log"]
    # #scaling c5, k tension
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_13.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_tension_14.log"]
    #scaling c5, k compression
    # logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_00.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_13.log",
    #             "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\oks001_compression_14.log"]


    # calibration results
    #oks001 - compression
    logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\\Uncoupled_Models\oks001\oks001_compression_r1.log",
                "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\\Uncoupled_Models\oks001\oks001_compression_r10.log",
                "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\\Uncoupled_Models\oks001\oks001_compression_r100.log"]
                # "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\\Uncoupled_Models\oks001\oks001_compression_r1000.log",
                # "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\\Uncoupled_Models\oks001\oks001_compression_r10000.log"]

    # oks001 tension compariosn
    logfiles = ["C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\FlexionStretch\oks003_tension_flex.log",
                "C:\\Users\schwara2\Documents\Open_Knees\\app\ACL_modeling\Investigation\FlexionStretch\oks003_tension.log"]

    rb_name = 'FMB'
    expected = (242, 28)  # mean and SD

    plt.title("Force-displacement results")
    plt.xlabel('Displacement [mm]')
    plt.ylabel('Reaction Force [N]')

    for i, lf in enumerate(logfiles):

        base = os.path.basename(lf)
        name = base.split(".")[0]
        name = name.replace("FeBio_custom_", "")


        disp, force, p, _ = FindStiffness(lf, rb_name)

        plt.plot(disp, force, label=name)

        # # plot the linear stiffness line
        # y = (p[0] * disp) + p[1]
        # plt.plot(disp, y, label='stiffness = {0:.2f} N/mm'.format(p[0]), linestyle='dashed')

    plt.legend(loc='best')
    plt.show()
    # plt.close()

if __name__ == '__main__':

    # logfile ="C:\\Users\schwara2\Documents\Open_Knees\du02_calibration\MaterialProperties\ACL_IP6\FeBio_custom_02.log"
    # rb_name = 'FMB'
    # FindStiffness(logfile, rb_name)

    # plot_mesh_convergence()
    plot_mat_props()
