clear

t0=0;x0=zeros(6,1);h=0.02;
m=1*eye(3,3);   k=1*[3 -1 0;-1 2 -1;0 -1 3];

a=[zeros(3,3),eye(3,3);-inv(m)*k,zeros(3,3)];

j=1;
t=0:h:50;n=length(t);


for i=1:n;
   l1=m_chap2_ex2_sub(t0,x0);
   l2=m_chap2_ex2_sub(t0+h/2,x0+(h/2)*l1);
   l3=m_chap2_ex2_sub(t0+h/2,x0+(h/2)*l2);
   l4=m_chap2_ex2_sub(t0+h,x0+h*l3);
   
   
   t1=t0+h;
   x1=x0+(h/6)*(l1+2*l2+2*l3+l4);
   
   acca=-inv(m)*k*x0(1:3)+inv(m)*[1;0;0]*sin(3*t0);

   u1(j)=x1(1);u2(j)=x1(2);u3(j)=x1(3);
   v1(j)=acca(1);v2(j)=acca(2);v3(j)=acca(3);
   
   t0=t1;x0=x1;
   j=j+1;
end

maxx=[max(abs(u1));max(abs(u2));max(abs(u3))];
maxa=[max(abs(v1));max(abs(v2));max(abs(v3))];

plot(t,u1)