% 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 q matrix.
% H(x) is p(r+1) by N-r+1 Hankel matrix. 
%
% The dual of this problem is
%  min   0.5*||gamma||_2^2 - bx'*gamma
%  s.t.  E^*(Lambda) + gamma = 0, sigma_max(Lambda) <= mu
% where E(x) = H(x)*U.
%
% This code applies ADM to the primal problem, with proximal terms added to
% regularize the mapping E(x).
%
% Input:  
% U       - N-r+1 by q matrix such that U'*U=I
% bx      - a p by N+1 real matrix (measured observations)
% r       - parameter
% mu      - regularization parameter
% tol     - termination tolerance 
% init    - 1 initialize all variables at zero
%           2 initialize x at bx
% freq    - frequency of checking termination criterion.
% maxiter - maximum number of iterations
%
% Output: 
% x       - p by N+1 real matrix, the approximate primal optimal solution
% fval    - approximate primal optimal value
% dval    - approximate dual optimal value
% t       - cputime
% iter    - number of iterations
% gap     - duality gap
%
% Written by Maryam Fazel, Ting Kei Pong and Defeng Sun
% Last modified Nov 27, 2012

function [x,fval,dval,t,iter,gap] = sysHankelADM2(U,bx,r,mu,tol,init,freq,maxiter)

fprintf(' start ADM\n')

tic

% parameters
meps = 1e-8; % machine epsilon
Ut = U';

q = size(U,2);
p = size(bx,1);
N = size(bx,2)-1;
m = (r+1)*p;

L = min(r+1,N-r+1);
beta = L*mu/(2*norm(bx,2)); 
taubeta = 1.61*beta;
sigma = 1/L; % so that sigma*(||H*H||)<=1 for ADM, but note that ||H*H||<=L

% Initialization

if init == 1
  x = zeros(p,N+1);
  X = zeros(m,q);
else
  x = bx;
  X = Hrx(-x,r)*U;
end
lambda = zeros(m,q);

Hankeltime = 0;
HankelAdjtime = 0;
svd_time = 0;

Hankeltime0 = clock;
Hx = Hrx(x,r)*U;
Hankeltime = Hankeltime + etime(clock,Hankeltime0);

fbest = inf;
dbest = -inf;
xbest = x;

% Loop

iter = 0;

while iter < maxiter
  
  xold = x;
  Xold = X;
  lambdaold = lambda;
  
  % ADM + linearization + proximal term
  
  GZ = lambda/beta - Hx;
  
  svd_time0 = clock;
  [R,S,T] = svd(GZ,'econ');
  svd_time = svd_time + etime(clock,svd_time0);
  
  X = R*max(S - mu/beta,0)*T'; % Matrix shrinkage operator
  HxXL = beta*(Hx + X) - lambda;
  
  HankelAdjtime0 = clock;
  HaU2 = HradjX(HxXL*Ut,N,r,p);
  HankelAdjtime = HankelAdjtime + etime(clock,HankelAdjtime0);

  x = sigma/(beta + sigma)*(beta/sigma*x + bx - HaU2);
  
  Hankeltime0 = clock;
  Hx = Hrx(x,r)*U;
  Hankeltime = Hankeltime + etime(clock,Hankeltime0);
  
  lambda = lambda - taubeta*(Hx + X);
  
  iter = iter + 1;
  
  if max([norm(x-xold,'fro'),norm(lambda-lambdaold,'fro'),norm(X-Xold,'fro')]) < meps % early termination
    
    fval = norm(x-bx,'fro')^2/2 + mu*sum(svd(Hx));
    
    svd_time0 = clock;
    [R,S,T] = svd(lambda,'econ');
    svd_time = svd_time + etime(clock,svd_time0);
    
    lambda2 = R*min(S,mu)*T';
    
    HankelAdjtime0 = clock;
    HaUU = HradjX(lambda2*Ut,N,r,p);
    HankelAdjtime = HankelAdjtime + etime(clock,HankelAdjtime0);
    
    x2 = HaUU + bx;
    Hankeltime0 = clock;
    Hx2 = Hrx(x2,r)*U;
    Hankeltime = Hankeltime + etime(clock,Hankeltime0);
    
    nrm = 0.5*norm(HaUU,'fro')^2;
    fval2 = nrm + mu*sum(svd(Hx2)); 
    dval = -(nrm + mytrace(HaUU,bx));
%    fprintf(' iter, fval, dval, time = %g, %g, %g, %g, %g\n', iter, fval, dval, toc)
%    fprintf('.')
    gap = abs(min(fval,fval2)-dval)/max(abs(dval),1);
    fbest = min(fval,fval2);
    dbest = dval;
    if fval > fval2
      xbest = x2;
    else
      xbest = x;
    end
    fprintf(' early termination \n')
%    fprintf(' iter, fval, dval, time = %g, %g, %g, %g\n', iter, fval, dval, toc)
%    fprintf('.')
    break
    
  end  
  
  if mod(iter,freq) == 0
    
    fval = norm(x-bx,'fro')^2/2 + mu*sum(svd(Hx));
    
    svd_time0 = clock;
    [R,S,T] = svd(lambda,'econ');
    svd_time = svd_time + etime(clock,svd_time0);
    
    lambda2 = R*min(S,mu)*T';
    
    HankelAdjtime0 = clock;
    HaUU = HradjX(lambda2*Ut,N,r,p);
    HankelAdjtime = HankelAdjtime + etime(clock,HankelAdjtime0);
    
    x2 = HaUU + bx;
    Hankeltime0 = clock;
    Hx2 = Hrx(x2,r)*U;
    Hankeltime = Hankeltime + etime(clock,Hankeltime0);
    
    nrm = 0.5*norm(HaUU,'fro')^2;
    fval2 = nrm + mu*sum(svd(Hx2));    
    dval = -(nrm + mytrace(HaUU,bx));
    
    if dbest < dval
      dbest = dval;
    end
    if fbest > fval
      fbest = fval;
      xbest = x;
    end
    if fbest > fval2
      fbest = fval2;
      xbest = x2;
    end
%    fprintf(' iter, fval, dval, time = %g, %g, %g, %g, %g\n', iter, fval, dval, toc)
%    fprintf('.')
    gap = abs(fbest-dbest)/max(abs(dbest),1);
    if gap < tol
      break
    end
    
  end
end

x = xbest;
fval = fbest;
dval = dbest;

t = toc;

fprintf('\n ADM2 terminates, init = %g \n', init);

fprintf(' #iter= %g  dobj= %4.3e  pobj= %4.3e  time= %g \n',iter,dval,fval,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);