function RetMatrix = DoublePendulumTRAP(T,dt,theta1_init,theta1dot_init,theta2_init,theta2dot_init,m1,m2,L1,L2,maxiters,Doplot,DoplotEig)
%
%Plotpen2(T,dt,theta1_init,theta1dot_init,theta2_init,theta2dot_init,m1,m2,L1,L2)
%----------------------------------------------------------------------------------- 
%Animated the rigid coupled pendulum system with desired parameters using
%Trapezoidal Rule
% @ Sohan Dharmaraja MIT OCT2006

% Variables we're using:
% a = theta1
% b = theta2
% p = m1
% q = m2
% i = L1
% j = L2
% X = theta1dot
% Y = theta2dot

%Vectors we're defining:
% timevec - times
% avec - to store the angle theta1
% bvec - to store the angle theta2
% adotvec - to store the velocity, theta1dot
% bdotvec - to store the velocity, theta2dot

%set up initial conditions
timevec(1)=0;
avec(1)=theta1_init;
adotvec(1)=theta1dot_init;
bvec(1)=theta2_init;
bdotvec(1)=theta2dot_init;
p=m1;
q=m2;
i=L1;
j=L2;
g=9.81;
N=T/dt;
Totiters=0;

xold=[theta1_init; theta1dot_init; theta2_init; theta2dot_init];
xnew=xold;

    %define the Trapezoidal scheme
    f1=inline('iternew1 - (dt/2)*iternew2 - orig1 - (dt/2)*orig2','dt','orig1','orig2','iternew1','iternew2');
    f2=inline('iternew2 - (dt/2)*(-9.81*(2*p + q)* sin(iternew1) - q*9.81*sin(iternew1 - 2*iternew3) - 2*sin(iternew1-iternew3)*q*((iternew4^2)*j + (iternew2^2)*i*cos(iternew1-iternew3)))/(i*(2*p + q - q*cos(2*iternew1-2*iternew3))) - orig2 - ((dt/2)*(-9.81*(2*p + q)* sin(orig1) - q*9.81*sin(orig1 - 2*orig3) - 2*sin(orig1-orig3)*q*((orig4^2)*j + (orig2^2)*i*cos(orig1-orig3))))/(i*(2*p + q - q*cos(2*orig1-2*orig3)))','dt','orig1','orig2','orig3','orig4','iternew1','iternew2','iternew3','iternew4','p','q','i','j');
    f3=inline('iternew3 - (dt/2)*iternew4 - orig3 - (dt/2)*orig4','dt','orig3','orig4','iternew3','iternew4');
    f4=inline('(iternew4 - ((dt/2)*(2*sin(iternew1-iternew3)*((iternew2^2)*i*(p+q) + 9.81*(p+q)*cos(iternew1) + (iternew4^2)*j*q*cos(iternew1-iternew3))))/(j*(2*p + q - q*cos(2*iternew1 - 2*iternew3)))) - orig4 - ((dt/2)*(2*sin(orig1-orig3)*((orig2^2)*i*(p+q) + 9.81*(p+q)*cos(orig1) + (orig4^2)*j*q*cos(orig1-orig3))))/(j*(2*p + q - q*cos(2*orig1 - 2*orig3)))','dt','orig1','orig2','orig3','orig4','iternew1','iternew2','iternew3','iternew4','p','q','i','j');

flagval=0;

%Let user know how far along the calculation is!
h = waitbar(0,'Calculating for the Trap scheme... Press ENTER afterwards to start animation');

tic
if DoplotEig =='Y'
    figure;
end

for iter=1:N
waitbar(iter/N)
xinit=xnew;
%Zero the Jacobian
J=zeros(4,4);

% DO FIVE NEWTON ITERATIONS: this can be changed for performance, if
% desired, but 5 does a fairly good job. Tolerance checking alone causes
% slight slow down...
delxvec=1;
deltax=1;
counter=0;

%dotimes=1;

