function [ysm,res]=rbsp(t,x,Tensn,ishow)
% function [ysm,res]=rbsp(t,x,Tensn,ishow), Ver. 991101 by LDM
% Robust smoother as suggested by Peter Bloomfield.  
% USES regress1 and Multi-diagonal m-files 
%     TDtrp, DxTD,TDxTD,TDxV,PDsolv.  
% ONLY ARGUMENTS x,t,Tensn ARE NECESSARY.
% (Default value is ishow=0)
% Inputs:
%   x = Nx1 vector, the dependent series
%   t= Nx2 vector of time (or other independent variable) 
%   Tensn = scaler tension parameter, positive or negative. Increasing Tensn
%      increases the smoothing of the output series.
%   ishow = output flag
%     If ishow=0 or is missing, there is no internal output (default).
%     If ishow=1, graph of smoothed and original series are 
%             shown superimposed for color monitors.
% Outputs:
%   ysm = Nx1 vector of smoothed series
%   res = Nx1 vector of residuals from smoothed series (rsm=x-ysm)
% The program uses an adaptive weighting scheme with "flattened weights"
% (Mosteller and Tukey, pg. 368) to fit a natural spline smoothed version 
% (Lancaster pg 110), y, of x. 

[N,m]=size(x);
if N<=m, error(' Series should be input as a column vector'), end,
if [N,m]~=size(t), 
        error('Input series and time values have different dimensions'), 
end;
if t(2)-t(1)<0,
   dflag=1;
   t=flipud(t);
   x=flipud(x);
else
   dflag=0;
end
if length(find(diff(t)==0))>0,
  error('Repeated time values exist. Use rbspmp.m for this case.')
end

% Initialize
if nargin==3,
        ishow=0;
end % o f if-block
ysm=zeros(size(x));      

%Set internal parameters
pwr=1;tol=.01;mscale=.5;

if ishow>0,
  disp('Smoother is running ...'),
end

% Set up the necessary matrices
dlt=diff(t);
idlt=dlt.^(-1);

%Set up the adjoined tri-diagonal matrix
% C1 set up as NxN tri-diagonal: 
%    top and bottom rows dropped later

C11=[0;idlt(1:N-2);0];
C12=[0;-(idlt(1:N-2)+idlt(2:N-1));0];
C13=[0;idlt(2:N-1);0];
C1=[C11,C12,C13];
%Set up tridiagonal matrix
C21=[0;dlt(2:N-2)/6];
C23=[dlt(2:N-2)/6;0];
C22=[(dlt(1:N-2)+dlt(2:N-1))/3];
C2=[C21,C22,C23];

