function y = fcn(u)

%% Zdefiniowanie zwenetrznych funkcji, ktorych ten blok natwynie nie obsluguje
% wynika to z faktu, ze nie sa to funkcje codera simulinka, z tego powodu
% wykonaywane sa one z poziomu silnika Matlaba
coder.extrinsic('lqry');
coder.extrinsic('ss');

%% Sygnały wpadające do funkcji
x1 = u(3);
x2 = u(4);
x3 = u(5);
x4 = u(6);
x5 = u(7);
x6 = u(8);

%% Parametry wykorzystwane w równaniach stanu
% potrzebne do macierzy powstalych po linearyzacji
kzz = 0.25;
a = kzz^2 + 0.0431;
a4 = 0.132;
b4 = 0.958;
k4 = b4;
c4 = 0.088;
a5 = 1.402;
a6 = 2.443;
b5 = 0.544;
k5 = b5;
b6 = 7.235;
d5 = 0.037;
d6 = 1.534;
k6 = 9.470;

%% Macierze powstale po linearyzacji
% sa one niezbedne do strzowenia macierzy modelu i wyznacznia macierzy F dla lokowania biegunow
A = [0 0 -x5*cos(x3)-x4*sin(x3) cos(x3) -sin(x3) 0;
    0 0 x4*cos(x3)-x5*sin(x3) sin(x3) cos(x3) 0;
    0 0 0 0 0 1;
    0 0 0 -a4*sqrt(x4^2+x5^2)-(a4*x4^2)/sqrt(x4^2+x5^2) -((a*x4*x5)/sqrt(x4^2+x5^2))+b4*x6+c4*2*x5 b4*x5;
    0 0 0 (-a5*x4*x5/sqrt(x4^2+x5^2))-(b5*x6)+((x5^3*x4)/((sqrt(x4^2+x5^2)*(x4^2+x5^2)))) (-a5*(2*x5^2+x4^2)/sqrt(x4^2+x5^2))-(((3*x5^2 * x4^2)+2*x5^4)/((sqrt(x4^2+x5^2)*(x4^2+x5^2)))) -b5*x4+2*x6*d5;
    0 0 0 ((a6*x4*x5)/sqrt(x4^2+x5^2))-b6*x5 (-a6*(2*x5^2+x4^2)/sqrt(x4^2+x5^2))-b6*x4 -d6*2*x6];
B = [0 0 0; 0 0 0; 0 0 0; k4 0 0; 0 k5 0; 0 0 k6];
C = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0];
D = [0 0 0; 0 0 0; 0 0 0];

%% Obiekt/model
xn = [x1, x2, x3, x4, x5, x6];
y = zeros(1,3);
M = ss(A,B,C,D); % stworzy obiekt w przestrzeni stanu, omija to tworzenie obiektu/modelu na pojedynczych blokach Gain itp. w simulinku tak jak na zajeciach z SOIM

%% Bieguny
PSI = eye(3,3);
FI = eye(3,3);
[F,P,p] = lqry(M,PSI,FI);

%% Wyjscie funkcji
% dodanie macierzy F do naszego obiektu

y = xn*F';