def Gstable(G,poles):
    Gmul = 1
    for p in poles:
        Gmul *= (s - p)/(s + p)
    Gs = Gmul*G
    num = Gs.numerator.c
    den = Gs.denominator.c
    sym_num = symbolic_poly(num)
    sym_den = symbolic_poly(den)
    fraction = (sym_num/sym_den).simplify()
    numer, denom = fraction.as_numer_denom()
    if numer.find('s'):
        num_coeff  = ceoff_symbolic_poly(numer)
    else:
        num_coeff = as_int(numer)
    if denom.find('s'):
        den_coeff  = ceoff_symbolic_poly(denom)
    else:
        den_coeff = denom
    return tf(num_coeff,den_coeff)

import matplotlib.pyplot as plt

from utils import tf
from utilsplot import bode

G = tf([5], [10, 1], deadtime=2)

plt.figure('Figure 2.2')
plt.title('Bode plots of G(s)')
bode(G, -3, 1)
plt.show()

import numpy as np
import matplotlib.pyplot as plt

from utilsplot import step_response_plot
from utils import feedback, tf


s = tf([1, 0], 1)

Kd = 0.5

G = 5/((10*s + 1)*(s - 1))
Gd = Kd/((s + 1)*(0.2*s + 1))
K = 0.04/s * ((10*s + 1)**2)/((0.1*s + 1)**2)

L = G * K

# Transfer function between disturbance and output y
S = feedback(1, L)*Gd

# Transfer function between disturbance and controller input u
Gu = -S*K
    
plt.figure('Figure 5.16 (a)')

plt.subplot(1, 2, 1)
w = np.logspace(-2, 1, 1000)
wi = w*1j 
plt.loglog(w, np.abs(G(wi)))
plt.loglog(w, np.abs(Gd(wi)))
plt.axhline(1, color='black', linestyle=':')

import matplotlib.pyplot as plt
from utils import phase, tf, feedback

# Loop shaping is an iterative procedure where the designer
# 1. shapes and reshapes |L(jw)| after computing PM and GM,
# 2. the peaks of closed loop frequency responses (Mt and Ms),
# 3. selected closed-loop time responses,
# 4. the magnitude of the input signal
#
# 1 to 4 are the important frequency domain measures used to assess
#   perfomance and characterise speed of response

w = np.logspace(-2, 1, 1000)
wi = w*1j

s = tf([1, 0])

Kc = 0.05
#plant model
G = 3*(-2*s+1)/((10*s+1)*(5*s+1))
#Controller model
K = Kc*(10*s+1)*(5*s+1)/(s*(2*s+1)*(0.33*s+1))
#closed-loop transfer function
L = G*K

#magnitude and phase
plt.subplot(2, 1, 1)
plt.loglog(w, abs(L(wi)))
plt.loglog(w, np.ones_like(w))
plt.ylabel('Magnitude')

"""
from __future__ import print_function
from utils import tf, feedback, tf_step

"""
This function aims to be the Python equivalent of the MatLab connect function
Reference:
http://www.mathworks.com/help/control/examples/connecting-models.html

