%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Movement along the surface of a cone
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

m=0.1; % (kg) object mass
w0=1; % (rad/s) initial angular velocity of the projection in the horizon plane

% Initial conditions
x10=0;   % theta (rad)
x20=1;   % r (m)
x30=0;   % dr/dt (m/s)

% Constants
g=9.8; % (m/s^2 gravity acceleration
alpha=pi/4;
c=(x20^2)*w0;
c2=c^2;
s2a=(sin(alpha))^2;
sca=sin(alpha)*cos(alpha);

% Experiment duration
T=20; % (s)

% Simulation)
sim('conedin');

theta=x1;
r=x2;
z=r*cot(alpha);

%conversion to rectangular coordinates
xx=r.*cos(theta);
yy=r.*sin(theta);

%--------------------------------------------------------------------------

figure(1)
subplot(311)
gg=plot(t,theta);
set(gg,'LineWidth',1.5);
gg=xlabel('t (s)');
set(gg,'FontSize',14)
gg=ylabel('\theta (rad)');
set(gg,'FontSize',14)

subplot(312)
gg=plot(t,r);
set(gg,'LineWidth',1.5);
xlabel('t (s)');
gg=ylabel('r (m)');
set(gg,'FontSize',14)

subplot(313)
gg=plot(t,z);
set(gg,'LineWidth',1.5);
gg=xlabel('t (s)');
set(gg,'FontSize',14)
ylabel('z (m)');
set(gg,'FontSize',14)
set(gg,'FontSize',14)

%--------------------------------------------------------------------------
% 3-D plot

figure(2)
gg=plot3(xx,yy,z);
set(gg,'Linewidth',1.5);
gg=xlabel('x');
set(gg,'FontSize',14)
ylabel('y');
set(gg,'FontSize',14)
zlabel('z');
set(gg,'FontSize',14)
hold on
gg=plot3(xx(1),yy(1),z(1),'or');
set(gg,'Linewidth',1.5);
hold off

%--------------------------------------------------------------------------
% animated 3-D plot

figure(3)
axis([-1 1 -1 1 0 1])
hold on
curve=animatedline;
set(gca,'Xlim',[-1 1])
for ii=1:length(t)
    addpoints(curve,xx(ii),yy(ii), z(ii));
    drawnow
    pause(0.1)
end
hold off

%--------------------------------------------------------------------------
% animated 2-D plot in the horizontal plane

figure(4)
axis([-1 1 -1 1 0 1])
hold on
curve=animatedline;
set(gca,'Xlim',[-1 1])
for ii=1:length(t)
    addpoints(curve,xx(ii),yy(ii));
    drawnow
    pause(0.1)
end
hold off

%--------------------------------------------------------------------------
% 3-D plot  plus 2-D plot in the horizontal plane

figure(5)
gg=plot3(xx,yy,z,'b');
set(gg,'Linewidth',1.5);
axis([-1 1 -1 1 0 1])
gg=xlabel('x');
set(gg,'FontSize',14)
gg=ylabel('y');
set(gg,'FontSize',14)
zlabel('z');
set(gg,'FontSize',14)
hold on
gg=plot3(xx(1),yy(1),z(1),'or');
set(gg,'Linewidth',1.5);

gg=plot(xx,yy,'r');
set(gg,'Linewidth',1.5);
hold on
gg=plot3(xx(1),yy(1),z(1),'or');
set(gg,'Linewidth',1.5);

hold off

%--------------------------------------------------------------------------
% EoF
