%%%%%%%%%
% Created by Adrian Bury & Jacek Kusibab
% Autor nie ponosi żadnej odpowiedzialności za ewentualne błędy
%%%%%%%%%

clear all
clc

%Jak chcesz na symbolach operować odkomentuj to, a zakomentuj dane
% syms M m g  l s p t
% symbolic = true;

syms s
g = 9.81;
t = 3;
p = 9;
M = 0.2;
m = 0.35;
l = 0.75;
symbolic = false;

J = 4*m*l^2 / 3


A = [
    0   1   0   0
    0   0   ( m ^2* l ^2* g )/(( M + m )*( m * l ^2 + J)- m ^2* l ^2)    0
    0   0   0   1
    0   0   ( m ^3* l ^3* g )/(( M + m )*( m * l ^2 + J)^2- m ^2* l ^2*( m * l ^2 +J)) + ( m * l * g /( m * l ^2+ J ))    0
    ];

B = [
    0
    ( m * l ^2+J)/(( M + m )*( m * l ^2+J)-  m ^2* l ^2)
    0
    ( m * l *( m * l ^2+J))/(( M + m )*( m * l ^2+J)^2-  m ^2* l ^2*( m * l ^2+J))

    ];

C = [1 0 0 0];

if(symbolic)
    A = simplify (A)
    B = simplify(B)
end

I=eye([4 4]);
Gs = C*(s*I-A)^(-1)*B
if(symbolic)
    Gs = simplify(Gs)
end

S = [B A*B A*A*B A*A*A*B];

if(symbolic)
    S=simplify(S)
end


As=[
    0   1   0   0
    0   0   1   0
    0   0   0   1
    0   0   (3*g*(M+m))/(l*(4*M+m)) 0
    ]

Bs=[
    0
    0
    0
    1
    ]

Ss = [Bs As*Bs As*As*Bs As*As*As*Bs]
Ps_1=Ss*(S^-1);
Ps=Ps_1^-1;

if(symbolic)
    Ps = simplify(Ps);
end


beta = sqrt( (log(p))^2/((log(p))^2+pi*pi))
fi = acos(beta)
sigma = 4.5 / t
wd = tan(fi)*sigma

s1 = -sigma + wd*j
s2 = -sigma - wd*j
s3 = -5*sigma
s4 = -5*sigma

c0 = s1*s2*s3*s4
c1 = (-s1*s2*s3-s1*s2*s4-s1*s3*s4 - s2*s3*s4)
c2 = (s1*s2+s1*s3 + s1*s4 + s2*s3 + s2*s4 + s3*s4)
c3 = -s1-s2-s3-s4

ks1 = c0;
ks2 = c1;
ks3 = c2 + (3*g*(M+m))/(l*(4*M + m ));
ks4 = c3;

Ks = [ ks1   ks2     ks3     ks4 ]

Cs=C*Ps
if(symbolic)
    simplify(Cs)
end

Aks=As-Bs*Ks
if(symbolic)
    simplify(Aks)
end


K=Ks*Ps_1


Ak=A-B*K

det(s*I - Ak)