%% Function to recover single column of mixing matrix
function [u,peak_val,diff,C] = ICA_power(X,maxruns, ...
                                         B,verbose, ...
                                         contrast_fn, ...
                                         u_init, ...
                                         C_init, ...
                                         M)
% Inputs : X - data with shape (n,k)
%          maxruns - number of iterations to run ICA
%          B - true mixing matrix, used only for debugging purposes
%          verbose - flag to print debug messages
%          contrast_fn - Constrast function to use for ICA, for example
%                        cgf or kurtosis

if nargin==3
    verbose=0;
end

% if nargin==8
%     k=size(X,2);
%     M=eye(k);
% end

addpath(genpath('../utils/'));
% u_init=B(:,1);u_init=u_init/norm(u_init);
%% Initialisation
k = size(X,2);
u = u_init;
u_prev = zeros(k,1);
% C = contrast_fn.estimate_C(20);
% C = C/norm(C);
C = C_init;

grad = u;
e=B\grad; e=e/norm(e);

%keyboard
peak_val = zeros(maxruns+1,1);
peak = zeros(maxruns+1,1);
off = zeros(maxruns+1,1);
diff = zeros(maxruns+1, k);
%B2 = B*diag(1./sqrt(diag(B'*B)));
B2=B;
%% Initialize at fixed point---for debugging only
% B1=inv(B'); u = B1(1,:); u = u'/norm(u);
% u=B(:,2); u = u/norm(u);
% Strue = B*B'+Sigma;
% C = B*B';
% d=sqrt(diag(B'*B));
% e = abs(grad'*B)./d';

if(verbose)
[peak_val(1),peak(1)]=max(abs(e));
diff(1,:) = e;
end

%keyboard
% if verbose
%     disp(e');
%     %inv(B)*grad
% end

%% Algorithm execution
j = 1;
% while ( (norm(u-u_prev) > epsilon) ...
%         && (norm(u + u_prev) > epsilon) ...
%         && (j <= maxruns) )
while(j <= maxruns)
    
    if verbose
        formatSpec = 'Iteration %d, Error %f\n';
        fprintf(formatSpec,j,1-peak_val(j));
    end
%     if ( (j == 1) && (abs(1-peak_val(j)) < 1e-2) )
%         keyboard
%     end
    % Perform algorithm update
    u_prev = u;
    u = M*u;
    u1 = pinv(C)*u;
%     u1
    grad = contrast_fn.get_gradient(u1);
%     grad
    grad = grad/norm(grad);
%     grad = grad/sqrt(abs(u'*pinv(C)*u));
    u = grad;
    
    % Error tracking
    if(verbose)
    e = B\u;
    e = e/norm(e);
    diff(j+1,:) = grad;

     % e = u'*B2;

%    e=abs(grad'*B)./d';
%     if verbose
%         disp(B\diff(j+1,:)');
%     end
    [peak_val(j+1),peak(j+1)] = max(abs(e));
    off(j) = max(max(abs(C-diag(diag(C)))))/min(abs(diag(C)));
    end

    j = j + 1;
    %keyboard
    %pause
end

if(verbose)
    formatSpec = 'Iteration %d, Error %f\n';
    fprintf(formatSpec,j,1-peak_val(j));
    if peak_val(j)<0
        keyboard
    end
    for i=j:maxruns+1
        peak_val(i) = peak_val(i-1);
    end
end
