% implement Group-LARS

% find group-LARS estimator using.

% Y = X1*Theta1 + ... + Xn*Thetan + E
% Y is [N,ny] matrix (N data point, ny components)
% Nx =[nx1,..,nxn]
% sum(Nx)=nx;
% X = [X1,...,Xn] is [N,nx] matrix
% Xi is [N,Nx(i)] regressor matrix (N regressor with Nx(i) components)
% Thetai is [nx(i),ny] coefficient vector

% t is [n,ny] regularization term sum(abs(Thetai(:,j))) < t(i,j)
% data have been preliminarly centered

% Q is the weighting matrix for the coefficient 2 norm within groups.
% This is only needed for the Mixed L1-L2 regularization

% This version fixes the number of non-zero groups to ky

function Theta = Group_Lars_fix_k(Y,X,Nx,ky,Q);

if nargin==4
    Q = [];
end;


lambda = 0;
[N,ny]=size(Y);
[N,nx]=size(X);

nx_max = max(Nx);
n = length(Nx);
Etai = zeros(nx_max,ny,n); % coefficient of normalized regressor matrix
Thetai = zeros(nx_max,ny,n);

Xi = zeros(N,nx_max,n);
Li = zeros(nx_max,nx_max,n);

% define Groups and normalize regressors
for i=1:n,
    min_ind = sum(Nx(1:i-1));
    Xi(:,1:Nx(i),i) = X(:,min_ind+1:min_ind+Nx(i));  
    %Li(1:Nx(i),1:Nx(i),i)=chol(squeeze(Xi(:,1:Nx(i),i))'*squeeze(Xi(:,1:Nx(i),i))+lambda*eye(Nx(i)))';
   
    [UU SS VV] = svd(squeeze(Xi(:,1:Nx(i),i))'*squeeze(Xi(:,1:Nx(i),i))+lambda*eye(Nx(i)));
    Li(1:Nx(i),1:Nx(i),i) = UU*sqrtm(SS);
    
    Li_inv(1:Nx(i),1:Nx(i),i) = pinv(Li(1:Nx(i),1:Nx(i),i));
    Xi_bar(:,1:Nx(i),i)=Xi(:,1:Nx(i),i)*Li_inv(1:Nx(i),1:Nx(i),i)';
    Sigma_inv(1:Nx(i),1:Nx(i),i) = squeeze(Li_inv(1:Nx(i),1:Nx(i),i))*squeeze(Li_inv(1:Nx(i),1:Nx(i),i))';  
    %Thetai(1:Nx(i),:,i) = squeeze(Etai(1:Nx(i),:,i))./kron(normX(min+1:min+Nx(i))',ones(1,ny));
end;





% % define Groups
% for i=1:n,
%     min = sum(Nx(1:i-1));
%     Xi(:,1:Nx(i),i) = X(:,min+1:min+Nx(i));
%     Thetai(1:Nx(i),:,i) = squeeze(Etai(1:Nx(i),:,i))./kron(normX(min+1:min+Nx(i))',ones(1,ny));
% end;





%gammaAmax = 100000; %maximum step for last components (do not have a smart rule yet

for j=1:ny,
    Yj=Y(:,j);
    %Etai=zeros(nx,1);
    %Thetai=zeros(nx,1);
    for i = 1:n,
        nt1(i)=sqrt(sum(squeeze(Thetai(:,j,i)).*squeeze(Thetai(:,j,i)))); % l1-norm
    end;

    %ky_hat = sum(nt1>0); % number of non-zero groups
    nt1 = sum(nt1);
    

    Gamma = [];
    Gamma_Theta = [];

    A=[];
    Ac =[1:n];
    XnA=zeros(N,0);
    XnAc=zeros(N,0);
    
    Etai_matrix = zeros(nx,1);
    Theta_matrix = zeros(nx,1);

    %GA = zeros(0,0);
    %GAinv=zeros(0,0);

   cA=[];
    for i = 1:n,
        XnAc=[XnAc squeeze(Xi_bar(:,1:Nx(i),i))];
        %Etai_matrix =[Etai_matrix; squeeze(Etai(1:Nx(i),j,i))];
    end;

    %Yjh=zeros(N,1);
    k=0;            % number of components already added


    while (k<ky), % continue until less than ky componends have already been added       Yjh = [XnA XnAc]*Etai_matrix;
       Yjh = [XnA XnAc]*Etai_matrix;
        %************************
        %find largest correlation
        %************************
        %k
        
         for i = 1:length(A),
             cA(i) = norm(squeeze(Xi_bar(:,1:Nx(A(i)),A(i)))'*(Yj-Yjh))^2/Nx(A(i));
         end;

% figure(1)
% subplot(211),plot(Yj),hold on ,plot(Yjh,':r')
% subplot(212),plot(Yj-Yjh)
% pause
        for i = 1:length(Ac),
            cAc(i) = norm(squeeze(Xi_bar(:,1:Nx(Ac(i)),Ac(i)))'*(Yj-Yjh))^2/Nx(Ac(i));
        end;
%         k
%         if ~isempty(cA)
%             abs(cA)
%         end;
%         
%         abs(cAc)
%         pause
        C=max(abs(cAc));

        imaxAc=find(C==abs(cAc));
        imax = Ac(imaxAc);  % index of maximal correlation in [XnAc];

        A = [A imax];
        % Last search direction (for normalized group regressors)
        
        
        
        
        XnA = zeros(N,0);
        for i = 1:length(A),
            XnA = [XnA squeeze(Xi_bar(:,1:Nx(A(i)),A(i)))];
            %(cA(i) = norm(squeeze(Xi_bar(:,1:Nx(A(i))))'*(Yi-Yih))^2/Nx(A(i));
        end;
        
        min_ind=0;
        for i=1:length(A),
            Sigma_inv_matrix(min_ind+1:min_ind+Nx(A(i)),min_ind+1:min_ind+Nx(A(i)))=squeeze(Sigma_inv(1:Nx(A(i)),1:Nx(A(i)),A(i)));
            min_ind=min_ind+Nx(A(i));
           
        end;
        %Gamma = [];
        
        Gamma = pinv(XnA'*XnA + lambda*Sigma_inv_matrix)*XnA'*(Yj-Yjh);
        %Gamma = XnA'*(Yj-Yjh);
        
        Gamma_Theta = [];
        ind_temp=0;
        for i=1:length(A),            
        %Gamma_last = squeeze(Xi_bar(:,1:Nx(A(i)),A(i)))'*(Yj-Yjh);
        Gamma_last_Theta = Li_inv(1:Nx(A(i)),1:Nx(A(i)),A(i))'*Gamma(ind_temp+1:ind_temp+Nx(A(i)),:);  % search direction for NON-NORMALIZED group regressors
        ind_temp = ind_temp + Nx(A(i));
        %Vector of search directions

       % Gamma = [Gamma ; Gamma_last];
        Gamma_Theta = [Gamma_Theta ; Gamma_last_Theta];
        end;
        
        %Ac
        %         imax
        %         if k==4 keyboard
        %         end

        remove = find(Ac==imax);
        cAcs = sign(cAc(remove));
        Ac = [Ac(1:remove-1) Ac(remove+1:end)];
        
        cAc =[cAc(1:remove-1) cAc(remove+1:end)];
        k = length(A); %increase number of added components (assume only one-at-a-time (generic))
        %keyboard



        XnA = zeros(N,0);

        for i = 1:length(A),
            XnA = [XnA squeeze(Xi_bar(:,1:Nx(A(i)),A(i)))];
            %(cA(i) = norm(squeeze(Xi_bar(:,1:Nx(A(i))))'*(Yi-Yih))^2/Nx(A(i));
        end;
        XnAc = zeros(N,0);

        for i = 1:length(Ac),
            XnAc = [XnAc squeeze(Xi_bar(:,1:Nx(Ac(i)),Ac(i)))];
            %(cA(i) = norm(squeeze(Xi_bar(:,1:Nx(A(i))))'*(Yi-Yih))^2/Nx(A(i));
        end;

        Ek=Yj-Yjh;
       
        ck=squeeze(Xi_bar(:,1:Nx(A(1)),A(1)))'*Ek-lambda*squeeze(Sigma_inv(1:Nx(A(1)),1:Nx(A(1)),A(1)))*Etai_matrix(1:Nx(A(1)));
        cktilde = squeeze(Xi_bar(:,1:Nx(A(1)),A(1)))'*XnA*Gamma+lambda*squeeze(Sigma_inv(1:Nx(A(1)),1:Nx(A(1)),A(1)))*Gamma(1:Nx(A(1)));
        alpha=[];
        for i = 1:length(Ac),
                      
            cki=squeeze(Xi_bar(:,1:Nx(Ac(i)),Ac(i)))'*Ek;
            ckitilde = squeeze(Xi_bar(:,1:Nx(Ac(i)),Ac(i)))'*XnA*Gamma;
             a = norm(cktilde)^2/Nx(A(1))-norm(ckitilde)^2/Nx(Ac(i));
            c = norm(ck)^2/Nx(A(1))-norm(cki)^2/Nx(Ac(i));
            b = -2*(ck'*cktilde/Nx(A(1))-cki'*ckitilde/Nx(Ac(i)));
            lambda1 = (-b + sqrt(b^2-4*a*c))/(2*a);
            lambda2 = (-b -sqrt(b^2-4*a*c))/(2*a);
            %lambda1 
            %lambda2
            %pause
            if imag(lambda1)~=0,
                alpha(i)=1;
            else
                if ((lambda1<lambda2) & (lambda1>0) & (lambda1<=1))
                    alpha(i)=lambda1;
                else
                    if ((lambda2>0) & (lambda2<=1))
                        alpha(i) = lambda2;
                    else
                        alpha(i)=1;
                    end;
                end;
            end;
        end;

        i_step = find(alpha==min(alpha));
        alpha_step =min(alpha);
        if isempty(alpha_step),
            alpha_step=1;
        end;
        
        p_gamma = length(Gamma);
        p_Eta = length(Etai_matrix);

        %cont = 1; 
        
        % bisection method to find step closer to desired threshold
        alpha_min=0;
        alpha_max = alpha_step;
        
        Etai_matrix_temp = Etai_matrix + alpha_step*[Gamma; zeros(p_Eta-p_gamma,1)];
        Theta_matrix_temp = Theta_matrix + alpha_step*[Gamma_Theta; zeros(p_Eta-p_gamma,1)];
        min_ind =0;
        for i =1:length(A),
            Thetai(1:Nx(A(i)),j,A(i))=Theta_matrix_temp(min_ind+1:min_ind+Nx(A(i)));
            min_ind =min_ind + Nx(A(i));
        end;
        Etai_matrix = Etai_matrix_temp;
        Theta_matrix = Theta_matrix_temp;
%         while cont
%             %alpha_min
%             %alpha_max
%             %alpha_step
%             %pause
%             %size(Etai_matrix)
%             %size(alpha_step*[Gamma; zeros(p_Eta-p_gamma,1)])
%             
%             %keyboard
%             
%             Etai_matrix_temp = Etai_matrix + alpha_step*[Gamma; zeros(p_Eta-p_gamma,1)];
%             Theta_matrix_temp = Theta_matrix + alpha_step*[Gamma_Theta; zeros(p_Eta-p_gamma,1)];
%             
% 
%             %% fill in structure with estimated theta
% 
%             min_ind =0;
%             for i =1:length(A),
%                 Thetai(1:Nx(A(i)),j,A(i))=Theta_matrix_temp(min_ind+1:min_ind+Nx(A(i)));
%                 min_ind =min_ind + Nx(A(i));
%             end;
% 
%             %% compute l1 norm
% 
%             for i = 1:n,
%                 
%                 nt1(i)=mynorm(squeeze(Thetai(:,j,i)),Q); % l1-norm
%                 %nt1(i)=sqrt(sum(squeeze(Thetai(:,j,i)).*squeeze(Thetai(:,j,i)))); % l1-norm
%             end;
% 
%             nt1 = sum(nt1);
%             
%             %if ((nt1 <=t(j)) & (alpha_min ==0)) | ((nt1>=t(j)) & (nt1<t(j)+0.001))
%             if (nt1<=t(j) &(alpha_step==alpha_max))|((nt1>=t(j))&(nt1<t(j)+0.001))
%                 cont =0;
%             else
%                 if (nt1 <t(j))
%                     %disp('norm < t')
%                     %alpha_min
%                     %alpha_step
%                     %alpha_max
%                     alpha_min = alpha_step;
%                     alpha_step = (alpha_step + alpha_max)/2;  
%                     %pause
%                 else
%                     %disp('norm > t')
%                     %alpha_min
%                     %alpha_step
%                     %alpha_max
%                     alpha_max = alpha_step;
%                     alpha_step = (alpha_step + alpha_min)/2;
%                     %pause
%                 end;
%             end;
%         
%         %alpha_min
%         %alpha_max
%         %alpha_step
%         %cont
%         %nt1
%         %t(j)
%         %disp('fine iterazione bisezione')
%         %pause
%        
%         end;
%         Etai_matrix = Etai_matrix_temp;
%         Theta_matrix = Theta_matrix_temp;
% 


        %%%% SISTEMATO FINO A QUI !

    %Thetai
    %k
    %nx
    %disp('Fine Iterazione k')
    %pause
    end;
    %keyboard


end;
Theta = Thetai;

%