function result = blasso_poisson_no_lock2(y, A, eps_step,p)
%no_lock means this version can prevent from locking in the iteration.
% =========================================================================
% function result = blassol2(y, x, eps_step)
% =========================================================================
% PURPOSE: 
% This function calculates the regularization path for the LASSO using the 
% BLASSO algorithm (Zhao & Yu (2004)). 
% =========================================================================
% INPUTS:
%  y is the dependent variable
%  x is a matrix with explanatory variables in its columns
%  eps_step is the step size for the algorithm
% =========================================================================
% OUTPUTS:
% A structure result containing the fields:
% 
% nbetas        a ??? x n_variables matrix containing the coefficients for 
%               the normalized regression (both y and x are normalized)
% betas         a ??? x n_variables matrix containing the coefficients for 
%               the unnormalized regression
% intercepts    a ??? dimensional vector containing the intercepts for the 
%               unnormalized regression
% lambdas       a ??? dimensional vector containing the estimates 
%               regularization penalties
% yscale        the factor used in the normalization of y
% ymean:        the mean of the dependent variable
% xscale:       the factor used in the normalization of x       
% xmean:        the means of the columns of x
% normalized_y: a normalized version of the input argument y
% original_y:   the input argument y
% normalized_x: a normalized version of the input argument x
% original_x    the input argument x
% =========================================================================
% KNOWN ISSUES:
% - The routine seems to be stopping too far from the OLS solution: problem
%   seems to be numeric but more testing is needed (2005/08/24)
% - Still need to figure out a good "zero_eps"
% - There may be a better way of doing the normalization
% =========================================================================
% Send comments, suggestions, bug reports to Guilherme Rocha: 
% gvrocha [AT] gmail [dot] com
% =========================================================================
% See also LASSO

% 0. Setting parameters and starting up some variables
% =========================================================================
zero_eps     = 1e-6;                                                       % numbers smaller than this will be considered zero
global epsilon;
%epsilon = 1e-10;
n_obs        = size(A, 1);                                        % this is the number of observations

% 0.1 Normalizing variables: 
% =========================================================================

%A = normalize_matrix(A);

%xtx         = ndictionary'*ndictionary;                                    % xtx is the correlation matrix of the explanatory variables
%xtr         = ndictionary'*ny;                                             % xty is a vector with the correlations between y and the explanatory variables


% 1. Initializing path:
% =========================================================================
curr_lambda   = Inf;                                                         % Path starts with lambda = +infinity ...
curr_beta     = zeros(size(A,2),1);                                % and beta = 0
betas         = curr_beta;                                                   % betas is a matrix containing the selected betas as its columns
lambdas       = curr_lambda;                                                 % lambdas is a vector contatining the values of the multipliers for which the columns in betas are an approximation to the optimal value
n_lambdas     = 1;                                                           % n_lamdas is the length of the lambdas vector and the number of columns in betas
backward_flag = [];                                                          % keep track of backward steps;

