% -----------------------------
% InversionCode.m
% Linearized inverse technique for sin/cos
% This method can be used regardless of what the forward model is, or what
% the data set is. 
%
% Citation:
% Orsi, A. J., B. D. Cornuelle, and J. P. Severinghaus (2012), Little Ice
% Age cold interval in West Antarctica: Evidence from borehole temperature
% at the West Antarctic Ice Sheet (WAIS) Divide, Geophys. Res. Lett., 39, 
% L09710, doi:10.1029/2012GL051260. 
% Questions go to Anais Orsi, aorsi@ucsd.edu
% -------------------------------------------------------------------------
clear all
% folder which contains the forward model output
fldr='Output01/';

% model parameters for previous iterations:
% sum of xj from 1 to n-1 in Equation (5) of the text.
sm=zeros(402,1);
% sm=m1;%+m2;%+m3;

% dbase : the base function I'm adding perturbation on
% corresponds to y0 in the text.
dbase0 = load(strcat(fldr,'firn_sin_p_10000.33'));
dbase = [dbase0(2:303,1) dbase0(2:303,2)];

% ---------- function I want to guess --------------------------------
% --2: WAIS divide data ----
toguess = load('WAIStemp2009c.txt');
% We define tgprim = toguess - dbase as the perturbation around our stationary profile. except that, of course, they don't have the same dimension
% start at 15 m because of the influence of the weather on surface
% measurements.
x1 = 6; %6
tgprim = zeros(length(toguess)-x1,1);
do = zeros(length(toguess)-x1,1);
Nt = length(toguess)-x1;
xvect = zeros(Nt,1); % vector of the coord of toguess in dbase, if it makes sense
for jd = x1+1:length(toguess)
  x = find(dbase(:,1)>=toguess(jd,1),1);% find the right depth
  xvect(jd-x1) = x;
  tgprim(jd-x1) = toguess(jd,3)-dbase(x,2); % define the values to find
  do(jd-x1) = dbase(x,2);
end

nharm= 201; % number of harmonics we decompose on,
C = 2*nharm; % total number of basis functions we have
%dp = zeros(Nt,C); %(sinmat and cosmat)


%run Fortran model once and get d(tref+Ap.m(i))
%We are building the matrix H of the main text.
dp = zeros(302,C);
for ii = 1:nharm
  str = num2str(ii-1, '%03.0f');
  %for the sin_p_
   filen = strcat(fldr,'firn_sin_p_10',str, '.33');
   dtemp = load(filen);
   dp(:,ii) = dtemp(2:303,2);
%       %for the cos_p_
    filen = strcat(fldr,'firn_cos_p_10',str, '.33');
   dtemp = load(filen);
   dp(:,ii+nharm) = dtemp(2:303,2);
end

dp = dp(xvect,:); % get only the good depths

% get the perturbation dpprime
G = (dp-do*ones(1,C)); % this is the matrix H of the text.


%% run the least square inverse: want to get m
% vector of frequencies (cycles/year)
freq = ([1:nharm 1:nharm] - 1);

% expected rms signal
signalrms =0.5; % the possbile pdf of the signal: how far do we expect the signal to be from our initial guess of constant temperature?
snr=500; % choose a signal to noise ratio, from 100 to 1000.
% if you make the snr too large, you will have spurious oscillations, if
% it's too low, you won't fit all the data.
% you can change snr for later iterations: start at 500 and increase to
% 1000, but you should not change signalrms.
sigma=signalrms/snr; % sigma should match the error in the data.

% --- prior spectrum of model parameters : matrix P ---
% choose an exponent
% do a freq ^ -2 power spectrum
 exponent = 2;
% %exponent = 0;
% %exponent = 1;
% v = (freq).^-exponent;
% %v = (freq+nv).^-exponent;
% % give power to the zero frequency parts

% try gaussian:
%    nv=60;
% %   c=[0.6 0.2 0.1];
%    v = exp(-1/2*(freq/freq(nv)).^2);

% try cosine:
%v = 1/2*(1+cos(3.1415927*freq/nv/2000));

% % try (k_ko)-2:
f0=0;
 v = (freq+f0).^(-exponent);
  nv=2;
v(1:nv) =v(nv);
v(nharm+1:nharm+nv) =v(nv);

%  minv=1.2e-4;
%  v(v<minv)=minv;

 v = v/sum(v);

   c=[0.1 0.8 0.2];

% renormalize to have variance = signalrms^2
%v = 2*v*signalrms^2/sum(v);
P = 2*signalrms^2*diag(v); %matrix of uncertainty on the model parameters
% multiply P by 2 because there are too many parameters, and we want the
% variance to be right. check it with diag(Tmat*P*Tmat') should be srms.
% choose an RMS signal-to-noise ratio 10 to 1000.
figure(10), hold on, plot(v(1:201), 'color', c)
title('diagonal of the prior')
xlabel('k, in per 4000 years')
ylabel('v')

% -- matrix of uncertainty in the data R=diag(sigma^2) 
R = diag((sigma^2)*ones(Nt,1));
% -- least square solution (Equation (5) of the paper):
m = P*G'*inv(G*P*G'+R)*(tgprim+G*sm)-sm;
% sm is the sum of previous model estimates. We need to add this term so
% that we keep track of where we started from.
B = P*G'*inv(G*P*G'+R)*G; % the resolution matrix

