%**********************************************************************
%* Initialize Variables
%**********************************************************************

%**********************************************************************
%* Note: x1,y1,x2,y2 are hardcoded here, but once we have this working,
%*       we can initialize them with calpoly
%**********************************************************************

%	qmin	= 1;	%???
%	qmax	= 2;	%???
%	gact	= 10;	%???

	w 	= 2;
%	interval=[0,1,0;1,0,0]; %interval polynomial
%	[ds] 	= calpoly(interval, qmin, qmax, w);
%	y1	= imag(ds(1));
%	y2	= imag(ds(2));
%	x1 	= real(ds(1));
%	x2 	= real(ds(2));


	qmin	= [1,2];
	qmax	= [3,4];
	x1 	= 1-qmax(2)*w^2;
	x2	= 1-qmin(2) *w^2;
	y1	= qmin(1)*w;
	y2	= qmax(1)*w;
%	x1	= -8;
%	x2	= -12;
	x_plus 	= max(x1, x2);
	x_minus = min(x1, x2);
	x_av	= (x_plus + x_minus) / 2;

%	y1	= 1;
%	y2	= 1; 
	y_plus 	= max(y1, y2);
	y_minus = min(y1, y2);
	y_av	= (y_plus + y_minus) / 2;

	B	= 1.4;
	N	= 5;
	M	= 2;
	NdivM=N/M
	gamma_max	= 20;
	
%**********************************************************************
	clg
	hold on
%	axis([-40 40 -30 30])
%axes
	xedge1=x_minus - NdivM;
	xedge2=x_plus + NdivM;
	yedge1=y_minus - NdivM;
	yedge2=y_plus + NdivM;
	%draw the bottom end pieces.
	%step=0.01
	step=0.1
	for yc = yedge1 : step : y_minus
		t_dist =sqrt(NdivM^2-(y_minus-yc)^2);
		xc=x_minus-t_dist;
		%plot(xc,yc);
		plot(-xc,-yc,'r');
		xc=x_plus+t_dist;
		%plot(xc,yc);
		plot(-xc,-yc,'r');
	end;
		
%fill([x1,x2,x2,x1],[y1,y1,y2,y2],'y');

plot([x_plus,x_minus,x_minus,x_plus,x_plus],[y_plus,y_plus,y_minus,y_minus,y_plus],'r');

%% These are the dashed lines splitting the hyperbola regions 
%% from the circle regions
%% 
%% Note... these numbers were hardcoded fror the specific problem.
%% Have not spent time to figure out how to parameterize them
%%
%plot([-x1,-x1],[-y1-15,-y1+15],':');
plot([-x1,-x1],[-y1-17,-y1+13],':r');
%plot([-x2,-x2],[-y2-15,-y2+15],':');
plot([-x2,-x2],[-y2-13,-y2+17],':r');

%plot([-x1-25,-x1+15],[-y1,-y1],':');
plot([-x1-28,-x1+20],[-y1,-y1],':r');
%plot([-x2-25,-x2+15],[-y2,-y2],':');
plot([-x2-20,-x2+28],[-y2,-y2],':r');

plot([0,0],[25,-25],'r');
plot([-25,25],[0,0],'r');
text(-29,4,'Value Set');
text(-27,22,'Actuator Boundary');
xlabel('xc');
ylabel('yc');

	%draw the top end pieces.
	for yc = y_plus : step : yedge2
		t_dist =sqrt(NdivM^2-(y_plus-yc)^2);
		xc=x_minus-t_dist;
		%plot(xc,yc);
		plot(-xc,-yc,'r');
		xc=x_plus+t_dist;
		%plot(xc,yc);
		plot(-xc,-yc,'r');
	end;
		
	% Draw the end lines
	%for yc = y_minus : step : y_plus
	
		%plot([xedge1,xedge1], [y_minus,y_plus],'y');
		plot(-[xedge1,xedge1], -[y_minus,y_plus],'r');
		%plot([xedge2,xedge2], [y_minus,y_plus],'y');
		plot(-[xedge2,xedge2], -[y_minus,y_plus],'r');
	end;
	
	%Now draw the top and bottom lines
	%for xc = x_minus : step : x_plus
		%plot([x_minus,x_plus], [yedge1,yedge1],'y');
		%plot([x_minus,x_plus], [yedge2,yedge2],'y');
		plot([-x_minus,-x_plus], [-yedge1,-yedge1],'r');
		plot([-x_minus,-x_plus], [-yedge2,-yedge2],'r');
		%plotpt(xc, y_max + (N/M),1);
		%plotpt(xc, y_max + (N/M),-1);
	end;

	% Now Draw the actuator boundary
	for xc = -1*gamma_max : step : gamma_max
	 	plot(xc,sqrt(gamma_max^2 - xc^2),'r');
	 	plot(xc,-1*sqrt(gamma_max^2 - xc^2),'r');
	end;

	%fplot('sqrt(gamma_max^2 - xc^2)',[-gamma_max gamma_max]);
