function [ c,ceq, gradc, gradceq ] = constraints( x, x_bar, robot, formation, A, b)

%% Setup
robot_r = double(robot.r);
robot_h = double(robot.h);


%% Formation
A=[A,zeros(size(A,1),1)];

r = formation.robot_pos;
f_centroid = formation.centroid;
f = formation.chull_pos;
d = formation.min_distance;

t = x(1:3);
t = [t, 0];
s = x(4);
q = x(5:8);

t_gtf = x_bar.position;
sbar = x_bar.scale;
qbar = x_bar.quaternion;

%% Constraints
c = [];
% nargout =3;
if nargout > 2
    gradc = [];
    gradceq = [];
end

for i=1:size(f,1)
    % Transformed locations of robots
    temp = t+[s*quatRotateVec(q',f_centroid'+f(i,:)')',0];
    
    c = [c;  A*temp' - b];
    
    if nargout > 2
       [r_rot,dr_rot] = quatRotateVec(q',f_centroid'+f(i,:)');
       gradc = [[A(:,1:3), A*[r_rot;0], s*A*[dr_rot(:,1:4); zeros(1,4)]]', gradc];
    end
end

for i=1:size(r,1)
        pts  = t+[s*quatRotateVec(q',r(i,:)')',0];
end

% Inter-robot distance constraint
c = [c; -1*s*d + 2*max(robot_r,robot_h)];

% Quaternion constraint
ceq = [norm(q,2)^2-1; q*[0,1,0,0]'; q*[0,0,1,0]'];

%Gradient required
if nargout > 2
    gradc = [gradc, [0,0,0,d,0,0,0,0]'];
    gradceq = [[0,0,0,0,2*q]',[0,0,0,0,0,1,0,0]',[0,0,0,0,0,0,1,0]'];
end

end
