import numpy as np
from matplotlib import pyplot as plt
from scipy.signal import tf2ss


def PID(ss,dt,T,k,Ti=None,Td=None):

    A,B,C,D = ss
    t = np.arange(0,T,dt)
    n = len(t)
    count_of_parameters = A.shape[0]
    xs = np.zeros((count_of_parameters,n+1))
    ys = np.zeros((1,n))
    set_point = 20
    prev_err,err = set_point,set_point
    acc_I = 0

    for i in range(0,n):
        P = k * err
        I = (k/Ti)*err + acc_I if Ti is not None else 0
        regD = k*Td * (err - prev_err) if Td is not None else 0

        u = P + I + regD

        x = np.transpose(np.dot(A,xs[:,i].reshape(count_of_parameters,1)) + B*u)
        xs[:,i+1] = xs[:,i] + x*dt
        y = np.dot(C,xs[:,i].reshape(count_of_parameters,1)) + D*u
        ys[0,i] = set_point - y

        prev_err,err = err,set_point - y
        acc_I = I

    return t, ys[0,:]

def Q(ss,dt,T,k,Ti=None,Td=None):

    A,B,C,D = ss
    t = np.arange(0,T,dt)
    n = len(t)
    count_of_parameters = A.shape[0]
    xs = np.zeros((count_of_parameters,n+1))
    ys = np.zeros((1,n))
    es = np.zeros((1,n))
    set_point = 20
    prev_err,err = set_point,set_point
    acc_I = 0

    for i in range(0,n):
        P = k*err
        I = (k/Ti)*err + acc_I if Ti is not None else 0
        regD = k*Td*(err - prev_err) if Td is not None else 0

        u = P + I + regD

        x = np.transpose(np.dot(A,xs[:,i].reshape(count_of_parameters,1)) + B*u)
        xs[:,i+1] = xs[:,i] + x*dt
        y = np.dot(C,xs[:,i].reshape(count_of_parameters,1)) + D*u
        ys[0,i] = set_point - y

        prev_err,err = err, set_point-y
        es[0,i] = err
        acc_I = I

    es = es - es[0,n-1]
    es = es ** 2
    es = es * dt

    return np.sum(es)


def Qk(ss,dt,T,Ti=None,Td=None):
    ks = np.arange(0,8,0.2)
    Qs = np.zeros((1,ks.size))

    for i,k in enumerate(ks):
        Qs[0,i] = Q(ss=ss,dt=dt,T=T,k=k,Ti=Ti,Td=Td)

    return ks,Qs[0,:]

def QTi(ss,dt,T,k,Td=None):
    Tis = np.arange(100,700,10)
    Qs = np.zeros((1,Tis.size))

    for i, Ti in enumerate(Tis):
        Qs[0,i] = Q(ss=ss,dt=dt,T=T,k=k,Ti=Ti,Td=Td)

    return Tis,Qs[0,:]

def QTd(ss,dt,T,k,Ti=None):
    Tds = np.arange(0.0001,0.08,0.0008)
    Qs = np.zeros((1,Tds.size))

    for i,Td in enumerate(Tds):
        Qs[0,i] = Q(ss=ss,dt=dt,T=T,k=k,Ti=Ti,Td=Td)

    return Tds,Qs[0,:]

