clear all;

close all;
clc;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PENDULO SIMPLE NO LINEAL
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%dy^2=-wsin(y);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ECUACION DIFERENCIAL A RESOLVER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% y1'=y2 y1(0)=pi/10;
% y2' = -wsin(y1) y2(0)=0; w=sqrt(g/l)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EQUIVALENTE EN MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% dy(1) = y(2);
% dy(2) = -w*sin(y(1));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CONDICIONES INICIALES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
theta=10;
theta_ini=theta*(pi/180);
theta_pto_ini=0;
theta_pto_ini=theta_pto_ini*(pi/180);
ti=0;
tf=20;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TOLERANCIA AL ERROR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
options = odeset('RelTol',1e-3,'AbsTol',[1e-3 1e-6]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% METODO NUMERICO IMPLEMANTADO POR RUNGE - KUTTA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[T,Y] = ode45(@odeMAS,[ti tf],[theta_ini theta_pto_ini]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ALGUNOS VALORES CONSTANTES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
l=1;
g=9.8;
w=sqrt(g/l);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% VARIABLES DE SOLUCION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
theta=Y(:,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% COORDENADAS CARTESIANAS DEL PROBLEMA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x=l*sin(theta);
y=-l*cos(theta);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LEE UNA IMAGEN EN CARPETA ACTUAL
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
I=imread('encab_sim.png');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CREAR OBJETO DE VIDEO - DEFINE EL NOMBRE DE ARCHIVO DE VIDEO Y FORMATO
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
writerObj = VideoWriter('video_pendulo_1.mp4','MPEG-4');
open(writerObj);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SIMULA EL COMPORTAMIENTO DEL PROBLEMA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h=figure('color','white');
set(h,'Position',[360 278 700 500]);
axes1 = axes('Parent',h);

for t=1:size(T,1);
cla(axes1,'reset');
if t>=6

plot([0 0],[0 -l],'-.k','LineWidth',2);

hold on;
 plot([0 x(t)],[0 y(t)],'-b','LineWidth',3);
hold on;
plot(x(t-5:t),y(t-5:t),'ok','MarkerFaceColor','k','MarkerSize',5);
hold on;
plot(x(t),y(t),'ok','MarkerFaceColor','r','MarkerSize',20);
hold on;
Fy=g*cos(Y(t,1));
Fx=g*sin(Y(t,1));
quiver(x(t),y(t),-0.3*cos(Y(t,1))*x(t),-0.3*cos(Y(t,1))*y(t),'LineWidth',3,'Color',[0 1 0]);
hold on;
quiver(x(t),y(t),0,-0.2,'LineWidth',3,'Color',[0 0 0]);
hold on;
quiver(x(t),y(t),0.15*(Y(t,2))*cos(Y(t,1)),0.15*Y(t,2)*sin(Y(t,1)),'LineWidth',3,'Color',[1 0 0]);
grid on;
title(' X Vs Y');
axis([-l l -l-l/5 0]);

text((l-abs(-l))/2,(-l-l/5)*(1-0.05),'N?stor Alonso Arias

Hernandez','FontSize',15,'FontWeight','Bold','Color',[1 0 0]);
text(-0.9,-0.1,['$' 'M=1 Kg' '$'],'Interpreter','latex','FontSize',20,'FontWeight','Bold');
text(-0.9,-0.2,['$' 'L=1 m' '$'],'Interpreter','latex','FontSize',20,'FontWeight','Bold');
text(-0.9,-0.3,['$' 'g=9.8 \frac{m}{s^{2}}' '$'],'Interpreter','latex','FontSize',20,'FontWeight','Bold');

text(-0.9,-0.4,['$' 'T (N)' '$'],'Interpreter','latex','FontSize',20,'FontWeight','Bold','Color',[0 1 0]);

text(-0.9,-0.5,['$' 'F_{g} (N)' '$'],'Interpreter','latex','FontSize',20,'FontWeight','Bold','Color',[0 0 0]);
text(-0.9,-0.6,['$' 'F_{t} (N)' '$'],'Interpreter','latex','FontSize',20,'FontWeight','Bold','Color',[1 0 0]);

xlabel('Y (m)','FontSize',15,'FontWeight','Bold');
ylabel('X (m)','FontSize',15,'FontWeight','Bold');

title('PENDULO NO LINEAL ','FontSize',18,'FontWeight','Bold','Color',[0 0 1]);

hold off;
pause(0.0000001);

F = getframe(gcf);
writeVideo(writerObj,F);
hold off;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CERRAMOS VIDEO
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close(writerObj);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% La función con el sistema de ecuaciones diferenciales a resolver
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function dy = odeMAS(t,y)
dy = zeros(2,1); % a column vector
l=1;
g=9.8;
w2=g/l;

dy(1) = y(2);
dy(2) = -w2*sin(y(1));