%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Template code for flocking                              %
% Kirstin Petersen, Asst. Prof. ECE, Cornell University   %
% Homework for SYSEN 6000 May 5th 2017                    %
% Updated May 17th 2017                                   % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Flocking()
    close all;
    
    %Settings
    N = 400;        %No. of boids
    frames = 200;   %No. of frames in movie
    limit = 40;    %Axis limits
    P = 10;         %Spread of initial position (gaussian)
    V = 10;         %Spread of initial velocity (gaussian)
    delta = 0.01;  %Time step
    c1 = 0.1;       %Attraction scaling factor
    c2 = 5;         %Repulsion scaling factor
    c3 = 1.1;       %Heading scaling factor
    c4 = 0.1;         %Randomness scaling factor
    vlimit = 50;    %Maximum velocity
    
    %Video-file
    myVideo = VideoWriter('flocking.avi');
    myVideo.FrameRate = 30;  % Default 30    
    myVideo.Quality = 50;    % Default 75
    M=moviein(frames);
    
    %Initialize
    p = P*randn(2,N); 
    v = V*randn(2,N); 
    hist = p; %For plotting the paths of all boids
    plot(p(1,:),p(2,:),'k+','Markersize',4);
   
    %Main loop
    for(k=1:frames)
        v1=zeros(2,N);
        v2=zeros(2,N);
        v3 = [sum(v(1,:))/N; sum(v(2,:))/N]*c3;             %Calculate average veolcity
        if(norm(v3) > vlimit), v3 = v3*vlimit/norm(v3); end %Limit max velocity
        for n=1:N
            for m=1:N
                if m ~= n
                    r = p(:,m)-p(:,n);                  %Vector from one agent to the next 
                    rmag=sqrt(r(1)^2+r(2)^2);           %Distance between agents
                    v1(:,n) = v1(:,n) + c1*r;           %Attraction
                    v2(:,n) = v2(:,n) - c2*r/(rmag^2);  %Repulsion (non-linear scaling)
                end
            end
            v4(:,n) = c4*randn(2,1);                    %Add randomness to motion
            v(:,n) = v1(:,n)+v2(:,n)+v4(:,n)+v3;   %Update velocity
        end
        p = p + v *delta;            %Update position
        hist = [hist p];             %Append new positions
        
        %Update plot:
        plot(hist(1,:),hist(2,:),'.','Markersize',0.5);
        hold on;
        plot(p(1,:),p(2,:),'k+','Markersize',4);
        axis([-limit limit -limit limit]);
        %quiver(p(1,:),p(2,:),v(1,:),v(2,:)); %For drawing velocity arrows
        drawnow;
        M(:,k) = getframe;
        hold off;
    end

    open(myVideo);
    writeVideo(myVideo, M);
end