import numpy as np
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
from scipy.optimize import fsolve
from scipy.interpolate import interp2d

# Constants
cylinder_diameter = 0.32  # meters (m)
accumulator_diameter = 0.32  # meters (m)
A_b = np.pi * (cylinder_diameter / 2)**2  # square meters (m²)
A_a = np.pi * (accumulator_diameter / 2)**2  # square meters (m²)
P_a0 = 120e5  # Pascals (Pa), initial pressure in accumulator
oil_bulk_modulus = 1.6e9  # Pascals (Pa)
V_accu0 = 0.5  # cubic meters (m³)
V_min = 0.05  # Minimum gas volume (m³)

d_orifice = 0.002  # meters (m), diameter of the orifice
A_orifice = (1/4) * np.pi * d_orifice**2  # square meters (m²)
rho_oil = 870  # kilograms per cubic meter (kg/m³)
sigma = 1e3  # Reduced threshold for outflow damping (Pa)

stroke_length = 4.0  # meters (m), stroke length of the cylinder
pipe_diameter = 0.03  # meters (m)
pipe_length = 2.0  # meters (m)
cc = 0.0001  # tolerances between piston and cylinder wall (m)
A_p = np.pi * (pipe_diameter / 2)**2  # square meters (m²), pipe cross-sectional area
V_cylinder = A_b * stroke_length  # cubic meters (m³)
V_pipes = np.pi * (pipe_diameter / 2)**2 * pipe_length  # cubic meters (m³)
V_chamber1 = V_cylinder + V_pipes  # cubic meters (m³)

rho_steel = 7850  # kilograms per cubic meter (kg/m³), density of steel
L_cyl = 0.5  # meters (m), length of the cylinder
L_accu = 0.5  # meters (m), length of the accumulator
m1 = rho_steel * (np.pi * (cylinder_diameter / 2)**2) * L_cyl  # kilograms (kg)
m2 = rho_steel * (np.pi * (accumulator_diameter / 2)**2) * L_accu  # kilograms (kg)
F1 = 1000000  # Newtons (N), force applied to the cylinder

nu_oil = 46e-6  # square meters per second (m²/s), kinematic viscosity of oil
mu = nu_oil * rho_oil  # Pascal-seconds (Pa·s), dynamic viscosity

# Damping parameters
e = 0.00015  # meters (m), roughness height
c1 = 2 * mu * np.pi * cylinder_diameter * L_cyl / cc  # Newton-seconds per meter (N·s/m)
c2 = 2 * mu * np.pi * accumulator_diameter * L_accu / cc  # Newton-seconds per meter (N·s/m)

# Definieer de tabel voor gamma
pressures = np.array([1, 50, 100, 150, 200, 250, 500])  # Drukken in bar
temperatures = np.array([200, 270, 300, 350, 400, 450, 500])  # Temperaturen in K
gamma_table = np.array([
    [1.4, 1.4, 1.4, 1.4, 1.4, 1.39, 1.39],  # 1 bar
    [1.68, 1.51, 1.48, 1.46, 1.44, 1.42, 1.41],  # 50 bar
    [2.01, 1.62, 1.56, 1.51, 1.47, 1.45, 1.43],  # 100 bar
    [2.16, 1.70, 1.62, 1.55, 1.50, 1.47, 1.45],  # 150 bar
    [2.14, 1.75, 1.67, 1.58, 1.53, 1.47, 1.45],  # 200 bar
    [2.06, 1.77, 1.69, 1.60, 1.54, 1.50, 1.48],  # 250 bar
    [1.8, 1.72, 1.69, 1.62, 1.57, 1.53, 1.5]   # 500 bar
])

# Interpolatiefunctie voor gamma bij T=318 K
T_constant = 318  # Kelvin (K), aangenomen constante temperatuur
gamma_at_T318 = [np.interp(T_constant, temperatures, gamma_table[i, :]) for i in range(len(pressures))]
gamma_interp_func = lambda P: np.interp(P, pressures, gamma_at_T318)

# Damping calculation
def colebrook(f, Re, e, D):
    return 1/np.sqrt(f) + 0.86 * np.log10((e/(3.7*D)) + (2.51/(Re*np.sqrt(f))))

def darcy_weisbach(v1, A_b, A_p, pipe_length, pipe_diameter, rho, mu, e):
    if A_p == 0:
        return 0, 0
    v = (A_b * v1) / A_p  # meters per second (m/s)
    Re = (rho * v * pipe_diameter) / mu if mu != 0 else 0  # dimensionless
    f_guess = 0.02
    f_darcy = fsolve(colebrook, f_guess, args=(Re, e, pipe_diameter))[0]
    dP = f_darcy * (pipe_length/pipe_diameter) * 0.5 * rho * v**2  # Pascals (Pa)
    return dP, f_darcy

def pipe_damping(v1, pipe_length, pipe_diameter, rho, mu, e, A_b):
    dP, f_darcy = darcy_weisbach(v1, A_b, A_p, pipe_length, pipe_diameter, rho, mu, e)
    f_damping = f_darcy * (pipe_length/pipe_diameter) * 0.5 * rho * v1**2 * A_b  # Newtons (N)
    return f_damping

