# Version using units in metres, Joules and seconds
import opencor as opencor
import math
import scipy
import numpy as np
import pdb
import matplotlib.pyplot as plt
from scipy.integrate import odeint

# Ensure the FTU model is the current simulation
s1 = opencor.open_simulation('Cardiac FTU v8.sedml')
s2 = opencor.open_simulation('main_cv7_diodeValves.sedml')
data1 = s1.data()
data2 = s2.data()

s1.reset(True)                                   # initialise OpenCOR module 1
s2.reset(True)                                   # initialise OpenCOR module 2

s1.clear_results()                               # clear graphs in OpenCOR module 1
s2.clear_results()                               # clear graphs in OpenCOR module 2

# iterate over the cardiac cycle

##################################### Initialise arrays #####################################

t     = np.zeros(501)
p_AA  = np.zeros(501)
p_LA  = np.zeros(501)
p_LV  = np.zeros(501)
q_LV  = np.zeros(501)
r_LV  = np.zeros(501)
l_LV  = np.zeros(501)
To    = np.zeros(501)
event = np.zeros(501)

t[0]  = 0                                        # set initial time (s) in cardiac cycle to zero
dt    = 0.01                                     # set time increment (s) for steps during cardiac cycle
To[0] = 0                                        # set initial active tension to zero

p_LV[0] = 0 
p_LA[0] = 0                                      # set initial LA pressure to small value >0 to open mitral valve
p_AA[0] = 1                                      # set aortic artery pressure
q_LV[0] = data2.states()["Heart/q_LV"]           # calculate initial LV volume
l_LV[0] = 1                                      # set initial LV extension ratio
r_LV[0] = 10*(q_LV[0]/(l_LV[0]*math.pi))**0.5

data1.constants()["main/T_0"]      = 0           # set active tension to zero
data1.constants()["main/r_endo"]   = r_LV[0]     # set initial r_endo in OpenCOR
data1.constants()["main/lambda_a"] = l_LV[0]     # set initial lambda_a in OpenCOR

i=0
def fxn(n):
    print("\n Pause - c to continue, q to quit, n for next pause, s to step through code")
    pdb.set_trace()

#fxn(1)

while 1>0:
    
 print("t[%3d]=%8.4f To=%8.4f p_LV=%8.4f p_LA=%8.4f p_AA=%8.4f r_LV=%8.4f q_LV=%9.6f"
          % (i, t[i], To[i], p_LV[i], p_LA[i], p_AA[i], r_LV[i], q_LV[i]))


##################################### Phase I Isovolumic contraction #####################################

 print("\n Phase I - LV isovolumic contraction")

 while p_LV[i] <= p_AA[i]:                        # isovolumic contraction until aortic valve closes
    i += 1
    t[i]    = t[i-1]+dt                          # increment time in cardiac cycle
    To[i]   = data2.algebraic()["Heart/To"]      # no active contraction
    q_LV[i] = q_LV[i-1]                          # keep LV volume at previous value                       
    l_LV[i] = l_LV[i-1]                          # keep at previous value
    r_LV[i] = (q_LV[i]/(l_LV[i]*math.pi))**0.5   # keep LV radius at previous value
    s1.reset(True)                               # reset OpenCOR
    data1.constants()["main/T_0"] = To[i]        # set T_0 in OpenCOR
    data1.constants()["main/r_endo"] = 10*r_LV[i]   # update r_endo in OpenCOR from previous (now constant) LV radius
    s1.run()                                     # run OpenCOR
    p_LV[i] =-data1.algebraic()["main/p_LV"]     # update LV pressure from OpenCOR
    l_LV[i] = data1.constants()["main/lambda_a"] # update extension ratio from OpenCOR

    data2.constants()["Heart/u_LV"] = p_LV[i]    # update u_LV in CV model with LV pressure from wall calculation
    data2.set_starting_point(t[i-1])             # set start time in s for OpenCOR CV model (module 2)
    data2.set_ending_point(t[i])                 # set end time
    s2.run()                                     # run CV model
    q_LV[i] = data2.states()["Heart/q_LV"]       # update q_LV from CV model
    p_AA[i] = data2.states()["Systemic/u_AA"]    # update p_LA from CV model
    p_LA[i] = data2.algebraic()["Heart/u_LA"]    # update p_LA from CV model

    print("Phase I: t[%3d]=%8.4f To=%7.2f p_LV=%7.2f p_LA=%7.2f p_AA=%7.2f r_LV=%7.3f q_LV=%9.6f"
          % (i, t[i], To[i], p_LV[i], p_LA[i], p_AA[i], r_LV[i], q_LV[i]))

