import numpy as np
import control
import matplotlib.pyplot as plt

# %% Creating the loop transfer function:

s = control.tf('s')

Kp = 1
C = Kp
P = 1/(s**3 + 2*s**2 + s)
L = C*P
L = control.minreal(L)  # To obtain minimum transf func 

# %% Frequencies:

w0 = 0.1
w1 = 10
dw = 0.001
nw = int((w1-w0)/dw) + 1 # Number of points of freq
w = np.linspace(w0, w1, nw)

# %% Plotting:

plt.close('all')
plt.figure(1, figsize=(12, 9))
(mag, phase_rad, w) = control.bode_plot(L, 
                                        w, 
                                        dB=True, 
                                        deg=True, 
                                        margins=True)
plt.grid()

# %% Calculating stability margins and crossover frequencies:

(GM, PM, wg, wp) = control.margin(L)

# %% Printing:

print(f'GM [1 (not dB)] = {GM:.2f}')
print(f'PM [deg] = {PM:.2f}')
print(f'wg [rad/s] = {wg:.2f}')
print(f'wp [rad/s] = {wp:.2f}')

# %% Generating pdf file of the plotting figure:

plt.savefig('bode_with_stab_margins.pdf')