% Initialize the N-2 second derivatives, (First and last = 0 for natural splines
z=zeros(N-2,1);
w=ones(1,N);

% Initialize y by linear regression.
y=regress1(x,t);
res=x-y;
ccost=res'*res;
imprv=1;
tmp=TDxV(C1,x);
C1x=tmp(2:N-1);

relwt=exp(2-29*sign(Tensn)*abs(Tensn^(1/1.3)));

% Begin the iterative minimization
Nc=0;
while imprv>=tol,

% Calculate the weights
        mr=mscale*median(abs(res));
        ind1=find(abs(res)<=mr);
        ind2=find(abs(res)>mr);
        w(ind1)=mr^(pwr-2)*ones(1,length(ind1));
        w(ind2)=abs(res(ind2)).^(pwr-2);
        w=w(:);
        W=relwt*w;
        invW=W.^(-1);
% Calculate transpose of C1
        C1trp=TDtrp(C1);
        tmp1=DxTD(invW,C1trp);
        tmp=TDxTD(C1,tmp1); %PentaDiagonal Form
% Delete top and bottom rows
        tmp=tmp(2:N-1,:);
% Convert TriDiag C2 to PentaDiagonal
        C2p=[zeros(N-2,1),C2,zeros(N-2,1)];
        A=tmp+C2p;
        z=PDSolv(A,C1x);
        za=[0;z;0];
        ysm=x-TDxV(tmp1,za);
        res=x-ysm;
	Cost1=sum(W.*(res.^2));
	Cost2=z'*TDxV(C2,z);
        ncost=Cost1+Cost2;
        imprv=abs((ccost-ncost)/ccost);
	impstr=num2str(imprv);
        if ishow>0,disp([' Current Relative Improvement = ',impstr]),end
        ccost=ncost;
        Nc=Nc+1;
        if Nc>10,
           disp('Iterations are not converging.')
           imprv=0;
        end
end %end while-loop

res=x-ysm;
if dflag==1,
   t=flipud(t);
   x=flipud(x);
   ysm=flipud(ysm);
   res=flipud(res);
end
if ishow>=1,
  clf,
  lst=num2str(Tensn);
  %pct=100*(std(ysm)/std(x)).^2;%pctvar(x,ysm);
  %pctst=num2str(100-pct); 
  subplot(2,1,1)
  plot(t,x,'r--',t,ysm,'g' ),
  title(['Original and Smoothed Series: Tension =   ',lst]),
  subplot(2,1,2),  
  plot(t, [res zeros(N,1)]),
  title('Residuals from the smoothing.')
end %if-block

% Allow next figure to plot on the full page
subplot(111);

function Ttrp=TDtrp(T)
% Ttrp=TDtrp(T); Function to calculate transpose of a tri-diagonal matrix
% in tri-diagonal form.

[n,m]=size(T);
if m~=3, error('Input should be in TriDiagonal (nx3) form'),end
C1=[0;T(1:n-1,3)];
C2=T(:,2);
C3=[T(2:n,1);0];
Ttrp=[C1,C2,C3];

function C=DxTD(D,T)
% C=DxTD(D,T) Function to left multiply a TriDiagonal matrix
% by a diagonal matrix 
% D=main diagonal of a diagonal matrix
% T=Tri-diagonal matrix in TriDiagonal form.

C=[D.*T(:,1) D.*T(:,2) D.*T(:,3)];

function C=TDxTD(A,B)
% C=TDxTD(A,B)  Function to multiply two tri-diagonal matrices
% stored in TriDiagonal Form. 
% C is the penta-diagonal product stored in PentaDiagonal form

[n,m]=size(A);
if m~=3 | [n,m]~=size(B), error('Both matrices should be in TriDiagonal form'),end,
C=zeros(n,5);
C(:,1)=[0;0;A(3:n,1).*B(2:n-1,1)];
C(:,2)=[0;A(2:n,1).*B(1:n-1,2)+A(2:n,2).*B(2:n,1)];
C(:,3)=[A(1,2)*B(1,2)+A(1,3)*B(2,1);...
A(2:n-1,1).*B(1:n-2,3)+A(2:n-1,2).*B(2:n-1,2)+A(2:n-1,3).*B(3:n,1);...
A(n,1)*B(n-1,3)+A(n,2)*B(n,2)];
C(:,4)=[A(1:n-1,2).*B(1:n-1,3)+A(1:n-1,3).*B(2:n,2);0];
C(:,5)=[A(1:n-2,3).*B(2:n-1,3);0;0];

function y=TDxV(A,x)
% y=TDxV(A,x)  Function calculating product of tri-diag * vector
% A=tri-diag. matrix in TriDiagonal form.
% x=vector
% y=product A*x
[n,m]=size(A);

if m~=3 | length(x)~=n,error('A should be in tri-diagonal form and x a vector'),end
y=zeros(size(x));
y(1)=A(1,2:3)*x(1:2);

for j=2:n-1,
  y(j)=A(j,:)*x(j-1:j+1);
end %for-loop
y(n)=A(n,1:2)*x(n-1:n);

function x=PDSolv(A,b)
% x=PDSolv(A,b)  Function to solve linear system with penta-diag.
%   matrix A in PentaDiagonal form.
% b=vector right hand side of the equation.

b=b(:);
[n,m]=size(A);
if m~=5 | [n,1]~=size(b),error('A should be nx5, b should be nx1'),end
Q=[A,b];
j=1;
while j<=n-1
  Q(j,:)=Q(j,:)/Q(j,3);
  Q(j+1,:)=Q(j+1,:)-Q(j+1,2)*[0,Q(j,3:5),0,Q(j,6)];
  if j<=n-2,
    Q(j+2,:)=Q(j+2,:)-Q(j+2,1)*[Q(j,3:5),0,0,Q(j,6)];
  end %if-block
  j=j+1;
end %while-loop

% Begin back substitution

x=zeros(n,1);
x(n)=Q(n,6)/Q(n,3);
x(n-1)=(Q(n-1,6)-Q(n-1,4)*x(n));
for j=2:n-1,
  x(n-j)=(Q(n-j,6)-Q(n-j,5)*x(n-j+2)-Q(n-j,4)*x(n-j+1));
end  %for-loop

function [ypred]=regress1(y,t)
y=y(:);t=t(:);
[t,isrt]=sort(t);
y=y(isrt);
r=length(t);
if length(y)~=r, error('Input vectors must have same length.'),end
onet=ones(r,1);
x=[onet,t];
x1=inv([r sum(t);sum(t) sum(t.^2)]);
beta=x1*x'*y;
ypred=x*beta;



