% Plot a stagnation point flow
%

 
clear; close all; clc

% Get the constants we need
%x(1)=input('What initial x position:');
%y(1)=input('What initial y position:');
%u=input('What is the velocity constant U:');
numpts=input('How many points to evaluate:');

numUnits = 4;

C0 = 1000; % Speed of sound

u=.005;

x(1,1) = -500;
x(1,2) = -450;
x(1,3) = -475;
x(1,4) = -490;
y(1,1) = 50;
y(1,2) = -150;
y(1,3) = 10;
y(1,4) = -70;


for j=1:numUnits
  for i=2:numpts
    x(i,j) = x(i-1,j)-u*x(i-1,j);
    %x(i,2) = x(i-1,2)-u*x(i-1,2);
    y(i,j) = y(i-1,j)+u*y(i-1,j);
    %y(i,2) = y(i-1,2)+u*y(i-1,2);
  end
end


Tij = zeros(numUnits,numUnits,numpts);

for k = 1:numpts
  for i = 1:numUnits
    for j = 1:numUnits
        Tij(i,j,k) = 2*sqrt((x(k,i)-x(k,j))^2+(y(k,i)-y(k,j))^2)/C0;               
    end
  end
  
end

 
% display results as animation
figure
hold on; 

for n=1:numpts
  for j=1:numUnits
    plot(x(n,j),y(n,j),'ro');
  end
[X0 Y0 nu] = ComputePositions( Tij(:,:,n), C0, x(n,:), y(n,:));  
%plot(X0,Y0,'x');


%Introduce random error based on 25m initial position from each other at deployment
deplyDistance = 25;
pctError = .4;
Error = pctError*deplyDistance*(rand-.5);

X0 = X0+Error;
Y0 = Y0+Error;

plot(X0,Y0,'x');

  %plot(x(n,2),y(n,2),'ro');
  %axis equal;
  %axis([min(x)-10,max(x)+10,min(y)-10,max(y)+10]);
  axis([-550 10 -250 150]);
  


end
xlabel('x (m)');
          ylabel('y (m)');
          title('Relative Instrument Positioning Simulation');
         % M(n)=getframe;
 
% play as smooth movie 3 times at 10 frames per second
% note that it goes through the frames 2x initially to get ready for
% full speed play.  So it will actually play 2x slower and 3x at full speed.
%numtimes=1;
%fps=10;
%movie(M,numtimes,fps)