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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Domain Initialization Part 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all
close all

s0 = [25 20 30];        % Center of inclusion

[nobbx,fcbbx] = meshabox([0 0 0],[80 70 60],15);    % Create sphere within a box domain
[nosp,fcsp] = meshasphere(s0,10,1);

[no,fc] = mergemesh(nobbx,fcbbx,nosp,fcsp);     % Mesh Merge (Iso2Mesh)
figure,plotmesh(no,fc,'y<40');
view([0.1 1 0.1])
title('Sphere within a box')

[cfg0.node,cfg0.elem] = s2m(no,fc(:,1:3),1,40,'tetgen',[41 1 1;s0]);    %Iso2Mesh conversion of surface to mesh

%=====Try changing the location, size, and properties of the sphere=====%


%% Domain Initialization Part 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%xi = [20:10:60];
%cfg0.srcpos = [xi(:),ones(length(xi(:)),1).*20,zeros(length(xi(:)),1)];   % Declaring source location and direction (line 17)
[xi,yi] = meshgrid(20:10:60,20:10:50);
cfg0.srcpos = [xi(:),yi(:),zeros(length(xi(:)),1)];     % Source locations
cfg0.detpos = [xi(:),yi(:),ones(length(xi(:)),1).*60];   % Detector location
cfg0.srcdir = [0 0 1];

cfg0.prop = [
    0 0 1 1
    0.005 1 0 1.37
    0.2 1 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 = 0;

cfg = cfg0;
cfg0 = rbmeshprep(cfg0);

%% Run Forward + Build Jacobian
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[detphi0,phi0] = rbrunforward(cfg0);

[sd0,dist0] = rbsdmap(cfg0);
%Jmua0 = rbfemmatrix(cfg0,sd0,phi0);
Jmua0 = rbjac(sd0,phi0,cfg0.deldotdel,cfg0.elem,cfg0.evol);

%% Figure 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


figure,plotmesh([cfg0.node log10(abs(Jmua0(17,:)'))],cfg0.elem,'y=20','facecolor','interp','linestyle','none');
view([0 1 0])
title('log10(abs(Jmua)) Source 1 Detector 17');


%% Setup Recon Meshes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[node,face,elem] = meshabox([0 0 0],[80 70 60],15);
cfg = rbsetmesh(cfg,node,elem,cfg.prop,ones(size(node,1),1));

[sd,dist] = rbsdmap(cfg);

[recon.node,recon.face,recon.elem] = meshabox([0 0 0],[80 70 60],40);      % Perform recon on coarser mesh
[f2rid, f2rweight] = tsearchn(recon.node,recon.elem,cfg.node);      % Calculate conversion from forward to recon mesh


%% Iterative Reconstruction
maxiter = 10;       % Set # Iterations
resid = zeros(1,maxiter);

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

for i = 1:maxiter
    tic
    [detphi,phi] = rbrunforward(cfg);       % Run forward for recon mesh
    %Jmua = rbfemmatrix(cfg,sd,phi);     % Build Jacobian
    Jmua = rbjac(sd,phi,cfg.deldotdel,cfg.elem,cfg.evol);
    Jmua_recon = meshremap(Jmua.',f2rid, f2rweight,recon.elem,size(recon.node,1)).';    %Map to reconstruction mesh for inversion
    
    misfit = detphi0(:) - detphi(:);    % Misfit from true measurements
    resid(i)=sum(abs(misfit));      % Calculate and store residual
    
    [Jmua_recon,misfit] = rbmatreform(Jmua_recon, detphi0(:), detphi(:), 'real');       % Log-amp/phase unwrap
    dmu_recon = rbreginv(Jmua_recon, misfit, 0.05);     % Solve for the update on mua
    dmu = meshinterp(dmu_recon,f2rid, f2rweight,recon.elem);    % Extrapolate back to forward mesh
    
    cfg.mua=cfg.mua + dmu(:); % Update mua
    fprintf(1,'iter [%4d]: residual=%e, relres=%e (time=%f s)\n',i, resid(i), resid(i)/resid(1), toc);
end

%% Figure 3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure,plotmesh([cfg.node,cfg.mua],cfg.elem,'y=20','facecolor','interp','linestyle','none')
hold on;
plotmesh([cfg.node,cfg.mua],cfg.elem,'z=30','facecolor','interp','linestyle','none')
plotmesh([cfg.node,cfg.mua],cfg.elem,'x=25','facecolor','interp','linestyle','none')
view(3);
title('Recovered \mu_a (mm^-^1)');






