###########################################################################
# Systemidentifiktaion einfacher Modelle
# M.Geissmann 14.3.2025
###########################################################################

import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import lti, lsim

# Lese Daten vom CSV File
path = 'static/download/PID_Systemidentifikation/'
file = 'Schrittversuch.csv'
data = np.genfromtxt(path+file, delimiter=';', skip_header=1)


Time = data[:,0] # Zeit in [s]
Ts = Time[1] - Time[0] # Abtastzeit
u = data[:,1] # Eingangssignal u(t)
y = data[:,2] # Ausgangssignal y(t)

# ## Plot
# fig, ax1 = plt.subplots()
# # Eingangssignal u(t)
# ax1.plot(Time, u,'b-')
# ax1.set_xlabel('Zeit [s]')
# ax1.set_ylabel('Eingang u(t)', color='b')
# # Ausgangssignal y(t)
# ax2 = ax1.twinx()  
# ax2.plot(Time, y,'r-')
# ax2.set_ylabel('Ausgang y(t)', color='r')
# plt.show()

## Preprocessing
y_init = y[0] # Anfangswert
y_fit = y - y_init

u_init = 0 # Arbeitspunkt
u_fit = u - u_init

#------------------------------------------------
# PT1
#------------------------------------------------
#           KP
# G = ----------------
#       Tau*s + 1
# Initialisieren von yk und X
yk = []
X = []

for k in range(1,len(Time)):
    yk.append(y_fit[k])
    X.append([-y_fit[k-1], u_fit[k-1]])

Theta = np.linalg.lstsq(X, yk, rcond=None)[0]
# Berechnung von Kp und Tau
Kp = Theta[1] / (Theta[0] + 1)
Tau = Ts / (Theta[0] + 1)

PT1 = lti([Kp], [Tau, 1])
yPT1 = lsim(PT1, u_fit, Time)

# # Plot
# plt.plot(Time, y, 'r-', label='y(t)')
# plt.plot(Time, yPT1[1]+y_init, 'b-', label='PT1')
# plt.xlabel('Zeit [s]')
# plt.ylabel('y(t)')
# plt.legend()
# plt.show()

#------------------------------------------------
# PT2
#------------------------------------------------
#           KP
# G = ----------------
#     A*s^2 + B*s + 1
yk = []
X = []
for k in range(2,len(Time)):
    yk.append(y_fit[k])
    X.append([-y_fit[k-1], -y_fit[k-2], u_fit[k-2]])
Theta = np.linalg.lstsq(X, yk, rcond=None)[0]
# Berechnung von Kp, A und B
Kp = Theta[2]/(Theta[0]+Theta[1]+1)
A = Ts**2/(Theta[0] + Theta[1] + 1)
B = (Ts*(Theta[0]+2))/(Theta[0]+Theta[1]+1)

PT2 = lti([Kp], [A, B, 1])
yPT2 = lsim(PT2, u_fit, Time)

# # Plot
# plt.plot(Time, y, 'r-', label='y(t)')
# plt.plot(Time, yPT2[1]+y_init, 'b-', label='PT2')
# plt.xlabel('Zeit [s]')
# plt.ylabel('y(t)')
# plt.legend()
# plt.show()

#------------------------------------------------
# ARX
#------------------------------------------------
# Siehe Matlab Script