g = 9.82;
%g = 0;
%antal bollar
n = 4;
%bollarnas startpositioner
Y = [1, 3, 5, 6];
%bollarnas starthastigheter
V = [0, 0, 0, 0];
%V = [1, -2, 0, 0];
%bollarnas massor
M = [3.3, 1, 0.9, 0.9];
%bollarnas storlekar
R = [0.4, 0.35, 0.3, 0.3];
%bollarnas fjäderkonstanter
%K = 5*(4 .* M .* g .* Y) ./ (R.^2);
K = [4000, 4000, 4000, 4000];
%sorterar alla bollarnas info efter stigande Y
[Y, NewIndex] = sort(Y);
for i = 1:n,
    VTemp(i) = V(NewIndex(i));
    MTemp(i) = M(NewIndex(i));
    RTemp(i) = R(NewIndex(i));
    KTemp(i) = K(NewIndex(i));
end
V = VTemp;
M = MTemp;
R = RTemp;
K = KTemp;

clear NewIndex
clear VTemp
clear MTemp
clear RTemp;
clear KTemp;

%bollarnas övre ihoptryckning
DU = [0, 0, 0, 0];
%bollarnas nedre ihoptryckning
DD = [0, 0, 0, 0];
%bollarnas kraftvektorer
F = [0, 0, 0, 0];
%bollarnas energi
E = [0, 0, 0, 0]; 
%bollarnas rörelsemängd
P = [0, 0, 0, 0];

x = max(R) / 2 + 1;

dt = 0.001;

daspect([1,1,1]);
axis([0, x * 2, 0, max(Y)*1.5 + 2 * max(R)]);
hold on;

for i = 1:n,
    BOLL(i) = rectangle('Position', [x - (R(i) + DU(i) + DD(i)), Y(i) - (R(i) - DD(i)), 2 * (R(i) + DU(i) + DD(i)), 2 * R(i) - DU(i) - DD(i)],'Curvature', [1,1]);    
    set(BOLL(i), 'erasemode', 'xor');
end

%sätter en initial hastighet för alla bollar - dvs tar ett halvt eulersteg
for i = 1:n,
    V(i) = V(i) + -g * dt/2;
end

plotdata_i = 1;

for t = 0:1000000,
    ETotal = 0;
    PTotal = 0;
    
    %uppdaterar alla bollars positioner
    for i = 1: n,
        Y(i) = Y(i) + (V(i) * dt);
    end
    
    %beräknar nya krafter och hastigheter
    for i = 1: n,
        %kollar om nedersta bollen tar i backen
        if(i == 1)
            if Y(i) < R(i)
                DD(i) = R(i) - Y(i);
            else
                DD(i) = 0;
            end
        %kollar om alla andra bollar krockar med bollen under    
        else
            if (Y(i) - Y(i - 1)) < (R(i) + R(i - 1)) 
                DD(i) = K(i - 1) * ((R(i) + R(i - 1) - (Y(i) - Y(i - 1))) / (K(i) + K(i - 1)));
            else
                DD(i) = 0;
            end
        end
        
        %kollar om alla förutom översta bollen krockar med ovanstående boll
        if(i ~= n)
            if (Y(i + 1) - Y(i)) < (R(i + 1) + R(i)) 
                DU(i) = K(i + 1) * ((R(i + 1) + R(i) - (Y(i + 1) - Y(i))) / (K(i + 1) + K(i)));
            else
                DU(i) = 0;
            end
        end
        
        F(i) = K(i) * DD(i) - K(i) * DU(i);
        V(i) = V(i) + ((-g + F(i)/M(i)) * dt);
        
        P(i) = M(i) * V(i);
        PTotal = PTotal + P(i);
        
        E(i) = (M(i) * g * Y(i)) + ((M(i) * V(i)^2)/2) + ((K(i) * (DU(i)^2 + DD(i)^2))/2);
        ETotal = ETotal + E(i);

        set(BOLL(i), 'Position', [x - (R(i) + DU(i) + DD(i)), Y(i) - (R(i) - DD(i)), 2 * (R(i) + DU(i) + DD(i)), 2 * R(i) - DU(i) - DD(i)]);
    end
    ETotal;
    PTotal;
    
    %för höjdgrafen
    %plotdata1(plotdata_i) = Y(1);
    
    %för rörelsemängdsgrafen
    %plotdata1(plotdata_i) = P(1);
    %plotdata2(plotdata_i) = P(2);
    
    %för energigrafen
    %plotdata1(plotdata_i) = E(1);
    %plotdata2(plotdata_i) = E(2);
    %plotdata3(plotdata_i) = E(3);
    %plotdata4(plotdata_i) = E(4);
    
    plotdata_i = plotdata_i + 1;
    
    pause(0.005);
end