# Equations of motion met dynamische gamma en relatieve demping
def system_dynamics(t, state):
    x1, v1, xa, va, P1 = state  # meters (m), meters per second (m/s), Pascals (Pa)
    Va = max(V_accu0 - A_a * xa, V_min)  # cubic meters (m³)
    
    # Initiële schatting van Pa
    Pa_initial = P_a0 * (V_accu0 / Va)**1.4  # Gebruik een standaard gamma als startpunt
    Pa_bar = Pa_initial / 1e5  # Converteer naar bar
    
    # Bereken gamma dynamisch
    gamma_current = gamma_interp_func(Pa_bar)
    
    # Herbereken Pa met de bijgewerkte gamma
    Pa = P_a0 * (V_accu0 / Va)**gamma_current  # Pascals (Pa)
    ka = (gamma_current * Pa * A_a**2) / Va  # Newtons per meter (N/m)
    
    Q_out = A_orifice * np.sqrt((2 * max(P1 - Pa, 0)) / rho_oil) * np.exp(-((P1 - Pa)**2) / (sigma**2)) if P1 > Pa else 0
    damping_force = pipe_damping(v1, pipe_length, pipe_diameter, rho_oil, mu, e, A_b)
    
    dx1_dt = v1
    dv1_dt = (F1 - P1 * A_b - damping_force - c1 * (v1 - va)) / m1  # Aangepast naar relatieve snelheid
    dva_dt = ((P1 - Pa) * A_a - ka * xa - c2 * va) - c1 * (va - v1)/ m2 if Va > V_min else 0
    dxa_dt = va
    dP1_dt = (oil_bulk_modulus / V_chamber1) * (A_b * v1 - A_a * va - Q_out)
    
    return [dx1_dt, dv1_dt, dxa_dt, dva_dt, dP1_dt]

# Initial conditions
initial_conditions = [-4.0, 0, 0, 0, P_a0]  # meters (m), meters per second (m/s), Pascals (Pa)
t_span = (0, 12)  # seconds (s)
t_eval = np.linspace(0, 12, 600)  # seconds (s)

# Solve ODE
solution = solve_ivp(system_dynamics, t_span, initial_conditions, t_eval=t_eval)

# Extract results
t = solution.t  # seconds (s)
x1 = solution.y[0]  # meters (m)
v1 = solution.y[1]  # meters per second (m/s)
xa = solution.y[2]  # meters (m)
va = solution.y[3]  # meters per second (m/s)
P1 = solution.y[4] / 1e5  # bar

# Bereken Pa dynamisch voor plotting
Pa = []
for i in range(len(xa)):
    Va = max(V_accu0 - A_a * xa[i], V_min)
    Pa_initial = P_a0 * (V_accu0 / Va)**1.4  # Initiële schatting
    Pa_bar = Pa_initial / 1e5
    gamma_current = gamma_interp_func(Pa_bar)
    Pa.append(P_a0 * (V_accu0 / Va)**gamma_current / 1e5)  # bar

# Plot results
plt.figure(figsize=(10, 6))
plt.plot(t, x1, 'r', linewidth=1.5, label='Hydraulic Cylinder Piston (x1)')
plt.xlabel('Time (s)')
plt.ylabel('Displacement (m)')
plt.title('Displacement of Hydraulic Cylinder Piston (x1)')
plt.legend()
plt.grid(True)
plt.show()

plt.figure(figsize=(10, 6))
plt.plot(t, xa, 'b', linewidth=1.5, label='Accumulator Piston (xa)')
plt.xlabel('Time (s)')
plt.ylabel('Displacement (m)')
plt.title('Displacement of Accumulator Piston (xa)')
plt.legend()
plt.grid(True)
plt.show()

plt.figure(figsize=(10, 6))
plt.plot(t, v1, 'r', linewidth=1.5, label='Cylinder Piston Velocity (v1)')
plt.xlabel('Time (s)')
plt.ylabel('Velocity (m/s)')
plt.title('Velocity of Hydraulic Cylinder Piston (v1)')
plt.legend()
plt.grid(True)
plt.show()

plt.figure(figsize=(10, 6))
plt.plot(t, va, 'b', linewidth=1.5, label='Accumulator Piston Velocity (va)')
plt.xlabel('Time (s)')
plt.ylabel('Velocity (m/s)')
plt.title('Velocity of Accumulator Piston (va)')
plt.legend()
plt.grid(True)
plt.show()

plt.figure(figsize=(10, 15))
plt.subplot(2, 1, 1)
plt.plot(t, P1, 'm', linewidth=1.5, label='Cylinder Pressure (P1)')
plt.xlabel('Time (s)')
plt.ylabel('Pressure (bar)')
plt.legend()
plt.grid(True)

plt.subplot(2, 1, 2)
plt.plot(t, Pa, 'c', linewidth=1.5, label='Accumulator Pressure (Pa)')
plt.xlabel('Time (s)')
plt.ylabel('Pressure (bar)')
plt.legend()
plt.grid(True)

plt.tight_layout()
plt.savefig('pressure_plots.png')