function h = findCHOP(tau,robots,s,g,R,vmax)
Nm = length(robots);
%find CHOP center
xh = [sum(s(:,1)), sum(s(:,2))]/Nm;
%find number of intermediate waypoints
Nh = 2*Nm;
delta_theta = 2*pi/Nh;
theta = zeros(length(Nh),1);
for i = 1:length(theta)
   theta(i) = (i-1)*delta_theta; 
end
min_radius = sqrt(2)*R/sin(delta_theta/2);
collision_radius = zeros(length(g),2);
for i = 1:length(g)
    dist = sqrt((xh(1)-g(i,1))^2 + (xh(2)-g(i,2))^2);
    collision_radius(i,:) = [dist - sqrt(2)*R, dist+sqrt(2)*R];
end
Rh = max(collision_radius, 'all');
for i = 1:length(collision_radius)
    temp = collision_radius(i,1);
    for j = 1:length(collision_radius)
       if( temp > collision_radius(i,1) && temp < collision_radius(i,2))
           temp = Rh;
       end
    end
    if( temp < Rh )
        Rh = temp;
    end
    temp = collision_radius(i,2);
    for j = 1:length(collision_radius)
       if( temp > collision_radius(i,1) && temp < collision_radius(i,2))
           temp = Rh;
       end
    end
    if( temp < Rh )
        Rh = temp;
    end
end
temp = min_radius;
for j = 1:length(collision_radius)
   if( temp > collision_radius(i,1) && temp < collision_radius(i,2))
       temp = Rh;
   end
end
if( temp < Rh )
    Rh = temp;
end
xw = zeros(length(theta),2);
for i = 1:length(Rh)
    xw(i,:) = [xh(i,1)+Rh*cos(theta(i)), xh(i,2)+Rh*sin(theta(i))];
end
%identify entry waypoints
Xw = zeros(ceil(length(xw)/2),2);
for i = 1:length(Xw)
    Xw(i,:) = xw(2*i-1,:);
end
phi_g = zeros(length(g),2);
for i = 1:length(g)
    min_dist = sqrt((xw(1,1)-g(i,1))^2 + (xw(1,2)-g(i,2))^2);
    min_idx = 1;
    for j = 2:length(xw)
        dist = sqrt((xw(j,1)-g(i,1))^2 + (xw(j,2)-g(i,2))^2);
        if dist < min_dist
           min_dist = dist;
           min_idx = j;
        end
    end
    phi_g(i,:) = xw(min_idx,:);
end
P = cell(length(phi_g),1);
for i = 1:length(P)
    for j = 1:length(P)
        if i~=j
            angle = atan2(g(j,2)-phi_g(j,2),g(j,1)-phi_g(j,1));
            xp = [phi_g(j,1)+R*cos(angle+pi/2), g(j,1)+R*cos(angle+pi/2), g(j,1)-R*cos(angle-pi/2), phi_g(j,1)-R*cos(angle-pi/2), phi_g(j,1)+R*cos(angle+pi/2)];
            yp = [phi_g(j,2)+R*sin(angle+pi/2), g(j,2)+R*sin(angle+pi/2), g(j,2)-R*sin(angle-pi/2), phi_g(j,2)-R*sin(angle-pi/2), phi_g(j,2)+R*cos(angle+pi/2)];
            if(inpolygon(g(i,1),g(i,2),xp,yp))
                P(i) = {[P(i); j]};
            end
        end
    end
end
[Mh,taufh] = findCHOPMotionPlan(taush,Rh,sh,xw,Nh,Xw,phi_g,P,gh,vmax);
h={taush,Rh,sh,gh,xh,Nh,Rh,taufh,Mh};
end



