%% Problema 6 de la guía de teoremas de conservación
% ar = m1 * w^2 * r - m2 * g / (m1+m2)
% a_ang = -2 * vr * w / r
% ar = (m1 * v0^2 / r0^2 * r - m2 * g )/ (m1+m2)
% Autor: Sebastián Schiavinato

clear all; close all

%% Parámetros
t0 = 0;         %[t0] = s;
tf = 10;        %[tf] = s;
g = 9.8;        %[g]=m/s^2
m1 = 1;         %[m1] = kg
m2 = 1;         %[m2] = kg
dt=0.01;        %Paso de integración [dt] = s
fps=29.97;      %cuadros por segundo. Para la película
fti=1/(dt*fps); %
metodo='RK';    %el método puede ser RK o euler (entre comillas). RK (de Runge-Kutta) es más eficiente.
verpeli=1;      % Si queres hacer la película poné 1, si no 0. Hacer la peli tarda más.
guardarpeli=0;  %Si querés guardar la película poné 1 si no 0. Guardar la película puede tardar bastante.
% Valores iniciales
r0 = 0.5; %[r0] = m
v0 = sqrt(m2 / m1 * r0 * g); %[v0] = m/s. Condición inicial para que no haya velocidad radial
z0 = 1;         %[z0] = m


%% Vectores
t = t0:dt:tf; %Vector tiempo [t] = s
% Dimensiono los vectores por una cuestión de rendimiento y para que funcione la graficación si se excede de la física la simulación
% Primero dimensiono el vector r usando el vector t y luego copio el vector r a los demás vectores
r = nan*t;    %[r] = m
vr = r;       %[vr] = m/s
ang = r;      %[ang] = rad
vang = r;     %[vang] = rad/s
x=r;
y=r;
r(1) = r0;
vr(1) = 0;
ang(1) = 0;
vang(1) = v0/r0+1.2;

%% busco los radios máximos y mínimos
L=m1*r(1)^2*vang(1);
a=m2*g;
b=0.5*L^2/m1;
c=0.5*(m1+m2)*vr(1)^2;
H=a*r(1)+b/(r(1)^2)+c;
xtemp=0.2:0.01:1;
Veff=a*xtemp+b./(xtemp.^2);
[~,iv]=min(Veff);
[~,imin]=min(abs(Veff(1:iv)-H));
[~,imax]=min(abs(Veff(iv:length(Veff))-H));
rmin=xtemp(imin);
rmax=xtemp(imax+iv-1);
xmin=rmin*cos(0:0.1:2*pi);
ymin=rmin*sin(0:0.1:2*pi);
xmax=rmax*cos(0:0.1:2*pi);
ymax=rmax*sin(0:0.1:2*pi);
z=zeros(length(t),1);

%% Creo la figura, dibujo la mesa, la soga y las masas.
%% Todo esto es para optimizar el dibujo
figure(1)
h1=plot3(x,y,z,'XDataSource','x','YDataSource','y','ZDataSource','z'); %Trayectoria
hold on
plot(0.8*[-1 1 1 -1 -1],.8*[-1 -1 1 1 -1],'k','linewidth',2) %Mesa
plot([xmin nan xmax],[ymin nan ymax],'k--') %Radios máximo y mínimo
xlin=[0 nan nan 0 0];ylin=[0 nan nan 0 0];zlin=[0 0 nan 0 nan];
h2=plot3(xlin,ylin,zlin,'k','XDataSource','xlin','YDataSource','ylin','ZDataSource','zlin');%Soga
h3=scatter3([nan 0],[nan 0],[0 nan],50,'k','filled');%Masas
set(h3,'XDataSource','xm','YDataSource','ym','ZDataSource','zm');
grid on
axis equal
xlabel('Posici\''on (m)','interpreter','latex')
ylabel('Posici\''on (m)','interpreter','latex')
zlabel('Posici\''on (m)','interpreter','latex')
xlim([-1 1]);ylim([-1 1]);zlim([-1 0.1])
set(gcf,'Position', [100 373 1280 720])
if guardarpeli==1
    M=getframe(gcf);
    M = repmat(M,floor(fps*tf),1);
end
%% Integración numérica
k=0;
for i = 1:length(t)-1
    if (r(i) < 0) %r >= 0 es condición física del problema
        break;
    end
    
    if strcmp(metodo,'euler')
        % Método de Euler
        vr(i+1) = vr(i) + dt*(m1*r(i)*vang(i)^2 - m2*g)/(m1+m2);
        vang(i+1) = vang(i)-dt*2*vang(i)*vr(i)/r(i);
        ang(i+1) = ang(i) + vang(i) * dt;
        r(i+1) = r(i) + vr(i)*dt;
    elseif strcmp(metodo,'RK')
        % Método de Runge-Kutta de 2º orden
        kvr1=(m1*r(i)*vang(i)^2 - m2*g)/(m1+m2);
        vr_estrella=vr(i)+dt*kvr1;
        kvtita1=-2*vang(i)*vr(i)/r(i);
        vtita_estrella=vang(i)+dt*kvtita1;
        kr1=vr(i);
        r_estrella=r(i)+dt*kr1;
        ka1=vang(i);
        ang_estrella=ang(i)+dt*ka1;
        kvr2=(m1*r_estrella*vtita_estrella^2 - m2*g)/(m1+m2);
        vr(i+1)=vr(i)+dt*(kvr1+kvr2)/2;
        kvtita2=-2*vtita_estrella*vr_estrella/r_estrella;
        vang(i+1)=vang(i)+dt*(kvtita1+kvtita2)/2;
        kr2=vr_estrella;
        r(i+1)=r(i)+dt*(kr1+kr2)/2;
        ka2=vtita_estrella;
        ang(i+1)=ang(i)+dt*(ka1+ka2)/2;
    else
        disp('el método tiene que ser RK o euler. Ver linea 15')
        break
    end
    
    if verpeli==1
        [x,y] = pol2cart(ang,r); %De esta forma se pasa de coordenadas polares a cartesianas
        if t(i)>=k/fps
            k=k+1;
            xlin=[0 x(i) nan 0 0];
            ylin=[0 y(i) nan 0 0];
            zlin=[0 0 nan 0 r(i)-1.5];
            xm=[x(i) 0];
            ym=[y(i) 0];
            zm=[0 r(i)-1.5];
            title(['tiempo=' num2str(t(i),'%2.2f') 's'])
            refreshdata(h1); %actualizo los datos del los gráficos
            refreshdata(h2);
            refreshdata(h3);
            drawnow          %dibujo los graficos con los datos actualizados
            if guardarpeli==1
                M(k)=getframe(gcf);
            end
        end
    end
end
%% Guardar película o ver gráficos
if guardarpeli==1
    if verpeli==0
        disp('Para guardar la pelicula tenés que verla primero. Ver linea 16')
    end
    movie2avi(M,'peli','fps',29.97,'quality',25)
end
if verpeli==0
    [x,y] = pol2cart(ang,r); 
    figure(1)
    hold off
    plot(xmin,ymin,'k--')
    hold on
    plot(xmax,ymax,'k--')
    plot(x,y)
    xlim([-1 1])
    ylim([-1 1])
    grid on
    axis equal
end