# fxn(1)

##################################### Phase II Systolic ejection #####################################

 print("\n Phase II - LV ejection")

 while p_LV[i] > p_AA[i]:                        # LV ejection phase
    i += 1
    t[i]    = t[i-1]+dt                          # increment time
    To[i]   = data2.algebraic()["Heart/To"]      # no active contraction    # define To(t)
    q_LV[i] = q_LV[i-1]                          # keep at previous value
    l_LV[i] = l_LV[i-1]                          # keep at previous value
    r_LV[i] = (q_LV[i]/(l_LV[i]*math.pi))**0.5   # calculate LV radius from LV volume & length
    s1.reset(True)                               # reset OpenCOR
    data1.constants()["main/T_0"] = To[i]        # set T_0 in OpenCOR
    data1.constants()["main/r_endo"] = 10*r_LV[i]   # update r_endo in OpenCOR from previous LV radius
    s1.run()                                     # run OpenCOR with current r_endo
    p_LV[i] =-data1.algebraic()["main/p_LV"]     # obtain LV pressure from OpenCOR
    l_LV[i] = data1.constants()["main/lambda_a"] # update extension ratio from OpenCOR

    data2.constants()["Heart/u_LV"] = p_LV[i]    # update u_LV in CV model with LV pressure from wall calculation
    data2.set_starting_point(t[i-1])             # set start time in s for OpenCOR CV model (module 2)
    data2.set_ending_point(t[i])                 # set end time
    s2.run()                                     # run CV model
    q_LV[i] = data2.states()["Heart/q_LV"]       # update q_LV from CV model
    p_LA[i] = data2.algebraic()["Heart/u_LA"]    # update p_LA from CV model
    p_AA[i] = data2.states()["Systemic/u_AA"]    # update p_AA from CV model

    print("Phase II: t[%3d]=%8.4f To=%7.2f p_LV=%7.2f p_LA=%7.2f p_AA=%7.2f r_LV=%7.3f q_LV=%9.6f"
          % (i, t[i], To[i], p_LV[i], p_LA[i], p_AA[i], r_LV[i], q_LV[i]))


##################################### Phase III Isovolumic relaxation #####################################

 print("\n Phase III - LV isovolumic relaxation - start time =%8.4f" % (t[i]))

 while p_LV[i] > p_LA[i]:                        # isovolumic relaxation
    i += 1
    t[i]    = t[i-1]+dt                          # increment time
    To[i]   = data2.algebraic()["Heart/To"]      # no active contraction    # define To(t)
    q_LV[i] = q_LV[i-1]                          # keep at previous value
    l_LV[i] = l_LV[i-1]                          # keep at previous value
    r_LV[i] = (q_LV[i]/(l_LV[i]*math.pi))**0.5   # calculate LV radius from LV volume & length
    s1.reset(True)                               # reset OpenCOR
    data1.constants()["main/T_0"] = To[i]        # set T_0 in OpenCOR
    data1.constants()["main/r_endo"] = 10*r_LV[i]   # update r_endo in OpenCOR from previous LV radius
    s1.run()                                     # run OpenCOR with current r_endo
    p_LV[i] =-data1.algebraic()["main/p_LV"]     # obtain LV pressure from OpenCOR
    l_LV[i] = data1.constants()["main/lambda_a"] # update extension ratio from OpenCOR

    data2.constants()["Heart/u_LV"] = p_LV[i]    # update u_LV in CV model with LV pressure from wall calculation
    data2.set_starting_point(t[i-1])             # set start time in s for OpenCOR CV model (module 2)
    data2.set_ending_point(t[i])                 # set end time
    s2.run()                                     # run CV model
    q_LV[i] = data2.states()["Heart/q_LV"]       # update q_LV from CV model
    p_LA[i] = data2.algebraic()["Heart/u_LA"]    # update p_LA from CV model
    p_AA[i] = data2.states()["Systemic/u_AA"]    # update p_AA from CV model

    print("Phase III: t[%3d]=%8.4f To=%7.2f p_LV=%7.2f p_LA=%7.2f p_AA=%7.2f r_LV=%7.3f q_LV=%9.6f"
          % (i, t[i], To[i], p_LV[i], p_LA[i], p_AA[i], r_LV[i], q_LV[i]))

