 function tbeta = res2beta(G, C, psf_fhandle, target_fwhm, psf_offset, mask)
%function tbeta = res2beta(G, C, psf_fhandle, target_fwhm, psf_offset, mask)
% Calculates a beta value corresponding to the
% desired resolution as given by target_fwhm.
% in:
%       G                       [Nt,N]                  system matrix for psf eval
%       C                                               penalty matrix with beta=1(!!!!) for psf eval
%       psf_fhandle:                                    function handle to the psf eval routine
%         @qpwls_psf                                    psf eval using fft when using 'qpwls_pcg'
%         @qpwls_psf_gram                               psf eval using fft when using 'qpwls_pcg'
%         @qpwls_psf_st                                 psf eval using fft when using 'qpwls_pcg_st'
%       target_fwhm             [Nbd,1]                 target fwhm [voxels]
%       psf_offset              [Noffsets,Nbd]          impulse offsets
%       mask                    [Nx,Ny]                 logical array
% out:
%       tbeta                   [Nbd,1]                 beta value for the target_fwhm
%
%  Copyright 2007-5-06, Valur Olafsson, The University of Michigan

if ~(nargin == 6)
    help mfilename
    error 'Wrong number of input variables'
end
if strcmp(func2str(psf_fhandle),'qpwls_psf_st')
    if ~(length(target_fwhm) == 2)
        error 'target_fwhm has to be of length 2 when using qpwls_psf_st'
        help mfilename
    end
elseif (length(target_fwhm) > 1)
    error 'target_fwhm has to be of length 1'
    help mfilename
end

dims = size(mask);


% Have to take care of the special case when calling qpwls_psf_st
switch func2str(psf_fhandle)
case 'qpwls_psf'
    % 1. Find a set [2^log2beta_a, 2^log2beta_b] s.t. fwhm(2^log2beta_a*R(x)) < target_fwhm
    %    and fwhm(2^log2beta_b*R(x)) > target_fwhm
    [log2beta_a, log2beta_b] = find_lb_ub(target_fwhm, G, C, psf_fhandle, psf_offset, mask);
    
%     % Initialize
%     log2beta_2 = 0;
%     fwhm_loop = beta2res(G, C, psf_fhandle, 2^log2beta_2, psf_offset, mask);
% 
%     sign_init = sign(fwhm_loop-target_fwhm);
%     while (sign_init == sign(fwhm_loop-target_fwhm))
%         % redefine the upper/lower bound
%         log2beta_1 = log2beta_2;
%         
%         % Find a lower/upper bound on the set
%         log2beta_2 = log2beta_2 + -sign(fwhm_loop-target_fwhm)*3;
%         fwhm_loop = beta2res(G, C, psf_fhandle, 2^log2beta_2, psf_offset, mask);
%     end
%     
%     if sign_init == 1  % before loop: fwhm_loop > target_fwhm
%         log2beta_a = log2beta_2;
%         log2beta_b = log2beta_1;
%     else               % before loop: fwhm_loop < target_fwhm
%         log2beta_a = log2beta_1;
%         log2beta_b = log2beta_2;
%     end

    % 2. Interpolate tbeta onto a grid of points within
    %    the set from 1.
    reg_beta = 2.^linspace(log2beta_a,log2beta_b,10).';
    fwhm_all = beta2res(G, C, psf_fhandle, reg_beta, psf_offset, mask);
    % x,y coordinate is fwhm averaged over the offsets
    tbeta = interp1(mean(fwhm_all,3),reg_beta,target_fwhm,'cubic');
case 'qpwls_psf_st'
    % 1. Find a set [2^log2beta_a, 2^log2beta_b] s.t. fwhm(2^log2beta_a*R(x)) < target_fwhm
    %    and fwhm(2^log2beta_b*R(x)) > target_fwhm
    [log2beta_a, log2beta_b] = find_lb_ub(target_fwhm, G, C, psf_fhandle, psf_offset, mask);
    

    % 2. Interpolate tbeta onto a grid of points within
    %    the set from 1.
    [beta1, beta2] = meshgrid(linspace(log2beta_a(1),log2beta_b(1),10),linspace(log2beta_a(2),log2beta_b(2),10));
    reg_beta = 2.^[beta1(:) beta2(:)];
    fwhm_all = beta2res(G, C, psf_fhandle, reg_beta, psf_offset, mask);

    % x,y coordinate is fwhm averaged over the offsets
    betaR = griddata(mean(fwhm_all(1,:,:),3).',mean(fwhm_all(2,:,:),3).',reg_beta(:,1),target_fwhm(1),target_fwhm(2),'cubic');
    betaI = griddata(mean(fwhm_all(1,:,:),3).',mean(fwhm_all(2,:,:),3).',reg_beta(:,2),target_fwhm(1),target_fwhm(2),'cubic');
    % set output variable
    tbeta = [betaR;betaI];
