% function in = resetRobots(in,RA,RB,RC,boundaryR)
%     % Start agents from a random corner
%     corners = [10 10; -10 10; -10 -10; 10 -10];
%     startloc = randperm(4,2);
%     xA0 = corners(startloc(1),1);
%     yA0 = corners(startloc(1),2);
%     xB0 = corners(startloc(2),1);
%     yB0 = corners(startloc(2),2);
%     
%     % Set the variables in the simulation input object
%     in = setVariable(in,'xA0',xA0);
%     in = setVariable(in,'xB0',xB0);
%     in = setVariable(in,'yA0',yA0);
%     in = setVariable(in,'yB0',yB0);
%     
%     % Set a post sim function for visualization
%     in = setPostSimFcn(in,@(out) localPostSim(out,RA,RB,RC,boundaryR));
% end
function in = resetRobots(in,RA,RB,RC,boundaryR)
    % Random initial positions
    valid = false;
    while ~valid
        xA0 = -10 + 20*rand;
        yA0 = -10 + 20*rand;
        xB0 = -10 + 20*rand;
        yB0 = -10 + 20*rand;
        r = 0.5*boundaryR*rand;
        th = -pi + 2*pi*rand;
        xC0 = r*cos(th);
        yC0 = r*sin(th);
        dAB = norm([xA0-xB0;yA0-yB0]);
        dBC = norm([xB0-xC0;yB0-yC0]);
        dAC = norm([xA0-xC0;yA0-yC0]);
        dA = norm([xA0;yA0]);
        dB = norm([xB0;yB0]);
        valid = dA > boundaryR && dB > boundaryR && dAB > (RA+RB) && dBC > (RB+RC) && dAC > (RA+RC);
    end
    
    % Set the variables in the simulation input object
    in = setVariable(in,'xA0',xA0);
    in = setVariable(in,'xB0',xB0);
    in = setVariable(in,'yA0',yA0);
    in = setVariable(in,'yB0',yB0);
    
    % Set a post sim function for visualization
    in = setPostSimFcn(in,@(out) localPostSim(out,RA,RB,RC,boundaryR));
end
function out = localPostSim(out,RA,RB,RC,boundaryR)
    tsqA = localGetState(out,'qA');
    tsqB = localGetState(out,'qB');
    tsqC = localGetState(out,'qC');
    xA = tsqA.Data(:,1);
    xB = tsqB.Data(:,1);
    xC = tsqC.Data(:,1);
    yA = tsqA.Data(:,2);
    yB = tsqB.Data(:,2);
    yC = tsqC.Data(:,2);
    t = tsqA.Time;
    for i = 1:numel(xA)
        plotEnvironment([xA(i);xB(i);xC(i)], [yA(i);yB(i);yC(i)], [RA;RB;RC], boundaryR, t(i));
        %drawnow limitrate;
        %pause(0.01);
        drawnow()
    end
end
function ts = localGetState(out,name)
    stateObj = out.logsout.get(name);
    ts = stateObj.Values;
end
