function [px,py,psi,delta,E,J,lambda] = mpccar(N,kmax,P,Q,R,x0)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% - Bemenetek:
%      N: horizont hossza               (skalár)
%   kmax: horizontok száma              (skalár)
%  P,Q,R: súlyozó mátrixok              (P és Q 4*4-es mátrix, R skalár)
%     x0: első horizont kezdeti értéke  (4*1 vektor)
%
% - Kimenetek:
%     px: x pozíció inercia rendszerben (kmax*1 vektor)
%     py: y pozíció inercia rendszerben (kmax*1 vektor)
%    psi: orientáció                    (kmax*1 vektor)
%  delta: kormányzási szög              (kmax*1 vektor)
%      E: hiba-állapotvektor            (kmax*4 mátrix)
%      J: költség                       (kmax*1 vektor)
% lambda: sajátértékek                  (kmax*4 mátrix)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

params;
m = size(Q,1);

% 1) Diszkrétidejű szakasz:
A = Acar;
B = Bcar;

% 2) F, G, Rh, Qh mátrixok:
F = [];
G = [];
Rh = [];
Qh = [];
for i = 1:N
	F = [F; A^i];
    Grow = [];
    for j = i-1:-1:0
        Grow = [Grow A^j*B];
    end
    for j = 1:N-i
        Grow = [Grow zeros(size(B))];
    end
    G = [G; Grow];
    Rh = blkdiag(Rh, R); %repmat([Rh R]);
    Qh = blkdiag(Qh, Q); %repmat([Qh Q]);
end

% 3) Gamma, Phi, Psi mátrixok és KN vektor:
Psi = F'*Qh*F + Q;
Gamma = G'*Qh*G + Rh;
Phi = G'*Qh*F;
KN = [eye(m) zeros(m, (N-1)*m)] * Psi\Phi;

% 4) MPC algoritmus:
for k = 1:kmax

    % 4a) Optimális beavatkozó jel (vektor)
    [uopt,~,~,~] = ...
        quadprog(2*Gamma,2*Phi);
    % 4b) Zárt kör sajátértékei (A - B*KN)
    [V,D] = eig(A-B*KN);
    % 4c) Aktuális költség
    % 4d) Állapotok mentése és frissítése (első beavatkozás alkalmazása)

end

% 5) Kimenetek
% px = ??
% py = ??
% pyi = ??
% delta = ??

end