otherwise
    error 'Unknown psf function handle'
end



function [log2beta_a, log2beta_b] = find_lb_ub(target_fwhm, G, C, psf_fhandle, psf_offset, mask)
    % Initialize
    log2beta_1 = ones(1,length(target_fwhm))*4;
    log2beta_2 = log2beta_1;
    fwhm_loop = beta2res(G, C, psf_fhandle, 2.^log2beta_2, psf_offset, mask);

    sign_init = sign(target_fwhm-fwhm_loop);
    sign_curr = sign_init;
    while any(sign_init == sign_curr)
        % redefine the upper/lower bound
        mm = logical(sign_init == sign_curr);
        log2beta_1(mm) = log2beta_2(mm) - sign_curr(mm).'*.5; % Make certain I'm well above/below limits
        
        % Find a lower/upper bound on the set
        log2beta_2(mm) = log2beta_2(mm) + sign_curr(mm).'*2;
        fwhm_loop = beta2res(G, C, psf_fhandle, 2.^log2beta_2, psf_offset, mask);
        sign_curr = sign(target_fwhm-fwhm_loop);
    end
    log2beta_2 = log2beta_2 - sign_curr.'*.5;  % Make certain I'm well above/below limits

    mmp = logical(sign_init == -1);
    mmn = logical(sign_init == 1);

    % set bounds for positive initial sign
    log2beta_a(mmp) = log2beta_2(mmp);
    log2beta_b(mmp) = log2beta_1(mmp);
    % set bounds for negative initial sign
    log2beta_a(mmn) = log2beta_1(mmn);
    log2beta_b(mmn) = log2beta_2(mmn);


