% invert_backslip.m
% This program does the bounded least squares inversion for the
% distribution of back-slip.

Lap = Laplacian(Ni,N); %discrete Laplacian operator for smoothing
sig=[sigxGPS;sigzGPS;sigzLEV];
C=repmat(sig,1,Ni+sum(N)+length(N)); %weighting matrix

%matrix of velocities
G=[-Gix;-Giz;-GizLEV];

backslipix=zeros(size(Gix,1),length(N));
backslipiz=zeros(size(Gix,1),length(N));
backslipizLEV=zeros(size(GizLEV,1),length(N));
for k=1:length(N)
   G=[G [-Gx{k};-Gz{k};-GzLEV{k}]];
     for j=1:k
        backslipix(:,k)=backslipix(:,k)-sum(Gix(:,index(j,:)'),2);
        backslipiz(:,k)=backslipiz(:,k)-sum(Giz(:,index(j,:)'),2);
        backslipizLEV(:,k)=backslipizLEV(:,k)-sum(GizLEV(:,index(j,:)'),2);
    end
    forwardslipx(:,k)=sum(Gx{k},2);
    forwardslipz(:,k)=sum(Gz{k},2);
    forwardslipzLEV(:,k)=sum(GzLEV{k},2);
end

ss_vx=backslipix+forwardslipx; %steady state velocity in x direction
ss_vz=backslipiz+forwardslipz; %steady state velocity in z direction
ss_vzLEV=backslipizLEV+forwardslipzLEV; 

%solve Am=d 
G=[G [ss_vx;ss_vz;ss_vzLEV]]./C;
Lap=[Lap zeros(size(Lap,1),length(N))];
A=[G;Lap*gamma];
d=[[dxGPS+convergence;dzGPS;dzLEV]./sig;zeros(size(Lap,1),1)];

%specify bounds for least squares (backslip rate cannot exceed steady-state
%rate)
B=zeros(size(A,2));
bounds=zeros(size(A,2),1);
BNi=zeros(Ni,Ni+sum(N)+length(N));
BNi(1:Ni,1:Ni)=eye(Ni);

for k=1:length(N)
    for j=k:length(N)
    BNi(index(k,:),end-(length(N)-j))=1;
    end
end
bounds(1:Ni)=convergence;
B(1:Ni,:)=BNi;
B(Ni+1:Ni+N(1),end-2)=-1;
B(Ni+1:Ni+N(1),Ni+1:Ni+N(1))=eye(N(1));
for k=2:length(N)
    B(Ni+sum(N(1:k-1))+1:Ni+sum(N(1:k)),end-(length(N)-k))=-1;
    B(Ni+sum(N(1:k-1))+1:Ni+sum(N(1:k)),Ni+sum(N(1:k-1))+1:Ni+sum(N(1:k)))=eye(N(k));
end

B=[B;-eye(size(B))];
bounds=[bounds;0*bounds];

%do least squares inversion with bounds
warning off %turn off warning about large-scale method
backslip=lsqlin(A,d,B,bounds);
warning on

%caclulate model velocities
dhat=G*backslip;
dhatxGPS=dhat(1:length(xGPS))-convergence;
dhatzGPS=dhat(length(xGPS)+1:2*length(xGPS));
dhatzLEV=dhat(2*length(xGPS)+1:end);
