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

# %% Creating the loop transfer function:

Kp = 1
C = control.tf([Kp],[1])
P = control.tf([1], [1, 2, 1, 0])
L = control.series(C, P)

# %% 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)

# %% Calculating stability margins and crossover frequencies:

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

# %% Printing:

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

# %% Generating pdf file of the plotting figure:

plt.savefig('bode_with_stab_margins.pdf')