function [beta,s,beta1,s1] = sreg(x,y,sub)

%function [beta,s,beta1,s1] = sreg(x,y,sub)
%S-estimator of regression, using Tukey's rho function
% (callibrated for 50% BDP and Normal consistency)
%
%INPUT ARGUMENTS:
%   x:      dependent variables (n x p)
%               NOTE: When x=0, S(0) is computed.
%   y:      independent variable (n x 1)
%   sub:    number of subsamples to find algorithm starting point (scalar)
%
%OUTPUT ARGUMENTS:
%   beta:   regression estimator (p x 1)
%   s:      associated scale estimator, callibrated for Normal consistency
%   beta1:  reweighted regression estimator (p x 1)
%   s1:     reweighted scale estimator
%

c = 1.547;
k = rhot(c,c)/2;

if x==0

beta=0;
s0 = 1.4826*median(abs(y));
err = 1; iter = 0;
while err>1.e-4 & iter<50,
	s = s0*sqrt(mean(rhot(y/s0,c))/k);
	err = abs(s/s0-1);
	iter = iter+1;
	s0 = s;
end

else

[n,p] = size(x);
cp = nchoosek(n,p);
if (3*cp)<sub, sub=3*cp; end

bopt = zeros(p,1);
s0 = 1.4826*median(abs(y));
err = 1; iter = 0;
while err>1.e-3 & iter<20,
	sopt = s0*sqrt(mean(rhot(y/s0,c))/k);
	err = abs(sopt/s0-1);
	iter = iter+1;
	s0 = sopt;
end
isub = zeros(1,n);
r = zeros(size(y));
for j = 1:sub,
    isub = randperm(n);
    xs = x(isub(1:p),:);
    ys = y(isub(1:p));
    if det(xs)~=0,
	bs = inv(xs)*ys;
	r = y-x*bs;
	if mean(rhot(r/sopt,c))<k,
	  bopt = bs;
	  s0 = 1.4826*median(abs(r));
	  err = 1; iter = 0;
	  while err>1.e-3 & iter<20,
		sopt = s0*sqrt(mean(rhot(r/s0,c))/k);
		err = abs(sopt/s0-1);
		iter = iter+1;
		s0 = sopt;
	  end
	end
    end
end
s0 = sopt;
beta0 = bopt;
err = 1; iter = 0;
while err>1.e-4 & iter<50,
	r = (y-x*beta0)/s0;
	w = psit(r,c)./(r+(r==0));
	beta = inv(x'*diag(w)*x)*x'*(w.*y);
	s = s0*sqrt(mean(rhot(r,c))/k);
	err = abs(s/s0-1);
	iter = iter+1;
	s0 = s;
	beta0 = beta;
end

r = (y-x*beta)/s;
w = r.^2<=chi2inv(.975,1);
beta1 = inv(x'*diag(w)*x)*x'*(w.*y);
s1 = sqrt(.975/chi2cdf(chi2inv(.975,1),3)*sum(w.*(y-x*beta1).^2)/sum(w));

end

% ------------------------------------------------------------

function y = rhot(x,c)
y=ones(size(x))*c^2/6;
i=find(abs(x)<=c);
y(i) = x(i).^2/2 - x(i).^4/(2*c^2) + x(i).^6/(6*c^4);

function y = psit(x,c)
y=zeros(size(x));
i=find(abs(x)<=c);
y(i) = x(i).*(1-(x(i)/c).^2).^2;