clear all;
numrobots = 3;
eps = 0.5;
x = 0;
dt = 0.2;
%=-=-=---=-=-=-=-=-=-Connection details=-=-=-=-=-=-=-=-==-=-=
REMIP = '128.97.19.104';
remoteHost = tcpip(REMIP,'RemotePort', 9613, 'LocalPort', 9613);
%remoteHost = tcpip('10.45.1.103',4551);
set(remoteHost, 'Timeout', 999);
set(remoteHost,'InputBufferSize',1000);
set(remoteHost,'OutputBufferSize',1000);
fopen(remoteHost);
%set(remoteHost, 'Timeout', 10);

while (x == 0)
    x = 1;
    robots = zeros(2*numrobots,1000,4);
    vec = zeros(numrobots+1,4);
    robots (1:numrobots,1,1) = 10 + 80*rand(numrobots,1);
    robots (1:numrobots,1,2) = 10 + 80*rand(numrobots,1);
    leader = zeros(1000,4);
    leader(1,1:2) = 50;
    t = 2;
    vec(1:numrobots,1:4) = robots(1:numrobots,1,1:4);
    vec(numrobots+1,1:4) = leader(1,1:4);
    dataSends = reshape(vec,[1,4*(numrobots+1)]);
    fwrite(remoteHost, dataSends, 'double');
    while(1)
        if(remoteHost.BytesAvailable == numrobots*8*4)
            dataRecds = fread(remoteHost, numrobots*4, 'double');
            break;
        end
    end
    dataRecds = reshape(dataRecds, numrobots, 4);
    robots(numrobots+1:2*numrobots,1,1:4) = dataRecds(1:numrobots,1:4);

    figure(1)
    hold on;
    subplot(1,3,1)
    plot(leader(1,1), leader(1,2), 'k+',...
        robots(1,1,1), robots(1,1,2), 'bo',...
        robots(2,1,1),robots(2,1,2), 'bo',...
        robots(3,1,1),robots(3,1,2), 'bo')
    axis([0 100 0 100]); axis('square');
    title('LA robots');

    subplot(1,3,2)
    plot(robots(4,1,1), robots(4,1,2), 'ro',...
        robots(5,1,1),robots(5,1,2), 'ro',...
        robots(6,1,1),robots(6,1,2), 'ro')
    axis([0 100 0 100]); axis('square');
    title('UC robots');

    subplot(1,3,3)
    plot(leader(1,1), leader(1,2), 'k+',...
        robots(1,1,1), robots(1,1,2), 'bo',...
        robots(2,1,1),robots(2,1,2), 'bo',...
        robots(3,1,1),robots(3,1,2), 'bo',...
        robots(4,1,1), robots(4,1,2), 'ro',...
        robots(5,1,1),robots(5,1,2), 'ro',...
        robots(6,1,1),robots(6,1,2), 'ro')
    axis([0 100 0 100]); axis('square');
    title('All robots');
    hold off;

    
    while (t <= 1000)
        robots = odesolve(robots, leader, t, 2*numrobots);
        leader(t,3) = 2*cos((pi/180)*t);
        leader(t,4) = 2*sin((pi/180)*t);
        leader(t,1) = leader(t-1,1) + dt*leader(t,3);
        leader(t,2) = leader(t-1,2) + dt*leader(t,4);
        for i = 1:numrobots
            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
        vec(1:numrobots,1:4) = robots(1:numrobots,t,1:4);
        vec(numrobots+1,1:4) = leader(t,1:4);
        dataSend = reshape(vec,[1,4*(numrobots+1)]);
        fwrite(remoteHost, dataSend, 'double');
        while(1)
            if(remoteHost.BytesAvailable == numrobots*8*4)
                dataRecd = fread(remoteHost, numrobots*4, 'double');
                break;
            end
        end
        dataRecd = reshape(dataRecd, numrobots, 4);
        robots(numrobots+1:2*numrobots,t,1:4) = dataRecd(1:numrobots,1:4);
        t = t + 1;
    end
end
for t=1:1000
    figure(1)
    subplot(1,3,1)
    plot(leader(1:t-1,1), leader(1:t-1,2), 'k', leader(t,1), leader(t,2), 'k+',...
        robots(1,1:t-1,1),robots(1,1:t-1,2), 'b', robots(1,t,1), robots(1,t,2), 'bo',...
        robots(2,1:t-1,1),robots(2,1:t-1,2), 'b',robots(2,t,1),robots(2,t,2), 'bo',...
        robots(3,1:t-1,1),robots(3,1:t-1,2), 'b',robots(3,t,1),robots(3,t,2), 'bo')
    axis([0 100 0 100]); axis('square');
    title('LA Robots');
    
    subplot(1,3,2)
        plot(robots(4,1:t-1,1),robots(4,1:t-1,2), 'r',robots(4,t,1),robots(4,t,2), 'ro',...
        robots(5,1:t-1,1),robots(5,1:t-1,2), 'r',robots(5,t,1),robots(5,t,2), 'ro',...
        robots(6,1:t-1,1),robots(6,1:t-1,2), 'r',robots(6,t,1),robots(6,t,2), 'ro')
    axis([0 100 0 100]); axis('square');
    title('UC robots');
    
    subplot(1,3,2)
        plot(leader(1:t-1,1), leader(1:t-1,2), 'k', leader(t,1), leader(t,2), 'k+',...
        robots(1,1:t-1,1),robots(1,1:t-1,2), 'b', robots(1,t,1), robots(1,t,2), 'bo',...
        robots(2,1:t-1,1),robots(2,1:t-1,2), 'b',robots(2,t,1),robots(2,t,2), 'bo',...
        robots(3,1:t-1,1),robots(3,1:t-1,2), 'b',robots(3,t,1),robots(3,t,2), 'bo',...
        robots(4,1:t-1,1),robots(4,1:t-1,2), 'r',robots(4,t,1),robots(4,t,2), 'ro',...
        robots(5,1:t-1,1),robots(5,1:t-1,2), 'r',robots(5,t,1),robots(5,t,2), 'ro',...
        robots(6,1:t-1,1),robots(6,1:t-1,2), 'r',robots(6,t,1),robots(6,t,2), 'ro')
    axis([0 100 0 100]); axis('square');
    title('All robots');
    
    M(t) = getframe;
end
%fclose(remoteHost);