import matplotlib
import numpy as np
import matplotlib.pyplot as plt
matplotlib.use("Agg")


# 1) PARAMETRE SYSTEMU

m = 1000            # kg
b = 50              # odpor
w = 27.7            # pozadovana rychlost (m/s)
kappa = 0.3         # PREKMIT (0 = bez prekmitu, 0.2 = 20%, ...)
sim_time = 200      # dlzka simulacie (s)


# 2) TABULKY PRE α A β PODLA PREKMITU (DMM)

kappa_table = np.array([0, 0.05, 0.10, 0.15, 0.20, 0.25, 0.30, 0.35, 0.40, 0.45, 0.50])
alpha_table = np.array([1.282, 0.984, 0.884, 0.832, 0.763, 0.697, 0.669, 0.640, 0.618, 0.599, 0.577])
beta_table  = np.array([2.718, 1.944, 1.720, 1.561, 1.437, 1.337, 1.248, 1.172, 1.104, 1.045, 0.992])

# Podla kappa indexu, vyber α a β na rovnakom indexe
alpha = np.interp(kappa, kappa_table, alpha_table)
beta  = np.interp(kappa, kappa_table, beta_table)

print(f"Interpolované hodnoty: α = {alpha:.3f}, β = {beta:.3f}")



# 3) MODELOVANY SYSTEM (FOPDT - (K / (T*s + 1)) * e^(-D*s))

# 1 / (m*s+b) ==> Gs(s)fopdt = (1/b) / (m/b)s + 1

K = 1 / b              # staticke zosilnenie
T = m / b              # casova konstanta
D = 0                  # dopravne oneskorenie (ignorujem)

print(f"Model systému: K = {K:.4f}, T = {T:.3f} s")



# 4) PI REGULATOR DMM (desired model method)

# Ziadany model DMM: G_dm(s) = α / (s(β T s + 1))
# Regulator PI: Gr(s) = Kp (1 + 1/(Ti s))

Kp = (alpha * T) / (K * beta * T) + 200
Ti = beta * T - 10

print(f"Navrhnutý PI regulátor:\n   Kp = {Kp:.3f}\n   Ti = {Ti:.3f}")



# 5) SIMULACIA – numericke riesenie ODE

dt = 0.01
steps = int(sim_time / dt)

y = 0.0          # rychlost
u = 0.0          # akcna velicina
integral = 0.0   # suma

time = []
vel = []
control = []

for k in range(steps):
    t = k * dt      # casova os
    e = w - y       # chyba tempomatu

    # PI regulator
    integral += e * dt                      # aproximuje integral (sum of e*dt)
    u  = Kp * (e + (1 / Ti) * integral)     # normovana forma

    # vykon / plnovy pedal
    u = np.clip(u, 0, 5000)   # max sila motora

    # simulacia poruchy 'd'
    if 60 < t < 110:        # v case 60 az 110 na casovej osi nastane:
        d = -500            # brzdenie vetrom / kopec
    else:
        d = 0

    # model auta: m dv/dt = -b v + u + d
    dydt = (-b * y + u + d) / m         # dydt je okamzite zrychlenie
    y += dydt * dt                      # zmena rychlosti

    # logovanie
    time.append(t)
    vel.append(y)
    control.append(u)



# 6) GRAFY

plt.figure(figsize=(10,6))
plt.plot(time, vel, label="Rýchlosť auta")
plt.axhline(w, color="black", linestyle="--", label="Požadovaná rýchlosť")
plt.xlabel("Čas [s]")
plt.ylabel("Rýchlosť [m/s]")
plt.grid(True)
plt.legend()
plt.title("Simulácia tempomatu – PI regulátor (DMM)")
plt.savefig("speed.png")
plt.close()

plt.figure(figsize=(10,5))
plt.plot(time, control, label="Akčná veličina u(t)")
plt.xlabel("Čas [s]")
plt.ylabel("Výkon auta")
plt.grid(True)
plt.legend()
plt.title("Ovládanie – výstup PI regulátora")
plt.savefig("control.png")
plt.close()