% 2. Compute inital step
% =========================================================================
% Here a warning for those trying to understand the code must be made:
% The path matrix betas contains the counts of number of steps since the
% beginning of the algorithm: this may help with numerical issues
% =========================================================================
%p = 1/2;
%AX = (A)*(curr_beta);
%warning off;
%AX_log = log(AX+epsilon);
%warning on;
%AX_log(isinf(AX_log)) = epsilon;
%f_old = sum(AX) - y * AX_log;
[f_new,j,sj,f_old] = log_likelihood_blasso_componenet_select_first(A, y,curr_beta,eps_step);
  %gradinit = (sum(A,1)  - y./(A*curr_beta+1e-12)'*A);                                        
new_beta          = curr_beta;    new_beta(j) = curr_beta(j) + sj;         % this is the new set of coefficients (in step count units)
loss_improv       = (f_old - f_new);                         % computes improvement in loss function
pre_j = j; pre_sj = sj;
new_lambda        = loss_improv/(norm(new_beta,p)-norm(curr_beta,p));                                           % computes approximate value of the penalty
if(new_lambda>0)                                                           % If step is not too big:
  lambdas         = [lambdas new_lambda];                                  % - record new lambda
  betas           = [betas new_beta];                                      % - adds new set of coefficients to the path
  backward_flag   = [backward_flag;0];                                     % - mark this step as forward
  n_lambdas       = n_lambdas + 1;                                         % - updates the number of points in the path
                            % - update correlation with residuals
  A_set           = [j];                                                   % - updates active set (in case new variable was not already selected)
  %f_old = f_new;
end;
curr_beta         = new_beta;                                              % Updates current coefficients
curr_lambda       = new_lambda;                                            % Updates current penalty
%AX = A*curr_beta; 

% 3. While improvement is possible
% =========================================================================
%ite = 0;
while(curr_lambda > 0.01)  
   
  % 3a. Find best direction for the backward step:
  % =======================================================================
%  A_set        = find(abs(curr_beta)>=zero_eps);                           % Update current active set, zero_eps is a small number included for numerical reasons. May be worth taking a look at an alternative approach where we ``count'' number of steps
  %[cj, j]      = max(-sign(curr_beta(A_set)).*xtr(A_set));                 % Chooses best direction to go backwards, the one that results in less increase in the penalty
  %sj           = sign(curr_beta(A_set(j)));                                % Get the sign of the term that is being ``downsized''
  %AX_act = (A(:,A_set))*curr_beta(A_set);
%  [f_backward,j,sj] = log_likelihood_blasso_componenet_backward_2(AX_act, A(:,A_set), y, curr_beta(A_set),eps_step);
%AX = A*curr_beta;  
[f_steepest_lasso,j,sj,f_old_lasso]=log_likelihood_blasso_steepest_lasso_loss(A, y, curr_beta,curr_lambda,eps_step,p);
  %gradinit = (sum(A,1)  - T./(A*X+1e-12)'*A)';
 % loss_improv  = -2*sj*xtr(A_set(j))-eps_step*xtx(A_set(j),A_set(j));      % This is multiplied by 1/eps_step to try to get more numerical stability
  lasso_improv = (f_old_lasso - f_steepest_lasso);                                % This is multiplied by 1/eps_step to try to get more numerical stability
  %loss_improv
  %curr_lambda
  
  % 3b.1 If backward step is good:
  % =======================================================================
  %zero_eps = 1e-5;
  if(lasso_improv>zero_eps)
    new_beta           = curr_beta; 
    new_beta(j) = curr_beta(j) + sj;                         % update beta for current lambda
    %new_beta(A_set(j))
    betas(:,n_lambdas) = new_beta;                                         % new_beta gets improvement in loss without incurring in more loss - it is better for the same lambda
    curr_beta          = new_beta;                                         % update current beta
    backward_flag(end) = 1;
    if (pre_j ==j) && (pre_sj + sj ==0)
        break;
    else
    pre_j = j; pre_sj = sj;
    end
    %f_old = f_steepest_lasso ;
  % 3b.2 If backward step is no good:
  % ===============================[f_new,j,sj] = log_likelihood_blasso_componenet_minimize_2(AX, A, y, curr_beta,eps_step);========================================
  else                  
    % 3b.2.1 Find best direction to move
    % =====================================================================
    %AX = (A)*curr_beta; 
    [f_forward,j,sj,f_old] = log_likelihood_blasso_forward_1(A, y, curr_beta,eps_step);
                                    % Get sign of direction to move
    new_beta          = curr_beta;    new_beta(j) = curr_beta(j) + sj;     % Computes the new beta (in step count units)
    lasso_improv = (f_old - f_forward);                     % This is the improvement in the loss function
    new_lambda   = lasso_improv/(norm(new_beta,p)-norm(curr_beta,p));                                       % Since (norm(new_beta,1) - norm(curr_beta,1)) = eps_step

    % 3b.2.1 If lambda decreased and improvement happened: relax constraint
    % =====================================================================
    if((new_lambda<curr_lambda)&(new_lambda>0))                            % If lambda has decreased, a relaxation happened:
      lambdas         = [lambdas new_lambda];                              % - add the new lambda to the vector of lambdas
      betas           = [betas new_beta];                                  % - add the new estimate to the path
      backward_flag   = [backward_flag;0];
      n_lambdas       = n_lambdas + 1;                                     % - increase the recorded size of the lambda vector
      curr_lambda     = new_lambda;                                        % - updates the value of lambda
    
    % 3b.2.1 If improvement happened for same lambda: record new optimum
    % =====================================================================
    elseif(new_lambda>0)
      betas(:,n_lambdas)          = new_beta;                              % If lambda > 0 but is greater than the current one, update beta for curr_lambda and discards new_lambda
      backward_flag(end) = 0;

    % 3b.2.1 If no improvement was possible do not record new set of 
    %        coefficients and set the stop condition
    % =====================================================================
    else
      curr_lambda     = new_lambda;                                        % If lambda < 0, there was no improvement. DO NOT include beta in vector of results and make exit condition be true
    end;
    %xtr               = xtr - eps_step*sj*xtx(:,j);                        % Updates correlation between explanatory variables and residuals
    curr_beta         = new_beta;                                          % Updates the vector of estimates
    %f_old             = f_forward;
    %find(new_beta>0)
    if (pre_j ==j) && (pre_sj + sj == 0)
        break;
    else
    pre_j = j; pre_sj = sj;
    end
  end;
end;

% 4. Saving the outputs (path included) in a structure:
% =========================================================================
% result.nbetas(:,variables_idx) = eps_step*betas';
% result.nbetas(:,constants_idx) = zeros(size(betas, 2), size(constants_idx, 2));
% result.betas(:,variables_idx)  = yscale*eps_step*betas'*diag(1./xscale(variables_idx));
% result.betas(:,constants_idx)  = zeros(size(betas, 2), size(constants_idx, 2));
% result.intercepts    = ymeans-(xmeans*result.betas')';
% result.lambdas       = lambdas;

%

% Added 09/16/2005: Guilherme Rocha
% result.fitted        = kron((result.intercepts'), ones(n_obs,1)) + dictionary*result.betas';
% result.residuals     = kron(y, ones(1, size(result.betas, 1))) - result.fitted;
% result.sample_size   = n_obs;
result.L1_nbetas     = betas;%sum(abs(result.nbetas'))';
result.L1_betas      = sum(abs(betas));
result.method        = 'blasso';
%result.backward_flag = backward_flag;
end

function [ f_min,hat_j,hat_s,f_old_empirical] = log_likelihood_blasso_componenet_select_first(A, T, betas,eps_step)
%T:obervation,N
%To select the first isoform, each componenet increase eps_step
global epsilon;
%max_val = 1e20;
%X_new = X + epsilon;
%beta = beta + epsilon;
AX = A*betas;
n_beta = length(betas);
f_vals = zeros(n_beta,1); %column 1: positive eps_step; column 2 :negative esp_step;
                          %column 3: how far can go in negative direction
% val_ori = A*betas;
% 
val_ori_log = log(AX+epsilon);
%warning off;
%val_ori_log = log(AX);
%warning on;
%val_ori_log(isinf(val_ori_log)) = 0;
f_old_empirical = sum(AX) - T * val_ori_log;

for j = 1:n_beta
    f_vals(j,1) = log_likelihood_blasso_sub(AX,A,T,j,eps_step,epsilon);
end

[f_min,hat_j]=min(f_vals);
hat_s = eps_step;

end


function [f] = log_likelihood_blasso_sub(AX,A,T,j,eps_step,eplison)
    %A = A + eplison;
    
    val = AX + A(:,j)*eps_step; 
    %val = 
    %warning off
    val_log = log(val+eplison);
    %val_log = log(val);
    %warning on
    %val_log(isinf(val_log)) = 0;
    f = sum(val) - T * val_log;
end

%Minimize the lasso loss function
function [f_min,hat_j,hat_s,f_old] = log_likelihood_blasso_steepest_lasso_loss(A, T, betas,curr_lambda,eps_step,p)
%T:obervation,N
global epsilon;
max_val = 1e20;
%X_new = X + epsilon;
%beta = beta + epsilon;

AX = A*betas;
n_beta = length(betas);
f_vals = zeros(n_beta,2); %column 1: positive eps_step; column 2 :negative esp_step;
                          %column 3: how far can go in negative direction
f_old = log_likelihood_blasso_lasso_loss(AX,A,T,1,betas,curr_lambda,p,0,epsilon);%eps_step is 0,j can be any value
for j = 1:n_beta
    f_vals(j,1) = log_likelihood_blasso_lasso_loss(AX,A,T,j,betas,curr_lambda,p,eps_step,epsilon);
    %objective function value when stepwise = -eps_step
    if betas(j) <= 0 % give a max_val to inhibit be selected
        f_vals(j,2) = max_val;
    else
        f_vals(j,2) = log_likelihood_blasso_lasso_loss(AX,A,T,j,betas,curr_lambda,p,-eps_step,epsilon);
    end
end
%f_vals_12 = f_vals(:,1:2);
[f_min,b]=min(f_vals(:));
if b<= n_beta,
    hat_j = b;
    hat_s = eps_step;
else
    hat_j = b - n_beta;
    hat_s = -eps_step;
end

end


function [f] = log_likelihood_blasso_lasso_loss(AX,A,T,j,betas,curr_lambda,p,eps_step,eplison)
    %A = A + eplison;
    
    val = AX + A(:,j)*eps_step; 
    new_beta = betas;
    new_beta(j) = new_beta(j) + eps_step;
    %val = 
    %warning off
    val_log = log(val+eplison);
    %val_log = log(val);
    %warning on
    %val_log(isinf(val_log)) = 0;
    f = sum(val) - T * val_log + curr_lambda*norm(new_beta,p);
end


%%%%step 3:Look forward
function [ f_min,hat_j,hat_s,f_old_empirical] = log_likelihood_blasso_forward_1(A, T, betas,eps_step)
%T:obervation,N
global epsilon;
max_val = 1e20;
%X_new = X + epsilon;
%beta = beta + epsilon;
%eps_step = eps_step*sqrt(2);
AX = A*betas;
n_beta = length(betas);
f_vals = zeros(n_beta,2); %column 1: positive eps_step; column 2 :negative esp_step;
f_old_empirical = log_likelihood_blasso_sub(AX,A,T,1,0,epsilon);
for j = 1:n_beta
    f_vals(j,1) = log_likelihood_blasso_sub(AX,A,T,j,eps_step,epsilon);
    %objective function value when stepwise = -eps_step
    if betas(j) <= 0 % give a max_val to inhibit be selected
        f_vals(j,2) = max_val;
    else
        f_vals(j,2) = log_likelihood_blasso_sub(AX,A,T,j,-eps_step,epsilon);
    end
end

[f_min,b]=min(f_vals(:));
if b<= n_beta,
    hat_j = b;
    hat_s = eps_step;
else
    hat_j = b - n_beta;
    hat_s = -eps_step;
end

end
