%Presentation in video: 
% English: https://personales.upv.es/asala/YT/V/tiovELsimEN.html
% Spanish: https://personales.upv.es/asala/YT/V/tiovELsim.html
L=1; R_0=0.5; M=1; g=9.8; I_0=.5;
EEE=eval(EcEstado4)
EcEstado4_num=matlabFunction(EEE,Vars={phi,theta,omega_0,omega_1,tau})

TAU=@(t) (.8*(t<15)-.6*(t>30));
StateEqODE=@(t,x) EcEstado4_num(x(1),x(2),x(3),x(4),TAU(t));

%actual simulation of the differential equation here:
x0=[0;.5;0;0];
[T,X]=ode45(StateEqODE,0:0.035:50,x0);

%plots as a function of time
fig=figure(1);
plot(T,X(:,2),T,X(:,3)), grid on
legend("angle \theta","platform ang. speed \omega_0")
fontsize(fig, 34, "points")  




%animation
N=length(T);
r_num=matlabFunction(eval(r),Vars={phi,theta})
Rsim=r_num(X(:,1)',X(:,2)'); %position trajectory
fig=figure(2);
for k=1:N
    %plot trailing trajectory segment
    pastwindow=max(1,k-35);
    plot3(Rsim(1,pastwindow:k),Rsim(2,pastwindow:k),Rsim(3,pastwindow:k),Color=[.6 .75 .85],LineWidth=2)
    hold on    
    rx=Rsim(1,k);ry=Rsim(2,k);rz=Rsim(3,k);
    %plot platform
    plot3([0 0 R_0*cos(X(k,1))],[0 0 R_0*sin(X(k,1))],[-.25 L L],LineWidth=7)
    %plot bar to suspended mass
    plot3([R_0*cos(X(k,1)) rx],[R_0*sin(X(k,1)) ry ],[L rz],LineWidth=2)
    %plot suspended mass
    plot3(rx,ry,rz,'k*',LineWidth=4,MarkerSize=10)
    hold off, grid on
    axis equal
    axis([-(1+R_0) (1+R_0) -(1+R_0) (1+R_0) -0.25 L+0.25])
    fontsize(fig, 34, "points") 
    drawnow
end