% This code solves   
%  min mu*||H(x)*U||_*+1/2*||x-bx||^2
% where H(x) is a block Hankel matrix formed from x, and bx and U come from measurements: 
% U is a matrix such that U'U=I.
% x is p by N+1 matrix.
% U is N-r+1 by n matrix.
% H(x) is p(r+1) by N-r+1 Hankel matrix. 
%
% The Fenchel dual of this problem is
%  min   0.5*||E^*(Y)||_2^2 + bx'*E^*(Y)
%  s.t.  sigma_max(Y) <= mu,
% where E(x)=H(x)*U.
%
% This code solves the dual problem by gradient-projection methods.
%
% Input:  
% U       - N-r+1 by n matrix such that U'*U=I
% bx      - a p by N+1 real matrix (measured observations)
% r       - parameter
% mu      - regularization parameter
% tol     - termination tolerance 
% alg     - 1 gradient-projection method with large constant stepsize + line search
%           2 gradient-projection method with constant stepsize 1.95/L
%           3 accelerated grad-proj method I with constant stepsize 1/L
% freq    - frequency of checking termination criterion.
% maxiter - maximum number of iterations
%
% Output: 
% x       - p by N+1 real matrix, the approximate primal optimal solution
% pobj    - approximate primal optimal value
% dobj    - approximate dual optimal value
% t       - cputime
% k       - number of iterations
% gap     - duality gap
%
% Written by Maryam Fazel, Ting Kei Pong, Defeng Sun and Paul Tseng
% Last modified Nov 27, 2012


function [x,pobj,dobj,t,k,gap] = sysHankelgp(U,bx,r,mu,tol,alg,freq,maxiter)

tic

Ut = U';
n = size(U,2);
p = size(bx,1);
N = size(bx,2)-1;
m = (r+1)*p;
  
% Set algorithm parameters and termination criterion

L = min(r+1,N-r+1);
meps = 1e-8;			% machine epsilon: used for early termination check

HankelAdjTime = 0;
HankelTime = 0;
svd_time = 0;

% Initialization  
Y = zeros(m,n);
fbest = inf;
dbest = -inf;

%%%% Gradient-projection method with line search %%%%
if alg == 1

  L = L/8;
  k = 0;
  while k < maxiter
    
    % Compute E^*(Y)
    HankelAdjTime0 = clock;
    EadjY = HradjX(Y*Ut,N,r,p);
    HankelAdjTime = HankelAdjTime + etime(clock,HankelAdjTime0);
    
    x = EadjY + bx;
    
    HankelTime0 = clock;
    G = Hrx(x,r)*U;
    HankelTime = HankelTime + etime(clock,HankelTime0);

    if (k > 0) && (mod(k,freq) == 0)
      
      nmEY = norm(EadjY,'fro')^2/2;
      pobj = nmEY + mu*sum(svd(G));
      dobj = -(nmEY + mytrace(bx,EadjY)); 
      if fbest > pobj
        fbest = pobj;
        xbest = x;
      end
      if dbest < dobj
        dbest = dobj;
      end
%      fprintf(' iter= %g  dobj= %g  pobj= %g  time= %g \n',k,dobj,pobj,toc);
%       fprintf('.');
      gap = abs(fbest-dbest)/max(abs(dbest),1);
      if gap < tol
        break
      end

    end   
 
    YGL = Y - G/L;
    svd_time0 = clock;
    [R,D,S] = svd(YGL,'econ');			%faster alternative?
    svd_time = svd_time + etime(clock,svd_time0);
    
    hY = R*min(D,mu)*S';
    dY = hY - Y;           % dY is the search direction
    
    % Compute ||E^*(dY)||^2
    HankelAdjTime0 = clock;
    EadjdY = HradjX(dY*Ut,N,r,p);
    HankelAdjTime = HankelAdjTime + etime(clock,HankelAdjTime0);
  
    step = min(1,-mytrace(G,dY)/norm(EadjdY,'fro')^2);
    k = k + 1;   

    % early termination if stepsize goes negative
    if step < meps
      fprintf(' termination due to small stepsize = %g \n',step);

      nmEY = norm(EadjY,'fro')^2/2;
      pobj = nmEY + mu*sum(svd(G));
      dobj = -(nmEY + mytrace(bx,EadjY)); 
      gap = abs(pobj-dobj)/max(abs(dobj),1);
      fbest = pobj;
      dbest = dobj;
      xbest = x;
      fprintf(' iter= %g  dobj= %g  pobj= %g  time= %g \n',k,dobj,pobj,toc);
      break
    end

    Y = Y + step*dY;

  end 
end

%%%% Gradient-projection method with constant stepsize %%%%
if alg == 2

  L = L/1.95;
  k = 0;
  while k < maxiter
    % Compute E^*(Y)
    HankelAdjTime0 = clock;
    EadjY = HradjX(Y*Ut,N,r,p);
    HankelAdjTime = HankelAdjTime + etime(clock,HankelAdjTime0);

    x = bx + EadjY;
    
    HankelTime0 = clock;
    G = Hrx(x,r)*U;
    HankelTime = HankelTime + etime(clock,HankelTime0);
    
    if (k > 0) && (mod(k,freq) == 0)

      nmEY = norm(EadjY,'fro')^2/2;
      pobj = nmEY + mu*sum(svd(G));
      dobj = -(nmEY + mytrace(bx,EadjY) );  
      if fbest > pobj
        fbest = pobj;
        xbest = x;
      end
      if dbest < dobj
        dbest = dobj;
      end
