%flight of a bumblebee

% idea: constant speed with diffusing  direction theta

% in addition  introduce some rules for how they orient themselves compared
% to their neighbors in an attempt to produce swarming
clear all

set(0,                           ...
   'defaultaxesfontsize', 20,   ...
   'defaultaxeslinewidth', 1.0, ...
   'defaultlinelinewidth', 1.2, ...
   'defaultpatchlinewidth', 0.7);
% set up scenarios
% for each scenario specify parameter values
%default values:
V = 1;  % specify the ballistic velocity
N=100;  %number of fliers
%ncase = 1; % in this case flyers look at neighbor's direction of flight
%ncase = 2;  % in this case flyers look at the direction toward their neighbors
ncase = 1;
theta_p = pi; % angle of peripheral vision <= pi
rho = 2.5 ;  % the length constant for effective sight
D_th = 0.001; %  the diffusion coefficient for theta
r0 =1;  % radius of initial distribution of points
n_time_steps = 2000;  %specify the number of time steps
add_dir=0 ; % =1 means add direction and target direction to plots of points 
plot_paths = 1;% =1 means plot pats, =0 means view motion as a movie
alpha = 1; % rate constant for adjusting direction angle

% pick a scenario
scenario = 9;
switch scenario
    case 1
 % use default values
 
 add_dir=1;
 
  case 2
        D_th = 0.5;
        n_time_steps = 4000;
        alpha=10;
        N=30;
        theta_p =  3*pi/4;
    case 3
        theta_p=pi/2;  %limited vision angle
       rho = 2.5;
        
    case 4
        rho = 3.5;
        theta_p=pi/4;
        alpha = 10;
        D_th=0.005;
     
    case 5  % circular motion
        D_th = .000001;
        ncase = 2;
        N=150;
        rho = 2.5;
        plot_paths=0;
        n_time_steps = 2000;
        alpha = 0.75;
        
        theta_p=pi;
        
   case 6
        D_th = 0.0001;
       ncase = 2;
        N=150;
        rho = 1;
        plot_paths=0;
        n_time_steps = 3000;
        alpha = 0.75;
        
    case 7 %gnats
        ncase = 2;
        N=100;
        D_th = 2;
        plot_paths=0;
        alpha = 1;
        theta_p=pi/2;
     rho=2;
     
     n_time_steps = 3000;
     case 8 % linear flight
        ncase = 2;
        N=100;
        D_th = 2;
        plot_paths=0;
        alpha = 10;
        theta_p=pi/2;
     rho=1;
     n_time_steps = 3000;
     case 9 % the slug
        ncase = 2;
        N=150;
        D_th = 0.1;
        plot_paths=0;
        alpha = 10;
        theta_p=pi/2;
     rho=2;
     n_time_steps = 3000;
end      

 
rsq = rho^2;
% radius of influence weight function
 
% initial position  
% c is the position vector for the particles
ci=rand(N,2); %initial positions
% distributed inside a circle of radius r0
 
 c =  r0*[ ci(:,1).*cos(2*pi*ci(:,2)), ci(:,1).*sin(2*pi*ci(:,2))];

theta = 2*pi*rand(N,1);  % random initial direction
dir = [cos(theta),sin(theta)]; % direction of flight for each particle 
figure(1)
plot(c(:,1),c(:,2),'*')
title('Initial/Final Positions','fontsize',20)
xlabel('x','fontsize',20)
ylabel('y','fontsize',20)
hold on
% for j = 1:N
%     plot([c(j,1),c(j,1)+dir(j,1)],[c(j,2),c(j,2)+dir(j,2)])
% end
 
dt = 0.01;  %time step

sc = sqrt(2*D_th*dt);

X = c(:,1); % store the position vector
Y = c(:,2);
T = [];

for j = 1:n_time_steps % number of time steps
 
 % current direction of flight is stored in dir
 
%direction between particles
vx = -c(:,1)*ones(1,N) + ones(N,1)*c(:,1)';  %vx(i,j) is the x component of the vector from i to j
vy = -c(:,2)*ones(1,N) + ones(N,1)*c(:,2)';  %vy(i,j) is the y component of the vector from i to j

for k = 1:N
    vx(k,k) = dir(k,1); %make the direction to self the dir vector
    vy(k,k) = dir(k,2);
end

dist  =  vx.^2+vy.^2;  %distance between particles
% this is a symmetric matrix

%rescale v
vx=vx./sqrt(dist); % (vx(i,j),vy(i,j)) is a unit vector pointing toward a neighboring particle
vy = vy./sqrt(dist);

% the angle between current direction and another particle 
 costh = vx.*( dir(:,1)*ones(1,N))+vy.*( dir(:,2)*ones(1,N));
cosmsk = (costh>=cos(theta_p)); % determines the particles  within peripheral vision

 
 if (ncase ==1) 
     for k = 1:N
    dist(k,k) = 0;  % distance to self is zero
