% Input:
% A: Matrix (from continuous dynamics)                             
% delta: double (timestep)

% Output:
% phi1,phi2,eAd: Matrix

% Desription:
% computes Phi1, Phi2 and expm(A*delta) (for algoSpaceEx)

function [phi1,phi2,eAd] = calcphis(A,delta)

if size(A,1) ~= size(A,2)
    fprintf('Matrix A has to be quadratic !');
    pause();
end;

temp = zeros(size(A,1)*3,size(A,1)*3);

Ad = A*delta;

% copy Ad to temp
for i=1:1:size(A,1)
    for j=1:1:size(A,2)
        temp(i,j) = Ad(i,j);
    end;
end;

Id = eye(size(A,1),size(A,1))*delta;

%copy Id to temp
for i=1:1:size(A,1)
    
    for j=1:1:size(A,1)
        temp(i,size(A,1)+j) = Id(i,j);
        temp(size(A,1)+i,2*size(A,1)+j) = Id(i,j);
    end;
end;

expmtemp = expm(temp);

%extract Phis
phi1 = expmtemp(1:size(A,1),size(A,1)+1:size(A,1)*2);
phi2 = expmtemp(1:size(A,1),2*size(A,1)+1:size(A,1)*3);
eAd = expmtemp(1:size(A,1),1:size(A,1));
