clear all;
dbstop if error;
dbstop if warning;
format long;

% This the length we draw unit vectors.
global sz;
sz = 0.5;
% Total number of frames the simulation will run for.
maxFrames = 50;
% Undeformed length of the rod
l_0 = 1;
% Number of bodies in the chain
n = 4;
% Number of constraints
m = n-1;

% Construct the simulation
simulation = Simulation;
simulation.gravity = [0 0 -10]';
timestep = 1/30;

% Fill body data
for i = 1:n
    simulation.bodies(i) = Body;
    simulation.bodies(i).position = [(i-1)*l_0 0 0]';
    simulation.timestep = timestep;
end

% Pin the first body
simulation.bodies(1).dynamic = false;
simulation.bodies(1).inverseMass = 0;
simulation.bodies(1).unitMassInverseInertia = zeros(3);

% Fill constraint data
for i = 1:m
    simulation.rodConstraints(i) = RodConstraint;
    simulation.rodConstraints(i).indexB = i;
    simulation.rodConstraints(i).indexA = i+1;
    simulation.rodConstraints(i).l_0 = l_0;
    simulation.rodConstraints(i).alpha_b = 0.05;
    simulation.rodConstraints(i).alpha_t = 0.05;
    simulation.rodConstraints(i).F_a = quaternionFromAxisAngle([0 1 0]*pi);
    simulation.rodConstraints(i).F_b = quaternionFromAxisAngle([0 1 0]*pi);
end

% Loop simulation
plotSimulation(simulation);
for i = 1:maxFrames   
    simulation = simulation.Update();
    plotSimulation(simulation);
    pause(timestep);
end 