% save m for future iterations:
% fid=fopen('reconstruction01_m1.txt', 'wt');
% fprintf(fid, '%6.4f\n', m');
% fclose(fid);
% 
% find the estimate dhat = G*mhat
dhat = G*m;

nm=m./sqrt(diag(P));
figure(5),hold on, plot(nm, '+-','color', c)
title('model parameters, normalised by the prior: m./sqrt(diag(P))');
xlabel('sine then cos')

% see how non linear it is:
diff = tgprim - dhat;
% it should be of order of signal to noise ratio snr

%$$$$$ --------------------- the end ------------- $$$$$%


% --- get the surface temperature profile ---  
tref = load(strcat(fldr,'firn_sin_p_10000.35'));
time = tref(:,1);
% time=[0:10000];
% tref=-29.9266*ones(length(time),2);
% tref(:,1)=time;

T = time(end);
pert = zeros(size(time));
final = zeros(size(time));

% create the matrix B of the text
Tmat = zeros(length(time),2*nharm);
for ii = 1:nharm
    arg = 2*pi*time*(ii-1)/(4000);
    Tmat(:,ii) =(1-2*mod(ii-1,2))*sin(arg);
    Tmat(:,ii+nharm) = (1-2*mod(ii-1,2))*cos(arg);
end
final=tref(:,2)+Tmat*m; % this is our reconstruction

% --- error bar calculation ---
% we have to calculate the uncertainty of the model parameters
Phat = P - P*G'*inv(G*P*G'+R)*G*P; %has to be positive, otherwise big problem

err2 = diag(Tmat*Phat*Tmat');
err = sqrt(err2); %add or substract error to the Tsurf profile
%figure(3), plot(t, err)
%title('error in degrees')

upbound = final+err;
lowbound = final-err;
 tosave=[time final upbound lowbound];
% 
%% -------- plots -------- 
figure(19),clf,set(gca, 'FontSize', 16,'FontWeight', 'bold')
figure(19),hold on, plot(time+7, final, 'color',c, 'LineWidth',1)
 hold on, plot(time+7,tref(:,2),'r')
   hold on, plot(time+7, upbound,'--b')
   hold on, plot(time+7, lowbound,'--b')
 title('surface temperature reconstruction')
 xlabel('time, years AD')
 ylabel('surface Temperature, Celcius')
% axis([1007 2007 -32.1 -29.6])
 figure(19),legend('new','old',2)

% plot model and data:
 figure(1),clf, plot(toguess(7:end,3), -toguess(7:end,1), '-+')
 hold on, plot(dbase(15:300,2), -dbase(15:300,1), 'r')
 hold on, plot(dhat+do, -toguess(7:end,1), 'color',c)
 legend('toguess','previous estimate','new estimate')
 
%  figure(3),hold on, plot(dbase(xvect,1), diff, 'color',c)
%  title(['difference between the data and dhat. snr=', num2str( snr), ...
%         ' nharm=',num2str(nharm) ])
%  ylabel('temperature in C')
 
 %% save files:
% %save m for future iterations:
% fid=fopen('reconstruction01_m1.txt', 'wt');
% fprintf(fid, '%6.4f\n', m');
% fclose(fid);

% %save the reconstruction with error bars:
% fid=fopen('reconstruction01_T.txt', 'wt');
% fprintf(fid, '%6.3f\t %6.4f\t %6.4f\t%6.4f\n', tosave');
% fclose(fid);
% 

%% ---- montecarlo ----
[U,D]=eig(Phat);
d=sqrt(diag(D));
iid=find(d<=0.004,1);
d(iid:end)=0;
D = diag(d);
a = randn(1000,C);
as=sum(a');
ss=length(D);
%x = a*sqrt(D)*U'./(as'*ones(1,ss));
x=m*ones(1,1000)+U*sqrt(D)*a';
%x = a*sqrt(D)*U';
y=(G*x);
kk=800;
% figure(11),clf, plot(tref(:,1)+8, tref(:,2)*ones(1,kk)+Tmat*x(1:kk,:)')
 z = tref(:,2)*ones(1,1000)+Tmat*x(:,1:1000);
%clean:
as=sum(a');
iias = find(abs(as)<=0.5);
z4=z; z4(:,iias)=[];
%save reconstruction01_z.txt z -ascii

%% plots monte carlo
figure(11),clf, plot(tref(:,1)+8,z)
title('monte carlo sampling of solutions - oct 26, 2009')
xlabel('year AD'), ylabel('surface temperature ?C')


figure(12),clf, plot(Tmat*U(:,1:5)), legend('1','2','3','4','5')
figure(13), clf, plot(toguess(7:end,1),G*U(:,1:5))

