close all
clear all
%better display values for plotting
fontsize=10;
linewidth=2;
markersize=20;

modes = 3;

%
% first, we declare the constants for the problem
%

global g m1 m2 k1 k2 k3

g = 9.8;
m1 = 1;
%m1 = 10;
m2 = 1;
k1 = 1;
k2 = 0.1;
%k2 = 1;
k3 = 1;

N = 1000;
A = 0.05;

if modes == 1
    xo(1)  = 10; %initial stretched distance
    xo(2)  = 10;
end
if modes == 2
    xo(1)  = 10; %initial stretched distance
    xo(2)  = -10;
end
if modes == 3
    xo(1)  = 10; %initial stretched distance
    xo(2)  = 0;
end

dxo(1) = 0;  %initial velocity (dx/dt)_o
dxo(2) = 0;

to = 0;     %starting time for my ODE solution
tf = 100;    %ending time for my ODE solution

%
%now we are ready to solve the ODE, first exactly, then using the book's
%approximation
%

options = odeset('RelTol',1e-4,'AbsTol',[1e-6 1e-6 1e-6 1e-6]);
%options = odeset('RelTol',1e-3,'AbsTol',[1e-6 1e-6 1e-6 1e-6]);
[t,x] = ode45(@carts,[to tf],[xo dxo],options);
%the syntax for any ODE integrator is:
%[t, f(t)] = odesolver(@(function that returns [t, df/dt]), [(start time) (end time)], [f(t=0) df/dt(t=0)])

x = interp1(t,x,linspace(t(1),t(end),length(t))','spline');
t = linspace(t(1),t(end),length(t))';

x1 = x(:,1);
x2 = x(:,2);

xspace(1) = 2*max(abs(x1));
xspace(2) = xspace(1)+2*(max(abs(x1))+max(abs(x2)));
xspace(3) = xspace(2)+2*max(abs(x2));

dx1 = x(:,3);
dx2 = x(:,4);

T1 = 0.5*m1*dx1.^2;
T2 = 0.5*m2*dx2.^2;
U1 = 0.5*(k1+k2)*x1.^2-0.5*k2*x1.*x2;
U2 = 0.5*(k2+k3)*x2.^2-0.5*k2*x1.*x2;
E  = T1+T2+U1+U2;

%
%now we can display our answer
%

%the rest is just plotting

subplot(3,1,1)
plot([0 0],[-1 1],'k-','LineWidth',linewidth)
hold all
plot([xspace(3) xspace(3)],[-1 1],'k-','LineWidth',linewidth)
plot([xspace(1) xspace(1)],[-1 1],'k:','LineWidth',linewidth)
plot([xspace(2) xspace(2)],[-1 1],'k:','LineWidth',linewidth)
xgrid(:,1) = linspace(0,x1(1)+xspace(1),N)';
xgrid(:,2) = linspace(x1(1)+xspace(1),x2(1)+xspace(2),N)';
xgrid(:,3) = linspace(x2(1)+xspace(2),xspace(3),N)';
h1 = plot(xgrid(:,1),A*sin(2*pi*10.*(xgrid(:,1)-xgrid(1,1))/(xgrid(end,1)-xgrid(1,1))),'k-','LineWidth',linewidth*sqrt(k1));
h2 = plot(xgrid(:,2),A*sin(2*pi*20.*(xgrid(:,2)-xgrid(1,2))/(xgrid(end,2)-xgrid(1,2))),'k-','LineWidth',linewidth*sqrt(k2));
h3 = plot(xgrid(:,3),A*sin(2*pi*10.*(xgrid(:,3)-xgrid(1,3))/(xgrid(end,3)-xgrid(1,3))),'k-','LineWidth',linewidth*sqrt(k3));
h4 = plot(x1(1)+xspace(1),0,'bs','LineWidth',linewidth,'MarkerSize',m1^(1/3)*markersize,'MarkerFaceColor','b');
h5 = plot(x2(1)+xspace(2),0,'rs','LineWidth',linewidth,'MarkerSize',m2^(1/3)*markersize,'MarkerFaceColor','r');
xlabel('x','FontSize',fontsize)
ylim([-1 1])
set(gca,'FontSize',fontsize,'LineWidth',linewidth)

subplot(3,1,2)
h6 = plot(t(1:1),x1(1:1),'b-','LineWidth',linewidth);
hold all
h7 = plot(t(1:1),x2(1:1),'r-','LineWidth',linewidth);
%legend('x_1','x_2')
xlabel('time','FontSize',fontsize)
ylabel('x','FontSize',fontsize)
xlim([0 t(end)])
ylim([-max(abs([x1(:,1);x2(:,1)])) max(abs([x1(:,1);x2(:,1)]))])
set(gca,'FontSize',fontsize,'LineWidth',linewidth)

subplot(3,1,3)
h8 = plot(t(1:1),E(1:1),'k-','LineWidth',linewidth);
hold all
h9 = plot(t(1:1),T1(1:1)+U1(1:1),'b-','LineWidth',linewidth);
h10 = plot(t(1:1),T2(1:1)+U2(1:1),'r-','LineWidth',linewidth);
%legend('E_1+E_2','E_1','E_2')
xlabel('time','FontSize',fontsize)
ylabel('Energy','FontSize',fontsize)
xlim([0 t(end)])
ylim([0 1.2*max(T1+U1+T2+U2)])
set(gca,'FontSize',fontsize,'LineWidth',linewidth)

pause(5)

for i=2:length(t)
    
    delete(h1)
    delete(h2)
    delete(h3)
    delete(h4)
    delete(h5)
    delete(h6)
    delete(h7)
    delete(h8)
    delete(h9)
    delete(h10)
    subplot(3,1,1)
    xgrid(:,1) = linspace(0,x1(i)+xspace(1),N);
    xgrid(:,2) = linspace(x1(i)+xspace(1),x2(i)+xspace(2),N);
    xgrid(:,3) = linspace(x2(i)+xspace(2),xspace(3),N);
    h1 = plot(xgrid(:,1),A*sin(2*pi*10.*(xgrid(:,1)-xgrid(1,1))/(xgrid(end,1)-xgrid(1,1))),'k-','LineWidth',linewidth*sqrt(k1));
    hold all
    h2 = plot(xgrid(:,2),A*sin(2*pi*20.*(xgrid(:,2)-xgrid(1,2))/(xgrid(end,2)-xgrid(1,2))),'k-','LineWidth',linewidth*sqrt(k2));
    h3 = plot(xgrid(:,3),A*sin(2*pi*10.*(xgrid(:,3)-xgrid(1,3))/(xgrid(end,3)-xgrid(1,3))),'k-','LineWidth',linewidth*sqrt(k3));
    h4 = plot(x1(i)+xspace(1),0,'bs','LineWidth',linewidth,'MarkerSize',m1^(1/3)*markersize,'MarkerFaceColor','b');
    h5 = plot(x2(i)+xspace(2),0,'rs','LineWidth',linewidth,'MarkerSize',m2^(1/3)*markersize,'MarkerFaceColor','r');
    
    subplot(3,1,2)
    h6 = plot(t(1:i),x1(1:i),'b-','LineWidth',linewidth);
    hold all
    h7 = plot(t(1:i),x2(1:i),'r-','LineWidth',linewidth);

    subplot(3,1,3)
    h8 = plot(t(1:i),E(1:i),'k-','LineWidth',linewidth);
    hold all
    h9 = plot(t(1:i),T1(1:i)+U1(1:i),'b-','LineWidth',linewidth);
    h10 = plot(t(1:i),T2(1:i)+U2(1:i),'r-','LineWidth',linewidth);
    
    pause(0.01)

end