P0 = [0.5; 0];
P1 = [1.5; 0];
P2 = [1.5; 1];
P3 = [0.5; 1];
P4 = P0;
TIP = [P0, P1, P2, P3, P4];
[P1, Q1] = RRMove(P0, P1, 1);
[P2, Q2] = RRMove(P1, P2, 1);
[P3, Q3] = RRMove(P2, P3, 1);
[P4, Q4] = RRMove(P3, P4, 1);
Qr = [Q1, Q2, Q3, Q4];

dQr = 0*Qr;
for i=2:length(Qr) - 1
    dQr(:,i) = (Qr(:,i+1) - Qr(:,i-1)) / 0.02;
    end

ddQr = 0*Qr;
for i=2:length(Qr) - 1
    ddQr(:,i) = (dQr(:,i+1) - dQr(:,i-1)) / 0.02;
    end


Q = Qr(:,1);
dQ = [0; 0];

t = 0;
dt = 0.001;

Qq = [];

% Start the simulation  (dt = 0.001 for stability concerns)


for i=1:length(Qr)
  
   for j=1:10

      T1 = 100*(Qr(:,i) - Q)  + 14*(dQr(:,i) - dQ);

% gravity
      Tg = -9.8 * [ 2*cos(Q(1)) + cos(Q(1) + Q(2));
                    cos(Q(1) + Q(2)) ]; 

% Coriolus Forces
      Tc = [ 2*sin(Q(2))*dQ(1)*dQ(2) + sin(Q(2))*dQ(2)*dQ(2) ;
            -sin(Q(2))*dQ(1)*dQ(1)         ];

% acceleration
      M = [ 2*(1+cos(Qr(2,i))),    (1+cos(Qr(2,i))) ;
              (1+cos(Qr(2,i))),     1];
      Tacc = M * ddQr(:,i);

      T = T1 - Tg - Tc + Tacc;
      ddQ = RRDynamics(Q, dQ, T);

      dQ = dQ + ddQ * dt;
      Q = Q + dQ*dt;
      t = t + dt;

      end

   RR(Q, Qr(:,i), TIP);

   Qq = [Qq, Q];

   end


t = [1:length(Qq)] * 0.01;
clf
plot(t,Qq,t,Qr);
xlabel('Time (seconds)');
ylabel('Angles (radians)');