function [Mh,taufh] = findCHOPMotionPlan(taush,Rh,sh,xw,Nh,Xw,phi_g,P,gh,vmax)
%move robots into the CHOP
%syms t;
tprev = taush;
[M0, tnext] = CAPT(sh,Xw,vmax,tprev);
scurr = Xw;
Rg = [];
j = 1;
Mh = M0;
while ~all(ismember(Rg,Rh))
    %construct the goal set for robots still in CHOP
    gcurr = []
    for i = 1:length(Rh)
       if ~any(ismember(Rh(i),Rg))
           if(scurr(i,:)==phi_g(i,:) && isempty(P{i}))
               gcurr = [gcurr; gh(i,:)];
           else
              [contains, idx] = ismember(scurr(i,:),xw,'rows');
              next_idx = idx+1;
              if(next_idx > length(gh))
                  next_idx = 1;
              end
              gcurr = [gcurr; xw(next_idx,:)];
           end
       end       
    end
    %get next trajectory segment
    tprev = tnext;
    [Mcapt,tnext] = CAPT(scurr,gcurr,tprev);
    Mindex = 1;
    for i = 1:length(Rh)
        if(any(ismember(Rh(i),Rg)))
            %Mh(i) = piecewise((tprev<=t)&(t<tnext),gh(i,:),Mh(i));
            Mh{i,1} = [Mh{i,1}; tnext];
            Mh{i,2} = [Mh{i,2}; gh(i,:)];
        else
            %Mh(i) = piecewise((tprev<=t)&(t<tnext),Mcapt(Mindex),Mh(i));
            Mh{i,1} = [Mh{i,1}; tnext];
            Mh{i,2} = [Mh{i,2}; Mcapt{Mindex,2}(end,:)];
            Mindex = Mindex + 1;
        end
    end
    %record robots that arrived at goals
    Rgnew = [];
    for i = 1:length(gcurr)
       [contains, idx] = ismember(gcurr(i,:),gh,'rows');
       if any(contains)
           Rgnew = [Rgnew; Rh(idx)];
           taufh(i) = tnext;
       end
    end
    Rg = [Rg; Rgnew];
    %define new start set as robots not yet at their goals
    scurr = [];
    for i = 1:length(gcurr)
       [contains, idx] = ismember(gcurr(i,:),gh,'rows');
       if ~any(contains)
           scurr = [scurr;gcurr(i,:)];
       end
    end
    j = j+1;
end
end