function []=plotresults(t,X)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                  %
% function []=plotresults(t,X)     %
% plotresults.m                    %
%                                  %
% Plots trajectories               %
% and thrust directions.           %
%                                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global c1 c2 c3

r  = X(:,1);
x2 = X(:,2);
vt = X(:,3);
l1 = X(:,4);
l2 = X(:,5);
l3 = X(:,6);

% Calculate the polar angle phi
fip=vt./r;              
dt=[0; diff(t)];
dfi=dt.*fip;
fi =cumsum(dfi);
fi=c3*fi;               % Variables are normalized

hold on
plot(r.*cos(fi),r.*sin(fi));                    % trajectory
plot(r(1)*cos(fi(1)),r(1)*sin(fi(1)),'o');      % startpoint
plot(0,0,'x');                                  % gravity center
axis('equal')

ffi = 0:0.01:(2*pi+0.1);
% Plot inner circle
plot(cos(ffi),sin(ffi),'r');                    
% Plot outer circle
plot(X(length(X),1)*cos(ffi),X(length(X),1)*sin(ffi),'b');     

% Thrust angle u(t)
for i = 1:length(l2)
normen=sqrt(l2(i).^2+(c3.*l3(i)).^2);
if normen ~= 0
su(i)=-l2(i)./normen;
cu(i)=-c3.*l3(i)./normen;
else
su(i) = 0;
cu(i) = 1;
end
end

u=atan2(su,cu);
vecl=0.1;             
for i=1:1:length(r)
    plot([r(i)*cos(fi(i)) r(i)*cos(fi(i))+vecl*cos(fi(i)+pi/2-u(i))],[r(i)*sin(fi(i)) r(i)*sin(fi(i))+vecl*sin(fi(i)+pi/2-u(i))],'c')
end