%    % Initialize
%    log2beta_2 = zeros(1,length(target_fwhm));
%    log2beta_1 = log2beta_2;
%    fwhm_loop = beta2res(G, C, psf_fhandle, 2.^log2beta_2, psf_offset, mask)
%
%    sign_init = sign(fwhm_loop-target_fwhm);
%    while any(sign_init == sign(fwhm_loop-target_fwhm))
%        % redefine the upper/lower bound
%        sign_curr = sign(fwhm_loop-target_fwhm);
%        mm = logical(sign_init == sign_curr);
%        log2beta_1(mm) = log2beta_2(mm);
%        
%        % Find a lower/upper bound on the set
%        log2beta_2(mm) = log2beta_2(mm) + -sign(fwhm_loop(mm)-target_fwhm(mm)).'*3;
%        fwhm_loop = beta2res(G, C, psf_fhandle, 2.^log2beta_2, psf_offset, mask)
%    end
%    keyboard
%
%    mmp = logical(sign_init == 1);
%    mmn = logical(sign_init == -1);
%
%    % set bounds for positive initial sign
%    log2beta_a(mmp) = log2beta_2(mmp);
%    log2beta_b(mmp) = log2beta_1(mmp);
%    % set bounds for negative initial sign
%    log2beta_a(mmn) = log2beta_1(mmn);
%    log2beta_b(mmn) = log2beta_2(mmn);


    
% %%%%%
% % Loop through all betas and local impulses
% for kk = 1:size(psf_offset,1)
%     offset = psf_offset(kk,:);
%     for ii = 1:size(reg_beta,1)
%         Cbeta = reg_beta(ii,:);
%         
%         % Finding the impulse response
%         [psf_tmp, var_tmp, fwhm_tmp] = psf_fhandle(G, C, Cbeta, mask, 1, offset);
%         
%         switch func2str(psf_fhandle)
%         case 'qpwls_psf'
%             % Relocating the impulse response to correct coordinate
%             % NO NEED FOR THIS FOR THE CURRENT IMPLIMENTATION OF qpwls_psf
%             %psf_tmp = circshift(psf_tmp,offset);
% 
%             % Only show the results from voxels within mask
%             psf_tmp = embed(psf_tmp(mask),mask);
%             
%             % Setting the output variables
%             psf(:,ii,kk) = psf_tmp(:);
%             fwhm(1,ii,kk) = fwhm_tmp;
% 
%             % Clear variables
%             clear psf_tmp var_tmp fwhm_tmp
%         case 'qpwls_psf_gram'
%             warning 'This has not been tested'
% 
%             % Relocating the impulse response to correct coordinate
%             % NO NEED FOR THIS FOR THE CURRENT IMPLIMENTATION OF qpwls_psf
%             %psf_tmp = circshift(psf_tmp,offset);
% 
%             % Only show the results from voxels within mask
%             psf_tmp = embed(psf_tmp(mask),mask);
%             
%             % Setting the output variables
%             psf(:,ii,kk) = psf_tmp(:);
%             fwhm(1,ii,kk) = fwhm_tmp;
% 
%             % Clear variables
%             clear psf_tmp var_tmp fwhm_tmp
%         case 'qpwls_psf_st'
%             % read psf into a temporary variable
%             psfR_tmp = psf_tmp(1:end/2,:);
%             psfI_tmp = psf_tmp(end/2+1:end,:);
%             
%             % Relocating the impulse response to correct coordinate
%             psfR_tmp = circshift(psfR_tmp,offset);
%             psfI_tmp = circshift(psfI_tmp,offset);
% 
%             % Only show the results from voxels within mask
%             psfR_tmp = embed(psfR_tmp(mask),mask);
%             psfI_tmp = embed(psfI_tmp(mask),mask);
%             
%             % Setting the output variables
%             psf(:,ii,kk) = [psfR_tmp(:);psfI_tmp(:)];
%             fwhm_tmp, fwhm(:,ii,kk) = fwhm_tmp(:);
% 
%             % clearing variables
%             clear psf_tmp psfR_tmp psfI_tmp var_tmp fwhm_tmp
%         otherwise
%             error 'unknown psf function handle'
%         end
%     end
% end
% 
% if exist('target_fwhm','var')
%     switch func2str(psf_fhandle)
%     case 'qpwls_psf_st'
%         if all((max(mean(fwhm,3),[],2) > target_fwhm) & (min(mean(fwhm,3),[],2) < target_fwhm))
%             % x,y coordinate is fwhm averaged over the offsets
%             betaR = griddata(mean(fwhm(1,:,:),3).',mean(fwhm(2,:,:),3).',reg_beta(:,1),target_fwhm(1),target_fwhm(2),'cubic');
%             betaI = griddata(mean(fwhm(1,:,:),3).',mean(fwhm(2,:,:),3).',reg_beta(:,2),target_fwhm(1),target_fwhm(2),'cubic');
%             
%             % set output variable
%             tbeta = [betaR;betaI];
%         else
%             warning 'target fwhm is not within the range of fwhm values calculated reg_beta, returning tbeta = [NaN;NaN]'
%             tbeta = [nan;nan];
%         end
%     case 'qpwls_psf'
%         if (max(mean(fwhm,3),[],2) > target_fwhm) & (min(mean(fwhm,3),[],2) < target_fwhm)
%             % x,y coordinate is fwhm averaged over the offsets
%             tbeta = interp1(mean(fwhm,3),reg_beta,target_fwhm(1),'cubic');
%         else
%             warning 'target fwhm is not within the range of fwhm values calculated reg_beta, returning tbeta = NaN'
%             tbeta = nan;
%         end
%     case 'qpwls_psf_gram'
%         if (max(mean(fwhm,3),[],2) > target_fwhm) & (min(mean(fwhm,3),[],2) < target_fwhm)
%             % x,y coordinate is fwhm averaged over the offsets
%             tbeta = interp1(mean(fwhm,3),reg_beta,target_fwhm(1),'cubic');
%         else
%             warning 'target fwhm is not within the range of fwhm values calculated reg_beta, returning tbeta = NaN'
%             tbeta = nan;
%         end
%     end
% end