%**********************************************************************
% Corner regions

%region 1

x_max = x_plus;
y_max = y_plus;
x_min = x_minus;
y_min = y_minus;

c_x = (B^2 * x_min - x_max)/(B^2 -1);
c_y = (B^2 * y_min - y_max)/(B^2 -1);
c_r = (B/(B^2-1)) * sqrt((x_max-x_min)^2 + (y_max-y_min)^2);

for xc = c_x - c_r : step : c_x + c_r
    if xc < x_minus
    	if (sqrt(c_r^2 - ((xc - c_x)^2)) + c_y) < y_minus
    		plot(-xc,-(sqrt(c_r^2 - ((xc - c_x)^2)) + c_y),'r');
    	end;
    	if (-sqrt(c_r^2 - ((xc - c_x)^2)) + c_y) < y_minus
    		plot(-xc,-(-sqrt(c_r^2 - ((xc - c_x)^2)) + c_y),'r');
    	end;
    end;
end;
%**********************************************************************
%region 2

x_max = x_plus;
y_max = y_minus;
x_min = x_minus;
y_min = y_plus;

c_x = (B^2 * x_min - x_max)/(B^2 -1);
c_y = (B^2 * y_min - y_max)/(B^2 -1);
c_r = (B/(B^2-1)) * sqrt((x_max-x_min)^2 + (y_max-y_min)^2);


for xc = c_x - c_r : step : c_x + c_r
    if xc < x_minus
    	if (sqrt(c_r^2 - ((xc - c_x)^2)) + c_y) > y_plus
    		plot(-xc,-(sqrt(c_r^2 - ((xc - c_x)^2)) + c_y),'r');
    	end;
    	if (-sqrt(c_r^2 - ((xc - c_x)^2)) + c_y) > y_plus
    		plot(-xc,-(-sqrt(c_r^2 - ((xc - c_x)^2)) + c_y),'r');
    	end;
    end;
end;

%**********************************************************************
%region 3

x_max = x_minus;
y_max = y_minus;
x_min = x_plus;
y_min = y_plus;

c_x = (B^2 * x_min - x_max)/(B^2 -1);
c_y = (B^2 * y_min - y_max)/(B^2 -1);
c_r = (B/(B^2-1)) * sqrt((x_max-x_min)^2 + (y_max-y_min)^2);


for xc = c_x - c_r : step : c_x + c_r
    if xc > x_plus
    	if (sqrt(c_r^2 - ((xc - c_x)^2)) + c_y) > y_plus
    		plot(-xc,-(sqrt(c_r^2 - ((xc - c_x)^2)) + c_y),'r');
    	end;
    	if (-sqrt(c_r^2 - ((xc - c_x)^2)) + c_y) > y_plus
    		plot(-xc,-(-sqrt(c_r^2 - ((xc - c_x)^2)) + c_y),'r');
    	end;
    end;
end;
%**********************************************************************
%region 4

x_max = x_minus;
y_max = y_plus;
x_min = x_plus;
y_min = y_minus;

c_x = (B^2 * x_min - x_max)/(B^2 -1);
c_y = (B^2 * y_min - y_max)/(B^2 -1);
c_r = (B/(B^2-1)) * sqrt((x_max-x_min)^2 + (y_max-y_min)^2);


for xc = c_x - c_r : step : c_x + c_r
    if xc > x_plus
    	if (sqrt(c_r^2 - ((xc - c_x)^2)) + c_y) < y_minus
    		plot(-xc,-(sqrt(c_r^2 - ((xc - c_x)^2)) + c_y),'r'); 
	end;
    	if (-sqrt(c_r^2 - ((xc - c_x)^2)) + c_y) < y_minus
    		plot(-xc,-(-sqrt(c_r^2 - ((xc - c_x)^2)) + c_y),'r');
    	end;
    end;
end;
%**********************************************************************
%Hyperbolas start here
%**********************************************************************
% region 7
x_max = x_plus;
y_max = y_minus;
y_min = y_plus;

