%% Animación de un péndulo con múltiples eslabones
% Vídeo: http://personales.upv.es/asala/YT/V/pendNSim.html
%% Modelado
%necesitamos el modelo

if ~exist('d2xdt')
    run('ModelPend.mlx')
end
if ~caso_concreto %el modelo "parámetros en letra" no me vale.
    return
end

%% Simulación

x0=0.25*(1:(2*N))/(2*N); %condición inicial (arbitraria)

pares=@(t) [5*sin(t); zeros(N-1,1)]; %pares aplicados a cada articulacion

%en el modelo los estados 1:N son las posiciones (N+1):(2*N) son las
%velocidades. d2xdt debe dar las aceleraciones (2as derivadas) generalizadas.
DerivEstado=@(t, estado) [estado((N+1):end); d2xdt(estado,pares(t))];

opts=odeset('AbsTol',2e-6,'RelTol',2e-5); %tolerancias
Tfinal=30;
[Tiempos,Estados]=ode45(DerivEstado,[0 Tfinal],x0,opts); %aquí hacemos la simulación.

%Presentemos gráficamente el resultado de la simulación:

size(Tiempos)
figure(1)
plot(Tiempos,Estados(:,1:N),'LineWidth',3), grid on %posiciones en función del tiempo
pause(2)

%% Animación gráfica

Ltot=sum(L);
dT=0.025; %paso de animación (ode45 es paso variable)
T=0:dT:Tfinal;
Nmuestras=length(T);
PosicionesAnim=interp1(Tiempos,Estados(:,1:N),T); %interpolamos a intervalos regulares para animación
%lo quiero de tamaño eslabones x tiempos
if size(PosicionesAnim,1)>size(PosicionesAnim,2)
    PosicionesAnim=PosicionesAnim';
end

figure(2)
for i=1:Nmuestras
    x=PosicionesAnim(:,i);
    PosBarras=PosArtNum(x); %dadas posiciones, obtener las posiciones de cada articulación (numérico).
    
    plot(PosBarras(1,1:(N+1)),PosBarras(2,1:(N+1)),LineWidth=3) %dibujamos los eslabones
    if(M_f>0)
        hold on
        plot(PosBarras(1,N+1),PosBarras(2,N+1),'or',MarkerSize=12,LineWidth=4) %bola al final.
        hold off
    end
    axis equal, axis([-Ltot Ltot -Ltot*1.05 Ltot]), grid on
    drawnow    
    pause(0.01) %si queremos hacer más lentos los frames
end