% while norm(deltax)>=(10*eps)
%for dotimes=1:maxiters
for reptimes=1:maxiters
    
    Totiters=Totiters+1;
    counter=counter+1;
    a=xnew(1,1);
    X=xnew(2,1);
    b=xnew(3,1);
    Y=xnew(4,1);

    Tempmat=zeros(4,4);
    %Fill in the Jacobian element wise - easier debugging!
    Tempmat(1,2)=1;
    Tempmat(2,1)=(-g*(2*p + q)*cos(a) -q*g*cos(a-2*b) - 2*cos(a-b)*q*((Y^2)*j+(X^2)*i*cos(a-b)) + 2*((sin(a-b))^2)*q*i*(X^2)) / (i*(2*p +q -q*(cos(2*a - 2*b)))) - ((2*(-g*(2*p + q)*sin(a) -q*g*sin(a-2*b) - 2*sin(a-b)*q*((Y^2)*j+(X^2)*i*cos(a-b)))*q*sin(2*a-2*b)) / (i*(((2*p +q -q*(cos(2*a - 2*b))))^2)));
    Tempmat(2,2)=-(4*sin(a-b)*q*X*cos(a-b))/(2*p +q -q*(cos(2*a - 2*b)));
    Tempmat(2,3)=(2*q*g*cos(a - 2*b) + 2*cos(a-b)*q*((Y^2)*j+(X^2)*i*cos(a-b)) - 2*((sin(a-b))^2)*q*i*(X^2)) /  (i*(2*p +q -q*(cos(2*a - 2*b))))                 + (2*(-g*(2*p + q)*sin(a) -q*g*sin(a-2*b) - 2*sin(a-b)*q*((Y^2)*j+(X^2)*i*cos(a-b)))*q*sin(2*a-2*b)) / (i*(((2*p +q -q*(cos(2*a - 2*b))))^2));
    Tempmat(2,4)=-(4*sin(a-b)*q*Y*j)/(i*(2*p +q -q*(cos(2*a - 2*b))));
    Tempmat(3,4)=1;
    Tempmat(4,1)=((((2*cos(a-b)*((X^2)*i*(p+q) + g*(p+q)*cos(a) + (Y^2)*j*q*cos(a-b))))/(j*(2*p + q - q*cos(2*a - 2*b))))) + ((2*sin(a-b)*(-g*(p+q)*sin(a) -(Y^2)*j*q*sin(a-b)))/(j*(2*p + q - q*cos(2*a - 2*b)))) -((4*sin(a-b)*((X^2)*i*(p+q) + g*(p+q)*cos(a) + (Y^2)*j*q*cos(a-b)))*q*sin(2*a-2*b)/(j*(((2*p + q - q*cos(2*a - 2*b))^2))));
    Tempmat(4,2)=(4*sin(a-b)*X*i*(p+q))/(j*(2*p + q - q*cos(2*a - 2*b)));
    Tempmat(4,3)=-((2*cos(a-b)*((X^2)*i*(p+q) + g*(p+q)*cos(a) + (Y^2)*j*q*cos(a-b)))/(j*(2*p + q - q*cos(2*a - 2*b)))) + ((2*((sin(a-b))^2)*(Y^2)*q)/(2*p + q - q*cos(2*a - 2*b)))     +((4*sin(a-b)*((X^2)*i*(p+q) + g*(p+q)*cos(a) + (Y^2)*j*q*cos(a-b)))*q*sin(2*a-2*b)/(j*(((2*p + q - q*cos(2*a - 2*b))^2))));
    Tempmat(4,4)=(4*sin(a-b)*Y*q*cos(a-b))/(2*p + q- q*cos(2*a - 2*b));

    % Compute the Jacobian to be used for the Newton iteration
    J=eye(4)-(dt/2)*Tempmat;
    RHSvec(1,1)=f1(dt,xinit(1,1),xinit(2,1),xold(1,1),xold(2,1));
    RHSvec(2,1)=f2(dt,xinit(1,1),xinit(2,1),xinit(3,1),xinit(4,1),xold(1,1),xold(2,1),xold(3,1),xold(4,1),p,q,i,j);
    RHSvec(3,1)=f3(dt,xinit(3,1),xinit(4,1),xold(3,1),xold(4,1));
    RHSvec(4,1)=f4(dt,xinit(1,1),xinit(2,1),xinit(3,1),xinit(4,1),xold(1,1),xold(2,1),xold(3,1),xold(4,1),p,q,i,j);
    
    deltax=inv(J)*((-1)*RHSvec);
    %det(inv(J));
    
    if DoplotEig=='Y'
    v=eig(J);
    plot(v,'bo','MarkerEdgeColor','k','MarkerFaceColor',[0 0 0],'MarkerSize',2)
    hold on
    end
    
    xnew=xold+deltax;
    xold=xnew;
    delxvec(counter)=norm(deltax);