end
distscal =  exp(-dist/rsq); % the distance weight function   
% so that the current direction is given the most weight
dirx = (ones(N,1)*dir(:,1)').*distscal;
diry = (ones(N,1)*dir(:,2)').*distscal;
  %this is the scaled direction vector 
 
xvn = sum(dirx.*cosmsk,2);  %add up all the directions within peripheral vision scaled by their distance away
yvn = sum(diry.*cosmsk,2);  % this includes the current direction in the sum
 
% now rescale these vectors to be unit length
lv = xvn.^2+yvn.^2;
newdirx =   xvn./sqrt(lv);   %This is the target x direction
newdiry =  yvn./sqrt(lv); % This is the target y direction


 end
 if(ncase ==2)

      for k = 1:N
    dist(k,k) = 10;  % distance to self is large to keep self
    %from having too big an influence.  Resulting flight patterns may be
    %influenced by this number
end
distscal =  exp(-dist/rsq); % the distance weight function 
 
% a different possible target
% move toward the particles that can be seen
xd = vx.*distscal; %this is the scaled distance vector 
yd = vy.*distscal;
%xd = vx.*distscal.*sqrt(dist); %to use center of mass 
%yd = vy.*distscal.*sqrt(dist);
xdn = sum(xd.*cosmsk,2);  %These are the distances to the positions that can be seen
ydn = sum(yd.*cosmsk,2);
% now rescale these vectors to be unit length
ld = xdn.^2+ydn.^2;

newdirx =  xdn./sqrt(ld) ;  %This is the target x direction
newdiry =  ydn./sqrt(ld);% This is the target y direction
 end
  
thmsk = (newdiry>=0);
thn = acos(newdirx);
thtarget = thn.*thmsk + (2*pi-thn).*(1-thmsk); % this is the target angle  

 
% create the differential equation rhs for theta
thrhs = alpha.*(mod(thtarget-theta+pi,2*pi)-pi);   
 
dtheta = sc*randn(N,1);  % diffusive part of dtheta
 %take a forward time step for theta
theta = theta + dtheta + dt*thrhs;
theta = mod(theta,2*pi);
dir = [cos(theta),sin(theta)];  % current direction of flight
cnew = c + V* dt*dir;
c = cnew; %update the positions
X = [X,c(:,1)]; % store the trajectories
Y = [Y,c(:,2)];

T(j) = j*dt;
%keep track of the mean swarm location
mnx(j) = mean(c(:,1));
mny(j) = mean(c(:,2));
mndirx(j) = mean(dir(:,1));
mndiry(j) = mean(dir(:,2));
mnnewdirx(j) = mean(newdirx);
mnnewdiry(j) = mean(newdiry);
vr(j) = (sum((c(:,1)-mnx(j)).^2)+sum((c(:,2)-mny(j)).^2))/N;
tgdir = [cos(thtarget),sin(thtarget)];
end
mx = max(max(abs(c)));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1) % plot the final positions
plot(c(:,1),c(:,2),'*')
xlabel('x','fontsize',20)
ylabel('y','fontsize',20)

 
if (add_dir==1)
  for j = 1:N
      plot([c(j,1),c(j,1)+dir(j,1)],[c(j,2),c(j,2)+dir(j,2)])
      plot([c(j,1),c(j,1)+tgdir(j,1)],[c(j,2),c(j,2)+tgdir(j,2)],'--')
  end
end
%  
%axis([-mx mx -mx mx])
  plot(mnx,mny,'linewidth',2)
 hold off
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure(3) % plot the mean position 
 plot(mnx,mny,'linewidth',2)
 xlabel('x','fontsize',20)
ylabel('y','fontsize',20)
title('Center of Mass')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure(2) % a plot of the particle movement
 mnx = min(X,[],'all');
     mxx = max(X,[],'all');
     mny = min(Y,[],'all');
     mxy = max(Y,[],'all');
     ydf = mxy-mny;
     xdf = mxx-mnx;
     mxdf = max(xdf,ydf);
 if(plot_paths)
     for j=1:N
         plot(X(j,:),Y(j,:),'b','linewidth',1)
         hold on
     end
     plot(c(:,1),c(:,2),'*')
     xlabel('x','fontsize',20)
ylabel('y','fontsize',20)
      axis([mnx mnx+mxdf mny mny+mxdf])
     hold off
 else
     
     
 for j = 1:n_time_steps/10
     plot(X(:,10*j),Y(:,10*j),'*','linewidth',2)
    xlabel('x','fontsize',20)
ylabel('y','fontsize',20)
    axis([ mnx mxx mny mxy])
     pause(0.1)
 end
 end
  
 hold off
 
 figure(4) % plot the variance
 plot(T,vr,'linewidth',2)
 xlabel('Time','fontsize',20)
 ylabel('Variance','fontsize',20)
 
  figure(5) % plot a sample path
  plot(X(1,:),Y(1,:),'b','linewidth',2)
  xlabel('x','fontsize',20)
ylabel('y','fontsize',20)
title('Sample path')