%      fprintf(' iter= %g  dobj= %g  pobj= %g  time= %g \n',k,dobj,pobj,toc);
%       fprintf('.');
      gap = abs(fbest-dbest)/max(abs(dbest),1);
      if gap < tol
        break
      end
    
    end

    YGL = Y - G/L;
    svd_time0 = clock;
    [R,D,S] = svd(YGL,'econ');			%faster alternative?
    svd_time = svd_time + etime(clock,svd_time0);
    %fprintf('time of svd: %g\n',toc-t);

    Yold = Y;
    Y = R*min(D,mu)*S';
    k = k + 1; 
    
    % early termination if stepsize goes negative
    step = norm(Y-Yold,'fro');
    if step < meps
      fprintf(' termination due to small stepsize = %g \n',step);      

      nmEY = norm(EadjY,'fro')^2/2;
      pobj = nmEY + mu*sum(svd(G));
      dobj = -(nmEY + mytrace(bx,EadjY));  
      gap = abs(pobj-dobj)/max(abs(dobj),1);
      fbest = pobj;
      dbest = dobj;
      xbest = x;
      fprintf(' iter= %g  dobj= %g  pobj= %g  time= %g \n',k,dobj,pobj,toc);
      break
    end

  end 
end

%%%% Accelerated gradient-projection method I %%%%
if alg == 3


  Yold = Y;
  % Compute E^*(Y)
  HankelAdjTime0 = clock;
  EadjY = HradjX(Y*Ut,N,r,p);
  HankelAdjTime = HankelAdjTime + etime(clock,HankelAdjTime0);
  
  % Compute E(x)
  x = bx + EadjY;
  HankelTime0 = clock;
  Ex = Hrx(x,r)*U;
  HankelTime = HankelTime + etime(clock,HankelTime0);
%   xavg = zeros(p,N+1);
  theta = 1;
  thetaold = theta;
  k = 0;

  while k < maxiter
    
    % Extrapolate Y in the direction Y-Yold
    Yxtrp = Y + (theta/thetaold-theta)*(Y - Yold);
    
    Exold = Ex;
    x = bx + EadjY;
      
    HankelTime0 = clock;
    Ex = Hrx(x,r)*U;
%     Exavg = Hrx(xavg,r)*U;
    HankelTime = HankelTime + etime(clock,HankelTime0);
    
    if (k > 0) && (mod(k,freq) == 0)
      nmEY = norm(EadjY,'fro')^2/2;
      pobj = nmEY + mu*sum(svd(Ex));
%       pavg = norm(xavg-bx,'fro')^2/2+mu*sum(svd(Exavg));
      dobj = -(nmEY + mytrace(bx,EadjY) );  
      if fbest > pobj
        fbest = pobj;
        xbest = x;
      end
      if dbest < dobj
        dbest = dobj;
      end
%      fprintf(' iter= %g  dobj= %g  pobj= %g  time= %g \n',k,dobj,pobj,toc);
%       fprintf('.');
%       gap = abs(min(pavg,pobj)-dobj)/max(abs(dobj),1);
      gap = abs(fbest-dbest)/max(abs(dbest),1);
      if gap < tol
        break
      end
    end
    
    G = Ex + (theta/thetaold-theta)*(Ex - Exold);
 
    YxGL = Yxtrp - G/L;
    svd_time0 = clock;
    [R,D,S] = svd(YxGL,'econ');			%faster alternative?
    svd_time = svd_time + etime(clock,svd_time0);

    Yold = Y;
    Y = R*min(D,mu)*S';
    HankelAdjTime0 = clock;
    EadjY = HradjX(Y*Ut,N,r,p);
    HankelAdjTime = HankelAdjTime + etime(clock,HankelAdjTime0);
%     xavg = (1-theta)*xavg+theta*(bx+EadjYxtrp);
    thetaold = theta;
    theta = 2*theta/(sqrt(theta^2+4)+theta);
    k = k + 1;
    
    % early termination if change in iterate is small
    step = norm(Y-Yold,'fro');
    if step < meps
      fprintf(' termination due to small stepsize = %g \n',step)

      nmEY = norm(EadjY,'fro')^2/2;
      pobj = nmEY + mu*sum(svd(Ex));
%       pavg = norm(xavg-bx,'fro')^2/2+mu*sum(svd(Exavg));
      dobj = -(nmEY + mytrace(bx,EadjY) );  
%       gap = abs(min(pavg,pobj)-dobj)/max(abs(dobj),1);
      gap = abs(pobj - dobj)/max(abs(dobj),1);
      fbest = pobj;
      dbest = dobj;
      xbest = x;
      fprintf(' iter= %g  dobj= %g  pobj= %g  time= %g \n',k,dobj,pobj,toc);
      break
    end

  end 
end

% if alg == 3 
%   if pavg < pobj
%     Ex = Exavg;
%     pobj = pavg;
%     fprintf(' pavg is used\n')
%   end
% end

x = xbest;
pobj = fbest;
dobj = dbest;
t = toc;

fprintf('\n Dual gradient projection alg. %g terminates \n',alg);

fprintf(' L= %g  #iter= %g  dobj= %g  pobj= %4.3e  time= %g \n',L,k,dobj,pobj,t);

fprintf(' Total time for computing the the Hankel operator & its adjoint: %g  %g\n ', HankelTime,HankelAdjTime);

fprintf(' Total time for svd:  %g\n', svd_time);





