function y = fcn(u, data, data1)
%% 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('lqi');
coder.extrinsic('ss');

%% Sygnaly wpadajace do funkcji
x1 = u(1);
x2 = u(2);
x3 = u(3);
x4 = u(4);
x5 = u(5);
x6 = u(6);
xi1 = u(7);
xi2 = u(8);
xi3 = u(9);

if x4 == 0 && x5 == 0
    x4 = 10e-10;
    x5 = 10e-10;
end

%% 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 to strzowenia 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];

%% Macierze dla LQI
% podane były podczas zajęć na tablicy
Ar = [A zeros(6,3);
    -C zeros(3,3)];
Br = [B;
    zeros(3,3)];
Cr = [C zeros(3,3)];
Dr = 0;

%% Obiekt/model
% xi = [xi1, xi2, xi3];
% x = [x1, x2, x3, x4, x5, x6];
xi = [xi1; xi2; xi3];
x = [x1; x2; x3; x4; x5; x6];
Ml = 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
Mr = ss(Ar,Br,Cr,Dr);
%% Bieguny
PSI = eye(9,9);
FI = eye(3,3);
F = zeros(3,9);
[F,P,p] = lqi(Mr,PSI,FI);

%% Wyjscie funkcji
% dodanie macierzy F do naszego obiektu
y = zeros(data,data1);
y = ([x;xi].*-F');