The example used here is the same as in the reference
"""

# First example in reference to demonstrate working of tf object

# Define s as tf to enable its use as a variable
s = tf([1, 0])

# Define the transfer functions
F = tf(1, [1, 1])
G = tf(100, [1, 5, 100])
C = 20*(s**2 + s + 60) / s / (s**2 + 40*s + 400)
S = tf(10, [1, 10])

T = F * feedback(G*C, S)

# This is the same figure as in the reference

tf_step(T, 6)

def step(G, t_end=100, initial_val=0, input_label=None, output_label=None, points=1000):
    """
    This function is similar to the MatLab step function.

    Parameters
    ----------
    G : tf
        Plant transfer function.
    t_end : integer
        Time period which the step response should occur (optional).
    initial_val : integer
        Starting value to evaluate step response (optional).
    input_label : array
        List of input variable labels.
    output_label : array
        List of output variable labels.

    Returns
    -------
    Plot : matplotlib figure
    """

    plt.gcf().set_facecolor('white')

    rows = numpy.shape(G(0))[0]
    columns = numpy.shape(G(0))[1]

    s = utils.tf([1, 0])
    system = G(s)

    if (input_label is None) and (output_label is None):
        labels = False
    elif (numpy.shape(input_label)[0] == columns) and (numpy.shape(output_label)[0] == rows):
        labels = True
    else:
        raise ValueError('Label count is inconsistent to plant size')

    fig = adjust_spine('Time', 'Output magnitude', -0.05, 0.1, 0.8, 0.9)

    cnt = 0
    tspace = numpy.linspace(0, t_end, points)
    for i in range(rows):
        for j in range(columns):
            cnt += 1
            nulspace = numpy.zeros(points)
            ax = fig.add_subplot(rows + 1, columns, cnt)
            tf = system[i, j]
            if all(tf.numerator) != 0:
                realstep = numpy.real(tf.step(initial_val, tspace))
                ax.plot(realstep[0], realstep[1])
            else:
                ax.plot(tspace, nulspace)
            if labels:
                ax.set_title('Output ({}) vs. Input ({})'.format(output_label[i], input_label[j]))
            else:
                ax.set_title('Output {} vs. Input {}'.format(i + 1, j + 1))

            if i == 0:
                xax = ax
            else:
                ax.sharex = xax

            if j == 0:
                yax = ax
                ax.set_ylabel = output_label[j]
            else:
                ax.sharey =yax

            plt.setp(ax.get_yticklabels(), fontsize=10)
            box = ax.get_position()
            ax.set_position([box.x0, box.y0 * 1.05 - 0.05,
                             box.width * 0.8, box.height * 0.9])

"""
Created on Tue May 26 21:05:19 2015