##################################### Phase IV Rapid filling #####################################

 print("\n Phase IV - LV rapid filling")

 while p_LV[i] < p_LA[i]:                        # LV filling driven by stored energy of contracted tissue
    i += 1
    t[i]    = t[i-1]+dt                          # increment time in cardiac cycle
    To[i]   = data2.algebraic()["Heart/To"]      # no active contraction    # define To(t)
    l_LV[i] = l_LV[i-1]                          # keep at previous value
    q_LV[i] = q_LV[i-1]                          # keep at previous value
    r_LV[i] = (q_LV[i]/(l_LV[i]*math.pi))**0.5   # calculate LV radius from LV volume
    s1.reset(True)                               # reset OpenCOR
    data1.constants()["main/T_0"] = To[i]        # set T_0 in OpenCOR
    data1.constants()["main/r_endo"] = 10*r_LV[i]   # update r_endo in OpenCOR from previous LV radius
    s1.run()                                     # run OpenCOR with current r_endo
    p_LV[i] =-data1.algebraic()["main/p_LV"]     # obtain LV pressure from OpenCOR
    l_LV[i] = data1.constants()["main/lambda_a"] # update extension ratio from OpenCOR

    data2.constants()["Heart/u_LV"] = p_LV[i]    # update u_LV in CV model with LV pressure from wall calculation
    data2.set_starting_point(t[i-1])             # set start time in ms for OpenCOR CV model (module 2)
    data2.set_ending_point(t[i])                 # set end time
    s2.run()                                     # run CV model
    q_LV[i] = data2.states()["Heart/q_LV"]       # update q_LV from CV model
    p_LA[i] = data2.algebraic()["Heart/u_LA"]    # update p_LA from CV model
    p_AA[i] = data2.states()["Systemic/u_AA"]    # update p_AA from CV model

    print("Phase IV: t[%3d]=%8.4f To=%7.2f p_LV=%7.2f p_LA=%7.2f p_AA=%7.2f r_LV=%7.3f q_LV=%9.6f"
          % (i, t[i], To[i], p_LV[i], p_LA[i], p_AA[i], r_LV[i], q_LV[i]))

##################################### Phase V Atrial Systole #####################################

 print("\n Phase V - LA contraction")

 while p_LA[i] > p_LV[i]:                        # LV filling driven by p_LA
    i += 1
    t[i] = t[i-1]+dt                             # increment time in cardiac cycle
    To[i]   = data2.algebraic()["Heart/To"]      # no active contraction    # define To(t)
    l_LV[i] = l_LV[i-1]                          # keep at previous value
    q_LV[i] = q_LV[i-1]                          # keep at previous value
    r_LV[i] = (q_LV[i]/(l_LV[i]*math.pi))**0.5   # calculate LV radius from LV volume
    s1.reset(True)                               # reset OpenCOR
    data1.constants()["main/T_0"] = To[i]        # set T_0 in OpenCOR
    data1.constants()["main/r_endo"] = 10*r_LV[i-1] # update r_endo in OpenCOR from previous LV radius
    s1.run()                                     # run OpenCOR with current r_endo
    p_LV[i] =-data1.algebraic()["main/p_LV"]     # obtain LV pressure from OpenCOR
    l_LV[i] = data1.constants()["main/lambda_a"] # obtain extension ratio from OpenCOR

    data2.constants()["Heart/u_LV"] = p_LV[i]    # update u_LV in CV model with LV pressure from wall calculation
    data2.set_starting_point(t[i-1])             # set start time in s for OpenCOR CV model (module 2)
    data2.set_ending_point(t[i])                 # set end time
    s2.run()                                     # run CV model
    q_LV[i] = data2.states()["Heart/q_LV"]       # update q_LV from CV model
    p_LA[i] = data2.algebraic()["Heart/u_LA"]    # update p_LA from CV model
    p_AA[i] = data2.states()["Systemic/u_AA"]    # update p_LA from CV model

    print("Phase V: t[%3d]=%8.4f To=%7.2f p_LV=%7.2f p_LA=%7.2f p_AA=%7.2f r_LV=%7.3f q_LV=%9.6f"
          % (i, t[i], To[i], p_LV[i], p_LA[i], p_AA[i], r_LV[i], q_LV[i]))