%% demo_fast
% Demonstrating fast 3D physics-driven localization (using sparse synthesis 
% regularization, but avoiding explicit computation of the dictionary)
%
%
%     Copyright (C) Srdan Kitic, Inria Rennes
% 
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU Affero General Public License as
%     published by the Free Software Foundation, either version 3 of the
%     License, or (at your option) any later version.
% 
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU Affero General Public License for more details.
% 
%     You should have received a copy of the GNU Affero General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
% 
% Contact: nancy.bertin@irisa.fr or remi.gribonval@inria.fr
% PANAMA Research Group,  IRISA  -  (Inria & CNRS)
% Campus de Beaulieu, 35042 Rennes cedex, France
%
%% 

clc
clear all
close all

% Setup path
setupwave

%% Setup problem

k = 2;        % Sources
m = 20;       % Microphones

sizeX = 10;   % Room dimensions
sizeY = 10;
sizeZ = 10;
T = 5;        % Acquisition time
ySNR = 30;    % Measurement SNR
min_dist = 2; % Minimal voxel distance between sources

spatialGrid = [sizeX sizeY sizeZ];

shape = zeros(spatialGrid);
shape([1,end],:,:) = 4; % Robin boundary conditions
shape(:,[1,end],:) = 4; 
shape(:,:,[1,end]) = 4;

shapeparam = zeros(spatialGrid);
shapeparam([1,end],:,:) = 10; % Reflective boundary (~Neumann)
shapeparam(:,[1,end],:) = 10; 
shapeparam(:,:,[1,end]) = 10; 

shapeinfo.type = shape;
shapeinfo.param = shapeparam; 

wf = WaveField([sizeX sizeY sizeZ],T,[],[],shapeinfo,false,[],false);
src_type = wf.white;    % white noise sources
src_cutoff = [];        % no lowpass filtering
start_t = 0.01;         % ignition time
active_t = T;           % emission active
magnitude = 1;          % source magnitude

n = prod(wf.gridSize);
s = prod(wf.spatialGridSize);
t = wf.timeGridSize;

disp(['Total number of voxels: ' num2str(n)])
disp(['Sources: ' num2str(k)])
disp(['Microphones: ' num2str(m) ' (SNR=' num2str(ySNR) 'dB)'])

% Generate sources and the wavefield
fprintf('Generating data...')
exclude = [];           % No forbidden voxels (except microphone positions)
wf.randomizeSources(k,src_type,start_t,active_t,magnitude,src_cutoff,exclude,min_dist);
fprintf('done!\n')

supp = find(wf.support); % Ground truth support

[y,M] = wf.quicksensing(m,ySNR);    % Random sampling
Mt = M';

%% Generating operators

fprintf('Building operators...')
A = wf.operators{1};
At = A';
AtA = At*A;
I = speye(size(A));

D = @(inp,t_str)D(inp,A,At,t_str);
%normA = norm2est(A,1e-3,n);
%normD = norm2est(D,1e-3,n);

MD = @(inp,t_str)MD(inp,A,At,M,Mt,t_str);     
fprintf('done!\n')

%disp(['Condition number: ' num2str(normA*normD)])

%% Experiment
% Synthesis Group-LASSO: min_z lambda||z||_{2,1} + lambda||MDz-y||^2

% Mask matrix for MMV groups
G = true(wf.timeGridSize, 1);
G = logical(kron(G, speye(prod(wf.spatialGridSize))));

lambda = 10; % Regularization parameter

xTruth = wf.data(:);
gTruthVal = lambda*funEval(A*xTruth,'GroupL1',G) + funEval(M*xTruth - y, 'L2SQ',[]);
disp(['Ground truth objective: ' num2str(gTruthVal)])

max_iter = 100;

fprintf('Running...')
[z, history] = SynthesisCP(MD,y,lambda,G,A,xTruth, 1e-1,k,supp,wf.gridSize,max_iter);
%[z, history] = FISTA(MD,y,lambda,G,A,xTruth, 1e-1,k,supp,wf.gridSize,[],max_iter);
fprintf('done!\n')
x = A\z;
disp(['After ' num2str(max_iter) ' iterations: ' ...
        num2str(lambda*funEval(A*x,'GroupL1',G) + funEval(M*x - y, 'L2SQ',[]))])


%% Restoring old path
setupwave(0)