@author: cronjej
"""

import utils
import utilsplot
import numpy as np
import matplotlib.pyplot as plt
import matplotlib as mpl

mpl.rcParams['figure.facecolor'] = 'white'


s = utils.tf([1, 0], 1)
A = np.asarray([[-10, 0], [0, -1]])
B = np.eye(A.shape[0])
C = np.asarray([[10., 1.1], [10., 0]])
D = np.asarray([[0., 0.], [0., 1.]])

G = utils.mimotf(C*utils.mimotf(s*np.eye(2) - A).inverse()*B + D)

# a) Controllability analysis

G_poles = G.poles()
G_rhp_poles = utils.RHPonly(G_poles)
# G has stable poles, -10 with multiplicity of 2 and -1
G_zeros = G.zeros()
G_rhp_zeros = utils.RHPonly(G_zeros)
# G has a LHP zero @ -10 and a RHP zero @ 0.1

import numpy as np
import matplotlib.pyplot as plt
from utils import tf

WP = tf([0.25, 0.1], [1, 0])
WU = tf([1, 0], [1, 1])
GK1 = tf([0, 0.5], [1, 0])
GK2 = tf([0, -0.5, 0.5], [1, 1, 0])

w = np.logspace(-2, 2, 500)
s = 1j*w
ru = np.linspace(0, 1, 10)

"""Solution to part a,b is on pg 284, only part c is illustrateed here"""
plt.loglog(w, np.abs(1/(1 + GK1(s))), label='$S_1$')
plt.loglog(w, np.abs(1/(1 + GK2(s))), label='$S_2$')

for i in range(np.size(ru)):
    if i == 1:
        plt.loglog(w, 1/(np.abs(WP(s)) + np.abs(ru[i]*WU(s))), 'k--', label='varying $r_u$')
    else:
        plt.loglog(w, 1/(np.abs(WP(s)) + np.abs(ru[i]*WU(s))), 'k--')

plt.loglog(w, 1/(np.abs(WP(s)) + np.abs(0.75*WU(s))), 'r', label='$r_u$ = 0.75')
plt.legend(loc=2)
plt.title(r'Figure 7.19')
plt.xlabel(r'Frequency [rad/s]', fontsize=14)
plt.ylabel(r'Magnitude', fontsize=15)
plt.show()

from utils import tf, mimotf, poles, zeros


s = tf([1,0],[1])
T1 = (2*s + 1)/(s**2 + 0.8*s + 1)
T2 = (-2*s + 1)/(s**2 + 0.8*s + 1)

S1 = 1 - T1
S2 = 1 - T2

print("S1 contains RHP zero %.2f, therefore L1 contains a RHP pole at %.2f" %(S1.zeros()[0],S1.zeros()[0]))
print("T2 contains RHP zero %.2f, therefore L2 contains the same RHP zero %.2f" %(T2.zeros(),T2.zeros()))

import matplotlib.pyplot as plt
from math import pi
from utils import tf

def LI(rk, tmax, w):
    L = np.zeros(np.size(w))
    for i in range(np.size(w)):
        if w[i-1] < pi/tmax:
            L[i-1] = np.sqrt(rk**2 + 2*(1+rk)*(1-np.cos(tmax*w[i-1])))
        else:
            L[i-1] = 2 + rk
    return L

rk = 0.2
tmax = 1

WI = tf([(1 + rk*0.5)*tmax, rk], [tmax*0.5, 1])
WI_improved = WI*tf([(tmax/2.363)**2, 2*0.838*(tmax/2.363), 1], [(tmax/2.363)**2, 2*0.685*(tmax/2.363), 1])

w = np.logspace(-2, 2, 500)
s = 1j*w

plt.loglog(w, LI(rk, tmax, w), 'r', label='$l_I$')
plt.loglog(w, np.abs(WI(s)), 'b--', label='$\omega_I$')
plt.loglog(w, np.abs(WI_improved(s)), color = 'lime', label='$improved$ $\omega_I$')
plt.legend()
plt.title(r'Figure 7.9')
plt.xlabel(r'Frequency [rad/s]', fontsize=12)
plt.ylabel(r'Magnitude', fontsize=12)
plt.show()

import numpy as np
import scipy as sc
import scipy.signal as scs
import matplotlib.pyplot as plt
from utils import phase, tf

G = tf(8, [1, 8]) * tf(1, [1, 1])

# freq(G) returns a frequency response function given a Laplace function

def freq(G):
    def Gw(w):
        return G(1j*w)
    return Gw

def margins(G):
    """ Calculate the gain margin and phase margin of a system.

    Input: G - a function of s
    Outputs:
       GM    Gain margin
       PM    Phase margin
       wc    Gain crossover frequency
       w_180 Phase Crossover frequency
    """

    Gw = freq(G)

    def mod(x):
        """to give the function to calculate |G(jw)| = 1"""
        return np.abs(Gw(x)) - 1

import utils
import numpy as np
import sympy as sp
from scipy.optimize import fsolve

s = utils.tf([1,0],[1])
L = (-s + 2)/(s*(s + 2))

_,_,_,w180 = utils.margins(L)
GM = 1/np.abs(L(1j*w180))
print('w_180',w180)
print('GM = 1/|L(w180)|',GM)
print('From 7.55, kmax = GM = ',GM)

omega = np.logspace(-2,2,1000)

def rk(kmax):
    return (kmax - 1)/(kmax + 1)

def kbar(kmax):
    return (kmax + 1)/2

def RScondition(kmax):
    abs_ineq = [abs(rk(kmax) * kbar(kmax) * L(1j*s)/(1 + kbar(kmax)*L(1j*s))) for s in omega]
    max_ineq = max(abs_ineq) - 1
    return max_ineq

kcal = fsolve(RScondition,1)
print('From 7.58, kmax = ',np.round(kcal,2))

def add_deadtime_SISO(G, deadtime):
    G = (tf(list(G.numerator.coeffs),
            list(G.denominator.coeffs), deadtime=deadtime))
    return G