c_y = (B^2 * y_min - y_max)/(B^2 -1);
c_ry = (B/(B^2-1)) * abs( (y_max-y_min));

if c_ry > 0 
	for xc = x_minus : step : x_av
		if (c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))) > y_plus
			plot(-xc,-(c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		%	plot(xc,(c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		end;
		if (c_y-sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))) > y_plus
			plot(-xc,-(c_y-sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		end;
	end;
end;
%**********************************************************************
%region 8
x_max = x_minus;
y_max = y_minus;
y_min = y_plus;

c_y = (B^2 * y_min - y_max)/(B^2 -1);
c_ry = (B/(B^2-1)) * abs( (y_max-y_min));


if c_ry > 0 
	for xc = x_av : step : x_plus
		if (c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))) > y_plus
			plot(-xc,-(c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		%	plot(xc,(c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		end;
		if (c_y-sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))) > y_plus
			plot(-xc,-(c_y-sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		end;
	end;
end;
%%**********************************************************************
%%region 11
x_max = x_minus;
y_max = y_plus;
y_min = y_minus;

c_y = (B^2 * y_min - y_max)/(B^2 -1);
c_ry = (B/(B^2-1)) * abs( (y_max-y_min));

if c_ry > 0
	for xc = x_av : step : x_plus
		if (c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))) < y_minus
			plot(-xc,-(c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		%	plot(xc,(c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		end;
		if (c_y-sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))) < y_minus
			plot(-xc,-(c_y-sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		%	plot(xc,(c_y-sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		end;
	end;
end;
%%**********************************************************************
%%region 12
x_max = x_plus;
y_max = y_plus;
y_min = y_minus;

c_y = (B^2 * y_min - y_max)/(B^2 -1);
c_ry = (B/(B^2-1)) * abs( (y_max-y_min));

if c_ry > 0
	for xc = x_minus : step : x_av
		if (c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))) < y_minus
			plot(-xc,-(c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		%	plot(xc,(c_y+sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		end;
		if (c_y-sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))) < y_minus
			plot(-xc,-(c_y-sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		%	plot(xc,(c_y-sqrt(c_ry^2+((xc-x_max)^2/(B^2-1)))),'r');
		end;
	end;
end;
%%**********************************************************************
%%region 5
x_max = x_plus;
y_max = y_plus;
x_min = x_minus;

c_x = (B^2 * x_min - x_max)/(B^2 -1);
c_rx = (B/(B^2-1)) * abs( (x_max-x_min));

if c_rx > 0
	for yc = y_minus : step : y_av
		if (c_x+sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))) < x_minus
			plot(-(c_x+sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))),-yc,'r');
		end;
		if (c_x-sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))) < x_minus
			plot(-(c_x-sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))),-yc,'r');
		end;
	end;
end;
%%**********************************************************************
%%region 6
x_max = x_plus;
y_max = y_minus;
x_min = x_minus;

c_x = (B^2 * x_min - x_max)/(B^2 -1);
c_rx = (B/(B^2-1)) * abs( (x_max-x_min));

if c_rx > 0
	for yc = y_av : step : y_plus
		if (c_x+sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))) < x_minus
			plot(-(c_x+sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))),-yc,'r');
		end;
		if (c_x-sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))) < x_minus
			plot(-(c_x-sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))),-yc,'r');
		end;
	end;
end;
%%**********************************************************************
%%region 9
x_max = x_minus;
y_max = y_minus;
x_min = x_plus;

c_x = (B^2 * x_min - x_max)/(B^2 -1);
c_rx = (B/(B^2-1)) * abs( (x_max-x_min));


if c_rx > 0
	for yc = y_av : step : y_plus
		if (c_x+sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))) > x_plus
			plot(-(c_x+sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))),-yc,'r');
		end;
		if (c_x-sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))) > x_plus
			plot(-(c_x-sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))),-yc,'r');
		end;
	end;
end;
%%**********************************************************************
%%region 10
x_max = x_minus;
y_max = y_plus;
x_min = x_plus;

c_x = (B^2 * x_min - x_max)/(B^2 -1);
c_rx = (B/(B^2-1)) * abs( (x_max-x_min));

if c_rx > 0
	for yc = y_minus : step : y_av
		if (c_x+sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))) > x_plus
			plot(-(c_x+sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))),-yc,'r');
		end;
		if (c_x-sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))) > x_plus
			plot(-(c_x-sqrt(c_rx^2+((yc-y_max)^2/(B^2-1)))),-yc,'r');
		end;
	end;
end;
%%%%**********************************************************************