%The second step is to write a constraint function for the
%optimization function fmincon. Constraints take the form of 
%equalities and/or inequalities and are stored in ceq and c

function [c,ceq] = constraint_u(x,D,H,beta)
B = beta/180*pi;

%The geometric parameters need to be calculated first
if beta<90
    xt = H/tan(B);
    yt = -H;
else
    xt = 0;
    yt = -H;
end

if x(1)<xt
    R = x(2)+D*H;
    
    x1 = x(1)-sqrt(R^2-x(2)^2);
    y1 = 0;
 
%x2f is the left intersection of the circle and the toe line, it is not 
%mentioned in the calculation of Part 1 but will be needed in the constraints
    if (R^2-(yt-x(2))^2) > 0
         xh =  x(1)+sqrt(R^2-(yt-x(2))^2);
         x2f = x(1)-sqrt(R^2-(yt-x(2))^2);
    else
         xh = 0;
         x2f = 0;
    end

    if xh > xt
        x2 = xh;
        y2 = -H;
    else if (x(1)-x(2)*tan(B))^2-(1+(tan(B))^2)*(x(1)^2+x(2)^2-R^2)>0
        x2 = (x(1)-x(2)*tan(B)+sqrt((x(1)-x(2)*tan(B))^2-...
        (1+(tan(B))^2)*(x(1)^2+x(2)^2-R^2)))/(1+(tan(B))^2);
        y2 = -x2*tan(B);
        else
            x2=xt;
            y2=yt;
        end
    end
else
    R = sqrt((x(2)-yt)^2+(xt-x(1))^2);
    
    x1 = x(1)-sqrt(R^2-x(2)^2);
    y1 = 0;
    
    x2=xt;
    y2=yt;
end

%Five constraints are specified. (1) x1 should be negative so that 
%the intersection of the circle and the crest line is always on the 
%left of the origin.(2) The center of the circle needs to be above 
%the crest line so x(2)=yc should be positive. (3) x2 must be 
%positive so the circle intersects the slope or the toe line. (4) 
%The left intersection of the circle and the toe line must be less 
%than xt so that the circle will not be in the air.

if x(1)<xt
    if D>1
        c(1)=x1+0.0010;
        c(2)=-x(2);
        c(3)=-x2+0.001;
        c(4)=x2f-xt+0.001;
        c(5)=x(1)-xt/2;%(5)Ensure the critical circle is a midpoint circle;
    else
        c(1)=x1+0.0010;
        c(2)=-x(2);
        c(3)=-x2+0.001;
        c(4)=-y2+x(2)-R+0.001;%(4)Ensure the critical circle tangential to 
        %the bottom line of D=1;
    end
        
else
%Four constraints are specified. (1) x1 should be negative so that
%the intersection of the circle and the crest line is always on the 
%left of the origin.(2) The center of the circle needs to be above 
%the crest line so x(2)=yc should be positive. (3) x2 must be 
%positive so the circle intersects the slope or the toe line.(4)Ensure the 
%center of the circle is on the right of x2;
    
    c(1)=x1+0.001;
    c(2)=-x(2);
    c(3)=-x2+0.001;
    c(4)=x2-x(1);
end
 
%The constraint of equality should be left blank
ceq=[];
end
