%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Redbird-m Demo - Reconstruction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath(fullfile(pwd, '../matlab'));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Bulk Fitting Example 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all
close all

minim = [0 0 0];maxim = [120 120 60];       % Create Initial Domain
[cfg0.node,face,cfg0.elem] = meshabox(minim,maxim,25);
cfg0.seg = ones(size(cfg0.elem,1),1);     % Single segmentation/"tissue" type

cfg0.srcpos = [30 30 0;30 90 0];        % Source and Detector locations + Source Direction
[xi,yi] = meshgrid(30:10:90,30:10:90);
cfg0.detpos = [diag(xi),diag(yi),ones(length(diag(xi)),1).*60];   
cfg0.srcdir = [0 0 1];

cfg0.prop = [          % Set Initial Properties for Forward Simulation
    0 0 1 1
    0.0015 1.2 0 1.37];

z0 = 1/(cfg0.prop(2,1)+cfg0.prop(2,2)*(1-cfg0.prop(2,3)));      % Sinking sources by 1/(mua+mus') of bounding box
cfg0.srcpos(:,3) = cfg0.srcpos(:,3) + cfg0.srcdir(:,3)*z0;
cfg0.detpos(:,3) = cfg0.detpos(:,3) - cfg0.srcdir(:,3)*z0;

cfg0.omega = 2*pi*67.5e6; % Modulation frequency for simultaneous recovery of mua and mus'

cfg = cfg0;
cfg0 = rbmeshprep(cfg0);


%% New Domain for Fitting - Used to recover previous optical properties
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[detphi0,phi0] = rbrunforward(cfg0);

[node,face,elem] = meshabox(minim,maxim,25);        % Create new homogenous domain with starting set of optical propertes
cfg.prop = [
    0 0 1 1
    0.005 1 0 1.37];
cfg = rbsetmesh(cfg,node,elem,cfg.prop,ones(size(node,1),1));   % Sets new properties to existing mesh

[sd,dist] = rbsdmap(cfg);
dist = dist';

[recon.node,recon.face,recon.elem] = meshabox(minim,maxim,100);      % Perform recon on coarser mesh
[f2rid, f2rweight] = tsearchn(recon.node,recon.elem,cfg.node);      % Calculate conversion from forward to recon mesh

maxiter = 15;       % Set # Iterations

%=====Play around with maxiter to check convergence=====%

resid = zeros(1,maxiter);

cfg.mua = ones(size(cfg.node,1),1)*cfg.prop(cfg.seg(1)+1,1);    % Declare mua var for updating
cfg.dcoeff = ones(size(cfg.node,1),1)*(1/(3*(cfg.prop(cfg.seg(1)+1,1) + cfg.prop(cfg.seg(1)+1,2))));


%% Iterative Fitting
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = 1:maxiter
    tic
    [detphi,phi] = rbrunforward(cfg);       % Run Initial Forward
    %Jmua = rbfemmatrix(cfg,sd,phi);
    [Jmua,Jmua_elem,JD,JD_elem] = rbjac(sd,phi,cfg.deldotdel,cfg.elem,cfg.evol);    % Build Jacobians for mua and D
    
    Jmua_recon = meshremap(Jmua.',f2rid, f2rweight,recon.elem,size(recon.node,1)).';        % Map to Recon Mesh for Inversion
    JD_recon = meshremap(JD.',f2rid, f2rweight,recon.elem,size(recon.node,1)).';
    Jmua_recon = sum(Jmua_recon,2);     % Sum across all channels (for Bulk Fitting Only)
    JD_recon = sum(JD_recon,2);
    
    J = [Jmua_recon JD_recon];      % Merge Jacobians
    
    misfit = detphi0(:) - detphi(:);        % Calculate Data Misfit
    resid(i)=sum(abs(misfit));      % Calculate Residual of Misfit
    
    [J,misfit] = rbmatreform(J,detphi0(:),detphi(:),'real');
    delta = rbreginv(J, misfit, 0.05);      % Solve for update on mua and D
    
    cfg.mua = cfg.mua + delta(1);       % Make Step
    cfg.dcoeff = cfg.dcoeff + delta(2);
    
    fprintf(1,'iter [%4d]: residual=%e, relres=%e (time=%f s)\n',i, resid(i), resid(i)/resid(1), toc);
    fprintf('mua = %f; mus = %f\n',cfg.mua(1),1/(3*cfg.dcoeff(1)) - cfg.mua(1));
end

fprintf('Bulk Fitting for mua : %f mm-1\n',cfg.mua(1));
fprintf('Bulk Fitting for mus : %f mm-1\n',1/(3*cfg.dcoeff(1)) - cfg.mua(1));


%% Figure 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure,plot(dist(:),angle(detphi(:)),'r+'); hold on
plot(dist(:),angle(detphi0(:)),'bx');

title('Phase Fitting');
legend('Final Iteration','Original Data')



figure,plot(dist(:),log10(abs(detphi(:))),'r+'); hold on
plot(dist(:),log10(abs(detphi0(:))),'bx');

title('Amplitude Fitting');
legend('Final Iteration','Original Data')
