% we implement thresholded FISTA for regulrizing Ax=b
% via minimization of J(x) = 2*tau*norm(xn,1) + norm(A*xn-b,2)^2;
% we have A,x,b; norm(A)<1
% 
% here's how we do thresholded FISTA:
% 
% let T(x) = S_tau( x + A'*b - A'*A*x )
% next,
% pick some x0 and iterate:
% x_{n+1}=T(x_n+\frac{t_{n}-1}{t_{n+1}} (x^{(n)}-x^{(n-1)}))
% where {t_n} is a sequence of numbers, generated by:
% t_{n+1}=\frac{1+\sqrt{1+4(t_{n})^2}}{2}$ and t_1=1
%
% sv 2009
function [x_sol,num_iters_taken,tElapsed,funct_vals,percent_errors] = thr_fista_solve(A,b,x,tau,x0,TOL,maxiters) 

tic;
m = size(A,1);
n = size(A,2);

funct_vals = zeros(maxiters,1);
percent_errors = zeros(maxiters,1);


% generate sequence of tn's needed for thresholded FISTA
tns = repmat(0,maxiters+10,1);
tns(1) = 1;
for i=2:length(tns)
	tns(i) = (1 + sqrt(1 + 4*tns(i-1)^2))/2;
end

% calculate z = A^t*b
z = A'*b;

num_iters_taken = 0;
xn = x0;
xn1 = x0;
quit_loop = 0;


fprintf('in thr_fista_solve, starting loop..\n');
fprintf('maxiters = %d, tau = %f\n', maxiters, tau);
for i=1:maxiters
    if quit_loop ~= 1
            fprintf('doing thresholded FISTA iter %d of %d with tau = %f\n', i, maxiters, tau);

            %x_{n+1}=T(x_n+\frac{t_{n}-1}{t_{n+1}} (x^{(n)}-x^{(n-1)}));
            num_iters_taken = i;

            if i==1
                pn = xn;
            else
                pn = xn + ((tns(i)-1)/tns(i+1))*(xn - xn1);
            end
            
            xn1 = xn; % record x_{n-1}
            un1 = 2*tau*norm(xn1,1) + norm(A*xn1-b,2)^2;

            vn = A*pn;
            wn = A'*vn;
            xn = softThreshold(pn + z - wn, tau);

            un = 2*tau*norm(xn,1) + norm(A*xn-b,2)^2;


            
        % remove small components
%            xnabssorted = sort(abs(xn),'descend');
%            xnabssorted = xnabssorted(find(xnabssorted>0));
%            threshold = xnabssorted(round(0.15*length(xnabssorted)));
%            fprintf('length(xnabssorted) = %d and threshold = %f\n', length(xnabssorted), threshold);
%            for j=1:n
%                if abs(xn(j)) < threshold
%                    %xn(j) = 0;
%                end
%            end
%


            percent_errors(i) = 100*norm(xn - x)/norm(x);
            funct_vals(i) = norm(A*xn - b,2)^2 + 2*tau*norm(xn,1);
            fprintf('fista percent_errors(%d) = %f\n', i, percent_errors(i));
            fprintf('fista funct_vals(%d) = %f\n', i, funct_vals(i));


            % compute diff between un and un1 and quit if small
            %if(i>3 && (100*(norm(xn - xn1)/norm(xn)))<TOL)
             %   fprintf('reached convergence..\n');
             %   quit_loop = 1;
            %end
    else
        break;
    end
end

% record solution
x_sol = xn;

% record time
tElapsed = toc();

