%Solve an LCP by SMOOTH. Input M,q. Usage: x = smooth_lcp(M,q)

function x = smooth_lcp(M,q)

max_iter = 100;
epsilon=1.0e-6;
tosmall=1.0e-12;
iteration=0;
n = length(q);

x=zeros(n,1);
y=M*x+q;
error2=norm(x-plusf(x-y));
errorinf=norm(x-plusf(x-y),inf);
if errorinf<epsilon
	return;
end;
alpha=sqrt(n)/error2;
if alpha<1.0
	alpha=sqrt(alpha);
end;
fx=norm(x-pfun(x-y,alpha));

while iteration<max_iter & errorinf>epsilon
	grad=pfun(x-y,alpha)-x;
	Hession=diag(sfun(x-y,alpha))*M+diag(ones(n,1)-sfun(x-y,alpha));
	searchdir=Hession\grad;
	Mx=M*searchdir;

%line search begin
	lambda=1.0;
	fx1=1.0e+20;

	while fx1>fx & lambda>tosmall
		x1=x+lambda*searchdir;
		y1=y+lambda*Mx;
		lambda=lambda*0.75;
		fx1=norm(x1-pfun(x1-y1,alpha));
		fx1;
	end;
	if lambda<=tosmall
		disp('Line Search Fails');
		break;
	end;
	x=x1;
	y=y1;
	fx=fx1;	
%line search end

	iteration=iteration+1;
	error2=norm(x-plusf(x-y));
	errorinf=norm(x-plusf(x-y),inf);
	if errorinf<epsilon
		break;
	end;
	newalpha=sqrt(n)/error2;
	if newalpha<1.0
		newalpha=sqrt(newalpha);
	end;
	if newalpha>alpha
		alpha=newalpha;
		fx=norm(x-pfun(x-y,alpha));
	end;
end;

if iteration==max_iter
	disp('TOO Many iterations');
end
	
return;