end

%Update our vectors with the new values: theta1, theta1dot, theta2, theta2dot
avec(iter+1)=xnew(1,1);
adotvec(iter+1)=xnew(2,1);
bvec(iter+1)=xnew(3,1);
bdotvec(iter+1)=xnew(4,1);
timevec(iter+1)=iter*dt;

end

if DoplotEig=='Y'
    
    v=[-pi:0.01:pi];
    circ=cos(v)+sqrt(-1)*sin(v);
    plot(circ)
    axis([0.7 1.3 -0.4 0.4])
    hold off
    title('Eigenvalues and the unit circle - Trapezoidal Rule')
    figure;
end

close(h)

timereq=toc;
fprintf('The time required using the trapezoidal-rule integration method was %u seconds.\n',timereq)
fprintf('%u iterations were performed!\n',Totiters)


modavec = avec;
modbvec = bvec;

theta1info = [num2str(T),' ', num2str(dt),' ', num2str(theta1_init),' ', num2str(theta1dot_init), ' '];
theta2info = [num2str(theta2_init),' ', num2str(theta2dot_init), ' '];

figure;
plot(modavec,adotvec,'LineWidth',1);xlabel('Angular displacement - \theta_{1}'); ylabel('Angular velocity - \theta_{1}'); title('Poincare Plot - Pendulum 1')
saveas(gcf,['Trap - Theta1 ' theta1info ' ' theta2info '.png']); close; figure;
plot(modbvec,bdotvec,'LineWidth',1);xlabel('Angular displacement - \theta_{2}'); ylabel('Angular velocity - \theta_{2}'); title('Poincare Plot - Pendulum 2')
saveas(gcf,['Trap - Theta2 ' theta1info ' ' theta2info '.png']); close; figure;

if Doplot=='Y'
    figure;

%Draw the axes where the animation will be
set(gca,'XLim',[-(i+j) (i+j)],'YLim',[-(i+j) (i+j)],'XTick',[-(i+j):(i+j)],'YTick',[-(i+j):(i+j)],'Drawmode','fast','Visible','on','NextPlot','add');

plot(0,0,'ks')
grid on
axis equal
axis manual

bob1 = line('color',[0.1 0.8 0.1],'Marker','.','markersize',35,'erase','xor','xdata',[],'ydata',[]);
rod1 = line('color',[0.4 0.4 1],'LineStyle','-','LineWidth',2.5,'erase','xor','xdata',[],'ydata',[]);
bob2 = line('color',[0.1 0.8 0.1],'Marker','.','markersize',35,'erase','xor','xdata',[],'ydata',[]);
rod2 = line('color',[0.4 0.4 1],'LineStyle','-','LineWidth',2.5,'erase','xor','xdata',[],'ydata',[]);

%Animate!
tic
for Curtime=0:(length(avec)-1)
    
  xbob1=i*sin(avec(Curtime+1));
  ybob1 = -i*cos(avec(Curtime+1));
  xrod1 = [0 xbob1]; yrod1 = [0 ybob1];
  
  xbob2=xbob1+ j*sin(bvec(Curtime+1));
  ybob2=ybob1+ -j*cos(bvec(Curtime+1));
  xrod2 = [xbob1 xbob2]; yrod2 = [ybob1 ybob2];
  
  set(bob1,'xdata',i*sin(avec(Curtime+1)),'ydata',-i*cos(avec(Curtime+1)))
  set(bob2,'xdata',i*sin(avec(Curtime+1))+ j*sin(bvec(Curtime+1)),'ydata',-i*cos(avec(Curtime+1))-j*cos(bvec(Curtime+1)))   
  set(rod1,'xdata',xrod1,'ydata',yrod1)
  set(rod2,'xdata',xrod2,'ydata',yrod2)
  
    drawnow; 
    if flagval==0
      flagval=1;
        saveas(gcf,['DPgridon' theta1info ' ' theta2info '.png']);
        grid off
        saveas(gcf,['DPgridoff' theta1info ' ' theta2info '.png']);
        grid on
      pause
    end

  pause(dt)

end
toc
end

RetMatrix=[timevec',avec',adotvec',bvec',bdotvec'];