clear all;
numrobots = 3;
eps = 0.2;
x = 0;
dt = 0.01;
%=-=-=---=-=-=-=-=-=-Connection details=-=-=-=-=-=-=-=-==-=-=
% REMIP = '128.97.19.104';
% remoteHost = tcpip(REMIP,'RemotePort', 2448, 'LocalPort', 2448);
% set(remoteHost, 'Timeout', 999);
% fopen(remoteHost);
% set(remoteHost, 'Timeout', 0.1);

while (x == 0)
    x = 1;
    %newrobinfo = zeros(5,1000,3);
    robots = zeros(numrobots,1000,4);
    robots (1:numrobots,1,2) = 0:numrobots-1;
    t = 2;
    %xreply = input('X-coordinate goal. Type 100 if done');
    xreply = -1;
    if (xreply == 100)
        break;
    end
    %yreply = input('Y-coordinate goal');
    yreply = 10;
    leader = true;
    target = [xreply, yreply];
    axis([-1, 8, -9, 9]);
    plot(robots(1,1,1),robots(2,1,2), 'r+',...
        robots(2,1,1),robots(2,1,2), 'g+',...
       robots(3,1,1),robots(3,1,2), 'm+',...
        xreply, yreply, 'ro');
    while (abs(robots(1,t-1,1)-xreply) > eps || abs(robots(1,t-1,2)-yreply) > eps)
        for i = 1:numrobots
            robots = odesolve(robots, leader, t, numrobots, target);
            robots(i,t,1) = robots(i,t-1,1) + dt*robots(i,t,3);
            robots(i,t,2) = robots(i,t-1,2) + dt*robots(i,t,4);
        end
%         dataSend = reshape(dataSend,[1,12]);
%         fwrite(remoteHost,dataSend,'double');
        axis([-1, 8, -9, 9]);
        plot(robots(1,1:t-1,1),robots(1,1:t-1,2), 'r', robots(1,t,1), robots(1,t,2), 'r+',...
        robots(2,1:t-1,1),robots(2,1:t-1,2), 'g',robots(2,t,1),robots(2,t,2), 'g+',...
        robots(3,1:t-1,1),robots(3,1:t-1,2), 'm',robots(3,t,1),robots(3,t,2), 'm+',...
         xreply, yreply, 'ro');
        M(t-1) = getframe;
        t = t + 1;
    end
end