if __name__ == "__main__" :

    num = [2]
    den = [1,5,4,3]
    ss = tf2ss(num,den)

    # num = [2]
    # den = [1, 5, 4, 3]
    # ss = tf2ss(num, den)

    # num1 = [2]
    # den1 = [1,5,4,3]
    # ss1 = tf2ss(num1,den1)
    #
    # num2 = [2]
    # den2 = [2, 5, 4, 3]
    # ss2 = tf2ss(num2, den2)
    #
    # num3 = [2]
    # den3 = [3,5,4,3]
    # ss3 = tf2ss(num3,den3)


    ### WYKRES DLA MICHALA

    # plt.title("Wykres dla kolegi")
    # plt.plot(*Qk(ss=ss,dt=0.05,T=50),"*")
    # plt.grid()
    # plt.show()

    ### ZABAWA Z ZIEGLEREM NICHOLSEM
    # plt.title("Szukamy oscylacji")
    # plt.plot(*PID(ss=ss,dt=0.005,T=50,k=4.01, Ti=1.35, Td=1.925))
    # plt.show()

    ### WYKRES 0
    # plt.title("Porownanie wykresow dla m = 1 (czerwony) 2 (niebieski) 3 (zielony)")
    # plt.plot(*PID(ss=ss1,dt=0.005,T=100,k=0.5),"r")
    # plt.plot(*PID(ss=ss2, dt=0.005, T=100, k=0.5), "b")
    # plt.plot(*PID(ss=ss3, dt=0.005, T=100, k=0.5), "g")
    # plt.show()

    ### WYKRES 1
    # plt.title("Regulator P: k = 0.5 (czerwony), 2 (zielony), 5 (niebieski)")
    # plt.grid()
    # plt.xlabel('czas (s) →')
    # plt.ylabel('uchyb →')
    # plt.plot(*PID(ss=ss,dt=0.005,T=100,k=0.5),"r")
    # plt.plot(*PID(ss=ss, dt=0.005, T=100, k=2), "g")
    # plt.plot(*PID(ss=ss, dt=0.005, T=100, k=5), "b")
    # plt.show()

    ###  WYKRES 2
    # plt.title("Regulator P: k = 7 (czerwony), 8 (zielony), 8.4 (niebieski)")
    # plt.grid()
    # plt.xlabel('czas (s) →')
    # plt.ylabel('uchyb →')
    # plt.plot(*PID(ss=ss, dt=0.005, T=100, k=7), "r")
    # plt.plot(*PID(ss=ss, dt=0.005, T=100, k=8), "g")
    # plt.plot(*PID(ss=ss, dt=0.005, T=100, k=8.4), "b")
    # plt.show()

    ### WYKRES 3
    # plt.title("Regulator PI: k = 0.5 Ti = 70 (czerwony), 80 (zielony), 100 (niebieski)")
    # plt.grid()
    # plt.xlabel('czas (s) →')
    # plt.ylabel('uchyb →')
    # plt.plot(*PID(ss=ss, dt=0.005, T=50, k=0.5, Ti=70), "r")
    # plt.plot(*PID(ss=ss, dt=0.005, T=50, k=0.5, Ti=80), "g")
    # plt.plot(*PID(ss=ss, dt=0.005, T=50, k=0.5, Ti=100), "b")
    # plt.show()

    ### WYKRES 4
    # plt.title("Regulator PI: k = 0.5 Ti = 100 (czerwony), 150 (zielony), 200 (niebieski), 500 (fioletowy)")
    # plt.grid()
    # plt.xlabel('czas (s) →')
    # plt.ylabel('uchyb →')
    # plt.plot(*PID(ss=ss, dt=0.005, T=50, k=0.5, Ti=100), "r")
    # plt.plot(*PID(ss=ss, dt=0.005, T=50, k=0.5, Ti=150), "g")
    # plt.plot(*PID(ss=ss, dt=0.005, T=50, k=0.5, Ti=200), "b")
    # plt.plot(*PID(ss=ss, dt=0.005, T=50, k=0.5, Ti=500), "purple")
    # plt.show()

    ### WYKRES 5
    # plt.title("Regulator PI: k = 0.5 Ti = 150, Td = 0.0001")
    # plt.grid()
    # plt.xlabel('czas (s) →')
    # plt.ylabel('uchyb →')
    # plt.plot(*PID(ss=ss, dt=0.005, T=50, k=0.5, Ti=150,Td = 0.002), "r")
    # plt.show()

    ### WYKRES 6
    # plt.title("Dobór parametrów regulatora P")
    # plt.grid()
    # plt.xlabel('k →')
    # plt.ylabel('Q(fk) →')
    # plt.plot(*Qk(ss=ss, dt=0.005, T=50), "*")
    # plt.show()

    ### WYKRES 7
    # plt.title("Dobór parametrów regulatora PI przy k=2")
    # plt.grid()
    # plt.xlabel('k →')
    # plt.ylabel('Q(fk) →')
    # plt.plot(*QTi(ss=ss, dt=0.005, T=50,k=2), "*")
    # plt.show()

    ### WYKRES 8
    # plt.title("Dobór parametrów regulatora PI przy k=0.5")
    # plt.grid()
    # plt.xlabel('k →')
    # plt.ylabel('Q(fk) →')
    # plt.plot(*QTi(ss=ss, dt=0.005, T=50, k=0.5), "*")
    # plt.show()

    ### WYKRES 9
    plt.title("PID dla k=0.5, Ti=150, Td= 0.001 (niebieski) 200 (czerwony)")
    plt.grid()
    plt.xlabel('czas')
    plt.ylabel('uchyb')
    plt.plot(*PID(ss=ss, dt=0.005, T=50, k=0.5,Ti=150, Td=0.001), "b")
    plt.plot(*PID(ss=ss, dt=0.005, T=50, k=0.5, Ti=150, Td=200), "r")
    plt.show()