close all
clear all
global m1 m2 l1 l2

eps =1e-6;
%phipert = 20;
phipert = 100;
modes = 4;

linewidth = 2;
markersize = 20;

phio(1) = phipert;
if modes == 1
    phio(2) = sqrt(2)*phipert;
end
if modes == 2
    phio(2) = -sqrt(2)*phipert;
end
if modes == 3
    phio(2) = 0;
end
if modes == 4
    phio(1) = 181;
    phio(2) = 0;
end
if modes == 5
    phio(1) = 270;
    phio(2) = 90;
end

double_pendulum
t1 = t;
phi1 = phi;
x11 = x1;
x21 = x2;
y11 = y1;
y21 = y2;
close all
phio(1) = phipert+eps;
if modes == 1
    phio(2) = sqrt(2)*phipert;
end
if modes == 2
    phio(2) = -sqrt(2)*phipert;
end
if modes == 3
    phio(2) = 0;
end
if modes == 4
    phio(1) = 181+eps;
    phio(2) = 0;
end
if modes == 5
    phio(1) = 270+eps;
    phio(2) = 90;
end
double_pendulum
t2 = t;
phi2 = phi;
x12 = x1;
x22 = x2;
y12 = y1;
y22 = y2;
close all
t = linspace(t1(1),t1(end),length(t1)/3);
phi1 = interp1(t1,phi1,t,'spline');
phi2 = interp1(t2,phi2,t,'spline');
x11 = interp1(t1,x11,t,'spline')';
x21 = interp1(t1,x21,t,'spline')';
y11 = interp1(t1,y11,t,'spline')';
y21 = interp1(t1,y21,t,'spline')';
x12 = interp1(t2,x12,t,'spline')';
x22 = interp1(t2,x22,t,'spline')';
y12 = interp1(t2,y12,t,'spline')';
y22 = interp1(t2,y22,t,'spline')';
subplot(1,2,1)
plot(0,0,'ks','LineWidth',linewidth)
hold all
line11 = line([0 x11(1,1)],[0 y11(1,1)],'LineWidth',linewidth,'Color',[0 0 1]);
line21 = line([x11(1,1) x21(1,1)],[y11(1,1) y21(1,1)],'LineWidth',linewidth,'Color',[0 0 1]);
line12 = line([0 x12(1,1)],[0 y12(1,1)],'LineWidth',linewidth,'Color',[1 0 0]);
line22 = line([x12(1,1) x22(1,1)],[y12(1,1) y22(1,1)],'LineWidth',linewidth,'Color',[1 0 0]);
h1 = plot(x11(1,1),y11(1,1),'bo','LineWidth',linewidth,'MarkerSize',m1^(1/3)*markersize,'MarkerFaceColor','b');
h2 = plot(x12(1,1),y12(1,1),'ro','LineWidth',linewidth,'MarkerSize',m1^(1/3)*markersize,'MarkerFaceColor','r');
h3 = plot(x21(1,1),y21(1,1),'bo','LineWidth',linewidth,'MarkerSize',m2^(1/3)*markersize,'MarkerFaceColor','b');
h4 = plot(x22(1,1),y22(1,1),'ro','LineWidth',linewidth,'MarkerSize',m2^(1/3)*markersize,'MarkerFaceColor','r');
xlim([-1.2*(l1+l2) 1.2*(l1+l2)])
ylim([-1.2*(l1+l2) 1.2*(l1+l2)])
xlabel('x','FontSize',fontsize)
ylabel('y','FontSize',fontsize)
set(gca,'FontSize',fontsize,'LineWidth',linewidth)
box on
axis square
subplot(1,2,2)
plot([180 180],[-180 180],'k:','LineWidth',linewidth)
hold all
plot([-180 -180],[-180 180],'k:','LineWidth',linewidth)
plot([-180 180],[180 180],'k:','LineWidth',linewidth)
plot([-180 180],[-180 -180],'k:','LineWidth',linewidth)
h5 = plot(phi1(1,1),phi1(1,2),'bo','LineWidth',linewidth,'MarkerSize',markersize);
h6 = plot(phi2(1,1),phi2(1,2),'ro','LineWidth',linewidth,'MarkerSize',markersize);
xlim([-max(abs([phi1(:,1);phi1(:,2)])) max(abs([phi1(:,1);phi1(:,2)]))])
ylim([-max(abs([phi2(:,1);phi2(:,2)])) max(abs([phi2(:,1);phi2(:,2)]))])
xlabel('\phi_1','FontSize',fontsize)
ylabel('\phi_2','FontSize',fontsize)
set(gca,'FontSize',fontsize,'LineWidth',linewidth)
box on
pause(5)
for i=2:length(t)
    delete(line11)
    delete(line21)
    delete(line12)
    delete(line22)
    delete(h1)
    delete(h2)
    delete(h3)
    delete(h4)
    delete(h5)
    delete(h6)
    subplot(1,2,1)
    line11 = line([0 x11(i,1)],[0 y11(i,1)],'LineWidth',linewidth,'Color',[0 0 1]);
    line21 = line([x11(i,1) x21(i,1)],[y11(i,1) y21(i,1)],'LineWidth',linewidth,'Color',[0 0 1]);
    line12 = line([0 x12(i,1)],[0 y12(i,1)],'LineWidth',linewidth,'Color',[1 0 0]);
    line22 = line([x12(i,1) x22(i,1)],[y12(i,1) y22(i,1)],'LineWidth',linewidth,'Color',[1 0 0]);
    h1 = plot(x11(i,1),y11(i,1),'bo','LineWidth',linewidth,'MarkerSize',m1^(1/3)*markersize,'MarkerFaceColor','b');
    h2 = plot(x12(i,1),y12(i,1),'ro','LineWidth',linewidth,'MarkerSize',m1^(1/3)*markersize,'MarkerFaceColor','r');
    h3 = plot(x21(i,1),y21(i,1),'bo','LineWidth',linewidth,'MarkerSize',m2^(1/3)*markersize,'MarkerFaceColor','b');
    h4 = plot(x22(i,1),y22(i,1),'ro','LineWidth',linewidth,'MarkerSize',m2^(1/3)*markersize,'MarkerFaceColor','r');
    axis square
    subplot(1,2,2)
    h5 = plot(phi1(i,1),phi1(i,2),'bo','LineWidth',linewidth,'MarkerSize',markersize);
    h6 = plot(phi2(i,1),phi2(i,2),'ro','LineWidth',linewidth,'MarkerSize',markersize);
    pause(0.05)
end